diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 5fc9d89372..7bcb841662 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -217,7 +217,7 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = { {"magADC", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), CONDITION(MAG)}, #endif #ifdef USE_BARO - {"BaroAlt", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), CONDITION(BARO)}, + {"baroAlt", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), CONDITION(BARO)}, #endif #ifdef USE_RANGEFINDER {"surfaceRaw", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), CONDITION(RANGEFINDER)}, @@ -319,7 +319,7 @@ typedef struct blackboxMainState_s { int32_t amperageLatest; #ifdef USE_BARO - int32_t BaroAlt; + int32_t baroAlt; #endif #ifdef USE_MAG int16_t magADC[XYZ_AXIS_COUNT]; @@ -614,7 +614,7 @@ static void writeIntraframe(void) #ifdef USE_BARO if (testBlackboxCondition(CONDITION(BARO))) { - blackboxWriteSignedVB(blackboxCurrent->BaroAlt); + blackboxWriteSignedVB(blackboxCurrent->baroAlt); } #endif @@ -762,7 +762,7 @@ static void writeInterframe(void) #ifdef USE_BARO if (testBlackboxCondition(CONDITION(BARO))) { - deltas[optionalFieldCount++] = blackboxCurrent->BaroAlt - blackboxLast->BaroAlt; + deltas[optionalFieldCount++] = blackboxCurrent->baroAlt - blackboxLast->baroAlt; } #endif @@ -1101,7 +1101,7 @@ static void loadMainState(timeUs_t currentTimeUs) blackboxCurrent->amperageLatest = getAmperageLatest(); #ifdef USE_BARO - blackboxCurrent->BaroAlt = baro.BaroAlt; + blackboxCurrent->baroAlt = baro.altitude; #endif #ifdef USE_RANGEFINDER diff --git a/src/main/fc/init.c b/src/main/fc/init.c index 419ac1eee1..5b82755bb0 100644 --- a/src/main/fc/init.c +++ b/src/main/fc/init.c @@ -98,6 +98,7 @@ #include "flight/gps_rescue.h" #include "flight/pid.h" #include "flight/pid_init.h" +#include "flight/position.h" #include "flight/servos.h" #include "io/asyncfatfs/asyncfatfs.h" diff --git a/src/main/fc/tasks.c b/src/main/fc/tasks.c index 419e136df9..0f63a46bb0 100644 --- a/src/main/fc/tasks.c +++ b/src/main/fc/tasks.c @@ -294,23 +294,7 @@ void taskUpdateRangefinder(timeUs_t currentTimeUs) #endif #if defined(USE_BARO) || defined(USE_GPS) -static bool taskAltitudeCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs) -{ - UNUSED(currentTimeUs); - -#if defined(USE_BARO) - if (isBaroSampleReady()) { - return true; - } -#endif - - if (currentDeltaTimeUs > getTask(TASK_ALTITUDE)->attribute->desiredPeriodUs) { - return true; - } - - return false; - } - static void taskCalculateAltitude(timeUs_t currentTimeUs) +static void taskCalculateAltitude(timeUs_t currentTimeUs) { UNUSED(currentTimeUs); calculateEstimatedAltitude(); @@ -395,7 +379,7 @@ task_attribute_t task_attributes[TASK_COUNT] = { #endif #if defined(USE_BARO) || defined(USE_GPS) - [TASK_ALTITUDE] = DEFINE_TASK("ALTITUDE", NULL, taskAltitudeCheck, taskCalculateAltitude, TASK_PERIOD_HZ(TASK_ALTITUDE_RATE_HZ), TASK_PRIORITY_LOW), + [TASK_ALTITUDE] = DEFINE_TASK("ALTITUDE", NULL, NULL, taskCalculateAltitude, TASK_PERIOD_HZ(TASK_ALTITUDE_RATE_HZ), TASK_PRIORITY_LOW), #endif #ifdef USE_DASHBOARD diff --git a/src/main/flight/position.h b/src/main/flight/position.h index 59e11603a6..3a3faacdb5 100644 --- a/src/main/flight/position.h +++ b/src/main/flight/position.h @@ -22,7 +22,8 @@ #include "common/time.h" -#define TASK_ALTITUDE_RATE_HZ 120 +#define TASK_ALTITUDE_RATE_HZ 100 + typedef struct positionConfig_s { uint8_t altitude_source; uint8_t altitude_prefer_baro; diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c index 64151e764a..b299079a2b 100644 --- a/src/main/sensors/barometer.c +++ b/src/main/sensors/barometer.c @@ -137,19 +137,13 @@ void pgResetFn_barometerConfig(barometerConfig_t *barometerConfig) barometerConfig->baro_xclr_tag = IO_TAG(BARO_XCLR_PIN); } +#define NUM_CALIBRATION_CYCLES 100 // 10 seconds init_delay + 100 * 25 ms = 12.5 seconds before valid baro altitude +#define NUM_GROUND_LEVEL_CYCLES 10 // calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking) + static uint16_t calibrationCycles = 0; // baro calibration = get new ground pressure value -static bool baroCalibrated = false; -static int32_t baroPressure = 0; -static int32_t baroTemperature = 0; static float baroGroundAltitude = 0.0f; -static float baroAltitudeRaw = 0.0f; - - -#define CALIBRATING_BARO_CYCLES 100 // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles -#define SET_GROUND_LEVEL_BARO_CYCLES 10 // calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking) - +static bool baroCalibrated = false; static bool baroReady = false; -static bool baroSampleReady = false; void baroPreInit(void) { @@ -296,8 +290,7 @@ static bool baroDetect(baroDev_t *baroDev, baroSensor_e baroHardwareToUse) void baroInit(void) { - baroDetect(&baro.dev, barometerConfig()->baro_hardware); - baroReady = true; + baroReady = baroDetect(&baro.dev, barometerConfig()->baro_hardware); } bool baroIsCalibrated(void) @@ -312,13 +305,15 @@ static void baroSetCalibrationCycles(uint16_t calibrationCyclesRequired) void baroStartCalibration(void) { - baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES); + baroSetCalibrationCycles(NUM_CALIBRATION_CYCLES); + baroGroundAltitude = 0; baroCalibrated = false; } void baroSetGroundLevel(void) { - baroSetCalibrationCycles(SET_GROUND_LEVEL_BARO_CYCLES); + baroSetCalibrationCycles(NUM_GROUND_LEVEL_CYCLES); + baroGroundAltitude = 0; baroCalibrated = false; } @@ -338,20 +333,13 @@ bool isBaroReady(void) return baroReady; } -bool isBaroSampleReady(void) -{ - if (baroSampleReady) { - baroSampleReady = false; - return true; - } - return false; - } - static float pressureToAltitude(const float pressure) { return (1.0f - powf(pressure / 101325.0f, 0.190295f)) * 4433000.0f; } +static void performBaroCalibrationCycle(const float altitude); + uint32_t baroUpdate(timeUs_t currentTimeUs) { static timeUs_t baroStateDurationUs[BARO_STATE_COUNT]; @@ -415,24 +403,23 @@ uint32_t baroUpdate(timeUs_t currentTimeUs) schedulerIgnoreTaskExecTime(); break; } - baro.dev.calculate(&baroPressure, &baroTemperature); - baro.baroPressure = baroPressure; - baro.baroTemperature = baroTemperature; - baroAltitudeRaw = pressureToAltitude(baroPressure); - - performBaroCalibrationCycle(); + // update baro data + baro.dev.calculate(&baro.pressure, &baro.temperature); + baro.altitude = pressureToAltitude(baro.pressure); if (baroIsCalibrated()) { - baro.BaroAlt = baroAltitudeRaw - baroGroundAltitude; + // zero baro altitude + baro.altitude -= baroGroundAltitude; } else { - baro.BaroAlt = 0.0f; + // establish stable baroGroundAltitude value to zero baro altitude with + performBaroCalibrationCycle(baro.altitude); + baro.altitude = 0.0f; } - DEBUG_SET(DEBUG_BARO, 1, baro.baroTemperature); - DEBUG_SET(DEBUG_BARO, 2, baro.BaroAlt); - - baroSampleReady = true; + DEBUG_SET(DEBUG_BARO, 1, lrintf(baro.pressure / 100.0f)); // hPa + DEBUG_SET(DEBUG_BARO, 2, baro.temperature); // c°C + DEBUG_SET(DEBUG_BARO, 3, lrintf(baro.altitude)); // cm if (baro.dev.combined_read) { state = BARO_STATE_PRESSURE_START; @@ -458,23 +445,22 @@ uint32_t baroUpdate(timeUs_t currentTimeUs) return sleepTime; } -// baroAltitude samples baro.BaroAlt the ALTITUDE task rate float getBaroAltitude(void) { - return baro.BaroAlt; + return baro.altitude; } -void performBaroCalibrationCycle(void) +static void performBaroCalibrationCycle(const float altitude) { static uint16_t cycleCount = 0; - if (!baroCalibrated) { - cycleCount++; - baroGroundAltitude += baroAltitudeRaw; - if (cycleCount == calibrationCycles) { - baroGroundAltitude = baroGroundAltitude / calibrationCycles; // simple average - baroCalibrated = true; - cycleCount = 0; - } + + baroGroundAltitude += altitude; + cycleCount++; + + if (cycleCount >= calibrationCycles) { + baroGroundAltitude /= cycleCount; // simple average + baroCalibrated = true; + cycleCount = 0; } } diff --git a/src/main/sensors/barometer.h b/src/main/sensors/barometer.h index c6c067151e..d95b033c48 100644 --- a/src/main/sensors/barometer.h +++ b/src/main/sensors/barometer.h @@ -22,7 +22,6 @@ #include "pg/pg.h" #include "drivers/barometer/barometer.h" -#include "flight/position.h" typedef enum { BARO_DEFAULT = 0, @@ -49,15 +48,13 @@ typedef struct barometerConfig_s { PG_DECLARE(barometerConfig_t, barometerConfig); -// #define TASK_BARO_DENOM 3 -// #define TASK_BARO_RATE_HZ (TASK_ALTITUDE_RATE_HZ / TASK_BARO_DENOM) -#define TASK_BARO_RATE_HZ 40 +#define TASK_BARO_RATE_HZ 40 // Will be overwritten by the baro device driver typedef struct baro_s { baroDev_t dev; - float BaroAlt; - int32_t baroTemperature; // Use temperature for telemetry - int32_t baroPressure; // Use pressure for telemetry + float altitude; + int32_t temperature; // Use temperature for telemetry + int32_t pressure; // Use pressure for telemetry } baro_t; extern baro_t baro; @@ -69,6 +66,4 @@ void baroStartCalibration(void); void baroSetGroundLevel(void); uint32_t baroUpdate(timeUs_t currentTimeUs); bool isBaroReady(void); -bool isBaroSampleReady(void); float getBaroAltitude(void); -void performBaroCalibrationCycle(void); diff --git a/src/main/telemetry/frsky_hub.c b/src/main/telemetry/frsky_hub.c index fdd302d23d..b028fc48c5 100644 --- a/src/main/telemetry/frsky_hub.c +++ b/src/main/telemetry/frsky_hub.c @@ -219,9 +219,9 @@ static void sendTemperature1(void) data = escData->dataAge < ESC_DATA_INVALID ? escData->temperature : 0; } #elif defined(USE_BARO) - data = (baro.baroTemperature + 50)/ 100; // Airmamaf + data = lrintf(baro.temperature / 100.0f); // Airmamaf #else - data = gyroGetTemperature() / 10; + data = lrintf(gyroGetTemperature() / 10.0f); #endif frSkyHubWriteFrame(ID_TEMPRATURE1, data); } diff --git a/src/main/telemetry/ibus_shared.c b/src/main/telemetry/ibus_shared.c index c3705bdfba..2518e97f82 100644 --- a/src/main/telemetry/ibus_shared.c +++ b/src/main/telemetry/ibus_shared.c @@ -32,6 +32,7 @@ #include #include #include +#include #include "platform.h" #include "telemetry/telemetry.h" @@ -219,7 +220,7 @@ static uint16_t getTemperature(void) uint16_t temperature = gyroGetTemperature() * 10; #if defined(USE_BARO) if (sensors(SENSOR_BARO)) { - temperature = (uint16_t) ((baro.baroTemperature + 50) / 10); + temperature = lrintf(baro.temperature / 10.0f); } #endif return temperature + IBUS_TEMPERATURE_OFFSET; @@ -425,10 +426,10 @@ static void setValue(uint8_t* bufferPtr, uint8_t sensorType, uint8_t length) #ifdef USE_BARO case IBUS_SENSOR_TYPE_ALT: case IBUS_SENSOR_TYPE_ALT_MAX: - value.int32 = baro.BaroAlt; + value.int32 = baro.altitude; break; case IBUS_SENSOR_TYPE_PRES: - value.uint32 = baro.baroPressure | (((uint32_t)getTemperature()) << 19); + value.uint32 = baro.pressure | (((uint32_t)getTemperature()) << 19); break; #endif #endif //defined(TELEMETRY_IBUS_EXTENDED) diff --git a/src/test/unit/rx_ibus_unittest.cc b/src/test/unit/rx_ibus_unittest.cc index b871dbf65b..32f933b087 100644 --- a/src/test/unit/rx_ibus_unittest.cc +++ b/src/test/unit/rx_ibus_unittest.cc @@ -42,7 +42,7 @@ extern "C" { uint8_t batteryCellCount = 3; float rcCommand[4] = {0, 0, 0, 0}; int16_t telemTemperature1 = 0; - baro_t baro = { .baroTemperature = 50 }; + baro_t baro = { .temperature = 50 }; telemetryConfig_t telemetryConfig_System; timeUs_t rxFrameTimeUs(void) { return 0; } } diff --git a/src/test/unit/rx_sumd_unittest.cc b/src/test/unit/rx_sumd_unittest.cc index 37fc58f470..bded0e4849 100644 --- a/src/test/unit/rx_sumd_unittest.cc +++ b/src/test/unit/rx_sumd_unittest.cc @@ -41,7 +41,7 @@ extern "C" { uint8_t batteryCellCount = 3; float rcCommand[4] = {0, 0, 0, 0}; int16_t telemTemperature1 = 0; - baro_t baro = { .baroTemperature = 50 }; + baro_t baro = { .temperature = 50 }; telemetryConfig_t telemetryConfig_System; timeUs_t rxFrameTimeUs(void) { return 0; } }