Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 23:33:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_HAL_Linux: I2CDevice: method to read multiple times

Browse Source
This commit is contained in:
Lucas De Marchi 2016-01-19 03:32:24 -02:00
parent c394de31a0
commit 5194f7e5ce
2 changed files with 53 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
libraries/AP_HAL_Linux/I2CDevice.cpp
View File

@ -43,9 +43,15 @@
#include <vector>
#include <AP_HAL/AP_HAL.h>
#include <AP_Math/AP_Math.h>
#include "Util.h"
/* Workaround broken header from i2c-tools */
#ifndef I2C_RDRW_IOCTL_MAX_MSGS
#define I2C_RDRW_IOCTL_MAX_MSGS 42
#endif
namespace Linux {
static const AP_HAL::HAL &hal = AP_HAL::get_HAL();
@ -154,6 +160,50 @@ bool I2CDevice::transfer(const uint8_t *send, uint32_t send_len,
return r >= 0;
}
bool I2CDevice::read_registers_multiple(uint8_t first_reg, uint8_t *recv,
uint32_t recv_len, uint8_t times)
{
const uint8_t max_times = I2C_RDRW_IOCTL_MAX_MSGS / 2;
while (times > 0) {
uint8_t n = MIN(times, max_times);
struct i2c_msg msgs[2 * n];
struct i2c_rdwr_ioctl_data i2c_data = { };
memset(msgs, 0, 2 * n * sizeof(*msgs));
i2c_data.msgs = msgs;
i2c_data.nmsgs = 2 * n;
for (uint8_t i = 0; i < i2c_data.nmsgs; i += 2) {
msgs[i].addr = _address;
msgs[i].flags = 0;
msgs[i].buf = &first_reg;
msgs[i].len = 1;
msgs[i + 1].addr = _address;
msgs[i + 1].flags = I2C_M_RD;
msgs[i + 1].buf = recv;
msgs[i + 1].len = recv_len;
recv += recv_len;
};
int r = -EINVAL;
unsigned retries = _retries;
do {
r = ::ioctl(_bus.fd, I2C_RDWR, &i2c_data);
} while (r < 0 && retries-- > 0);
if (r < 0) {
return false;
}
times -= n;
}
return true;
}
AP_HAL::Semaphore *I2CDevice::get_semaphore()
{
return &_bus.sem;

3
libraries/AP_HAL_Linux/I2CDevice.h
View File

@ -62,6 +62,9 @@ public:
bool transfer(const uint8_t *send, uint32_t send_len,
uint8_t *recv, uint32_t recv_len) override;
bool read_registers_multiple(uint8_t first_reg, uint8_t *recv,
uint32_t recv_len, uint8_t times) override;
/* See AP_HAL::Device::get_semaphore() */
AP_HAL::Semaphore *get_semaphore() override;