From 0359a3280b90e8eb271c2aeb75254bf0ef9a8e45 Mon Sep 17 00:00:00 2001
From: Ben Hitchcock <ben@zensoft.com.au>
Date: Sun, 12 Apr 2015 15:06:04 +0800
Subject: [PATCH] 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.
---
 src/main/flight/altitudehold.c | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/src/main/flight/altitudehold.c b/src/main/flight/altitudehold.c
index 16259b381..c19cfdb44 100644
--- a/src/main/flight/altitudehold.c
+++ b/src/main/flight/altitudehold.c
@@ -90,10 +90,10 @@ static void applyMultirotorAltHold(void)
     // multirotor alt hold
     if (rcControlsConfig->alt_hold_fast_change) {
         // rapid alt changes
-        if (ABS(rcCommand[THROTTLE] - initialThrottleHold) > rcControlsConfig->alt_hold_deadband) {
+        if (ABS(rcData[THROTTLE] - initialThrottleHold) > rcControlsConfig->alt_hold_deadband) {
             errorVelocityI = 0;
             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 {
             if (isAltHoldChanged) {
                 AltHold = EstAlt;
@@ -103,9 +103,9 @@ static void applyMultirotorAltHold(void)
         }
     } else {
         // 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
-            setVelocity = (rcCommand[THROTTLE] - initialThrottleHold) / 2;
+            setVelocity = (rcData[THROTTLE] - initialThrottleHold) / 2;
             velocityControl = 1;
             isAltHoldChanged = 1;
         } else if (isAltHoldChanged) {
@@ -146,7 +146,7 @@ void updateAltHoldState(void)
     if (!FLIGHT_MODE(BARO_MODE)) {
         ENABLE_FLIGHT_MODE(BARO_MODE);
         AltHold = EstAlt;
-        initialThrottleHold = rcCommand[THROTTLE];
+        initialThrottleHold = rcData[THROTTLE];
         errorVelocityI = 0;
         altHoldThrottleAdjustment = 0;
     }
@@ -163,7 +163,7 @@ void updateSonarAltHoldState(void)
     if (!FLIGHT_MODE(SONAR_MODE)) {
         ENABLE_FLIGHT_MODE(SONAR_MODE);
         AltHold = EstAlt;
-        initialThrottleHold = rcCommand[THROTTLE];
+        initialThrottleHold = rcData[THROTTLE];
         errorVelocityI = 0;
         altHoldThrottleAdjustment = 0;
     }