Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_BattMonitor: added SUI SMBUS battery backend 

originally by Kelly Schrock
This commit is contained in:
Andrew Tridgell 2019-12-04 23:03:33 +11:00 committed by Randy Mackay
parent 6f77a75be5
commit 9c16e30178
7 changed files with 218 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AP_BattMonitor/AP_BattMonitor.cpp
View File

@ -3,6 +3,7 @@
#include "AP_BattMonitor_SMBus.h"
#include "AP_BattMonitor_Bebop.h"
#include "AP_BattMonitor_BLHeliESC.h"
#include "AP_BattMonitor_SMBus_SUI.h"
#include "AP_BattMonitor_Sum.h"
#include "AP_BattMonitor_FuelFlow.h"
#include "AP_BattMonitor_FuelLevel_PWM.h"
@ -119,6 +120,18 @@ AP_BattMonitor::init()
hal.i2c_mgr->get_device(_params[instance]._i2c_bus, AP_BATTMONITOR_SMBUS_I2C_ADDR,
100000, true, 20));
break;
case AP_BattMonitor_Params::BattMonitor_TYPE_SUI3:
_params[instance]._i2c_bus.set_default(AP_BATTMONITOR_SMBUS_BUS_INTERNAL),
drivers[instance] = new AP_BattMonitor_SMBus_SUI(*this, state[instance], _params[instance],
hal.i2c_mgr->get_device(_params[instance]._i2c_bus, AP_BATTMONITOR_SMBUS_I2C_ADDR,
100000, true, 20), 3);
break;
case AP_BattMonitor_Params::BattMonitor_TYPE_SUI6:
_params[instance]._i2c_bus.set_default(AP_BATTMONITOR_SMBUS_BUS_INTERNAL),
drivers[instance] = new AP_BattMonitor_SMBus_SUI(*this, state[instance], _params[instance],
hal.i2c_mgr->get_device(_params[instance]._i2c_bus, AP_BATTMONITOR_SMBUS_I2C_ADDR,
100000, true, 20), 6);
break;
case AP_BattMonitor_Params::BattMonitor_TYPE_MAXELL:
_params[instance]._i2c_bus.set_default(AP_BATTMONITOR_SMBUS_BUS_EXTERNAL);
drivers[instance] = new AP_BattMonitor_SMBus_Maxell(*this, state[instance], _params[instance],

2
libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp
View File

@ -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
// @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
// @User: Standard
// @RebootRequired: True
AP_GROUPINFO_FLAGS("MONITOR", 1, AP_BattMonitor_Params, _type, BattMonitor_TYPE_NONE, AP_PARAM_FLAG_ENABLE),

2
libraries/AP_BattMonitor/AP_BattMonitor_Params.h
View File

@ -25,6 +25,8 @@ public:
BattMonitor_TYPE_Sum = 10,
BattMonitor_TYPE_FuelFlow = 11,
BattMonitor_TYPE_FuelLevel_PWM = 12,
BattMonitor_TYPE_SUI3 = 13,
BattMonitor_TYPE_SUI6 = 14,
};
// low voltage sources (used for BATT_LOW_TYPE parameter)

2
libraries/AP_BattMonitor/AP_BattMonitor_SMBus.cpp
View File

@ -16,7 +16,7 @@ AP_BattMonitor_SMBus::AP_BattMonitor_SMBus(AP_BattMonitor &mon,
void AP_BattMonitor_SMBus::init(void)
{
if (_dev) {
_dev->register_periodic_callback(100000, FUNCTOR_BIND_MEMBER(&AP_BattMonitor_SMBus::timer, void));
timer_handle = _dev->register_periodic_callback(100000, FUNCTOR_BIND_MEMBER(&AP_BattMonitor_SMBus::timer, void));
}
}

3
libraries/AP_BattMonitor/AP_BattMonitor_SMBus.h
View File

@ -49,7 +49,7 @@ public:
// return true if cycle count can be provided and fills in cycles argument
bool get_cycle_count(uint16_t &cycles) const override;
void init(void) override;
virtual void init(void) override;
protected:
@ -94,6 +94,7 @@ protected:
virtual void timer(void) = 0; // timer function to read from the battery
AP_HAL::Device::PeriodicHandle timer_handle;
};
// include specific implementations

164
libraries/AP_BattMonitor/AP_BattMonitor_SMBus_SUI.cpp Normal file
View File

