Plane: initial implementation of QRTL for quadplane RTL

ArduPlane/ArduPlane.cpp

@@ -712,7 +712,8 @@ void Plane::update_flight_mode(void)
     case QSTABILIZE:
     case QHOVER:
     case QLOITER:
-    case QLAND: {
+    case QLAND:
+    case QRTL: {
         // set nav_roll and nav_pitch using sticks
         nav_roll_cd  = channel_roll->norm_input() * roll_limit_cd;
         nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd);
@@ -795,6 +796,7 @@ void Plane::update_navigation()
     case QHOVER:
     case QLOITER:
     case QLAND:
+    case QRTL:
         // nothing to do
         break;
     }

ArduPlane/Attitude.cpp

@@ -144,6 +144,7 @@ void Plane::stabilize_stick_mixing_direct()
         control_mode == QHOVER ||
         control_mode == QLOITER ||
         control_mode == QLAND ||
+        control_mode == QRTL ||
         control_mode == TRAINING) {
         return;
     }
@@ -167,6 +168,7 @@ void Plane::stabilize_stick_mixing_fbw()
         control_mode == QHOVER ||
         control_mode == QLOITER ||
         control_mode == QLAND ||
+        control_mode == QRTL ||
         control_mode == TRAINING ||
         (control_mode == AUTO && g.auto_fbw_steer == 42)) {
         return;
@@ -367,7 +369,8 @@ void Plane::stabilize()
     } else if (control_mode == QSTABILIZE ||
                control_mode == QHOVER ||
                control_mode == QLOITER ||
-               control_mode == QLAND) {
+               control_mode == QLAND ||
+               control_mode == QRTL) {
         quadplane.control_run();
     } else {
         if (g.stick_mixing == STICK_MIXING_FBW && control_mode != STABILIZE) {

ArduPlane/GCS_Mavlink.cpp

@@ -51,6 +51,7 @@ void Plane::send_heartbeat(mavlink_channel_t chan)
     case LOITER:
     case GUIDED:
     case CIRCLE:
+    case QRTL:
         base_mode = MAV_MODE_FLAG_GUIDED_ENABLED |
                     MAV_MODE_FLAG_STABILIZE_ENABLED;
         // note that MAV_MODE_FLAG_AUTO_ENABLED does not match what
@@ -203,6 +204,7 @@ void Plane::send_extended_status1(mavlink_channel_t chan)
     case LOITER:
     case GUIDED:
     case CIRCLE:
+    case QRTL:
         control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control
         control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION; // attitude stabilisation
         control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_YAW_POSITION; // yaw position

ArduPlane/Plane.h

@@ -832,7 +832,7 @@ private:
     void set_guided_WP(void);
     void init_home();
     void update_home();
-    void do_RTL(void);
+    void do_RTL(int32_t alt);
     bool verify_takeoff();
     bool verify_loiter_unlim();
     bool verify_loiter_time();

ArduPlane/altitude.cpp

@@ -582,7 +582,9 @@ void Plane::rangefinder_height_update(void)
                 (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH ||
                  flight_stage == AP_SpdHgtControl::FLIGHT_LAND_PREFLARE ||
                  flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL ||
-                 control_mode == QLAND) &&
+                 control_mode == QLAND ||
+                 control_mode == QRTL ||
+                 (control_mode == AUTO && plane.mission.get_current_nav_cmd().id == MAV_CMD_NAV_VTOL_LAND)) &&
                 g.rangefinder_landing) {
                 rangefinder_state.in_use = true;
                 gcs_send_text_fmt(MAV_SEVERITY_INFO, "Rangefinder engaged at %.2fm", (double)rangefinder_state.height_estimate);

ArduPlane/commands_logic.cpp

@@ -284,7 +284,7 @@ bool Plane::verify_command(const AP_Mission::Mission_Command& cmd)        // Ret
         return quadplane.verify_vtol_takeoff(cmd);
 
     case MAV_CMD_NAV_VTOL_LAND:
-        return quadplane.verify_vtol_land(cmd);
+        return quadplane.verify_vtol_land();
         
     // do commands (always return true)
     case MAV_CMD_DO_CHANGE_SPEED:
@@ -321,12 +321,12 @@ bool Plane::verify_command(const AP_Mission::Mission_Command& cmd)        // Ret
 //  Nav (Must) commands
 /********************************************************************************/
 
-void Plane::do_RTL(void)
+void Plane::do_RTL(int32_t rtl_altitude)
 {
     auto_state.next_wp_no_crosstrack = true;
     auto_state.no_crosstrack = true;
     prev_WP_loc = current_loc;
-    next_WP_loc = rally.calc_best_rally_or_home_location(current_loc, get_RTL_altitude());
+    next_WP_loc = rally.calc_best_rally_or_home_location(current_loc, rtl_altitude);
     setup_terrain_target_alt(next_WP_loc);
     set_target_altitude_location(next_WP_loc);
 

ArduPlane/defines.h

@@ -65,7 +65,8 @@ enum FlightMode {
     QSTABILIZE    = 17,
     QHOVER        = 18,
     QLOITER       = 19,
-    QLAND         = 20
+    QLAND         = 20,
+    QRTL          = 21
 };
 
 // type of stick mixing enabled

ArduPlane/events.cpp

@@ -52,6 +52,7 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype)
     case CIRCLE:
     case RTL:
     case QLAND:
+    case QRTL:
     default:
         break;
     }
