Merge pull request #7927 from krzysztofmatula/km-odometer

Flight statistics (odometer) added.

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 \

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"
@@ -1412,6 +1413,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_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_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
 };
 
 const uint16_t valueTableEntryCount = ARRAYLEN(valueTable);

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) {

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

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"
 
@@ -371,6 +372,10 @@ 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 | ARMING_DISABLED_CRASH_DETECTED))) {
             beeper(BEEPER_DISARMING);      // emit disarm tone
@@ -485,6 +490,10 @@ void tryArm(void)
         beeper(BEEPER_ARMING);
 #endif
 
+#ifdef USE_PERSISTENT_STATS
+        statsOnArm();
+#endif
+
 #ifdef USE_RUNAWAY_TAKEOFF
         runawayTakeoffDeactivateUs = 0;
         runawayTakeoffAccumulatedUs = 0;
@@ -974,7 +983,12 @@ bool processRx(timeUs_t currentTimeUs)
 #endif
 
     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;
 }
 

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

src/main/fc/stats.c

@@ -0,0 +1,76 @@
+
+#include "platform.h"
+
+#ifdef USE_PERSISTENT_STATS
+
+#include "drivers/time.h"
+
+#include "fc/config.h"
+#include "fc/runtime_config.h"
+#include "fc/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 timeMs_t arm_millis;
+static uint32_t arm_distance_cm;
+static timeMs_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_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();
+        }
+    }
+}
+
+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

src/main/fc/stats.h

@@ -0,0 +1,4 @@
+
+void statsOnArm(void);
+void statsOnDisarm(void);
+void statsOnLoop(void);

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)

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 <http://www.gnu.org/licenses/>.
+ */
+
+#include "platform.h"
+
+#ifdef USE_PERSISTENT_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_s = 0,
+    .stats_total_dist_m = 0,
+);
+
+#endif

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 <http://www.gnu.org/licenses/>.
+ */
+
+#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; // [s]
+    uint32_t stats_total_dist_m; // [m]
+    uint8_t  stats_enabled;
+} statsConfig_t;
+
+PG_DECLARE(statsConfig_t, statsConfig);

src/main/target/common_pre.h

@@ -298,5 +298,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
 

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) {}