Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: change FBWB altitude control algorithm

Browse Source 
this makes FBWB much less sensitive to airframe tuning. When the
elevator stick first goes neutral it locks in the current altitude as
the target altitude. When the elevator stick is off neutral, it moves
the target altitude in proportion to the elevator, at a rate goverened
by the new FBWB_CLIMB_RATE parameter

This prevents the aircraft from slowly drifting in altitude in FBWB,
and gives a more intuitive control mechanism for altitude.

Thanks to Chris Miser from Falcon UAV for help in designing this
change
This commit is contained in:
Andrew Tridgell 2013-03-28 10:27:25 +11:00
parent fcde3ceb24
commit 7db7d7db77
4 changed files with 37 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
ArduPlane/ArduPlane.pde
View File

@ -1160,7 +1160,8 @@ static void update_current_flight_mode(void)
break;
}
case FLY_BY_WIRE_B:
case FLY_BY_WIRE_B: {
static float last_elevator_input;
// Substitute stick inputs for Navigation control output
// We use g.pitch_limit_min because its magnitude is
// normally greater than g.pitch_limit_max
@ -1175,16 +1176,26 @@ static void update_current_flight_mode(void)
if (g.flybywire_elev_reverse) {
elevator_input = -elevator_input;
}
if ((adjusted_altitude_cm() >= home.alt+g.FBWB_min_altitude_cm) || (g.FBWB_min_altitude_cm == 0)) {
altitude_error_cm = elevator_input * g.pitch_limit_min_cd;
} else {
altitude_error_cm = (home.alt + g.FBWB_min_altitude_cm) - adjusted_altitude_cm();
if (elevator_input < 0) {
altitude_error_cm += elevator_input * g.pitch_limit_min_cd;
}
target_altitude_cm += g.flybywire_climb_rate * elevator_input * delta_ms_fast_loop * 0.1f;
if (elevator_input == 0.0f && last_elevator_input != 0.0f) {
// the user has just released the elevator, lock in
// the current altitude
target_altitude_cm = current_loc.alt;
}
// check for FBWB altitude limit
if (g.FBWB_min_altitude_cm != 0 && target_altitude_cm < home.alt + g.FBWB_min_altitude_cm) {
target_altitude_cm = home.alt + g.FBWB_min_altitude_cm;
}
altitude_error_cm = target_altitude_cm - adjusted_altitude_cm();
last_elevator_input = elevator_input;
calc_throttle();
calc_nav_pitch();
}
break;
case STABILIZE:

2
ArduPlane/Parameters.h
View File

@ -99,6 +99,7 @@ public:
k_param_FBWB_min_altitude_cm, // 0=disabled, minimum value for altitude in cm (for first time try 30 meters = 3000 cm)
k_param_flybywire_elev_reverse,
k_param_alt_control_algorithm,
k_param_flybywire_climb_rate,
//
// 130: Sensor parameters
@ -285,6 +286,7 @@ public:
AP_Int16 flybywire_airspeed_min;
AP_Int16 flybywire_airspeed_max;
AP_Int8 flybywire_elev_reverse;
AP_Int8 flybywire_climb_rate;
// Throttle
//

13
ArduPlane/Parameters.pde
View File

@ -286,6 +286,14 @@ const AP_Param::Info var_info[] PROGMEM = {
// @User: Standard
GSCALAR(flybywire_elev_reverse, "FBWB_ELEV_REV", 0),
// @Param: FBWB_CLIMB_RATE
// @DisplayName: Fly By Wire B alttitude change rate
// @Description: This sets the rate in m/s at which FBWB will change its target altitude for full elevator deflection. Note that the actual climb rate of the aircraft can be lower than this, depending on your airspeed and throttle control settings. If you have this parameter set to the default value of 2.0, then holding the elevator at maximum deflection for 10 seconds would change the target altitude by 20 meters.
// @Range: 1-10
// @Increment: 0.1
// @User: Standard
GSCALAR(flybywire_climb_rate, "FBWB_CLIMB_RATE", 2.0f),
// @Param: THR_MIN
// @DisplayName: Minimum Throttle
// @Description: The minimum throttle setting to which the autopilot will apply.
@ -549,6 +557,11 @@ const AP_Param::Info var_info[] PROGMEM = {
// @User: User
GSCALAR(RTL_altitude_cm, "ALT_HOLD_RTL", ALT_HOLD_HOME_CM),
// @Param: ALT_HOLD_FBWCM
// @DisplayName: Minimum altitude for FBWB mode
// @Description: This is the minimum altitude in centimeters that FBWB will allow. If you attempt to descend below this altitude then the plane will level off. A value of zero means no limit.
// @Units: centimeters
// @User: User
GSCALAR(FBWB_min_altitude_cm, "ALT_HOLD_FBWCM", ALT_HOLD_FBW_CM),
// @Param: MAG_ENABLE

3
ArduPlane/system.pde
View File

@ -340,7 +340,10 @@ static void set_mode(enum FlightMode mode)
case STABILIZE:
case TRAINING:
case FLY_BY_WIRE_A:
break;
case FLY_BY_WIRE_B:
target_altitude_cm = current_loc.alt;
break;
case CIRCLE: