forked from Archive/PX4-Autopilot
commander: move battery handling into arming checks
This commit is contained in:
parent
5cb44a521c
commit
cfe3d793bf
|
@ -118,5 +118,4 @@ bool parachute_system_healthy
|
||||||
bool avoidance_system_required # Set to true if avoidance system is enabled via COM_OBS_AVOID parameter
|
bool avoidance_system_required # Set to true if avoidance system is enabled via COM_OBS_AVOID parameter
|
||||||
bool avoidance_system_valid # Status of the obstacle avoidance system
|
bool avoidance_system_valid # Status of the obstacle avoidance system
|
||||||
|
|
||||||
bool battery_healthy # set if battery is available and not low
|
|
||||||
|
|
||||||
|
|
|
@ -42,3 +42,8 @@ bool rc_signal_found_once
|
||||||
bool rc_calibration_in_progress
|
bool rc_calibration_in_progress
|
||||||
bool vtol_transition_failure # Set to true if vtol transition failed
|
bool vtol_transition_failure # Set to true if vtol transition failed
|
||||||
|
|
||||||
|
bool battery_low_remaining_time
|
||||||
|
bool battery_unhealthy
|
||||||
|
|
||||||
|
uint8 battery_warning
|
||||||
|
|
||||||
|
|
|
@ -532,54 +532,6 @@ static constexpr const char *arm_disarm_reason_str(arm_disarm_reason_t calling_r
|
||||||
return "";
|
return "";
|
||||||
};
|
};
|
||||||
|
|
||||||
using battery_fault_reason_t = events::px4::enums::battery_fault_reason_t;
|
|
||||||
static_assert(battery_status_s::BATTERY_FAULT_COUNT == (static_cast<uint8_t>(battery_fault_reason_t::_max) + 1)
|
|
||||||
, "Battery fault flags mismatch!");
|
|
||||||
|
|
||||||
static constexpr const char *battery_fault_reason_str(battery_fault_reason_t battery_fault_reason)
|
|
||||||
{
|
|
||||||
switch (battery_fault_reason) {
|
|
||||||
case battery_fault_reason_t::deep_discharge: return "under voltage";
|
|
||||||
|
|
||||||
case battery_fault_reason_t::voltage_spikes: return "over voltage";
|
|
||||||
|
|
||||||
case battery_fault_reason_t::cell_fail: return "cell fault";
|
|
||||||
|
|
||||||
case battery_fault_reason_t::over_current: return "over current";
|
|
||||||
|
|
||||||
case battery_fault_reason_t::fault_temperature: return "critical temperature";
|
|
||||||
|
|
||||||
case battery_fault_reason_t::under_temperature: return "under temperature";
|
|
||||||
|
|
||||||
case battery_fault_reason_t::incompatible_voltage: return "voltage mismatch";
|
|
||||||
|
|
||||||
case battery_fault_reason_t::incompatible_firmware: return "incompatible firmware";
|
|
||||||
|
|
||||||
case battery_fault_reason_t::incompatible_model: return "incompatible model";
|
|
||||||
|
|
||||||
case battery_fault_reason_t::hardware_fault: return "hardware fault";
|
|
||||||
|
|
||||||
case battery_fault_reason_t::over_temperature: return "near temperature limit";
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
return "";
|
|
||||||
};
|
|
||||||
|
|
||||||
using battery_mode_t = events::px4::enums::battery_mode_t;
|
|
||||||
static_assert(battery_status_s::BATTERY_MODE_COUNT == (static_cast<uint8_t>(battery_mode_t::_max) + 1)
|
|
||||||
, "Battery mode flags mismatch!");
|
|
||||||
static constexpr const char *battery_mode_str(battery_mode_t battery_mode)
|
|
||||||
{
|
|
||||||
switch (battery_mode) {
|
|
||||||
case battery_mode_t::autodischarging: return "auto discharging";
|
|
||||||
|
|
||||||
case battery_mode_t::hotswap: return "hot-swap";
|
|
||||||
|
|
||||||
default: return "unknown";
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
using esc_fault_reason_t = events::px4::enums::esc_fault_reason_t;
|
using esc_fault_reason_t = events::px4::enums::esc_fault_reason_t;
|
||||||
static_assert(esc_report_s::ESC_FAILURE_COUNT == (static_cast<uint8_t>(esc_fault_reason_t::_max) + 1)
|
static_assert(esc_report_s::ESC_FAILURE_COUNT == (static_cast<uint8_t>(esc_fault_reason_t::_max) + 1)
|
||||||
, "ESC fault flags mismatch!");
|
, "ESC fault flags mismatch!");
|
||||||
|
@ -2421,9 +2373,7 @@ Commander::run()
|
||||||
_geofence_warning_action_on = false;
|
_geofence_warning_action_on = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (_battery_status_subs.updated()) {
|
|
||||||
battery_status_check();
|
battery_status_check();
|
||||||
}
|
|
||||||
|
|
||||||
/* If in INIT state, try to proceed to STANDBY state */
|
/* If in INIT state, try to proceed to STANDBY state */
|
||||||
if (!_vehicle_status_flags.calibration_enabled && _arm_state_machine.isInit()) {
|
if (!_vehicle_status_flags.calibration_enabled && _arm_state_machine.isInit()) {
|
||||||
|
@ -3691,112 +3641,11 @@ void Commander::data_link_check()
|
||||||
|
|
||||||
void Commander::battery_status_check()
|
void Commander::battery_status_check()
|
||||||
{
|
{
|
||||||
size_t battery_required_count = 0;
|
|
||||||
bool battery_has_fault = false;
|
|
||||||
// There are possibly multiple batteries, and we can't know which ones serve which purpose. So the safest
|
|
||||||
// option is to check if ANY of them have a warning, and specifically find which one has the most
|
|
||||||
// urgent warning.
|
|
||||||
uint8_t worst_warning = battery_status_s::BATTERY_WARNING_NONE;
|
|
||||||
// To make sure that all connected batteries are being regularly reported, we check which one has the
|
|
||||||
// oldest timestamp.
|
|
||||||
hrt_abstime oldest_update = hrt_absolute_time();
|
|
||||||
float worst_battery_time_s{NAN};
|
|
||||||
|
|
||||||
for (auto &battery_sub : _battery_status_subs) {
|
|
||||||
int index = battery_sub.get_instance();
|
|
||||||
battery_status_s battery;
|
|
||||||
|
|
||||||
if (!battery_sub.copy(&battery)) {
|
|
||||||
continue;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (battery.is_required) {
|
|
||||||
battery_required_count++;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (_arm_state_machine.isArmed()) {
|
|
||||||
|
|
||||||
if (_last_connected_batteries[index] && !battery.connected) {
|
|
||||||
mavlink_log_critical(&_mavlink_log_pub, "Battery %d disconnected. Land now! \t", index + 1);
|
|
||||||
events::send<uint8_t>(events::ID("commander_battery_disconnected"), {events::Log::Emergency, events::LogInternal::Warning},
|
|
||||||
"Battery {1} disconnected. Land now!", index + 1);
|
|
||||||
// trigger a battery failsafe action if a battery disconnects in flight
|
|
||||||
worst_warning = battery_status_s::BATTERY_WARNING_CRITICAL;
|
|
||||||
}
|
|
||||||
|
|
||||||
if ((battery.mode > 0) && (battery.mode != _last_battery_mode[index])) {
|
|
||||||
|
|
||||||
mavlink_log_critical(&_mavlink_log_pub, "Battery %d is in %s mode! \t", index + 1,
|
|
||||||
battery_mode_str(static_cast<battery_mode_t>(battery.mode)));
|
|
||||||
events::send<uint8_t, events::px4::enums::battery_mode_t>(events::ID("commander_battery_mode"), {events::Log::Critical, events::LogInternal::Warning},
|
|
||||||
"Battery {1} mode: {2}. Land now!", index + 1, static_cast<battery_mode_t>(battery.mode));
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
_last_connected_batteries.set(index, battery.connected);
|
|
||||||
_last_battery_mode[index] = battery.mode;
|
|
||||||
|
|
||||||
if (battery.connected) {
|
|
||||||
if (battery.warning > worst_warning) {
|
|
||||||
worst_warning = battery.warning;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (battery.timestamp < oldest_update) {
|
|
||||||
oldest_update = battery.timestamp;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (battery.faults > 0) {
|
|
||||||
// MAVLink supported faults, can be checked on the ground station
|
|
||||||
battery_has_fault = true;
|
|
||||||
|
|
||||||
if (battery.faults != _last_battery_fault[index] || battery.custom_faults != _last_battery_custom_fault[index]) {
|
|
||||||
for (uint8_t fault_index = 0; fault_index <= static_cast<uint8_t>(battery_fault_reason_t::_max);
|
|
||||||
fault_index++) {
|
|
||||||
if (battery.faults & (1 << fault_index)) {
|
|
||||||
mavlink_log_emergency(&_mavlink_log_pub, "Battery %d: %s. %s \t", index + 1,
|
|
||||||
battery_fault_reason_str(static_cast<battery_fault_reason_t>(fault_index)),
|
|
||||||
_arm_state_machine.isArmed() ? "Land now!" : "");
|
|
||||||
|
|
||||||
events::px4::enums::suggested_action_t action = _arm_state_machine.isArmed() ?
|
|
||||||
events::px4::enums::suggested_action_t::land :
|
|
||||||
events::px4::enums::suggested_action_t::none;
|
|
||||||
|
|
||||||
/* EVENT
|
|
||||||
* @description
|
|
||||||
* The battery reported a failure which might be dangerous to fly.
|
|
||||||
* Manufacturer error code: {4}
|
|
||||||
*/
|
|
||||||
events::send<uint8_t, battery_fault_reason_t, events::px4::enums::suggested_action_t, uint32_t>
|
|
||||||
(events::ID("commander_battery_fault"), {events::Log::Emergency, events::LogInternal::Warning},
|
|
||||||
"Battery {1}: {2}. {3}", index + 1, static_cast<battery_fault_reason_t>(fault_index), action, battery.custom_faults);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
_last_battery_fault[index] = battery.faults;
|
|
||||||
_last_battery_custom_fault[index] = battery.custom_faults;
|
|
||||||
|
|
||||||
if (PX4_ISFINITE(battery.time_remaining_s)
|
|
||||||
&& (!PX4_ISFINITE(worst_battery_time_s)
|
|
||||||
|| (PX4_ISFINITE(worst_battery_time_s) && (battery.time_remaining_s < worst_battery_time_s)))) {
|
|
||||||
worst_battery_time_s = battery.time_remaining_s;
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
rtl_time_estimate_s rtl_time_estimate{};
|
|
||||||
|
|
||||||
// Compare estimate of RTL time to estimate of remaining flight time
|
// Compare estimate of RTL time to estimate of remaining flight time
|
||||||
if (_rtl_time_estimate_sub.copy(&rtl_time_estimate)
|
if (_vehicle_status_flags.battery_low_remaining_time
|
||||||
&& (hrt_absolute_time() - rtl_time_estimate.timestamp) < 2_s
|
|
||||||
&& rtl_time_estimate.valid
|
|
||||||
&& _arm_state_machine.isArmed()
|
&& _arm_state_machine.isArmed()
|
||||||
&& !_vehicle_land_detected.ground_contact // not in any landing stage
|
&& !_vehicle_land_detected.ground_contact // not in any landing stage
|
||||||
&& !_rtl_time_actions_done
|
&& !_rtl_time_actions_done
|
||||||
&& PX4_ISFINITE(worst_battery_time_s)
|
|
||||||
&& rtl_time_estimate.safe_time_estimate >= worst_battery_time_s
|
|
||||||
&& _commander_state.main_state != commander_state_s::MAIN_STATE_AUTO_RTL
|
&& _commander_state.main_state != commander_state_s::MAIN_STATE_AUTO_RTL
|
||||||
&& _commander_state.main_state != commander_state_s::MAIN_STATE_AUTO_LAND) {
|
&& _commander_state.main_state != commander_state_s::MAIN_STATE_AUTO_LAND) {
|
||||||
// Try to trigger RTL
|
// Try to trigger RTL
|
||||||
|
@ -3819,31 +3668,21 @@ void Commander::battery_status_check()
|
||||||
bool update_internal_battery_state = false;
|
bool update_internal_battery_state = false;
|
||||||
|
|
||||||
if (_arm_state_machine.isArmed()) {
|
if (_arm_state_machine.isArmed()) {
|
||||||
if (worst_warning > _battery_warning) {
|
if (_vehicle_status_flags.battery_warning > _battery_warning) {
|
||||||
battery_warning_level_increased_while_armed = true;
|
battery_warning_level_increased_while_armed = true;
|
||||||
update_internal_battery_state = true;
|
update_internal_battery_state = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
if (_battery_warning != worst_warning) {
|
if (_battery_warning != _vehicle_status_flags.battery_warning) {
|
||||||
update_internal_battery_state = true;
|
update_internal_battery_state = true;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
if (update_internal_battery_state) {
|
if (update_internal_battery_state) {
|
||||||
_battery_warning = worst_warning;
|
_battery_warning = _vehicle_status_flags.battery_warning;
|
||||||
}
|
}
|
||||||
|
|
||||||
_vehicle_status.battery_healthy =
|
|
||||||
// All connected batteries are regularly being published
|
|
||||||
(hrt_elapsed_time(&oldest_update) < 5_s)
|
|
||||||
// There is at least one connected battery (in any slot)
|
|
||||||
&& (_last_connected_batteries.count() >= battery_required_count)
|
|
||||||
// No currently-connected batteries have any warning
|
|
||||||
&& (_battery_warning == battery_status_s::BATTERY_WARNING_NONE)
|
|
||||||
// No currently-connected batteries have any fault
|
|
||||||
&& (!battery_has_fault);
|
|
||||||
|
|
||||||
// execute battery failsafe if the state has gotten worse while we are armed
|
// execute battery failsafe if the state has gotten worse while we are armed
|
||||||
if (battery_warning_level_increased_while_armed) {
|
if (battery_warning_level_increased_while_armed) {
|
||||||
uint8_t failsafe_action = get_battery_failsafe_action(_commander_state, _battery_warning,
|
uint8_t failsafe_action = get_battery_failsafe_action(_commander_state, _battery_warning,
|
||||||
|
|
|
@ -41,7 +41,6 @@
|
||||||
#include "worker_thread.hpp"
|
#include "worker_thread.hpp"
|
||||||
#include "HealthAndArmingChecks/HealthAndArmingChecks.hpp"
|
#include "HealthAndArmingChecks/HealthAndArmingChecks.hpp"
|
||||||
|
|
||||||
#include <containers/Bitset.hpp>
|
|
||||||
#include <lib/controllib/blocks.hpp>
|
#include <lib/controllib/blocks.hpp>
|
||||||
#include <lib/hysteresis/hysteresis.h>
|
#include <lib/hysteresis/hysteresis.h>
|
||||||
#include <lib/mathlib/mathlib.h>
|
#include <lib/mathlib/mathlib.h>
|
||||||
|
@ -341,12 +340,6 @@ private:
|
||||||
|
|
||||||
uint8_t _battery_warning{battery_status_s::BATTERY_WARNING_NONE};
|
uint8_t _battery_warning{battery_status_s::BATTERY_WARNING_NONE};
|
||||||
hrt_abstime _battery_failsafe_timestamp{0};
|
hrt_abstime _battery_failsafe_timestamp{0};
|
||||||
px4::Bitset<battery_status_s::MAX_INSTANCES> _last_connected_batteries;
|
|
||||||
uint32_t _last_battery_custom_fault[battery_status_s::MAX_INSTANCES] {};
|
|
||||||
uint16_t _last_battery_fault[battery_status_s::MAX_INSTANCES] {};
|
|
||||||
uint8_t _last_battery_mode[battery_status_s::MAX_INSTANCES] {};
|
|
||||||
|
|
||||||
|
|
||||||
Hysteresis _auto_disarm_landed{false};
|
Hysteresis _auto_disarm_landed{false};
|
||||||
Hysteresis _auto_disarm_killed{false};
|
Hysteresis _auto_disarm_killed{false};
|
||||||
Hysteresis _offboard_available{false};
|
Hysteresis _offboard_available{false};
|
||||||
|
@ -405,7 +398,6 @@ private:
|
||||||
uORB::Subscription _iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)};
|
uORB::Subscription _iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)};
|
||||||
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
|
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
|
||||||
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
|
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
|
||||||
uORB::Subscription _rtl_time_estimate_sub{ORB_ID(rtl_time_estimate)};
|
|
||||||
uORB::Subscription _system_power_sub{ORB_ID(system_power)};
|
uORB::Subscription _system_power_sub{ORB_ID(system_power)};
|
||||||
uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
|
uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
|
||||||
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
|
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
|
||||||
|
@ -416,7 +408,6 @@ private:
|
||||||
|
|
||||||
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
|
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
|
||||||
|
|
||||||
uORB::SubscriptionMultiArray<battery_status_s, battery_status_s::MAX_INSTANCES> _battery_status_subs{ORB_ID::battery_status};
|
|
||||||
uORB::SubscriptionMultiArray<telemetry_status_s> _telemetry_status_subs{ORB_ID::telemetry_status};
|
uORB::SubscriptionMultiArray<telemetry_status_s> _telemetry_status_subs{ORB_ID::telemetry_status};
|
||||||
|
|
||||||
#if defined(BOARD_HAS_POWER_CONTROL)
|
#if defined(BOARD_HAS_POWER_CONTROL)
|
||||||
|
|
|
@ -33,13 +33,201 @@
|
||||||
|
|
||||||
#include "batteryCheck.hpp"
|
#include "batteryCheck.hpp"
|
||||||
|
|
||||||
|
#include <px4_platform_common/events.h>
|
||||||
|
|
||||||
using namespace time_literals;
|
using namespace time_literals;
|
||||||
|
|
||||||
|
using battery_fault_reason_t = events::px4::enums::battery_fault_reason_t;
|
||||||
|
static_assert(battery_status_s::BATTERY_FAULT_COUNT == (static_cast<uint8_t>(battery_fault_reason_t::_max) + 1)
|
||||||
|
, "Battery fault flags mismatch!");
|
||||||
|
|
||||||
|
static constexpr const char *battery_fault_reason_str(battery_fault_reason_t battery_fault_reason)
|
||||||
|
{
|
||||||
|
switch (battery_fault_reason) {
|
||||||
|
case battery_fault_reason_t::deep_discharge: return "under voltage";
|
||||||
|
|
||||||
|
case battery_fault_reason_t::voltage_spikes: return "over voltage";
|
||||||
|
|
||||||
|
case battery_fault_reason_t::cell_fail: return "cell fault";
|
||||||
|
|
||||||
|
case battery_fault_reason_t::over_current: return "over current";
|
||||||
|
|
||||||
|
case battery_fault_reason_t::fault_temperature: return "critical temperature";
|
||||||
|
|
||||||
|
case battery_fault_reason_t::under_temperature: return "under temperature";
|
||||||
|
|
||||||
|
case battery_fault_reason_t::incompatible_voltage: return "voltage mismatch";
|
||||||
|
|
||||||
|
case battery_fault_reason_t::incompatible_firmware: return "incompatible firmware";
|
||||||
|
|
||||||
|
case battery_fault_reason_t::incompatible_model: return "incompatible model";
|
||||||
|
|
||||||
|
case battery_fault_reason_t::hardware_fault: return "hardware fault";
|
||||||
|
|
||||||
|
case battery_fault_reason_t::over_temperature: return "near temperature limit";
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
return "";
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
using battery_mode_t = events::px4::enums::battery_mode_t;
|
||||||
|
static_assert(battery_status_s::BATTERY_MODE_COUNT == (static_cast<uint8_t>(battery_mode_t::_max) + 1)
|
||||||
|
, "Battery mode flags mismatch!");
|
||||||
|
static constexpr const char *battery_mode_str(battery_mode_t battery_mode)
|
||||||
|
{
|
||||||
|
switch (battery_mode) {
|
||||||
|
case battery_mode_t::autodischarging: return "auto discharging";
|
||||||
|
|
||||||
|
case battery_mode_t::hotswap: return "hot-swap";
|
||||||
|
|
||||||
|
default: return "unknown";
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
void BatteryChecks::checkAndReport(const Context &context, Report &reporter)
|
void BatteryChecks::checkAndReport(const Context &context, Report &reporter)
|
||||||
{
|
{
|
||||||
if (!context.status().battery_healthy) {
|
int battery_required_count = 0;
|
||||||
|
bool battery_has_fault = false;
|
||||||
|
// There are possibly multiple batteries, and we can't know which ones serve which purpose. So the safest
|
||||||
|
// option is to check if ANY of them have a warning, and specifically find which one has the most
|
||||||
|
// urgent warning.
|
||||||
|
uint8_t worst_warning = battery_status_s::BATTERY_WARNING_NONE;
|
||||||
|
// To make sure that all connected batteries are being regularly reported, we check which one has the
|
||||||
|
// oldest timestamp.
|
||||||
|
hrt_abstime oldest_update = hrt_absolute_time();
|
||||||
|
float worst_battery_time_s{NAN};
|
||||||
|
int num_connected_batteries{0};
|
||||||
|
|
||||||
|
for (auto &battery_sub : _battery_status_subs) {
|
||||||
|
int index = battery_sub.get_instance();
|
||||||
|
battery_status_s battery;
|
||||||
|
|
||||||
|
if (!battery_sub.copy(&battery)) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (battery.is_required) {
|
||||||
|
battery_required_count++;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (context.isArmed()) {
|
||||||
|
|
||||||
|
if (!battery.connected) {
|
||||||
|
/* EVENT
|
||||||
|
*/
|
||||||
|
reporter.healthFailure<uint8_t>(NavModes::All, health_component_t::battery, events::ID("check_battery_disconnected"),
|
||||||
|
events::Log::Emergency, "Battery {1} disconnected", index + 1);
|
||||||
|
|
||||||
|
if (reporter.mavlink_log_pub()) {
|
||||||
|
mavlink_log_critical(reporter.mavlink_log_pub(), "Battery %i disconnected\t", index + 1);
|
||||||
|
}
|
||||||
|
|
||||||
|
// trigger a battery failsafe action if a battery disconnects in flight
|
||||||
|
worst_warning = battery_status_s::BATTERY_WARNING_CRITICAL;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (battery.mode != 0) {
|
||||||
|
/* EVENT
|
||||||
|
*/
|
||||||
|
reporter.healthFailure<uint8_t, events::px4::enums::battery_mode_t>(NavModes::All, health_component_t::battery,
|
||||||
|
events::ID("check_battery_mode"),
|
||||||
|
events::Log::Critical, "Battery {1} mode: {2}", index + 1, static_cast<battery_mode_t>(battery.mode));
|
||||||
|
|
||||||
|
if (reporter.mavlink_log_pub()) {
|
||||||
|
mavlink_log_critical(reporter.mavlink_log_pub(), "Battery %d is in %s mode!\t", index + 1,
|
||||||
|
battery_mode_str(static_cast<battery_mode_t>(battery.mode)));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (battery.connected) {
|
||||||
|
++num_connected_batteries;
|
||||||
|
|
||||||
|
if (battery.warning > worst_warning) {
|
||||||
|
worst_warning = battery.warning;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (battery.timestamp < oldest_update) {
|
||||||
|
oldest_update = battery.timestamp;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (battery.faults > 0) {
|
||||||
|
battery_has_fault = true;
|
||||||
|
|
||||||
|
for (uint8_t fault_index = 0; fault_index <= static_cast<uint8_t>(battery_fault_reason_t::_max); fault_index++) {
|
||||||
|
if (battery.faults & (1 << fault_index)) {
|
||||||
|
events::px4::enums::suggested_action_t action = context.isArmed() ?
|
||||||
|
events::px4::enums::suggested_action_t::land :
|
||||||
|
events::px4::enums::suggested_action_t::none;
|
||||||
|
|
||||||
/* EVENT
|
/* EVENT
|
||||||
|
* @description
|
||||||
|
* The battery reported a failure which might be dangerous to fly with.
|
||||||
|
* Manufacturer error code: {4}
|
||||||
|
*/
|
||||||
|
reporter.healthFailure<uint8_t, battery_fault_reason_t, events::px4::enums::suggested_action_t, uint32_t>
|
||||||
|
(NavModes::All, health_component_t::battery, events::ID("check_battery_fault"), {events::Log::Emergency, events::LogInternal::Warning},
|
||||||
|
"Battery {1}: {2}. {3}", index + 1, static_cast<battery_fault_reason_t>(fault_index), action, battery.custom_faults);
|
||||||
|
|
||||||
|
if (reporter.mavlink_log_pub()) {
|
||||||
|
mavlink_log_emergency(reporter.mavlink_log_pub(), "Battery %d: %s. %s \t", index + 1,
|
||||||
|
battery_fault_reason_str(
|
||||||
|
static_cast<battery_fault_reason_t>(fault_index)),
|
||||||
|
context.isArmed() ? "Land now!" : "");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (PX4_ISFINITE(battery.time_remaining_s)
|
||||||
|
&& (!PX4_ISFINITE(worst_battery_time_s)
|
||||||
|
|| (PX4_ISFINITE(worst_battery_time_s) && (battery.time_remaining_s < worst_battery_time_s)))) {
|
||||||
|
worst_battery_time_s = battery.time_remaining_s;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (context.isArmed()) {
|
||||||
|
if (worst_warning > reporter.failsafeFlags().battery_warning) {
|
||||||
|
reporter.failsafeFlags().battery_warning = worst_warning;
|
||||||
|
}
|
||||||
|
|
||||||
|
} else {
|
||||||
|
reporter.failsafeFlags().battery_warning = worst_warning;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (reporter.failsafeFlags().battery_warning > battery_status_s::BATTERY_WARNING_NONE) {
|
||||||
|
bool critical_or_higher = reporter.failsafeFlags().battery_warning >= battery_status_s::BATTERY_WARNING_CRITICAL;
|
||||||
|
NavModes affected_modes = critical_or_higher ? NavModes::All : NavModes::None;
|
||||||
|
events::LogLevel log_level = critical_or_higher ? events::Log::Critical : events::Log::Warning;
|
||||||
|
/* EVENT
|
||||||
|
*/
|
||||||
|
reporter.armingCheckFailure(affected_modes, health_component_t::battery, events::ID("check_battery_low"), log_level,
|
||||||
|
"Low battery");
|
||||||
|
|
||||||
|
if (reporter.mavlink_log_pub()) {
|
||||||
|
mavlink_log_emergency(reporter.mavlink_log_pub(), "Preflight Fail: low battery\t");
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
rtlEstimateCheck(context, reporter, worst_battery_time_s);
|
||||||
|
|
||||||
|
reporter.failsafeFlags().battery_unhealthy =
|
||||||
|
// All connected batteries are regularly being published
|
||||||
|
hrt_elapsed_time(&oldest_update) > 5_s
|
||||||
|
// There is at least one connected battery (in any slot)
|
||||||
|
|| num_connected_batteries < battery_required_count
|
||||||
|
// No currently-connected batteries have any fault
|
||||||
|
|| battery_has_fault;
|
||||||
|
|
||||||
|
if (reporter.failsafeFlags().battery_unhealthy && !battery_has_fault) { // faults are reported above already
|
||||||
|
/* EVENT
|
||||||
|
* @description
|
||||||
|
* Make sure all batteries are connected.
|
||||||
*/
|
*/
|
||||||
reporter.healthFailure(NavModes::All, health_component_t::battery, events::ID("check_battery_unhealthy"),
|
reporter.healthFailure(NavModes::All, health_component_t::battery, events::ID("check_battery_unhealthy"),
|
||||||
events::Log::Error, "Battery unhealthy");
|
events::Log::Error, "Battery unhealthy");
|
||||||
|
@ -49,6 +237,29 @@ void BatteryChecks::checkAndReport(const Context &context, Report &reporter)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
reporter.setIsPresent(health_component_t::battery); // assume it's present
|
if (num_connected_batteries > 0) {
|
||||||
|
reporter.setIsPresent(health_component_t::battery);
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
void BatteryChecks::rtlEstimateCheck(const Context &context, Report &reporter, float worst_battery_time_s)
|
||||||
|
{
|
||||||
|
rtl_time_estimate_s rtl_time_estimate;
|
||||||
|
|
||||||
|
// Compare estimate of RTL time to estimate of remaining flight time
|
||||||
|
reporter.failsafeFlags().battery_low_remaining_time = _rtl_time_estimate_sub.copy(&rtl_time_estimate)
|
||||||
|
&& (hrt_absolute_time() - rtl_time_estimate.timestamp) < 2_s
|
||||||
|
&& rtl_time_estimate.valid
|
||||||
|
&& context.isArmed()
|
||||||
|
&& PX4_ISFINITE(worst_battery_time_s)
|
||||||
|
&& rtl_time_estimate.safe_time_estimate >= worst_battery_time_s;
|
||||||
|
|
||||||
|
if (reporter.failsafeFlags().battery_low_remaining_time) {
|
||||||
|
/* EVENT
|
||||||
|
*/
|
||||||
|
reporter.armingCheckFailure(NavModes::All, health_component_t::battery, events::ID("check_battery_rem_flight_time_low"),
|
||||||
|
events::Log::Error, "Remaining flight time low");
|
||||||
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
|
@ -36,6 +36,9 @@
|
||||||
#include "../Common.hpp"
|
#include "../Common.hpp"
|
||||||
|
|
||||||
#include <uORB/Subscription.hpp>
|
#include <uORB/Subscription.hpp>
|
||||||
|
#include <uORB/SubscriptionMultiArray.hpp>
|
||||||
|
#include <uORB/topics/battery_status.h>
|
||||||
|
#include <uORB/topics/rtl_time_estimate.h>
|
||||||
|
|
||||||
class BatteryChecks : public HealthAndArmingCheckBase
|
class BatteryChecks : public HealthAndArmingCheckBase
|
||||||
{
|
{
|
||||||
|
@ -46,4 +49,9 @@ public:
|
||||||
void checkAndReport(const Context &context, Report &reporter) override;
|
void checkAndReport(const Context &context, Report &reporter) override;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
void rtlEstimateCheck(const Context &context, Report &reporter, float worst_battery_time_s);
|
||||||
|
|
||||||
|
uORB::SubscriptionMultiArray<battery_status_s, battery_status_s::MAX_INSTANCES> _battery_status_subs{ORB_ID::battery_status};
|
||||||
|
uORB::Subscription _rtl_time_estimate_sub{ORB_ID(rtl_time_estimate)};
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
Loading…
Reference in New Issue