rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Some baro cleanups to avoid using or exposing incomplete and 

uninitialised baro data.

imu code size reduction (treym)
This commit is contained in:
Dominic Clifton 2014-06-24 00:14:30 +01:00
parent 2413130c0f
commit f127847bf2
4 changed files with 43 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/main/drivers/barometer_bmp085.c
View File

@ -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;

37
src/main/flight/imu.c
View File

@ -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;

35
src/main/sensors/barometer.c
View File

@ -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--;

2
src/main/sensors/barometer.h
View File

@ -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