int
lis3dh_read_regs(const lis3dh_context dev, uint8_t reg, uint8_t* buffer, int len)
{
assert(dev != NULL);
if (dev->spi) {
// Needed for read with address autoincrement
reg |= 0xC0;
uint8_t sbuf[len + 1];
memset((char*) sbuf, 0, len + 1);
sbuf[0] = reg;
_csOn(dev);
if (mraa_spi_transfer_buf(dev->spi, sbuf, sbuf, len + 1)) {
_csOff(dev);
printf("%s: mraa_spi_transfer_buf() failed\n", __FUNCTION__);
return -1;
}
_csOff(dev);
// Now copy it into user buffer
for (int i = 0; i < len; i++) {
buffer[i] = sbuf[i + 1];
}
} else {
// Needed for read with address autoincrement
reg |= 0x80;
if (mraa_i2c_read_bytes_data(dev->i2c, reg, buffer, len) != len) {
return -1;
}
}
return len;
}
upm_result_t bno055_read_regs(const bno055_context dev, uint8_t reg,
uint8_t *buffer, size_t len)
{
assert(dev != NULL);
if (mraa_i2c_read_bytes_data(dev->i2c, reg, buffer, len) < 0)
{
printf("%s: mraa_i2c_read_bytes() failed\n",
__FUNCTION__);
return UPM_ERROR_OPERATION_FAILED;
}
return UPM_SUCCESS;
}
void getangle()
{
mraa_i2c_read_bytes_data(i2c,MPU6050_RA_ACCEL_XOUT_H,buffer,14);
ax = (((int16_t)buffer[0]) << 8) | buffer[1];
ay = (((int16_t)buffer[2]) << 8) | buffer[3];
az = (((int16_t)buffer[4]) << 8) | buffer[5];
gx = (((int16_t)buffer[8]) << 8) | buffer[9];
gy = (((int16_t)buffer[10]) << 8) | buffer[11];
gz = (((int16_t)buffer[12]) << 8) | buffer[13];
// angleAx=atan2(ax,az)*180/PI;//计算与x轴夹角
// gyroGy=-gy/131.00;//计算角速度
angleAx=atan2(ay,az)*180/PI;//计算与y轴夹角
gyroGy=-gx/131.00;//
yijielvbo(angleAx,gyroGy);//一阶滤波
erjielvbo(angleAx,gyroGy);//二阶滤波
kalman_Filter(angleAx,gyroGy); //卡尔曼滤波
}
static upm_result_t kx122_read_registers(const kx122_context dev, uint8_t reg, uint8_t *buffer, uint len)
{
if(dev->using_spi){
reg |= SPI_READ;
uint8_t spi_data_buffer[len + 1];
spi_data_buffer[0] = reg;
kx122_chip_select_on(dev);
if(mraa_spi_transfer_buf(dev->spi,spi_data_buffer,spi_data_buffer,
(sizeof(spi_data_buffer) / sizeof(uint8_t))) != MRAA_SUCCESS){
printf("%s: mraa_spi_transfer_buf() failed.\n", __FUNCTION__);
kx122_chip_select_off(dev);
return UPM_ERROR_OPERATION_FAILED;
}
for (size_t i = 0; i < len; i++) {
buffer[i] = spi_data_buffer[i + 1];
}
kx122_chip_select_off(dev);
return UPM_SUCCESS;
}
else{
uint8_t data_buffer[len];
if(mraa_i2c_read_bytes_data(dev->i2c,reg,data_buffer,len) != (int)len){
return UPM_ERROR_OPERATION_FAILED;
}
else{
for(size_t i = 0; i < len; i++){
buffer[i] = data_buffer[i];
}
return UPM_SUCCESS;
}
}
}
int edison_i2c_read(unsigned char slave_addr,
unsigned char reg_addr,
unsigned char length,
unsigned char *data)
{
int ret = mraa_i2c_read_bytes_data(/*mraa_i2c_context*/ i2c, /*uint8_t command*/reg_addr, /*uint8_t* data*/ data, /*int length*/ length);
if (ret == length){
return 0;
}
else if (ret == -1){
fprintf(stderr, "MRAA i2c bus read bytes operation failed.");
return -1;
}
else if (ret > 0 || ret < length){
fprintf(stderr, "MRAA i2c the length of bus bytes were read is not equal to which was needed.");
return -1;
}
else{
fprintf(stderr, "MRAA i2c bus read bytes length error.");
return -1;
}
/*
unsigned long start, cur;
if (!i2c.enabled)
return -1;
if (!length)
return 0;
// Populate struct.
i2c.state = STATE_READING;
i2c.slave_reg = reg_addr;
i2c.slave_reg_written = 0;
i2c.data = data;
i2c.length = length;
I2CSA = slave_addr;
CTL1 |= UCTR | UCTXSTT;
msp430_get_clock_ms(&start);
while (i2c.state != STATE_WAITING) {
__bis_SR_register(LPM0_bits + GIE);
msp430_get_clock_ms(&cur);
if (cur >= (start + I2C_TIMEOUT_MS)) {
CTL1 |= UCTXSTP;
i2c.state = STATE_WAITING;
msp430_i2c_disable();
msp430_delay_ms(1);
CLEAR_SCL();
CLEAR_SDA();
msp430_delay_ms(1);
SET_SCL();
SET_SDA();
msp430_i2c_enable();
return -1;
}
}
return 0;
*/
}
/**
* Fast call for multiple bytes read from LSM9DS0 Accelerometer / Magnetometer
*
*/
void lsm_xm_read_bytes(unsigned char reg, unsigned char *buf, int len) {
mraa_i2c_address(i2c, LSM_ADDRESS_XM);
if(mraa_i2c_read_bytes_data(i2c, reg|0x80, buf, len) == -1) {
printf("read multiple bytes from LSM9DS0 XM failed...\n");
}
}
/**
* Read length bytes from the bus into *data pointer starting from
* an i2c register
*
* @param reg Register to read from
* @param data pointer to the byte array to read data in to
* @param length max number of bytes to read
* @return length passed to the function or -1
*/
int
readBytesReg(uint8_t reg, uint8_t* data, int length)
{
return mraa_i2c_read_bytes_data(m_i2c, reg, data, length);
}