@@ -109,6 +110,7 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype)
 
     case RTL:
     case QLAND:
+    case QRTL:
     default:
         break;
     }

ArduPlane/quadplane.cpp

@@ -254,6 +254,15 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = {
     // @Increment: 0.1
     // @User: Standard
     AP_GROUPINFO("WVANE_MINROLL", 34, QuadPlane, weathervane.min_roll, 1),
+
+    // @Param: RTL_ALT
+    // @DisplayName: QRTL return altitude
+    // @Description: The altitude which QRTL mode heads to initially
+    // @Units: m
+    // @Range: 1 200
+    // @Increment: 1
+    // @User: Standard
+    AP_GROUPINFO("RTL_ALT", 35, QuadPlane, qrtl_alt, 15),
     
     AP_GROUPEND
 };
@@ -922,6 +931,10 @@ void QuadPlane::control_run(void)
     case QLOITER:
     case QLAND:
         control_loiter();
+        break;
+    case QRTL:
+        control_qrtl();
+        break;
     default:
         break;
     }
@@ -956,6 +969,9 @@ bool QuadPlane::init_mode(void)
     case QLAND:
         init_land();
         break;
+    case QRTL:
+        init_qrtl();
+        break;
     default:
         break;
     }
@@ -1031,6 +1047,7 @@ bool QuadPlane::in_vtol_mode(void)
             plane.control_mode == QHOVER ||
             plane.control_mode == QLOITER ||
             plane.control_mode == QLAND ||
+            plane.control_mode == QRTL ||
             in_vtol_auto());
 }
 
