mirror of https://github.com/ArduPilot/ardupilot
parent
1ccff12b54
commit
2fb8a13ac6
|
@ -1137,15 +1137,19 @@ static void update_current_flight_mode(void)
|
||||||
// Thanks to Yury MonZon for the altitude limit code!
|
// Thanks to Yury MonZon for the altitude limit code!
|
||||||
|
|
||||||
nav_roll_cd = g.channel_roll.norm_input() * g.roll_limit_cd;
|
nav_roll_cd = g.channel_roll.norm_input() * g.roll_limit_cd;
|
||||||
altitude_error_cm = g.channel_pitch.norm_input() * g.pitch_limit_min_cd;
|
|
||||||
|
|
||||||
|
float elevator_input;
|
||||||
|
elevator_input = g.channel_pitch.norm_input();
|
||||||
|
|
||||||
|
if (g.flybywire_elev_reverse) {
|
||||||
|
elevator_input = -elevator_input;
|
||||||
|
}
|
||||||
if ((current_loc.alt >= home.alt+g.FBWB_min_altitude_cm) || (g.FBWB_min_altitude_cm == 0)) {
|
if ((current_loc.alt >= home.alt+g.FBWB_min_altitude_cm) || (g.FBWB_min_altitude_cm == 0)) {
|
||||||
altitude_error_cm = g.channel_pitch.norm_input() * g.pitch_limit_min_cd;
|
altitude_error_cm = elevator_input * g.pitch_limit_min_cd;
|
||||||
} else {
|
} else {
|
||||||
if (g.channel_pitch.norm_input()<0) {
|
altitude_error_cm = (home.alt + g.FBWB_min_altitude_cm) - current_loc.alt;
|
||||||
altitude_error_cm =( (home.alt + g.FBWB_min_altitude_cm) - current_loc.alt) + g.channel_pitch.norm_input() * g.pitch_limit_min_cd;
|
if (elevator_input < 0) {
|
||||||
} else {
|
altitude_error_cm += elevator_input * g.pitch_limit_min_cd;
|
||||||
altitude_error_cm =( (home.alt + g.FBWB_min_altitude_cm) - current_loc.alt);
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
calc_throttle();
|
calc_throttle();
|
||||||
|
|
|
@ -87,6 +87,7 @@ public:
|
||||||
k_param_flybywire_airspeed_min = 120,
|
k_param_flybywire_airspeed_min = 120,
|
||||||
k_param_flybywire_airspeed_max,
|
k_param_flybywire_airspeed_max,
|
||||||
k_param_FBWB_min_altitude_cm, // 0=disabled, minimum value for altitude in cm (for first time try 30 meters = 3000 cm)
|
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,
|
||||||
|
|
||||||
//
|
//
|
||||||
// 130: Sensor parameters
|
// 130: Sensor parameters
|
||||||
|
@ -253,6 +254,7 @@ public:
|
||||||
//
|
//
|
||||||
AP_Int16 flybywire_airspeed_min;
|
AP_Int16 flybywire_airspeed_min;
|
||||||
AP_Int16 flybywire_airspeed_max;
|
AP_Int16 flybywire_airspeed_max;
|
||||||
|
AP_Int8 flybywire_elev_reverse;
|
||||||
|
|
||||||
// Throttle
|
// Throttle
|
||||||
//
|
//
|
||||||
|
|
|
@ -199,6 +199,13 @@ const AP_Param::Info var_info[] PROGMEM = {
|
||||||
// @User: Standard
|
// @User: Standard
|
||||||
GSCALAR(flybywire_airspeed_max, "ARSPD_FBW_MAX", AIRSPEED_FBW_MAX),
|
GSCALAR(flybywire_airspeed_max, "ARSPD_FBW_MAX", AIRSPEED_FBW_MAX),
|
||||||
|
|
||||||
|
// @Param: FBWB_ELEV_REV
|
||||||
|
// @DisplayName: Fly By Wire elevator reverse
|
||||||
|
// @Description: Reverse sense of elevator in FBWB. When set to 0 up elevator (pulling back on the stick) means to lower altitude. When set to 1, up elevator means to raise altitude.
|
||||||
|
// @Values: 0:Disabled,1:Enabled
|
||||||
|
// @User: Standard
|
||||||
|
GSCALAR(flybywire_elev_reverse, "FBWB_ELEV_REV", 0),
|
||||||
|
|
||||||
// @Param: THR_MIN
|
// @Param: THR_MIN
|
||||||
// @DisplayName: Minimum Throttle
|
// @DisplayName: Minimum Throttle
|
||||||
// @Description: The minimum throttle setting to which the autopilot will apply.
|
// @Description: The minimum throttle setting to which the autopilot will apply.
|
||||||
|
|
Loading…
Reference in New Issue