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

AP_Motors: tradheli - fix quad heli collective structure

Browse Source
This commit is contained in:
bnsgeyer 2020-01-23 22:05:24 -05:00 committed by Randy Mackay
parent e36d4d19e2
commit d4e5e1bd3d
1 changed files with 16 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/AP_Motors/AP_MotorsHeli_Quad.cpp
View File

@ -162,9 +162,9 @@ void AP_MotorsHeli_Quad::calculate_roll_pitch_collective_factors()
// reverse yaw for H frame
clockwise = !clockwise;
}
_rollFactor[CH_1+i] = -0.5*sinf(radians(angles[i]))/cos45;
_pitchFactor[CH_1+i] = 0.5*cosf(radians(angles[i]))/cos45;
_yawFactor[CH_1+i] = clockwise?-0.5:0.5;
_rollFactor[CH_1+i] = -0.25*sinf(radians(angles[i]))/cos45;
_pitchFactor[CH_1+i] = 0.25*cosf(radians(angles[i]))/cos45;
_yawFactor[CH_1+i] = clockwise?-0.25:0.25;
_collectiveFactor[CH_1+i] = 1;
}
}
@ -236,14 +236,14 @@ void AP_MotorsHeli_Quad::move_actuators(float roll_out, float pitch_out, float c
float collective_range = (_collective_max - _collective_min) * 0.001f;
if (_heliflags.inverted_flight) {
collective_out = 1 - collective_out;
collective_out = 1.0f - collective_out;
}
// feed power estimate into main rotor controller
_main_rotor.set_collective(fabsf(collective_out));
// scale collective to -1 to 1
collective_out = collective_out*2-1;
// rescale collective for overhead calc
collective_out -= _collective_mid_pct;
// reserve some collective for attitude control
collective_out *= collective_range;
@ -258,11 +258,11 @@ void AP_MotorsHeli_Quad::move_actuators(float roll_out, float pitch_out, float c
// see if we need to scale down yaw_out
for (uint8_t i=0; i<AP_MOTORS_HELI_QUAD_NUM_MOTORS; i++) {
float y = _yawFactor[CH_1+i] * yaw_out;
if (_out[i] < 0) {
if (_out[i] < 0.0f) {
// the slope of the yaw effect changes at zero collective
y = -y;
}
if (_out[i] * (_out[i] + y) < 0) {
if (_out[i] * (_out[i] + y) < 0.0f) {
// applying this yaw demand would change the sign of the
// collective, which means the yaw would not be applied
// evenly. We scale down the overall yaw demand to prevent
@ -275,13 +275,19 @@ void AP_MotorsHeli_Quad::move_actuators(float roll_out, float pitch_out, float c
// now apply the yaw correction
for (uint8_t i=0; i<AP_MOTORS_HELI_QUAD_NUM_MOTORS; i++) {
float y = _yawFactor[CH_1+i] * yaw_out;
if (_out[i] < 0) {
if (_out[i] < 0.0f) {
// the slope of the yaw effect changes at zero collective
y = -y;
}
_out[i] += y;
}
for (uint8_t i=0; i<AP_MOTORS_HELI_QUAD_NUM_MOTORS; i++) {
// scale output to 0 to 1
_out[i] += _collective_mid_pct;
// scale output to -1 to 1 for servo output
_out[i] = _out[i] * 2.0f - 1.0f;
}
}
void AP_MotorsHeli_Quad::output_to_motors()