From dea7e969bd9dafdeb40e42e6050439347c5d6a5f Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 6 Dec 2021 22:32:49 +0900 Subject: [PATCH] AP_BattMonitor_UAVCAN: re-format text based fault reporting and add get_mavlink_fault_bitmask --- libraries/AP_BattMonitor/AP_BattMonitor.cpp | 12 ++++++ libraries/AP_BattMonitor/AP_BattMonitor.h | 3 ++ .../AP_BattMonitor/AP_BattMonitor_Backend.h | 3 ++ .../AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp | 43 +++++++++++-------- .../AP_BattMonitor/AP_BattMonitor_UAVCAN.h | 23 +++++----- 5 files changed, 56 insertions(+), 28 deletions(-) diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.cpp b/libraries/AP_BattMonitor/AP_BattMonitor.cpp index d30b18e651..52b3d63800 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor.cpp @@ -689,6 +689,9 @@ MAV_BATTERY_CHARGE_STATE AP_BattMonitor::get_mavlink_charge_state(const uint8_t switch (state[instance].failsafe) { case Failsafe::None: + if (get_mavlink_fault_bitmask(instance) != 0 || !healthy()) { + return MAV_BATTERY_CHARGE_STATE_UNHEALTHY; + } return MAV_BATTERY_CHARGE_STATE_OK; case Failsafe::Low: @@ -702,6 +705,15 @@ MAV_BATTERY_CHARGE_STATE AP_BattMonitor::get_mavlink_charge_state(const uint8_t return MAV_BATTERY_CHARGE_STATE_UNDEFINED; } +// Returns mavlink fault state +uint32_t AP_BattMonitor::get_mavlink_fault_bitmask(const uint8_t instance) const +{ + if (drivers[instance] == nullptr) { + return 0; + } + return drivers[instance]->get_mavlink_fault_bitmask(); +} + namespace AP { AP_BattMonitor &battery() diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.h b/libraries/AP_BattMonitor/AP_BattMonitor.h index b4ebcf1f81..2381fa7c3a 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor.h @@ -241,6 +241,9 @@ public: // Returns mavlink charge state MAV_BATTERY_CHARGE_STATE get_mavlink_charge_state(const uint8_t instance) const; + // Returns mavlink fault state + uint32_t get_mavlink_fault_bitmask(const uint8_t instance) const; + static const struct AP_Param::GroupInfo var_info[]; protected: diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h index 43387214cc..14dffab7b3 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h @@ -72,6 +72,9 @@ public: // reset remaining percentage to given value virtual bool reset_remaining(float percentage); + // return mavlink fault bitmask (see MAV_BATTERY_FAULT enum) + virtual uint32_t get_mavlink_fault_bitmask() const { return 0; } + // logging functions void Log_Write_BAT(const uint8_t instance, const uint64_t time_us) const; void Log_Write_BCL(const uint8_t instance, const uint64_t time_us) const; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp index 66b7b06eef..02403351a2 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp @@ -353,37 +353,44 @@ void AP_BattMonitor_UAVCAN::mppt_set_powered_state(bool power_on, bool force) } // report changes in MPPT faults -void AP_BattMonitor_UAVCAN::mppt_check_and_report_faults(const uint8_t flags) +void AP_BattMonitor_UAVCAN::mppt_check_and_report_faults(uint8_t fault_flags) { - if (_mppt.fault_flags == flags) { - // only report flags when they change + // return immediately if no changes + if (_mppt.fault_flags == fault_flags) { return; } - _mppt.fault_flags = flags; + _mppt.fault_flags = fault_flags; + // handle recovery + if (_mppt.fault_flags == 0) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Battery %u: OK", (unsigned)_instance+1); + return; + } + + // send battery faults via text messages for (uint8_t fault_bit=0x01; fault_bit <= 0x08; fault_bit <<= 1) { // this loop is to generate multiple messages if there are multiple concurrent faults, but also run once if there are no faults - if ((fault_bit & flags) != 0 || flags == 0) { - const MPPT_FaultFlags err = (flags == 0) ? MPPT_FaultFlags::NONE : (MPPT_FaultFlags)fault_bit; - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Battery %u: Fault: %s", (unsigned)_instance+1, mppt_fault_string(err)); - } - if (flags == 0) { - return; + if ((fault_bit & fault_flags) != 0) { + const MPPT_FaultFlags err = (MPPT_FaultFlags)fault_bit; + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Battery %u: %s", (unsigned)_instance+1, mppt_fault_string(err)); } } } // returns string description of MPPT fault bit. Only handles single bit faults -const char* AP_BattMonitor_UAVCAN::mppt_fault_string(const MPPT_FaultFlags fault) +const char* AP_BattMonitor_UAVCAN::mppt_fault_string(MPPT_FaultFlags fault) { switch (fault) { - case MPPT_FaultFlags::NONE: return "None"; - case MPPT_FaultFlags::OVER_VOLTAGE: return "Over-Voltage"; - case MPPT_FaultFlags::UNDER_VOLTAGE: return "Under-Voltage"; - case MPPT_FaultFlags::OVER_CURRENT: return "Over-Current"; - case MPPT_FaultFlags::OVER_TEMPERATURE: return "Over-Temp"; + case MPPT_FaultFlags::OVER_VOLTAGE: + return "over voltage"; + case MPPT_FaultFlags::UNDER_VOLTAGE: + return "under voltage"; + case MPPT_FaultFlags::OVER_CURRENT: + return "over current"; + case MPPT_FaultFlags::OVER_TEMPERATURE: + return "over temp"; } - return "Unknown"; + return "unknown"; } // return mavlink fault bitmask (see MAV_BATTERY_FAULT enum) @@ -404,6 +411,8 @@ uint32_t AP_BattMonitor_UAVCAN::get_mavlink_fault_bitmask() const } if (_mppt.fault_flags & (uint8_t)MPPT_FaultFlags::OVER_TEMPERATURE) { mav_fault_bitmask |= MAV_BATTERY_FAULT_OVER_TEMPERATURE; + } + return mav_fault_bitmask; } #endif diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.h b/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.h index 3425903abc..272ba1857b 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.h @@ -19,14 +19,6 @@ public: UAVCAN_BATTERY_INFO = 0 }; - enum class MPPT_FaultFlags : uint8_t { - NONE = 0, - OVER_VOLTAGE = (1U<<0), - UNDER_VOLTAGE = (1U<<1), - OVER_CURRENT = (1U<<2), - OVER_TEMPERATURE = (1U<<3), - }; - /// Constructor AP_BattMonitor_UAVCAN(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state, BattMonitor_UAVCAN_Type type, AP_BattMonitor_Params ¶ms); @@ -50,6 +42,9 @@ public: bool get_cycle_count(uint16_t &cycles) const override; + // return mavlink fault bitmask (see MAV_BATTERY_FAULT enum) + uint32_t get_mavlink_fault_bitmask() const override; + static void subscribe_msgs(AP_UAVCAN* ap_uavcan); static AP_BattMonitor_UAVCAN* get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id, uint8_t battery_id); static void handle_battery_info_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const BattInfoCb &cb); @@ -67,13 +62,19 @@ private: return (AP::battery().get_serial_number(instance) < 0) || (AP::battery().get_serial_number(instance) == (int32_t)battery_id); } - // MPPT related methods + // MPPT related enums and methods + enum class MPPT_FaultFlags : uint8_t { + OVER_VOLTAGE = (1U<<0), + UNDER_VOLTAGE = (1U<<1), + OVER_CURRENT = (1U<<2), + OVER_TEMPERATURE = (1U<<3), + }; void handle_mppt_stream(const MpptStreamCb &cb); void mppt_set_bootup_powered_state(); void mppt_set_armed_powered_state(); void mppt_set_powered_state(bool power_on, bool force); - void mppt_check_and_report_faults(const uint8_t flags); - static const char* mppt_fault_string(const MPPT_FaultFlags fault); + void mppt_check_and_report_faults(uint8_t fault_flags); + const char* mppt_fault_string(MPPT_FaultFlags fault); AP_BattMonitor::BattMonitor_State _interim_state; BattMonitor_UAVCAN_Type _type;