@ -0,0 +1,164 @@
#include <AP_HAL/AP_HAL.h>
#include <AP_Common/AP_Common.h>
#include <AP_Math/AP_Math.h>
#include <AP_Notify/AP_Notify.h>
#include "AP_BattMonitor.h"
#include "AP_BattMonitor_SMBus_SUI.h"
extern const AP_HAL::HAL& hal;
#define REG_CELL_VOLTAGE 0x28
#define REG_CURRENT 0x2a
// maximum number of cells that we can read data for
#define SUI_MAX_CELL_READ 4
// Constructor
AP_BattMonitor_SMBus_SUI::AP_BattMonitor_SMBus_SUI(AP_BattMonitor &mon,
AP_BattMonitor::BattMonitor_State &mon_state,
AP_BattMonitor_Params &params,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
uint8_t _cell_count)
: AP_BattMonitor_SMBus(mon, mon_state, params, std::move(dev)),
cell_count(_cell_count)
{
_pec_supported = false;
_dev->set_retries(2);
}
void AP_BattMonitor_SMBus_SUI::init(void)
{
AP_BattMonitor_SMBus::init();
if (_dev && timer_handle) {
// run twice as fast for two phases
_dev->adjust_periodic_callback(timer_handle, 50000);
}
}
void AP_BattMonitor_SMBus_SUI::timer()
{
uint32_t tnow = AP_HAL::micros();
// we read in two phases as the device can stall if you read
// current too rapidly after voltages
phase_voltages = !phase_voltages;
if (phase_voltages) {
read_cell_voltages();
update_health();
return;
}
// read current
int32_t current_ma;
if (read_block_bare(REG_CURRENT, (uint8_t *)&current_ma, sizeof(current_ma))) {
_state.current_amps = current_ma * -0.001;
_state.last_time_micros = tnow;
}
read_full_charge_capacity();
read_temp();
read_serial_number();
read_remaining_capacity();
update_health();
}
// read_block - returns true if successful
bool AP_BattMonitor_SMBus_SUI::read_block(uint8_t reg, uint8_t* data, uint8_t len) const
{
// buffer to hold results (2 extra byte returned holding length and PEC)
uint8_t buff[len+2];
// read bytes
if (!_dev->read_registers(reg, buff, sizeof(buff))) {
return false;
}
// get length
uint8_t bufflen = buff[0];
// sanity check length returned by smbus
if (bufflen == 0 || bufflen > len) {
return false;
}
// check PEC
uint8_t pec = get_PEC(AP_BATTMONITOR_SMBUS_I2C_ADDR, reg, true, buff, bufflen+1);
if (pec != buff[bufflen+1]) {
return false;
}
// copy data (excluding PEC)
memcpy(data, &buff[1], bufflen);
// return success
return true;
}
// read_bare_block - returns true if successful
bool AP_BattMonitor_SMBus_SUI::read_block_bare(uint8_t reg, uint8_t* data, uint8_t len) const
{
// read bytes
if (!_dev->read_registers(reg, data, len)) {
return false;
}
// return success
return true;
}
void AP_BattMonitor_SMBus_SUI::read_cell_voltages()
{
// read cell voltages
uint16_t voltbuff[SUI_MAX_CELL_READ];
if (!read_block(REG_CELL_VOLTAGE, (uint8_t *)voltbuff, sizeof(voltbuff))) {
return;
}
float pack_voltage_mv = 0.0f;
for (uint8_t i = 0; i < MIN(SUI_MAX_CELL_READ, cell_count); i++) {
const uint16_t cell = voltbuff[i];
_state.cell_voltages.cells[i] = cell;
pack_voltage_mv += (float)cell;
}
if (cell_count >= SUI_MAX_CELL_READ) {
// we can't read voltage of all cells. get overall pack voltage to work out
// an average for remaining cells
uint16_t total_mv;
if (read_block(BATTMONITOR_SMBUS_VOLTAGE, (uint8_t *)&total_mv, sizeof(total_mv))) {
// if total voltage is below pack_voltage_mv then we will
// read zero volts for the extra cells.
total_mv = MAX(total_mv, pack_voltage_mv);
const uint16_t cell_mv = (total_mv - pack_voltage_mv) / (cell_count - SUI_MAX_CELL_READ);
for (uint8_t i = SUI_MAX_CELL_READ; i < cell_count; i++) {
_state.cell_voltages.cells[i] = cell_mv;
}
pack_voltage_mv = total_mv;
} else {
// we can't get total pack voltage. Use average of cells we have so far
for (uint8_t i = SUI_MAX_CELL_READ; i < cell_count; i++) {
_state.cell_voltages.cells[i] = pack_voltage_mv / SUI_MAX_CELL_READ;
pack_voltage_mv += _state.cell_voltages.cells[i];
}
}
}
_has_cell_voltages = true;
// accumulate the pack voltage out of the total of the cells
_state.voltage = pack_voltage_mv * 0.001;
last_volt_read_us = AP_HAL::micros();
}
/*
update healthy flag
*/
void AP_BattMonitor_SMBus_SUI::update_health()
{
uint32_t now = AP_HAL::micros();
_state.healthy = (now - last_volt_read_us < AP_BATTMONITOR_SMBUS_TIMEOUT_MICROS) &&
(now - _state.last_time_micros < AP_BATTMONITOR_SMBUS_TIMEOUT_MICROS);
}

35
libraries/AP_BattMonitor/AP_BattMonitor_SMBus_SUI.h Normal file
View File

@ -0,0 +1,35 @@
#pragma once
#include <AP_Common/AP_Common.h>
#include <AP_Math/AP_Math.h>
#include "AP_BattMonitor_SMBus.h"
#include <AP_HAL/I2CDevice.h>
// Base SUI class
class AP_BattMonitor_SMBus_SUI : public AP_BattMonitor_SMBus
{
public:
// Constructor
AP_BattMonitor_SMBus_SUI(AP_BattMonitor &mon,
AP_BattMonitor::BattMonitor_State &mon_state,
AP_BattMonitor_Params &params,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
uint8_t cell_count
);
void init(void) override;
private:
void timer(void) override;
void read_cell_voltages();
void update_health();
// read_block - returns number of characters read if successful, zero if unsuccessful
bool read_block(uint8_t reg, uint8_t* data, uint8_t len) const;
bool read_block_bare(uint8_t reg, uint8_t* data, uint8_t len) const;
const uint8_t cell_count;
bool phase_voltages;
uint32_t last_volt_read_us;
};