Sub: Add AP_Stats

Co-authored-by: Peter Barker <pb-gh@barker.dropbear.id.au>

ArduSub/ArduSub.cpp

@@ -101,6 +101,9 @@ const AP_Scheduler::Task Sub::scheduler_tasks[] = {
 #if AP_GRIPPER_ENABLED
     SCHED_TASK_CLASS(AP_Gripper,          &sub.g2.gripper,   update,              10,  75,  75),
 #endif
+#if STATS_ENABLED == ENABLED
+    SCHED_TASK(stats_update,           1,    200,  76),
+#endif
 #ifdef USERHOOK_FASTLOOP
     SCHED_TASK(userhook_FastLoop,    100,     75,  78),
 #endif
@@ -347,4 +350,15 @@ bool Sub::get_wp_crosstrack_error_m(float &xtrack_error) const
     return true;
 }
 
+#if STATS_ENABLED == ENABLED
+/*
+  update AP_Stats
+*/
+void Sub::stats_update(void)
+{
+    g2.stats.set_flying(motors.armed());
+    g2.stats.update();
+}
+#endif
+
 AP_HAL_MAIN_CALLBACKS(&sub);

ArduSub/Parameters.cpp

@@ -622,7 +622,11 @@ const AP_Param::Info Sub::var_info[] = {
   2nd group of parameters
  */
 const AP_Param::GroupInfo ParametersG2::var_info[] = {
-
+#if STATS_ENABLED == ENABLED
+    // @Group: STAT
+    // @Path: ../libraries/AP_Stats/AP_Stats.cpp
+    AP_SUBGROUPINFO(stats, "STAT", 1, ParametersG2, AP_Stats),
+#endif
 #if HAL_PROXIMITY_ENABLED
     // @Group: PRX
     // @Path: ../libraries/AP_Proximity/AP_Proximity.cpp

ArduSub/Parameters.h

@@ -5,6 +5,7 @@
 #include <AP_Common/AP_Common.h>
 
 #include <AP_Gripper/AP_Gripper.h>
+#include <AP_Stats/AP_Stats.h>
 
 #if AP_SCRIPTING_ENABLED
 #include <AP_Scripting/AP_Scripting.h>
@@ -318,6 +319,10 @@ public:
 class ParametersG2 {
 public:
     ParametersG2(void);
+#if STATS_ENABLED == ENABLED
+    // vehicle statistics
+    AP_Stats stats;
+#endif
 
     // var_info for holding Parameter information
     static const struct AP_Param::GroupInfo var_info[];

ArduSub/Sub.h

@@ -573,6 +573,8 @@ private:
     bool surface_init(void);
     void surface_run();
 
+    void stats_update();
+
     uint16_t get_pilot_speed_dn() const;
 
     void convert_old_parameters(void);

ArduSub/config.h

@@ -190,6 +190,12 @@
 # define LOGGING_ENABLED                ENABLED
 #endif
 
+// Statistics
+#ifndef STATS_ENABLED
+ # define STATS_ENABLED (AP_STATS_ENABLED ? ENABLED : DISABLED)
+#endif
+
+
 // Default logging bitmask
 #ifndef DEFAULT_LOG_BITMASK
 # define DEFAULT_LOG_BITMASK \

ArduSub/system.cpp

@@ -19,6 +19,11 @@ void Sub::init_ardupilot()
     can_mgr.init();
 #endif
 
+#if STATS_ENABLED == ENABLED
+    // initialise stats module
+    g2.stats.init();
+#endif
+
     // init cargo gripper
 #if AP_GRIPPER_ENABLED
     g2.gripper.init();