Battery critical/low actions

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 @@ class Rover : public AP_HAL::HAL::Callbacks {
     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 @@ class Rover : public AP_HAL::HAL::Callbacks {
 
     // 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 @@ class Rover : public AP_HAL::HAL::Callbacks {
     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);

AntennaTracker/GCS_Mavlink.cpp

@@ -830,4 +830,4 @@ void AP_Camera::send_feedback(mavlink_channel_t chan) {}
 /* end dummy methods to avoid having to link against AP_Camera */
 
 // dummy method to avoid linking AFS
-bool AP_AdvancedFailsafe::gcs_terminate(bool should_terminate) {return false;}
+bool AP_AdvancedFailsafe::gcs_terminate(bool should_terminate, const char *reason) {return false;}

AntennaTracker/Tracker.h

@@ -151,7 +151,9 @@ class Tracker : public AP_HAL::HAL::Callbacks {
 #endif
 
     // Battery Sensors
-    AP_BattMonitor battery{MASK_LOG_CURRENT};
+    AP_BattMonitor battery{MASK_LOG_CURRENT,
+                           FUNCTOR_BIND_MEMBER(&Tracker::handle_battery_failsafe, void, const char*, const int8_t),
+                           nullptr};
 
     struct Location current_loc;
 
@@ -263,6 +265,7 @@ class Tracker : public AP_HAL::HAL::Callbacks {
     void Log_Write_Vehicle_Startup_Messages();
     void log_init(void);
     bool should_log(uint32_t mask);
+    void handle_battery_failsafe(const char* type_str, const int8_t action);
 
 public:
     void mavlink_snoop(const mavlink_message_t* msg);

AntennaTracker/sensors.cpp

@@ -127,3 +127,10 @@ void Tracker::update_GPS(void)
     }
 }
 
+void Tracker::handle_battery_failsafe(const char* type_str, const int8_t action)
+{
+    // NOP
+    // useful failsafes in the future would include actually recalling the vehicle
+    // that is tracked before the tracker loses power to continue tracking it
+}
+

ArduCopter/AP_Arming.cpp

@@ -146,30 +146,15 @@ bool AP_Arming_Copter::board_voltage_checks(bool display_failure)
         return false;
     }
 
