Merge pull request #42 from luggi/betteralthold

new althold PID

src/config.c

@@ -230,9 +230,9 @@ static void resetConf(void)
     cfg.P8[YAW] = 85;
     cfg.I8[YAW] = 45;
     cfg.D8[YAW] = 0;
-    cfg.P8[PIDALT] = 40;
-    cfg.I8[PIDALT] = 25;
-    cfg.D8[PIDALT] = 60;
+    cfg.P8[PIDALT] = 50;
+    cfg.I8[PIDALT] = 0;
+    cfg.D8[PIDALT] = 0;
     cfg.P8[PIDPOS] = 11; // POSHOLD_P * 100;
     cfg.I8[PIDPOS] = 0; // POSHOLD_I * 100;
     cfg.D8[PIDPOS] = 0;
@@ -246,9 +246,9 @@ static void resetConf(void)
     cfg.I8[PIDLEVEL] = 10;
     cfg.D8[PIDLEVEL] = 100;
     cfg.P8[PIDMAG] = 40;
-    cfg.P8[PIDVEL] = 0;
-    cfg.I8[PIDVEL] = 0;
-    cfg.D8[PIDVEL] = 0;
+    cfg.P8[PIDVEL] = 120;
+    cfg.I8[PIDVEL] = 45;
+    cfg.D8[PIDVEL] = 1;
     cfg.rcRate8 = 90;
     cfg.rcExpo8 = 65;
     cfg.rollPitchRate = 0;
@@ -266,8 +266,8 @@ static void resetConf(void)
     cfg.accxy_deadband = 40;
     cfg.baro_tab_size = 21;
     cfg.baro_noise_lpf = 0.6f;
-    cfg.baro_cf_vel = 0.995f;
-    cfg.baro_cf_alt = 0.950f;
+    cfg.baro_cf_vel = 0.985f;
+    cfg.baro_cf_alt = 0.965f;
     cfg.acc_unarmedcal = 1;
 
     // Radio

src/imu.c

@@ -174,7 +174,7 @@ int32_t applyDeadband(int32_t value, int32_t deadband)
     return value;
 }
 
-#define F_CUT_ACCZ 20.0f
+#define F_CUT_ACCZ 10.0f // 10Hz should still be fast enough
 static const float fc_acc = 0.5f / (M_PI * F_CUT_ACCZ);
 
 // rotate acc into Earth frame and calculate acceleration in it
@@ -332,8 +332,11 @@ int getEstimatedAltitude(void)
     int32_t baroVel;
     int32_t vel_tmp;
     int32_t BaroAlt_tmp;
+    int32_t setVel;
     float dt;
     float vel_acc;
+    float accZ_tmp;
+    static float accZ_old = 0.0f;
     static float vel = 0.0f;
     static float accAlt = 0.0f;
     static int32_t lastBaroAlt;
@@ -364,7 +367,8 @@ int getEstimatedAltitude(void)
     dt = accTimeSum * 1e-6f; // delta acc reading time in seconds
 
     // Integrator - velocity, cm/sec
-    vel_acc = (float)accSum[2] * accVelScale * (float)accTimeSum / (float)accSumCount;
+    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)
@@ -380,16 +384,6 @@ int getEstimatedAltitude(void)
 
     accSum_reset();
 
-    //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), -200, +200);
-
-    //I
-    errorAltitudeI += cfg.I8[PIDALT] * error / 64;
-    errorAltitudeI = constrain(errorAltitudeI, -50000, 50000);
-    BaroPID += errorAltitudeI / 512;     // I in range +/-100
-
     baroVel = (BaroAlt - lastBaroAlt) * 1000000.0f / dTime;
     lastBaroAlt = BaroAlt;
 
@@ -399,13 +393,30 @@ 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);
-    vel = constrain(vel, -1000, 1000);                // limit max velocity to +/- 10m/s (36km/h)
 
-    // D
+    // set vario
     vel_tmp = lrintf(vel);
     vel_tmp = applyDeadband(vel_tmp, 5);
     vario = vel_tmp;
-    BaroPID -= constrain(cfg.D8[PIDALT] * vel_tmp / 16, -150, 150);
+
+    // Altitude P-Controller
+    error = constrain(AltHold - EstAlt, -500, 500);
+    error = applyDeadband(error, 10);       // remove small P parametr to reduce noise near zero position
+    setVel = constrain((cfg.P8[PIDALT] * error / 128), -300, +300); // limit velocity to +/- 3 m/s
+
+    // Velocity PID-Controller
+    // P
+    error = setVel - lrintf(vel);
+    BaroPID = constrain((cfg.P8[PIDVEL] * error / 32), -300, +300);
+
+    // I
+    errorAltitudeI += (cfg.I8[PIDVEL] * error) / 8;
+    errorAltitudeI = constrain(errorAltitudeI, -(1024 * 200), (1024 * 200));
+    BaroPID += errorAltitudeI / 1024;     // I in range +/-200
+
+    // D
+    accZ_old = accZ_tmp;
+    BaroPID -= constrain(cfg.D8[PIDVEL] * (accZ_tmp + accZ_old) / 64, -150, 150);
 
     return 1;
 }