Sub: support ABOVE_TERRAIN frame in auto mode

ArduSub/commands_logic.cpp

@@ -15,16 +15,20 @@ bool Sub::start_command(const AP_Mission::Mission_Command& cmd)
 #endif
 
     const Location &target_loc = cmd.content.location;
+    auto alt_frame = target_loc.get_alt_frame();
 
-    // target alt must be negative (underwater)
-    if (target_loc.alt > 0.0f) {
-        gcs().send_text(MAV_SEVERITY_WARNING, "BAD NAV ALT %0.2f", (double)target_loc.alt);
-        return false;
-    }
-
-    // only tested/supported alt frame so far is AltFrame::ABOVE_HOME, where Home alt is always water's surface ie zero depth
-    if (target_loc.get_alt_frame() != Location::AltFrame::ABOVE_HOME) {
-        gcs().send_text(MAV_SEVERITY_WARNING, "BAD NAV AltFrame %d", (int8_t)target_loc.get_alt_frame());
+    if (alt_frame == Location::AltFrame::ABOVE_HOME) {
+        if (target_loc.alt > 0) {
+            gcs().send_text(MAV_SEVERITY_WARNING, "Alt above home must be negative");
+            return false;
+        }
+    } else if (alt_frame == Location::AltFrame::ABOVE_TERRAIN) {
+        if (target_loc.alt < 0) {
+            gcs().send_text(MAV_SEVERITY_WARNING, "Alt above terrain must be positive");
+            return false;
+        }
+    } else {
+        gcs().send_text(MAV_SEVERITY_WARNING, "Bad alt frame");
         return false;
     }
 
@@ -111,6 +115,7 @@ bool Sub::start_command(const AP_Mission::Mission_Command& cmd)
 
     default:
         // unable to use the command, allow the vehicle to try the next command
+        gcs().send_text(MAV_SEVERITY_WARNING, "Ignoring command %d", cmd.id);
         return false;
     }
 

ArduSub/mode_auto.cpp

@@ -88,6 +88,7 @@ void ModeAuto::auto_wp_start(const Location& dest_loc)
     // send target to waypoint controller
     if (!sub.wp_nav.set_wp_destination_loc(dest_loc)) {
         // failure to set destination can only be because of missing terrain data
+        gcs().send_text(MAV_SEVERITY_WARNING, "Terrain data (rangefinder) not available");
         sub.failsafe_terrain_on_event();
         return;
     }

Tools/autotest/ArduSub_Tests/TerrainMission/terrain_mission.txt

@@ -0,0 +1,16 @@
+QGC WPL 110
+
+# Item 0 is the home position, frame=3
+0	0	3	16	0.000000	0.000000	0.000000	0.000000	33.809780	-118.393982	0.0	1
+
+# Descend to -25m, frame=3
+1	0	3	16	0.000000	0.000000	0.000000	0.000000	33.809780	-118.393982	-25.0	1
+
+# Travel at current rangefinder reading (~25m), frame=10
+2	0	10	16	0.000000	0.000000	0.000000	0.000000	33.809780	-118.394432	0.0	1
+
+# Descend to 10m above floor, frame=10
+3	0	10	16	0.000000	0.000000	0.000000	0.000000	33.810310	-118.394432	10.0	1
+
+# Ascend to 15m above floor, frame=10
+4	0	10	16	0.000000	0.000000	0.000000	0.000000	33.810310	-118.393982	15.0	1

Tools/autotest/ardusub.py

@@ -1,6 +1,9 @@
 '''
 Dive ArduSub in SITL
 
+Depth of water is 50m, the ground is flat
+Parameters are in-code defaults plus default_params/sub.parm
+
 AP_FLAKE8_CLEAN
 '''
 
@@ -563,7 +566,7 @@ def MAV_CMD_MISSION_START(self):
     def MAV_CMD_DO_CHANGE_SPEED(self):
         '''ensure vehicle changes speeds when DO_CHANGE_SPEED received'''
         items = [
-            (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, -3),  # Dive so we have constrat drag
+            (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, -3),  # Dive so we have constant drag
             (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 2000, 0, -1),
             (mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0),
         ]
@@ -621,6 +624,27 @@ def MAV_CMD_CONDITION_YAW(self):
         self._MAV_CMD_CONDITION_YAW(self.run_cmd)
         self._MAV_CMD_CONDITION_YAW(self.run_cmd_int)
 
+    def TerrainMission(self):
+        """Mission using surface tracking"""
+
+        if self.get_parameter('RNGFND1_MAX_CM') != 3000.0:
+            raise PreconditionFailedException("RNGFND1_MAX_CM is not %g" % 3000.0)
+
+        filename = "terrain_mission.txt"
+        self.progress("Executing mission %s" % filename)
+        self.load_mission(filename)
+        self.set_rc_default()
+        self.arm_vehicle()
+        self.change_mode('AUTO')
+        self.wait_waypoint(1, 4, max_dist=5)
+        self.delay_sim_time(3)
+
+        # Expect sub to hover at final altitude
+        self.assert_altitude(-36.0)
+
+        self.disarm_vehicle()
+        self.progress("Mission OK")
+
     def tests(self):
         '''return list of all tests'''
         ret = super(AutoTestSub, self).tests()
@@ -644,6 +668,7 @@ def tests(self):
             self.MAV_CMD_MISSION_START,
             self.MAV_CMD_DO_CHANGE_SPEED,
             self.MAV_CMD_CONDITION_YAW,
+            self.TerrainMission,
         ])
 
         return ret