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

AP_BattMonitor: fixed thread usage for SMBus_I2C

This commit is contained in:
Andrew Tridgell 2016-11-04 14:54:51 +11:00
parent e88519364f
commit 4f231f9355
2 changed files with 11 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
libraries/AP_BattMonitor/AP_BattMonitor_SMBus_I2C.cpp
View File

@ -9,8 +9,6 @@ extern const AP_HAL::HAL& hal;
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD != HAL_BOARD_PX4
#define BATTMONITOR_SMBUS_TEMP 0x08 // temperature register
#define BATTMONITOR_SMBUS_VOLTAGE 0x09 // voltage register
#define BATTMONITOR_SMBUS_FULL_CHARGE_CAPACITY 0x10 // full capacity register
@ -31,10 +29,17 @@ AP_BattMonitor_SMBus_I2C::AP_BattMonitor_SMBus_I2C(AP_BattMonitor &mon, uint8_t
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev)
: AP_BattMonitor_SMBus(mon, instance, mon_state)
, _dev(std::move(dev))
{}
{
_dev->register_periodic_callback(100000, FUNCTOR_BIND_MEMBER(&AP_BattMonitor_SMBus_I2C::timer, bool));
}
/// Read the battery voltage and current. Should be called at 10hz
void AP_BattMonitor_SMBus_I2C::read()
{
// nothing to do - all done in timer()
}
bool AP_BattMonitor_SMBus_I2C::timer()
{
uint16_t data;
uint8_t buff[4];
@ -57,38 +62,29 @@ void AP_BattMonitor_SMBus_I2C::read()
if ((tnow - _state.last_time_micros) > AP_BATTMONITOR_SMBUS_TIMEOUT_MICROS) {
_state.healthy = false;
}
return true;
}
// read word from register
// returns true if read was successful, false if failed
bool AP_BattMonitor_SMBus_I2C::read_word(uint8_t reg, uint16_t& data) const
{
// take i2c bus semaphore
if (!_dev->get_semaphore()->take_nonblocking()) {
return false;
}
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))) {
_dev->get_semaphore()->give();
return false;
}
// check PEC
uint8_t pec = get_PEC(BATTMONITOR_SMBUS_I2C_ADDR, reg, true, buff, 2);
if (pec != buff[2]) {
_dev->get_semaphore()->give();
return false;
}
// convert buffer to word
data = (uint16_t)buff[1]<<8 | (uint16_t)buff[0];
// give back i2c semaphore
_dev->get_semaphore()->give();
// return success
return true;
}
@ -98,20 +94,11 @@ uint8_t AP_BattMonitor_SMBus_I2C::read_block(uint8_t reg, uint8_t* data, uint8_t
{
uint8_t buff[max_len+2]; // buffer to hold results (2 extra byte returned holding length and PEC)
// take i2c bus semaphore
if (!_dev->get_semaphore()->take_nonblocking()) {
return 0;
}
// read bytes
if (!_dev->read_registers(reg, buff, sizeof(buff))) {
_dev->get_semaphore()->give();
return 0;
}
// give back i2c semaphore
_dev->get_semaphore()->give();
// get length
uint8_t bufflen = buff[0];
@ -179,4 +166,3 @@ uint8_t AP_BattMonitor_SMBus_I2C::get_PEC(const uint8_t i2c_addr, uint8_t cmd, b
return crc;
}
#endif // CONFIG_HAL_BOARD != HAL_BOARD_PX4

2
libraries/AP_BattMonitor/AP_BattMonitor_SMBus_I2C.h
View File

@ -23,6 +23,8 @@ public:
private:
bool 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;