rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-17 02:52:12 +01:00
Code Issues Releases Activity

OP-378: Start of IMU3000 fifo reading code

Browse Source
This commit is contained in:
James Cotton 2011-05-31 01:51:05 -05:00
parent b6a8293f69
commit 524cdf7743
3 changed files with 78 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
flight/INS/ins.c
View File

@ -607,8 +607,6 @@ bool get_accel_gyro_data()
int32_t accel_accum[3] = {0, 0, 0};
accel_samples = 0;
int16_t gyro[3];
t_fifo_buffer * accel_fifo = PIOS_BMA180_GetFifo();
while(fifoBuf_getUsed(accel_fifo) < sizeof(accel));
while(fifoBuf_getUsed(accel_fifo) >= sizeof(accel)) {
@ -622,7 +620,20 @@ bool get_accel_gyro_data()
accel[1] = accel_accum[1] / accel_samples;
accel[2] = accel_accum[2] / accel_samples;
PIOS_IMU3000_ReadGyros(gyro);
int16_t gyro_fifo[4 * 100];
uint8_t gyro_len;
int32_t gyro_accum[3] = {0, 0, 0};
int16_t gyro[3];
gyro_len = PIOS_IMU3000_ReadFifo((uint8_t *) gyro_fifo, sizeof(gyro_fifo));
gyro_len /= 8;
for(int i = 0; i < gyro_len; i++) {
gyro_accum[0] += gyro_fifo[i * 4];
gyro_accum[1] += gyro_fifo[1 + i * 4];
gyro_accum[2] += gyro_fifo[2 + i * 4];
}
gyro[0] = gyro_accum[0] / gyro_len;
gyro[1] = gyro_accum[1] / gyro_len;
gyro[2] = gyro_accum[2] / gyro_len;
// Not the swaping of channel orders
accel_data.filtered.x = accel[0] * PIOS_BMA180_GetScale();

63
flight/PiOS/Common/pios_imu3000.c
View File

@ -156,6 +156,69 @@ int32_t PIOS_IMU3000_ReadID()
return id;
}
/**
* @brief Reads the data from the IMU3000 FIFO
* \param[out] buffer destination buffer
* \param[in] len maximum number of bytes which should be read
* \return number of bytes transferred if operation was successful
* \return -1 if error during I2C transfer
*/
int32_t PIOS_IMU3000_ReadFifo(uint8_t * buffer, uint16_t len)
{
uint16_t fifo_level;
uint8_t addr_buffer[] = {
0x3A,
};
const struct pios_i2c_txn txn_list[] = {
{
.info = __func__,
.addr = PIOS_IMU3000_I2C_ADDR,
.rw = PIOS_I2C_TXN_WRITE,
.len = sizeof(addr_buffer),
.buf = addr_buffer,
}
,
{
.info = __func__,
.addr = PIOS_IMU3000_I2C_ADDR,
.rw = PIOS_I2C_TXN_READ,
.len = 2,
.buf = (uint8_t *) &fifo_level,
}
};
// Get the number of bytes in the fifo
PIOS_I2C_Transfer(PIOS_I2C_GYRO_ADAPTER, txn_list, NELEMENTS(txn_list));
addr_buffer[0] = 0x3C;
if(len > fifo_level)
len = fifo_level;
len &= 0x01f8; // only read chunks of 8 bytes (includes footer)
const struct pios_i2c_txn txn_list2[] = {
{
.info = __func__,
.addr = PIOS_IMU3000_I2C_ADDR,
.rw = PIOS_I2C_TXN_WRITE,
.len = sizeof(addr_buffer),
.buf = addr_buffer,
}
,
{
.info = __func__,
.addr = PIOS_IMU3000_I2C_ADDR,
.rw = PIOS_I2C_TXN_READ,
.len = len,
.buf = buffer,
}
};
return PIOS_I2C_Transfer(PIOS_I2C_GYRO_ADAPTER, txn_list2, NELEMENTS(txn_list)) ? len : -1;
}
/**
* @brief Reads one or more bytes from IMU3000 into a buffer
* \param[in] address IMU3000 register address (depends on size)

1
flight/PiOS/inc/pios_imu3000.h
View File

@ -119,6 +119,7 @@ struct pios_imu3000_data {
/* Public Functions */
extern void PIOS_IMU3000_Init(void);
extern bool PIOS_IMU3000_NewDataAvailable(void);
extern int32_t PIOS_IMU3000_ReadFifo(uint8_t * buffer, uint16_t len);
extern int32_t PIOS_IMU3000_ReadGyros(int16_t * data);
extern int32_t PIOS_IMU3000_ReadID();
extern uint8_t PIOS_IMU3000_Test();