/****************************************************************************
*
* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file batt_smbus.cpp
*
* Driver for a battery monitor connected via SMBus (I2C).
*
* @author Randy Mackay <rmackay9@yahoo.com>
*/
#include <nuttx/config.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdlib.h>
#include <stdbool.h>
#include <sched.h>
#include <semaphore.h>
#include <string.h>
#include <fcntl.h>
#include <poll.h>
#include <errno.h>
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <ctype.h>
#include <nuttx/arch.h>
#include <nuttx/wqueue.h>
#include <nuttx/clock.h>
#include <board_config.h>
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
#include <uORB/uORB.h>
#include <uORB/topics/subsystem_info.h>
#include <uORB/topics/battery_status.h>
#include <float.h>
#include <drivers/device/i2c.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_batt_smbus.h>
#include <drivers/device/ringbuffer.h>
#define BATT_SMBUS_ADDR_MIN 0x08 ///< lowest possible address
#define BATT_SMBUS_ADDR_MAX 0x7F ///< highest possible address
#define BATT_SMBUS_I2C_BUS PX4_I2C_BUS_EXPANSION
#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
#define BATT_SMBUS_MANUFACTURE_NAME 0x20 ///< manufacturer name
#define BATT_SMBUS_MANUFACTURE_INFO 0x25 ///< cell voltage register
#define BATT_SMBUS_CURRENT 0x2a ///< current register
#define BATT_SMBUS_MEASUREMENT_INTERVAL_MS (1000000 / 10) ///< time in microseconds, measure at 10hz
#define BATT_SMBUS_TIMEOUT_MS 10000000 ///< timeout looking for battery 10seconds after startup
#define BATT_SMBUS_PEC_POLYNOMIAL 0x07 ///< Polynomial for calculating PEC
#ifndef CONFIG_SCHED_WORKQUEUE
# error This requires CONFIG_SCHED_WORKQUEUE.
#endif
class BATT_SMBUS : public device::I2C
{
public:
BATT_SMBUS(int bus = PX4_I2C_BUS_EXPANSION, uint16_t batt_smbus_addr = BATT_SMBUS_ADDR);
virtual ~BATT_SMBUS();
/**
* Initialize device
*
* Calls probe() to check for device on bus.
*
* @return 0 on success, error code on failure
*/
virtual int init();
/**
* Test device
*
* @return 0 on success, error code on failure
*/
virtual int test();
/**
* Search all possible slave addresses for a smart battery
*/
int search();
protected:
/**
* Check if the device can be contacted
*/
virtual int probe();
private:
/**
* Start periodic reads from the battery
*/
void start();
/**
* Stop periodic reads from the battery
*/
void stop();
/**
* static function that is called by worker queue
*/
static void cycle_trampoline(void *arg);
/**
* perform a read from the battery
*/
void cycle();
/**
* Read a word from specified register
*/
int read_reg(uint8_t reg, uint16_t &val);
/**
* Read block from bus
* @return 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);
/**
* Calculate PEC for a read or write from the battery
* @param buff is the data that was read or will be written
*/
uint8_t get_PEC(uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len) const;
// internal variables
bool _enabled; ///< true if we have successfully connected to battery
work_s _work; ///< work queue for scheduling reads
RingBuffer *_reports; ///< buffer of recorded voltages, currents
struct battery_status_s _last_report; ///< last published report, used for test()
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
{
BATT_SMBUS *g_batt_smbus; ///< device handle. For now, we only support one BATT_SMBUS device
}
void batt_smbus_usage();
extern "C" __EXPORT int batt_smbus_main(int argc, char *argv[]);
BATT_SMBUS::BATT_SMBUS(int bus, uint16_t batt_smbus_addr) :
I2C("batt_smbus", BATT_SMBUS_DEVICE_PATH, bus, batt_smbus_addr, 400000),
_enabled(false),
_work{},
_reports(nullptr),
_batt_topic(-1),
_batt_orb_id(nullptr),
_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));
// capture startup time
_start_time = hrt_absolute_time();
}
BATT_SMBUS::~BATT_SMBUS()
{
// make sure we are truly inactive
stop();
if (_reports != nullptr) {
delete _reports;
}
}
int
BATT_SMBUS::init()
{
int ret = ENOTTY;
// attempt to initialise I2C bus
ret = I2C::init();
if (ret != OK) {
errx(1, "failed to init I2C");
return ret;
} else {
// allocate basic report buffers
_reports = new RingBuffer(2, sizeof(struct battery_status_s));
if (_reports == nullptr) {
ret = ENOTTY;
} else {
// start work queue
start();
}
}
// init orb id
_batt_orb_id = ORB_ID(battery_status);
return ret;
}
int
BATT_SMBUS::test()
{
int sub = orb_subscribe(ORB_ID(battery_status));
bool updated = false;
struct battery_status_s status;
uint64_t start_time = hrt_absolute_time();
// loop for 5 seconds
while ((hrt_absolute_time() - start_time) < 5000000) {
// display new info that has arrived from the orb
orb_check(sub, &updated);
if (updated) {
if (orb_copy(ORB_ID(battery_status), sub, &status) == OK) {
warnx("V=%4.2f C=%4.2f DismAh=%4.2f", (float)status.voltage_v, (float)status.current_a, (float)status.discharged_mah);
}
}
// sleep for 0.05 seconds
usleep(50000);
}
return OK;
}
int
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++) {
set_address(i);
if (read_reg(BATT_SMBUS_VOLTAGE, tmp) == OK) {
warnx("battery found at 0x%x", (int)i);
found_slave = true;
}
// short sleep
usleep(1);
}
// restore original i2c address
set_address(orig_addr);
// display completion message
if (found_slave) {
warnx("Done.");
} else {
warnx("No smart batteries found.");
}
return OK;
}
int
BATT_SMBUS::probe()
{
// always return OK to ensure device starts
return OK;
}
void
BATT_SMBUS::start()
{
// reset the report ring and state machine
_reports->flush();
// schedule a cycle to start things
work_queue(HPWORK, &_work, (worker_t)&BATT_SMBUS::cycle_trampoline, this, 1);
}
void
BATT_SMBUS::stop()
{
work_cancel(HPWORK, &_work);
}
void
BATT_SMBUS::cycle_trampoline(void *arg)
{
BATT_SMBUS *dev = (BATT_SMBUS *)arg;
dev->cycle();
}
void
BATT_SMBUS::cycle()
{
// get current time
uint64_t now = hrt_absolute_time();
// exit without rescheduling if we have failed to find a battery after 10 seconds
if (!_enabled && (now - _start_time > BATT_SMBUS_TIMEOUT_MS)) {
warnx("did not find smart battery");
return;
}
// read data from sensor
struct battery_status_s new_report;
// set time of reading
new_report.timestamp = now;
// read voltage
uint16_t tmp;
if (read_reg(BATT_SMBUS_VOLTAGE, tmp) == OK) {
// initialise new_report
memset(&new_report, 0, sizeof(new_report));
// convert millivolts to volts
new_report.voltage_v = ((float)tmp) / 1000.0f;
// read current
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);
} else {
_batt_topic = orb_advertise(_batt_orb_id, &new_report);
if (_batt_topic < 0) {
errx(1, "ADVERT FAIL");
}
}
// copy report for test()
_last_report = new_report;
// post a report to the ring
_reports->force(&new_report);
// notify anyone waiting for data
poll_notify(POLLIN);
// record we are working
_enabled = true;
}
// schedule a fresh cycle call when the measurement is done
work_queue(HPWORK, &_work, (worker_t)&BATT_SMBUS::cycle_trampoline, this,
USEC2TICK(BATT_SMBUS_MEASUREMENT_INTERVAL_MS));
}
int
BATT_SMBUS::read_reg(uint8_t reg, uint16_t &val)
{
uint8_t buff[3]; // 2 bytes of data + PEC
// read from register
int ret = transfer(®, 1, buff, 3);
if (ret == OK) {
// check PEC
uint8_t pec = get_PEC(reg, true, buff, 2);
if (pec == buff[2]) {
val = (uint16_t)buff[1] << 8 | (uint16_t)buff[0];
} else {
ret = ENOTTY;
}
}
// return success or failure
return ret;
}
uint8_t
BATT_SMBUS::read_block(uint8_t reg, uint8_t *data, uint8_t max_len, bool append_zero)
{
uint8_t buff[max_len + 2]; // buffer to hold results
// read bytes including PEC
int ret = transfer(®, 1, buff, max_len + 2);
// return zero on failure
if (ret != OK) {
return 0;
}
// get length
uint8_t bufflen = buff[0];
// sanity check length returned by smbus
if (bufflen == 0 || bufflen > max_len) {
return 0;
}
// check PEC
uint8_t pec = get_PEC(reg, true, buff, bufflen + 1);
if (pec != buff[bufflen + 1]) {
return 0;
}
// copy data
memcpy(data, &buff[1], bufflen);
// optionally add zero to end
if (append_zero) {
data[bufflen] = '\0';
}
// return success
return bufflen;
}
uint8_t
BATT_SMBUS::get_PEC(uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len) const
{
// exit immediately if no data
if (len <= 0) {
return 0;
}
// prepare temp buffer for calcing crc
uint8_t tmp_buff[len + 3];
tmp_buff[0] = (uint8_t)get_address() << 1;
tmp_buff[1] = cmd;
tmp_buff[2] = tmp_buff[0] | (uint8_t)reading;
memcpy(&tmp_buff[3], buff, len);
// initialise crc to zero
uint8_t crc = 0;
uint8_t shift_reg = 0;
bool do_invert;
// for each byte in the stream
for (uint8_t i = 0; i < sizeof(tmp_buff); i++) {
// load next data byte into the shift register
shift_reg = tmp_buff[i];
// for each bit in the current byte
for (uint8_t j = 0; j < 8; j++) {
do_invert = (crc ^ shift_reg) & 0x80;
crc <<= 1;
shift_reg <<= 1;
if (do_invert) {
crc ^= BATT_SMBUS_PEC_POLYNOMIAL;
}
}
}
// return result
return crc;
}
///////////////////////// shell functions ///////////////////////
void
batt_smbus_usage()
{
warnx("missing command: try 'start', 'test', 'stop', 'search'");
warnx("options:");
warnx(" -b i2cbus (%d)", BATT_SMBUS_I2C_BUS);
warnx(" -a addr (0x%x)", BATT_SMBUS_ADDR);
}
int
batt_smbus_main(int argc, char *argv[])
{
int i2cdevice = BATT_SMBUS_I2C_BUS;
int batt_smbusadr = BATT_SMBUS_ADDR; // 7bit address
int ch;
// jump over start/off/etc and look at options first
while ((ch = getopt(argc, argv, "a:b:")) != EOF) {
switch (ch) {
case 'a':
batt_smbusadr = strtol(optarg, NULL, 0);
break;
case 'b':
i2cdevice = strtol(optarg, NULL, 0);
break;
default:
batt_smbus_usage();
exit(0);
}
}
if (optind >= argc) {
batt_smbus_usage();
exit(1);
}
const char *verb = argv[optind];
if (!strcmp(verb, "start")) {
if (g_batt_smbus != nullptr) {
errx(1, "already started");
} else {
// create new global object
g_batt_smbus = new BATT_SMBUS(i2cdevice, batt_smbusadr);
if (g_batt_smbus == nullptr) {
errx(1, "new failed");
}
if (OK != g_batt_smbus->init()) {
delete g_batt_smbus;
g_batt_smbus = nullptr;
errx(1, "init failed");
}
}
exit(0);
}
// need the driver past this point
if (g_batt_smbus == nullptr) {
warnx("not started");
batt_smbus_usage();
exit(1);
}
if (!strcmp(verb, "test")) {
g_batt_smbus->test();
exit(0);
}
if (!strcmp(verb, "stop")) {
delete g_batt_smbus;
g_batt_smbus = nullptr;
exit(0);
}
if (!strcmp(verb, "search")) {
g_batt_smbus->search();
exit(0);
}
batt_smbus_usage();
exit(0);
}