Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: tailsitter: provide min throttle limit based on disk therory outflow velocity

Browse Source
This commit is contained in:
Peter Hall 2021-11-14 19:15:35 +00:00 committed by Andrew Tridgell
parent 0fa1ddf7c2
commit 2ffebebddc
3 changed files with 38 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduPlane/quadplane.cpp
View File

@ -635,7 +635,8 @@ bool QuadPlane::setup(void)
break;
case AP_Motors::MOTOR_FRAME_TAILSITTER:
// this is a duo-motor tailsitter
motors = new AP_MotorsTailsitter(plane.scheduler.get_loop_rate_hz(), rc_speed);
tailsitter.tailsitter_motors = new AP_MotorsTailsitter(plane.scheduler.get_loop_rate_hz(), rc_speed);
motors = tailsitter.tailsitter_motors;
motors_var_info = AP_MotorsTailsitter::var_info;
break;
case AP_Motors::MOTOR_FRAME_DYNAMIC_SCRIPTING_MATRIX:

33
ArduPlane/tailsitter.cpp
View File

@ -159,6 +159,12 @@ const AP_Param::GroupInfo Tailsitter::var_info[] = {
// @Range: 0 2
AP_GROUPINFO("VT_Y_P", 21, Tailsitter, VTOL_yaw_scale, 1),
// @Param: MIN_VO
// @DisplayName: Tailsitter Disk loading minimum outflow speed
// @Description: Use in conjunction with disk therory gain scaling and Q_TAILSIT_DSKLD to specify minumum airspeed over control surfaces, this will be used to boost throttle, when decending for example, 0 disables
// @Range: 0 15
AP_GROUPINFO("MIN_VO", 22, Tailsitter, disk_loading_min_outflow, 0),
AP_GROUPEND
};
@ -388,6 +394,8 @@ void Tailsitter::output(void)
if (hal.util->get_soft_armed()) {
// scale surfaces for throttle
speed_scaling();
} else if (tailsitter_motors != nullptr) {
tailsitter_motors->set_min_throttle(0.0);
}
tilt_left = 0.0f;
@ -584,8 +592,9 @@ int8_t Tailsitter::get_transition_angle_vtol() const
void Tailsitter::speed_scaling(void)
{
const float hover_throttle = motors->get_throttle_hover();
const float throttle = motors->get_throttle();
const float throttle = motors->get_throttle_out();
float spd_scaler = 1.0f;
float disk_loading_min_throttle = 0.0;
// Scaleing with throttle
float throttle_scaler = throttle_scale_max;
@ -679,6 +688,24 @@ void Tailsitter::speed_scaling(void)
if (is_positive(sq_outflow)) {
spd_scaler = constrain_float(sq_hover_outflow / sq_outflow, gain_scaling_min.get(), throttle_scale_max.get());
}
if (is_positive(disk_loading_min_outflow)) {
// calculate throttle required to give minimum outflow speed over control surfaces
if (is_positive(airspeed)) {
disk_loading_min_throttle = (((sq(disk_loading_min_outflow) - sq(airspeed)) * (0.5 * rho)) / (disk_loading.get() * GRAVITY_MSS)) * hover_throttle;
} else {
// estimate backwards airspeed
float reverse_airspeed = 0.0;
Vector3f vel;
if (quadplane.ahrs.get_velocity_NED(vel)) {
reverse_airspeed = quadplane.ahrs.earth_to_body(vel - quadplane.ahrs.wind_estimate()).x;
}
// make sure actually negative
reverse_airspeed = MIN(reverse_airspeed, 0.0);
disk_loading_min_throttle = (((sq(disk_loading_min_outflow) + sq(reverse_airspeed)) * (0.5 * rho)) / (disk_loading.get() * GRAVITY_MSS)) * hover_throttle;
}
disk_loading_min_throttle = MAX(disk_loading_min_throttle, 0.0);
}
}
} else if ((gain_scaling_mask & TAILSITTER_GSCL_THROTTLE) != 0) {
@ -709,6 +736,10 @@ void Tailsitter::speed_scaling(void)
}
SRV_Channels::set_output_scaled(functions[i], v);
}
if (tailsitter_motors != nullptr) {
tailsitter_motors->set_min_throttle(disk_loading_min_throttle);
}
}
// return true if pitch control should be relaxed

4
ArduPlane/tailsitter.h
View File

@ -16,6 +16,7 @@
#include <AP_Param/AP_Param.h>
#include "transition.h"
#include <AP_Motors/AP_MotorsTailsitter.h>
class QuadPlane;
class AP_MotorsMulticopter;
@ -105,6 +106,9 @@ public:
AP_Float VTOL_roll_scale;
AP_Float VTOL_pitch_scale;
AP_Float VTOL_yaw_scale;
AP_Float disk_loading_min_outflow;
AP_MotorsTailsitter* tailsitter_motors;
private: