/*
check if we are flying as a tailsitter
*/
bool QuadPlane::tailsitter_active(void)
{
if (!is_tailsitter()) {
return false;
}
if (in_vtol_mode()) {
return true;
}
// check if we are in ANGLE_WAIT fixed wing transition
if (transition_state == TRANSITION_ANGLE_WAIT_FW) {
return true;
}
return false;
}
/*
update motor tilt for binary tilt servos
*/
void QuadPlane::tiltrotor_binary_update(void)
{
// motors always active
tilt.motors_active = true;
if (!in_vtol_mode()) {
// we are in pure fixed wing mode. Move the tiltable motors
// all the way forward and run them as a forward motor
tiltrotor_binary_slew(true);
float new_throttle = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle)*0.01f;
if (tilt.current_tilt >= 1) {
uint8_t mask = is_zero(new_throttle)?0:(uint8_t)tilt.tilt_mask.get();
// the motors are all the way forward, start using them for fwd thrust
motors->output_motor_mask(new_throttle, mask, plane.rudder_dt);
}
} else {
tiltrotor_binary_slew(false);
}
}
/*
calculate maximum tilt change as a proportion from 0 to 1 of tilt
*/
float QuadPlane::tilt_max_change(bool up)
{
float rate;
if (up || tilt.max_rate_down_dps <= 0) {
rate = tilt.max_rate_up_dps;
} else {
rate = tilt.max_rate_down_dps;
}
if (tilt.tilt_type != TILT_TYPE_BINARY && !up) {
bool fast_tilt = false;
if (plane.control_mode == &plane.mode_manual) {
fast_tilt = true;
}
if (hal.util->get_soft_armed() && !in_vtol_mode() && !assisted_flight) {
fast_tilt = true;
}
if (fast_tilt) {
// allow a minimum of 90 DPS in manual or if we are not
// stabilising, to give fast control
rate = MAX(rate, 90);
}
}
return rate * plane.G_Dt / 90.0f;
}
/*
check if we are flying as a tailsitter
*/
bool QuadPlane::tailsitter_active(void)
{
return is_tailsitter() && in_vtol_mode();
}
/*
return true if we are a tailsitter transitioning to VTOL flight
*/
bool QuadPlane::in_tailsitter_vtol_transition(void) const
{
return is_tailsitter() && in_vtol_mode() && transition_state == TRANSITION_ANGLE_WAIT_VTOL;
}
/*
update motor tilt for continuous tilt servos
*/
void QuadPlane::tiltrotor_continuous_update(void)
{
// default to inactive
tilt.motors_active = false;
// the maximum rate of throttle change
float max_change;
if (!in_vtol_mode() && (!hal.util->get_soft_armed() || !assisted_flight)) {
// we are in pure fixed wing mode. Move the tiltable motors all the way forward and run them as
// a forward motor
tiltrotor_slew(1);
max_change = tilt_max_change(false);
float new_throttle = constrain_float(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle)*0.01, 0, 1);
if (tilt.current_tilt < 1) {
tilt.current_throttle = constrain_float(new_throttle,
tilt.current_throttle-max_change,
tilt.current_throttle+max_change);
} else {
tilt.current_throttle = new_throttle;
}
if (!hal.util->get_soft_armed()) {
tilt.current_throttle = 0;
} else {
// the motors are all the way forward, start using them for fwd thrust
uint8_t mask = is_zero(tilt.current_throttle)?0:(uint8_t)tilt.tilt_mask.get();
motors->output_motor_mask(tilt.current_throttle, mask, plane.rudder_dt);
// prevent motor shutdown
tilt.motors_active = true;
}
return;
}
// remember the throttle level we're using for VTOL flight
float motors_throttle = motors->get_throttle();
max_change = tilt_max_change(motors_throttle<tilt.current_throttle);
tilt.current_throttle = constrain_float(motors_throttle,
tilt.current_throttle-max_change,
tilt.current_throttle+max_change);
/*
we are in a VTOL mode. We need to work out how much tilt is
needed. There are 3 strategies we will use:
1) in QSTABILIZE or QHOVER the angle will be set to zero. This
enables these modes to be used as a safe recovery mode.
2) in fixed wing assisted flight or velocity controlled modes we
will set the angle based on the demanded forward throttle,
with a maximum tilt given by Q_TILT_MAX. This relies on
Q_VFWD_GAIN being set
3) if we are in TRANSITION_TIMER mode then we are transitioning
to forward flight and should put the rotors all the way forward
*/
if (plane.control_mode == &plane.mode_qstabilize ||
plane.control_mode == &plane.mode_qhover ||
plane.control_mode == &plane.mode_qautotune) {
tiltrotor_slew(0);
return;
}
if (assisted_flight &&
transition_state >= TRANSITION_TIMER) {
// we are transitioning to fixed wing - tilt the motors all
// the way forward
tiltrotor_slew(1);
} else {
// until we have completed the transition we limit the tilt to
// Q_TILT_MAX. Anything above 50% throttle gets
// Q_TILT_MAX. Below 50% throttle we decrease linearly. This
// relies heavily on Q_VFWD_GAIN being set appropriately.
float settilt = constrain_float(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) / 50.0f, 0, 1);
tiltrotor_slew(settilt * tilt.max_angle_deg / 90.0f);
}
}