Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: improve tiltrotor transition strategy

Browse Source 
avoid running forward motors at high throttle when transitioning to a
VTOL mode
This commit is contained in:
Andrew Tridgell 2017-03-13 21:53:22 +11:00
parent 3f7e7d456f
commit e3a8d477f5
2 changed files with 80 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
ArduPlane/quadplane.h
View File

@ -370,7 +370,12 @@ private:
void tiltrotor_update(void);
void tiltrotor_continuous_update(void);
void tiltrotor_binary_update(void);
void tilt_compensate_up(float *thrust, uint8_t num_motors);
void tilt_compensate_down(float *thrust, uint8_t num_motors);
void tilt_compensate(float *thrust, uint8_t num_motors);
bool is_motor_tilting(uint8_t motor) const {
return (((uint8_t)tilt.tilt_mask.get()) & (1U<<motor));
}
bool tiltrotor_fully_fwd(void);
float tilt_max_change(bool up);

84
ArduPlane/tiltrotor.cpp
View File

@ -198,14 +198,23 @@ void QuadPlane::tiltrotor_update(void)
By applying _tilt_equal_thrust the tilted motors effectively become
a single pitch control motor.
Note that we use a different strategy for when we are transitioning
into VTOL as compared to from VTOL flight. The reason for that is
we want to lean towards higher tilted motor throttle when
transitioning to fixed wing flight, in order to gain airspeed,
whereas when transitioning to VTOL flight we want to lean to towards
lower fwd throttle. So we raise the throttle on the tilted motors
when transitioning to fixed wing, and lower throttle on tilted
motors when transitioning to VTOL
*/
void QuadPlane::tilt_compensate(float *thrust, uint8_t num_motors)
void QuadPlane::tilt_compensate_down(float *thrust, uint8_t num_motors)
{
float tilt_factor;
float inv_tilt_factor;
if (tilt.current_tilt > 0.98f) {
tilt_factor = 1.0 / cosf(radians(0.98f*90));
inv_tilt_factor = 1.0 / cosf(radians(0.98f*90));
} else {
tilt_factor = 1.0 / cosf(radians(tilt.current_tilt*90));
inv_tilt_factor = 1.0 / cosf(radians(tilt.current_tilt*90));
}
// when we got past Q_TILT_MAX we gang the tilted motors together
@ -218,12 +227,11 @@ void QuadPlane::tilt_compensate(float *thrust, uint8_t num_motors)
float tilt_total = 0;
uint8_t tilt_count = 0;
uint8_t mask = tilt.tilt_mask;
// apply _tilt_factor first
// apply inv_tilt_factor first
for (uint8_t i=0; i<num_motors; i++) {
if (mask & (1U<<i)) {
thrust[i] *= tilt_factor;
if (is_motor_tilting(i)) {
thrust[i] *= inv_tilt_factor;
tilt_total += thrust[i];
tilt_count++;
}
@ -233,7 +241,7 @@ void QuadPlane::tilt_compensate(float *thrust, uint8_t num_motors)
// now constrain and apply _tilt_equal_thrust if enabled
for (uint8_t i=0; i<num_motors; i++) {
if (mask & (1U<<i)) {
if (is_motor_tilting(i)) {
if (equal_thrust) {
thrust[i] = tilt_total / tilt_count;
}
@ -252,6 +260,64 @@ void QuadPlane::tilt_compensate(float *thrust, uint8_t num_motors)
}
}
/*
tilt compensation when transitioning to VTOL flight
*/
void QuadPlane::tilt_compensate_up(float *thrust, uint8_t num_motors)
{
float tilt_factor = cosf(radians(tilt.current_tilt*90));
// when we got past Q_TILT_MAX we gang the tilted motors together
// to generate equal thrust. This makes them act as a single pitch
// control motor while preventing them trying to do roll and yaw
// control while angled over. This greatly improves the stability
// of the last phase of transitions
float tilt_threshold = (tilt.max_angle_deg/90.0f);
bool equal_thrust = (tilt.current_tilt > tilt_threshold);
float tilt_total = 0;
uint8_t tilt_count = 0;
// apply tilt_factor first
for (uint8_t i=0; i<num_motors; i++) {
if (!is_motor_tilting(i)) {
thrust[i] *= tilt_factor;
} else {
tilt_total += thrust[i];
tilt_count++;
}
}
// now constrain and apply _tilt_equal_thrust if enabled
for (uint8_t i=0; i<num_motors; i++) {
if (is_motor_tilting(i)) {
if (equal_thrust) {
thrust[i] = tilt_total / tilt_count;
}
}
}
}
/*
choose up or down tilt compensation based on flight mode When going
to a fixed wing mode we use tilt_compensate_down, when going to a
VTOL mode we use tilt_compensate_up
*/
void QuadPlane::tilt_compensate(float *thrust, uint8_t num_motors)
{
if (tilt.current_tilt <= 0) {
// the motors are not tilted, no compensation needed
return;
}
if (in_vtol_mode()) {
// we are transitioning to VTOL flight
tilt_compensate_up(thrust, num_motors);
} else {
tilt_compensate_down(thrust, num_motors);
}
}
/*
return true if the rotors are fully tilted forward
*/