Plane: added glider pullup support

ArduPlane/ArduPlane.cpp

@@ -216,6 +216,13 @@ void Plane::update_speed_height(void)
         should_run_tecs = false;
     }
 #endif
+
+#if AP_PLANE_GLIDER_PULLUP_ENABLED
+    if (mode_auto.in_pullup()) {
+        should_run_tecs = false;
+    }
+#endif
+
     if (should_run_tecs) {
 	    // Call TECS 50Hz update. Note that we call this regardless of
 	    // throttle suppressed, as this needs to be running for
@@ -512,6 +519,12 @@ void Plane::update_fly_forward(void)
     }
 #endif
 
+    if (auto_state.idle_mode) {
+        // don't fuse airspeed when in balloon lift
+        ahrs.set_fly_forward(false);
+        return;
+    }
+
     if (flight_stage == AP_FixedWing::FlightStage::LAND) {
         ahrs.set_fly_forward(landing.is_flying_forward());
         return;
@@ -579,6 +592,16 @@ void Plane::update_alt()
     }
 #endif
 
+    if (auto_state.idle_mode) {
+        should_run_tecs = false;
+    }
+
+#if AP_PLANE_GLIDER_PULLUP_ENABLED
+    if (mode_auto.in_pullup()) {
+        should_run_tecs = false;
+    }
+#endif
+    
     if (should_run_tecs && !throttle_suppressed) {
 
         float distance_beyond_land_wp = 0;

ArduPlane/Parameters.cpp

@@ -1019,6 +1019,12 @@ const AP_Param::Info Plane::var_info[] = {
     // @Path: mode_takeoff.cpp
     GOBJECT(mode_takeoff, "TKOFF_", ModeTakeoff),
 
+#if AP_PLANE_GLIDER_PULLUP_ENABLED
+    // @Group: PUP_
+    // @Path: pullup.cpp
+    GOBJECTN(mode_auto.pullup, pullup, "PUP_", GliderPullup),
+#endif
+    
     // @Group:
     // @Path: ../libraries/AP_Vehicle/AP_Vehicle.cpp
     PARAM_VEHICLE_INFO,

ArduPlane/Parameters.h

@@ -359,6 +359,8 @@ public:
         k_param_autotune_options,
         k_param_takeoff_throttle_min,
         k_param_takeoff_options,
+
+        k_param_pullup = 270,
     };
 
     AP_Int16 format_version;

ArduPlane/Plane.h

@@ -121,6 +121,7 @@
 #include "avoidance_adsb.h"
 #endif
 #include "AP_Arming.h"
+#include "pullup.h"
 
 /*
   main APM:Plane class
@@ -175,6 +176,9 @@ public:
 #if AP_EXTERNAL_CONTROL_ENABLED
     friend class AP_ExternalControl_Plane;
 #endif
+#if AP_PLANE_GLIDER_PULLUP_ENABLED
+    friend class GliderPullup;
+#endif
 
     Plane(void);
 
@@ -515,6 +519,11 @@ private:
         // have we checked for an auto-land?
         bool checked_for_autoland;
 
+        // Altitude threshold to complete a takeoff command in autonomous modes.  Centimeters
+        // are we in idle mode? used for balloon launch to stop servo
+        // movement until altitude is reached
+        bool idle_mode;
+        
         // are we in VTOL mode in AUTO?
         bool vtol_mode;
 
@@ -959,6 +968,7 @@ private:
     void do_loiter_turns(const AP_Mission::Mission_Command& cmd);
     void do_loiter_time(const AP_Mission::Mission_Command& cmd);
     void do_continue_and_change_alt(const AP_Mission::Mission_Command& cmd);
+    void do_altitude_wait(const AP_Mission::Mission_Command& cmd);
     void do_loiter_to_alt(const AP_Mission::Mission_Command& cmd);
     void do_vtol_takeoff(const AP_Mission::Mission_Command& cmd);
     void do_vtol_land(const AP_Mission::Mission_Command& cmd);

ArduPlane/commands_logic.cpp

@@ -27,6 +27,9 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
 
         nav_controller->set_data_is_stale();
 
+        // start non-idle
+        auto_state.idle_mode = false;
+        
         // reset loiter start time. New command is a new loiter
         loiter.start_time_ms = 0;
 
@@ -92,6 +95,7 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
         break;
 
     case MAV_CMD_NAV_ALTITUDE_WAIT:
+        do_altitude_wait(cmd);
         break;
 
 #if HAL_QUADPLANE_ENABLED
@@ -501,6 +505,15 @@ void Plane::do_continue_and_change_alt(const AP_Mission::Mission_Command& cmd)
     reset_offset_altitude();
 }
 
+void Plane::do_altitude_wait(const AP_Mission::Mission_Command& cmd)
+{
+    // set all servos to trim until we reach altitude or descent speed
+    auto_state.idle_mode = true;
+#if AP_PLANE_GLIDER_PULLUP_ENABLED
+    mode_auto.pullup.reset();
+#endif
+}
+
 void Plane::do_loiter_to_alt(const AP_Mission::Mission_Command& cmd)
 {
     //set target alt  
@@ -831,17 +844,46 @@ bool Plane::verify_continue_and_change_alt()
  */
 bool ModeAuto::verify_altitude_wait(const AP_Mission::Mission_Command &cmd)
 {
-    if (plane.current_loc.alt > cmd.content.altitude_wait.altitude*100.0f) {
+#if AP_PLANE_GLIDER_PULLUP_ENABLED
-        gcs().send_text(MAV_SEVERITY_INFO,"Reached altitude");
+    if (pullup.in_pullup()) {
-        return true;
+        return pullup.verify_pullup();
     }
-    if (plane.auto_state.sink_rate > cmd.content.altitude_wait.descent_rate) {
+#endif
+
+    /*
+      the target altitude in param1 is always AMSL
+     */
+    const float alt_diff = plane.current_loc.alt*0.01 - cmd.content.altitude_wait.altitude;
+    bool completed = false;
+    if (alt_diff > 0) {
+        gcs().send_text(MAV_SEVERITY_INFO,"Reached altitude");
+        completed = true;
+    } else if (cmd.content.altitude_wait.descent_rate > 0 &&
+        plane.auto_state.sink_rate > cmd.content.altitude_wait.descent_rate) {
         gcs().send_text(MAV_SEVERITY_INFO, "Reached descent rate %.1f m/s", (double)plane.auto_state.sink_rate);
+        completed = true;
+    }
+
+    if (completed) {
+#if AP_PLANE_GLIDER_PULLUP_ENABLED
+        if (pullup.pullup_start()) {
+            // we are doing a pullup, ALTITUDE_WAIT not complete until pullup is done
+            return false;
+        }
+#endif
         return true;
     }
 
-    // if requested, wiggle servos
+    const float time_to_alt = alt_diff / MIN(plane.auto_state.sink_rate, -0.01);
-    if (cmd.content.altitude_wait.wiggle_time != 0) {
+
+    /*
+      if requested, wiggle servos
+
+      we don't start a wiggle if we expect to release soon as we don't
+      want the servos to be off trim at the time of release
+    */
+    if (cmd.content.altitude_wait.wiggle_time != 0 &&
+        (plane.auto_state.sink_rate > 0 || time_to_alt > cmd.content.altitude_wait.wiggle_time*5)) {
         if (wiggle.stage == 0 &&
             AP_HAL::millis() - wiggle.last_ms > cmd.content.altitude_wait.wiggle_time*1000) {
             wiggle.stage = 1;
@@ -1291,3 +1333,4 @@ bool Plane::in_auto_mission_id(uint16_t command) const
 {
     return control_mode == &mode_auto && mission.get_current_nav_id() == command;
 }
+

ArduPlane/mode.cpp

@@ -101,6 +101,9 @@ bool Mode::enter()
     plane.target_altitude.terrain_following_pending = false;
 #endif
 
+    // disable auto mode servo idle during altitude wait command
+    plane.auto_state.idle_mode = false;
+
     bool enter_result = _enter();
 
     if (enter_result) {

ArduPlane/mode.h

@@ -9,6 +9,7 @@
 #include "quadplane.h"
 #include <AP_AHRS/AP_AHRS.h>
 #include <AP_Mission/AP_Mission.h>
+#include "pullup.h"
 
 class AC_PosControl;
 class AC_AttitudeControl_Multi;
@@ -208,6 +209,7 @@ protected:
 class ModeAuto : public Mode
 {
 public:
+    friend class Plane;
 
     Number mode_number() const override { return Number::AUTO; }
     const char *name() const override { return "AUTO"; }
@@ -237,6 +239,10 @@ public:
 
     void run() override;
 
+#if AP_PLANE_GLIDER_PULLUP_ENABLED
+    bool in_pullup() const { return pullup.in_pullup(); }
+#endif
+
 protected:
 
     bool _enter() override;
@@ -257,6 +263,10 @@ private:
         uint8_t stage;
         uint32_t last_ms;
     } wiggle;
+
+#if AP_PLANE_GLIDER_PULLUP_ENABLED
+    GliderPullup pullup;
+#endif // AP_PLANE_GLIDER_PULLUP_ENABLED
 };
 
 

ArduPlane/mode_auto.cpp

@@ -79,6 +79,12 @@ void ModeAuto::update()
     }
 #endif
 
+#if AP_PLANE_GLIDER_PULLUP_ENABLED
+    if (pullup.in_pullup()) {
+        return;
+    }
+#endif
+
     if (nav_cmd_id == MAV_CMD_NAV_TAKEOFF ||
         (nav_cmd_id == MAV_CMD_NAV_LAND && plane.flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING)) {
         plane.takeoff_calc_roll();
@@ -166,6 +172,13 @@ bool ModeAuto::is_landing() const
 
 void ModeAuto::run()
 {
+#if AP_PLANE_GLIDER_PULLUP_ENABLED
+    if (pullup.in_pullup()) {
+        pullup.stabilize_pullup();
+        return;
+    }
+#endif
+    
     if (plane.mission.get_current_nav_cmd().id == MAV_CMD_NAV_ALTITUDE_WAIT) {
 
         wiggle_servos();

ArduPlane/navigation.cpp

@@ -321,7 +321,7 @@ void Plane::update_loiter_update_nav(uint16_t radius)
     if ((loiter.start_time_ms == 0 &&
          (control_mode == &mode_auto || control_mode == &mode_guided) &&
          auto_state.crosstrack &&
-         current_loc.get_distance(next_WP_loc) > radius*3) ||
+         current_loc.get_distance(next_WP_loc) > 3 * nav_controller->loiter_radius(radius)) ||
         quadplane_qrtl_switch) {
         /*
           if never reached loiter point and using crosstrack and somewhat far away from loiter point

ArduPlane/pullup.cpp

@@ -0,0 +1,239 @@
+#include "Plane.h"
+/*
+  support for pullup after an alitude wait. Used for high altitude gliders
+ */
+
+#if AP_PLANE_GLIDER_PULLUP_ENABLED
+
+// Pullup control parameters
+const AP_Param::GroupInfo GliderPullup::var_info[] = {
+
+    // @Param: ENABLE
+    // @DisplayName: Enable pullup after altitude wait
+    // @Description: Enable pullup after altitude wait
+    // @Values: 0:Disabled, 1:Enabled
+    // @User: Advanced
+    AP_GROUPINFO_FLAGS("ENABLE", 1, GliderPullup,  enable, 0, AP_PARAM_FLAG_ENABLE),
+
+    // @Param: ELEV_OFS
+    // @DisplayName: Elevator deflection used before starting pullup
+    // @Description: Elevator deflection offset from -1 to 1 while waiting for airspeed to rise before starting close loop control of the pullup.
+    // @Range: -1.0 1.0
+    // @User: Advanced
+    AP_GROUPINFO("ELEV_OFS", 2, GliderPullup,  elev_offset, 0.1f),
+
+    // @Param: NG_LIM
+    // @DisplayName: Maximum normal load factor during pullup
+    // @Description: This is the nominal maximum value of normal load factor used during the closed loop pitch rate control of the pullup.
+    // @Range: 1.0 4.0
+    // @User: Advanced
+    AP_GROUPINFO("NG_LIM", 3, GliderPullup,  ng_limit, 2.0f),
+
+    // @Param: NG_JERK_LIM
+    // @DisplayName: Maximum normal load factor rate of change during pullup
+    // @Description: The normal load factor used for closed loop pitch rate control of the pullup will be ramped up to the value set by PUP_NG_LIM at the rate of change set by this parameter. The parameter value specified will be scaled internally by 1/EAS2TAS.
+    // @Units: 1/s
+    // @Range: 0.1 10.0
+    // @User: Advanced
+    AP_GROUPINFO("NG_JERK_LIM", 4, GliderPullup,  ng_jerk_limit, 4.0f),
+
+    // @Param: PITCH
+    // @DisplayName: Target pitch angle during pullup
+    // @Description: The vehicle will attempt achieve this pitch angle during the pull-up maneouvre.
+    // @Units: deg
+    // @Range: -5 15
+    // @User: Advanced
+    AP_GROUPINFO("PITCH", 5, GliderPullup,  pitch_dem, 3),
+
+    // @Param: ARSPD_START
+    // @DisplayName: Pullup target airspeed
+    // @Description: Target airspeed for initial airspeed wait
+    // @Units: m/s
+    // @Range: 0 100
+    // @User: Advanced
+    AP_GROUPINFO("ARSPD_START", 6, GliderPullup,  airspeed_start, 30),
+
+    // @Param: PITCH_START
+    // @DisplayName: Pullup target pitch
+    // @Description: Target pitch for initial pullup
+    // @Units: deg
+    // @Range: -80 0
+    // @User: Advanced
+    AP_GROUPINFO("PITCH_START", 7, GliderPullup,  pitch_start, -60),
+    
+    AP_GROUPEND
+};
+
+// constructor
+GliderPullup::GliderPullup(void)
+{
+    AP_Param::setup_object_defaults(this, var_info);
+}
+
+/*
+  return true if in a pullup manoeuvre at the end of NAV_ALTITUDE_WAIT
+*/
+bool GliderPullup::in_pullup(void) const
+{
+    return plane.control_mode == &plane.mode_auto &&
+        plane.mission.get_current_nav_cmd().id == MAV_CMD_NAV_ALTITUDE_WAIT &&
+        stage != Stage::NONE;
+}
+
+/*
+  start a pullup maneouvre, called when NAV_ALTITUDE_WAIT has reached
+  altitude or exceeded descent rate
+*/
+bool GliderPullup::pullup_start(void)
+{
+    if (enable != 1) {
+        return false;
+    }
+
+    // release balloon
+    SRV_Channels::set_output_scaled(SRV_Channel::k_lift_release, 100);
+
+    stage = Stage::WAIT_AIRSPEED;
+    plane.auto_state.idle_mode = false;
+    float aspeed;
+    if (!plane.ahrs.airspeed_estimate(aspeed)) {
+        aspeed = -1;
+    }
+    gcs().send_text(MAV_SEVERITY_INFO, "Start pullup airspeed %.1fm/s at %.1fm AMSL", aspeed, plane.current_loc.alt*0.01);
+    return true;
+}
+
+/*
+  first stage pullup from balloon release, verify completion
+ */
+bool GliderPullup::verify_pullup(void)
+{
+    const auto &ahrs = plane.ahrs;
+    const auto &current_loc = plane.current_loc;
+    const auto &aparm = plane.aparm;
+
+    switch (stage) {
+    case Stage::WAIT_AIRSPEED: {
+        float aspeed;
+        if (ahrs.airspeed_estimate(aspeed) && aspeed > airspeed_start) {
+            gcs().send_text(MAV_SEVERITY_INFO, "Pullup airspeed %.1fm/s alt %.1fm AMSL", aspeed, current_loc.alt*0.01);
+            stage = Stage::WAIT_PITCH;
+        }
+        return false;
+    }
+
+    case Stage::WAIT_PITCH: {
+        if (ahrs.pitch_sensor*0.01 > pitch_start && fabsf(ahrs.roll_sensor*0.01) < 90) {
+            gcs().send_text(MAV_SEVERITY_INFO, "Pullup pitch p=%.1f r=%.1f alt %.1fm AMSL",
+                            ahrs.pitch_sensor*0.01,
+                            ahrs.roll_sensor*0.01,
+                            current_loc.alt*0.01);
+            stage = Stage::WAIT_LEVEL;
+        }
+        return false;
+    }
+
+    case Stage::PUSH_NOSE_DOWN: {
+        if (fabsf(ahrs.roll_sensor*0.01) < aparm.roll_limit) {
+            stage = Stage::WAIT_LEVEL;
+        }
+        return false;
+    }
+
+    case Stage::WAIT_LEVEL: {
+        // When pitch has raised past lower limit used by speed controller, wait for airspeed to approach
+        // target value before handing over control of pitch demand to speed controller
+        bool pitchup_complete = ahrs.pitch_sensor*0.01 > MIN(0, aparm.pitch_limit_min);
+        const float pitch_lag_time = 1.0f * sqrtf(ahrs.get_EAS2TAS());
+        float aspeed;
+        const float aspeed_derivative = (ahrs.get_accel().x + GRAVITY_MSS * ahrs.get_DCM_rotation_body_to_ned().c.x) / ahrs.get_EAS2TAS();
+        bool airspeed_low = ahrs.airspeed_estimate(aspeed) ? (aspeed + aspeed_derivative * pitch_lag_time) < 0.01f * (float)plane.target_airspeed_cm : true;
+        bool roll_control_lost = fabsf(ahrs.roll_sensor*0.01) > aparm.roll_limit;
+        if (pitchup_complete && airspeed_low && !roll_control_lost) {
+                gcs().send_text(MAV_SEVERITY_INFO, "Pullup level r=%.1f p=%.1f alt %.1fm AMSL",
+                                ahrs.roll_sensor*0.01, ahrs.pitch_sensor*0.01, current_loc.alt*0.01);
+                break;
+        } else if (pitchup_complete && roll_control_lost) {
+                // push nose down and wait to get roll control back
+                gcs().send_text(MAV_SEVERITY_ALERT, "Pullup level roll bad r=%.1f p=%.1f",
+                                ahrs.roll_sensor*0.01,
+                                ahrs.pitch_sensor*0.01);
+                stage = Stage::PUSH_NOSE_DOWN;
+        }
+        return false;
+    }
+    case Stage::NONE:
+        break;
+    }
+
+    // all done
+    stage = Stage::NONE;
+    return true;
+}
+
+/*
+  stabilize during pullup from balloon drop
+ */
+void GliderPullup::stabilize_pullup(void)
+{
+    const float speed_scaler = plane.get_speed_scaler();
+    switch (stage) {
+    case Stage::WAIT_AIRSPEED: {
+        plane.pitchController.reset_I();
+        plane.yawController.reset_I();
+        SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, elev_offset*4500);
+        SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, 0);
+        plane.nav_pitch_cd = 0;
+        plane.nav_roll_cd = 0;
+        SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, plane.rollController.get_rate_out(0, speed_scaler));
+        ng_demand = 0.0;
+        break;
+    }
+    case Stage::WAIT_PITCH: {
+        plane.yawController.reset_I();
+        plane.nav_roll_cd = 0;
+        plane.nav_pitch_cd = 0;
+        SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, 0);
+        SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, plane.rollController.get_rate_out(0, speed_scaler));
+        float aspeed;
+        const auto &ahrs = plane.ahrs;
+        if (ahrs.airspeed_estimate(aspeed)) {
+            // apply a rate of change limit to the ng pullup demand
+            ng_demand += MAX(ng_jerk_limit / ahrs.get_EAS2TAS(), 0.1f) * plane.scheduler.get_loop_period_s();
+            ng_demand = MIN(ng_demand, ng_limit);
+            const float VTAS_ref = ahrs.get_EAS2TAS() * aspeed;
+            const float pullup_accel = ng_demand * GRAVITY_MSS;
+            const float demanded_rate_dps = degrees(pullup_accel / VTAS_ref);
+            const uint32_t elev_trim_offset_cd = 4500.0f * elev_offset * (1.0f - ng_demand / ng_limit);
+            SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, elev_trim_offset_cd + plane.pitchController.get_rate_out(demanded_rate_dps, speed_scaler));
+        } else {
+            SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, elev_offset*4500);
+        }
+        break;
+    }
+    case Stage::PUSH_NOSE_DOWN: {
+        plane.nav_pitch_cd = plane.aparm.pitch_limit_min*100;
+        plane.stabilize_pitch();
+        plane.nav_roll_cd = 0;
+        plane.stabilize_roll();
+        plane.stabilize_yaw();
+        ng_demand = 0.0f;
+        break;
+    }
+    case Stage::WAIT_LEVEL:
+        plane.nav_pitch_cd = MAX((plane.aparm.pitch_limit_min + 5), pitch_dem)*100;
+        plane.stabilize_pitch();
+        plane.nav_roll_cd = 0;
+        plane.stabilize_roll();
+        plane.stabilize_yaw();
+        ng_demand = 0.0f;
+        break;
+    case Stage::NONE:
+        break;
+    }
+
+    // we have done stabilisation
+    plane.last_stabilize_ms = AP_HAL::millis();
+}
+
+#endif // AP_PLANE_GLIDER_PULLUP_ENABLED

ArduPlane/pullup.h

@@ -0,0 +1,50 @@
+#pragma once
+
+/*
+  support for pullup after NAV_ALTITUDE_WAIT for gliders
+ */
+
+#ifndef AP_PLANE_GLIDER_PULLUP_ENABLED
+#define AP_PLANE_GLIDER_PULLUP_ENABLED CONFIG_HAL_BOARD == HAL_BOARD_SITL
+#endif
+
+#if AP_PLANE_GLIDER_PULLUP_ENABLED
+
+class GliderPullup
+{
+public:
+    GliderPullup(void);
+
+    void reset(void) {
+        stage = Stage::NONE;
+    }
+    bool in_pullup() const;
+    bool verify_pullup(void);
+    void stabilize_pullup(void);
+    bool pullup_complete(void);
+    bool pullup_start(void);
+
+    enum class Stage : uint8_t {
+        NONE=0,
+        WAIT_AIRSPEED,
+        WAIT_PITCH,
+        WAIT_LEVEL,
+        PUSH_NOSE_DOWN,
+    };
+
+    static const struct AP_Param::GroupInfo var_info[];
+
+private:
+    Stage stage;
+    AP_Int8  enable;
+    AP_Float elev_offset; // fraction of full elevator applied during WAIT_AIRSPEED and released during WAIT_PITCH
+    AP_Float ng_limit;
+    AP_Float airspeed_start;
+    AP_Float pitch_start;
+    AP_Float ng_jerk_limit;
+    AP_Float pitch_dem;
+    float ng_demand;
+};
+
+#endif // AP_PLANE_GLIDER_PULLUP_ENABLED
+