AltHold cleanup.

* Renamed several methods and variables so they make more sense.
* Move more altitude hold related code out of imu.c/h into
altitudehold.c/h.
* Fixed a unsigned integer being using instead of an signed integer in
the throttle calculation code.

src/main/common/maths.c

@@ -20,6 +20,18 @@
 
 #include "maths.h"
 
+int32_t applyDeadband(int32_t value, int32_t deadband)
+{
+    if (abs(value) < deadband) {
+        value = 0;
+    } else if (value > 0) {
+        value -= deadband;
+    } else if (value < 0) {
+        value += deadband;
+    }
+    return value;
+}
+
 int constrain(int amt, int low, int high)
 {
     if (amt < low)

src/main/common/maths.h

@@ -40,6 +40,8 @@ typedef struct stdev_t
     int m_n;
 } stdev_t;
 
+int32_t applyDeadband(int32_t value, int32_t deadband);
+
 int constrain(int amt, int low, int high);
 float constrainf(float amt, float low, float high);
 
@@ -48,4 +50,5 @@ void devPush(stdev_t *dev, float x);
 float devVariance(stdev_t *dev);
 float devStandardDeviation(stdev_t *dev);
 float degreesToRadians(int16_t degrees);
+
 int scaleRange(int x, int srcMin, int srcMax, int destMin, int destMax);

src/main/config/config.c

@@ -54,6 +54,7 @@
 #include "io/ledstrip.h"
 #include "io/gps.h"
 #include "flight/failsafe.h"
+#include "flight/altitudehold.h"
 #include "flight/imu.h"
 #include "flight/navigation.h"
 
@@ -453,7 +454,8 @@ void activateConfig(void)
     imuRuntimeConfig.acc_unarmedcal = currentProfile->acc_unarmedcal;;
     imuRuntimeConfig.small_angle = masterConfig.small_angle;
 
-    configureImu(&imuRuntimeConfig, &currentProfile->pidProfile, &currentProfile->barometerConfig, &currentProfile->accDeadband);
+    configureImu(&imuRuntimeConfig, &currentProfile->pidProfile, &currentProfile->accDeadband);
+    configureAltitudeHold(&currentProfile->pidProfile, &currentProfile->barometerConfig);
 
     calculateThrottleAngleScale(currentProfile->throttle_correction_angle);
     calculateAccZLowPassFilterRCTimeConstant(currentProfile->accz_lpf_cutoff);

src/main/flight/altitudehold.c

@@ -19,6 +19,9 @@
 #include <stdbool.h>
 #include <stdint.h>
 #include <stdlib.h>
+#include <math.h>
+
+#include "common/maths.h"
 
 #include "platform.h"
 
@@ -42,6 +45,7 @@
 #include "sensors/battery.h"
 #include "sensors/boardalignment.h"
 #include "sensors/gyro.h"
+#include "sensors/sonar.h"
 
 #include "io/escservo.h"
 #include "io/gimbal.h"
@@ -63,9 +67,26 @@
 #include "config/config_master.h"
 
 #if defined(BARO) || defined(SONAR)
+
+barometerConfig_t *barometerConfig;
+pidProfile_t *pidProfile;
+
+void configureAltitudeHold(pidProfile_t *initialPidProfile, barometerConfig_t *intialBarometerConfig)
+{
+    pidProfile = initialPidProfile;
+    barometerConfig = intialBarometerConfig;
+}
+
+
 static int16_t initialThrottleHold;
 
-static void multirotorAltHold(void)
+
+// 40hz update rate (20hz LPF on acc)
+#define BARO_UPDATE_FREQUENCY_40HZ (1000 * 25)
+
+#define DEGREES_80_IN_DECIDEGREES 800
+
+static void applyMultirotorAltHold(void)
 {
     static uint8_t isAltHoldChanged = 0;
     // multirotor alt hold
@@ -80,7 +101,7 @@ static void multirotorAltHold(void)
                 AltHold = EstAlt;
                 isAltHoldChanged = 0;
             }
-            rcCommand[THROTTLE] = constrain(initialThrottleHold + BaroPID, masterConfig.escAndServoConfig.minthrottle, masterConfig.escAndServoConfig.maxthrottle);
+            rcCommand[THROTTLE] = constrain(initialThrottleHold + altHoldThrottleAdjustment, masterConfig.escAndServoConfig.minthrottle, masterConfig.escAndServoConfig.maxthrottle);
         }
     } else {
         // slow alt changes, mostly used for aerial photography
@@ -94,25 +115,25 @@ static void multirotorAltHold(void)
             velocityControl = 0;
             isAltHoldChanged = 0;
         }
