dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge pull request #1934 from rmackay9/smbus-batt-currdis 

batt_smbus: calculate current discharged
This commit is contained in:
Lorenz Meier 2015-03-19 23:52:20 +01:00
parent 662ec3f62f 4e83f43ea3
commit bb525d68a1
1 changed files with 25 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
src/drivers/batt_smbus/batt_smbus.cpp
View File

@ -84,6 +84,7 @@
#define BATT_SMBUS_ADDR 0x0B ///< I2C address
#define BATT_SMBUS_TEMP 0x08 ///< temperature register
#define BATT_SMBUS_VOLTAGE 0x09 ///< voltage register
#define BATT_SMBUS_REMAINING_CAPACITY 0x0f ///< predicted remaining battery capacity as a percentage
#define BATT_SMBUS_DESIGN_CAPACITY 0x18 ///< design capacity register
#define BATT_SMBUS_DESIGN_VOLTAGE 0x19 ///< design voltage register
#define BATT_SMBUS_SERIALNUM 0x1c ///< serial number register
@ -179,6 +180,7 @@ private:
orb_advert_t _batt_topic; ///< uORB battery topic
orb_id_t _batt_orb_id; ///< uORB battery topic ID
uint64_t _start_time; ///< system time we first attempt to communicate with battery
uint16_t _batt_design_capacity; ///< battery's design capacity in mAh (0 means unknown)
};
namespace
@ -197,7 +199,8 @@ BATT_SMBUS::BATT_SMBUS(int bus, uint16_t batt_smbus_addr) :
_reports(nullptr),
_batt_topic(-1),
_batt_orb_id(nullptr),
_start_time(0)
_start_time(0),
_batt_design_capacity(0)
{
// work_cancel in the dtor will explode if we don't do this...
memset(&_work, 0, sizeof(_work));
@ -263,7 +266,7 @@ BATT_SMBUS::test()
if (updated) {
if (orb_copy(ORB_ID(battery_status), sub, &status) == OK) {
warnx("V=%4.2f C=%4.2f", status.voltage_v, status.current_a);
warnx("V=%4.2f C=%4.2f DismAh=%4.2f", (float)status.voltage_v, (float)status.current_a, (float)status.discharged_mah);
}
}
@ -279,6 +282,7 @@ BATT_SMBUS::search()
{
bool found_slave = false;
uint16_t tmp;
int16_t orig_addr = get_address();
// search through all valid SMBus addresses
for (uint8_t i = BATT_SMBUS_ADDR_MIN; i <= BATT_SMBUS_ADDR_MAX; i++) {
@ -293,6 +297,9 @@ BATT_SMBUS::search()
usleep(1);
}
// restore original i2c address
set_address(orig_addr);
// display completion message
if (found_slave) {
warnx("Done.");
@ -364,13 +371,28 @@ BATT_SMBUS::cycle()
new_report.voltage_v = ((float)tmp) / 1000.0f;
// read current
usleep(1);
uint8_t buff[4];
if (read_block(BATT_SMBUS_CURRENT, buff, 4, false) == 4) {
new_report.current_a = -(float)((int32_t)((uint32_t)buff[3] << 24 | (uint32_t)buff[2] << 16 | (uint32_t)buff[1] << 8 | (uint32_t)buff[0])) / 1000.0f;
}
// read battery design capacity
if (_batt_design_capacity == 0) {
if (read_reg(BATT_SMBUS_DESIGN_CAPACITY, tmp) == OK) {
_batt_design_capacity = tmp;
}
}
// read remaining capacity
if (_batt_design_capacity > 0) {
if (read_reg(BATT_SMBUS_REMAINING_CAPACITY, tmp) == OK) {
if (tmp < _batt_design_capacity) {
new_report.discharged_mah = _batt_design_capacity - tmp;
}
}
}
// publish to orb
if (_batt_topic != -1) {
orb_publish(_batt_orb_id, _batt_topic, &new_report);
@ -430,8 +452,6 @@ BATT_SMBUS::read_block(uint8_t reg, uint8_t *data, uint8_t max_len, bool append_
{
uint8_t buff[max_len + 2]; // buffer to hold results
usleep(1);
// read bytes including PEC
int ret = transfer(&reg, 1, buff, max_len + 2);