Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Plane: new tilt compensation method

This commit is contained in:
Andrew Tridgell 2016-05-09 09:11:11 +10:00
parent 401b8d4fa6
commit 6fdaaa7d98
2 changed files with 75 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduPlane/quadplane.h
View File

@ -304,6 +304,7 @@ private:
void tiltrotor_slew(float tilt);
void tiltrotor_update(void);
void tilt_compensate(float *thrust, uint8_t num_motors);
public:
void motor_test_output();

95
ArduPlane/tiltrotor.cpp
View File

@ -19,27 +19,8 @@ void QuadPlane::tiltrotor_slew(float newtilt)
// translate to 0..1000 range and output
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_motor_tilt, 1000 * tilt.current_tilt);
/*
we need to tell AP_Motors about the tilt so it can compensate
for reduced vertical thrust
*/
float tilt_factor;
if (tilt.current_tilt > 0.98f) {
tilt_factor = 1.0 / cosf(radians(0.98f*90));
} else {
tilt_factor = 1.0 / 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);
// tell motors library about the tilt
motors->set_motor_tilt_factor(tilt_factor, tilt.tilt_mask, equal_thrust);
// setup tilt compensation
motors->set_thrust_compensation_callback(FUNCTOR_BIND_MEMBER(&QuadPlane::tilt_compensate, void, float *, uint8_t));
}
/*
@ -121,3 +102,75 @@ void QuadPlane::tiltrotor_update(void)
tiltrotor_slew(settilt * tilt.max_angle_deg / 90.0f);
}
}
/*
compensate for tilt in a set of motor outputs
Compensation is of two forms. The first is to apply _tilt_factor,
which is a compensation for the reduces vertical thrust when
tilted. This is supplied by set_motor_tilt_factor().
The second compensation is to use equal thrust on all tilted motors
when _tilt_equal_thrust is true. This is used when the motors are
tilted by a large angle to prevent the roll and yaw controllers from
causing instability. Typically this would be used when the motors
are tilted beyond 45 degrees. At this angle it is assumed that roll
control can be achieved using fixed wing control surfaces and yaw
control with the remaining multicopter motors (eg. tricopter tail).
By applying _tilt_equal_thrust the tilted motors effectively become
a single pitch control motor.
*/
void QuadPlane::tilt_compensate(float *thrust, uint8_t num_motors)
{
float tilt_factor;
if (tilt.current_tilt > 0.98f) {
tilt_factor = 1.0 / cosf(radians(0.98f*90));
} else {
tilt_factor = 1.0 / 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;
uint8_t mask = tilt.tilt_mask;
// apply _tilt_factor first
for (uint8_t i=0; i<num_motors; i++) {
if (mask & (1U<<i)) {
thrust[i] *= tilt_factor;
tilt_total += thrust[i];
tilt_count++;
}
}
float largest_tilted = 0;
// now constrain and apply _tilt_equal_thrust if enabled
for (uint8_t i=0; i<num_motors; i++) {
if (mask & (1U<<i)) {
if (equal_thrust) {
thrust[i] = tilt_total / tilt_count;
}
largest_tilted = MAX(largest_tilted, thrust[i]);
}
}
// if we are saturating one of the tilted motors then reduce all
// motors to keep them in proportion to the original thrust. This
// helps maintain stability when tilted at a large angle
if (largest_tilted > 1.0f) {
float scale = 1.0f / largest_tilted;
for (uint8_t i=0; i<num_motors; i++) {
thrust[i] *= scale;
}
}
}