-        rcCommand[THROTTLE] = constrain(initialThrottleHold + BaroPID, masterConfig.escAndServoConfig.minthrottle, masterConfig.escAndServoConfig.maxthrottle);
+        rcCommand[THROTTLE] = constrain(initialThrottleHold + altHoldThrottleAdjustment, masterConfig.escAndServoConfig.minthrottle, masterConfig.escAndServoConfig.maxthrottle);
     }
 }
 
-static void fixedWingAltHold()
+static void applyFixedWingAltHold()
 {
     // handle fixedwing-related althold. UNTESTED! and probably wrong
     // most likely need to check changes on pitch channel and 'reset' althold similar to
     // how throttle does it on multirotor
 
-    rcCommand[PITCH] += BaroPID * masterConfig.fixedwing_althold_dir;
+    rcCommand[PITCH] += altHoldThrottleAdjustment * masterConfig.fixedwing_althold_dir;
 }
 
-void updateAltHold(void)
+void applyAltHold(void)
 {
     if (STATE(FIXED_WING)) {
-        fixedWingAltHold();
+        applyFixedWingAltHold();
     } else {
-        multirotorAltHold();
+        applyMultirotorAltHold();
     }
 }
 
@@ -129,7 +150,7 @@ void updateAltHoldState(void)
         AltHold = EstAlt;
         initialThrottleHold = rcCommand[THROTTLE];
         errorVelocityI = 0;
-        BaroPID = 0;
+        altHoldThrottleAdjustment = 0;
     }
 }
 
@@ -146,8 +167,163 @@ void updateSonarAltHoldState(void)
         AltHold = EstAlt;
         initialThrottleHold = rcCommand[THROTTLE];
         errorVelocityI = 0;
-        BaroPID = 0;	// TODO: Change naming of BaroPID to "AltPID" as this is used by both sonar and baro
+        altHoldThrottleAdjustment = 0;
     }
 }
 
