New quadplane landing approach system

ArduPlane/Attitude.cpp

@@ -109,9 +109,19 @@ void Plane::stabilize_roll(float speed_scaler)
     if (control_mode == &mode_stabilize && channel_roll->get_control_in() != 0) {
         disable_integrator = true;
     }
-    SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, rollController.get_servo_out(nav_roll_cd - ahrs.roll_sensor, 
-                                                                                         speed_scaler, 
-                                                                                         disable_integrator));
+    int32_t roll_out;
+    if (!quadplane.use_fw_attitude_controllers()) {
+        // use the VTOL rate for control, to ensure consistency
+        const auto &pid_info = quadplane.attitude_control->get_rate_roll_pid().get_pid_info();
+        roll_out = rollController.get_rate_out(degrees(pid_info.target), speed_scaler);
+        /* when slaving fixed wing control to VTOL control we need to decay the integrator to prevent
+           opposing integrators balancing between the two controllers
+        */
+        rollController.decay_I();
+    } else {
+        roll_out = rollController.get_servo_out(nav_roll_cd - ahrs.roll_sensor, speed_scaler, disable_integrator);
+    }
+    SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, roll_out);
 }
 
 /*
@@ -134,14 +144,28 @@ void Plane::stabilize_pitch(float speed_scaler)
         disable_integrator = true;
     }
 
-   // if LANDING_FLARE RCx_OPTION switch is set and in FW mode, manual throttle,throttle idle then set pitch to LAND_PITCH_CD if flight option FORCE_FLARE_ATTITUDE is set
-    if (!quadplane.in_transition() && !control_mode->is_vtol_mode() && channel_throttle->in_trim_dz() && !control_mode->does_auto_throttle() && flare_mode == FlareMode::ENABLED_PITCH_TARGET) {
-       demanded_pitch = landing.get_pitch_cd();
-   }
+    int32_t pitch_out;
+    if (!quadplane.use_fw_attitude_controllers()) {
+        // use the VTOL rate for control, to ensure consistency
+        const auto &pid_info = quadplane.attitude_control->get_rate_pitch_pid().get_pid_info();
+        pitch_out = pitchController.get_rate_out(degrees(pid_info.target), speed_scaler);
+        /* when slaving fixed wing control to VTOL control we need to decay the integrator to prevent
+           opposing integrators balancing between the two controllers
+        */
+        pitchController.decay_I();
+    } else {
+        // if LANDING_FLARE RCx_OPTION switch is set and in FW mode, manual throttle,throttle idle then set pitch to LAND_PITCH_CD if flight option FORCE_FLARE_ATTITUDE is set
+        if (!quadplane.in_transition() &&
+            !control_mode->is_vtol_mode() &&
+            channel_throttle->in_trim_dz() &&
+            !control_mode->does_auto_throttle() &&
+            flare_mode == FlareMode::ENABLED_PITCH_TARGET) {
+            demanded_pitch = landing.get_pitch_cd();
+        }
 
-    SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pitchController.get_servo_out(demanded_pitch - ahrs.pitch_sensor, 
-                                                                                           speed_scaler, 
-                                                                                           disable_integrator));
+        pitch_out = pitchController.get_servo_out(demanded_pitch - ahrs.pitch_sensor, speed_scaler, disable_integrator);
+    }
+    SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pitch_out);
 }
 
 /*

ArduPlane/Log.cpp

@@ -314,7 +314,6 @@ const struct LogStructure Plane::log_structure[] = {
 
 // @LoggerMessage: NTUN
 // @Description: Navigation Tuning information - e.g. vehicle destination
-// @URL: http://ardupilot.org/rover/docs/navigation.html
 // @Field: TimeUS: Time since system startup
 // @Field: Dist: distance to the current navigation waypoint
 // @Field: TBrg: bearing to the current navigation waypoint

ArduPlane/Plane.h

@@ -814,7 +814,7 @@ class Plane : public AP_Vehicle {
     void update_load_factor(void);
     void adjust_altitude_target();
     void setup_glide_slope(void);
-    int32_t get_RTL_altitude() const;
+    int32_t get_RTL_altitude_cm() const;
     float relative_ground_altitude(bool use_rangefinder_if_available);
     void set_target_altitude_current(void);
     void set_target_altitude_current_adjusted(void);

ArduPlane/altitude.cpp

@@ -52,7 +52,9 @@ void Plane::adjust_altitude_target()
         set_target_altitude_location(temp);
     } else 
 #endif // OFFBOARD_GUIDED == ENABLED
-      if (landing.is_flaring()) {
+      if (control_mode->update_target_altitude()) {
+          // handled in mode specific code
+    } else if (landing.is_flaring()) {
         // during a landing flare, use TECS_LAND_SINK as a target sink
         // rate, and ignores the target altitude
         set_target_altitude_location(next_WP_loc);
@@ -135,9 +137,9 @@ void Plane::setup_glide_slope(void)
 }
 
 /*
-  return RTL altitude as AMSL altitude
+  return RTL altitude as AMSL cm
  */
