Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-26 17:55:28 +03:00
Code Issues Wiki Activity

Merge pull request #8821 from Scavanger/ContinuousServoAutotrim-Fix-INAV6

Browse source 
Continuous Servo Autotrim Fix (Replacement for #8816)
This commit is contained in:
Paweł Spychalski 2023-02-23 09:31:58 +01:00 • committed by GitHub
commit db9911a623
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
1 changed files with 2 additions and 2 deletions
Download patch file Download diff file Expand all files Collapse all files

4
src/main/flight/servos.c
View file

@ -525,7 +525,7 @@ void processContinuousServoAutotrim(const float dT)
isGPSHeadingValid() // TODO: proper flying detection isGPSHeadingValid() // TODO: proper flying detection
) { ) {
// Plane is flying straight and level: trim servos // Plane is flying straight and level: trim servos
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
// For each stabilized axis, add 5 units of I-term to all associated servo midpoints // For each stabilized axis, add 5 units of I-term to all associated servo midpoints
const float axisIterm = getAxisIterm(axis); const float axisIterm = getAxisIterm(axis);
if (fabsf(axisIterm) > SERVO_AUTOTRIM_UPDATE_SIZE) { if (fabsf(axisIterm) > SERVO_AUTOTRIM_UPDATE_SIZE) {
@ -542,7 +542,7 @@ void processContinuousServoAutotrim(const float dT)
// Convert axis I-term to servo PWM and add to midpoint // Convert axis I-term to servo PWM and add to midpoint
const float mixerRate = currentServoMixer[i].rate / 100.0f; const float mixerRate = currentServoMixer[i].rate / 100.0f;
const float servoRate = servoParams(target)->rate / 100.0f; const float servoRate = servoParams(target)->rate / 100.0f;
servoParamsMutable(target)->middle += ItermUpdate * mixerRate * servoRate; servoParamsMutable(target)->middle += (int16_t)(ItermUpdate * mixerRate * servoRate);
servoParamsMutable(target)->middle = constrain(servoParamsMutable(target)->middle, SERVO_AUTOTRIM_CENTER_MIN, SERVO_AUTOTRIM_CENTER_MAX); servoParamsMutable(target)->middle = constrain(servoParamsMutable(target)->middle, SERVO_AUTOTRIM_CENTER_MIN, SERVO_AUTOTRIM_CENTER_MAX);
} }
} }