From f127847bf2838cdeaba4876e9a18311d47a14900 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Tue, 24 Jun 2014 00:14:30 +0100 Subject: [PATCH] Some baro cleanups to avoid using or exposing incomplete and uninitialised baro data. imu code size reduction (treym) --- src/main/drivers/barometer_bmp085.c | 4 ++-- src/main/flight/imu.c | 37 ++++++++++++++--------------- src/main/sensors/barometer.c | 35 +++++++++++++++++---------- src/main/sensors/barometer.h | 2 +- 4 files changed, 43 insertions(+), 35 deletions(-) diff --git a/src/main/drivers/barometer_bmp085.c b/src/main/drivers/barometer_bmp085.c index a9c4a10cf6..e90073ed93 100755 --- a/src/main/drivers/barometer_bmp085.c +++ b/src/main/drivers/barometer_bmp085.c @@ -167,8 +167,8 @@ bool bmp085Detect(baro_t *baro) bmp085.al_version = BMP085_GET_BITSLICE(data, BMP085_AL_VERSION); /* get AL Version */ bmp085_get_cal_param(); /* readout bmp085 calibparam structure */ bmp085InitDone = true; - baro->ut_delay = 6000; - baro->up_delay = 27000; + baro->ut_delay = 6000; // 1.5ms margin according to the spec (4.5ms T convetion time) + baro->up_delay = 27000; // 6000+21000=27000 1.5ms margin according to the spec (25.5ms P convetion time with OSS=3) baro->start_ut = bmp085_start_ut; baro->get_ut = bmp085_get_ut; baro->start_up = bmp085_start_up; diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 8471c8ff92..603b14c142 100755 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -307,12 +307,6 @@ static void getEstimatedAttitude(void) accMag = accMag * 100 / ((int32_t)acc_1G * acc_1G); rotateV(&EstG.V, &deltaGyroAngle); - if (sensors(SENSOR_MAG)) { - rotateV(&EstM.V, &deltaGyroAngle); - } else { - rotateV(&EstN.V, &deltaGyroAngle); - normalizeV(&EstN.V, &EstN.V); - } // Apply complimentary filter (Gyro drift correction) // If accel magnitude >1.15G or <0.85G and ACC vector outside of the limit range => we neutralize the effect of accelerometers in the angle estimation. @@ -325,14 +319,6 @@ static void getEstimatedAttitude(void) EstG.A[axis] = (EstG.A[axis] * imuRuntimeConfig->gyro_cmpf_factor + accSmooth[axis]) * invGyroComplimentaryFilterFactor; } - // FIXME what does the _M_ mean? - float invGyroComplimentaryFilter_M_Factor = (1.0f / (imuRuntimeConfig->gyro_cmpfm_factor + 1.0f)); - - if (sensors(SENSOR_MAG)) { - for (axis = 0; axis < 3; axis++) - EstM.A[axis] = (EstM.A[axis] * imuRuntimeConfig->gyro_cmpfm_factor + magADC[axis]) * invGyroComplimentaryFilter_M_Factor; - } - f.SMALL_ANGLE = (EstG.A[Z] > smallAngle); // Attitude of the estimated vector @@ -341,10 +327,19 @@ static void getEstimatedAttitude(void) inclination.values.rollDeciDegrees = lrintf(anglerad[AI_ROLL] * (1800.0f / M_PI)); inclination.values.pitchDeciDegrees = lrintf(anglerad[AI_PITCH] * (1800.0f / M_PI)); - if (sensors(SENSOR_MAG)) + if (sensors(SENSOR_MAG)) { + rotateV(&EstM.V, &deltaGyroAngle); + // FIXME what does the _M_ mean? + float invGyroComplimentaryFilter_M_Factor = (1.0f / (imuRuntimeConfig->gyro_cmpfm_factor + 1.0f)); + for (axis = 0; axis < 3; axis++) { + EstM.A[axis] = (EstM.A[axis] * imuRuntimeConfig->gyro_cmpfm_factor + magADC[axis]) * invGyroComplimentaryFilter_M_Factor; + } heading = calculateHeading(&EstM); - else + } else { + rotateV(&EstN.V, &deltaGyroAngle); + normalizeV(&EstN.V, &EstN.V); heading = calculateHeading(&EstN); + } acc_calc(deltaT); // rotate acc vector into earth frame } @@ -433,7 +428,6 @@ void calculateEstimatedAltitude(uint32_t currentTime) accAlt = 0; } BaroAlt = baroCalculateAltitude(); - dt = accTimeSum * 1e-6f; // delta acc reading time in seconds // Integrator - velocity, cm/sec @@ -443,16 +437,21 @@ void calculateEstimatedAltitude(uint32_t currentTime) // Integrator - Altitude in cm accAlt += (vel_acc * 0.5f) * dt + vel * dt; // integrate velocity to get distance (x= a/2 * t^2) accAlt = accAlt * barometerConfig->baro_cf_alt + (float)BaroAlt * (1.0f - barometerConfig->baro_cf_alt); // complementary filter for Altitude estimation (baro & acc) - EstAlt = accAlt; vel += vel_acc; + accSum_reset(); + + if (!isBaroCalibrationComplete()) { + return; + } + #if 1 debug[1] = accSum[2] / accSumCount; // acceleration debug[2] = vel; // velocity debug[3] = accAlt; // height #endif - accSum_reset(); + EstAlt = accAlt; baroVel = (BaroAlt - lastBaroAlt) * 1000000.0f / dTime; lastBaroAlt = BaroAlt; diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c index 808dbb0948..d7c56e7a13 100644 --- a/src/main/sensors/barometer.c +++ b/src/main/sensors/barometer.c @@ -55,6 +55,10 @@ void baroSetCalibrationCycles(uint16_t calibrationCyclesRequired) calibratingB = calibrationCyclesRequired; } +static bool baroReady = false; + +#define PRESSURE_SAMPLE_COUNT (barometerConfig->baro_sample_count - 1) + static uint32_t recalculateBarometerTotal(uint8_t baroSampleCount, uint32_t pressureTotal, int32_t newPressureReading) { static int32_t barometerSamples[BARO_SAMPLE_COUNT_MAX]; @@ -65,10 +69,12 @@ static uint32_t recalculateBarometerTotal(uint8_t baroSampleCount, uint32_t pres nextSampleIndex = (currentSampleIndex + 1); if (nextSampleIndex == baroSampleCount) { nextSampleIndex = 0; + baroReady = true; } barometerSamples[currentSampleIndex] = newPressureReading; // recalculate pressure total + // Note, the pressure total is made up of baroSampleCount - 1 samples - See PRESSURE_SAMPLE_COUNT pressureTotal += barometerSamples[currentSampleIndex]; pressureTotal -= barometerSamples[nextSampleIndex]; @@ -79,10 +85,10 @@ static uint32_t recalculateBarometerTotal(uint8_t baroSampleCount, uint32_t pres typedef enum { BAROMETER_NEEDS_SAMPLES = 0, - BAROMETER_NEEDS_CALCULATION + BAROMETER_NEEDS_CALCULATION, + BAROMETER_NEEDS_PROCESSING } barometerState_e; -static bool baroReady = false; bool isBaroReady(void) { return baroReady; @@ -99,22 +105,25 @@ void baroUpdate(uint32_t currentTime) baroDeadline = currentTime; switch (state) { + case BAROMETER_NEEDS_SAMPLES: + baro.get_ut(); + baro.start_up(); + state = BAROMETER_NEEDS_CALCULATION; + baroDeadline += baro.up_delay; + break; + case BAROMETER_NEEDS_CALCULATION: baro.get_up(); baro.start_ut(); baroDeadline += baro.ut_delay; baro.calculate(&baroPressure, &baroTemperature); - baroReady = true; - state = BAROMETER_NEEDS_SAMPLES; - break; + state = BAROMETER_NEEDS_PROCESSING; + break; - case BAROMETER_NEEDS_SAMPLES: - baro.get_ut(); - baro.start_up(); + case BAROMETER_NEEDS_PROCESSING: + state = BAROMETER_NEEDS_SAMPLES; baroPressureSum = recalculateBarometerTotal(barometerConfig->baro_sample_count, baroPressureSum, baroPressure); - state = BAROMETER_NEEDS_CALCULATION; - baroDeadline += baro.up_delay; - break; + break; } } @@ -124,7 +133,7 @@ int32_t baroCalculateAltitude(void) // calculates height from ground via baro readings // see: https://github.com/diydrones/ardupilot/blob/master/libraries/AP_Baro/AP_Baro.cpp#L140 - BaroAlt_tmp = lrintf((1.0f - powf((float)(baroPressureSum / (barometerConfig->baro_sample_count - 1)) / 101325.0f, 0.190295f)) * 4433000.0f); // in cm + BaroAlt_tmp = lrintf((1.0f - powf((float)(baroPressureSum / PRESSURE_SAMPLE_COUNT) / 101325.0f, 0.190295f)) * 4433000.0f); // in cm BaroAlt_tmp -= baroGroundAltitude; BaroAlt = lrintf((float)BaroAlt * barometerConfig->baro_noise_lpf + (float)BaroAlt_tmp * (1.0f - barometerConfig->baro_noise_lpf)); // additional LPF to reduce baro noise @@ -134,7 +143,7 @@ int32_t baroCalculateAltitude(void) void performBaroCalibrationCycle(void) { baroGroundPressure -= baroGroundPressure / 8; - baroGroundPressure += baroPressureSum / (barometerConfig->baro_sample_count - 1); + baroGroundPressure += baroPressureSum / PRESSURE_SAMPLE_COUNT; baroGroundAltitude = (1.0f - powf((baroGroundPressure / 8) / 101325.0f, 0.190295f)) * 4433000.0f; calibratingB--; diff --git a/src/main/sensors/barometer.h b/src/main/sensors/barometer.h index b829857840..2ec3570eb8 100644 --- a/src/main/sensors/barometer.h +++ b/src/main/sensors/barometer.h @@ -20,7 +20,7 @@ #define BARO_SAMPLE_COUNT_MAX 48 typedef struct barometerConfig_s { - uint8_t baro_sample_count; // size of baro filter array + uint8_t baro_sample_count; // size of baro filter array float baro_noise_lpf; // additional LPF to reduce baro noise float baro_cf_vel; // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity) float baro_cf_alt; // apply CF to use ACC for height estimation