mirror of https://github.com/ArduPilot/ardupilot
Plane: improve vectored tailsitter takeoff
this gives extra pitch input when taking off a vectored tailsitter, allowing the motors to point straight up for easy takeoff thanks to Leonard for the idea!
This commit is contained in:
parent
1f847132d7
commit
6eaad59115
|
@ -387,6 +387,13 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = {
|
|||
// @Range: 0 30
|
||||
AP_GROUPINFO("TILT_YAW_ANGLE", 55, QuadPlane, tilt.tilt_yaw_angle, 0),
|
||||
|
||||
// @Param: TAILSIT_VHPOW
|
||||
// @DisplayName: Tailsitter vector thrust gain power
|
||||
// @Description: This controls the amount of extra pitch given to the vectored control when at high pitch errors
|
||||
// @Range: 0 4
|
||||
// @Increment: 0.1
|
||||
AP_GROUPINFO("TAILSIT_VHPOW", 56, QuadPlane, tailsitter.vectored_hover_power, 2.5),
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
|
|
|
@ -369,6 +369,7 @@ private:
|
|||
AP_Int8 input_mask_chan;
|
||||
AP_Float vectored_forward_gain;
|
||||
AP_Float vectored_hover_gain;
|
||||
AP_Float vectored_hover_power;
|
||||
} tailsitter;
|
||||
|
||||
// the attitude view of the VTOL attitude controller
|
||||
|
|
|
@ -66,8 +66,22 @@ void QuadPlane::tailsitter_output(void)
|
|||
// thrust vectoring VTOL modes
|
||||
float aileron = SRV_Channels::get_output_scaled(SRV_Channel::k_aileron);
|
||||
float elevator = SRV_Channels::get_output_scaled(SRV_Channel::k_elevator);
|
||||
float tilt_left = (elevator + aileron) * tailsitter.vectored_hover_gain;
|
||||
float tilt_right = (elevator - aileron) * tailsitter.vectored_hover_gain;
|
||||
/*
|
||||
apply extra elevator when at high pitch errors, using a
|
||||
power law. This allows the motors to point straight up for
|
||||
takeoff without integrator windup
|
||||
*/
|
||||
int32_t pitch_error_cd = (plane.nav_pitch_cd - ahrs_view->pitch_sensor) * 0.5;
|
||||
float extra_pitch = constrain_float(pitch_error_cd, -4500, 4500) / 4500.0;
|
||||
float extra_sign = extra_pitch > 0?1:-1;
|
||||
float extra_elevator = extra_sign * powf(fabsf(extra_pitch), tailsitter.vectored_hover_power) * 4500;
|
||||
float tilt_left = extra_elevator + (elevator + aileron) * tailsitter.vectored_hover_gain;
|
||||
float tilt_right = extra_elevator + (elevator - aileron) * tailsitter.vectored_hover_gain;
|
||||
if (fabsf(tilt_left) >= 4500 || fabsf(tilt_right) >= 4500) {
|
||||
// prevent integrator windup
|
||||
motors->limit.roll_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);
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue