mirror of
https://github.com/RobotechLille/cdf2018-principal
synced 2024-05-07 06:11:45 +00:00
117 lines
3.1 KiB
C
117 lines
3.1 KiB
C
#include <math.h>
|
|
#include <pthread.h>
|
|
#include <stdlib.h>
|
|
#include <unistd.h>
|
|
|
|
#include "i2c.h"
|
|
#include "imu.h"
|
|
|
|
int gyroFd;
|
|
int acelFd;
|
|
pthread_t tIMU;
|
|
|
|
void configureIMU()
|
|
{
|
|
gyroFd = openI2C(GYRO_ADDR);
|
|
acelFd = openI2C(ACEL_ADDR);
|
|
|
|
writeI2C(gyroFd, GYRO_CTRL_REG1, 0x0F); // PD = 1 (normal mode); Zen = Yen = Xen = 1 (all axes enabled)
|
|
writeI2C(gyroFd, GYRO_CTRL_REG4, 0xB0); // Block data update, 2000 dps full scale
|
|
|
|
writeI2C(acelFd, ACEL_CTRL1, 0x6F); // 50 Hz Acceleration data-rate; block data update; Zen = Yen = Xen = 1 (all axes enabled)
|
|
writeI2C(acelFd, ACEL_CTRL2, 0x20); // +/- 16G full scale
|
|
|
|
pthread_create(&tIMU, NULL, TaskIMU, NULL);
|
|
}
|
|
|
|
bool connectedIMU()
|
|
{
|
|
return (uint8_t)readI2C(gyroFd, GYRO_WHO_AM_I) == GYRO_IDENTITY && (uint8_t)readI2C(acelFd, ACEL_WHO_AM_I) == ACEL_IDENTITY;
|
|
}
|
|
|
|
struct axesRaw readGyroRaw()
|
|
{
|
|
struct axesRaw 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;
|
|
}
|
|
|
|
struct axes readGyro()
|
|
{
|
|
struct axesRaw raw = readGyroRaw();
|
|
struct axes computed;
|
|
computed.x = (float)raw.x * GYRO_GAIN;
|
|
computed.y = (float)raw.y * GYRO_GAIN;
|
|
computed.z = (float)raw.z * GYRO_GAIN;
|
|
return computed;
|
|
}
|
|
|
|
struct axesRaw readAcelRaw()
|
|
{
|
|
struct axesRaw res;
|
|
res.x = (readI2C(acelFd, ACEL_OUT_X_H_A) << 8 | readI2C(acelFd, ACEL_OUT_X_L_A));
|
|
res.y = (readI2C(acelFd, ACEL_OUT_Y_H_A) << 8 | readI2C(acelFd, ACEL_OUT_Y_L_A));
|
|
res.z = (readI2C(acelFd, ACEL_OUT_Z_H_A) << 8 | readI2C(acelFd, ACEL_OUT_Z_L_A));
|
|
return res;
|
|
}
|
|
|
|
struct axes readAcel()
|
|
{
|
|
struct axesRaw raw = readAcelRaw();
|
|
struct axes computed;
|
|
computed.x = atan2(raw.y, raw.z) + M_PI;
|
|
computed.y = atan2(raw.z, raw.x) + M_PI;
|
|
computed.z = atan2(raw.x, raw.y) + M_PI;
|
|
return computed;
|
|
}
|
|
|
|
struct timespec imuStart;
|
|
struct timespec imuNow;
|
|
struct timespec imuEcoule;
|
|
|
|
void* TaskIMU(void* pData)
|
|
{
|
|
(void)pData;
|
|
|
|
struct axes gyroNew;
|
|
struct axes gyro;
|
|
struct axes acel;
|
|
struct axes complemFilter;
|
|
|
|
clock_gettime(CLOCK_REALTIME, &imuStart);
|
|
|
|
for (;;) {
|
|
clock_gettime(CLOCK_REALTIME, &imuNow);
|
|
if ((imuNow.tv_nsec - imuStart.tv_nsec) < 0) {
|
|
imuEcoule.tv_sec = imuNow.tv_sec - imuStart.tv_sec - 1;
|
|
imuEcoule.tv_nsec = imuNow.tv_nsec - imuStart.tv_nsec + 1000000000UL;
|
|
} else {
|
|
imuEcoule.tv_sec = imuNow.tv_sec - imuStart.tv_sec;
|
|
imuEcoule.tv_nsec = imuNow.tv_nsec - imuStart.tv_nsec;
|
|
}
|
|
imuStart.tv_sec = imuNow.tv_sec;
|
|
imuStart.tv_nsec = imuNow.tv_nsec;
|
|
float dt = imuEcoule.tv_sec + imuStart.tv_nsec * 1E-9;
|
|
|
|
gyroNew = readGyro();
|
|
gyro.x += gyroNew.x * dt;
|
|
gyro.y += gyroNew.y * dt;
|
|
gyro.z += gyroNew.z * dt;
|
|
|
|
acel = readAcel();
|
|
|
|
/* complemFilter.x = c * (complemFilter.x + gyroNew.x * dt) + (1 - AA) * acel.x; */
|
|
|
|
usleep(IMU_INTERVAL * 1000);
|
|
}
|
|
|
|
return NULL;
|
|
}
|
|
|
|
void deconfigureIMU()
|
|
{
|
|
pthread_cancel(tIMU);
|
|
}
|