From 080e477b367765a12957334a2d444443c12abb26 Mon Sep 17 00:00:00 2001 From: Kelly Schrock Date: Thu, 16 Jan 2020 12:54:18 -0600 Subject: [PATCH] AP_BattMonitor: NeoDesign battery driver Adds a driver for the NeoDesign BMS, with variable cell count. --- libraries/AP_BattMonitor/AP_BattMonitor.cpp | 7 ++ .../AP_BattMonitor/AP_BattMonitor_Params.cpp | 2 +- .../AP_BattMonitor/AP_BattMonitor_Params.h | 1 + .../AP_BattMonitor_SMBus_NeoDesign.cpp | 75 +++++++++++++++++++ .../AP_BattMonitor_SMBus_NeoDesign.h | 18 +++++ 5 files changed, 102 insertions(+), 1 deletion(-) create mode 100644 libraries/AP_BattMonitor/AP_BattMonitor_SMBus_NeoDesign.cpp create mode 100644 libraries/AP_BattMonitor/AP_BattMonitor_SMBus_NeoDesign.h diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.cpp b/libraries/AP_BattMonitor/AP_BattMonitor.cpp index eb086a755f..b79233162f 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor.cpp @@ -4,6 +4,7 @@ #include "AP_BattMonitor_Bebop.h" #include "AP_BattMonitor_BLHeliESC.h" #include "AP_BattMonitor_SMBus_SUI.h" +#include "AP_BattMonitor_SMBus_NeoDesign.h" #include "AP_BattMonitor_Sum.h" #include "AP_BattMonitor_FuelFlow.h" #include "AP_BattMonitor_FuelLevel_PWM.h" @@ -165,6 +166,12 @@ AP_BattMonitor::init() drivers[instance] = new AP_BattMonitor_FuelLevel_PWM(*this, state[instance], _params[instance]); break; #endif // HAL_BATTMON_FUEL_ENABLE + case AP_BattMonitor_Params::BattMonitor_TYPE_NeoDesign: + _params[instance]._i2c_bus.set_default(AP_BATTMONITOR_SMBUS_BUS_INTERNAL), + drivers[instance] = new AP_BattMonitor_SMBus_NeoDesign(*this, state[instance], _params[instance], + hal.i2c_mgr->get_device(_params[instance]._i2c_bus, AP_BATTMONITOR_SMBUS_I2C_ADDR, + 100000, true, 20)); + break; case AP_BattMonitor_Params::BattMonitor_TYPE_NONE: default: break; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp index e1ec939fa2..d5e51fe899 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp @@ -13,7 +13,7 @@ const AP_Param::GroupInfo AP_BattMonitor_Params::var_info[] = { // @Param: MONITOR // @DisplayName: Battery monitoring // @Description: Controls enabling monitoring of the battery's voltage and current - // @Values: 0:Disabled,3:Analog Voltage Only,4:Analog Voltage and Current,5:Solo,6:Bebop,7:SMBus-Maxell,8:UAVCAN-BatteryInfo,9:BLHeli ESC,10:SumOfFollowing,11:FuelFlow,12:FuelLevelPWM,13:SMBUS-SUI3,14:SMBUS-SUI6 + // @Values: 0:Disabled,3:Analog Voltage Only,4:Analog Voltage and Current,5:Solo,6:Bebop,7:SMBus-Maxell,8:UAVCAN-BatteryInfo,9:BLHeli ESC,10:SumOfFollowing,11:FuelFlow,12:FuelLevelPWM,13:SMBUS-SUI3,14:SMBUS-SUI6,15:NeoDesign // @User: Standard // @RebootRequired: True AP_GROUPINFO_FLAGS("MONITOR", 1, AP_BattMonitor_Params, _type, BattMonitor_TYPE_NONE, AP_PARAM_FLAG_ENABLE), diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Params.h b/libraries/AP_BattMonitor/AP_BattMonitor_Params.h index 2b052f6d6b..c514a16d42 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Params.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Params.h @@ -27,6 +27,7 @@ public: BattMonitor_TYPE_FuelLevel_PWM = 12, BattMonitor_TYPE_SUI3 = 13, BattMonitor_TYPE_SUI6 = 14, + BattMonitor_TYPE_NeoDesign = 15, }; // low voltage sources (used for BATT_LOW_TYPE parameter) diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_NeoDesign.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_NeoDesign.cpp new file mode 100644 index 0000000000..d3877f72bd --- /dev/null +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_NeoDesign.cpp @@ -0,0 +1,75 @@ +#include +#include +#include "AP_BattMonitor.h" +#include "AP_BattMonitor_SMBus_NeoDesign.h" + +#define BATTMONITOR_ND_CELL_COUNT 0x5C // cell-count register +#define BATTMONITOR_ND_CELL_START 0x30 // first cell register + +// Constructor +AP_BattMonitor_SMBus_NeoDesign::AP_BattMonitor_SMBus_NeoDesign(AP_BattMonitor &mon, + AP_BattMonitor::BattMonitor_State &mon_state, + AP_BattMonitor_Params ¶ms, + AP_HAL::OwnPtr dev) + : AP_BattMonitor_SMBus(mon, mon_state, params, std::move(dev)) +{ + _pec_supported = true; +} + +void AP_BattMonitor_SMBus_NeoDesign::timer() +{ + uint16_t data; + uint32_t tnow = AP_HAL::micros(); + + // Get the cell count once, it's not likely to change in flight + if (_cell_count == 0) { + if (read_word(BATTMONITOR_ND_CELL_COUNT, data)) { + _cell_count = data; + } else { + return; // something wrong, don't try anything else + } + } + + bool read_all_cells = true; + for(uint8_t i = 0; i < _cell_count; ++i) { + if(read_word(BATTMONITOR_ND_CELL_START + i, data)) { + _state.cell_voltages.cells[i] = data; + _has_cell_voltages = true; + } else { + read_all_cells = false; + } + } + + if (read_all_cells && (_cell_count > 0)) { + uint32_t summed = 0; + for (int i = 0; i < _cell_count; i++) { + summed += _state.cell_voltages.cells[i]; + } + _state.voltage = (float)summed * 1e-3f; + _state.last_time_micros = tnow; + _state.healthy = true; + } else if (read_word(BATTMONITOR_SMBUS_VOLTAGE, data)) { + // fallback to the voltage register if we didn't manage to poll the cells + _state.voltage = (float)data * 1e-3f; + _state.last_time_micros = tnow; + _state.healthy = true; + } + + // timeout after 5 seconds + if ((tnow - _state.last_time_micros) > AP_BATTMONITOR_SMBUS_TIMEOUT_MICROS) { + _state.healthy = false; + // do not attempt to read any more data from battery + return; + } + + // read current (A) + if (read_word(BATTMONITOR_SMBUS_CURRENT, data)) { + _state.current_amps = -(float)((int16_t)data) * 1e-3f; + _state.last_time_micros = tnow; + } + + read_full_charge_capacity(); + read_remaining_capacity(); + read_temp(); +} + diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_NeoDesign.h b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_NeoDesign.h new file mode 100644 index 0000000000..0c8b56cb7b --- /dev/null +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_NeoDesign.h @@ -0,0 +1,18 @@ +#pragma once + +#include "AP_BattMonitor_SMBus.h" + +class AP_BattMonitor_SMBus_NeoDesign : public AP_BattMonitor_SMBus +{ +public: + AP_BattMonitor_SMBus_NeoDesign(AP_BattMonitor &mon, + AP_BattMonitor::BattMonitor_State &mon_state, + AP_BattMonitor_Params ¶ms, + AP_HAL::OwnPtr dev); + +private: + + void timer(void) override; + + uint8_t _cell_count; +};