From 2e92089ce644e0352962f52984dfb839ee355a35 Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Fri, 29 Jan 2016 23:22:21 -0800 Subject: [PATCH] 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 | 4 ++ ArduPlane/Attitude.cpp | 111 ++++++++++++++++++++++++++++++----- ArduPlane/Parameters.cpp | 35 +++++++++++ ArduPlane/Parameters.h | 7 +++ ArduPlane/Plane.h | 4 ++ ArduPlane/commands_logic.cpp | 1 + ArduPlane/defines.h | 16 +++++ ArduPlane/landing.cpp | 12 +++- ArduPlane/radio.cpp | 8 ++- 9 files changed, 182 insertions(+), 16 deletions(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 4037056433..00526b4f0a 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/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; diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 417f5c1eaf..50fb51e37b 100644 --- a/ArduPlane/Attitude.cpp +++ b/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) { diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 9b5ebbec55..6a2acde124 100644 --- a/ArduPlane/Parameters.cpp +++ b/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. diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 21f7f1057e..81179bed52 100644 --- a/ArduPlane/Parameters.h +++ b/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; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index ce53abb658..3993003b93 100644 --- a/ArduPlane/Plane.h +++ b/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); diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index a422e68f73..966fabbb03 100644 --- a/ArduPlane/commands_logic.cpp +++ b/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 diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index 94ea0c8aed..d27473af9d 100644 --- a/ArduPlane/defines.h +++ b/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 diff --git a/ArduPlane/landing.cpp b/ArduPlane/landing.cpp index e3a048f28e..3751f6cdb4 100644 --- a/ArduPlane/landing.cpp +++ b/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(); + } } /* diff --git a/ArduPlane/radio.cpp b/ArduPlane/radio.cpp index dafa8cce73..a79b01bc71 100644 --- a/ArduPlane/radio.cpp +++ b/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());