2017-02-11 01:50:03 -04:00
|
|
|
/*
|
|
|
|
This program is free software: you can redistribute it and/or modify
|
|
|
|
it under the terms of the GNU General Public License as published by
|
|
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
|
|
(at your option) any later version.
|
|
|
|
|
|
|
|
This program is distributed in the hope that it will be useful,
|
|
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
|
|
GNU General Public License for more details.
|
|
|
|
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
|
|
*/
|
|
|
|
/*
|
|
|
|
control code for tailsitters. Enabled by setting Q_FRAME_CLASS=10
|
2019-01-17 13:54:15 -04:00
|
|
|
or by setting Q_TAILSIT_MOTMX nonzero and Q_FRAME_CLASS and Q_FRAME_TYPE
|
|
|
|
to a configuration supported by AP_MotorsMatrix
|
2017-02-11 01:50:03 -04:00
|
|
|
*/
|
|
|
|
|
2019-04-25 16:39:27 -03:00
|
|
|
#include <math.h>
|
2017-02-11 01:50:03 -04:00
|
|
|
#include "Plane.h"
|
|
|
|
|
|
|
|
/*
|
|
|
|
return true when flying a tailsitter
|
|
|
|
*/
|
2017-10-30 01:19:38 -03:00
|
|
|
bool QuadPlane::is_tailsitter(void) const
|
2017-02-11 01:50:03 -04:00
|
|
|
{
|
2020-01-02 15:07:09 -04:00
|
|
|
return available()
|
2019-03-18 21:00:25 -03:00
|
|
|
&& ((frame_class == AP_Motors::MOTOR_FRAME_TAILSITTER) || (tailsitter.motor_mask != 0))
|
|
|
|
&& (tilt.tilt_type != TILT_TYPE_BICOPTER);
|
2017-02-11 01:50:03 -04:00
|
|
|
}
|
|
|
|
|
2020-01-02 15:07:09 -04:00
|
|
|
/*
|
2020-09-02 23:45:52 -03:00
|
|
|
return true when flying a control surface only tailsitter
|
2020-01-02 15:07:09 -04:00
|
|
|
*/
|
2020-09-02 23:45:52 -03:00
|
|
|
bool QuadPlane::is_control_surface_tailsitter(void) const
|
2020-01-02 15:07:09 -04:00
|
|
|
{
|
|
|
|
return frame_class == AP_Motors::MOTOR_FRAME_TAILSITTER
|
|
|
|
&& ( is_zero(tailsitter.vectored_hover_gain) || !SRV_Channels::function_assigned(SRV_Channel::k_tiltMotorLeft));
|
|
|
|
}
|
|
|
|
|
2017-02-11 04:12:56 -04:00
|
|
|
/*
|
|
|
|
check if we are flying as a tailsitter
|
|
|
|
*/
|
|
|
|
bool QuadPlane::tailsitter_active(void)
|
|
|
|
{
|
2017-10-29 18:47:04 -03:00
|
|
|
if (!is_tailsitter()) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
if (in_vtol_mode()) {
|
|
|
|
return true;
|
|
|
|
}
|
2017-10-30 01:19:38 -03:00
|
|
|
// check if we are in ANGLE_WAIT fixed wing transition
|
|
|
|
if (transition_state == TRANSITION_ANGLE_WAIT_FW) {
|
2017-10-29 18:47:04 -03:00
|
|
|
return true;
|
|
|
|
}
|
|
|
|
return false;
|
2017-02-11 04:12:56 -04:00
|
|
|
}
|
|
|
|
|
2017-02-11 01:50:03 -04:00
|
|
|
/*
|
|
|
|
run output for tailsitters
|
|
|
|
*/
|
|
|
|
void QuadPlane::tailsitter_output(void)
|
|
|
|
{
|
2020-01-19 12:46:00 -04:00
|
|
|
if (!is_tailsitter() || motor_test.running) {
|
|
|
|
// if motor test is running we don't want to overwrite it with output_motor_mask or motors_output
|
2017-04-22 22:35:25 -03:00
|
|
|
return;
|
|
|
|
}
|
2018-10-21 14:53:09 -03:00
|
|
|
|
|
|
|
float tilt_left = 0.0f;
|
|
|
|
float tilt_right = 0.0f;
|
|
|
|
|
2019-01-17 13:54:15 -04:00
|
|
|
|
2020-02-17 20:17:50 -04:00
|
|
|
|
|
|
|
// handle forward flight modes and transition to VTOL modes
|
|
|
|
if (!tailsitter_active() || in_tailsitter_vtol_transition()) {
|
2019-01-17 13:54:15 -04:00
|
|
|
// get FW controller throttle demand and mask of motors enabled during forward flight
|
|
|
|
float throttle = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle);
|
2020-02-17 20:17:50 -04:00
|
|
|
if (hal.util->get_soft_armed() && in_tailsitter_vtol_transition() && !throttle_wait && is_flying()) {
|
|
|
|
/*
|
|
|
|
during transitions to vtol mode set the throttle to
|
|
|
|
hover thrust, center the rudder and set the altitude controller
|
|
|
|
integrator to the same throttle level
|
2020-11-13 20:28:42 -04:00
|
|
|
convert the hover throttle to the same output that would result if used via AP_Motors
|
|
|
|
apply expo, battery scaling and SPIN min/max.
|
2020-02-17 20:17:50 -04:00
|
|
|
*/
|
2020-11-13 20:28:42 -04:00
|
|
|
throttle = motors->thrust_to_actuator(motors->get_throttle_hover()) * 100;
|
2020-11-12 17:57:34 -04:00
|
|
|
throttle = MAX(throttle,plane.aparm.throttle_cruise.get());
|
2020-11-13 20:28:42 -04:00
|
|
|
|
2020-02-17 20:17:50 -04:00
|
|
|
SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, 0);
|
|
|
|
pos_control->get_accel_z_pid().set_integrator(throttle*10);
|
|
|
|
|
|
|
|
// override AP_MotorsTailsitter throttles during back transition
|
2020-11-13 20:28:42 -04:00
|
|
|
|
|
|
|
// apply PWM min and MAX to throttle left and right, just as via AP_Motors
|
|
|
|
uint16_t throttle_pwm = motors->get_pwm_output_min() + (motors->get_pwm_output_max() - motors->get_pwm_output_min()) * throttle * 0.01f;
|
|
|
|
SRV_Channels::set_output_pwm(SRV_Channel::k_throttleLeft, throttle_pwm);
|
|
|
|
SRV_Channels::set_output_pwm(SRV_Channel::k_throttleRight, throttle_pwm);
|
|
|
|
|
|
|
|
// throttle output is not used by AP_Motors so might have diffrent PWM range, set scaled
|
2020-02-17 20:17:50 -04:00
|
|
|
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, throttle);
|
|
|
|
}
|
|
|
|
|
|
|
|
if (!assisted_flight) {
|
|
|
|
// set AP_MotorsMatrix throttles for forward flight
|
|
|
|
motors->output_motor_mask(throttle * 0.01f, tailsitter.motor_mask, plane.rudder_dt);
|
|
|
|
|
|
|
|
// in forward flight: set motor tilt servos and throttles using FW controller
|
|
|
|
if (tailsitter.vectored_forward_gain > 0) {
|
2021-01-03 16:43:53 -04:00
|
|
|
// 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());
|
2020-02-17 20:17:50 -04:00
|
|
|
// 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);
|
2021-01-03 16:43:53 -04:00
|
|
|
tilt_left = (elevator + aileron) * tailsitter.vectored_forward_gain * scaler;
|
|
|
|
tilt_right = (elevator - aileron) * tailsitter.vectored_forward_gain * scaler;
|
2019-01-17 13:54:15 -04:00
|
|
|
}
|
2020-02-17 20:17:50 -04:00
|
|
|
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, tilt_left);
|
|
|
|
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, tilt_right);
|
|
|
|
return;
|
2017-10-30 01:19:38 -03:00
|
|
|
}
|
2017-02-24 03:02:34 -04:00
|
|
|
}
|
2019-03-10 18:11:07 -03:00
|
|
|
|
2020-05-26 13:43:05 -03:00
|
|
|
// handle Copter controller
|
2019-04-25 16:39:27 -03:00
|
|
|
// the MultiCopter rate controller has already been run in an earlier call
|
2020-01-02 15:07:09 -04:00
|
|
|
// to motors_output() from quadplane.update(), unless we are in assisted flight
|
2020-11-16 13:28:31 -04:00
|
|
|
// tailsitter in TRANSITION_ANGLE_WAIT_FW is not really in assisted flight, its still in a VTOL mode
|
|
|
|
if (assisted_flight && (transition_state != TRANSITION_ANGLE_WAIT_FW)) {
|
2020-01-02 15:07:09 -04:00
|
|
|
hold_stabilize(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) * 0.01f);
|
|
|
|
motors_output(true);
|
2020-05-26 13:43:05 -03:00
|
|
|
|
|
|
|
if ((options & OPTION_TAILSIT_Q_ASSIST_MOTORS_ONLY) != 0) {
|
|
|
|
// only use motors for Q assist, control surfaces remain under plane control
|
|
|
|
// zero copter I terms and use plane
|
|
|
|
attitude_control->reset_rate_controller_I_terms();
|
|
|
|
|
|
|
|
// output tilt motors
|
2020-11-09 20:43:07 -04:00
|
|
|
tilt_left = 0.0f;
|
|
|
|
tilt_right = 0.0f;
|
2020-05-26 13:43:05 -03:00
|
|
|
if (tailsitter.vectored_hover_gain > 0) {
|
2020-11-06 14:41:02 -04:00
|
|
|
const float hover_throttle = motors->get_throttle_hover();
|
|
|
|
const float throttle = motors->get_throttle();
|
|
|
|
float throttle_scaler = tailsitter.throttle_scale_max;
|
|
|
|
if (is_positive(throttle)) {
|
|
|
|
throttle_scaler = constrain_float(hover_throttle / throttle, tailsitter.gain_scaling_min, tailsitter.throttle_scale_max);
|
|
|
|
}
|
2020-11-09 20:43:07 -04:00
|
|
|
tilt_left = SRV_Channels::get_output_scaled(SRV_Channel::k_tiltMotorLeft) * tailsitter.vectored_hover_gain * throttle_scaler;
|
|
|
|
tilt_right = SRV_Channels::get_output_scaled(SRV_Channel::k_tiltMotorRight) * tailsitter.vectored_hover_gain * throttle_scaler;
|
2020-05-26 13:43:05 -03:00
|
|
|
}
|
2020-11-09 20:43:07 -04:00
|
|
|
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, tilt_left);
|
|
|
|
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, tilt_right);
|
|
|
|
|
2020-05-26 13:43:05 -03:00
|
|
|
|
|
|
|
// skip remainder of the function that overwrites plane control surface outputs with copter
|
|
|
|
return;
|
|
|
|
}
|
2020-01-02 15:07:09 -04:00
|
|
|
} else {
|
|
|
|
motors_output(false);
|
|
|
|
}
|
|
|
|
|
2020-05-26 13:43:05 -03:00
|
|
|
// In full Q assist it is better to use cotper I and zero plane
|
2017-02-24 03:02:34 -04:00
|
|
|
plane.pitchController.reset_I();
|
|
|
|
plane.rollController.reset_I();
|
|
|
|
|
2019-01-31 16:40:19 -04:00
|
|
|
// pull in copter control outputs
|
|
|
|
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, (motors->get_yaw())*-SERVO_MAX);
|
|
|
|
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, (motors->get_pitch())*SERVO_MAX);
|
|
|
|
SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, (motors->get_roll())*SERVO_MAX);
|
|
|
|
|
2017-11-05 02:05:08 -04:00
|
|
|
if (hal.util->get_soft_armed()) {
|
2021-03-12 22:51:35 -04:00
|
|
|
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, motors->thrust_to_actuator(motors->get_throttle()) * 100);
|
2017-11-05 02:05:08 -04:00
|
|
|
// scale surfaces for throttle
|
|
|
|
tailsitter_speed_scaling();
|
2021-03-14 23:36:14 -03:00
|
|
|
} else {
|
|
|
|
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, motors->get_throttle() * 100);
|
2017-11-05 02:05:08 -04:00
|
|
|
}
|
|
|
|
|
2020-11-09 20:43:07 -04:00
|
|
|
tilt_left = 0.0f;
|
|
|
|
tilt_right = 0.0f;
|
2017-04-09 21:01:58 -03:00
|
|
|
if (tailsitter.vectored_hover_gain > 0) {
|
|
|
|
// thrust vectoring VTOL modes
|
2018-10-21 14:53:09 -03:00
|
|
|
tilt_left = SRV_Channels::get_output_scaled(SRV_Channel::k_tiltMotorLeft);
|
|
|
|
tilt_right = SRV_Channels::get_output_scaled(SRV_Channel::k_tiltMotorRight);
|
2017-05-04 18:34:27 -03:00
|
|
|
/*
|
|
|
|
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
|
|
|
|
*/
|
2019-04-25 16:39:27 -03:00
|
|
|
float des_pitch_cd = attitude_control->get_att_target_euler_cd().y;
|
|
|
|
int32_t pitch_error_cd = (des_pitch_cd - ahrs_view->pitch_sensor) * 0.5;
|
2019-01-31 16:40:19 -04:00
|
|
|
float extra_pitch = constrain_float(pitch_error_cd, -SERVO_MAX, SERVO_MAX) / SERVO_MAX;
|
2017-05-04 18:34:27 -03:00
|
|
|
float extra_sign = extra_pitch > 0?1:-1;
|
2019-04-19 12:12:00 -03:00
|
|
|
float extra_elevator = 0;
|
2019-04-25 16:39:27 -03:00
|
|
|
if (!is_zero(extra_pitch) && in_vtol_mode()) {
|
2019-04-19 12:12:00 -03:00
|
|
|
extra_elevator = extra_sign * powf(fabsf(extra_pitch), tailsitter.vectored_hover_power) * SERVO_MAX;
|
|
|
|
}
|
2018-10-21 14:53:09 -03:00
|
|
|
tilt_left = extra_elevator + tilt_left * tailsitter.vectored_hover_gain;
|
|
|
|
tilt_right = extra_elevator + tilt_right * tailsitter.vectored_hover_gain;
|
2017-04-09 21:01:58 -03:00
|
|
|
}
|
2020-11-09 20:43:07 -04:00
|
|
|
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, tilt_left);
|
|
|
|
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, tilt_right);
|
2018-10-21 14:53:09 -03:00
|
|
|
|
2020-11-05 15:02:57 -04:00
|
|
|
// 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;
|
|
|
|
}
|
2018-10-21 14:53:09 -03:00
|
|
|
|
2017-02-24 03:02:34 -04:00
|
|
|
if (tailsitter.input_mask_chan > 0 &&
|
|
|
|
tailsitter.input_mask > 0 &&
|
2021-02-03 13:21:39 -04:00
|
|
|
RC_Channels::get_radio_in(tailsitter.input_mask_chan-1) > RC_Channel::AUX_PWM_TRIGGER_HIGH) {
|
2017-02-24 03:02:34 -04:00
|
|
|
// the user is learning to prop-hang
|
|
|
|
if (tailsitter.input_mask & TAILSITTER_MASK_AILERON) {
|
|
|
|
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, plane.channel_roll->get_control_in_zero_dz());
|
|
|
|
}
|
|
|
|
if (tailsitter.input_mask & TAILSITTER_MASK_ELEVATOR) {
|
|
|
|
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, plane.channel_pitch->get_control_in_zero_dz());
|
|
|
|
}
|
|
|
|
if (tailsitter.input_mask & TAILSITTER_MASK_THROTTLE) {
|
2018-11-09 18:38:43 -04:00
|
|
|
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.get_throttle_input(true));
|
2017-02-24 03:02:34 -04:00
|
|
|
}
|
|
|
|
if (tailsitter.input_mask & TAILSITTER_MASK_RUDDER) {
|
|
|
|
SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, plane.channel_rudder->get_control_in_zero_dz());
|
|
|
|
}
|
2017-02-11 01:50:03 -04:00
|
|
|
}
|
|
|
|
}
|
2017-02-11 04:12:56 -04:00
|
|
|
|
2017-02-12 05:58:00 -04:00
|
|
|
|
|
|
|
/*
|
|
|
|
return true when we have completed enough of a transition to switch to fixed wing control
|
|
|
|
*/
|
2017-10-30 01:19:38 -03:00
|
|
|
bool QuadPlane::tailsitter_transition_fw_complete(void)
|
2017-02-12 05:58:00 -04:00
|
|
|
{
|
|
|
|
if (plane.fly_inverted()) {
|
|
|
|
// transition immediately
|
|
|
|
return true;
|
|
|
|
}
|
2017-10-30 01:19:38 -03:00
|
|
|
int32_t roll_cd = labs(ahrs_view->roll_sensor);
|
|
|
|
if (roll_cd > 9000) {
|
|
|
|
roll_cd = 18000 - roll_cd;
|
|
|
|
}
|
2017-02-24 01:47:09 -04:00
|
|
|
if (labs(ahrs_view->pitch_sensor) > tailsitter.transition_angle*100 ||
|
2017-10-30 01:19:38 -03:00
|
|
|
roll_cd > tailsitter.transition_angle*100 ||
|
2017-11-05 06:25:37 -04:00
|
|
|
AP_HAL::millis() - transition_start_ms > uint32_t(transition_time_ms)) {
|
2017-10-30 01:19:38 -03:00
|
|
|
return true;
|
|
|
|
}
|
|
|
|
// still waiting
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
return true when we have completed enough of a transition to switch to VTOL control
|
|
|
|
*/
|
|
|
|
bool QuadPlane::tailsitter_transition_vtol_complete(void) const
|
|
|
|
{
|
|
|
|
if (plane.fly_inverted()) {
|
|
|
|
// transition immediately
|
|
|
|
return true;
|
|
|
|
}
|
2020-12-01 11:10:40 -04:00
|
|
|
// for vectored tailsitters at zero pilot throttle
|
|
|
|
if ((plane.quadplane.get_pilot_throttle() < .05f) && plane.quadplane._is_vectored) {
|
|
|
|
// if we are not moving (hence on the ground?) or don't know
|
|
|
|
// transition immediately to tilt motors up and prevent prop strikes
|
|
|
|
if (ahrs.groundspeed() < 1.0f) {
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
}
|
2017-10-30 01:19:38 -03:00
|
|
|
if (labs(plane.ahrs.pitch_sensor) > tailsitter.transition_angle*100 ||
|
|
|
|
labs(plane.ahrs.roll_sensor) > tailsitter.transition_angle*100 ||
|
2017-02-24 01:47:09 -04:00
|
|
|
AP_HAL::millis() - transition_start_ms > 2000) {
|
2017-02-12 05:58:00 -04:00
|
|
|
return true;
|
|
|
|
}
|
|
|
|
// still waiting
|
2017-11-05 02:25:00 -04:00
|
|
|
attitude_control->reset_rate_controller_I_terms();
|
2017-02-12 05:58:00 -04:00
|
|
|
return false;
|
|
|
|
}
|
2017-02-24 01:47:09 -04:00
|
|
|
|
|
|
|
// handle different tailsitter input types
|
|
|
|
void QuadPlane::tailsitter_check_input(void)
|
|
|
|
{
|
|
|
|
if (tailsitter_active() &&
|
2019-12-01 21:11:06 -04:00
|
|
|
(tailsitter.input_type & TAILSITTER_INPUT_PLANE)) {
|
2017-02-24 01:47:09 -04:00
|
|
|
// the user has asked for body frame controls when tailsitter
|
|
|
|
// is active. We switch around the control_in value for the
|
|
|
|
// channels to do this, as that ensures the value is
|
|
|
|
// consistent throughout the code
|
|
|
|
int16_t roll_in = plane.channel_roll->get_control_in();
|
|
|
|
int16_t yaw_in = plane.channel_rudder->get_control_in();
|
|
|
|
plane.channel_roll->set_control_in(yaw_in);
|
|
|
|
plane.channel_rudder->set_control_in(-roll_in);
|
|
|
|
}
|
|
|
|
}
|
2017-10-30 01:19:38 -03:00
|
|
|
|
|
|
|
/*
|
2019-02-26 00:14:04 -04:00
|
|
|
return true if we are a tailsitter transitioning to VTOL flight
|
2017-10-30 01:19:38 -03:00
|
|
|
*/
|
2020-10-30 20:26:00 -03:00
|
|
|
bool QuadPlane::in_tailsitter_vtol_transition(uint32_t now) const
|
2017-10-30 01:19:38 -03:00
|
|
|
{
|
2020-10-30 20:26:00 -03:00
|
|
|
if (!is_tailsitter() || !in_vtol_mode()) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
if (transition_state == TRANSITION_ANGLE_WAIT_VTOL) {
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
if ((now != 0) && ((now - last_vtol_mode_ms) > 1000)) {
|
|
|
|
// only just come out of forward flight
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
return false;
|
2017-10-30 01:19:38 -03:00
|
|
|
}
|
2017-11-05 02:05:08 -04:00
|
|
|
|
2020-10-13 13:59:30 -03:00
|
|
|
/*
|
|
|
|
return true if we are a tailsitter in FW flight
|
|
|
|
*/
|
|
|
|
bool QuadPlane::is_tailsitter_in_fw_flight(void) const
|
|
|
|
{
|
|
|
|
return is_tailsitter() && !in_vtol_mode() && transition_state == TRANSITION_DONE;
|
|
|
|
}
|
|
|
|
|
2017-11-05 02:05:08 -04:00
|
|
|
/*
|
2019-02-26 00:14:04 -04:00
|
|
|
account for speed scaling of control surfaces in VTOL modes
|
2017-11-05 02:05:08 -04:00
|
|
|
*/
|
|
|
|
void QuadPlane::tailsitter_speed_scaling(void)
|
|
|
|
{
|
|
|
|
const float hover_throttle = motors->get_throttle_hover();
|
|
|
|
const float throttle = motors->get_throttle();
|
2019-04-25 16:39:27 -03:00
|
|
|
float spd_scaler = 1.0f;
|
2018-04-15 08:31:53 -03:00
|
|
|
|
2020-11-03 19:52:08 -04:00
|
|
|
// Scaleing with throttle
|
|
|
|
float throttle_scaler = tailsitter.throttle_scale_max;
|
|
|
|
if (is_positive(throttle)) {
|
|
|
|
throttle_scaler = constrain_float(hover_throttle / throttle, tailsitter.gain_scaling_min, tailsitter.throttle_scale_max);
|
|
|
|
}
|
|
|
|
|
|
|
|
if ((tailsitter.gain_scaling_mask & TAILSITTER_GSCL_ATT_THR) != 0) {
|
2019-02-26 00:14:04 -04:00
|
|
|
// reduce gains when flying at high speed in Q modes:
|
|
|
|
|
|
|
|
// critical parameter: violent oscillations if too high
|
|
|
|
// sudden loss of attitude control if too low
|
2019-04-25 16:39:27 -03:00
|
|
|
const float min_scale = tailsitter.gain_scaling_min;
|
2019-02-26 00:14:04 -04:00
|
|
|
float tthr = 1.25f * hover_throttle;
|
2019-04-25 16:39:27 -03:00
|
|
|
|
|
|
|
// reduce control surface throws at large tilt
|
|
|
|
// angles (assuming high airspeed)
|
|
|
|
// ramp down from 1 to max_atten at tilt angles over trans_angle
|
|
|
|
// (angles here are represented by their cosines)
|
|
|
|
|
|
|
|
// Note that the cosf call will be necessary if trans_angle becomes a parameter
|
|
|
|
// but the C language spec does not guarantee that trig functions can be used
|
|
|
|
// in constant expressions, even though gcc currently allows it.
|
|
|
|
constexpr float c_trans_angle = 0.9238795; // cosf(.125f * M_PI)
|
|
|
|
|
|
|
|
// alpha = (1 - max_atten) / (c_trans_angle - cosf(radians(90)));
|
|
|
|
const float alpha = (1 - min_scale) / c_trans_angle;
|
|
|
|
const float beta = 1 - alpha * c_trans_angle;
|
|
|
|
|
|
|
|
const float c_tilt = ahrs_view->get_rotation_body_to_ned().c.z;
|
|
|
|
if (c_tilt < c_trans_angle) {
|
|
|
|
spd_scaler = constrain_float(beta + alpha * c_tilt, min_scale, 1.0f);
|
|
|
|
// reduce throttle attenuation threshold too
|
|
|
|
tthr = 0.5f * hover_throttle;
|
2019-02-26 00:14:04 -04:00
|
|
|
}
|
|
|
|
// if throttle is above hover thrust, apply additional attenuation
|
|
|
|
if (throttle > tthr) {
|
|
|
|
const float throttle_atten = 1 - (throttle - tthr) / (1 - tthr);
|
|
|
|
spd_scaler *= throttle_atten;
|
2019-04-25 16:39:27 -03:00
|
|
|
spd_scaler = constrain_float(spd_scaler, min_scale, 1.0f);
|
2019-02-26 00:14:04 -04:00
|
|
|
}
|
2019-04-25 16:39:27 -03:00
|
|
|
|
|
|
|
// limit positive and negative slew rates of applied speed scaling
|
|
|
|
constexpr float posTC = 2.0f; // seconds
|
|
|
|
constexpr float negTC = 1.0f; // seconds
|
|
|
|
const float posdelta = plane.G_Dt / posTC;
|
|
|
|
const float negdelta = plane.G_Dt / negTC;
|
|
|
|
spd_scaler = constrain_float(spd_scaler, last_spd_scaler - negdelta, last_spd_scaler + posdelta);
|
|
|
|
last_spd_scaler = spd_scaler;
|
|
|
|
|
2020-11-03 19:52:08 -04:00
|
|
|
// also apply throttle scaling if enabled
|
|
|
|
if ((spd_scaler >= 1.0f) && ((tailsitter.gain_scaling_mask & TAILSITTER_GSCL_THROTTLE) != 0)) {
|
|
|
|
spd_scaler = MAX(throttle_scaler,1.0f);
|
|
|
|
}
|
|
|
|
|
|
|
|
} else if (((tailsitter.gain_scaling_mask & TAILSITTER_GSCL_DISK_THEORY) != 0) && is_positive(tailsitter.disk_loading.get())) {
|
|
|
|
// Use disk theory to estimate the velocity over the control surfaces
|
|
|
|
// https://web.mit.edu/16.unified/www/FALL/thermodynamics/notes/node86.html
|
|
|
|
|
|
|
|
float airspeed;
|
|
|
|
if (!ahrs.airspeed_estimate(airspeed)) {
|
|
|
|
// No airspeed estimate, use throttle scaling
|
|
|
|
spd_scaler = throttle_scaler;
|
|
|
|
|
2019-02-26 00:14:04 -04:00
|
|
|
} else {
|
2020-11-03 19:52:08 -04:00
|
|
|
|
|
|
|
|
|
|
|
// use the equation: T = 0.5 * rho * A (Ue^2 - U0^2) solved for Ue^2:
|
|
|
|
// Ue^2 = (T / (0.5 * rho *A)) + U0^2
|
|
|
|
// We don't know thrust or disk area, use T = (throttle/throttle_hover) * weight
|
|
|
|
// ((t / t_h ) * weight) / (0.5 * rho * A) = ((t / t_h) * mass * 9.81) / (0.5 * rho * A)
|
|
|
|
// (mass / A) is disk loading DL so:
|
|
|
|
// Ue^2 = (((t / t_h) * DL * 9.81)/(0.5 * rho)) + U0^2
|
|
|
|
|
|
|
|
const float rho = SSL_AIR_DENSITY * plane.barometer.get_air_density_ratio();
|
|
|
|
float hover_rho = rho;
|
|
|
|
if ((tailsitter.gain_scaling_mask & TAILSITTER_GSCL_ALTITUDE) != 0) {
|
|
|
|
// if applying altitude correction use sea level density for hover case
|
|
|
|
hover_rho = SSL_AIR_DENSITY;
|
|
|
|
}
|
|
|
|
|
|
|
|
// hover case: (t / t_h) = 1 and U0 = 0
|
|
|
|
const float sq_hover_outflow = (tailsitter.disk_loading.get() * GRAVITY_MSS) / (0.5f * hover_rho);
|
|
|
|
|
|
|
|
|
|
|
|
// calculate the true outflow speed
|
|
|
|
const float sq_outflow = (((throttle/hover_throttle) * tailsitter.disk_loading.get() * GRAVITY_MSS) / (0.5f * rho)) + sq(MAX(airspeed,0));
|
|
|
|
|
|
|
|
// Scale by the ratio of squared hover outflow velocity to squared actual outflow velocity
|
|
|
|
spd_scaler = tailsitter.throttle_scale_max;
|
|
|
|
if (is_positive(sq_outflow)) {
|
|
|
|
spd_scaler = constrain_float(sq_hover_outflow / sq_outflow, tailsitter.gain_scaling_min.get(), tailsitter.throttle_scale_max.get());
|
|
|
|
}
|
2019-02-26 00:14:04 -04:00
|
|
|
}
|
2020-11-03 19:52:08 -04:00
|
|
|
|
|
|
|
} else if ((tailsitter.gain_scaling_mask & TAILSITTER_GSCL_THROTTLE) != 0) {
|
|
|
|
spd_scaler = throttle_scaler;
|
|
|
|
}
|
|
|
|
|
|
|
|
if ((tailsitter.gain_scaling_mask & TAILSITTER_GSCL_ALTITUDE) != 0) {
|
|
|
|
// air density correction
|
|
|
|
spd_scaler /= plane.barometer.get_air_density_ratio();
|
2017-11-05 02:05:08 -04:00
|
|
|
}
|
2018-04-15 08:31:53 -03:00
|
|
|
|
2020-11-01 18:36:55 -04:00
|
|
|
// record for QTUN log
|
|
|
|
log_spd_scaler = spd_scaler;
|
|
|
|
|
2019-04-25 16:39:27 -03:00
|
|
|
const SRV_Channel::Aux_servo_function_t functions[] = {
|
2017-11-05 02:05:08 -04:00
|
|
|
SRV_Channel::Aux_servo_function_t::k_aileron,
|
2018-10-21 14:53:09 -03:00
|
|
|
SRV_Channel::Aux_servo_function_t::k_elevator,
|
2020-11-03 19:52:08 -04:00
|
|
|
SRV_Channel::Aux_servo_function_t::k_rudder,
|
|
|
|
SRV_Channel::Aux_servo_function_t::k_tiltMotorLeft,
|
|
|
|
SRV_Channel::Aux_servo_function_t::k_tiltMotorRight};
|
2017-11-05 02:05:08 -04:00
|
|
|
for (uint8_t i=0; i<ARRAY_SIZE(functions); i++) {
|
|
|
|
int32_t v = SRV_Channels::get_output_scaled(functions[i]);
|
2020-11-03 19:52:08 -04:00
|
|
|
if ((functions[i] == SRV_Channel::Aux_servo_function_t::k_tiltMotorLeft) || (functions[i] == SRV_Channel::Aux_servo_function_t::k_tiltMotorRight)) {
|
|
|
|
// always apply throttle scaling to tilts
|
|
|
|
v *= throttle_scaler;
|
|
|
|
} else {
|
|
|
|
v *= spd_scaler;
|
|
|
|
}
|
2017-11-05 02:05:08 -04:00
|
|
|
v = constrain_int32(v, -SERVO_MAX, SERVO_MAX);
|
|
|
|
SRV_Channels::set_output_scaled(functions[i], v);
|
|
|
|
}
|
|
|
|
}
|