Browse Source

Plane: fixed servo jitter due to airspeed estimate from throttle

when we have no other airspeed source and we are armed we get airspeed
from throttle. the default setting of k_throttle in the radio code
caused an oscillation of the value used in the airspeed estimate, each
time a RC frame came in.
master
Andrew Tridgell 6 years ago
parent
commit
c290b1f3b5
  1. 15
      ArduPlane/servos.cpp

15
ArduPlane/servos.cpp

@ -386,16 +386,21 @@ void Plane::set_servos_controlled(void) @@ -386,16 +386,21 @@ void Plane::set_servos_controlled(void)
// manual pass through of throttle while throttle is suppressed
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, get_throttle_input(true));
}
} else if (g.throttle_passthru_stabilize &&
(control_mode == STABILIZE ||
} else if (control_mode == STABILIZE ||
control_mode == TRAINING ||
control_mode == ACRO ||
control_mode == FLY_BY_WIRE_A ||
control_mode == AUTOTUNE) &&
!failsafe.throttle_counter) {
control_mode == AUTOTUNE) {
// a manual throttle mode
if (failsafe.throttle_counter) {
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
} else if (g.throttle_passthru_stabilize) {
// manual pass through of throttle while in FBWA or
// STABILIZE mode with THR_PASS_STAB set
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, get_throttle_input(true));
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, channel_throttle->get_control_in_zero_dz());
} else {
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, channel_throttle->get_control_in());
}
} else if ((control_mode == GUIDED || control_mode == AVOID_ADSB) &&
guided_throttle_passthru) {
// manual pass through of throttle while in GUIDED

Loading…
Cancel
Save