aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

HAL_Linux: include readRegistersMultiple in I2CDriver

Browse Source 
AP_HAL_Linux implemments this new method.
This commit is contained in:
Víctor Mayoral Vilches 2014-05-21 12:09:57 +02:00 committed by Andrew Tridgell
parent ea7dcc188b
commit c4b6026d53
4 changed files with 65 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_HAL/I2CDriver.h
View File

@ -26,11 +26,18 @@ public:
/* readRegister: read from a device register - writes the register,
* then reads back an 8-bit value. */
virtual uint8_t readRegister(uint8_t addr, uint8_t reg, uint8_t* data) = 0;
/* readRegister: read contigious device registers - writes the first
/* readRegisters: read contigious device registers - writes the first
* register, then reads back multiple bytes */
virtual uint8_t readRegisters(uint8_t addr, uint8_t reg,
uint8_t len, uint8_t* data) = 0;
/* readRegistersMultiple: read contigious device registers.
Equivalent to count calls to readRegisters() */
virtual uint8_t readRegistersMultiple(uint8_t addr, uint8_t reg,
uint8_t len, uint8_t count,
uint8_t* data) = 0;
virtual uint8_t lockup_count() = 0;
void ignore_errors(bool b) { _ignore_errors = b; }
virtual AP_HAL::Semaphore* get_semaphore() = 0;

58
libraries/AP_HAL_Linux/I2CDriver.cpp
View File

@ -137,15 +137,59 @@ uint8_t LinuxI2CDriver::read(uint8_t addr, uint8_t len, uint8_t* data)
uint8_t LinuxI2CDriver::readRegisters(uint8_t addr, uint8_t reg,
uint8_t len, uint8_t* data)
{
if (!set_address(addr)) {
struct i2c_msg msgs[] = {
{
addr : addr,
flags : 0,
len : 1,
buf : &reg
},
{
addr : addr,
flags : I2C_M_RD,
len : len,
buf : data,
}
};
struct i2c_rdwr_ioctl_data i2c_data = {
msgs : msgs,
nmsgs : 2
};
if (ioctl(_fd, I2C_RDWR, &i2c_data) == -1) {
return 1;
}
// send the address to read from
if (::write(_fd, &reg, 1) != 1) {
return 1;
}
if (::read(_fd, data, len) != len) {
return 1;
return 0;
}
uint8_t LinuxI2CDriver::readRegistersMultiple(uint8_t addr, uint8_t reg,
uint8_t len,
uint8_t count, uint8_t* data)
{
while (count > 0) {
uint8_t n = count>8?8:count;
struct i2c_msg msgs[2*n];
struct i2c_rdwr_ioctl_data i2c_data = {
msgs : msgs,
nmsgs : 2*n
};
for (uint8_t i=0; i<n; i++) {
msgs[i*2].addr = addr;
msgs[i*2].flags = 0;
msgs[i*2].len = 1;
msgs[i*2].buf = &reg;
msgs[i*2+1].addr = addr;
msgs[i*2+1].flags = I2C_M_RD;
msgs[i*2+1].len = len;
msgs[i*2+1].buf = data;
data += len;
};
if (ioctl(_fd, I2C_RDWR, &i2c_data) == -1) {
return 1;
}
count -= n;
}
return 0;
}

5
libraries/AP_HAL_Linux/I2CDriver.h
View File

@ -26,11 +26,16 @@ public:
/* readRegister: read from a device register - writes the register,
* then reads back an 8-bit value. */
uint8_t readRegister(uint8_t addr, uint8_t reg, uint8_t* data);
/* readRegister: read contigious device registers - writes the first
* register, then reads back multiple bytes */
uint8_t readRegisters(uint8_t addr, uint8_t reg,
uint8_t len, uint8_t* data);
uint8_t readRegistersMultiple(uint8_t addr, uint8_t reg,
uint8_t len, uint8_t count,
uint8_t* data);
uint8_t lockup_count();
AP_HAL::Semaphore* get_semaphore() { return _semaphore; }

2
libraries/AP_HAL_Linux/Scheduler.cpp
View File

@ -227,7 +227,7 @@ void *LinuxScheduler::_timer_thread(void)
poll(NULL, 0, 1);
}
while (true) {
_microsleep(5000);
_microsleep(1000);
// run registered timers
_run_timers(true);