From 64e8f247bf87e354789cfc6366b672711c72d38c Mon Sep 17 00:00:00 2001
From: "timecop@gmail.com"
 <timecop@gmail.com@7c89a4a9-59b9-e629-4cfe-3a2d53b20e61>
Date: Sat, 21 Sep 2013 11:18:15 +0000
Subject: [PATCH] oops. gyro only does NOT need getEstimatedAttitude. bad!
 ms5611 driver improvements (was failing below 20c) merged some althold
 cleanups

git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@411 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
---
 src/drv_ms5611.c | 26 ++++++++++++--------------
 src/imu.c        | 12 ++++++------
 src/mw.c         |  9 ++++-----
 src/mw.h         |  2 +-
 4 files changed, 23 insertions(+), 26 deletions(-)

diff --git a/src/drv_ms5611.c b/src/drv_ms5611.c
index eee205e0d..42cd3bb14 100644
--- a/src/drv_ms5611.c
+++ b/src/drv_ms5611.c
@@ -154,29 +154,27 @@ static void ms5611_get_up(void)
 
 static void ms5611_calculate(int32_t *pressure, int32_t *temperature)
 {
-    int32_t temp, off2 = 0, sens2 = 0, delt;
-    int32_t press;
-
-    int64_t dT = ms5611_ut - ((int32_t)ms5611_c[5] << 8);
-    int64_t off = ((uint32_t)ms5611_c[2] << 16) + ((dT * ms5611_c[4]) >> 7);
-    int64_t sens = ((uint32_t)ms5611_c[1] << 15) + ((dT * ms5611_c[3]) >> 8);
-    temp = 2000 + ((dT * ms5611_c[6]) >> 23);
+    uint32_t press;
+    int64_t temp;
+    int64_t delt;    
+    int32_t dT = (int64_t)ms5611_ut - ((uint64_t)ms5611_c[5] * 256);
+    int64_t off = ((int64_t)ms5611_c[2] << 16) + (((int64_t)ms5611_c[4] * dT) >> 7);
+    int64_t sens = ((int64_t)ms5611_c[1] << 15) + (((int64_t)ms5611_c[3] * dT) >> 8);
+    temp = 2000 + ((dT * (int64_t)ms5611_c[6]) >> 23);    
 
     if (temp < 2000) { // temperature lower than 20degC
         delt = temp - 2000;
         delt = 5 * delt * delt;
-        off2 = delt >> 1;
-        sens2 = delt >> 2;
+        off -= delt >> 1;
+        sens -= delt >> 2;
         if (temp < -1500) { // temperature lower than -15degC
             delt = temp + 1500;
             delt = delt * delt;
-            off2  += 7 * delt;
-            sens2 += (11 * delt) >> 1;
+            off  -= 7 * delt;
+            sens -= (11 * delt) >> 1;
         }
     }
-    off  -= off2;
-    sens -= sens2;
-    press = (((ms5611_up * sens ) >> 21) - off) >> 15;
+    press = ((((int64_t)ms5611_up * sens ) >> 21) - off) >> 15;
 
     if (pressure)
         *pressure = press;
diff --git a/src/imu.c b/src/imu.c
index a67abf183..684470635 100755
--- a/src/imu.c
+++ b/src/imu.c
@@ -8,7 +8,7 @@ int accSumCount = 0;
 int16_t acc_25deg = 0;
 int32_t baroPressure = 0;
 int32_t baroTemperature = 0;
-int32_t baroPressureSum = 0;
+uint32_t baroPressureSum = 0;
 int32_t BaroAlt = 0;
 int32_t sonarAlt;              // to think about the unit
 int32_t EstAlt;                // in cm
@@ -50,12 +50,12 @@ void computeIMU(void)
     Gyro_getADC();
     if (sensors(SENSOR_ACC)) {
         ACC_getADC();
+        getEstimatedAttitude();
     } else {
         accADC[X] = 0;
         accADC[Y] = 0;
         accADC[Z] = 0;
     }
-    getEstimatedAttitude();
 
     if (feature(FEATURE_GYRO_SMOOTHING)) {
         static uint8_t Smoothing[3] = { 0, 0, 0 };
@@ -367,13 +367,12 @@ int getEstimatedAltitude(void)
     //P
     error = constrain(AltHold - EstAlt, -300, 300);
     error = applyDeadband(error, 10);       // remove small P parametr to reduce noise near zero position
-    BaroPID = constrain((cfg.P8[PIDALT] * error / 128), -150, +150);
+    BaroPID = constrain((cfg.P8[PIDALT] * error / 128), -200, +200);
 
     //I
     errorAltitudeI += cfg.I8[PIDALT] * error / 64;
-    errorAltitudeI = constrain(errorAltitudeI, -30000, 30000);
-    BaroPID += errorAltitudeI / 512;     // I in range +/-60
-
+    errorAltitudeI = constrain(errorAltitudeI, -50000, 50000);
+    BaroPID += errorAltitudeI / 512;     // I in range +/-100
 
     baroVel = (BaroAlt - lastBaroAlt) * 1000000.0f / dTime;
     lastBaroAlt = BaroAlt;
@@ -384,6 +383,7 @@ int getEstimatedAltitude(void)
     // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity).
     // By using CF it's possible to correct the drift of integrated accZ (velocity) without loosing the phase, i.e without delay
     vel = vel * cfg.baro_cf_vel + baroVel * (1 - cfg.baro_cf_vel);
+    constrain(vel, -1000, 1000);                // limit max velocity to +/- 10m/s (36km/h)
 
     // D
     vel_tmp = vel;
diff --git a/src/mw.c b/src/mw.c
index 5e34c0ffc..5c4c5df56 100755
--- a/src/mw.c
+++ b/src/mw.c
@@ -823,17 +823,16 @@ void loop(void)
                     if (abs(rcCommand[THROTTLE] - initialThrottleHold) > cfg.alt_hold_throttle_neutral) {
                         // Slowly increase/decrease AltHold proportional to stick movement ( +100 throttle gives ~ +50 cm in 1 second with cycle time about 3-4ms)
                         AltHoldCorr += rcCommand[THROTTLE] - initialThrottleHold;
-                        if (abs(AltHoldCorr) > 500) {
-                            AltHold += AltHoldCorr / 500;
-                            AltHoldCorr %= 500;
-                        }
-                        errorAltitudeI = 0;
+                        AltHold += AltHoldCorr / 2000;
+                        AltHoldCorr %= 2000;
                         isAltHoldChanged = 1;
                     } else if (isAltHoldChanged) {
                         AltHold = EstAlt;
+                        AltHoldCorr = 0;
                         isAltHoldChanged = 0;
                     }
                     rcCommand[THROTTLE] = initialThrottleHold + BaroPID;
+                    rcCommand[THROTTLE] = constrain(rcCommand[THROTTLE], mcfg.minthrottle + 150, mcfg.maxthrottle);
                 }
             }
         }
diff --git a/src/mw.h b/src/mw.h
index 597eb0cb1..b0436beae 100755
--- a/src/mw.h
+++ b/src/mw.h
@@ -345,7 +345,7 @@ extern uint16_t calibratingG;
 extern int16_t heading;
 extern int32_t baroPressure;
 extern int32_t baroTemperature;
-extern int32_t baroPressureSum;
+extern uint32_t baroPressureSum;
 extern int32_t BaroAlt;
 extern int32_t sonarAlt;
 extern int32_t EstAlt;