mirror of
https://github.com/RobotechLille/cdf2018-principal
synced 2024-05-20 13:49:37 +02:00
35 lines
916 B
C
35 lines
916 B
C
#include "imu.h"
|
|
#include "i2c.h"
|
|
|
|
int gyroFd;
|
|
int accelFd;
|
|
|
|
void configureIMU()
|
|
{
|
|
gyroFd = openI2C(GYRO_ADDR);
|
|
accelFd = openI2C(ACCEL_ADDR);
|
|
|
|
writeI2C(gyroFd, GYRO_LOW_ODR, 0x00); // Low_ODR = 0 (low speed ODR disabled)
|
|
writeI2C(gyroFd, GYRO_CTRL_REG4, 0x00); // FS = 00 (+/- 250 dps full scale)
|
|
writeI2C(gyroFd, GYRO_CTRL_REG1, 0x6F); // DR = 01 (200 Hz ODR); BW = 10 (50 Hz bandwidth); PD = 1 (normal mode); Zen = Yen = Xen = 1 (all axes enabled)
|
|
}
|
|
|
|
bool connectedIMU()
|
|
{
|
|
return (uint8_t) readI2C(gyroFd, GYRO_WHO_AM_I) == GYRO_IDENTITY;
|
|
}
|
|
|
|
struct gyroRaw readGyroRaw()
|
|
{
|
|
struct gyroRaw res;
|
|
res.x = (readI2C(gyroFd, GYRO_OUT_X_H) << 8 | readI2C(gyroFd, GYRO_OUT_X_L));
|
|
res.y = (readI2C(gyroFd, GYRO_OUT_Y_H) << 8 | readI2C(gyroFd, GYRO_OUT_Y_L));
|
|
res.z = (readI2C(gyroFd, GYRO_OUT_Z_H) << 8 | readI2C(gyroFd, GYRO_OUT_Z_L));
|
|
return res;
|
|
}
|
|
|
|
void deconfigureIMU()
|
|
{
|
|
|
|
}
|