Sub: Implement DO_NAV_LAND commands.

Auto return to surface in analagous manner to Copter's auto land.

ArduSub/Sub.h

@@ -312,7 +312,6 @@ private:
     int32_t home_distance;
     // distance between plane and next waypoint in cm.
     uint32_t wp_distance;
-    uint8_t land_state;              // records state of land (flying to location, descending)
 
     // Auto
     AutoMode auto_mode;   // controls which auto controller is run
@@ -605,9 +604,6 @@ private:
     void auto_wp_start(const Location_Class& dest_loc);
     void auto_wp_run();
     void auto_spline_run();
-    void auto_land_start();
-    void auto_land_start(const Vector3f& destination);
-    void auto_land_run();
     void auto_circle_movetoedge_start(const Location_Class &circle_center, float radius_m);
     void auto_circle_start();
     void auto_circle_run();
@@ -789,7 +785,7 @@ private:
 
     bool do_guided(const AP_Mission::Mission_Command& cmd);
     void do_nav_wp(const AP_Mission::Mission_Command& cmd);
-    void do_land(const AP_Mission::Mission_Command& cmd);
+    void do_surface(const AP_Mission::Mission_Command& cmd);
     void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd);
     void do_circle(const AP_Mission::Mission_Command& cmd);
     void do_loiter_time(const AP_Mission::Mission_Command& cmd);
@@ -814,6 +810,7 @@ private:
     void do_gripper(const AP_Mission::Mission_Command& cmd);
 #endif
     bool verify_nav_wp(const AP_Mission::Mission_Command& cmd);
+    bool verify_surface(const AP_Mission::Mission_Command& cmd);
     bool verify_circle(const AP_Mission::Mission_Command& cmd);
     bool verify_spline_wp(const AP_Mission::Mission_Command& cmd);
 #if NAV_GUIDED == ENABLED

ArduSub/commands_logic.cpp

@@ -1,5 +1,7 @@
 #include "Sub.h"
 
+static enum AutoSurfaceState auto_surface_state = AUTO_SURFACE_STATE_GO_TO_LOCATION;
+
 // start_command - this function will be called when the ap_mission lib wishes to start a new command
 bool Sub::start_command(const AP_Mission::Mission_Command& cmd)
 {
@@ -18,7 +20,7 @@ bool Sub::start_command(const AP_Mission::Mission_Command& cmd)
         break;
 
     case MAV_CMD_NAV_LAND:              // 21 LAND to Waypoint
-        do_land(cmd);
+        do_surface(cmd);
         break;
 
     case MAV_CMD_NAV_LOITER_UNLIM:              // 17 Loiter indefinitely
@@ -174,6 +176,9 @@ bool Sub::verify_command(const AP_Mission::Mission_Command& cmd)
     case MAV_CMD_NAV_WAYPOINT:
         return verify_nav_wp(cmd);
 
+    case MAV_CMD_NAV_LAND:
+        return verify_surface(cmd);
+
     case MAV_CMD_NAV_LOITER_UNLIM:
         return verify_loiter_unlimited();
 
@@ -282,39 +287,40 @@ void Sub::do_nav_wp(const AP_Mission::Mission_Command& cmd)
     }
 }
 
-// do_land - initiate landing procedure
-void Sub::do_land(const AP_Mission::Mission_Command& cmd)
+// do_surface - initiate surface procedure
+void Sub::do_surface(const AP_Mission::Mission_Command& cmd)
 {
-    // To-Do: check if we have already landed
+    Location_Class target_location;
 
     // if location provided we fly to that location at current altitude
     if (cmd.content.location.lat != 0 || cmd.content.location.lng != 0) {
-        // set state to fly to location
-        land_state = LAND_STATE_FLY_TO_LOCATION;
+        // set state to go to location
+        auto_surface_state = AUTO_SURFACE_STATE_GO_TO_LOCATION;
 
-        // calculate and set desired location above landing target
+        // calculate and set desired location below surface target
         // convert to location class
-        Location_Class target_loc(cmd.content.location);
+        target_location = Location_Class(cmd.content.location);
 
         // decide if we will use terrain following
         int32_t curr_terr_alt_cm, target_terr_alt_cm;
         if (current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, curr_terr_alt_cm) &&
-                target_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, target_terr_alt_cm)) {
-            curr_terr_alt_cm = MAX(curr_terr_alt_cm,200);
+                target_location.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, target_terr_alt_cm)) {
             // if using terrain, set target altitude to current altitude above terrain
-            target_loc.set_alt_cm(curr_terr_alt_cm, Location_Class::ALT_FRAME_ABOVE_TERRAIN);
+            target_location.set_alt_cm(curr_terr_alt_cm, Location_Class::ALT_FRAME_ABOVE_TERRAIN);
         } else {
             // set target altitude to current altitude above home
-            target_loc.set_alt_cm(current_loc.alt, Location_Class::ALT_FRAME_ABOVE_HOME);
+            target_location.set_alt_cm(current_loc.alt, Location_Class::ALT_FRAME_ABOVE_HOME);
         }