@@ -1189,7 +1206,18 @@ void QuadPlane::land_controller(void)
     switch (land.state) {
     case QLAND_POSITION1:
     case QLAND_POSITION2:
-        pos_control->set_alt_target_from_climb_rate(0, plane.G_Dt, false);
+        if (plane.control_mode == QRTL) {
+            plane.ahrs.get_position(plane.current_loc);
+            plane.auto_state.wp_proportion = location_path_proportion(plane.current_loc, 
+                                                                      plane.prev_WP_loc, plane.next_WP_loc);
+            float target_altitude = linear_interpolate(plane.prev_WP_loc.alt,
+                                                       plane.next_WP_loc.alt,
+                                                       plane.auto_state.wp_proportion,
+                                                       0, 1);
+            pos_control->set_alt_target(target_altitude - plane.home.alt);
+        } else {
+            pos_control->set_alt_target_from_climb_rate(0, plane.G_Dt, false);
+        }
         break;
 
     case QLAND_DESCEND: {
@@ -1319,6 +1347,31 @@ void QuadPlane::control_auto(const Location &loc)
     
 }
 
+/*
+  handle QRTL mode
+ */
+void QuadPlane::control_qrtl(void)
+{
+    land_controller();
+    if (land.state >= QLAND_POSITION2) {
+        // change target altitude to home alt
+        plane.next_WP_loc.alt = plane.home.alt;
+        verify_vtol_land();
+    } else {
+        pos_control->set_alt_target(qrtl_alt*100UL);
+    }
+}
+
+/*
+  handle QRTL mode
+ */
+void QuadPlane::init_qrtl(void)
+{
+    // use do_RTL() to setup next_WP_loc
+    plane.do_RTL(plane.home.alt + qrtl_alt*100UL);
+    plane.prev_WP_loc = plane.current_loc;
+}
+
 /*
   start a VTOL takeoff
  */
@@ -1418,7 +1471,7 @@ void QuadPlane::check_land_complete(void)
 /*
   check if a VTOL landing has completed
  */
-bool QuadPlane::verify_vtol_land(const AP_Mission::Mission_Command &cmd)
+bool QuadPlane::verify_vtol_land(void)
 {
     if (!available()) {
         return true;
@@ -1427,7 +1480,7 @@ bool QuadPlane::verify_vtol_land(const AP_Mission::Mission_Command &cmd)
         plane.auto_state.wp_distance < 2) {
         land.state = QLAND_DESCEND;
         plane.gcs_send_text(MAV_SEVERITY_INFO,"Land descend started");
-        plane.set_next_WP(cmd.content.location);        
+        plane.set_next_WP(plane.next_WP_loc);
     }
 
     if (should_relax()) {

ArduPlane/quadplane.h

@@ -62,7 +62,7 @@ public:
     bool do_vtol_takeoff(const AP_Mission::Mission_Command& cmd);
     bool do_vtol_land(const AP_Mission::Mission_Command& cmd);
     bool verify_vtol_takeoff(const AP_Mission::Mission_Command &cmd);
-    bool verify_vtol_land(const AP_Mission::Mission_Command &cmd);
+    bool verify_vtol_land(void);
     bool in_vtol_auto(void);
     bool in_vtol_mode(void);
 
@@ -156,6 +156,9 @@ private:
     void control_loiter(void);
     void check_land_complete(void);
 
+    void init_qrtl(void);
+    void control_qrtl(void);
+    
     float assist_climb_rate_cms(void);
 
     // calculate desired yaw rate for assistance
@@ -187,6 +190,9 @@ private:
     // landing speed in cm/s
     AP_Int16 land_speed_cms;
 
+    // QRTL start altitude, meters
+    AP_Int16 qrtl_alt;
+    
     // alt to switch to QLAND_FINAL
     AP_Float land_final_alt;
     

ArduPlane/system.cpp

@@ -439,7 +439,7 @@ void Plane::set_mode(enum FlightMode mode)
     case RTL:
         auto_throttle_mode = true;
         prev_WP_loc = current_loc;
-        do_RTL();
+        do_RTL(get_RTL_altitude());
         break;
 
     case LOITER:
@@ -462,6 +462,7 @@ void Plane::set_mode(enum FlightMode mode)
     case QHOVER:
     case QLOITER:
     case QLAND:
+    case QRTL:
         if (!quadplane.init_mode()) {
             control_mode = previous_mode;
         } else {
@@ -507,6 +508,7 @@ bool Plane::mavlink_set_mode(uint8_t mode)
     case QHOVER:
     case QLOITER:
     case QLAND:
+    case QRTL:
         set_mode((enum FlightMode)mode);
         return true;
     }
@@ -711,6 +713,9 @@ void Plane::print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode)
     case QLAND:
         port->print("QLand");
         break;
+    case QRTL:
+        port->print("QRTL");
+        break;
     default:
         port->printf("Mode(%u)", (unsigned)mode);
         break;