5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-06 07:58:28 -04:00

AP_BattMonitor: Unify read_word interface

This commit is contained in:
Michael du Breuil 2017-04-19 13:10:15 -07:00 committed by Francisco Ferreira
parent 3319c3ccdc
commit 9c3b97347a
6 changed files with 43 additions and 80 deletions

View File

@ -2,6 +2,34 @@
#define AP_BATTMONITOR_SMBUS_PEC_POLYNOME 0x07 // Polynome for CRC generation
// read word from register
// returns true if read was successful, false if failed
bool AP_BattMonitor_SMBus::read_word(uint8_t reg, uint16_t& data) const
{
// buffer to hold results (1 extra byte returned holding PEC)
const uint8_t read_size = 2 + (_pec_supported ? 1 : 0);
uint8_t buff[read_size]; // buffer to hold results
// read the appropriate register from the device
if (!_dev->read_registers(reg, buff, sizeof(buff))) {
return false;
}
// check PEC
if (_pec_supported) {
const uint8_t pec = get_PEC(AP_BATTMONITOR_SMBUS_I2C_ADDR, reg, true, buff, 2);
if (pec != buff[2]) {
return false;
}
}
// convert buffer to word
data = (uint16_t)buff[1]<<8 | (uint16_t)buff[0];
// return success
return true;
}
/// get_PEC - calculate packet error correction code of buffer
uint8_t AP_BattMonitor_SMBus::get_PEC(const uint8_t i2c_addr, uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len) const
{

View File

@ -4,6 +4,7 @@
#include <AP_Param/AP_Param.h>
#include <AP_Math/AP_Math.h>
#include "AP_BattMonitor_Backend.h"
#include <utility>
#define AP_BATTMONITOR_SMBUS_BUS_INTERNAL 0
#define AP_BATTMONITOR_SMBUS_BUS_EXTERNAL 1
@ -15,8 +16,11 @@ class AP_BattMonitor_SMBus : public AP_BattMonitor_Backend
public:
/// Constructor
AP_BattMonitor_SMBus(AP_BattMonitor &mon, uint8_t instance, AP_BattMonitor::BattMonitor_State &mon_state) :
AP_BattMonitor_Backend(mon, instance, mon_state)
AP_BattMonitor_SMBus(AP_BattMonitor &mon, uint8_t instance,
AP_BattMonitor::BattMonitor_State &mon_state,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev)
: AP_BattMonitor_Backend(mon, instance, mon_state),
_dev(std::move(dev))
{}
// virtual destructor to reduce compiler warnings
@ -25,10 +29,16 @@ public:
protected:
// read word from register
// returns true if read was successful, false if failed
bool read_word(uint8_t reg, uint16_t& data) const;
// get_PEC - calculate PEC for a read or write from the battery
// buff is the data that was read or will be written
uint8_t get_PEC(const uint8_t i2c_addr, uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len) const;
AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev;
bool _pec_supported; // true if PEC is supported
};
// include specific implementations

View File

