mirror of
https://github.com/RobotechLille/cdf2018-principal
synced 2024-09-28 16:23:04 +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()
|
||
|
{
|
||
|
|
||
|
}
|