Rover: Support new battery interface

APMrover2/APMrover2.cpp

@@ -97,6 +97,8 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] = {
 #endif
 };
 
+constexpr int8_t Rover::_failsafe_priorities[7];
+
 #if STATS_ENABLED == ENABLED
 /*
   update AP_Stats

APMrover2/GCS_Mavlink.h

@@ -3,7 +3,7 @@
 #include <GCS_MAVLink/GCS.h>
 
 // default sensors are present and healthy: gyro, accelerometer, rate_control, attitude_stabilization, yaw_position, altitude control, x/y position control, motor_control
-#define MAVLINK_SENSOR_PRESENT_DEFAULT (MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL | MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL | MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION | MAV_SYS_STATUS_SENSOR_YAW_POSITION | MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL | MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS | MAV_SYS_STATUS_AHRS)
+#define MAVLINK_SENSOR_PRESENT_DEFAULT (MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL | MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL | MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION | MAV_SYS_STATUS_SENSOR_YAW_POSITION | MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL | MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS | MAV_SYS_STATUS_AHRS | MAV_SYS_STATUS_SENSOR_BATTERY)
 
 class GCS_MAVLINK_Rover : public GCS_MAVLINK
 {

APMrover2/Rover.h

@@ -292,7 +292,9 @@ private:
     aux_switch_pos aux_ch7;
 
     // Battery Sensors
-    AP_BattMonitor battery{MASK_LOG_CURRENT};
+    AP_BattMonitor battery{MASK_LOG_CURRENT,
+                           FUNCTOR_BIND_MEMBER(&Rover::handle_battery_failsafe, void, const char*, const int8_t),
+                           _failsafe_priorities};
 
 #if FRSKY_TELEM_ENABLED == ENABLED
     // FrSky telemetry support
@@ -457,6 +459,7 @@ private:
 
     // failsafe.cpp
     void failsafe_trigger(uint8_t failsafe_type, bool on);
+    void handle_battery_failsafe(const char* type_str, const int8_t action);
 #if ADVANCED_FAILSAFE == ENABLED
     void afs_fs_check(void);
 #endif
@@ -553,6 +556,28 @@ private:
     bool disarm_motors(void);
     bool is_boat() const;
 
+    enum Failsafe_Action {
+        Failsafe_Action_None          = 0,
+        Failsafe_Action_RTL           = 1,
+        Failsafe_Action_Hold          = 2,
+        Failsafe_Action_SmartRTL      = 3,
+        Failsafe_Action_SmartRTL_Hold = 4,
+        Failsafe_Action_Terminate     = 5
+    };
+
+    static constexpr int8_t _failsafe_priorities[] = {
+                                                       Failsafe_Action_Terminate,
+                                                       Failsafe_Action_Hold,
+                                                       Failsafe_Action_RTL,
+                                                       Failsafe_Action_SmartRTL_Hold,
+                                                       Failsafe_Action_SmartRTL,
+                                                       Failsafe_Action_None,
+                                                       -1 // the priority list must end with a sentinel of -1
+                                                      };
+    static_assert(_failsafe_priorities[ARRAY_SIZE(_failsafe_priorities) - 1] == -1,
+                  "_failsafe_priorities is missing the sentinel");
+
+
 public:
     void mavlink_delay_cb();
     void failsafe_check();

APMrover2/failsafe.cpp

@@ -96,6 +96,41 @@ void Rover::failsafe_trigger(uint8_t failsafe_type, bool on)
     }
 }
 
+void Rover::handle_battery_failsafe(const char* type_str, const int8_t action)
+{
+        switch ((Failsafe_Action)action) {
+            case Failsafe_Action_None:
+                break;
+            case Failsafe_Action_SmartRTL:
+                if (set_mode(mode_smartrtl, MODE_REASON_FAILSAFE)) {
+                    break;
+                }
+                FALLTHROUGH;
+            case Failsafe_Action_RTL:
+                if (set_mode(mode_rtl, MODE_REASON_FAILSAFE)) {
+                    break;
+                }
+                FALLTHROUGH;
+            case Failsafe_Action_Hold:
+                set_mode(mode_hold, MODE_REASON_FAILSAFE);
+                break;
+            case Failsafe_Action_SmartRTL_Hold:
+                if (!set_mode(mode_smartrtl, MODE_REASON_FAILSAFE)) {
+                    set_mode(mode_hold, MODE_REASON_FAILSAFE);
+                }
+                break;
+            case Failsafe_Action_Terminate:
+#if ADVANCED_FAILSAFE == ENABLED
+                char battery_type_str[17];
+                snprintf(battery_type_str, 17, "%s battery", type_str);
+                afs.gcs_terminate(true, battery_type_str);
+#else
+                disarm_motors();
+#endif // ADVANCED_FAILSAFE == ENABLED
+                break;
+        }
+}
+
 #if ADVANCED_FAILSAFE == ENABLED
 /*
    check for AFS failsafe check

APMrover2/sensors.cpp

@@ -293,7 +293,8 @@ void Rover::update_sensor_status_flags(void)
                                                          ~MAV_SYS_STATUS_SENSOR_YAW_POSITION &
                                                          ~MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL &
                                                          ~MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS &
-                                                         ~MAV_SYS_STATUS_LOGGING);
+                                                         ~MAV_SYS_STATUS_LOGGING &
+                                                         ~MAV_SYS_STATUS_SENSOR_BATTERY);
     if (control_mode->attitude_stabilized()) {
         control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control
         control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION; // 3D angular rate control
@@ -312,6 +313,10 @@ void Rover::update_sensor_status_flags(void)
         control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS;
     }
 
+    if (battery.num_instances() > 0) {
+        control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_BATTERY;
+    }
+
     // default to all healthy except compass and gps which we set individually
     control_sensors_health = control_sensors_present & (~MAV_SYS_STATUS_SENSOR_3D_MAG & ~MAV_SYS_STATUS_SENSOR_GPS);
     if (g.compass_enabled && compass.healthy(0) && ahrs.use_compass()) {
@@ -352,6 +357,10 @@ void Rover::update_sensor_status_flags(void)
         control_sensors_health &= ~MAV_SYS_STATUS_LOGGING;
     }
 
+    if (!battery.healthy() || battery.has_failsafed()) {
+        control_sensors_enabled &= ~MAV_SYS_STATUS_SENSOR_BATTERY;
+    }
+
     if (!initialised || ins.calibrating()) {
         // while initialising the gyros and accels are not enabled
         control_sensors_enabled &= ~(MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL);