ArduCopter: handle MAV_CMD_NAV_TAKEOFF via command_long and command_in

ArduPilot · Oct 24, 2023 · ca3b5a8 · ca3b5a8
1 parent 21eaa08
commit ca3b5a8
Show file tree

Hide file tree

Showing 2 changed files with 21 additions and 9 deletions.
diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp
@@ -770,6 +770,10 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_int_packet(const mavlink_command_i
     case MAV_CMD_DO_MOTOR_TEST:
         return handle_MAV_CMD_DO_MOTOR_TEST(packet);
 
+    case MAV_CMD_NAV_TAKEOFF:
+    case MAV_CMD_NAV_VTOL_TAKEOFF:
+        return handle_MAV_CMD_NAV_TAKEOFF(packet);
+
 #if PARACHUTE == ENABLED
     case MAV_CMD_DO_PARACHUTE:
         return handle_MAV_CMD_DO_PARACHUTE(packet);
@@ -847,31 +851,37 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_mount(const mavlink_command_int_t
 }
 #endif
 
-MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_long_t &packet, const mavlink_message_t &msg)
+MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_NAV_TAKEOFF(const mavlink_command_int_t &packet)
 {
-    switch(packet.command) {
+    if (packet.frame != MAV_FRAME_GLOBAL_RELATIVE_ALT) {
+        return MAV_RESULT_DENIED;  // meaning some parameters are bad
+    }
 
-    case MAV_CMD_NAV_VTOL_TAKEOFF:
-    case MAV_CMD_NAV_TAKEOFF: {
         // param3 : horizontal navigation by pilot acceptable
         // param4 : yaw angle   (not supported)
         // param5 : latitude    (not supported)
         // param6 : longitude   (not supported)
         // param7 : altitude [metres]
 
-        float takeoff_alt = packet.param7 * 100;      // Convert m to cm
+        float takeoff_alt = packet.z * 100;      // Convert m to cm
 
         if (!copter.flightmode->do_user_takeoff(takeoff_alt, is_zero(packet.param3))) {
             return MAV_RESULT_FAILED;
         }
         return MAV_RESULT_ACCEPTED;
-    }
+}
 
-    default:
-        return GCS_MAVLINK::handle_command_long_packet(packet, msg);
+bool GCS_MAVLINK_Copter::mav_frame_for_command_long(MAV_FRAME &frame, MAV_CMD packet_command) const
+{
+    if (packet_command == MAV_CMD_NAV_TAKEOFF ||
+        packet_command == MAV_CMD_NAV_VTOL_TAKEOFF) {
+        frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
+        return true;
     }
+    return GCS_MAVLINK::mav_frame_for_command_long(frame, packet_command);
 }
 
+
 MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_CONDITION_YAW(const mavlink_command_int_t &packet)
 {
         // param1 : target angle [0-360]

diff --git a/ArduCopter/GCS_Mavlink.h b/ArduCopter/GCS_Mavlink.h
@@ -38,7 +38,6 @@ class GCS_MAVLINK_Copter : public GCS_MAVLINK
     MAV_RESULT handle_command_mount(const mavlink_command_int_t &packet, const mavlink_message_t &msg) override;
 #endif
     MAV_RESULT handle_command_int_packet(const mavlink_command_int_t &packet, const mavlink_message_t &msg) override;
-    MAV_RESULT handle_command_long_packet(const mavlink_command_long_t &packet, const mavlink_message_t &msg) override;
     MAV_RESULT handle_command_int_do_reposition(const mavlink_command_int_t &packet);
     MAV_RESULT handle_command_pause_continue(const mavlink_command_int_t &packet);
 
@@ -109,7 +108,10 @@ class GCS_MAVLINK_Copter : public GCS_MAVLINK
     MAV_RESULT handle_MAV_CMD_SOLO_BTN_PAUSE_CLICK(const mavlink_command_int_t &packet);
 #endif
 
+    bool mav_frame_for_command_long(MAV_FRAME &frame, MAV_CMD packet_command) const override;
+
     MAV_RESULT handle_MAV_CMD_MISSION_START(const mavlink_command_int_t &packet);
+    MAV_RESULT handle_MAV_CMD_NAV_TAKEOFF(const mavlink_command_int_t &packet);
 
 #if AP_WINCH_ENABLED
     MAV_RESULT handle_MAV_CMD_DO_WINCH(const mavlink_command_int_t &packet);