Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
This commit is contained in:
Tom Pittenger 2016-01-29 23:22:21 -08:00 committed by Andrew Tridgell
parent 5ba2bff85f
commit 2e92089ce6
9 changed files with 182 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduPlane/ArduPlane.cpp
View File

@ -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;

111
ArduPlane/Attitude.cpp
View File

@ -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) {

35
ArduPlane/Parameters.cpp
View File

@ -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.

7
ArduPlane/Parameters.h
View File

@ -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;

4
ArduPlane/Plane.h
View File

@ -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);

1
ArduPlane/commands_logic.cpp
View File

@ -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

16
ArduPlane/defines.h
View File

@ -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

12
ArduPlane/landing.cpp
View File

@ -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();
}
}
/*

8
ArduPlane/radio.cpp
View File

@ -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());