From 36c8f1e22429e27682171b98805a520b87589e6d Mon Sep 17 00:00:00 2001 From: Krzysztof Matula Date: Wed, 13 Mar 2019 20:46:49 +0100 Subject: [PATCH 1/4] Flight statistics (odometer) added. Supported counters: - total flights count - total flight time - total flight distance (if GPS available) --- make/source.mk | 1 + src/main/cli/settings.c | 8 ++++ src/main/fc/config.c | 13 +++++++ src/main/fc/config.h | 3 ++ src/main/fc/core.c | 9 ++++- src/main/fc/rc_adjustments.c | 3 ++ src/main/fc/stats.c | 75 ++++++++++++++++++++++++++++++++++++ src/main/fc/stats.h | 15 ++++++++ src/main/pg/pg_ids.h | 3 +- src/main/pg/stats.c | 39 +++++++++++++++++++ src/main/pg/stats.h | 33 ++++++++++++++++ src/main/target/common_pre.h | 1 + 12 files changed, 201 insertions(+), 2 deletions(-) create mode 100644 src/main/fc/stats.c create mode 100644 src/main/fc/stats.h create mode 100644 src/main/pg/stats.c create mode 100644 src/main/pg/stats.h diff --git a/make/source.mk b/make/source.mk index 036fa32f6..f84c9b25f 100644 --- a/make/source.mk +++ b/make/source.mk @@ -46,6 +46,7 @@ COMMON_SRC = \ fc/hardfaults.c \ fc/tasks.c \ fc/runtime_config.c \ + fc/stats.c \ io/beeper.c \ io/piniobox.c \ io/serial.c \ diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index dd1f4c1ff..cd8fcc18e 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -87,6 +87,7 @@ #include "pg/usb.h" #include "pg/sdio.h" #include "pg/rcdevice.h" +#include "pg/stats.h" #include "rx/rx.h" #include "rx/cc2500_frsky_common.h" @@ -1415,6 +1416,13 @@ const clivalue_t valueTable[] = { { "flysky_spi_tx_id", VAR_UINT32 | MASTER_VALUE, .config.u32Max = UINT32_MAX, PG_FLYSKY_CONFIG, offsetof(flySkyConfig_t, txId) }, { "flysky_spi_rf_channels", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = 16, PG_FLYSKY_CONFIG, offsetof(flySkyConfig_t, rfChannelMap) }, #endif + +#ifdef USE_STATS + { "stats", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_STATS_CONFIG, offsetof(statsConfig_t, stats_enabled) }, + { "stats_total_flights", VAR_UINT32 | MASTER_VALUE, .config.u32Max = UINT32_MAX, PG_STATS_CONFIG, offsetof(statsConfig_t, stats_total_flights) }, + { "stats_total_time", VAR_UINT32 | MASTER_VALUE, .config.u32Max = UINT32_MAX, PG_STATS_CONFIG, offsetof(statsConfig_t, stats_total_time) }, + { "stats_total_dist", VAR_UINT32 | MASTER_VALUE, .config.u32Max = UINT32_MAX, PG_STATS_CONFIG, offsetof(statsConfig_t, stats_total_dist) }, +#endif }; const uint16_t valueTableEntryCount = ARRAYLEN(valueTable); diff --git a/src/main/fc/config.c b/src/main/fc/config.c index d0a9bd33d..cf72a214e 100644 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -67,6 +67,8 @@ #include "scheduler/scheduler.h" +static bool configIsDirty; /* someone indicated that the config is modified and it is not yet saved */ + pidProfile_t *currentPidProfile; #ifndef RX_SPI_DEFAULT_PROTOCOL @@ -619,6 +621,7 @@ static void ValidateAndWriteConfigToEEPROM(bool setConfigured) writeConfigToEEPROM(); resumeRxPwmPpmSignal(); + configIsDirty = false; } void writeEEPROM(void) @@ -658,6 +661,16 @@ void saveConfigAndNotify(void) beeperConfirmationBeeps(1); } +void setConfigDirty(void) +{ + configIsDirty = true; +} + +bool isConfigDirty(void) +{ + return configIsDirty; +} + void changePidProfileFromCellCount(uint8_t cellCount) { if (currentPidProfile->auto_profile_cell_count == cellCount || currentPidProfile->auto_profile_cell_count == AUTO_PROFILE_CELL_COUNT_STAY) { diff --git a/src/main/fc/config.h b/src/main/fc/config.h index bafae9d19..44c265566 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -63,6 +63,9 @@ void ensureEEPROMStructureIsValid(void); void saveConfigAndNotify(void); void validateAndFixGyroConfig(void); +void setConfigDirty(void); +bool isConfigDirty(void); + uint8_t getCurrentPidProfileIndex(void); void changePidProfile(uint8_t pidProfileIndex); void changePidProfileFromCellCount(uint8_t cellCount); diff --git a/src/main/fc/core.c b/src/main/fc/core.c index c7b7bafd1..b8860368c 100644 --- a/src/main/fc/core.c +++ b/src/main/fc/core.c @@ -65,6 +65,7 @@ #include "fc/rc_adjustments.h" #include "fc/rc_controls.h" #include "fc/runtime_config.h" +#include "fc/stats.h" #include "msp/msp_serial.h" @@ -370,6 +371,8 @@ void disarm(void) #endif flipOverAfterCrashActive = false; + statsOnDisarm(); + // if ARMING_DISABLED_RUNAWAY_TAKEOFF is set then we want to play it's beep pattern instead if (!(getArmingDisableFlags() & ARMING_DISABLED_RUNAWAY_TAKEOFF)) { beeper(BEEPER_DISARMING); // emit disarm tone @@ -483,6 +486,7 @@ void tryArm(void) #else beeper(BEEPER_ARMING); #endif + statsOnArm(); #ifdef USE_RUNAWAY_TAKEOFF runawayTakeoffDeactivateUs = 0; @@ -973,7 +977,10 @@ bool processRx(timeUs_t currentTimeUs) #endif pidSetAntiGravityState(IS_RC_MODE_ACTIVE(BOXANTIGRAVITY) || featureIsEnabled(FEATURE_ANTI_GRAVITY)); - + + /* allow the stats collector to do periodic tasks */ + statsOnLoop(); + return true; } diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c index 1a16d55cd..f4941f6c9 100644 --- a/src/main/fc/rc_adjustments.c +++ b/src/main/fc/rc_adjustments.c @@ -470,6 +470,7 @@ static int applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t a break; }; + setConfigDirty(); return newValue; } @@ -635,6 +636,7 @@ static int applyAbsoluteAdjustment(controlRateConfig_t *controlRateConfig, adjus break; }; + setConfigDirty(); return newValue; } @@ -694,6 +696,7 @@ static uint8_t applySelectAdjustment(adjustmentFunction_e adjustmentFunction, ui beeperConfirmationBeeps(beeps); } + setConfigDirty(); return position; } diff --git a/src/main/fc/stats.c b/src/main/fc/stats.c new file mode 100644 index 000000000..05917d98f --- /dev/null +++ b/src/main/fc/stats.c @@ -0,0 +1,75 @@ + +#include "platform.h" + +#ifdef USE_STATS + +#include "drivers/time.h" + +#include "fc/config.h" +#include "fc/runtime_config.h" +#include "fc/stats.h" +#include "pg/stats.h" + +#include "io/beeper.h" +#include "io/gps.h" + + +#define MIN_FLIGHT_TIME_TO_RECORD_STATS_S 10 //prevent recording stats for that short "flights" [s] +#define STATS_SAVE_DELAY_MS 500 //let disarming complete and save stats after this time + +static uint32_t arm_millis; +static uint32_t arm_distance_cm; +static uint32_t save_pending_millis; // 0 = none + +#ifdef USE_GPS + #define DISTANCE_FLOWN_CM (GPS_distanceFlownInCm) +#else + #define DISTANCE_FLOWN_CM (0) +#endif + +void statsOnArm(void) +{ + arm_millis = millis(); + arm_distance_cm = DISTANCE_FLOWN_CM; +} + +void statsOnDisarm(void) +{ + if (statsConfig()->stats_enabled) { + uint32_t dt = (millis() - arm_millis) / 1000; + if (dt >= MIN_FLIGHT_TIME_TO_RECORD_STATS_S) { + statsConfigMutable()->stats_total_flights += 1; //arm/flight counter + statsConfigMutable()->stats_total_time += dt; //[s] + statsConfigMutable()->stats_total_dist += (DISTANCE_FLOWN_CM - arm_distance_cm) / 100; //[m] + /* signal that stats need to be saved but don't execute time consuming flash operation + now - let the disarming process complete and then execute the actual save */ + save_pending_millis = millis(); + } + } +} + +void statsOnLoop(void) +{ + /* check for pending flash write */ + if (save_pending_millis && millis()-save_pending_millis > STATS_SAVE_DELAY_MS) { + if (ARMING_FLAG(ARMED)) { + /* re-armed - don't save! */ + } + else { + if (isConfigDirty()) { + /* There are some adjustments made in the configuration and we don't want + to implicitly save it... We can't currently save part of the configuration, + so we simply don't execute the stats save operation at all. This will result + in missing stats update *if* rc-adjustments were made during the flight. */ + } + else { + writeEEPROM(); + /* repeat disarming beep indicating the stats save is complete */ + beeper(BEEPER_DISARMING); + } + } + save_pending_millis = 0; + } +} + +#endif diff --git a/src/main/fc/stats.h b/src/main/fc/stats.h new file mode 100644 index 000000000..8061b6cee --- /dev/null +++ b/src/main/fc/stats.h @@ -0,0 +1,15 @@ + +#pragma once +#ifdef USE_STATS + +void statsOnArm(void); +void statsOnDisarm(void); +void statsOnLoop(void); + +#else + +#define statsOnArm() do {} while (0) +#define statsOnDisarm() do {} while (0) +#define statsOnLoop() do {} while (0) + +#endif diff --git a/src/main/pg/pg_ids.h b/src/main/pg/pg_ids.h index 3b67e65b4..9abc9712a 100644 --- a/src/main/pg/pg_ids.h +++ b/src/main/pg/pg_ids.h @@ -141,7 +141,8 @@ #define PG_RPM_FILTER_CONFIG 544 #define PG_LED_STRIP_STATUS_MODE_CONFIG 545 // Used to hold the configuration for the LED_STRIP status mode (not built on targets with limited flash) #define PG_VTX_TABLE_CONFIG 546 -#define PG_BETAFLIGHT_END 546 +#define PG_STATS_CONFIG 547 +#define PG_BETAFLIGHT_END 547 // OSD configuration (subject to change) diff --git a/src/main/pg/stats.c b/src/main/pg/stats.c new file mode 100644 index 000000000..c8c8d140d --- /dev/null +++ b/src/main/pg/stats.c @@ -0,0 +1,39 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#include "platform.h" + +#ifdef USE_STATS + +#include "pg/pg.h" +#include "pg/pg_ids.h" + +#include "stats.h" + +PG_REGISTER_WITH_RESET_TEMPLATE(statsConfig_t, statsConfig, PG_STATS_CONFIG, 1); + +PG_RESET_TEMPLATE(statsConfig_t, statsConfig, + .stats_enabled = 0, + .stats_total_flights = 0, + .stats_total_time = 0, + .stats_total_dist = 0, +); + +#endif diff --git a/src/main/pg/stats.h b/src/main/pg/stats.h new file mode 100644 index 000000000..940d36cc7 --- /dev/null +++ b/src/main/pg/stats.h @@ -0,0 +1,33 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#pragma once + +#include "pg/pg.h" +#include "drivers/io_types.h" + +typedef struct statsConfig_s { + uint32_t stats_total_flights; + uint32_t stats_total_time; // [s] + uint32_t stats_total_dist; // [m] + uint8_t stats_enabled; +} statsConfig_t; + +PG_DECLARE(statsConfig_t, statsConfig); diff --git a/src/main/target/common_pre.h b/src/main/target/common_pre.h index edf8d029c..24bc97d3e 100644 --- a/src/main/target/common_pre.h +++ b/src/main/target/common_pre.h @@ -178,6 +178,7 @@ #define USE_SERIALRX_FPORT // FrSky FPort #define USE_TELEMETRY_CRSF #define USE_TELEMETRY_SRXL +#define USE_STATS #if ((FLASH_SIZE > 256) || (FEATURE_CUT_LEVEL < 12)) #define USE_CMS From a0b410b9021319c4b5c81b09eafab84a8e66beff Mon Sep 17 00:00:00 2001 From: Krzysztof Matula Date: Sat, 6 Apr 2019 21:13:33 +0200 Subject: [PATCH 2/4] unit tests update --- src/test/unit/rc_controls_unittest.cc | 1 + 1 file changed, 1 insertion(+) diff --git a/src/test/unit/rc_controls_unittest.cc b/src/test/unit/rc_controls_unittest.cc index 567da6ab7..afb835dfc 100644 --- a/src/test/unit/rc_controls_unittest.cc +++ b/src/test/unit/rc_controls_unittest.cc @@ -700,6 +700,7 @@ TEST_F(RcControlsAdjustmentsTest, processPIDIncreasePidController2) #endif extern "C" { +void setConfigDirty(void) {} void saveConfigAndNotify(void) {} void initRcProcessing(void) {} void changePidProfile(uint8_t) {} From fea686f2b5f29a8cb7aaa5591364a5f6320fb3fc Mon Sep 17 00:00:00 2001 From: Krzysztof Matula Date: Sat, 6 Apr 2019 22:22:35 +0200 Subject: [PATCH 3/4] enable stats only on targets with > 256M flash --- src/main/target/common_pre.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/target/common_pre.h b/src/main/target/common_pre.h index 24bc97d3e..05129748f 100644 --- a/src/main/target/common_pre.h +++ b/src/main/target/common_pre.h @@ -178,13 +178,13 @@ #define USE_SERIALRX_FPORT // FrSky FPort #define USE_TELEMETRY_CRSF #define USE_TELEMETRY_SRXL -#define USE_STATS #if ((FLASH_SIZE > 256) || (FEATURE_CUT_LEVEL < 12)) #define USE_CMS #define USE_MSP_DISPLAYPORT #define USE_MSP_OVER_TELEMETRY #define USE_LED_STRIP +#define USE_STATS #endif #if ((FLASH_SIZE > 256) || (FEATURE_CUT_LEVEL < 11)) From eb99c7e0d1a48875dfed4b4abb44500a92234bc7 Mon Sep 17 00:00:00 2001 From: Krzysztof Matula Date: Sun, 7 Apr 2019 12:07:30 +0200 Subject: [PATCH 4/4] stats: - cosmetic changes from code review - not enabling stats on any F3 (not enough flash) --- src/main/cli/settings.c | 6 +++--- src/main/fc/core.c | 7 +++++++ src/main/fc/stats.c | 13 +++++++------ src/main/fc/stats.h | 11 ----------- src/main/pg/stats.c | 6 +++--- src/main/pg/stats.h | 4 ++-- src/main/target/common_pre.h | 2 +- 7 files changed, 23 insertions(+), 26 deletions(-) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index cd8fcc18e..4fb2463db 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1417,11 +1417,11 @@ const clivalue_t valueTable[] = { { "flysky_spi_rf_channels", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = 16, PG_FLYSKY_CONFIG, offsetof(flySkyConfig_t, rfChannelMap) }, #endif -#ifdef USE_STATS +#ifdef USE_PERSISTENT_STATS { "stats", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_STATS_CONFIG, offsetof(statsConfig_t, stats_enabled) }, { "stats_total_flights", VAR_UINT32 | MASTER_VALUE, .config.u32Max = UINT32_MAX, PG_STATS_CONFIG, offsetof(statsConfig_t, stats_total_flights) }, - { "stats_total_time", VAR_UINT32 | MASTER_VALUE, .config.u32Max = UINT32_MAX, PG_STATS_CONFIG, offsetof(statsConfig_t, stats_total_time) }, - { "stats_total_dist", VAR_UINT32 | MASTER_VALUE, .config.u32Max = UINT32_MAX, PG_STATS_CONFIG, offsetof(statsConfig_t, stats_total_dist) }, + { "stats_total_time_s", VAR_UINT32 | MASTER_VALUE, .config.u32Max = UINT32_MAX, PG_STATS_CONFIG, offsetof(statsConfig_t, stats_total_time_s) }, + { "stats_total_dist_m", VAR_UINT32 | MASTER_VALUE, .config.u32Max = UINT32_MAX, PG_STATS_CONFIG, offsetof(statsConfig_t, stats_total_dist_m) }, #endif }; diff --git a/src/main/fc/core.c b/src/main/fc/core.c index b8860368c..427fc38ef 100644 --- a/src/main/fc/core.c +++ b/src/main/fc/core.c @@ -371,7 +371,9 @@ void disarm(void) #endif flipOverAfterCrashActive = false; +#ifdef USE_PERSISTENT_STATS statsOnDisarm(); +#endif // if ARMING_DISABLED_RUNAWAY_TAKEOFF is set then we want to play it's beep pattern instead if (!(getArmingDisableFlags() & ARMING_DISABLED_RUNAWAY_TAKEOFF)) { @@ -486,7 +488,10 @@ void tryArm(void) #else beeper(BEEPER_ARMING); #endif + +#ifdef USE_PERSISTENT_STATS statsOnArm(); +#endif #ifdef USE_RUNAWAY_TAKEOFF runawayTakeoffDeactivateUs = 0; @@ -978,8 +983,10 @@ bool processRx(timeUs_t currentTimeUs) pidSetAntiGravityState(IS_RC_MODE_ACTIVE(BOXANTIGRAVITY) || featureIsEnabled(FEATURE_ANTI_GRAVITY)); +#ifdef USE_PERSISTENT_STATS /* allow the stats collector to do periodic tasks */ statsOnLoop(); +#endif return true; } diff --git a/src/main/fc/stats.c b/src/main/fc/stats.c index 05917d98f..8bc030385 100644 --- a/src/main/fc/stats.c +++ b/src/main/fc/stats.c @@ -1,25 +1,26 @@ #include "platform.h" -#ifdef USE_STATS +#ifdef USE_PERSISTENT_STATS #include "drivers/time.h" #include "fc/config.h" #include "fc/runtime_config.h" #include "fc/stats.h" -#include "pg/stats.h" #include "io/beeper.h" #include "io/gps.h" +#include "pg/stats.h" + #define MIN_FLIGHT_TIME_TO_RECORD_STATS_S 10 //prevent recording stats for that short "flights" [s] #define STATS_SAVE_DELAY_MS 500 //let disarming complete and save stats after this time -static uint32_t arm_millis; +static timeMs_t arm_millis; static uint32_t arm_distance_cm; -static uint32_t save_pending_millis; // 0 = none +static timeMs_t save_pending_millis; // 0 = none #ifdef USE_GPS #define DISTANCE_FLOWN_CM (GPS_distanceFlownInCm) @@ -39,8 +40,8 @@ void statsOnDisarm(void) uint32_t dt = (millis() - arm_millis) / 1000; if (dt >= MIN_FLIGHT_TIME_TO_RECORD_STATS_S) { statsConfigMutable()->stats_total_flights += 1; //arm/flight counter - statsConfigMutable()->stats_total_time += dt; //[s] - statsConfigMutable()->stats_total_dist += (DISTANCE_FLOWN_CM - arm_distance_cm) / 100; //[m] + statsConfigMutable()->stats_total_time_s += dt; //[s] + statsConfigMutable()->stats_total_dist_m += (DISTANCE_FLOWN_CM - arm_distance_cm) / 100; //[m] /* signal that stats need to be saved but don't execute time consuming flash operation now - let the disarming process complete and then execute the actual save */ save_pending_millis = millis(); diff --git a/src/main/fc/stats.h b/src/main/fc/stats.h index 8061b6cee..d9a7336fb 100644 --- a/src/main/fc/stats.h +++ b/src/main/fc/stats.h @@ -1,15 +1,4 @@ -#pragma once -#ifdef USE_STATS - void statsOnArm(void); void statsOnDisarm(void); void statsOnLoop(void); - -#else - -#define statsOnArm() do {} while (0) -#define statsOnDisarm() do {} while (0) -#define statsOnLoop() do {} while (0) - -#endif diff --git a/src/main/pg/stats.c b/src/main/pg/stats.c index c8c8d140d..6b9acc1b5 100644 --- a/src/main/pg/stats.c +++ b/src/main/pg/stats.c @@ -20,7 +20,7 @@ #include "platform.h" -#ifdef USE_STATS +#ifdef USE_PERSISTENT_STATS #include "pg/pg.h" #include "pg/pg_ids.h" @@ -32,8 +32,8 @@ PG_REGISTER_WITH_RESET_TEMPLATE(statsConfig_t, statsConfig, PG_STATS_CONFIG, 1); PG_RESET_TEMPLATE(statsConfig_t, statsConfig, .stats_enabled = 0, .stats_total_flights = 0, - .stats_total_time = 0, - .stats_total_dist = 0, + .stats_total_time_s = 0, + .stats_total_dist_m = 0, ); #endif diff --git a/src/main/pg/stats.h b/src/main/pg/stats.h index 940d36cc7..5e02c211d 100644 --- a/src/main/pg/stats.h +++ b/src/main/pg/stats.h @@ -25,8 +25,8 @@ typedef struct statsConfig_s { uint32_t stats_total_flights; - uint32_t stats_total_time; // [s] - uint32_t stats_total_dist; // [m] + uint32_t stats_total_time_s; // [s] + uint32_t stats_total_dist_m; // [m] uint8_t stats_enabled; } statsConfig_t; diff --git a/src/main/target/common_pre.h b/src/main/target/common_pre.h index 05129748f..3a9155834 100644 --- a/src/main/target/common_pre.h +++ b/src/main/target/common_pre.h @@ -184,7 +184,6 @@ #define USE_MSP_DISPLAYPORT #define USE_MSP_OVER_TELEMETRY #define USE_LED_STRIP -#define USE_STATS #endif #if ((FLASH_SIZE > 256) || (FEATURE_CUT_LEVEL < 11)) @@ -300,5 +299,6 @@ #define USE_TELEMETRY_SENSORS_DISABLED_DETAILS // Re-enable this after 4.0 has been released, and remove the define from STM32F4DISCOVERY //#define USE_VTX_TABLE +#define USE_PERSISTENT_STATS #endif