From 63304562966bf85c685019c7d81a9bf354f83b85 Mon Sep 17 00:00:00 2001 From: treymarc Date: Wed, 16 Apr 2014 22:48:37 +0000 Subject: [PATCH] remove unnecessary var, inline accsum_reset, lrintf , rename smallAngle --- src/imu.c | 56 +++++++++++++++++++++--------------------------------- src/main.c | 2 +- src/mw.c | 4 ++-- 3 files changed, 25 insertions(+), 37 deletions(-) diff --git a/src/imu.c b/src/imu.c index 324fbf3b2..4bb5cec09 100644 --- a/src/imu.c +++ b/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); - 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) -{ - 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); - normalizeV(&EstN.V, &EstN.V); + rotateV(&EstN.V, deltaGyroAngle); } // 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_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); @@ -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); - 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 +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(cfg.D8[PIDVEL] * (accZ_tmp + accZ_old) / 64, -150, 150); - + BaroPID -= constrain(lrintf(cfg.D8[PIDVEL] * accZ_tmp / 32), -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 */ diff --git a/src/main.c b/src/main.c index 58f60eb09..6855f8a71 100755 --- a/src/main.c +++ b/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) { diff --git a/src/mw.c b/src/mw.c index 218c98945..96303ad75 100755 --- a/src/mw.c +++ b/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;