diff --git a/src/config.c b/src/config.c index 61dd3c5bf3..8618843f70 100755 --- a/src/config.c +++ b/src/config.c @@ -14,6 +14,7 @@ #include "statusindicator.h" #include "sensors_acceleration.h" +#include "sensors_barometer.h" #include "telemetry_common.h" #include "gps_common.h" @@ -90,8 +91,11 @@ void activateConfig(void) gpsUseProfile(¤tProfile.gpsProfile); gpsUsePIDs(¤tProfile.pidProfile); useFailsafeConfig(¤tProfile.failsafeConfig); - setAccelerationTrims(&masterConfig.accZero); + +#ifdef BARO + useBarometerConfig(¤tProfile.barometerConfig); +#endif } void readEEPROM(void) @@ -223,6 +227,14 @@ void resetGpsProfile(gpsProfile_t *gpsProfile) gpsProfile->ap_mode = 40; } +void resetBarometerConfig(barometerConfig_t *barometerConfig) +{ + barometerConfig->baro_sample_count = 21; + barometerConfig->baro_noise_lpf = 0.6f; + barometerConfig->baro_cf_vel = 0.985f; + barometerConfig->baro_cf_alt = 0.965f; +} + // Default settings static void resetConf(void) { @@ -306,15 +318,16 @@ static void resetConf(void) currentProfile.controlRateConfig.thrExpo8 = 0; // for (i = 0; i < CHECKBOXITEMS; i++) // cfg.activate[i] = 0; + resetRollAndPitchTrims(¤tProfile.accelerometerTrims); + currentProfile.mag_declination = 0; // For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero. currentProfile.acc_lpf_factor = 4; currentProfile.accz_deadband = 40; currentProfile.accxy_deadband = 40; - currentProfile.baro_tab_size = 21; - currentProfile.baro_noise_lpf = 0.6f; - currentProfile.baro_cf_vel = 0.985f; - currentProfile.baro_cf_alt = 0.965f; + + resetBarometerConfig(¤tProfile.barometerConfig); + currentProfile.acc_unarmedcal = 1; // Radio diff --git a/src/config_profile.h b/src/config_profile.h index b7dc748b07..f3c828c98d 100644 --- a/src/config_profile.h +++ b/src/config_profile.h @@ -16,10 +16,10 @@ typedef struct profile_s { uint8_t acc_lpf_factor; // Set the Low Pass Filter factor for ACC. Increasing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time. Zero = no filter uint8_t accz_deadband; // set the acc deadband for z-Axis, this ignores small accelerations uint8_t accxy_deadband; // set the acc deadband for xy-Axis - uint8_t baro_tab_size; // 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 + + barometerConfig_t barometerConfig; + + uint8_t acc_unarmedcal; // turn automatic acc compensation on/off uint16_t activate[CHECKBOX_ITEM_COUNT]; // activate switches diff --git a/src/flight_imu.c b/src/flight_imu.c index a144a1428d..c5b6904ff5 100644 --- a/src/flight_imu.c +++ b/src/flight_imu.c @@ -54,11 +54,7 @@ int32_t vario = 0; // variometer in cm/s int16_t throttleAngleCorrection = 0; // correction of throttle in lateral wind, float throttleAngleScale; -int32_t baroPressure = 0; -int32_t baroTemperature = 0; -uint32_t baroPressureSum = 0; int32_t BaroPID = 0; -int32_t BaroAlt = 0; float magneticDeclination = 0.0f; // calculated at startup from config @@ -400,7 +396,7 @@ int getEstimatedAltitude(void) // Integrator - Altitude in cm accAlt += (vel_acc * 0.5f) * dt + vel * dt; // integrate velocity to get distance (x= a/2 * t^2) - accAlt = accAlt * currentProfile.baro_cf_alt + (float)BaroAlt * (1.0f - currentProfile.baro_cf_alt); // complementary filter for Altitude estimation (baro & acc) + accAlt = accAlt * currentProfile.barometerConfig.baro_cf_alt + (float)BaroAlt * (1.0f - currentProfile.barometerConfig.baro_cf_alt); // complementary filter for Altitude estimation (baro & acc) EstAlt = accAlt; vel += vel_acc; @@ -420,7 +416,7 @@ 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 * currentProfile.baro_cf_vel + baroVel * (1 - currentProfile.baro_cf_vel); + vel = vel * currentProfile.barometerConfig.baro_cf_vel + baroVel * (1 - currentProfile.barometerConfig.baro_cf_vel); vel_tmp = lrintf(vel); // set vario diff --git a/src/mw.c b/src/mw.c index 19efbdd2b7..17b2d595e0 100755 --- a/src/mw.c +++ b/src/mw.c @@ -489,7 +489,7 @@ void loop(void) case 1: taskOrder++; #ifdef BARO - if (sensors(SENSOR_BARO) && baroUpdate()) + if (sensors(SENSOR_BARO) && baroUpdate(currentTime) != BAROMETER_ACTION_NOT_READY) break; #endif case 2: diff --git a/src/mw.h b/src/mw.h index 1398f72ee8..261cf15e64 100755 --- a/src/mw.h +++ b/src/mw.h @@ -7,7 +7,6 @@ /* for VBAT monitoring frequency */ #define VBATFREQ 6 // to read battery voltage - nth number of loop iterations -#define BARO_TAB_SIZE_MAX 48 #define VERSION 230 @@ -20,6 +19,7 @@ enum { ALIGN_MAG = 2 }; +#include "sensors_barometer.h" #include "serial_common.h" #include "rc_controls.h" #include "rx_common.h" diff --git a/src/sensors_barometer.c b/src/sensors_barometer.c index 517e67d0ab..58b1f97cd9 100644 --- a/src/sensors_barometer.c +++ b/src/sensors_barometer.c @@ -1,61 +1,97 @@ -#include "board.h" -#include "mw.h" +#include +#include +#include + +#include "platform.h" #include "drivers/barometer_common.h" +#include "config.h" + +#include "sensors_barometer.h" baro_t baro; // barometer access functions uint16_t calibratingB = 0; // baro calibration = get new ground pressure value +int32_t baroPressure = 0; +int32_t baroTemperature = 0; +uint32_t baroPressureSum = 0; +int32_t BaroAlt = 0; #ifdef BARO static int32_t baroGroundAltitude = 0; static int32_t baroGroundPressure = 0; +barometerConfig_t *barometerConfig; + +void useBarometerConfig(barometerConfig_t *barometerConfigToUse) +{ + barometerConfig = barometerConfigToUse; +} + +bool isBaroCalibrationComplete(void) +{ + return calibratingB == 0; +} + void baroSetCalibrationCycles(uint16_t calibrationCyclesRequired) { calibratingB = calibrationCyclesRequired; } -void baroCommon(void) +static uint32_t recalculateBarometerTotal(uint8_t baroSampleCount, uint32_t pressureTotal, int32_t newPressureReading) { - static int32_t baroHistTab[BARO_TAB_SIZE_MAX]; - static int baroHistIdx; - int indexplus1; + static int32_t barometerSamples[BARO_SAMPLE_COUNT_MAX]; + static int currentSampleIndex = 0; + int nextSampleIndex; - indexplus1 = (baroHistIdx + 1); - if (indexplus1 == currentProfile.baro_tab_size) - indexplus1 = 0; - baroHistTab[baroHistIdx] = baroPressure; - baroPressureSum += baroHistTab[baroHistIdx]; - baroPressureSum -= baroHistTab[indexplus1]; - baroHistIdx = indexplus1; + // store current pressure in barometerSamples + nextSampleIndex = (currentSampleIndex + 1); + if (nextSampleIndex == baroSampleCount) { + nextSampleIndex = 0; + } + barometerSamples[currentSampleIndex] = newPressureReading; + + // recalculate pressure total + pressureTotal += barometerSamples[currentSampleIndex]; + pressureTotal -= barometerSamples[nextSampleIndex]; + + currentSampleIndex = nextSampleIndex; + + return pressureTotal; } +typedef enum { + BAROMETER_NEEDS_SAMPLES = 0, + BAROMETER_NEEDS_CALCULATION +} barometerState_e; -int baroUpdate(void) +barometerAction_e baroUpdate(uint32_t currentTime) { static uint32_t baroDeadline = 0; - static int state = 0; + static barometerState_e state = BAROMETER_NEEDS_SAMPLES; if ((int32_t)(currentTime - baroDeadline) < 0) - return 0; + return BAROMETER_ACTION_NOT_READY; baroDeadline = currentTime; - if (state) { - baro.get_up(); - baro.start_ut(); - baroDeadline += baro.ut_delay; - baro.calculate(&baroPressure, &baroTemperature); - state = 0; - return 2; - } else { - baro.get_ut(); - baro.start_up(); - baroCommon(); - state = 1; - baroDeadline += baro.up_delay; - return 1; + switch (state) { + case BAROMETER_NEEDS_CALCULATION: + baro.get_up(); + baro.start_ut(); + baroDeadline += baro.ut_delay; + baro.calculate(&baroPressure, &baroTemperature); + state = BAROMETER_NEEDS_SAMPLES; + return BAROMETER_ACTION_PERFORMED_CALCULATION; + + case BAROMETER_NEEDS_SAMPLES: + default: + baro.get_ut(); + baro.start_up(); + baroPressureSum = recalculateBarometerTotal(barometerConfig->baro_sample_count, baroPressureSum, baroPressure); + state = BAROMETER_NEEDS_CALCULATION; + baroDeadline += baro.up_delay; + return BAROMETER_ACTION_OBTAINED_SAMPLES; } } @@ -65,9 +101,9 @@ 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 / (currentProfile.baro_tab_size - 1)) / 101325.0f, 0.190295f)) * 4433000.0f); // in cm + BaroAlt_tmp = lrintf((1.0f - powf((float)(baroPressureSum / (barometerConfig->baro_sample_count - 1)) / 101325.0f, 0.190295f)) * 4433000.0f); // in cm BaroAlt_tmp -= baroGroundAltitude; - BaroAlt = lrintf((float)BaroAlt * currentProfile.baro_noise_lpf + (float)BaroAlt_tmp * (1.0f - currentProfile.baro_noise_lpf)); // additional LPF to reduce baro noise + BaroAlt = lrintf((float)BaroAlt * barometerConfig->baro_noise_lpf + (float)BaroAlt_tmp * (1.0f - barometerConfig->baro_noise_lpf)); // additional LPF to reduce baro noise return BaroAlt; } @@ -75,14 +111,10 @@ int32_t baroCalculateAltitude(void) void performBaroCalibrationCycle(void) { baroGroundPressure -= baroGroundPressure / 8; - baroGroundPressure += baroPressureSum / (currentProfile.baro_tab_size - 1); + baroGroundPressure += baroPressureSum / (barometerConfig->baro_sample_count - 1); baroGroundAltitude = (1.0f - powf((baroGroundPressure / 8) / 101325.0f, 0.190295f)) * 4433000.0f; calibratingB--; } -bool isBaroCalibrationComplete(void) -{ - return calibratingB == 0; -} #endif /* BARO */ diff --git a/src/sensors_barometer.h b/src/sensors_barometer.h index a48d6722ff..4f6efd8622 100644 --- a/src/sensors_barometer.h +++ b/src/sensors_barometer.h @@ -1,10 +1,27 @@ #pragma once +#define BARO_SAMPLE_COUNT_MAX 48 + +typedef struct barometerConfig_s { + 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 +} barometerConfig_t; + +typedef enum { + BAROMETER_ACTION_NOT_READY = 0, + BAROMETER_ACTION_OBTAINED_SAMPLES, + BAROMETER_ACTION_PERFORMED_CALCULATION +} barometerAction_e; + +extern int32_t BaroAlt; + #ifdef BARO -void baroSetCalibrationCycles(uint16_t calibrationCyclesRequired); -void baroCommon(void); -int baroUpdate(void); -int32_t baroCalculateAltitude(void); +void useBarometerConfig(barometerConfig_t *barometerConfigToUse); bool isBaroCalibrationComplete(void); +void baroSetCalibrationCycles(uint16_t calibrationCyclesRequired); +barometerAction_e baroUpdate(uint32_t currentTime); +int32_t baroCalculateAltitude(void); void performBaroCalibrationCycle(void); #endif diff --git a/src/serial_cli.c b/src/serial_cli.c index cb4dc39811..4abbb16866 100644 --- a/src/serial_cli.c +++ b/src/serial_cli.c @@ -196,10 +196,10 @@ const clivalue_t valueTable[] = { { "acc_unarmedcal", VAR_UINT8, ¤tProfile.acc_unarmedcal, 0, 1 }, { "acc_trim_pitch", VAR_INT16, ¤tProfile.accelerometerTrims.trims.pitch, -300, 300 }, { "acc_trim_roll", VAR_INT16, ¤tProfile.accelerometerTrims.trims.roll, -300, 300 }, - { "baro_tab_size", VAR_UINT8, ¤tProfile.baro_tab_size, 0, BARO_TAB_SIZE_MAX }, - { "baro_noise_lpf", VAR_FLOAT, ¤tProfile.baro_noise_lpf, 0, 1 }, - { "baro_cf_vel", VAR_FLOAT, ¤tProfile.baro_cf_vel, 0, 1 }, - { "baro_cf_alt", VAR_FLOAT, ¤tProfile.baro_cf_alt, 0, 1 }, + { "baro_tab_size", VAR_UINT8, ¤tProfile.barometerConfig.baro_sample_count, 0, BARO_SAMPLE_COUNT_MAX }, + { "baro_noise_lpf", VAR_FLOAT, ¤tProfile.barometerConfig.baro_noise_lpf, 0, 1 }, + { "baro_cf_vel", VAR_FLOAT, ¤tProfile.barometerConfig.baro_cf_vel, 0, 1 }, + { "baro_cf_alt", VAR_FLOAT, ¤tProfile.barometerConfig.baro_cf_alt, 0, 1 }, { "mag_declination", VAR_INT16, ¤tProfile.mag_declination, -18000, 18000 }, { "gps_pos_p", VAR_UINT8, ¤tProfile.pidProfile.P8[PIDPOS], 0, 200 }, { "gps_pos_i", VAR_UINT8, ¤tProfile.pidProfile.I8[PIDPOS], 0, 200 },