+bool isThrustFacingDownwards(rollAndPitchInclination_t *inclination)
+{
+    return abs(inclination->values.rollDeciDegrees) < DEGREES_80_IN_DECIDEGREES && abs(inclination->values.pitchDeciDegrees) < DEGREES_80_IN_DECIDEGREES;
+}
+
+int16_t calculateTiltAngle(rollAndPitchInclination_t *inclination)
+{
+    return max(abs(inclination->values.rollDeciDegrees), abs(inclination->values.pitchDeciDegrees));
+}
+
+
+int32_t calculateAltHoldThrottleAdjustment(int32_t vel_tmp, float accZ_tmp, float accZ_old)
+{
+    int32_t result = 0;
+    int32_t error;
+    int32_t setVel;
+
+    if (!isThrustFacingDownwards(&inclination)) {
+        return result;
+    }
+
+    // Altitude P-Controller
+
+    if (!velocityControl) {
+        error = constrain(AltHold - EstAlt, -500, 500);
+        error = applyDeadband(error, 10); // remove small P parameter to reduce noise near zero position
+        setVel = constrain((pidProfile->P8[PIDALT] * error / 128), -300, +300); // limit velocity to +/- 3 m/s
+    } else {
+        setVel = setVelocity;
+    }
+    // Velocity PID-Controller
+
+    // P
+    error = setVel - vel_tmp;
+    result = constrain((pidProfile->P8[PIDVEL] * error / 32), -300, +300);
+
+    // I
+    errorVelocityI += (pidProfile->I8[PIDVEL] * error);
+    errorVelocityI = constrain(errorVelocityI, -(8192 * 200), (8192 * 200));
+    result += errorVelocityI / 8192;     // I in range +/-200
+
+    // D
+    result -= constrain(pidProfile->D8[PIDVEL] * (accZ_tmp + accZ_old) / 512, -150, 150);
+
+    return result;
+}
+
+void calculateEstimatedAltitude(uint32_t currentTime)
+{
+    static uint32_t previousTime;
+    uint32_t dTime;
+    int32_t baroVel;
+    float dt;
+    float vel_acc;
+    int32_t vel_tmp;
+    float accZ_tmp;
+    static float accZ_old = 0.0f;
+    static float vel = 0.0f;
+    static float accAlt = 0.0f;
+    static int32_t lastBaroAlt;
+
+    static int32_t baroAlt_offset = 0;
+    float sonarTransition;
+
+#ifdef SONAR
+    int16_t tiltAngle;
 #endif
+
+
+
+    dTime = currentTime - previousTime;
+    if (dTime < BARO_UPDATE_FREQUENCY_40HZ)
+        return;
+
+    previousTime = currentTime;
+
+#ifdef BARO
+    if (!isBaroCalibrationComplete()) {
+        performBaroCalibrationCycle();
+        vel = 0;
+        accAlt = 0;
+    }
+
+    BaroAlt = baroCalculateAltitude();
+#else
+    BaroAlt = 0;
+#endif
+
+#ifdef SONAR
+    tiltAngle = calculateTiltAngle(&inclination);
+    sonarAlt = sonarCalculateAltitude(sonarAlt, tiltAngle);
+#endif
+
+    if (sonarAlt > 0 && sonarAlt < 200) {
+        baroAlt_offset = BaroAlt - sonarAlt;
+        BaroAlt = sonarAlt;
+    } else {
+        BaroAlt -= baroAlt_offset;
+        if (sonarAlt > 0) {
+            sonarTransition = (300 - sonarAlt) / 100.0f;
+            BaroAlt = sonarAlt * sonarTransition + BaroAlt * (1.0f - sonarTransition);
+        }
+    }
+
+    dt = accTimeSum * 1e-6f; // delta acc reading time in seconds
+
+    // Integrator - velocity, cm/sec
+    accZ_tmp = (float)accSum[2] / (float)accSumCount;
+    vel_acc = accZ_tmp * accVelScale * (float)accTimeSum;
+
+    // Integrator - Altitude in cm
+    accAlt += (vel_acc * 0.5f) * dt + vel * dt;                                                                 // integrate velocity to get distance (x= a/2 * t^2)
+    accAlt = accAlt * barometerConfig->baro_cf_alt + (float)BaroAlt * (1.0f - barometerConfig->baro_cf_alt);    // complementary filter for altitude estimation (baro & acc)
+    vel += vel_acc;
+
+#if 0
+    debug[1] = accSum[2] / accSumCount; // acceleration
+    debug[2] = vel;                     // velocity
+    debug[3] = accAlt;                  // height
+#endif
+
+    accSum_reset();
+
+#ifdef BARO
+    if (!isBaroCalibrationComplete()) {
+        return;
+    }
+#endif
+
+    if (sonarAlt > 0 && sonarAlt < 200) {
+        // the sonar has the best range
+        EstAlt = BaroAlt;
+    } else {
+        EstAlt = accAlt;
+    }
+
+    baroVel = (BaroAlt - lastBaroAlt) * 1000000.0f / dTime;
+    lastBaroAlt = BaroAlt;
+
+    baroVel = constrain(baroVel, -1500, 1500);  // constrain baro velocity +/- 1500cm/s
+    baroVel = applyDeadband(baroVel, 10);       // to reduce noise near zero
+
+    // 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 * barometerConfig->baro_cf_vel + baroVel * (1.0f - barometerConfig->baro_cf_vel);
+    vel_tmp = lrintf(vel);
+
+    // set vario
+    vario = applyDeadband(vel_tmp, 5);
+
+    altHoldThrottleAdjustment = calculateAltHoldThrottleAdjustment(vel_tmp, accZ_tmp, accZ_old);
+
+    accZ_old = accZ_tmp;
+}
+#endif
+

src/main/flight/altitudehold.h

@@ -15,8 +15,9 @@
  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+void configureAltitudeHold(pidProfile_t *initialPidProfile, barometerConfig_t *intialBarometerConfig);
 
-void updateAltHold(void);
+void applyAltHold(void);
 void updateAltHoldState(void);
 void updateSonarAltHoldState(void);
 

src/main/flight/imu.c

@@ -65,7 +65,7 @@ int32_t vario = 0;                      // variometer in cm/s
 float throttleAngleScale;
 float fc_acc;
 
-int32_t BaroPID = 0;
+int32_t altHoldThrottleAdjustment = 0;
 
 float magneticDeclination = 0.0f;       // calculated at startup from config
 float gyroScaleRad;
@@ -83,14 +83,12 @@ static void getEstimatedAttitude(void);
 
 imuRuntimeConfig_t *imuRuntimeConfig;
 pidProfile_t *pidProfile;
-barometerConfig_t *barometerConfig;
 accDeadband_t *accDeadband;
 
-void configureImu(imuRuntimeConfig_t *initialImuRuntimeConfig, pidProfile_t *initialPidProfile, barometerConfig_t *intialBarometerConfig, accDeadband_t *initialAccDeadband)
+void configureImu(imuRuntimeConfig_t *initialImuRuntimeConfig, pidProfile_t *initialPidProfile, accDeadband_t *initialAccDeadband)
 {
     imuRuntimeConfig = initialImuRuntimeConfig;
     pidProfile = initialPidProfile;
-    barometerConfig = intialBarometerConfig;
     accDeadband = initialAccDeadband;
 }
 
