From 63e67db5167128ed3c5f59bdf8a57777b0e80a2d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 8 Apr 2023 13:53:24 +1000 Subject: [PATCH] AP_BattMonitor: rename UAVCAN drivers to DroneCAN --- .../AP_BattMonitor_DroneCAN.cpp | 411 ++++++++++++++++++ .../AP_BattMonitor/AP_BattMonitor_DroneCAN.h | 115 +++++ 2 files changed, 526 insertions(+) create mode 100644 libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp create mode 100644 libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.h diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp new file mode 100644 index 0000000000..434292f870 --- /dev/null +++ b/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp @@ -0,0 +1,411 @@ +#include "AP_BattMonitor_config.h" + +#if AP_BATTERY_UAVCAN_BATTERYINFO_ENABLED + +#include +#include "AP_BattMonitor.h" +#include "AP_BattMonitor_UAVCAN.h" + +#include +#include +#include +#include +#include +#include + +#define LOG_TAG "BattMon" + +extern const AP_HAL::HAL& hal; + +const AP_Param::GroupInfo AP_BattMonitor_UAVCAN::var_info[] = { + + // @Param: CURR_MULT + // @DisplayName: Scales reported power monitor current + // @Description: Multiplier applied to all current related reports to allow for adjustment if no UAVCAN param access or current splitting applications + // @Range: .1 10 + // @User: Advanced + AP_GROUPINFO("CURR_MULT", 30, AP_BattMonitor_UAVCAN, _curr_mult, 1.0), + + // Param indexes must be between 30 and 39 to avoid conflict with other battery monitor param tables loaded by pointer + + AP_GROUPEND +}; + +/// Constructor +AP_BattMonitor_UAVCAN::AP_BattMonitor_UAVCAN(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state, BattMonitor_UAVCAN_Type type, AP_BattMonitor_Params ¶ms) : + AP_BattMonitor_Backend(mon, mon_state, params), + _type(type) +{ + AP_Param::setup_object_defaults(this,var_info); + _state.var_info = var_info; + + // starts with not healthy + _state.healthy = false; +} + +void AP_BattMonitor_UAVCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan) +{ + if (ap_dronecan == nullptr) { + return; + } + + if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_battery_info_trampoline, ap_dronecan->get_driver_index()) == nullptr) { + AP_BoardConfig::allocation_error("battinfo_sub"); + } + + if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_battery_info_aux_trampoline, ap_dronecan->get_driver_index()) == nullptr) { + AP_BoardConfig::allocation_error("battinfo_aux_sub"); + } + + if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_mppt_stream_trampoline, ap_dronecan->get_driver_index()) == nullptr) { + AP_BoardConfig::allocation_error("mppt_stream_sub"); + } +} + +AP_BattMonitor_UAVCAN* AP_BattMonitor_UAVCAN::get_uavcan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id, uint8_t battery_id) +{ + if (ap_dronecan == nullptr) { + return nullptr; + } + for (uint8_t i = 0; i < AP::battery()._num_instances; i++) { + if (AP::battery().drivers[i] == nullptr || + AP::battery().get_type(i) != AP_BattMonitor::Type::UAVCAN_BatteryInfo) { + continue; + } + AP_BattMonitor_UAVCAN* driver = (AP_BattMonitor_UAVCAN*)AP::battery().drivers[i]; + if (driver->_ap_dronecan == ap_dronecan && driver->_node_id == node_id && match_battery_id(i, battery_id)) { + return driver; + } + } + // find empty uavcan driver + for (uint8_t i = 0; i < AP::battery()._num_instances; i++) { + if (AP::battery().drivers[i] != nullptr && + AP::battery().get_type(i) == AP_BattMonitor::Type::UAVCAN_BatteryInfo && + match_battery_id(i, battery_id)) { + + AP_BattMonitor_UAVCAN* batmon = (AP_BattMonitor_UAVCAN*)AP::battery().drivers[i]; + if(batmon->_ap_dronecan != nullptr || batmon->_node_id != 0) { + continue; + } + batmon->_ap_dronecan = ap_dronecan; + batmon->_node_id = node_id; + batmon->_instance = i; + batmon->init(); + AP::can().log_text(AP_CANManager::LOG_INFO, + LOG_TAG, + "Registered BattMonitor Node %d on Bus %d\n", + node_id, + ap_dronecan->get_driver_index()); + return batmon; + } + } + return nullptr; +} + +void AP_BattMonitor_UAVCAN::handle_battery_info(const uavcan_equipment_power_BatteryInfo &msg) +{ + update_interim_state(msg.voltage, msg.current, msg.temperature, msg.state_of_charge_pct); + + WITH_SEMAPHORE(_sem_battmon); + _remaining_capacity_wh = msg.remaining_capacity_wh; + _full_charge_capacity_wh = msg.full_charge_capacity_wh; +} + +void AP_BattMonitor_UAVCAN::update_interim_state(const float voltage, const float current, const float temperature_K, const uint8_t soc) +{ + WITH_SEMAPHORE(_sem_battmon); + + _interim_state.voltage = voltage; + _interim_state.current_amps = _curr_mult * current; + _soc = soc; + + if (!isnan(temperature_K) && temperature_K > 0) { + // Temperature reported from battery in kelvin and stored internally in Celsius. + _interim_state.temperature = KELVIN_TO_C(temperature_K); + _interim_state.temperature_time = AP_HAL::millis(); + } + + const uint32_t tnow = AP_HAL::micros(); + + if (!_has_battery_info_aux || _mppt.is_detected) { + const uint32_t dt_us = tnow - _interim_state.last_time_micros; + + // update total current drawn since startup + update_consumed(_interim_state, dt_us); + } + + // record time + _interim_state.last_time_micros = tnow; + _interim_state.healthy = true; +} + +void AP_BattMonitor_UAVCAN::handle_battery_info_aux(const ardupilot_equipment_power_BatteryInfoAux &msg) +{ + WITH_SEMAPHORE(_sem_battmon); + uint8_t cell_count = MIN(ARRAY_SIZE(_interim_state.cell_voltages.cells), msg.voltage_cell.len); + float remaining_capacity_ah = _remaining_capacity_wh / msg.nominal_voltage; + float full_charge_capacity_ah = _full_charge_capacity_wh / msg.nominal_voltage; + + _cycle_count = msg.cycle_count; + for (uint8_t i = 0; i < cell_count; i++) { + _interim_state.cell_voltages.cells[i] = msg.voltage_cell.data[i] * 1000; + } + _interim_state.is_powering_off = msg.is_powering_off; + _interim_state.consumed_mah = (full_charge_capacity_ah - remaining_capacity_ah) * 1000; + _interim_state.consumed_wh = _full_charge_capacity_wh - _remaining_capacity_wh; + _interim_state.time_remaining = is_zero(_interim_state.current_amps) ? 0 : (remaining_capacity_ah / _interim_state.current_amps * 3600); + _interim_state.has_time_remaining = true; + + _has_cell_voltages = true; + _has_time_remaining = true; + _has_consumed_energy = true; + _has_battery_info_aux = true; +} + +void AP_BattMonitor_UAVCAN::handle_mppt_stream(const mppt_Stream &msg) +{ + const bool use_input_value = option_is_set(AP_BattMonitor_Params::Options::MPPT_Use_Input_Value); + const float voltage = use_input_value ? msg.input_voltage : msg.output_voltage; + const float current = use_input_value ? msg.input_current : msg.output_current; + + // use an invalid soc so we use the library calculated one + const uint8_t soc = 127; + + // convert C to Kelvin + const float temperature_K = isnan(msg.temperature) ? 0 : C_TO_KELVIN(msg.temperature); + + update_interim_state(voltage, current, temperature_K, soc); + + if (!_mppt.is_detected) { + // this is the first time the mppt message has been received + // so set powered up state + _mppt.is_detected = true; + + // Boot/Power-up event + if (option_is_set(AP_BattMonitor_Params::Options::MPPT_Power_On_At_Boot)) { + mppt_set_powered_state(true); + } else if (option_is_set(AP_BattMonitor_Params::Options::MPPT_Power_Off_At_Boot)) { + mppt_set_powered_state(false); + } + } + +#if AP_BATTMONITOR_UAVCAN_MPPT_DEBUG + if (_mppt.fault_flags != msg.fault_flags) { + mppt_report_faults(_instance, msg.fault_flags); + } +#endif + _mppt.fault_flags = msg.fault_flags; +} + +void AP_BattMonitor_UAVCAN::handle_battery_info_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_power_BatteryInfo &msg) +{ + AP_BattMonitor_UAVCAN* driver = get_uavcan_backend(ap_dronecan, transfer.source_node_id, msg.battery_id); + if (driver == nullptr) { + return; + } + driver->handle_battery_info(msg); +} + +void AP_BattMonitor_UAVCAN::handle_battery_info_aux_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const ardupilot_equipment_power_BatteryInfoAux &msg) +{ + AP_BattMonitor_UAVCAN* driver = get_uavcan_backend(ap_dronecan, transfer.source_node_id, msg.battery_id); + if (driver == nullptr) { + return; + } + driver->handle_battery_info_aux(msg); +} + +void AP_BattMonitor_UAVCAN::handle_mppt_stream_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const mppt_Stream &msg) +{ + AP_BattMonitor_UAVCAN* driver = get_uavcan_backend(ap_dronecan, transfer.source_node_id, transfer.source_node_id); + if (driver == nullptr) { + return; + } + driver->handle_mppt_stream(msg); +} + +// read - read the voltage and current +void AP_BattMonitor_UAVCAN::read() +{ + uint32_t tnow = AP_HAL::micros(); + + // timeout after 5 seconds + if ((tnow - _interim_state.last_time_micros) > AP_BATTMONITOR_UAVCAN_TIMEOUT_MICROS) { + _interim_state.healthy = false; + } + // Copy over relevant states over to main state + WITH_SEMAPHORE(_sem_battmon); + _state.temperature = _interim_state.temperature; + _state.temperature_time = _interim_state.temperature_time; + _state.voltage = _interim_state.voltage; + _state.current_amps = _interim_state.current_amps; + _state.consumed_mah = _interim_state.consumed_mah; + _state.consumed_wh = _interim_state.consumed_wh; + _state.last_time_micros = _interim_state.last_time_micros; + _state.healthy = _interim_state.healthy; + _state.time_remaining = _interim_state.time_remaining; + _state.has_time_remaining = _interim_state.has_time_remaining; + _state.is_powering_off = _interim_state.is_powering_off; + memcpy(_state.cell_voltages.cells, _interim_state.cell_voltages.cells, sizeof(_state.cell_voltages)); + + _has_temperature = (AP_HAL::millis() - _state.temperature_time) <= AP_BATT_MONITOR_TIMEOUT; + + // check if MPPT should be powered on/off depending upon arming state + if (_mppt.is_detected) { + mppt_check_powered_state(); + } +} + +/// capacity_remaining_pct - returns true if the percentage is valid and writes to percentage argument +bool AP_BattMonitor_UAVCAN::capacity_remaining_pct(uint8_t &percentage) const +{ + if ((uint32_t(_params._options.get()) & uint32_t(AP_BattMonitor_Params::Options::Ignore_UAVCAN_SoC)) || + _mppt.is_detected || + _soc == 127) { + // a UAVCAN battery monitor may not be able to supply a state of charge. If it can't then + // the user can set the option to use current integration in the backend instead. + // SOC of 127 is used as an invalid SOC flag ie system configuration errors or SOC estimation unavailable + return AP_BattMonitor_Backend::capacity_remaining_pct(percentage); + } + + // the monitor must have current readings in order to estimate consumed_mah and be healthy + if (!has_current() || !_state.healthy) { + return false; + } + + percentage = _soc; + return true; +} + +/// get_cycle_count - return true if cycle count can be provided and fills in cycles argument +bool AP_BattMonitor_UAVCAN::get_cycle_count(uint16_t &cycles) const +{ + if (_has_battery_info_aux) { + cycles = _cycle_count; + return true; + } + return false; +} + +// request MPPT board to power on/off depending upon vehicle arming state as specified by BATT_OPTIONS +void AP_BattMonitor_UAVCAN::mppt_check_powered_state() +{ + if ((_mppt.powered_state_remote_ms != 0) && (AP_HAL::millis() - _mppt.powered_state_remote_ms >= 1000)) { + // there's already a set attempt that didnt' respond. Retry at 1Hz + mppt_set_powered_state(_mppt.powered_state); + } + + // check if vehicle armed state has changed + const bool vehicle_armed = hal.util->get_soft_armed(); + if ((!_mppt.vehicle_armed_last && vehicle_armed) && option_is_set(AP_BattMonitor_Params::Options::MPPT_Power_On_At_Arm)) { + // arm event + mppt_set_powered_state(true); + } else if ((_mppt.vehicle_armed_last && !vehicle_armed) && option_is_set(AP_BattMonitor_Params::Options::MPPT_Power_Off_At_Disarm)) { + // disarm event + mppt_set_powered_state(false); + } + _mppt.vehicle_armed_last = vehicle_armed; +} + +// request MPPT board to power on or off +// power_on should be true to power on the MPPT, false to power off +// force should be true to force sending the state change request to the MPPT +void AP_BattMonitor_UAVCAN::mppt_set_powered_state(bool power_on) +{ + if (_ap_dronecan == nullptr || !_mppt.is_detected) { + return; + } + + _mppt.powered_state = power_on; + + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Battery %u: powering %s%s", (unsigned)_instance+1, _mppt.powered_state ? "ON" : "OFF", + (_mppt.powered_state_remote_ms == 0) ? "" : " Retry"); + + mppt_OutputEnableRequest request; + request.enable = _mppt.powered_state; + request.disable = !request.enable; + + if (mppt_outputenable_client == nullptr) { + mppt_outputenable_client = new Canard::Client{_ap_dronecan->get_canard_iface(), mppt_outputenable_res_cb}; + if (mppt_outputenable_client == nullptr) { + return; + } + } + mppt_outputenable_client->request(_node_id, request); +} + +// callback from outputEnable to verify it is enabled or disabled +void AP_BattMonitor_UAVCAN::handle_outputEnable_response(const CanardRxTransfer& transfer, const mppt_OutputEnableResponse& response) +{ + if (transfer.source_node_id != _node_id) { + // this response is not from the node we are looking for + return; + } + + if (response.enabled == _mppt.powered_state) { + // we got back what we expected it to be. We set it on, it now says it on (or vice versa). + // Clear the timer so we don't re-request + _mppt.powered_state_remote_ms = 0; + } +} + +#if AP_BATTMONITOR_UAVCAN_MPPT_DEBUG +// report changes in MPPT faults +void AP_BattMonitor_UAVCAN::mppt_report_faults(const uint8_t instance, const uint8_t fault_flags) +{ + // handle recovery + if (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 & 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) +{ + switch (fault) { + 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"; +} +#endif + +// return mavlink fault bitmask (see MAV_BATTERY_FAULT enum) +uint32_t AP_BattMonitor_UAVCAN::get_mavlink_fault_bitmask() const +{ + // return immediately if not mppt or no faults + if (!_mppt.is_detected || (_mppt.fault_flags == 0)) { + return 0; + } + + // convert mppt fault bitmask to mavlink fault bitmask + uint32_t mav_fault_bitmask = 0; + if ((_mppt.fault_flags & (uint8_t)MPPT_FaultFlags::OVER_VOLTAGE) || (_mppt.fault_flags & (uint8_t)MPPT_FaultFlags::UNDER_VOLTAGE)) { + mav_fault_bitmask |= MAV_BATTERY_FAULT_INCOMPATIBLE_VOLTAGE; + } + if (_mppt.fault_flags & (uint8_t)MPPT_FaultFlags::OVER_CURRENT) { + mav_fault_bitmask |= MAV_BATTERY_FAULT_OVER_CURRENT; + } + if (_mppt.fault_flags & (uint8_t)MPPT_FaultFlags::OVER_TEMPERATURE) { + mav_fault_bitmask |= MAV_BATTERY_FAULT_OVER_TEMPERATURE; + } + return mav_fault_bitmask; +} + +#endif // AP_BATTERY_UAVCAN_BATTERYINFO_ENABLED diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.h b/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.h new file mode 100644 index 0000000000..22e4cfd170 --- /dev/null +++ b/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.h @@ -0,0 +1,115 @@ +#pragma once + +#include "AP_BattMonitor.h" +#include "AP_BattMonitor_Backend.h" +#if HAL_ENABLE_LIBUAVCAN_DRIVERS +#include + +#define AP_BATTMONITOR_UAVCAN_TIMEOUT_MICROS 5000000 // sensor becomes unhealthy if no successful readings for 5 seconds + +#ifndef AP_BATTMONITOR_UAVCAN_MPPT_DEBUG +#define AP_BATTMONITOR_UAVCAN_MPPT_DEBUG 0 +#endif + +class AP_BattMonitor_UAVCAN : public AP_BattMonitor_Backend +{ +public: + enum BattMonitor_UAVCAN_Type { + UAVCAN_BATTERY_INFO = 0 + }; + + /// Constructor + AP_BattMonitor_UAVCAN(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state, BattMonitor_UAVCAN_Type type, AP_BattMonitor_Params ¶ms); + + static const struct AP_Param::GroupInfo var_info[]; + + void init() override {} + + /// Read the battery voltage and current. Should be called at 10hz + void read() override; + + /// capacity_remaining_pct - returns true if the percentage is valid and writes to percentage argument + bool capacity_remaining_pct(uint8_t &percentage) const override; + + bool has_temperature() const override { return _has_temperature; } + + bool has_current() const override { return true; } + + bool has_consumed_energy() const override { return _has_consumed_energy; } + + bool has_time_remaining() const override { return _has_time_remaining; } + + bool has_cell_voltages() const override { return _has_cell_voltages; } + + 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_DroneCAN* ap_dronecan); + static AP_BattMonitor_UAVCAN* get_uavcan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id, uint8_t battery_id); + static void handle_battery_info_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_power_BatteryInfo &msg); + static void handle_battery_info_aux_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const ardupilot_equipment_power_BatteryInfoAux &msg); + static void handle_mppt_stream_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const mppt_Stream &msg); + + void mppt_set_powered_state(bool power_on) override; + +private: + void handle_battery_info(const uavcan_equipment_power_BatteryInfo &msg); + void handle_battery_info_aux(const ardupilot_equipment_power_BatteryInfoAux &msg); + void update_interim_state(const float voltage, const float current, const float temperature_K, const uint8_t soc); + + static bool match_battery_id(uint8_t instance, uint8_t battery_id) { + // when serial number is negative, all batteries are accepted. Else, it must match + return (AP::battery().get_serial_number(instance) < 0) || (AP::battery().get_serial_number(instance) == (int32_t)battery_id); + } + + // 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 mppt_Stream &msg); + void mppt_check_powered_state(); + +#if AP_BATTMONITOR_UAVCAN_MPPT_DEBUG + static void mppt_report_faults(const uint8_t instance, const uint8_t fault_flags); + static const char* mppt_fault_string(const MPPT_FaultFlags fault); +#endif + + AP_BattMonitor::BattMonitor_State _interim_state; + BattMonitor_UAVCAN_Type _type; + + HAL_Semaphore _sem_battmon; + + AP_DroneCAN* _ap_dronecan; + uint8_t _soc; + uint8_t _node_id; + uint16_t _cycle_count; + float _remaining_capacity_wh; + float _full_charge_capacity_wh; + bool _has_temperature; + bool _has_cell_voltages; + bool _has_time_remaining; + bool _has_consumed_energy; + bool _has_battery_info_aux; + uint8_t _instance; // instance of this battery monitor + + AP_Float _curr_mult; // scaling multiplier applied to current reports for adjustment + // MPPT variables + struct { + bool is_detected; // true if this UAVCAN device is a Packet Digital MPPT + bool powered_state; // true if the mppt is powered on, false if powered off + bool vehicle_armed_last; // latest vehicle armed state. used to detect changes and power on/off MPPT board + uint8_t fault_flags; // bits holding fault flags + uint32_t powered_state_remote_ms; // timestamp of when request was sent, zeroed on response. Used to retry + } _mppt; + + void handle_outputEnable_response(const CanardRxTransfer&, const mppt_OutputEnableResponse&); + + Canard::ObjCallback mppt_outputenable_res_cb{this, &AP_BattMonitor_UAVCAN::handle_outputEnable_response}; + Canard::Client *mppt_outputenable_client; +}; +#endif