From 09769790457a15b0d5a8ff1adc124907f0867f1d Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Thu, 5 Nov 2020 19:02:57 +0000 Subject: [PATCH] Plane: tailsitter: set motor limit flags --- ArduPlane/tailsitter.cpp | 21 +++++++++++++++------ 1 file changed, 15 insertions(+), 6 deletions(-) diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp index 6f8a0aea0c..ec4392aff6 100644 --- a/ArduPlane/tailsitter.cpp +++ b/ArduPlane/tailsitter.cpp @@ -180,16 +180,25 @@ void QuadPlane::tailsitter_output(void) } tilt_left = extra_elevator + tilt_left * tailsitter.vectored_hover_gain; tilt_right = extra_elevator + tilt_right * tailsitter.vectored_hover_gain; - if (fabsf(tilt_left) >= SERVO_MAX || fabsf(tilt_right) >= SERVO_MAX) { - // prevent integrator windup - motors->limit.roll = 1; - motors->limit.pitch = 1; - motors->limit.yaw = 1; - } SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, tilt_left); SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, tilt_right); } + // Check for saturated limits + bool tilt_lim = (labs(SRV_Channels::get_output_scaled(SRV_Channel::Aux_servo_function_t::k_tiltMotorLeft)) == SERVO_MAX) || (labs(SRV_Channels::get_output_scaled(SRV_Channel::Aux_servo_function_t::k_tiltMotorRight)) == SERVO_MAX); + bool roll_lim = labs(SRV_Channels::get_output_scaled(SRV_Channel::Aux_servo_function_t::k_rudder)) == SERVO_MAX; + bool pitch_lim = labs(SRV_Channels::get_output_scaled(SRV_Channel::Aux_servo_function_t::k_elevator)) == SERVO_MAX; + bool yaw_lim = labs(SRV_Channels::get_output_scaled(SRV_Channel::Aux_servo_function_t::k_aileron)) == SERVO_MAX; + + if (roll_lim) { + motors->limit.roll = true; + } + if (pitch_lim || tilt_lim) { + motors->limit.pitch = true; + } + if (yaw_lim || tilt_lim) { + motors->limit.yaw = true; + } if (tailsitter.input_mask_chan > 0 && tailsitter.input_mask > 0 &&