Line data    Source code

       1             : #include "AP_Stats.h"
       2             : 
       3             : #include <AP_Math/AP_Math.h>
       4             : #include <AP_RTC/AP_RTC.h>
       5             : #include <AP_AHRS/AP_AHRS.h>
       6             : 
       7             : const extern AP_HAL::HAL& hal;
       8             : 
       9             : // table of user settable parameters
      10             : const AP_Param::GroupInfo AP_Stats::var_info[] = {
      11             : 
      12             :     // @Param: _BOOTCNT
      13             :     // @DisplayName: Boot Count
      14             :     // @Description: Number of times board has been booted
      15             :     // @ReadOnly: True
      16             :     // @User: Standard
      17             :     AP_GROUPINFO("_BOOTCNT",    0, AP_Stats, params.bootcount, 0),
      18             : 
      19             :     // @Param: _FLTTIME
      20             :     // @DisplayName: Total FlightTime
      21             :     // @Description: Total FlightTime (seconds)
      22             :     // @Units: s
      23             :     // @ReadOnly: True
      24             :     // @User: Standard
      25             :     AP_GROUPINFO("_FLTTIME",    1, AP_Stats, params.flttime, 0),
      26             : 
      27             :     // @Param: _RUNTIME
      28             :     // @DisplayName: Total RunTime
      29             :     // @Description: Total time autopilot has run
      30             :     // @Units: s
      31             :     // @ReadOnly: True
      32             :     // @User: Standard
      33             :     AP_GROUPINFO("_RUNTIME",    2, AP_Stats, params.runtime, 0),
      34             : 
      35             :     // @Param: _RESET
      36             :     // @DisplayName: Statistics Reset Time
      37             :     // @Description: Seconds since January 1st 2016 (Unix epoch+1451606400) since statistics reset (set to 0 to reset statistics, other set values will be ignored)
      38             :     // @Units: s
      39             :     // @ReadOnly: True
      40             :     // @User: Standard
      41             :     AP_GROUPINFO("_RESET",    3, AP_Stats, params.reset, 1),
      42             : 
      43             :     // @Param: _FLTCNT
      44             :     // @DisplayName: Total Flight Count
      45             :     // @Description: Total number of flights
      46             :     // @ReadOnly: True
      47             :     // @User: Standard
      48             :     AP_GROUPINFO("_FLTCNT",    4, AP_Stats, params.fltcount, 0),
      49             : 
      50             :     // @Param: _DISTFLWN
      51             :     // @DisplayName: Total Distance Flown
      52             :     // @Description: Estimate of total distance flown since statistics reset
      53             :     // @Units: m
      54             :     // @ReadOnly: True
      55             :     // @User: Standard
      56             :     AP_GROUPINFO("_DISTFLWN",    5, AP_Stats, params.distance_flown_m, 0),
      57             : 
      58             :     AP_GROUPEND
      59             : };
      60             : 
      61             : AP_Stats *AP_Stats::_singleton;
      62             : 
      63             : // constructor
      64        2098 : AP_Stats::AP_Stats(void)
      65             : {
      66        2098 :     AP_Param::setup_object_defaults(this, var_info);
      67        2098 :     _singleton = this;
      68        2098 : }
      69             : 
      70        2094 : void AP_Stats::copy_variables_from_parameters()
      71             : {
      72        2094 :     flttime = params.flttime;
      73        2094 :     runtime = params.runtime;
      74        2094 :     reset = params.reset;
      75        2094 :     flttime_boot = flttime;
      76        2094 :     fltcount = params.fltcount;
      77        2094 :     distance_flown_m = params.distance_flown_m;
      78        2094 : }
      79             : 
      80        2094 : void AP_Stats::init()
      81             : {
      82        2094 :     params.bootcount.set_and_save(params.bootcount+1);
      83             : 
      84             :     // initialise our variables from parameters:
      85        2094 :     copy_variables_from_parameters();
      86        2094 : }
      87             : 
      88        4990 : void AP_Stats::flush()
      89             : {
      90        4990 :     params.flttime.set_and_save_ifchanged(flttime);
      91        4990 :     params.runtime.set_and_save_ifchanged(runtime);
      92        4990 :     params.fltcount.set_and_save_ifchanged(fltcount);
      93        4990 :     params.distance_flown_m.set_and_save_ifchanged(distance_flown_m);
      94        4990 :     last_flush_ms = AP_HAL::millis();
      95        4990 : }
      96             : 
      97        4990 : void AP_Stats::update_flighttime()
      98             : {
      99        4990 :     if (_flying_ms) {
     100        3908 :         WITH_SEMAPHORE(sem);
     101        3908 :         const uint32_t now = AP_HAL::millis();
     102        3908 :         const uint32_t delta = (now - _flying_ms)/1000;
     103        3908 :         flttime += delta;
     104        3908 :         _flying_ms += delta*1000;
     105        3908 :     }
     106        4990 : }
     107             : 
     108        4990 : void AP_Stats::update_runtime()
     109             : {
     110        4990 :     const uint32_t now = AP_HAL::millis();
     111        4990 :     const uint32_t delta = (now - _last_runtime_ms)/1000;
     112        4990 :     runtime += delta;
     113        4990 :     _last_runtime_ms += delta*1000;
     114        4990 : }
     115             : 
     116      149217 : void AP_Stats::update()
     117             : {
     118      149217 :     WITH_SEMAPHORE(sem);
     119      149217 :     const uint32_t now_ms = AP_HAL::millis();
     120             : #if AP_AHRS_ENABLED
     121      149217 :     update_distance_flown();
     122             : #endif  // AP_AHRS_ENABLED
     123      149217 :     if (now_ms -  last_flush_ms > flush_interval_ms) {
     124        4114 :         update_flighttime();
     125        4114 :         update_runtime();
     126        4114 :         flush();
     127             :     }
     128      149217 :     const uint32_t params_reset = params.reset;
     129      149217 :     if (params_reset == 0) {
     130             :         // Only reset statistics if the user explicitly sets AP_STATS_RESET parameter to zero.
     131             :         // This allows users to load parameter files (in MP, MAVProxy or any other GCS) without
     132             :         // accidentally resetting the statistics, because the AP_STATS_RESET value contained in
     133             :         // the parameter file will be ignored (unless it is zero and it is usually not zero).
     134             :         // The other statistics parameters are read-only, and the GCS should be clever enough to not set those.
     135           0 :         params.bootcount.set_and_save_ifchanged(0);
     136           0 :         params.flttime.set_and_save_ifchanged(0);
     137           0 :         params.runtime.set_and_save_ifchanged(0);
     138           0 :         params.fltcount.set_and_save_ifchanged(0);
     139           0 :         params.distance_flown_m.set_and_save_ifchanged(0);
     140           0 :         uint32_t system_clock = 0; // in seconds
     141             : #if AP_RTC_ENABLED
     142             :         uint64_t rtc_clock_us;
     143           0 :         if (AP::rtc().get_utc_usec(rtc_clock_us)) {
     144           0 :             system_clock = rtc_clock_us / 1000000;
     145             :             // can't store Unix seconds in a 32-bit float.  Change the
     146             :             // time base to Jan 1st 2016:
     147           0 :             system_clock -= 1451606400;
     148             :         }
     149             : #endif
     150           0 :         params.reset.set_and_save_ifchanged(system_clock);
     151           0 :         copy_variables_from_parameters();
     152             :         // Reset distance tracking state
     153           0 :         _last_position_valid = false;
     154             :     }
     155      149217 : }
     156             : 
     157      294970 : void AP_Stats::set_flying(const bool is_flying)
     158             : {
     159      294970 :     if (is_flying) {
     160      205347 :         if (!_flying_ms) {
     161         927 :             fltcount += 1;
     162         927 :             _flying_ms = AP_HAL::millis();
     163             :         }
     164             :     } else {
     165       89623 :         if (_flying_ms) {
     166         876 :             update_flighttime();
     167         876 :             update_runtime();
     168         876 :             update_distance_flown();
     169         876 :             flush();
     170             :         }
     171       89623 :         _flying_ms = 0;
     172             :     }
     173      294970 : }
     174             : 
     175             : #if AP_AHRS_ENABLED
     176      150093 : void AP_Stats::update_distance_flown()
     177             : {
     178      150093 :     if (!_flying_ms) {
     179             :         // Dont count unless flying
     180       55421 :         _last_position_valid = false;
     181       69453 :         return;
     182             :     }
     183             : 
     184             :     // Run at 1Hz to reduce how much GPS variance is counted as distance
     185       94672 :     const uint32_t now_ms = AP_HAL::millis();
     186       94672 :     if (now_ms - _last_distance_update_ms < 1000) {
     187       13061 :         return;
     188             :     }
     189       81611 :     _last_distance_update_ms = now_ms;
     190             : 
     191       81611 :     const AP_AHRS &ahrs = AP::ahrs();
     192       81611 :     if (!ahrs.healthy()) {
     193         377 :         _last_position_valid = false;
     194         377 :         return;
     195             :     }
     196             : 
     197       81234 :     Location current_loc;
     198       81234 :     if (!ahrs.get_location(current_loc)) {
     199         594 :         _last_position_valid = false;
     200         594 :         return;
     201             :     }
     202             : 
     203       80640 :     if (_last_position_valid) {
     204       79715 :         float delta_m = _last_position.get_distance_NED(current_loc).length();
     205             : 
     206             :         // Avoid counting small distances that are probably error
     207       79715 :         if (delta_m >= 0.5) {
     208             :             // Dont blindly add large distances that are likely just gross error / spoofing
     209             :             // Assumes vehicle traveling below mach ~0.9
     210       59399 :             if (delta_m > 300.0f) {
     211          41 :                 delta_m = 300.0f;
     212             :             }
     213       59399 :             distance_flown_m += delta_m;
     214             :         }
     215             :     }
     216             : 
     217       80640 :     _last_position = current_loc;
     218       80640 :     _last_position_valid = true;
     219             : }
     220             : #endif  // AP_AHRS_ENABLED
     221             : 
     222             : /*
     223             :   get time in flight since boot
     224             :  */
     225           0 : uint32_t AP_Stats::get_flight_time_s(void)
     226             : {
     227           0 :     update_flighttime();
     228           0 :     return flttime - flttime_boot;
     229             : }
     230             : 
     231      294970 : AP_Stats *AP::stats(void)
     232             : {
     233      294970 :     return AP_Stats::get_singleton();
     234             : }