Plane: Add options to USE_REVERSE_THRUST to cover all flight modes

This commit is contained in:
Hwurzburg 2021-05-25 12:49:38 -05:00 committed by Andrew Tridgell
parent 08b8a43ab5
commit e958306343
4 changed files with 23 additions and 6 deletions

View File

@ -209,9 +209,9 @@ const AP_Param::Info Plane::var_info[] = {
// @Param: USE_REV_THRUST
// @DisplayName: Bitmask for when to allow negative reverse thrust
// @Description: This controls when to use reverse thrust. If set to zero then reverse thrust is never used. If set to a non-zero value then the bits correspond to flight stages where reverse thrust may be used. Note that reverse thrust is only ever auto-enabled in auto-throttle modes. In modes where throttle control is pilot controlled the ability to do reverse thrust is controlled by throttle stick input. The most commonly used value for USE_REV_THRUST is 2, which means AUTO_LAND only. That enables reverse thrust in the landing stage of AUTO mode. Another common choice is 1, which means to use reverse thrust in all auto flight stages.
// @Values: 0:Never,1:AutoAlways,2:AutoLanding
// @Bitmask: 0:AUTO_ALWAYS,1:AUTO_LAND,2:AUTO_LOITER_TO_ALT,3:AUTO_LOITER_ALL,4:AUTO_WAYPOINTS,5:LOITER,6:RTL,7:CIRCLE,8:CRUISE,9:FBWB,10:GUIDED,11:AUTO_LANDING_PATTERN
// @Description: This controls when to use reverse thrust. If set to zero then reverse thrust is never used. If set to a non-zero value then the bits correspond to flight stages where reverse thrust may be used. The most commonly used value for USE_REV_THRUST is 2, which means AUTO_LAND only. That enables reverse thrust in the landing stage of AUTO mode. Another common choice is 1, which means to use reverse thrust in all auto flight stages. Reverse thrust is always used in MANUAL mode if enabled with THR_MIN < 0. In non-autothrottle controlled modes, if reverse thrust is not used, then THR_MIN is effectively set to 0 for that mode.
// @Values: 0:MANUAL ONLY,1:AutoAlways,2:AutoLanding
// @Bitmask: 0:AUTO_ALWAYS,1:AUTO_LAND,2:AUTO_LOITER_TO_ALT,3:AUTO_LOITER_ALL,4:AUTO_WAYPOINTS,5:LOITER,6:RTL,7:CIRCLE,8:CRUISE,9:FBWB,10:GUIDED,11:AUTO_LANDING_PATTERN,12:FBWA,13:ACRO,14:STABILIZE,15:THERMAL
// @User: Advanced
GSCALAR(use_reverse_thrust, "USE_REV_THRUST", USE_REVERSE_THRUST_AUTO_LAND_APPROACH),
@ -1489,6 +1489,8 @@ void Plane::load_parameters(void)
g.terrain_follow.convert_parameter_width(AP_PARAM_INT8);
#endif
g.use_reverse_thrust.convert_parameter_width(AP_PARAM_INT16);
hal.console->printf("load_all took %uus\n", (unsigned)(micros() - before));
}

View File

@ -410,7 +410,7 @@ public:
AP_Int8 throttle_fs_enabled;
AP_Int16 throttle_fs_value;
AP_Int8 throttle_nudge;
AP_Int16 use_reverse_thrust;
AP_Int32 use_reverse_thrust;
// Failsafe
AP_Int8 fs_action_short;

View File

@ -145,6 +145,10 @@ enum {
USE_REVERSE_THRUST_FBWB = (1<<9),
USE_REVERSE_THRUST_GUIDED = (1<<10),
USE_REVERSE_THRUST_AUTO_LANDING_PATTERN = (1<<11),
USE_REVERSE_THRUST_FBWA = (1<<12),
USE_REVERSE_THRUST_ACRO = (1<<13),
USE_REVERSE_THRUST_STABILIZE = (1<<14),
USE_REVERSE_THRUST_THERMAL = (1<<15),
};
enum FlightOptions {

View File

@ -90,9 +90,20 @@ bool Plane::allow_reverse_thrust(void) const
case Mode::Number::TAKEOFF:
allow = false;
break;
case Mode::Number::FLY_BY_WIRE_A:
allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_FBWA);
break;
case Mode::Number::ACRO:
allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_ACRO);
break;
case Mode::Number::STABILIZE:
allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_STABILIZE);
break;
case Mode::Number::THERMAL:
allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_THERMAL);
break;
default:
// all other control_modes are auto_throttle_mode=false.
// If we are not controlling throttle, don't limit it.
// all other control_modes allow independent of mask(MANUAL)
allow = true;
break;
}