Merge pull request #86 from treymarc/patch_speed

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,17 +197,13 @@ 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);
-    accel_ned.V.X = applyDeadband(lrintf(accel_ned.V.X), cfg.accxy_deadband);
-    accel_ned.V.Y = applyDeadband(lrintf(accel_ned.V.Y), cfg.accxy_deadband);
+    accSum[X] += applyDeadband(lrintf(accel_ned.V.X), cfg.accxy_deadband);
+    accSum[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)
@@ -240,10 +236,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();
@@ -287,10 +283,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_ANGLES_25 = 1;
-    else
-        f.SMALL_ANGLES_25 = 0;
+    f.SMALL_ANGLE = (EstG.A[Z] > smallAngle);
 
     // Attitude of the estimated vector
     anglerad[ROLL] = atan2f(EstG.V.Y, EstG.V.Z);
@@ -394,11 +387,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);
-    vel_tmp = applyDeadband(vel_tmp, 5);
-    vario = vel_tmp;
+    vario = applyDeadband(vel_tmp, 5);
 
     // Altitude P-Controller
     error = constrain(AltHold - EstAlt, -500, 500);
@@ -407,7 +399,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,8 +408,8 @@ int getEstimatedAltitude(void)
     BaroPID += errorAltitudeI / 1024;     // I in range +/-200
 
     // D
-    accZ_old = accZ_tmp;
     BaroPID -= constrain(cfg.D8[PIDVEL] * (accZ_tmp + accZ_old) / 64, -150, 150);
+    accZ_old = accZ_tmp;
 
     return 1;
 }

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;

src/mw.h

@@ -318,7 +318,7 @@ typedef struct flags_t {
     uint8_t PASSTHRU_MODE;
     uint8_t GPS_FIX;
     uint8_t GPS_FIX_HOME;
-    uint8_t SMALL_ANGLES_25;
+    uint8_t SMALL_ANGLE;
     uint8_t CALIBRATE_MAG;
     uint8_t VARIO_MODE;
     uint8_t FIXED_WING;                     // set when in flying_wing or airplane mode. currently used by althold selection code