@@ -203,16 +201,13 @@ void rotateV(struct fp_vector *v, fp_angles_t *delta)
     v->Z = v_tmp.X * mat[0][2] + v_tmp.Y * mat[1][2] + v_tmp.Z * mat[2][2];
 }
 
-int32_t applyDeadband(int32_t value, int32_t deadband)
+void accSum_reset(void)
 {
-    if (abs(value) < deadband) {
+    accSum[0] = 0;
-        value = 0;
+    accSum[1] = 0;
-    } else if (value > 0) {
+    accSum[2] = 0;
-        value -= deadband;
+    accSumCount = 0;
-    } else if (value < 0) {
+    accTimeSum = 0;
-        value += deadband;
-    }
-    return value;
 }
 
 // rotate acc into Earth frame and calculate acceleration in it
@@ -259,15 +254,6 @@ void acc_calc(uint32_t deltaT)
     accSumCount++;
 }
 
-void accSum_reset(void)
-{
-    accSum[0] = 0;
-    accSum[1] = 0;
-    accSum[2] = 0;
-    accSumCount = 0;
-    accTimeSum = 0;
-}
-
 // baseflight calculation by Luggi09 originates from arducopter
 static int16_t calculateHeading(t_fp_vector *vec)
 {
@@ -371,165 +357,3 @@ int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value)
         angle = 900;
     return lrintf(throttle_correction_value * sinf(angle / (900.0f * M_PI / 2.0f)));
 }
