forked from Archive/PX4-Autopilot
extend support for Battery status message
Signed-off-by: Claudio Micheli <claudio@auterion.com>
This commit is contained in:
Claudio Micheli
Beat Küng
parent
bb1177d504
commit
d122513197
|
|
@ -22,7 +22,7 @@ uint16 cycle_count # number of discharge cycles the battery has experienced
|
|||
uint16 average_time_to_empty # predicted remaining battery capacity based on the average rate of discharge in min
|
||||
uint16 serial_number # serial number of the battery pack
|
||||
uint16 manufacture_date # manufacture date, part of serial number of the battery pack. formated as: Day + Month×32 + (Year–1980)×512
|
||||
uint16 state_of_health # state of health. FullChargeCapacity/DesignCapacity.
|
||||
uint16 state_of_health # state of health. FullChargeCapacity/DesignCapacity, 0-100%.
|
||||
uint16 max_error # max error, expected margin of error in % in the state-of-charge calculation with a range of 1 to 100%
|
||||
uint8 id # ID number of a battery. Should be unique and consistent for the lifetime of a vehicle. 1-indexed.
|
||||
uint16 interface_error # interface error counter
|
||||
|
|
@ -31,6 +31,7 @@ float32[14] voltage_cell_v # Battery individual cell voltages, 0 if unknown
|
|||
float32 max_cell_voltage_delta # Max difference between individual cell voltages
|
||||
|
||||
bool is_powering_off # Power off event imminent indication, false if unknown
|
||||
bool is_required # Set if the battery is explicitly required before arming
|
||||
|
||||
|
||||
uint8 BATTERY_WARNING_NONE = 0 # no battery low voltage warning active
|
||||
|
|
@ -38,8 +39,31 @@ uint8 BATTERY_WARNING_LOW = 1 # warning of low voltage
|
|||
uint8 BATTERY_WARNING_CRITICAL = 2 # critical voltage, return / abort immediately
|
||||
uint8 BATTERY_WARNING_EMERGENCY = 3 # immediate landing required
|
||||
uint8 BATTERY_WARNING_FAILED = 4 # the battery has failed completely
|
||||
uint8 BATTERY_STATE_UNHEALTHY = 6 # Battery is diagnosed to be defective or an error occurred, usage is discouraged / prohibited. Possible causes (faults) are listed in faults field.
|
||||
uint8 BATTERY_STATE_CHARGING = 7 # Battery is charging
|
||||
|
||||
uint8 warning # current battery warning
|
||||
uint8 BATTERY_FAULT_DEEP_DISCHARGE = 0 # Battery has deep discharged
|
||||
uint8 BATTERY_FAULT_SPIKES = 1 # Voltage spikes
|
||||
uint8 BATTERY_FAULT_CELL_FAIL= 2 # One or more cells have failed
|
||||
uint8 BATTERY_FAULT_OVER_CURRENT = 3 # Over-current
|
||||
uint8 BATTERY_FAULT_OVER_TEMPERATURE = 4 # Over-temperature
|
||||
uint8 BATTERY_FAULT_UNDER_TEMPERATURE = 5 # Under-temperature fault
|
||||
uint8 BATTERY_FAULT_INCOMPATIBLE_VOLTAGE = 6 # Vehicle voltage is not compatible with battery one
|
||||
uint8 BATTERY_FAULT_INCOMPATIBLE_FIRMWARE = 7 # Battery firmware is not compatible with current autopilot firmware
|
||||
uint8 BATTERY_FAULT_INCOMPATIBLE_MODEL = 8 # Battery model is not supported by the system
|
||||
uint8 BATTERY_FAULT_HARDWARE_FAILURE = 9 # hardware problem
|
||||
uint8 BATTERY_WARNING_OVER_TEMPERATURE = 10 # Over-temperature
|
||||
uint8 BATTERY_FAULT_COUNT = 11 # Counter - keep it as last element!
|
||||
|
||||
uint16 faults # Smart battery supply status/fault flags (bitmask) for health indication.
|
||||
uint32 custom_faults # Bitmask indicating smart battery internal manufacturer faults, those are not user actionable.
|
||||
uint8 warning # Current battery warning
|
||||
uint8 mode # Battery mode. Note, the normal operation mode
|
||||
|
||||
uint8 BATTERY_MODE_UNKNOWN = 0 # Battery does not support a mode, or if it does, is operational
|
||||
uint8 BATTERY_MODE_AUTO_DISCHARGING = 1 # Battery is auto discharging (towards storage level)
|
||||
uint8 BATTERY_MODE_HOT_SWAP = 2 # Battery in hot-swap mode
|
||||
uint8 BATTERY_MODE_COUNT = 3 # Counter - keep it as last element (once we're fully migrated to events interface we can just comment this)!
|
||||
|
||||
|
||||
uint8 MAX_INSTANCES = 4
|
||||
|
|
|
|||
|
|
@ -244,6 +244,138 @@
|
|||
"description": "[Unknown]"
|
||||
}
|
||||
}
|
||||
},
|
||||
"battery_fault_reason_t": {
|
||||
"type": "uint8_t",
|
||||
"description": "Reason for battery fault",
|
||||
"entries": {
|
||||
"0": {
|
||||
"name": "deep_discharge",
|
||||
"description": "Battery has deep discharged"
|
||||
},
|
||||
"1": {
|
||||
"name": "voltage_spikes",
|
||||
"description": "Battery detected voltage spikes"
|
||||
},
|
||||
"2": {
|
||||
"name": "cell_fail",
|
||||
"description": "One or more battery cells have failed"
|
||||
},
|
||||
"3": {
|
||||
"name": "over_current",
|
||||
"description": "Battery reported over-current"
|
||||
},
|
||||
"4": {
|
||||
"name": "fault_temperature",
|
||||
"description": "Battery has reached a critical temperature which can result in a critical failure"
|
||||
},
|
||||
"5": {
|
||||
"name": "under_temperature",
|
||||
"description": "Battery temperature is below operating range"
|
||||
},
|
||||
"6": {
|
||||
"name": "incompatible_voltage",
|
||||
"description": "Vehicle voltage is not compatible with battery one"
|
||||
},
|
||||
"7": {
|
||||
"name": "incompatible_firmware",
|
||||
"description": "Battery firmware is not compatible with current autopilot firmware"
|
||||
},
|
||||
"8": {
|
||||
"name": "incompatible_model",
|
||||
"description": "Battery model is not supported by the system"
|
||||
},
|
||||
"9": {
|
||||
"name": "hardware_fault",
|
||||
"description": "Battery reported an hardware problem"
|
||||
},
|
||||
"10": {
|
||||
"name": "over_temperature",
|
||||
"description": "Battery is over-heating"
|
||||
}
|
||||
}
|
||||
},
|
||||
"battery_mode_t": {
|
||||
"type": "uint8_t",
|
||||
"description": "Smart battery modes of operation",
|
||||
"entries": {
|
||||
"0": {
|
||||
"name": "unknown",
|
||||
"description": "unknown"
|
||||
},
|
||||
"1": {
|
||||
"name": "autodischarging",
|
||||
"description": "auto discharging towards storage level"
|
||||
},
|
||||
"2": {
|
||||
"name": "hotswap",
|
||||
"description": "hot-swap"
|
||||
}
|
||||
}
|
||||
},
|
||||
"esc_fault_reason_t": {
|
||||
"type": "uint8_t",
|
||||
"description": "Bitfield for ESC failure reason",
|
||||
"entries": {
|
||||
"0": {
|
||||
"name": "over_current",
|
||||
"description": "detected over current"
|
||||
},
|
||||
"1": {
|
||||
"name": "over_voltage",
|
||||
"description": "detected over voltage"
|
||||
},
|
||||
"2": {
|
||||
"name": "motor_over_temp",
|
||||
"description": "Motor has reached a critical temperature"
|
||||
},
|
||||
"3": {
|
||||
"name": "over_rpm",
|
||||
"description": "Motor RPM is exceeding the limits"
|
||||
},
|
||||
"4": {
|
||||
"name": "inconsistent_cmd",
|
||||
"description": "received an invalid control command"
|
||||
},
|
||||
"5": {
|
||||
"name": "motor_stuck",
|
||||
"description": "Motor stalled"
|
||||
},
|
||||
"6": {
|
||||
"name": "failure_generic",
|
||||
"description": "detected a generic hardware failure"
|
||||
},
|
||||
"7": {
|
||||
"name": "motor_warn_temp",
|
||||
"description": "Motor is over-heating"
|
||||
},
|
||||
"8": {
|
||||
"name": "esc_warn_temp",
|
||||
"description": "is over-heating"
|
||||
},
|
||||
"9": {
|
||||
"name": "esc_over_temp",
|
||||
"description": "reached a critical temperature"
|
||||
}
|
||||
}
|
||||
},
|
||||
"suggested_action_t": {
|
||||
"type": "uint8_t",
|
||||
"description": "Suggested actions for the user in case of a safety critical event is triggered",
|
||||
"entries": {
|
||||
"0": {
|
||||
"name": "none",
|
||||
"description": ""
|
||||
},
|
||||
"1": {
|
||||
"name": "land",
|
||||
"description": "Land now!"
|
||||
},
|
||||
"2": {
|
||||
"name": "reduce_throttle",
|
||||
"description": "Reduce throttle!"
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -237,6 +237,24 @@ constexpr int16_t negate<int16_t>(int16_t value)
|
|||
return -value;
|
||||
}
|
||||
|
||||
/*
|
||||
* This function calculates the Hamming weight, i.e. counts the number of bits that are set
|
||||
* in a given integer.
|
||||
*/
|
||||
|
||||
template<typename T>
|
||||
int countSetBits(T n)
|
||||
{
|
||||
int count = 0;
|
||||
|
||||
while (n) {
|
||||
count += n & 1;
|
||||
n >>= 1;
|
||||
}
|
||||
|
||||
return count;
|
||||
}
|
||||
|
||||
inline bool isFinite(const float &value)
|
||||
{
|
||||
return PX4_ISFINITE(value);
|
||||
|
|
|
|||
|
|
@ -212,3 +212,12 @@ TEST(FunctionsTest, lerp)
|
|||
EXPECT_FLOAT_EQ(lerp(.2f, .3f, -.1f), .19f);
|
||||
EXPECT_FLOAT_EQ(lerp(-.4f, .3f, 1.1f), .37f);
|
||||
}
|
||||
|
||||
TEST(FunctionsTest, countSetBits)
|
||||
{
|
||||
EXPECT_EQ(countSetBits(255), 8);
|
||||
EXPECT_EQ(countSetBits(65535), 16);
|
||||
EXPECT_EQ(countSetBits(0), 0);
|
||||
EXPECT_EQ(countSetBits(0xffffffffu), 32);
|
||||
EXPECT_EQ(countSetBits(754323), 9);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -38,21 +38,10 @@
|
|||
#include <lib/parameters/param.h>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/topics/system_power.h>
|
||||
#include <math.h>
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
unsigned int countSetBits(unsigned int n)
|
||||
{
|
||||
unsigned int count = 0;
|
||||
|
||||
while (n) {
|
||||
count += n & 1;
|
||||
n >>= 1;
|
||||
}
|
||||
|
||||
return count;
|
||||
}
|
||||
|
||||
bool PreFlightCheck::powerCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail,
|
||||
const bool prearm)
|
||||
{
|
||||
|
|
@ -100,7 +89,7 @@ bool PreFlightCheck::powerCheck(orb_advert_t *mavlink_log_pub, const vehicle_sta
|
|||
}
|
||||
|
||||
|
||||
const int power_module_count = countSetBits(system_power.brick_valid);
|
||||
const int power_module_count = math::countSetBits(system_power.brick_valid);
|
||||
|
||||
if (power_module_count < required_power_module_count) {
|
||||
success = false;
|
||||
|
|
|
|||
|
|
@ -510,6 +510,44 @@ static constexpr const char *arm_disarm_reason_str(arm_disarm_reason_t calling_r
|
|||
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::battery_fault_count)
|
||||
, "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";
|
||||
|
||||
case battery_fault_reason_t::battery_fault_count: return "error! battery fault count";
|
||||
|
||||
}
|
||||
|
||||
return "";
|
||||
};
|
||||
|
||||
|
||||
|
||||
using navigation_mode_t = events::px4::enums::navigation_mode_t;
|
||||
|
||||
static inline navigation_mode_t navigation_mode(uint8_t main_state)
|
||||
|
|
@ -3652,6 +3690,8 @@ void Commander::avoidance_check()
|
|||
|
||||
void Commander::battery_status_check()
|
||||
{
|
||||
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.
|
||||
|
|
@ -3659,20 +3699,57 @@ void Commander::battery_status_check()
|
|||
// 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();
|
||||
size_t num_connected_batteries{0};
|
||||
float worst_battery_time_s{NAN};
|
||||
|
||||
_battery_current = 0.0f;
|
||||
|
||||
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 (_armed.armed) {
|
||||
|
||||
if ((_last_connected_batteries & (1 << 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])) {
|
||||
const char *mode_text = nullptr;
|
||||
|
||||
switch (battery.mode) {
|
||||
case (battery_status_s::BATTERY_MODE_AUTO_DISCHARGING):
|
||||
mode_text = "auto discharging";
|
||||
break;
|
||||
|
||||
case (battery_status_s::BATTERY_MODE_HOT_SWAP):
|
||||
mode_text = "hot swap";
|
||||
break;
|
||||
|
||||
default:
|
||||
mode_text = "unknown";
|
||||
break;
|
||||
}
|
||||
|
||||
mavlink_log_critical(&_mavlink_log_pub, "Battery %d is in %s mode!", index + 1, mode_text);
|
||||
}
|
||||
}
|
||||
|
||||
_last_battery_mode[index] = battery.mode;
|
||||
|
||||
if (battery.connected) {
|
||||
num_connected_batteries++;
|
||||
_last_connected_batteries |= 1 << index;
|
||||
|
||||
if (battery.warning > worst_warning) {
|
||||
worst_warning = battery.warning;
|
||||
|
|
@ -3682,6 +3759,27 @@ void Commander::battery_status_check()
|
|||
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::battery_fault_count);
|
||||
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)), _armed.armed ? "Land now!" : "");
|
||||
|
||||
events::send<uint8_t, events::px4::enums::battery_fault_reason_t>(events::ID("battery_fault"), {events::Log::Emergency, events::LogInternal::Warning},
|
||||
"Battery {1}: {2}", index + 1, static_cast<battery_fault_reason_t>(fault_index));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
_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)))) {
|
||||
|
|
@ -3745,9 +3843,11 @@ void Commander::battery_status_check()
|
|||
// 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 > 0)
|
||||
&& (math::countSetBits(_last_connected_batteries) >= battery_required_count)
|
||||
// No currently-connected batteries have any warning
|
||||
&& (_battery_warning == battery_status_s::BATTERY_WARNING_NONE);
|
||||
&& (_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
|
||||
if (battery_warning_level_increased_while_armed) {
|
||||
|
|
@ -4063,7 +4163,7 @@ void Commander::esc_status_check()
|
|||
}
|
||||
}
|
||||
|
||||
mavlink_log_critical(&_mavlink_log_pub, "%soffline\t", esc_fail_msg);
|
||||
mavlink_log_critical(&_mavlink_log_pub, "%soffline. %s\t", esc_fail_msg, _armed.armed ? "Land now!" : "");
|
||||
|
||||
_last_esc_online_flags = esc_status.esc_online_flags;
|
||||
_status_flags.condition_escs_error = true;
|
||||
|
|
@ -4073,54 +4173,57 @@ void Commander::esc_status_check()
|
|||
|
||||
for (int index = 0; index < esc_status.esc_count; index++) {
|
||||
|
||||
if (esc_status.esc[index].failures > esc_report_s::FAILURE_NONE) {
|
||||
_status_flags.condition_escs_failure = true;
|
||||
_status_flags.condition_escs_failure |= esc_status.esc[index].failures > esc_report_s::FAILURE_NONE;
|
||||
|
||||
if (esc_status.esc[index].failures != _last_esc_failure[index]) {
|
||||
if (esc_status.esc[index].failures != _last_esc_failure[index]) {
|
||||
|
||||
if (esc_status.esc[index].failures & esc_report_s::FAILURE_OVER_CURRENT_MASK) {
|
||||
mavlink_log_critical(&_mavlink_log_pub, "ESC%d: over current\t", index + 1);
|
||||
events::send<uint8_t>(events::ID("commander_esc_over_current"), events::Log::Critical,
|
||||
"ESC{1}: over current", index + 1);
|
||||
}
|
||||
if (esc_status.esc[index].failures & esc_report_s::FAILURE_OVER_CURRENT_MASK) {
|
||||
mavlink_log_critical(&_mavlink_log_pub, "ESC%d: over current\t", index + 1);
|
||||
events::send<uint8_t>(events::ID("commander_esc_over_current"), events::Log::Critical,
|
||||
"ESC{1}: over current", index + 1);
|
||||
}
|
||||
|
||||
if (esc_status.esc[index].failures & esc_report_s::FAILURE_OVER_VOLTAGE_MASK) {
|
||||
mavlink_log_critical(&_mavlink_log_pub, "ESC%d: over voltage\t", index + 1);
|
||||
events::send<uint8_t>(events::ID("commander_esc_over_voltage"), events::Log::Critical,
|
||||
"ESC{1}: over voltage", index + 1);
|
||||
}
|
||||
if (esc_status.esc[index].failures & esc_report_s::FAILURE_OVER_VOLTAGE_MASK) {
|
||||
mavlink_log_critical(&_mavlink_log_pub, "ESC%d: over voltage\t", index + 1);
|
||||
events::send<uint8_t>(events::ID("commander_esc_over_voltage"), events::Log::Critical,
|
||||
"ESC{1}: over voltage", index + 1);
|
||||
}
|
||||
|
||||
if (esc_status.esc[index].failures & esc_report_s::FAILURE_OVER_TEMPERATURE_MASK) {
|
||||
mavlink_log_critical(&_mavlink_log_pub, "ESC%d: over temperature\t", index + 1);
|
||||
events::send<uint8_t>(events::ID("commander_esc_over_temp"), events::Log::Critical,
|
||||
"ESC{1}: over temperature", index + 1);
|
||||
}
|
||||
if (esc_status.esc[index].failures & esc_report_s::FAILURE_WARN_ESC_TEMPERATURE_MASK) {
|
||||
mavlink_log_critical(&_mavlink_log_pub, "ESC%d: over temperature\t", index + 1);
|
||||
events::send<uint8_t>(events::ID("commander_motor_over_temp"), events::Log::Critical,
|
||||
"ESC{1}: over temperature", index + 1);
|
||||
}
|
||||
|
||||
if (esc_status.esc[index].failures & esc_report_s::FAILURE_OVER_RPM_MASK) {
|
||||
mavlink_log_critical(&_mavlink_log_pub, "ESC%d: over RPM\t", index + 1);
|
||||
events::send<uint8_t>(events::ID("commander_esc_over_rpm"), events::Log::Critical,
|
||||
"ESC{1}: over RPM", index + 1);
|
||||
}
|
||||
if (esc_status.esc[index].failures & esc_report_s::FAILURE_OVER_ESC_TEMPERATURE_MASK) {
|
||||
mavlink_log_critical(&_mavlink_log_pub, "ESC%d: over temperature\t", index + 1);
|
||||
events::send<uint8_t>(events::ID("commander_esc_over_temp"), events::Log::Critical,
|
||||
"ESC{1}: over temperature", index + 1);
|
||||
}
|
||||
|
||||
if (esc_status.esc[index].failures & esc_report_s::FAILURE_INCONSISTENT_CMD_MASK) {
|
||||
mavlink_log_critical(&_mavlink_log_pub, "ESC%d: command inconsistency\t", index + 1);
|
||||
events::send<uint8_t>(events::ID("commander_esc_cmd_inconsistent"), events::Log::Critical,
|
||||
"ESC{1}: command inconsistency", index + 1);
|
||||
}
|
||||
if (esc_status.esc[index].failures & esc_report_s::FAILURE_OVER_RPM_MASK) {
|
||||
mavlink_log_critical(&_mavlink_log_pub, "ESC%d: over RPM\t", index + 1);
|
||||
events::send<uint8_t>(events::ID("commander_esc_over_rpm"), events::Log::Critical,
|
||||
"ESC{1}: over RPM", index + 1);
|
||||
}
|
||||
|
||||
if (esc_status.esc[index].failures & esc_report_s::FAILURE_MOTOR_STUCK_MASK) {
|
||||
mavlink_log_critical(&_mavlink_log_pub, "ESC%d: motor stuck\t", index + 1);
|
||||
events::send<uint8_t>(events::ID("commander_esc_motor_stuck"), events::Log::Critical,
|
||||
"ESC{1}: motor stuck", index + 1);
|
||||
}
|
||||
if (esc_status.esc[index].failures & esc_report_s::FAILURE_INCONSISTENT_CMD_MASK) {
|
||||
mavlink_log_critical(&_mavlink_log_pub, "ESC%d: command inconsistency\t", index + 1);
|
||||
events::send<uint8_t>(events::ID("commander_esc_cmd_inconsistent"), events::Log::Critical,
|
||||
"ESC{1}: command inconsistency", index + 1);
|
||||
}
|
||||
|
||||
if (esc_status.esc[index].failures & esc_report_s::FAILURE_GENERIC_MASK) {
|
||||
mavlink_log_critical(&_mavlink_log_pub, "ESC%d: generic failure - code %d\t", index + 1,
|
||||
esc_status.esc[index].esc_state);
|
||||
events::send<uint8_t, uint8_t>(events::ID("commander_esc_generic_failure"), events::Log::Critical,
|
||||
"ESC{1}: generic failure (code {2})", index + 1, esc_status.esc[index].esc_state);
|
||||
}
|
||||
if (esc_status.esc[index].failures & esc_report_s::FAILURE_MOTOR_STUCK_MASK) {
|
||||
mavlink_log_critical(&_mavlink_log_pub, "ESC%d: motor stuck\t", index + 1);
|
||||
events::send<uint8_t>(events::ID("commander_esc_motor_stuck"), events::Log::Critical,
|
||||
"ESC{1}: motor stuck", index + 1);
|
||||
}
|
||||
|
||||
if (esc_status.esc[index].failures & esc_report_s::FAILURE_GENERIC_MASK) {
|
||||
mavlink_log_critical(&_mavlink_log_pub, "ESC%d: generic failure - code %d\t", index + 1,
|
||||
esc_status.esc[index].esc_state);
|
||||
events::send<uint8_t, uint8_t>(events::ID("commander_esc_generic_failure"), events::Log::Critical,
|
||||
"ESC{1}: generic failure (code {2})", index + 1, esc_status.esc[index].esc_state);
|
||||
}
|
||||
|
||||
_last_esc_failure[index] = esc_status.esc[index].failures;
|
||||
|
|
|
|||
|
|
@ -344,6 +344,11 @@ private:
|
|||
|
||||
uint8_t _battery_warning{battery_status_s::BATTERY_WARNING_NONE};
|
||||
float _battery_current{0.0f};
|
||||
uint8_t _last_connected_batteries{0};
|
||||
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_killed{false};
|
||||
|
|
|
|||
|
|
@ -101,11 +101,35 @@ private:
|
|||
bat_msg.charge_state = MAV_BATTERY_CHARGE_STATE_FAILED;
|
||||
break;
|
||||
|
||||
case (battery_status_s::BATTERY_STATE_UNHEALTHY):
|
||||
bat_msg.charge_state = MAV_BATTERY_CHARGE_STATE_UNHEALTHY;
|
||||
break;
|
||||
|
||||
case (battery_status_s::BATTERY_STATE_CHARGING):
|
||||
bat_msg.charge_state = MAV_BATTERY_CHARGE_STATE_CHARGING;
|
||||
break;
|
||||
|
||||
default:
|
||||
bat_msg.charge_state = MAV_BATTERY_CHARGE_STATE_UNDEFINED;
|
||||
break;
|
||||
}
|
||||
|
||||
switch (battery_status.mode) {
|
||||
case (battery_status_s::BATTERY_MODE_AUTO_DISCHARGING):
|
||||
bat_msg.mode = MAV_BATTERY_MODE_AUTO_DISCHARGING;
|
||||
break;
|
||||
|
||||
case (battery_status_s::BATTERY_MODE_HOT_SWAP):
|
||||
bat_msg.mode = MAV_BATTERY_MODE_HOT_SWAP;
|
||||
break;
|
||||
|
||||
default:
|
||||
bat_msg.mode = MAV_BATTERY_MODE_UNKNOWN;
|
||||
break;
|
||||
}
|
||||
|
||||
bat_msg.fault_bitmask = battery_status.faults;
|
||||
|
||||
// check if temperature valid
|
||||
if (battery_status.connected && PX4_ISFINITE(battery_status.temperature)) {
|
||||
bat_msg.temperature = battery_status.temperature * 100.f;
|
||||
|
|
|
|||
Loading…
Reference in New Issue