Plane: Reverse Thrust

Reverse thrust for controlled landings, even with much steeper approach slopes. This is achieved by allowing throttle demand to go negative to maintain a target airspeed. A Pre-Flare stage was added, triggered by an altitude, to allow for a slower airspeed just before land. That lower airspeed can be near stall.
new params LAND_PF_ALT, LAND_PF_SEC, LAND_PF_ARSPD, USE_REV_THRUST

ArduPlane/ArduPlane.cpp

@@ -529,6 +529,7 @@ void Plane::handle_auto_mode(void)
             steer_state.hold_course_cd = -1;
         }
         auto_state.land_complete = false;
+        auto_state.land_pre_flare = false;
         calc_nav_roll();
         calc_nav_pitch();
         calc_throttle();
@@ -885,6 +886,7 @@ void Plane::update_alt()
         SpdHgt_Controller->update_pitch_throttle(relative_target_altitude_cm(),
                                                  target_airspeed_cm,
                                                  flight_stage,
+                                                 mission.get_current_nav_cmd().id,
                                                  auto_state.takeoff_pitch_cd,
                                                  throttle_nudge,
                                                  tecs_hgt_afe(),
@@ -916,6 +918,8 @@ void Plane::update_flight_stage(void)
                     set_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_ABORT);
                 } else if (auto_state.land_complete == true) {
                     set_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_FINAL);
