From 45e8cdea701318df71b9bd7c3d481db53815b806 Mon Sep 17 00:00:00 2001 From: Hwurzburg Date: Wed, 12 Jan 2022 10:58:45 -0600 Subject: [PATCH] AP_BatteryMonitor: add current scaler for UAVCAN --- .../AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp | 16 +++++++++++++++- libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.h | 5 +++-- 2 files changed, 18 insertions(+), 3 deletions(-) diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp index 87530c8a6e..8dff529119 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp @@ -21,6 +21,17 @@ extern const AP_HAL::HAL& hal; +const AP_Param::GroupInfo AP_BattMonitor_UAVCAN::var_info[] = { + + // @Param: CUR_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("CUR_MULT", 27, AP_BattMonitor_UAVCAN, _cur_mult, 1.0), + AP_GROUPEND +}; + UC_REGISTRY_BINDER(BattInfoCb, uavcan::equipment::power::BatteryInfo); UC_REGISTRY_BINDER(BattInfoAuxCb, ardupilot::equipment::power::BatteryInfoAux); UC_REGISTRY_BINDER(MpptStreamCb, mppt::Stream); @@ -30,6 +41,9 @@ AP_BattMonitor_UAVCAN::AP_BattMonitor_UAVCAN(AP_BattMonitor &mon, AP_BattMonitor 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; } @@ -125,7 +139,7 @@ void AP_BattMonitor_UAVCAN::update_interim_state(const float voltage, const floa WITH_SEMAPHORE(_sem_battmon); _interim_state.voltage = voltage; - _interim_state.current_amps = current; + _interim_state.current_amps = _cur_mult * current; _soc = soc; if (!isnanf(temperature_K) && temperature_K > 0) { diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.h b/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.h index 272ba1857b..ddc89d5f35 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.h @@ -22,6 +22,8 @@ public: /// 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 @@ -54,7 +56,6 @@ public: private: void handle_battery_info(const BattInfoCb &cb); void handle_battery_info_aux(const BattInfoAuxCb &cb); - 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) { @@ -94,7 +95,7 @@ private: bool _has_battery_info_aux; uint8_t _instance; // instance of this battery monitor uavcan::Node<0> *_node; // UAVCAN node id - + AP_Float _cur_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