-        auto_wp_start(target_loc);
     } else {
-        // set landing state
-        land_state = LAND_STATE_DESCENDING;
+        // set surface state to ascend
+        auto_surface_state = AUTO_SURFACE_STATE_ASCEND;
 
-        // initialise landing controller
-        auto_land_start();
+        // Set waypoint destination to current location at zero depth
+        target_location = Location_Class(current_loc.lat, current_loc.lng, 0, Location_Class::ALT_FRAME_ABOVE_HOME);
     }
+
+    // Go to wp location
+    auto_wp_start(target_location);
 }
 
 // do_loiter_unlimited - start loitering with no end conditions
@@ -563,6 +569,44 @@ bool Sub::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
     }
 }
 
+// verify_land - returns true if landing has been completed
+bool Sub::verify_surface(const AP_Mission::Mission_Command& cmd)
+{
+    bool retval = false;
+
+    switch (auto_surface_state) {
+        case AUTO_SURFACE_STATE_GO_TO_LOCATION:
+            // check if we've reached the location
+            if (wp_nav.reached_wp_destination()) {
+                // Set target to current xy and zero depth
+                // TODO get xy target from current wp destination, because current location may be acceptance-radius away from original destination
+                Location_Class target_location(cmd.content.location.lat, cmd.content.location.lng, 0, Location_Class::ALT_FRAME_ABOVE_HOME);
+
+                auto_wp_start(target_location);
+
+                // advance to next state
+                auto_surface_state = AUTO_SURFACE_STATE_ASCEND;
+            }
+            break;
+
+        case AUTO_SURFACE_STATE_ASCEND:
+            // rely on THROTTLE_LAND mode to correctly update landing status
+            if (wp_nav.reached_wp_destination()) {
+                retval = true;
+            }
+            break;
+
+        default:
+            // this should never happen
+            // TO-DO: log an error
+            retval = true;
+            break;
+    }
+
+    // true is returned if we've successfully surfaced
+    return retval;
+}
+
 bool Sub::verify_loiter_unlimited()
 {
     return false;

ArduSub/control_auto.cpp

@@ -56,10 +56,6 @@ void Sub::auto_run()
         auto_wp_run();
         break;
 
-    case Auto_Land:
-        auto_land_run();
-        break;
-
     case Auto_Circle:
         auto_circle_run();
         break;
@@ -249,56 +245,6 @@ void Sub::auto_spline_run()
     }
 }
 
-// auto_land_start - initialises controller to implement a landing
-void Sub::auto_land_start()
-{
-    // set target to stopping point
-    Vector3f stopping_point;
-    wp_nav.get_loiter_stopping_point_xy(stopping_point);
-
-    // call location specific land start function
-    auto_land_start(stopping_point);
-}
-
-// auto_land_start - initialises controller to implement a landing
-void Sub::auto_land_start(const Vector3f& destination)
-{
-    auto_mode = Auto_Land;
-
-    // initialise loiter target destination
-    wp_nav.init_loiter_target(destination);
-
-    // initialise position and desired velocity
-    pos_control.set_alt_target(inertial_nav.get_altitude());
-    pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
-
-    // initialise yaw
-    set_auto_yaw_mode(AUTO_YAW_HOLD);
-}
-
-// auto_land_run - lands in auto mode
-//      called by auto_run at 100hz or more
-void Sub::auto_land_run()
-{
-    // if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately
-    if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {
-        motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
-        // Sub vehicles do not stabilize roll/pitch/yaw when disarmed
-        attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
-
-        // set target to current position
-        wp_nav.init_loiter_target();
-        return;
-    }
-
-    // set motors to full range
-    motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
-
-    // land mode replaced by surface mode, does not have this functionality
-    //    land_run_horizontal_control();
-    //    land_run_vertical_control();
-}
-
 // auto_circle_movetoedge_start - initialise waypoint controller to move to edge of a circle with it's center at the specified location
 //  we assume the caller has set the circle's circle with circle_nav.set_center()
 //  we assume the caller has performed all required GPS_ok checks

ArduSub/defines.h

@@ -13,6 +13,11 @@
 #define BOTTOM_DETECTOR_TRIGGER_SEC 1.0
 #define SURFACE_DETECTOR_TRIGGER_SEC 1.0
 
+enum AutoSurfaceState {
+    AUTO_SURFACE_STATE_GO_TO_LOCATION,
+    AUTO_SURFACE_STATE_ASCEND
+};
+
 // Autopilot Yaw Mode enumeration
 enum autopilot_yaw_mode {
     AUTO_YAW_HOLD =              0,  // pilot controls the heading
@@ -166,10 +171,6 @@ enum RTLState {
     RTL_Land
 };
 
-// LAND state
-#define LAND_STATE_FLY_TO_LOCATION  0
-#define LAND_STATE_DESCENDING       1
-
 //  Logging parameters
 #define TYPE_AIRSTART_MSG               0x00
 #define TYPE_GROUNDSTART_MSG            0x01