-int32_t Plane::get_RTL_altitude() const
+int32_t Plane::get_RTL_altitude_cm() const
 {
     if (g.RTL_altitude_cm < 0) {
         return current_loc.alt;

ArduPlane/commands_logic.cpp

@@ -302,12 +302,12 @@ bool Plane::verify_command(const AP_Mission::Mission_Command& cmd)        // Ret
 //  Nav (Must) commands
 /********************************************************************************/
 
-void Plane::do_RTL(int32_t rtl_altitude)
+void Plane::do_RTL(int32_t rtl_altitude_AMSL_cm)
 {
     auto_state.next_wp_crosstrack = false;
     auto_state.crosstrack = false;
     prev_WP_loc = current_loc;
-    next_WP_loc = rally.calc_best_rally_or_home_location(current_loc, rtl_altitude);
+    next_WP_loc = rally.calc_best_rally_or_home_location(current_loc, rtl_altitude_AMSL_cm);
     setup_terrain_target_alt(next_WP_loc);
     set_target_altitude_location(next_WP_loc);
 

ArduPlane/fence.cpp

@@ -71,7 +71,7 @@ void Plane::fence_check()
             g.auto_trim.set(saved_auto_trim);
 
             if (fence.get_return_rally() != 0 || fence_act == AC_FENCE_ACTION_RTL_AND_LAND) {
-                guided_WP_loc = rally.calc_best_rally_or_home_location(current_loc, get_RTL_altitude());
+                guided_WP_loc = rally.calc_best_rally_or_home_location(current_loc, get_RTL_altitude_cm());
             } else {
                 //return to fence return point, not a rally point
                 guided_WP_loc = {};
@@ -131,4 +131,4 @@ bool Plane::fence_stickmixing(void) const
     return true;
 }
 
-#endif
+#endif

ArduPlane/mode.h

@@ -101,6 +101,9 @@ class Mode
     // whether control input is ignored with STICK_MIXING=0
     virtual bool does_auto_throttle() const { return false; }
 
+    // method for mode specific target altitude profiles
+    virtual bool update_target_altitude() { return false; }
+
 protected:
 
     // subclasses override this to perform checks before entering the mode
@@ -524,6 +527,10 @@ class ModeQRTL : public Mode
 
     bool allows_arming() const override { return false; }
 
+    bool does_auto_throttle() const override { return true; }
+
+    bool update_target_altitude() override;
+
 protected:
 
     bool _enter() override;

ArduPlane/mode_guided.cpp

@@ -9,6 +9,14 @@ bool ModeGuided::_enter()
       location. This matches the behaviour of the copter code
     */
     plane.guided_WP_loc = plane.current_loc;
+
+    if (plane.quadplane.guided_mode_enabled()) {
+        /*
+          if using Q_GUIDED_MODE then project forward by the stopping distance
+        */
+        plane.guided_WP_loc.offset_bearing(degrees(plane.ahrs.groundspeed_vector().angle()),
+                                           plane.quadplane.stopping_distance());
+    }
     plane.set_guided_WP();
     return true;
 }

ArduPlane/mode_qrtl.cpp

@@ -11,3 +11,35 @@ void ModeQRTL::update()
     plane.mode_qstabilize.update();
 }
 
+/*
+  update target altitude for QRTL profile
+ */
+bool ModeQRTL::update_target_altitude()
+{
+    /*
+      update height target in approach
+     */
+    if (plane.quadplane.poscontrol.get_state() != QuadPlane::QPOS_APPROACH) {
+        return false;
+    }
+
+    /*
+      initially approach at RTL_ALT_CM, then drop down to QRTL_ALT based on maximum sink rate from TECS,
+      giving time to lose speed before we transition
+     */
+    const float radius = MAX(fabsf(plane.aparm.loiter_radius), fabsf(plane.g.rtl_radius));
+    const float rtl_alt_delta = MAX(0, plane.g.RTL_altitude_cm*0.01 - plane.quadplane.qrtl_alt);
+    const float sink_time = rtl_alt_delta / MAX(0.6*plane.SpdHgt_Controller->get_max_sinkrate(), 1);
+    const float sink_dist = plane.aparm.airspeed_cruise_cm * 0.01 * sink_time;
+    const float dist = plane.auto_state.wp_distance;
+    const float rad_min = 2*radius;
+    const float rad_max = 20*radius;
+    float alt = linear_interpolate(0, rtl_alt_delta,
+                                   dist,
+                                   rad_min, MAX(rad_min, MIN(rad_max, rad_min+sink_dist)));
+    Location loc = plane.next_WP_loc;
+    loc.alt += alt*100;
+    plane.set_target_altitude_location(loc);
+    return true;
+}
+

ArduPlane/mode_rtl.cpp

@@ -4,7 +4,7 @@
 bool ModeRTL::_enter()
 {
     plane.prev_WP_loc = plane.current_loc;
-    plane.do_RTL(plane.get_RTL_altitude());
+    plane.do_RTL(plane.get_RTL_altitude_cm());
     plane.rtl.done_climb = false;
     plane.vtol_approach_s.approach_stage = Plane::Landing_ApproachStage::RTL;
 

ArduPlane/navigation.cpp

@@ -199,6 +199,8 @@ void Plane::calc_airspeed_errors()
                          // fallover to normal airspeed
                          target_airspeed_cm = aparm.airspeed_cruise_cm;
                      }
+        } else if (quadplane.in_vtol_land_approach()) {
+            target_airspeed_cm = quadplane.get_land_airspeed() * 100;
         } else {
             // normal AUTO mode and new_airspeed variable was set by DO_CHANGE_SPEED command while in AUTO mode
             if (new_airspeed_cm > 0) {
@@ -208,6 +210,8 @@ void Plane::calc_airspeed_errors()
                 target_airspeed_cm = aparm.airspeed_cruise_cm;
             }
         }
+    } else if (control_mode == &mode_qrtl && quadplane.in_vtol_land_approach()) {
+        target_airspeed_cm = quadplane.get_land_airspeed() * 100;
     } else {
         // Normal airspeed target for all other cases
         target_airspeed_cm = aparm.airspeed_cruise_cm;