@ -5,12 +5,6 @@
#include "AP_BattMonitor_SMBus_Maxell.h"
#include <utility>
extern const AP_HAL::HAL& hal;
#include <AP_HAL/AP_HAL.h>
#define BATTMONITOR_SMBUS_MAXELL_TEMP 0x08 // temperature register
#define BATTMONITOR_SMBUS_MAXELL_VOLTAGE 0x09 // voltage register
#define BATTMONITOR_SMBUS_MAXELL_CURRENT 0x0a // current register
#define BATTMONITOR_SMBUS_MAXELL_SPECIFICATION_INFO 0x1a // specification info
@ -45,8 +39,7 @@ uint8_t maxell_cell_ids[] = { 0x3f, // cell 1
AP_BattMonitor_SMBus_Maxell::AP_BattMonitor_SMBus_Maxell(AP_BattMonitor &mon, uint8_t instance,
AP_BattMonitor::BattMonitor_State &mon_state,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev)
: AP_BattMonitor_SMBus(mon, instance, mon_state)
, _dev(std::move(dev))
: AP_BattMonitor_SMBus(mon, instance, mon_state, std::move(dev))
{
_dev->register_periodic_callback(100000, FUNCTOR_BIND_MEMBER(&AP_BattMonitor_SMBus_Maxell::timer, void));
}
@ -102,34 +95,6 @@ void AP_BattMonitor_SMBus_Maxell::timer()
}
}
// 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
{
// buffer to hold results (1 extra byte returned holding PEC)
const uint8_t read_size = 2 + (_pec_supported ? 1 : 0);
uint8_t buff[read_size]; // 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;
}
// check PEC
if (_pec_supported) {
const uint8_t pec = get_PEC(AP_BATTMONITOR_SMBUS_I2C_ADDR, reg, true, buff, 2);
if (pec != buff[2]) {
return false;
}
}
// convert buffer to word
data = (uint16_t)buff[1]<<8 | (uint16_t)buff[0];
// return success
return true;
}
// read_block - returns number of characters read if successful, zero if unsuccessful
uint8_t AP_BattMonitor_SMBus_Maxell::read_block(uint8_t reg, uint8_t* data, bool append_zero) const
{

View File

@ -22,10 +22,6 @@ 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;
// check if PEC supported with the version value in SpecificationInfo() function
// returns true once PEC is confirmed as working or not working
bool check_pec_support();
@ -33,7 +29,5 @@ private:
// read_block - returns number of characters read if successful, zero if unsuccessful
uint8_t read_block(uint8_t reg, uint8_t* data, bool append_zero) const;
AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev;
uint8_t _pec_confirmed; // count of the number of times PEC has been confirmed as working
bool _pec_supported; // true if pec is supported
};

View File

@ -6,11 +6,6 @@
#include "AP_BattMonitor_SMBus_Solo.h"
#include <utility>
extern const AP_HAL::HAL& hal;
#include <AP_HAL/AP_HAL.h>
#define BATTMONITOR_SMBUS_SOLO_TEMP 0x08 // temperature register
#define BATTMONITOR_SMBUS_SOLO_REMAINING_CAPACITY 0x0f // predicted remaining battery capacity in milliamps
#define BATTMONITOR_SMBUS_SOLO_FULL_CHARGE_CAPACITY 0x10 // full capacity register
#define BATTMONITOR_SMBUS_SOLO_MANUFACTURE_DATA 0x23 /// manufacturer data
@ -37,9 +32,9 @@ extern const AP_HAL::HAL& hal;
AP_BattMonitor_SMBus_Solo::AP_BattMonitor_SMBus_Solo(AP_BattMonitor &mon, uint8_t instance,
AP_BattMonitor::BattMonitor_State &mon_state,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev)
: AP_BattMonitor_SMBus(mon, instance, mon_state)
, _dev(std::move(dev))
: AP_BattMonitor_SMBus(mon, instance, mon_state, std::move(dev))
{
_pec_supported = true;
_dev->register_periodic_callback(100000, FUNCTOR_BIND_MEMBER(&AP_BattMonitor_SMBus_Solo::timer, void));
}
@ -129,30 +124,6 @@ void AP_BattMonitor_SMBus_Solo::timer()
}
}
// read word from register
// returns true if read was successful, false if failed
bool AP_BattMonitor_SMBus_Solo::read_word(uint8_t reg, uint16_t& data) const
{
uint8_t buff[3]; // 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;
}
// check PEC
uint8_t pec = get_PEC(AP_BATTMONITOR_SMBUS_I2C_ADDR, reg, true, buff, 2);
if (pec != buff[2]) {
return false;
}
// convert buffer to word
data = (uint16_t)buff[1]<<8 | (uint16_t)buff[0];
// return success
return true;
}
// read_block - returns number of characters read if successful, zero if unsuccessful
uint8_t AP_BattMonitor_SMBus_Solo::read_block(uint8_t reg, uint8_t* data, uint8_t max_len, bool append_zero) const
{

View File

@ -22,13 +22,8 @@ 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;
// read_block - returns number of characters read if successful, zero if unsuccessful
uint8_t read_block(uint8_t reg, uint8_t* data, uint8_t max_len, bool append_zero) const;
AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev;
uint8_t _button_press_count;
};