+                } else if (auto_state.land_pre_flare == true) {
+                    set_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_PREFLARE);
                 } else if (flight_stage != AP_SpdHgtControl::FLIGHT_LAND_APPROACH) {
                     float path_progress = location_path_proportion(current_loc, prev_WP_loc, next_WP_loc);
                     bool lined_up = abs(nav_controller->bearing_error_cd()) < 1000;

ArduPlane/Attitude.cpp

@@ -23,7 +23,7 @@ float Plane::get_speed_scaler(void)
     } else {
         if (channel_throttle->servo_out > 0) {
             speed_scaler = 0.5f + ((float)THROTTLE_CRUISE / channel_throttle->servo_out / 2.0f);                 // First order taylor expansion of square root
-            // Should maybe be to the 2/7 power, but we aren't goint to implement that...
+            // Should maybe be to the 2/7 power, but we aren't going to implement that...
         }else{
             speed_scaler = 1.67f;
         }
@@ -96,7 +96,7 @@ void Plane::stabilize_pitch(float speed_scaler)
 {
     int8_t force_elevator = takeoff_tail_hold();
     if (force_elevator != 0) {
-        // we are holding the tail down during takeoff. Just covert
+        // we are holding the tail down during takeoff. Just convert
         // from a percentage to a -4500..4500 centidegree angle
         channel_pitch->servo_out = 45*force_elevator;
         return;
@@ -762,10 +762,13 @@ void Plane::set_servos_idle(void)
 }
 
 /*
-  return minimum throttle, taking account of throttle reversal
+  return minimum throttle PWM value, taking account of throttle reversal. For reverse thrust you get the throttle off position
  */
 uint16_t Plane::throttle_min(void) const
 {
+    if (aparm.throttle_min < 0) {
+        return channel_throttle->radio_trim;
+    }
     return channel_throttle->get_reverse() ? channel_throttle->radio_max : channel_throttle->radio_min;
 };
 
@@ -892,18 +895,26 @@ void Plane::set_servos(void)
 #if THROTTLE_OUT == 0
         channel_throttle->servo_out = 0;
 #else
-        // convert 0 to 100% into PWM
-        uint8_t min_throttle = aparm.throttle_min.get();
-        uint8_t max_throttle = aparm.throttle_max.get();
-        if (control_mode == AUTO && flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL) {
-            min_throttle = 0;
+        // convert 0 to 100% (or -100 to +100) into PWM
+        int8_t min_throttle = aparm.throttle_min.get();
+        int8_t max_throttle = aparm.throttle_max.get();
+
+        if (min_throttle < 0 && !allow_reverse_thrust()) {
+           // reverse thrust is available but inhibited.
+           min_throttle = 0;
         }
-        if (control_mode == AUTO &&
-            (flight_stage == AP_SpdHgtControl::FLIGHT_TAKEOFF || flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT)) {
-            if(aparm.takeoff_throttle_max != 0) {
-                max_throttle = aparm.takeoff_throttle_max;
-            } else {
-                max_throttle = aparm.throttle_max;
+
+        if (control_mode == AUTO) {
+            if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL) {
+                min_throttle = 0;
+            }
+
+            if (flight_stage == AP_SpdHgtControl::FLIGHT_TAKEOFF || flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT) {
+                if(aparm.takeoff_throttle_max != 0) {
+                    max_throttle = aparm.takeoff_throttle_max;
+                } else {
+                    max_throttle = aparm.throttle_max;
+                }
             }
         }
         channel_throttle->servo_out = constrain_int16(channel_throttle->servo_out, 
@@ -1086,6 +1097,78 @@ void Plane::set_servos(void)
     RC_Channel_aux::output_ch_all();
 }
 
+bool Plane::allow_reverse_thrust(void)
+{
+    // check if we should allow reverse thrust
+    bool allow = false;
+
+    if (g.use_reverse_thrust == USE_REVERSE_THRUST_NEVER) {
+        return false;
+    }
+
+    switch (control_mode) {
+    case AUTO:
+        {
+        uint8_t nav_cmd = mission.get_current_nav_cmd().id;
+
+        // never allow reverse thrust during takeoff
+        if (nav_cmd == MAV_CMD_NAV_TAKEOFF) {
+            return false;
+        }
+
+        // always allow regardless of mission item
+        allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_AUTO_ALWAYS);
+
+        // landing
+        allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_AUTO_LAND_APPROACH) &&
+                (nav_cmd == MAV_CMD_NAV_LAND);
+
+        // LOITER_TO_ALT
+        allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_AUTO_LOITER_TO_ALT) &&
+                (nav_cmd == MAV_CMD_NAV_LOITER_TO_ALT);
+
+        // any Loiter (including LOITER_TO_ALT)
+        allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_AUTO_LOITER_ALL) &&
+                    (nav_cmd == MAV_CMD_NAV_LOITER_TIME ||
+                     nav_cmd == MAV_CMD_NAV_LOITER_TO_ALT ||
+                     nav_cmd == MAV_CMD_NAV_LOITER_TURNS ||
+                     nav_cmd == MAV_CMD_NAV_LOITER_UNLIM);
+
+        // waypoints
+        allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_AUTO_WAYPOINT) &&
+                    (nav_cmd == MAV_CMD_NAV_WAYPOINT ||
+                     nav_cmd == MAV_CMD_NAV_SPLINE_WAYPOINT);
+        }
+        break;
+
+    case LOITER:
+        allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_LOITER);
+        break;
+    case RTL:
+        allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_RTL);
+        break;
+    case CIRCLE:
+        allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_CIRCLE);
+        break;
+    case CRUISE:
+        allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_CRUISE);
+        break;
+    case FLY_BY_WIRE_B:
+        allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_FBWB);
+        break;
+    case GUIDED:
+        allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_GUIDED);
+        break;
+    default:
+        // all other control_modes are auto_throttle_mode=false.
+        // If we are not controlling throttle, don't limit it.
+        allow = true;
+        break;
+    }
+
+    return allow;
+}
+
 void Plane::demo_servos(uint8_t i) 
 {
     while(i > 0) {

ArduPlane/Parameters.cpp

@@ -244,6 +244,41 @@ const AP_Param::Info Plane::var_info[] = {
     // @User: Advanced
     ASCALAR(land_flare_sec,          "LAND_FLARE_SEC",  2.0),
 
+    // @Param: LAND_PF_ALT
+    // @DisplayName: Landing pre-flare altitude
+    // @Description: Altitude to trigger pre-flare flight stage where LAND_PF_ARSPD controls airspeed. The pre-flare flight stage trigger works just like LAND_FLARE_ALT but higher. Disabled when LAND_PF_ARSPD is 0.
+    // @Units: meters
+    // @Range: 0 30
+    // @Increment: 0.1
+    // @User: Advanced
+    GSCALAR(land_pre_flare_alt     , "LAND_PF_ALT",  10.0),
+
+    // @Param: LAND_PF_SEC
+    // @DisplayName: Landing pre-flare time
+    // @Description: Vertical time to ground to trigger pre-flare flight stage where LAND_PF_ARSPD controls airspeed. This pre-flare flight stage trigger works just like LAND_FLARE_SEC but earlier. Disabled when LAND_PF_ARSPD is 0.
+    // @Units: seconds
+    // @Range: 0 10
+    // @Increment: 0.1
+    // @User: Advanced
+    GSCALAR(land_pre_flare_sec     , "LAND_PF_SEC",  6.0),
+
+    // @Param: LAND_PF_ARSPD
+    // @DisplayName: Landing pre-flare airspeed
+    // @Description: Desired airspeed during pre-flare flight stage. This is useful to reduce airspeed just before the flare. Use 0 to disable.
+    // @Units: m/s
+    // @Range: 0 30
+    // @Increment: 0.1
+    // @User: Advanced
+    ASCALAR(land_pre_flare_airspeed, "LAND_PF_ARSPD",  0),
+
+    // @Param: USE_REV_THRUST
+    // @DisplayName: Bitmask for when to allow negative reverse thrust
+    // @Description: Typically THR_MIN will be clipped to zero unless reverse thrust is available. Since you may not want negative thrust available at all times this bitmask allows THR_MIN to go below 0 while executing certain auto-mission commands.
+    // @Values: 0:Disabled,1:AlwaysAllowed,2:LandApproach,4:LoiterToAlt,8:Loiter,16:Waypoint
+    // @Bitmask: 0:ALWAYS,1:LAND,2:LOITER_TO_ALT,3:LOITER_ALL,4:WAYPOINTS
+    // @User: Advanced
+    GSCALAR(use_reverse_thrust,     "USE_REV_THRUST",  USE_REVERSE_THRUST_AUTO_LAND_APPROACH),
+
     // @Param: LAND_DISARMDELAY
     // @DisplayName: Landing disarm delay
     // @Description: After a landing has completed using a LAND waypoint, automatically disarm after this many seconds have passed. Use 0 to not disarm.

ArduPlane/Parameters.h

@@ -178,6 +178,7 @@ public:
         k_param_acro_roll_rate,
         k_param_acro_pitch_rate,
         k_param_acro_locking,
+        k_param_use_reverse_thrust = 129,
 
         //
         // 130: Sensor parameters
@@ -201,6 +202,8 @@ public:
         k_param_mission, // mission library
         k_param_serial_manager, // serial manager library
         k_param_NavEKF2,  // EKF2
+        k_param_land_pre_flare_alt,
+        k_param_land_pre_flare_airspeed = 149,
 
         //
         // 150: Navigation parameters
@@ -225,6 +228,7 @@ public:
         k_param_camera_mount2,      // unused
         k_param_adsb,
         k_param_notify,
+        k_param_land_pre_flare_sec = 165,
 
         //
         // Battery monitoring parameters
@@ -410,6 +414,7 @@ public:
     AP_Int8 throttle_fs_enabled;
     AP_Int16 throttle_fs_value;
     AP_Int8 throttle_nudge;
+    AP_Int16 use_reverse_thrust;
 
     // Failsafe
     AP_Int8 short_fs_action;
@@ -459,6 +464,8 @@ public:
     AP_Float land_flare_alt;
     AP_Int8 land_disarm_delay;
     AP_Int8 land_abort_throttle_enable;
+    AP_Float land_pre_flare_alt;
+    AP_Float land_pre_flare_sec;
     AP_Int32 min_gndspeed_cm;
     AP_Int16 pitch_trim_cd;
     AP_Int16 FBWB_min_altitude_cm;

ArduPlane/Plane.h

@@ -427,6 +427,9 @@ private:
         // Set land_complete if we are within 2 seconds distance or within 3 meters altitude of touchdown
         bool land_complete:1;
 
+        // Flag to indicate if we have triggered pre-flare. This occurs when we have reached LAND_PF_ALT
+        bool land_pre_flare:1;
+
         // should we fly inverted?
         bool inverted_flight:1;
 
@@ -952,6 +955,7 @@ private:
     void stabilize();
     void set_servos_idle(void);
     void set_servos();
+    bool allow_reverse_thrust(void);
     void update_aux();
     void update_is_flying_5Hz(void);
     void crash_detection_update(void);

ArduPlane/commands_logic.cpp

@@ -16,6 +16,7 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
     if (AP_Mission::is_nav_cmd(cmd)) {
         // set land_complete to false to stop us zeroing the throttle
         auto_state.land_complete = false;
+        auto_state.land_pre_flare = false;
         auto_state.sink_rate = 0;
 
         // set takeoff_complete to true so we don't add extra evevator

ArduPlane/defines.h

@@ -193,4 +193,20 @@ enum {
     CRASH_DETECT_ACTION_BITMASK_DISARM = (1<<0),
     // note: next enum will be (1<<1), then (1<<2), then (1<<3)
 };
+
+enum {
+    USE_REVERSE_THRUST_NEVER                    = 0,
+    USE_REVERSE_THRUST_AUTO_ALWAYS              = (1<<0),
+    USE_REVERSE_THRUST_AUTO_LAND_APPROACH       = (1<<1),
+    USE_REVERSE_THRUST_AUTO_LOITER_TO_ALT       = (1<<2),
+    USE_REVERSE_THRUST_AUTO_LOITER_ALL          = (1<<3),
+    USE_REVERSE_THRUST_AUTO_WAYPOINT            = (1<<4),
+    USE_REVERSE_THRUST_LOITER                   = (1<<5),
+    USE_REVERSE_THRUST_RTL                      = (1<<6),
+    USE_REVERSE_THRUST_CIRCLE                   = (1<<7),
+    USE_REVERSE_THRUST_CRUISE                   = (1<<8),
+    USE_REVERSE_THRUST_FBWB                     = (1<<9),
+    USE_REVERSE_THRUST_GUIDED                   = (1<<10),
+};
+
 #endif // _DEFINES_H

ArduPlane/landing.cpp

@@ -22,6 +22,7 @@ bool Plane::verify_land()
 
         throttle_suppressed = false;
         auto_state.land_complete = false;
+        auto_state.land_pre_flare = false;
         nav_controller->update_heading_hold(get_bearing_cd(prev_WP_loc, next_WP_loc));
 
         // see if we have reached abort altitude
@@ -72,8 +73,10 @@ bool Plane::verify_land()
                                   (double)gps.ground_speed(),
                                   (double)get_distance(current_loc, next_WP_loc));
             }
+            auto_state.land_complete = true;
+            update_flight_stage();
         }
-        auto_state.land_complete = true;
+
 
         if (gps.ground_speed() < 3) {
             // reload any airspeed or groundspeed parameters that may have
@@ -84,6 +87,13 @@ bool Plane::verify_land()
             g.min_gndspeed_cm.load();
             aparm.throttle_cruise.load();
         }
+    } else if (!auto_state.land_complete && !auto_state.land_pre_flare && aparm.land_pre_flare_airspeed > 0) {
+        bool reached_pre_flare_alt = g.land_pre_flare_alt > 0 && (height <= g.land_pre_flare_alt);
+        bool reached_pre_flare_sec = g.land_pre_flare_sec > 0 && (height <= auto_state.sink_rate * g.land_pre_flare_sec);
+        if (reached_pre_flare_alt || reached_pre_flare_sec) {
+            auto_state.land_pre_flare = true;
+            update_flight_stage();
+        }
     }
 
     /*

ArduPlane/radio.cpp

@@ -25,7 +25,13 @@ void Plane::set_control_channels(void)
     channel_roll->set_angle(SERVO_MAX);
     channel_pitch->set_angle(SERVO_MAX);
     channel_rudder->set_angle(SERVO_MAX);
-    channel_throttle->set_range(0, 100);
+    if (aparm.throttle_min >= 0) {
+        // normal operation
+        channel_throttle->set_range(0, 100);
+    } else {
+        // reverse thrust
+        channel_throttle->set_angle(100);
+    }
 
     if (!arming.is_armed() && arming.arming_required() == AP_Arming::YES_MIN_PWM) {
         hal.rcout->set_safety_pwm(1UL<<(rcmap.throttle()-1), throttle_min());