diff --git a/libraries/AP_Stats/AP_Stats.cpp b/libraries/AP_Stats/AP_Stats.cpp index 7f249750f0..cdf123b819 100644 --- a/libraries/AP_Stats/AP_Stats.cpp +++ b/libraries/AP_Stats/AP_Stats.cpp @@ -1,5 +1,9 @@ #include "AP_Stats.h" +#include + +const extern AP_HAL::HAL& hal; + // table of user settable parameters const AP_Param::GroupInfo AP_Stats::var_info[] = { @@ -21,16 +25,28 @@ const AP_Param::GroupInfo AP_Stats::var_info[] = { // @User: Standard AP_GROUPINFO("_RUNTIME", 2, AP_Stats, params.runtime, 0), + // @Param: _RESET + // @DisplayName: Reset time + // @Description: Seconds since epoch since reset (set to 0 to reset) + // @User: Standard + AP_GROUPINFO("_RESET", 3, AP_Stats, params.reset, 1), + AP_GROUPEND }; +void AP_Stats::copy_variables_from_parameters() +{ + flttime = params.flttime; + runtime = params.runtime; + reset = params.reset; +} + void AP_Stats::init() { params.bootcount.set_and_save(params.bootcount+1); // initialise our variables from parameters: - flttime = params.flttime; - runtime = params.runtime; + copy_variables_from_parameters(); } @@ -67,6 +83,16 @@ void AP_Stats::update() flush(); last_flush_ms = now_ms; } + + const uint32_t params_reset = params.reset; + if (params_reset != reset || params_reset == 0) { + params.bootcount.set_and_save(params_reset == 0 ? 1 : 0); + params.flttime.set_and_save(0); + params.runtime.set_and_save(0); + params.reset.set_and_save(hal.util->get_system_clock_ms() / 1000); + copy_variables_from_parameters(); + } + } void AP_Stats::set_flying(const bool is_flying) diff --git a/libraries/AP_Stats/AP_Stats.h b/libraries/AP_Stats/AP_Stats.h index f927ccafe7..3bf49ac82f 100644 --- a/libraries/AP_Stats/AP_Stats.h +++ b/libraries/AP_Stats/AP_Stats.h @@ -15,6 +15,7 @@ public: // init() to set initial values from the parameters! uint32_t flttime; // seconds in flight (or driving) uint32_t runtime; // total wallclock time spent running ArduPilot (seconds) + uint32_t reset; // last time parameters were reset void init(); @@ -35,8 +36,11 @@ private: AP_Int16 bootcount; AP_Int32 flttime; AP_Int32 runtime; + AP_Int32 reset; } params; + void copy_variables_from_parameters(); + uint64_t last_flush_ms; // in terms of system uptime const uint16_t flush_interval_ms = 30000;