From 3e445fd4b8cc1c1bb704bf0f28c0174be3d63194 Mon Sep 17 00:00:00 2001 From: Tatsuya Yamaguchi Date: Thu, 9 Feb 2017 09:28:57 +0900 Subject: [PATCH] AP_BattMonitor: add Maxell Battery support --- libraries/AP_BattMonitor/AP_BattMonitor.cpp | 12 ++- libraries/AP_BattMonitor/AP_BattMonitor.h | 5 +- .../AP_BattMonitor/AP_BattMonitor_SMBus.h | 1 + .../AP_BattMonitor_SMBus_Maxell.cpp | 96 +++++++++++++++++++ .../AP_BattMonitor_SMBus_Maxell.h | 30 ++++++ 5 files changed, 139 insertions(+), 5 deletions(-) create mode 100644 libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Maxell.cpp create mode 100644 libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Maxell.h diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.cpp b/libraries/AP_BattMonitor/AP_BattMonitor.cpp index b0baf49c01..4e229720ac 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor.cpp @@ -74,7 +74,7 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = { // @Param: 2_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:SMBus,6:Bebop + // @Values: 0:Disabled,3:Analog Voltage Only,4:Analog Voltage and Current,5:Solo,6:Bebop,7:Maxell // @User: Standard AP_GROUPINFO("2_MONITOR", 11, AP_BattMonitor, _monitoring[1], BattMonitor_TYPE_NONE), @@ -180,6 +180,10 @@ AP_BattMonitor::init() hal.i2c_mgr->get_device(AP_BATTMONITOR_SMBUS_BUS_INTERNAL, AP_BATTMONITOR_SMBUS_I2C_ADDR)); _num_instances++; break; + case BattMonitor_TYPE_MAXELL: + state[instance].instance = instance; + drivers[instance] = new AP_BattMonitor_SMBus_Maxell(*this, instance, state[instance], + hal.i2c_mgr->get_device(AP_BATTMONITOR_SMBUS_BUS_EXTERNAL, AP_BATTMONITOR_SMBUS_I2C_ADDR)); _num_instances++; break; case BattMonitor_TYPE_BEBOP: @@ -224,8 +228,9 @@ bool AP_BattMonitor::has_current(uint8_t instance) const // check for analog voltage and current monitor or smbus monitor if (instance < _num_instances && drivers[instance] != nullptr) { return (_monitoring[instance] == BattMonitor_TYPE_ANALOG_VOLTAGE_AND_CURRENT || - _monitoring[instance] == BattMonitor_TYPE_BEBOP); _monitoring[instance] == BattMonitor_TYPE_SOLO || + _monitoring[instance] == BattMonitor_TYPE_BEBOP || + _monitoring[instance] == BattMonitor_TYPE_MAXELL); } // not monitoring current @@ -279,7 +284,7 @@ uint8_t AP_BattMonitor::capacity_remaining_pct(uint8_t instance) const return 0; } } - + /// exhausted - returns true if the voltage remains below the low_voltage for 10 seconds or remaining capacity falls below min_capacity_mah bool AP_BattMonitor::exhausted(uint8_t instance, float low_voltage, float min_capacity_mah) { @@ -332,4 +337,3 @@ bool AP_BattMonitor::overpower_detected(uint8_t instance) const return false; #endif } - diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.h b/libraries/AP_BattMonitor/AP_BattMonitor.h index 07b9e1ab8b..8baf656823 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor.h @@ -20,6 +20,7 @@ class AP_BattMonitor_Analog; class AP_BattMonitor_SMBus; class AP_BattMonitor_SMBus_Solo; class AP_BattMonitor_SMBus_PX4; +class AP_BattMonitor_SMBus_Maxell; class AP_BattMonitor { @@ -28,6 +29,7 @@ class AP_BattMonitor friend class AP_BattMonitor_SMBus; friend class AP_BattMonitor_SMBus_Solo; friend class AP_BattMonitor_SMBus_PX4; + friend class AP_BattMonitor_SMBus_Maxell; public: @@ -39,8 +41,9 @@ public: BattMonitor_TYPE_NONE = 0, BattMonitor_TYPE_ANALOG_VOLTAGE_ONLY = 3, BattMonitor_TYPE_ANALOG_VOLTAGE_AND_CURRENT = 4, - BattMonitor_TYPE_BEBOP = 6 BattMonitor_TYPE_SOLO = 5, + BattMonitor_TYPE_BEBOP = 6, + BattMonitor_TYPE_MAXELL = 7 }; // The BattMonitor_State structure is filled in by the backend driver diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.h b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.h index 349ba1c8b3..16cdc0b592 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.h @@ -27,3 +27,4 @@ public: // include specific implementations #include "AP_BattMonitor_SMBus_PX4.h" #include "AP_BattMonitor_SMBus_Solo.h" +#include "AP_BattMonitor_SMBus_Maxell.h" diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Maxell.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Maxell.cpp new file mode 100644 index 0000000000..285c3400be --- /dev/null +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Maxell.cpp @@ -0,0 +1,96 @@ +#include +#include +#include +#include "AP_BattMonitor.h" +#include "AP_BattMonitor_SMBus_Maxell.h" +#include + +extern const AP_HAL::HAL& hal; + +#include + + +#define BATTMONITOR_SMBUS_MAXELL_VOLTAGE 0x09 // voltage register +#define BATTMONITOR_SMBUS_MAXELL_CURRENT 0x0a // current register + +/* + * Other potentially useful registers, listed here for future use + * #define BATTMONITOR_SMBUS_MAXELL_TEMP 0x08 // temperature register + * #define BATTMONITOR_SMBUS_MAXELL_CHARGE_STATUS 0x0d // relative state of charge + * #define BATTMONITOR_SMBUS_MAXELL_BATTERY_STATUS 0x16 // battery status register including alarms + * #define BATTMONITOR_SMBUS_MAXELL_BATTERY_CYCLE_COUNT 0x17 // cycle count + * #define BATTMONITOR_SMBUS_MAXELL_DESIGN_VOLTAGE 0x19 // design voltage register + * #define BATTMONITOR_SMBUS_MAXELL_SPECIFICATION_INFO 0x1a // specification info + * #define BATTMONITOR_SMBUS_MAXELL_MANUFACTURE_NAME 0x1b // manufacturer name + * #define BATTMONITOR_SMBUS_MAXELL_SERIALNUM 0x1c // serial number register + * #define BATTMONITOR_SMBUS_MAXELL_CELL_VOLTAGE6 0x3a // cell voltage register + * #define BATTMONITOR_SMBUS_MAXELL_CELL_VOLTAGE5 0x3b // cell voltage register + * #define BATTMONITOR_SMBUS_MAXELL_CELL_VOLTAGE4 0x3c // cell voltage register + * #define BATTMONITOR_SMBUS_MAXELL_CELL_VOLTAGE3 0x3d // cell voltage register + * #define BATTMONITOR_SMBUS_MAXELL_CELL_VOLTAGE2 0x3e // cell voltage register + * #define BATTMONITOR_SMBUS_MAXELL_CELL_VOLTAGE1 0x3f // cell voltage register + * #define BATTMONITOR_SMBUS_MAXELL_HEALTH_STATUS 0x4f // state of health + * #define BATTMONITOR_SMBUS_MAXELL_SAFETY_ALERT 0x50 // safety alert + * #define BATTMONITOR_SMBUS_MAXELL_SAFETY_STATUS 0x50 // safety status + * #define BATTMONITOR_SMBUS_MAXELL_PF_ALERT 0x52 // safety status + * #define BATTMONITOR_SMBUS_MAXELL_PF_STATUS 0x53 // safety status +*/ + +// Constructor +AP_BattMonitor_SMBus_Maxell::AP_BattMonitor_SMBus_Maxell(AP_BattMonitor &mon, uint8_t instance, + AP_BattMonitor::BattMonitor_State &mon_state, + AP_HAL::OwnPtr dev) + : AP_BattMonitor_SMBus(mon, instance, mon_state) + , _dev(std::move(dev)) +{ + _dev->register_periodic_callback(100000, FUNCTOR_BIND_MEMBER(&AP_BattMonitor_SMBus_Maxell::timer, void)); +} + +/// Read the battery voltage and current. Should be called at 10hz +void AP_BattMonitor_SMBus_Maxell::read() +{ + // nothing to do - all done in timer() +} + +void AP_BattMonitor_SMBus_Maxell::timer() +{ + uint16_t data; + uint32_t tnow = AP_HAL::micros(); + + // read voltage (V) + if (read_word(BATTMONITOR_SMBUS_MAXELL_VOLTAGE, data)) { + _state.voltage = (float)data / 1000.0f; + _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; + return; + } + + // read current (A) + if (read_word(BATTMONITOR_SMBUS_MAXELL_CURRENT, data)) { + _state.current_amps = -(float)((int16_t)data) / 1000.0f; + _state.last_time_micros = tnow; + } +} + +// read word from register +// returns true if read was successful, false if failed +bool AP_BattMonitor_SMBus_Maxell::read_word(uint8_t reg, uint16_t& data) const +{ + uint8_t buff[2]; // buffer to hold results + + // read three bytes and place in last three bytes of buffer + if (!_dev->read_registers(reg, buff, sizeof(buff))) { + return false; + } + + // convert buffer to word + data = (uint16_t)buff[1]<<8 | (uint16_t)buff[0]; + + // return success + return true; +} diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Maxell.h b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Maxell.h new file mode 100644 index 0000000000..9b0f39a3c9 --- /dev/null +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Maxell.h @@ -0,0 +1,30 @@ +#pragma once + +#include +#include +#include +#include "AP_BattMonitor_SMBus.h" +#include + +class AP_BattMonitor_SMBus_Maxell : public AP_BattMonitor_SMBus +{ +public: + + // Constructor + AP_BattMonitor_SMBus_Maxell(AP_BattMonitor &mon, uint8_t instance, + AP_BattMonitor::BattMonitor_State &mon_state, + AP_HAL::OwnPtr dev); + + // read does nothing, all done in timer + void read() override; + +private: + + void timer(void); + + // read word from register + // returns true if read was successful, false if failed + bool read_word(uint8_t reg, uint16_t& data) const; + + AP_HAL::OwnPtr _dev; +};