void *imuUpdate(void *arg) {
//struct timeval t0, t1;
//float elapsed;
struct sched_param param = {.sched_priority = 15};
sched_setscheduler(0, SCHED_RR, ¶m);
prctl(PR_SET_NAME,"imu",0,0,0);
while (1) {
sem_wait(&sem_imu_trigger);
//gettimeofday(&t0, NULL);
pthread_mutex_lock(&_imu_lock);
sensorRead(&_imu_working_reg); // read 9-dof sensor
//gettimeofday(&t1, NULL);
//elapsed = (t1.tv_sec - t0.tv_sec)*1000 + (t1.tv_usec - t0.tv_usec) / 1000.0f;
//printf("Sensor read time: %f ms\n", elapsed);
AHRSupdate(&_imu_working_reg); // call AHRS update routine
getYawPitchRoll(&_imu_working_reg); // return RPY representation
pthread_mutex_unlock(&_imu_lock);
}
}
void FreeIMU1::getYawPitchRoll(float * ypr) {
int i;
double dYPR[3];
getYawPitchRoll(dYPR);
for (i = 0; i < 3; i++)
ypr[i] = (float)dYPR[i];
}
void Accelerometer::bypassDrift()
{
sleep(20);
resetFIFO();
while(getFIFOCount() < 42);
float calibratedValues[3];
getYawPitchRoll(calibratedValues);
zeroValues[0] = calibratedValues[0];
zeroValues[1] = calibratedValues[1];
zeroValues[2] = calibratedValues[2];
}
uint8_t Accelerometer::getYawPitchRoll(float *data)
{
uint8_t fifoBuffer[64];
Quaternion q;
VectorFloat gravity;
uint16_t packetSize = getFIFOPacketSize();
getFIFOBytes(fifoBuffer, packetSize);
getQuaternion(&q, fifoBuffer);
getGravity(&gravity, &q);
getYawPitchRoll(data, &q, &gravity);
data[0] = (data[0] * 180/M_PI) - zeroValues[0];
data[1] = (data[1] * 180/M_PI) - zeroValues[1];
data[2] = (data[2] * 180/M_PI) - zeroValues[2];
return 0;
}