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

AP_Motors: Dualheli- fix bug for scaling second swashplate

Browse Source
This commit is contained in:
bnsgeyer 2018-12-16 23:08:19 -05:00 committed by Randy Mackay
parent 536a894850
commit 1b90ef34cc
1 changed files with 13 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
libraries/AP_Motors/AP_MotorsHeli_Dual.cpp
View File

@ -475,19 +475,18 @@ void AP_MotorsHeli_Dual::move_actuators(float roll_out, float pitch_out, float c
limit.throttle_upper = true;
}
// Set rear collective to midpoint if required
float collective2_out = collective_out;
if (_servo_mode == SERVO_CONTROL_MODE_MANUAL_CENTER) {
collective2_out = _collective2_mid_pct;
}
// ensure not below landed/landing collective
if (_heliflags.landing_collective && collective_out < (_land_collective_min*0.001f)) {
collective_out = _land_collective_min*0.001f;
limit.throttle_lower = true;
}
// Set rear collective to midpoint if required
float collective2_out = collective_out;
if (_servo_mode == SERVO_CONTROL_MODE_MANUAL_CENTER) {
collective2_out = _collective2_mid_pct;
}
// scale collective pitch for front swashplate (servos 1,2,3)
float collective_scaler = ((float)(_collective_max-_collective_min))*0.001f;
float collective_out_scaled = collective_out * collective_scaler + (_collective_min - 1000)*0.001f;
@ -509,9 +508,13 @@ void AP_MotorsHeli_Dual::move_actuators(float roll_out, float pitch_out, float c
// swashplate servos
float servo_out[AP_MOTORS_HELI_DUAL_NUM_SWASHPLATE_SERVOS];
for (uint8_t i=0; i<AP_MOTORS_HELI_DUAL_NUM_SWASHPLATE_SERVOS; i++) {
servo_out[i] = (_rollFactor[i] * roll_out + _pitchFactor[i] * pitch_out + _yawFactor[i] * yaw_out)*0.45f + _collectiveFactor[i] * collective_out_scaled;
}
servo_out[CH_1] = (_rollFactor[CH_1] * roll_out + _pitchFactor[CH_1] * pitch_out + _yawFactor[CH_1] * yaw_out)*0.45f + _collectiveFactor[CH_1] * collective_out_scaled;
servo_out[CH_2] = (_rollFactor[CH_2] * roll_out + _pitchFactor[CH_2] * pitch_out + _yawFactor[CH_2] * yaw_out)*0.45f + _collectiveFactor[CH_2] * collective_out_scaled;
servo_out[CH_3] = (_rollFactor[CH_3] * roll_out + _pitchFactor[CH_3] * pitch_out + _yawFactor[CH_3] * yaw_out)*0.45f + _collectiveFactor[CH_3] * collective_out_scaled;
servo_out[CH_4] = (_rollFactor[CH_4] * roll_out + _pitchFactor[CH_4] * pitch_out + _yawFactor[CH_4] * yaw_out)*0.45f + _collectiveFactor[CH_4] * collective2_out_scaled;
servo_out[CH_5] = (_rollFactor[CH_5] * roll_out + _pitchFactor[CH_5] * pitch_out + _yawFactor[CH_5] * yaw_out)*0.45f + _collectiveFactor[CH_5] * collective2_out_scaled;
servo_out[CH_6] = (_rollFactor[CH_6] * roll_out + _pitchFactor[CH_6] * pitch_out + _yawFactor[CH_6] * yaw_out)*0.45f + _collectiveFactor[CH_6] * collective2_out_scaled;
// rescale from -1..1, so we can use the pwm calc that includes trim
for (uint8_t i=0; i<AP_MOTORS_HELI_DUAL_NUM_SWASHPLATE_SERVOS; i++) {