remove unnecessary var, inline accsum_reset, lrintf , rename smallAngle

src/imu.c

@@ -5,7 +5,7 @@ int16_t gyroADC[3], accADC[3], accSmooth[3], magADC[3];
 int32_t accSum[3];
 uint32_t accTimeSum = 0;        // keep track for integration of acc
 int accSumCount = 0;
-int16_t accZ_25deg = 0;
+int16_t smallAngle = 0;
 int32_t baroPressure = 0;
 int32_t baroTemperature = 0;
 uint32_t baroPressureSum = 0;
@@ -33,7 +33,7 @@ static void getEstimatedAttitude(void);
 
 void imuInit(void)
 {
-    accZ_25deg = acc_1G * cosf(RAD * 25.0f);
+    smallAngle = lrintf(acc_1G * cosf(RAD * 25.0f));
     accVelScale = 9.80665f / acc_1G / 10000.0f;
     throttleAngleScale = (1800.0f / M_PI) * (900.0f / cfg.throttle_correction_angle);
 
@@ -197,27 +197,15 @@ void acc_calc(uint32_t deltaT)
     accz_smooth = accz_smooth + (deltaT / (fc_acc + deltaT)) * (accel_ned.V.Z - accz_smooth); // low pass filter
 
     // apply Deadband to reduce integration drift and vibration influence
-    accel_ned.V.Z = applyDeadband(lrintf(accz_smooth), cfg.accz_deadband);
+    accSum[X] += applyDeadband(lrintf(accel_ned.V.X), cfg.accxy_deadband);
-    accel_ned.V.X = applyDeadband(lrintf(accel_ned.V.X), cfg.accxy_deadband);
+    accSum[Y] += applyDeadband(lrintf(accel_ned.V.Y), cfg.accxy_deadband);
-    accel_ned.V.Y = applyDeadband(lrintf(accel_ned.V.Y), cfg.accxy_deadband);
+    accSum[Z] += applyDeadband(lrintf(accz_smooth), cfg.accz_deadband);
 
     // sum up Values for later integration to get velocity and distance
     accTimeSum += deltaT;
     accSumCount++;
-
-    accSum[X] += lrintf(accel_ned.V.X);
-    accSum[Y] += lrintf(accel_ned.V.Y);
-    accSum[Z] += lrintf(accel_ned.V.Z);
 }
 
-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)
@@ -240,10 +228,10 @@ static int16_t calculateHeading(t_fp_vector *vec)
 
 static void getEstimatedAttitude(void)
 {
-    uint32_t axis;
+    int32_t axis;
     int32_t accMag = 0;
     static t_fp_vector EstM;
-    static t_fp_vector EstN = { .A = { 1000.0f, 0.0f, 0.0f } };
+    static t_fp_vector EstN = { .A = { 1.0f, 0.0f, 0.0f } };
     static float accLPF[3];
     static uint32_t previousT;
     uint32_t currentT = micros();
@@ -270,8 +258,7 @@ static void getEstimatedAttitude(void)
     if (sensors(SENSOR_MAG)) {
         rotateV(&EstM.V, deltaGyroAngle);
     } else {
-        rotateV(&EstN.V, deltaGyroAngle);
+        rotateV(&EstN.V, deltaGyroAngle);
-        normalizeV(&EstN.V, &EstN.V);
     }
 
     // Apply complimentary filter (Gyro drift correction)
@@ -287,10 +274,7 @@ static void getEstimatedAttitude(void)
             EstM.A[axis] = (EstM.A[axis] * (float)mcfg.gyro_cmpfm_factor + magADC[axis]) * INV_GYR_CMPFM_FACTOR;
     }
 
-   if (EstG.A[Z] > accZ_25deg)
+    f.SMALL_ANGLE = (EstG.A[Z] > smallAngle);
-        f.SMALL_ANGLES_25 = 1;
-    else
-        f.SMALL_ANGLES_25 = 0;
 
     // Attitude of the estimated vector
     anglerad[ROLL] = atan2f(EstG.V.Y, EstG.V.Z);
@@ -337,7 +321,6 @@ int getEstimatedAltitude(void)
     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;
@@ -383,7 +366,12 @@ int getEstimatedAltitude(void)
     debug[2] = accAlt;                  // height
 #endif
 
-    accSum_reset();
+    // Integrator done - reset accSum
+    accSum[0] = 0;
+    accSum[1] = 0;
+    accSum[2] = 0;
+    accSumCount = 0;
+    accTimeSum = 0;
 
     baroVel = (BaroAlt - lastBaroAlt) * 1000000.0f / dTime;
     lastBaroAlt = BaroAlt;
@@ -394,11 +382,10 @@ 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_tmp = lrintf(vel);
 
     // set vario
-    vel_tmp = lrintf(vel);
+    vario = applyDeadband(vel_tmp, 5);
-    vel_tmp = applyDeadband(vel_tmp, 5);
-    vario = vel_tmp;
 
     // Altitude P-Controller
     error = constrain(AltHold - EstAlt, -500, 500);
@@ -407,7 +394,7 @@ int getEstimatedAltitude(void)
 
     // Velocity PID-Controller
     // P
-    error = setVel - lrintf(vel);
+    error = setVel - vel_tmp;
     BaroPID = constrain((cfg.P8[PIDVEL] * error / 32), -300, +300);
 
     // I
@@ -416,9 +403,10 @@ int getEstimatedAltitude(void)
     BaroPID += errorAltitudeI / 1024;     // I in range +/-200
 
     // D
-    accZ_old = accZ_tmp;
+    BaroPID -= constrain(lrintf(cfg.D8[PIDVEL] * accZ_tmp / 32), -150, 150);
-    BaroPID -= constrain(cfg.D8[PIDVEL] * (accZ_tmp + accZ_old) / 64, -150, 150);
+	// was not that supposed to be :
-
+	//BaroPID -= constrain(cfg.D8[PIDVEL] * (accZ_tmp + accZ_old) / 64, -150, 150);
+    //accZ_old = accZ_tmp;  
     return 1;
 }
 #endif /* BARO */

src/main.c

@@ -172,7 +172,7 @@ int main(void)
         calibratingA = CALIBRATING_ACC_CYCLES;
     calibratingG = CALIBRATING_GYRO_CYCLES;
     calibratingB = CALIBRATING_BARO_CYCLES;             // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles
-    f.SMALL_ANGLES_25 = 1;
+    f.SMALL_ANGLE = 1;
 
     // loopy
     while (1) {

src/mw.c

@@ -193,7 +193,7 @@ void annexCode(void)
 #endif
 
     if ((int32_t)(currentTime - calibratedAccTime) >= 0) {
-        if (!f.SMALL_ANGLES_25) {
+        if (!f.SMALL_ANGLE) {
             f.ACC_CALIBRATED = 0; // the multi uses ACC and is not calibrated or is too much inclinated
             LED0_TOGGLE;
             calibratedAccTime = currentTime + 500000;
@@ -832,7 +832,7 @@ void loop(void)
                 if (dif >= +180)
                     dif -= 360;
                 dif *= -mcfg.yaw_control_direction;
-                if (f.SMALL_ANGLES_25)
+                if (f.SMALL_ANGLE)
                     rcCommand[YAW] -= dif * cfg.P8[PIDMAG] / 30;    // 18 deg
             } else
                 magHold = heading;