rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Bugfix to stop altitude hold drift, especially noticeable in sonar mode. 

This uses the raw stick values instead of the calculated RCCommand values to determine whether or not the stick has moved.  The problem was that the RcCommand variable wasn't being reset to the stick position each loop, and so outputs were being treated as inputs.
This commit is contained in:
Ben Hitchcock 2015-04-12 15:06:04 +08:00
parent 35abdb89f5
commit 0359a3280b
1 changed files with 6 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/main/flight/altitudehold.c
View File

@ -90,10 +90,10 @@ static void applyMultirotorAltHold(void)
// multirotor alt hold // multirotor alt hold
if (rcControlsConfig->alt_hold_fast_change) { if (rcControlsConfig->alt_hold_fast_change) {
// rapid alt changes // rapid alt changes
if (ABS(rcCommand[THROTTLE] - initialThrottleHold) > rcControlsConfig->alt_hold_deadband) { if (ABS(rcData[THROTTLE] - initialThrottleHold) > rcControlsConfig->alt_hold_deadband) {
errorVelocityI = 0; errorVelocityI = 0;
isAltHoldChanged = 1; isAltHoldChanged = 1;
rcCommand[THROTTLE] += (rcCommand[THROTTLE] > initialThrottleHold) ? -rcControlsConfig->alt_hold_deadband : rcControlsConfig->alt_hold_deadband; rcCommand[THROTTLE] += (rcData[THROTTLE] > initialThrottleHold) ? -rcControlsConfig->alt_hold_deadband : rcControlsConfig->alt_hold_deadband;
} else { } else {
if (isAltHoldChanged) { if (isAltHoldChanged) {
AltHold = EstAlt; AltHold = EstAlt;
@ -103,9 +103,9 @@ static void applyMultirotorAltHold(void)
} }
} else { } else {
// slow alt changes, mostly used for aerial photography // slow alt changes, mostly used for aerial photography
if (ABS(rcCommand[THROTTLE] - initialThrottleHold) > rcControlsConfig->alt_hold_deadband) { if (ABS(rcData[THROTTLE] - initialThrottleHold) > rcControlsConfig->alt_hold_deadband) {
// set velocity proportional to stick movement +100 throttle gives ~ +50 cm/s // set velocity proportional to stick movement +100 throttle gives ~ +50 cm/s
setVelocity = (rcCommand[THROTTLE] - initialThrottleHold) / 2; setVelocity = (rcData[THROTTLE] - initialThrottleHold) / 2;
velocityControl = 1; velocityControl = 1;
isAltHoldChanged = 1; isAltHoldChanged = 1;
} else if (isAltHoldChanged) { } else if (isAltHoldChanged) {
@ -146,7 +146,7 @@ void updateAltHoldState(void)
if (!FLIGHT_MODE(BARO_MODE)) { if (!FLIGHT_MODE(BARO_MODE)) {
ENABLE_FLIGHT_MODE(BARO_MODE); ENABLE_FLIGHT_MODE(BARO_MODE);
AltHold = EstAlt; AltHold = EstAlt;
initialThrottleHold = rcCommand[THROTTLE]; initialThrottleHold = rcData[THROTTLE];
errorVelocityI = 0; errorVelocityI = 0;
altHoldThrottleAdjustment = 0; altHoldThrottleAdjustment = 0;
} }
@ -163,7 +163,7 @@ void updateSonarAltHoldState(void)
if (!FLIGHT_MODE(SONAR_MODE)) { if (!FLIGHT_MODE(SONAR_MODE)) {
ENABLE_FLIGHT_MODE(SONAR_MODE); ENABLE_FLIGHT_MODE(SONAR_MODE);
AltHold = EstAlt; AltHold = EstAlt;
initialThrottleHold = rcCommand[THROTTLE]; initialThrottleHold = rcData[THROTTLE];
errorVelocityI = 0; errorVelocityI = 0;
altHoldThrottleAdjustment = 0; altHoldThrottleAdjustment = 0;
} }