forked from Archive/PX4-Autopilot
SMBus battery driver: further code style fixes
This commit is contained in:
Lorenz Meier
parent
17e4e283d8
commit
1106e0bff3
|
|
@ -104,8 +104,22 @@ 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
|
||||
*/
|
||||
|
|
@ -164,10 +178,9 @@ private:
|
|||
orb_id_t _batt_orb_id; ///< uORB battery topic ID
|
||||
};
|
||||
|
||||
/* for now, we only support one BATT_SMBUS */
|
||||
namespace
|
||||
{
|
||||
BATT_SMBUS *g_batt_smbus;
|
||||
BATT_SMBUS *g_batt_smbus; ///< device handle. For now, we only support one BATT_SMBUS device
|
||||
}
|
||||
|
||||
void batt_smbus_usage();
|
||||
|
|
@ -206,11 +219,14 @@ BATT_SMBUS::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();
|
||||
|
|
@ -236,6 +252,7 @@ BATT_SMBUS::test()
|
|||
|
||||
// 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", status.voltage_v, status.current_a);
|
||||
|
|
@ -258,10 +275,12 @@ BATT_SMBUS::search()
|
|||
// 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);
|
||||
}
|
||||
|
|
@ -269,6 +288,7 @@ BATT_SMBUS::search()
|
|||
// display completion message
|
||||
if (found_slave) {
|
||||
warnx("Done.");
|
||||
|
||||
} else {
|
||||
warnx("No smart batteries found.");
|
||||
}
|
||||
|
|
@ -318,6 +338,7 @@ BATT_SMBUS::cycle()
|
|||
|
||||
// read voltage
|
||||
uint16_t tmp;
|
||||
|
||||
if (read_reg(BATT_SMBUS_VOLTAGE, tmp) == OK) {
|
||||
// initialise new_report
|
||||
memset(&new_report, 0, sizeof(new_report));
|
||||
|
|
@ -328,15 +349,19 @@ BATT_SMBUS::cycle()
|
|||
// 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;
|
||||
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;
|
||||
}
|
||||
|
||||
// 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");
|
||||
}
|
||||
|
|
@ -353,7 +378,8 @@ BATT_SMBUS::cycle()
|
|||
}
|
||||
|
||||
// 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));
|
||||
work_queue(HPWORK, &_work, (worker_t)&BATT_SMBUS::cycle_trampoline, this,
|
||||
USEC2TICK(BATT_SMBUS_MEASUREMENT_INTERVAL_MS));
|
||||
}
|
||||
|
||||
int
|
||||
|
|
@ -363,11 +389,14 @@ BATT_SMBUS::read_reg(uint8_t reg, uint16_t &val)
|
|||
|
||||
// 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;
|
||||
}
|
||||
|
|
@ -402,10 +431,12 @@ BATT_SMBUS::read_block(uint8_t reg, uint8_t* data, uint8_t max_len, bool append_
|
|||
|
||||
// check PEC
|
||||
uint8_t pec = get_PEC(reg, true, buff, bufflen + 1);
|
||||
|
||||
if (pec != buff[bufflen + 1]) {
|
||||
// debug
|
||||
warnx("CurrPEC:%x vs MyPec:%x", (int)buff[bufflen + 1], (int)pec);
|
||||
return 0;
|
||||
|
||||
} else {
|
||||
warnx("CurPEC ok: %x", (int)pec);
|
||||
}
|
||||
|
|
@ -446,11 +477,13 @@ BATT_SMBUS::get_PEC(uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len
|
|||
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;
|
||||
}
|
||||
|
|
@ -507,6 +540,7 @@ batt_smbus_main(int argc, char *argv[])
|
|||
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);
|
||||
|
|
|
|||
Loading…
Reference in New Issue