Sub: accept NAV_LOITER_UNLIM and NAV_LAND as command_int too

ArduSub/GCS_Mavlink.cpp

@@ -459,21 +459,40 @@ bool GCS_MAVLINK_Sub::set_home(const Location& loc, bool _lock) {
     return sub.set_home(loc, _lock);
 }
 
-
-MAV_RESULT GCS_MAVLINK_Sub::handle_command_long_packet(const mavlink_command_long_t &packet, const mavlink_message_t &msg)
+MAV_RESULT GCS_MAVLINK_Sub::handle_command_int_packet(const mavlink_command_int_t &packet, const mavlink_message_t &msg)
 {
-    switch (packet.command) {
+    switch(packet.command) {
+
     case MAV_CMD_NAV_LOITER_UNLIM:
+        return handle_MAV_CMD_NAV_LOITER_UNLIM(packet);
+
+    case MAV_CMD_NAV_LAND:
+        return handle_MAV_CMD_NAV_LAND(packet);
+
+    }
+
+    return GCS_MAVLINK::handle_command_int_packet(packet, msg);
+}
+
+MAV_RESULT GCS_MAVLINK_Sub::handle_MAV_CMD_NAV_LOITER_UNLIM(const mavlink_command_int_t &packet)
+{
         if (!sub.set_mode(Mode::Number::POSHOLD, ModeReason::GCS_COMMAND)) {
             return MAV_RESULT_FAILED;
         }
         return MAV_RESULT_ACCEPTED;
+}
 
-    case MAV_CMD_NAV_LAND:
+MAV_RESULT GCS_MAVLINK_Sub::handle_MAV_CMD_NAV_LAND(const mavlink_command_int_t &packet)
+{
         if (!sub.set_mode(Mode::Number::SURFACE, ModeReason::GCS_COMMAND)) {
             return MAV_RESULT_FAILED;
         }
         return MAV_RESULT_ACCEPTED;
+}
+
+MAV_RESULT GCS_MAVLINK_Sub::handle_command_long_packet(const mavlink_command_long_t &packet, const mavlink_message_t &msg)
+{
+    switch (packet.command) {
 
     case MAV_CMD_CONDITION_YAW:
         // param1 : target angle [0-360]

ArduSub/GCS_Mavlink.h

@@ -22,6 +22,8 @@ class GCS_MAVLINK_Sub : public GCS_MAVLINK {
     MAV_RESULT _handle_command_preflight_calibration(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_packet(const mavlink_command_int_t &packet, const mavlink_message_t &msg) override;
+
     // override sending of scaled_pressure3 to send on-board temperature:
     void send_scaled_pressure3() override;
 
@@ -51,6 +53,9 @@ class GCS_MAVLINK_Sub : public GCS_MAVLINK {
 
     int16_t vfr_hud_throttle() const override;
 
+    MAV_RESULT handle_MAV_CMD_NAV_LOITER_UNLIM(const mavlink_command_int_t &packet);
+    MAV_RESULT handle_MAV_CMD_NAV_LAND(const mavlink_command_int_t &packet);
+
 #if HAL_HIGH_LATENCY2_ENABLED
     int16_t high_latency_target_altitude() const override;
     uint8_t high_latency_tgt_heading() const override;

Tools/autotest/ardusub.py

@@ -425,6 +425,20 @@ def disabled_tests(self):
         })
         return ret
 
+    def MAV_CMD_NAV_LOITER_UNLIM(self):
+        '''test handling of MAV_CMD_NAV_LOITER_UNLIM received via mavlink'''
+        for cmd in self.run_cmd, self.run_cmd_int:
+            self.change_mode('CIRCLE')
+            cmd(mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM)
+            self.assert_mode('POSHOLD')
+
+    def MAV_CMD_NAV_LAND(self):
+        '''test handling of MAV_CMD_NAV_LAND received via mavlink'''
+        for cmd in self.run_cmd, self.run_cmd_int:
+            self.change_mode('CIRCLE')
+            cmd(mavutil.mavlink.MAV_CMD_NAV_LAND)
+            self.assert_mode('SURFACE')
+
     def tests(self):
         '''return list of all tests'''
         ret = super(AutoTestSub, self).tests()
@@ -440,6 +454,8 @@ def tests(self):
             self.MotorThrustHoverParameterIgnore,
             self.SET_POSITION_TARGET_GLOBAL_INT,
             self.TestLogDownloadMAVProxy,
+            self.MAV_CMD_NAV_LOITER_UNLIM,
+            self.MAV_CMD_NAV_LAND,
         ])
 
         return ret