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:
Andrew Tridgell 2017-05-05 07:34:27 +10:00
parent 1f847132d7
commit 6eaad59115
3 changed files with 24 additions and 2 deletions

View File

@ -386,6 +386,13 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = {
// @Description: This is the angle of the tilt servos when in VTOL mode and at minimum output. This needs to be set for Q_TILT_TYPE=3 to enable vectored control for yaw of tricopter tilt quadplanes.
// @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
};

View File

@ -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

View File

@ -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);
}