Browse Source

Plane: added scaling for Q_TAILSIT_VFGAIN

this scales the vectoring in fixed wing flight to remove the impact of
surface speed scaling and add throttle scaling
zr-v5.1
Andrew Tridgell 4 years ago
parent
commit
96c2a5c77c
  1. 6
      ArduPlane/tailsitter.cpp

6
ArduPlane/tailsitter.cpp

@ -108,11 +108,13 @@ void QuadPlane::tailsitter_output(void) @@ -108,11 +108,13 @@ void QuadPlane::tailsitter_output(void)
// in forward flight: set motor tilt servos and throttles using FW controller
if (tailsitter.vectored_forward_gain > 0) {
// remove scaling from surface speed scaling and apply throttle scaling
const float scaler = plane.control_mode == &plane.mode_manual?1:(tilt_throttle_scaling() / plane.get_speed_scaler());
// thrust vectoring in fixed wing flight
float aileron = SRV_Channels::get_output_scaled(SRV_Channel::k_aileron);
float elevator = SRV_Channels::get_output_scaled(SRV_Channel::k_elevator);
tilt_left = (elevator + aileron) * tailsitter.vectored_forward_gain;
tilt_right = (elevator - aileron) * tailsitter.vectored_forward_gain;
tilt_left = (elevator + aileron) * tailsitter.vectored_forward_gain * scaler;
tilt_right = (elevator - aileron) * tailsitter.vectored_forward_gain * scaler;
}
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, tilt_left);
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, tilt_right);

Loading…
Cancel
Save