-
-#if defined(BARO) || defined(SONAR)
-
-// 40hz update rate (20hz LPF on acc)
-#define BARO_UPDATE_FREQUENCY_40HZ (1000 * 25)
-
-#define DEGREES_80_IN_DECIDEGREES 800
-
-bool isThrustFacingDownwards(rollAndPitchInclination_t *inclination)
-{
-    return abs(inclination->values.rollDeciDegrees) < DEGREES_80_IN_DECIDEGREES && abs(inclination->values.pitchDeciDegrees) < DEGREES_80_IN_DECIDEGREES;
-}
-
-int16_t calculateTiltAngle(rollAndPitchInclination_t *inclination)
-{
-	return max(abs(inclination->values.rollDeciDegrees), abs(inclination->values.pitchDeciDegrees));
-}
-
-int32_t calculateBaroPid(int32_t vel_tmp, float accZ_tmp, float accZ_old)
-{
-    uint32_t newBaroPID = 0;
-    int32_t error;
-    int32_t setVel;
-
-    if (!isThrustFacingDownwards(&inclination)) {
-        return newBaroPID;
-    }
-
-    // Altitude P-Controller
-
-    if (!velocityControl) {
-        error = constrain(AltHold - EstAlt, -500, 500);
-        error = applyDeadband(error, 10); // remove small P parameter to reduce noise near zero position
-        setVel = constrain((pidProfile->P8[PIDALT] * error / 128), -300, +300); // limit velocity to +/- 3 m/s
-    } else {
-        setVel = setVelocity;
-    }
-    // Velocity PID-Controller
-
-    // P
-    error = setVel - vel_tmp;
-    newBaroPID = constrain((pidProfile->P8[PIDVEL] * error / 32), -300, +300);
-
-    // I
-    errorVelocityI += (pidProfile->I8[PIDVEL] * error);
-    errorVelocityI = constrain(errorVelocityI, -(8192 * 200), (8192 * 200));
-    newBaroPID += errorVelocityI / 8192;     // I in range +/-200
-
-    // D
-    newBaroPID -= constrain(pidProfile->D8[PIDVEL] * (accZ_tmp + accZ_old) / 512, -150, 150);
-
-    return newBaroPID;
-}
-
-void calculateEstimatedAltitude(uint32_t currentTime)
-{
-    static uint32_t previousTime;
-    uint32_t dTime;
-    int32_t baroVel;
-    float dt;
-    float vel_acc;
-    int32_t vel_tmp;
-    float accZ_tmp;
-    static float accZ_old = 0.0f;
-    static float vel = 0.0f;
-    static float accAlt = 0.0f;
-    static int32_t lastBaroAlt;
-
-    static int32_t baroAlt_offset = 0;
-    float sonarTransition;
-
-#ifdef SONAR
-    int16_t tiltAngle;
-#endif
-
-
-
-    dTime = currentTime - previousTime;
-    if (dTime < BARO_UPDATE_FREQUENCY_40HZ)
-        return;
-        
-    previousTime = currentTime;
-
-#ifdef BARO
-    if (!isBaroCalibrationComplete()) {
-        performBaroCalibrationCycle();
-        vel = 0;
-        accAlt = 0;
-    }
-
-    BaroAlt = baroCalculateAltitude();
-#else
-	BaroAlt = 0;
-#endif
-
-#ifdef SONAR
-    tiltAngle = calculateTiltAngle(&inclination);
-    sonarAlt = sonarCalculateAltitude(sonarAlt, tiltAngle);
-#endif
-
-    if (sonarAlt > 0 && sonarAlt < 200) {
-        baroAlt_offset = BaroAlt - sonarAlt;
-        BaroAlt = sonarAlt;
-    } else {
-        BaroAlt -= baroAlt_offset;
-        if (sonarAlt > 0) {
-            sonarTransition = (300 - sonarAlt) / 100.0f;
-            BaroAlt = sonarAlt * sonarTransition + BaroAlt * (1.0f - sonarTransition);
-        }
-    }
-
-    dt = accTimeSum * 1e-6f; // delta acc reading time in seconds
-
-    // Integrator - velocity, cm/sec
-    accZ_tmp = (float)accSum[2] / (float)accSumCount;
-    vel_acc = accZ_tmp * accVelScale * (float)accTimeSum;
-
-    // Integrator - Altitude in cm
-    accAlt += (vel_acc * 0.5f) * dt + vel * dt;                                                                 // integrate velocity to get distance (x= a/2 * t^2)
-    accAlt = accAlt * barometerConfig->baro_cf_alt + (float)BaroAlt * (1.0f - barometerConfig->baro_cf_alt);    // complementary filter for altitude estimation (baro & acc)
-    vel += vel_acc;
-
-#if 0
-    debug[1] = accSum[2] / accSumCount; // acceleration
-    debug[2] = vel;                     // velocity
-    debug[3] = accAlt;                  // height
-#endif
-
-    accSum_reset();
-
-#ifdef BARO
-    if (!isBaroCalibrationComplete()) {
-        return;
-    }
-#endif
-
-    if (sonarAlt > 0 && sonarAlt < 200) {
-        // the sonar has the best range
-        EstAlt = BaroAlt;
-    } else {
-        EstAlt = accAlt;
-    }
-
-    baroVel = (BaroAlt - lastBaroAlt) * 1000000.0f / dTime;
-    lastBaroAlt = BaroAlt;
-
-    baroVel = constrain(baroVel, -1500, 1500);  // constrain baro velocity +/- 1500cm/s
-    baroVel = applyDeadband(baroVel, 10);       // to reduce noise near zero
-
-    // 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 * barometerConfig->baro_cf_vel + baroVel * (1.0f - barometerConfig->baro_cf_vel);
-    vel_tmp = lrintf(vel);
-
-    // set vario
-    vario = applyDeadband(vel_tmp, 5);
-
-    BaroPID = calculateBaroPid(vel_tmp, accZ_tmp, accZ_old);
-
-    accZ_old = accZ_tmp;
-}
-#endif /* BARO */

src/main/flight/imu.h

@@ -20,8 +20,11 @@
 extern int32_t errorVelocityI;
 extern uint8_t velocityControl;
 extern int32_t setVelocity;
-extern int32_t BaroPID;
+extern int32_t altHoldThrottleAdjustment;
 extern int16_t throttleAngleCorrection;
+extern uint32_t accTimeSum;
+extern int accSumCount;
+extern float accVelScale;
 
 typedef struct imuRuntimeConfig_s {
     uint8_t acc_lpf_factor;
@@ -31,10 +34,12 @@ typedef struct imuRuntimeConfig_s {
     int8_t small_angle;
 } imuRuntimeConfig_t;
 
-void configureImu(imuRuntimeConfig_t *initialImuRuntimeConfig, pidProfile_t *initialPidProfile, barometerConfig_t *intialBarometerConfig, accDeadband_t *initialAccDeadband);
+void configureImu(imuRuntimeConfig_t *initialImuRuntimeConfig, pidProfile_t *initialPidProfile, accDeadband_t *initialAccDeadband);
 
 void calculateEstimatedAltitude(uint32_t currentTime);
 void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerConfiguration);
 void calculateThrottleAngleScale(uint16_t throttle_correction_angle);
 int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value);
 void calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff);
+
+void accSum_reset(void);

src/main/mw.c

@@ -632,7 +632,7 @@ void loop(void)
 #if defined(BARO) || defined(SONAR)
         if (sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)) {
             if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE)) {
-                updateAltHold();
+                applyAltHold();
             }
         }
 #endif