-    Parameters &g = copter.g;
-
     // check battery voltage
     if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_VOLTAGE)) {
-        if (copter.failsafe.battery) {
+        if (copter.battery.has_failsafed()) {
             if (display_failure) {
                 gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: Battery failsafe");
             }
             return false;
         }
 
-        // all following checks are skipped if USB is connected
-        if (copter.ap.usb_connected) {
-            return true;
-        }
-
-        // check if battery is exhausted
-        if (copter.battery.exhausted(g.fs_batt_voltage, g.fs_batt_mah)) {
-            if (display_failure) {
-                gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: Check Battery");
-            }
-            return false;
-        }
-
         // call parent battery checks
         if (!AP_Arming::battery_checks(display_failure)) {
             return false;

ArduCopter/AP_State.cpp

@@ -70,13 +70,6 @@ void Copter::set_failsafe_radio(bool b)
 }
 
 
-// ---------------------------------------------
-void Copter::set_failsafe_battery(bool b)
-{
-    failsafe.battery = b;
-    AP_Notify::flags.failsafe_battery = b;
-}
-
 // ---------------------------------------------
 void Copter::set_failsafe_gcs(bool b)
 {

ArduCopter/ArduCopter.cpp

@@ -191,6 +191,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
 #endif
 };
 
+constexpr int8_t Copter::_failsafe_priorities[7];
 
 void Copter::setup()
 {
@@ -297,7 +298,7 @@ void Copter::throttle_loop()
 void Copter::update_batt_compass(void)
 {
     // read battery before compass because it may be used for motor interference compensation
-    read_battery();
+    battery.read();
 
     if(g.compass_enabled) {
         // update compass with throttle value - used for compassmot

ArduCopter/Copter.h

@@ -373,19 +373,18 @@ class Copter : public AP_HAL::HAL::Callbacks {
 
     // Failsafe
     struct {
-        uint8_t rc_override_active  : 1; // 0   // true if rc control are overwritten by ground station
-        uint8_t radio               : 1; // 1   // A status flag for the radio failsafe
-        uint8_t battery             : 1; // 2   // A status flag for the battery failsafe
-        uint8_t gcs                 : 1; // 4   // A status flag for the ground station failsafe
-        uint8_t ekf                 : 1; // 5   // true if ekf failsafe has occurred
-        uint8_t terrain             : 1; // 6   // true if the missing terrain data failsafe has occurred
-        uint8_t adsb                : 1; // 7   // true if an adsb related failsafe has occurred
-
-        int8_t radio_counter;            // number of iterations with throttle below throttle_fs_value
-
         uint32_t last_heartbeat_ms;      // the time when the last HEARTBEAT message arrived from a GCS - used for triggering gcs failsafe
         uint32_t terrain_first_failure_ms;  // the first time terrain data access failed - used to calculate the duration of the failure
         uint32_t terrain_last_failure_ms;   // the most recent time terrain data access failed
+
+        int8_t radio_counter;            // number of iterations with throttle below throttle_fs_value
+
+        uint8_t rc_override_active  : 1; // true if rc control are overwritten by ground station
+        uint8_t radio               : 1; // A status flag for the radio failsafe
+        uint8_t gcs                 : 1; // A status flag for the ground station failsafe
+        uint8_t ekf                 : 1; // true if ekf failsafe has occurred
+        uint8_t terrain             : 1; // true if the missing terrain data failsafe has occurred
+        uint8_t adsb                : 1; // true if an adsb related failsafe has occurred
     } failsafe;
 
     // sensor health for logging
@@ -425,7 +424,9 @@ class Copter : public AP_HAL::HAL::Callbacks {
     int32_t initial_armed_bearing;
 
     // Battery Sensors
-    AP_BattMonitor battery{MASK_LOG_CURRENT};
+    AP_BattMonitor battery{MASK_LOG_CURRENT,
+                           FUNCTOR_BIND_MEMBER(&Copter::handle_battery_failsafe, void, const char*, const int8_t),
+                           _failsafe_priorities};
 
 #if FRSKY_TELEM_ENABLED == ENABLED
     // FrSky telemetry support
@@ -628,11 +629,37 @@ class Copter : public AP_HAL::HAL::Callbacks {
     static const AP_Param::Info var_info[];
     static const struct LogStructure log_structure[];
 
+    enum Failsafe_Action {
+        Failsafe_Action_None           = 0,
+        Failsafe_Action_Land           = 1,
+        Failsafe_Action_RTL            = 2,
+        Failsafe_Action_SmartRTL       = 3,
+        Failsafe_Action_SmartRTL_Land  = 4,
+        Failsafe_Action_Terminate      = 5
+    };
+
+    static constexpr int8_t _failsafe_priorities[] = {
+                                                      Failsafe_Action_Terminate,
+                                                      Failsafe_Action_Land,
+                                                      Failsafe_Action_RTL,
+                                                      Failsafe_Action_SmartRTL_Land,
+                                                      Failsafe_Action_SmartRTL,
+                                                      Failsafe_Action_None,
+                                                      -1 // the priority list must end with a sentinel of -1
+                                                     };
+
+    #define FAILSAFE_LAND_PRIORITY 1
+    static_assert(_failsafe_priorities[FAILSAFE_LAND_PRIORITY] == Failsafe_Action_Land,
+                  "FAILSAFE_LAND_PRIORITY must match the entry in _failsafe_priorities");
+    static_assert(_failsafe_priorities[ARRAY_SIZE(_failsafe_priorities) - 1] == -1,
+                  "_failsafe_priorities is missing the sentinel");
+
+
+
     // AP_State.cpp
     void set_auto_armed(bool b);
     void set_simple_mode(uint8_t b);
     void set_failsafe_radio(bool b);
-    void set_failsafe_battery(bool b);
     void set_failsafe_gcs(bool b);
     void update_using_interlock();
     void set_motor_emergency_stop(bool b);
@@ -718,7 +745,7 @@ class Copter : public AP_HAL::HAL::Callbacks {
     // events.cpp
     void failsafe_radio_on_event();
     void failsafe_radio_off_event();
-    void failsafe_battery_event(void);
+    void handle_battery_failsafe(const char* type_str, const int8_t action);
     void failsafe_gcs_check();
     void failsafe_gcs_off_event(void);
     void failsafe_terrain_check();
@@ -880,7 +907,6 @@ class Copter : public AP_HAL::HAL::Callbacks {
     void compass_accumulate(void);
     void init_optflow();
     void update_optical_flow(void);
-    void read_battery(void);
     void read_receiver_rssi(void);
     void compass_cal_update(void);
     void accel_cal_update(void);

ArduCopter/GCS_Mavlink.cpp

@@ -29,7 +29,7 @@ NOINLINE void Copter::send_heartbeat(mavlink_channel_t chan)
     uint32_t custom_mode = control_mode;
 
     // set system as critical if any failsafe have triggered
-    if (failsafe.radio || failsafe.battery || failsafe.gcs || failsafe.ekf || failsafe.terrain || failsafe.adsb)  {
+    if (failsafe.radio || battery.has_failsafed() || failsafe.gcs || failsafe.ekf || failsafe.terrain || failsafe.adsb)  {
         system_status = MAV_STATE_CRITICAL;
     }
 

ArduCopter/GCS_Mavlink.h

@@ -3,7 +3,7 @@
 #include <GCS_MAVLink/GCS.h>
 
 // default sensors are present and healthy: gyro, accelerometer, barometer, 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_ABSOLUTE_PRESSURE | MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL | MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION | MAV_SYS_STATUS_SENSOR_YAW_POSITION | MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL | 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_ABSOLUTE_PRESSURE | MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL | MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION | MAV_SYS_STATUS_SENSOR_YAW_POSITION | MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL | 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_Copter : public GCS_MAVLINK
 {

ArduCopter/Log.cpp

@@ -183,7 +183,7 @@ void Copter::Log_Write_MotBatt()
         time_us         : AP_HAL::micros64(),
         lift_max        : (float)(motors->get_lift_max()),
         bat_volt        : (float)(motors->get_batt_voltage_filt()),
-        bat_res         : (float)(motors->get_batt_resistance()),
+        bat_res         : (float)(battery.get_resistance()),
         th_limit        : (float)(motors->get_throttle_limit())
     };
     DataFlash.WriteBlock(&pkt_mot, sizeof(pkt_mot));