diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.cpp b/libraries/AP_BattMonitor/AP_BattMonitor.cpp index 2db67492d8..09d382b731 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor.cpp @@ -417,6 +417,19 @@ bool AP_BattMonitor::get_temperature(float &temperature, const uint8_t instance) } } +bool AP_BattMonitor::arming_checks(size_t buflen, char *buffer) const +{ + char temp_buffer[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1] {}; + + for (uint8_t i = 0; i < AP_BATT_MONITOR_MAX_INSTANCES; i++) { + if (drivers[i] != nullptr && !(drivers[i]->arming_checks(temp_buffer, sizeof(temp_buffer)))) { + hal.util->snprintf(buffer, buflen, "Battery %d %s", i + 1, temp_buffer); + return false; + } + } + + return true; +} namespace AP { diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.h b/libraries/AP_BattMonitor/AP_BattMonitor.h index ad655f1100..5080936e9c 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor.h @@ -164,6 +164,9 @@ public: float get_resistance() const { return get_resistance(AP_BATT_PRIMARY_INSTANCE); } float get_resistance(uint8_t instance) const { return state[instance].resistance; } + // returns false if we fail arming checks, in which case the buffer will be populated with a failure message + bool arming_checks(size_t buflen, char *buffer) const; + static const struct AP_Param::GroupInfo var_info[]; protected: diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp index 758ad2161d..f5054b9729 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp @@ -143,6 +143,35 @@ AP_BattMonitor::BatteryFailsafe AP_BattMonitor_Backend::update_failsafes(void) return AP_BattMonitor::BatteryFailsafe_None; } +bool update_check(size_t buflen, char *buffer, bool failed, const char *message) +{ + if (failed) { + strncpy(buffer, message, buflen); + return false; + } + return true; +} + +bool AP_BattMonitor_Backend::arming_checks(char * buffer, size_t buflen) const +{ + bool low_voltage, low_capacity, critical_voltage, critical_capacity; + check_failsafe_types(low_voltage, low_capacity, critical_voltage, critical_capacity); + + bool below_arming_voltage = is_positive(_params._arming_minimum_voltage) && + (_state.voltage < _params._arming_minimum_voltage); + bool below_arming_capacity = (_params._arming_minimum_capacity > 0) && + ((_params._pack_capacity - _state.consumed_mah) < _params._arming_minimum_capacity); + + bool result = update_check(buflen, buffer, low_voltage, "low voltage failsafe"); + result = result && update_check(buflen, buffer, low_capacity, "low capacity failsafe"); + result = result && update_check(buflen, buffer, critical_voltage, "critical voltage failsafe"); + result = result && update_check(buflen, buffer, critical_capacity, "critical capacity failsafe"); + result = result && update_check(buflen, buffer, below_arming_voltage, "below minimum arming voltage"); + result = result && update_check(buflen, buffer, below_arming_capacity, "below minimum arming capacity"); + + return result; +} + void AP_BattMonitor_Backend::check_failsafe_types(bool &low_voltage, bool &low_capacity, bool &critical_voltage, bool &critical_capacity) const { // use voltage or sag compensated voltage @@ -160,12 +189,11 @@ void AP_BattMonitor_Backend::check_failsafe_types(bool &low_voltage, bool &low_c // check critical battery levels if ((voltage_used > 0) && (_params._critical_voltage > 0) && (voltage_used < _params._critical_voltage)) { critical_voltage = true; - // this is the first time our voltage has dropped below minimum so start timer } else { critical_voltage = false; } - // check capacity if current monitoring is enabled + // check capacity failsafe if current monitoring is enabled if (has_current() && (_params._critical_capacity > 0) && ((_params._pack_capacity - _state.consumed_mah) < _params._critical_capacity)) { critical_capacity = true; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h index d22de89a97..33fc9277bb 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h @@ -56,6 +56,9 @@ public: // updates failsafe timers, and returns what failsafes are active AP_BattMonitor::BatteryFailsafe update_failsafes(void); + // returns false if we fail arming checks, in which case the buffer will be populated with a failure message + bool arming_checks(char * buffer, size_t buflen) const; + protected: AP_BattMonitor &_mon; // reference to front-end AP_BattMonitor::BattMonitor_State &_state; // reference to this instances state (held in the front-end) diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp index f6d8ead943..cba9787b96 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp @@ -146,6 +146,22 @@ const AP_Param::GroupInfo AP_BattMonitor_Params::var_info[] = { // @User: Standard AP_GROUPINFO("FS_CRT_ACT", 17, AP_BattMonitor_Params, _failsafe_critical_action, 0), + // @Param: ARM_VOLT + // @DisplayName: Required arming voltage + // @Description: Battery voltage level which is required to arm the aircraft. Set to 0 to allow arming at any voltage. + // @Units: V + // @Increment: 0.1 + // @User: Advanced + AP_GROUPINFO("ARM_VOLT", 18, AP_BattMonitor_Params, _arming_minimum_voltage, 0), + + // @Param: ARM_MAH + // @DisplayName: Required arming remaining capacity + // @Description: Battery capacity remaining which is required to arm the aircraft. Set to 0 to allow arming at any capacity. Note that execept for smart batteries rebooting the vehicle will always reset the remaining capacity estimate, which can lead to this check not providing sufficent protection, it is recommended to always use this in conjunction with the @PREFIX@_ARM_VOLT parameter. + // @Units: mAh + // @Increment: 50 + // @User: Advanced + AP_GROUPINFO("ARM_MAH", 19, AP_BattMonitor_Params, _arming_minimum_capacity, 0), + AP_GROUPEND }; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Params.h b/libraries/AP_BattMonitor/AP_BattMonitor_Params.h index 9f791a3922..f4e1c3b18a 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Params.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Params.h @@ -50,5 +50,7 @@ public: AP_Float _critical_capacity; /// capacity level used to trigger a critical battery failsafe AP_Int8 _failsafe_low_action; /// action to preform on a low battery failsafe AP_Int8 _failsafe_critical_action; /// action to preform on a critical battery failsafe + AP_Int32 _arming_minimum_capacity; /// capacity level required to arm + AP_Float _arming_minimum_voltage; /// voltage level required to arm };