From b27d74cf1a9f712e5177df2234d679f9f543b155 Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Fri, 15 Jul 2022 18:38:40 -0300 Subject: [PATCH 1/5] Add Baro Healthy function --- src/main/sensors/barometer.c | 32 ++++++++++++++++++++++++++------ src/main/sensors/barometer.h | 11 ++++++++--- 2 files changed, 34 insertions(+), 9 deletions(-) diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c index a99fc08cf5e..0ba3faae345 100644 --- a/src/main/sensors/barometer.c +++ b/src/main/sensors/barometer.c @@ -60,6 +60,10 @@ baro_t baro; // barometer access functions #ifdef USE_BARO +// timeouts for health reporting +#define BARO_TIMEOUT_MS 500 // timeout in ms since last successful read +#define BARO_DATA_CHANGE_TIMEOUT_MS 2000 // timeout in ms since last successful read that involved temperature of pressure changing + PG_REGISTER_WITH_RESET_TEMPLATE(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 4); PG_RESET_TEMPLATE(barometerConfig_t, barometerConfig, @@ -67,6 +71,11 @@ PG_RESET_TEMPLATE(barometerConfig_t, barometerConfig, .baro_calibration_tolerance = SETTING_BARO_CAL_TOLERANCE_DEFAULT ); +typedef enum { + BAROMETER_NEEDS_SAMPLES = 0, + BAROMETER_NEEDS_CALCULATION +} barometerState_e; + static zeroCalibrationScalar_t zeroCalibration; static float baroGroundAltitude = 0; static float baroGroundPressure = 101325.0f; // 101325 pascal, 1 standard atmosphere @@ -247,11 +256,6 @@ bool baroInit(void) return true; } -typedef enum { - BAROMETER_NEEDS_SAMPLES = 0, - BAROMETER_NEEDS_CALCULATION -} barometerState_e; - uint32_t baroUpdate(void) { static barometerState_e state = BAROMETER_NEEDS_SAMPLES; @@ -276,7 +280,20 @@ uint32_t baroUpdate(void) if (baro.dev.start_ut) { baro.dev.start_ut(&baro.dev); } + baro.dev.calculate(&baro.dev, &baro.baroPressure, &baro.baroTemperature); + + timeMs_t now = millis(); + + // Check for changes in data values + if ((baro.baroLastPressure != baro.baroPressure) || (baro.baroLastTemperature != baro.baroTemperature)) { + baro.lastChangeMs = now; + } + + baro.baroLastPressure = baro.baroPressure; + baro.baroLastTemperature = baro.baroTemperature; + baro.lastUpdateMs = now; + state = BAROMETER_NEEDS_SAMPLES; return baro.dev.ut_delay; break; @@ -343,7 +360,10 @@ int16_t baroGetTemperature(void) bool baroIsHealthy(void) { - return true; + // Consider a sensor as healthy if it has had an update in the last 0.5 seconds and values are non-zero and have changed within the last 2 seconds + const timeMs_t now = millis(); + + return (now - baro.lastUpdateMs < BARO_TIMEOUT_MS) && (now - baro.lastChangeMs < BARO_DATA_CHANGE_TIMEOUT_MS) && (baro.baroLastPressure != 0); } #endif /* BARO */ diff --git a/src/main/sensors/barometer.h b/src/main/sensors/barometer.h index c2722e7d3c9..c375426928a 100644 --- a/src/main/sensors/barometer.h +++ b/src/main/sensors/barometer.h @@ -41,8 +41,14 @@ typedef enum { typedef struct baro_s { baroDev_t dev; int32_t BaroAlt; - int32_t baroTemperature; // Use temperature for telemetry - int32_t baroPressure; // Use pressure for telemetry + int32_t baroTemperature; + int32_t baroPressure; + + int32_t baroLastTemperature; + int32_t baroLastPressure; + + timeMs_t lastChangeMs; + timeMs_t lastUpdateMs; } baro_t; extern baro_t baro; @@ -56,7 +62,6 @@ typedef struct barometerConfig_s { PG_DECLARE(barometerConfig_t, barometerConfig); - bool baroInit(void); bool baroIsCalibrationComplete(void); void baroStartCalibration(void); From b118b9805e71505132968480d939968c6dbeef8c Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Fri, 15 Jul 2022 19:33:25 -0300 Subject: [PATCH 2/5] add const --- src/main/sensors/barometer.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c index 0ba3faae345..dec79800fb5 100644 --- a/src/main/sensors/barometer.c +++ b/src/main/sensors/barometer.c @@ -283,7 +283,7 @@ uint32_t baroUpdate(void) baro.dev.calculate(&baro.dev, &baro.baroPressure, &baro.baroTemperature); - timeMs_t now = millis(); + const timeMs_t now = millis(); // Check for changes in data values if ((baro.baroLastPressure != baro.baroPressure) || (baro.baroLastTemperature != baro.baroTemperature)) { From 7117e0c1b8c87f0a0785901632876c4524e1cc97 Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Fri, 15 Jul 2022 19:41:50 -0300 Subject: [PATCH 3/5] remove space --- src/main/sensors/barometer.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/sensors/barometer.h b/src/main/sensors/barometer.h index c375426928a..0f7965acfab 100644 --- a/src/main/sensors/barometer.h +++ b/src/main/sensors/barometer.h @@ -43,10 +43,8 @@ typedef struct baro_s { int32_t BaroAlt; int32_t baroTemperature; int32_t baroPressure; - int32_t baroLastTemperature; int32_t baroLastPressure; - timeMs_t lastChangeMs; timeMs_t lastUpdateMs; } baro_t; From 1d23b5e5e76ccdc58e6c544e1bd688988901bb42 Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Fri, 15 Jul 2022 19:45:51 -0300 Subject: [PATCH 4/5] fix coments --- src/main/sensors/barometer.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c index dec79800fb5..b3075264076 100644 --- a/src/main/sensors/barometer.c +++ b/src/main/sensors/barometer.c @@ -60,9 +60,9 @@ baro_t baro; // barometer access functions #ifdef USE_BARO -// timeouts for health reporting -#define BARO_TIMEOUT_MS 500 // timeout in ms since last successful read -#define BARO_DATA_CHANGE_TIMEOUT_MS 2000 // timeout in ms since last successful read that involved temperature of pressure changing +// Timeouts for health reporting +#define BARO_TIMEOUT_MS 500 // Timeout in ms since last successful read +#define BARO_DATA_CHANGE_TIMEOUT_MS 2000 // Timeout in ms since last successful read that involved temperature of pressure changing PG_REGISTER_WITH_RESET_TEMPLATE(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 4); From 5471003d8d973c1da55bca933ba93ff45da6ab0f Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Fri, 15 Jul 2022 20:05:16 -0300 Subject: [PATCH 5/5] fix compilation --- src/main/sensors/barometer.c | 1 - src/main/sensors/barometer.h | 1 + 2 files changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c index b3075264076..d6429de5dbd 100644 --- a/src/main/sensors/barometer.c +++ b/src/main/sensors/barometer.c @@ -25,7 +25,6 @@ #include "common/calibration.h" #include "common/log.h" #include "common/maths.h" -#include "common/time.h" #include "common/utils.h" #include "config/parameter_group.h" diff --git a/src/main/sensors/barometer.h b/src/main/sensors/barometer.h index 0f7965acfab..b3d87ce85a9 100644 --- a/src/main/sensors/barometer.h +++ b/src/main/sensors/barometer.h @@ -20,6 +20,7 @@ #include "config/parameter_group.h" #include "drivers/barometer/barometer.h" +#include "common/time.h" typedef enum { BARO_NONE = 0,