From 63304562966bf85c685019c7d81a9bf354f83b85 Mon Sep 17 00:00:00 2001 From: treymarc Date: Wed, 16 Apr 2014 22:48:37 +0000 Subject: [PATCH 1/4] 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 324fbf3b28..4bb5cec09e 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 58f60eb092..6855f8a71d 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 218c98945a..96303ad753 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; From f6024c5406cf91c36460fbd301fa8e70e4d1448f Mon Sep 17 00:00:00 2001 From: treymarc Date: Wed, 16 Apr 2014 22:51:03 +0000 Subject: [PATCH 2/4] update header --- src/mw.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/mw.h b/src/mw.h index 473a38c6c6..64077c98c5 100755 --- a/src/mw.h +++ b/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 From 5468a9cfa061f3c158ab980f7fbe4ea92ad75a99 Mon Sep 17 00:00:00 2001 From: treymarc Date: Thu, 17 Apr 2014 20:25:08 +0000 Subject: [PATCH 3/4] correct lpf for d term renormalize vect (is that required or the real cause of the gyro headfree?) --- src/imu.c | 25 ++++++++++++++----------- 1 file changed, 14 insertions(+), 11 deletions(-) diff --git a/src/imu.c b/src/imu.c index 4bb5cec09e..ae757194e4 100644 --- a/src/imu.c +++ b/src/imu.c @@ -206,6 +206,14 @@ 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) @@ -258,7 +266,8 @@ 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) @@ -321,6 +330,7 @@ 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; @@ -366,12 +376,7 @@ int getEstimatedAltitude(void) debug[2] = accAlt; // height #endif - // Integrator done - reset accSum - accSum[0] = 0; - accSum[1] = 0; - accSum[2] = 0; - accSumCount = 0; - accTimeSum = 0; + accSum_reset(); baroVel = (BaroAlt - lastBaroAlt) * 1000000.0f / dTime; lastBaroAlt = BaroAlt; @@ -403,10 +408,8 @@ int getEstimatedAltitude(void) BaroPID += errorAltitudeI / 1024; // I in range +/-200 // D - 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; + BaroPID -= constrain(cfg.D8[PIDVEL] * (accZ_tmp + accZ_old) / 64, -150, 150); + accZ_old = accZ_tmp; return 1; } #endif /* BARO */ From e39636cc00beeb538a7249e8101eb7755218a587 Mon Sep 17 00:00:00 2001 From: treymarc Date: Thu, 17 Apr 2014 20:45:09 +0000 Subject: [PATCH 4/4] format --- src/imu.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/imu.c b/src/imu.c index ae757194e4..70a1fb51cd 100644 --- a/src/imu.c +++ b/src/imu.c @@ -409,7 +409,8 @@ int getEstimatedAltitude(void) // D BaroPID -= constrain(cfg.D8[PIDVEL] * (accZ_tmp + accZ_old) / 64, -150, 150); - accZ_old = accZ_tmp; + accZ_old = accZ_tmp; + return 1; } #endif /* BARO */