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);
}
}
/**
* Populates array q with a quaternion representing the IMU orientation with respect to the Earth
*
* @param q the quaternion to populate
*/
void FreeIMU::getQ(float * q) {
float val[9];
getValues(val);
DEBUG_PRINT(val[3] * M_PI/180);
DEBUG_PRINT(val[4] * M_PI/180);
DEBUG_PRINT(val[5] * M_PI/180);
DEBUG_PRINT(val[0]);
DEBUG_PRINT(val[1]);
DEBUG_PRINT(val[2]);
DEBUG_PRINT(val[6]);
DEBUG_PRINT(val[7]);
DEBUG_PRINT(val[8]);
now = micros();
sampleFreq = 1.0 / ((now - lastUpdate) / 1000000.0);
lastUpdate = now;
// gyro values are expressed in deg/sec, the * M_PI/180 will convert it to radians/sec
#if IS_9DOM()
#if HAS_AXIS_ALIGNED()
AHRSupdate(val[3] * M_PI/180, val[4] * M_PI/180, val[5] * M_PI/180, val[0], val[1], val[2], val[6], val[7], val[8]);
#elif defined(SEN_10724)
AHRSupdate(val[3] * M_PI/180, val[4] * M_PI/180, val[5] * M_PI/180, val[0], val[1], val[2], val[7], -val[6], val[8]);
#elif defined(ARDUIMU_v3)
AHRSupdate(val[3] * M_PI/180, val[4] * M_PI/180, val[5] * M_PI/180, val[0], val[1], val[2], -val[6], -val[7], val[8]);
#endif
#else
AHRSupdate(val[3] * M_PI/180, val[4] * M_PI/180, val[5] * M_PI/180, val[0], val[1], val[2]);
#endif
q[0] = q0;
q[1] = q1;
q[2] = q2;
q[3] = q3;
}
/**
* Populates array q with a quaternion representing the IMU orientation with respect to the Earth
*
* @param q the quaternion to populate
*/
void FreeIMU1::getQ(double * q) {
double val[6];
getValues(val);
now = micros();
sampleFreq = 1.0 / ((now - lastUpdate) / 1000000.0);
lastUpdate = now;
// gyro values are expressed in deg/sec, the * M_PI/180 will convert it to radians/sec
#if IS_6DOM()
AHRSupdate(val[3] * M_PI/180, val[4] * M_PI/180, val[5] * M_PI/180, val[0], val[1], val[2]);
#endif
q[0] = q0;
q[1] = q1;
q[2] = q2;
q[3] = q3;
}
PROCESS_THREAD(ahrs_process,ev,data)
{
static hrclock_t timestamp;
static float gx,gy,gz,ax,ay,az,mx,my,mz,td;
static struct sensors_sensor *gyr, *acc, *mag;
if(ev == PROCESS_EVENT_EXIT) { /* switch off sampling */
if (gyr!=NULL) gyr->configure(SENSORS_ACTIVE,0);
if (acc!=NULL) acc->configure(SENSORS_ACTIVE,0);
if (mag!=NULL) mag->configure(SENSORS_ACTIVE,0);
} else if (ev == sensors_event) {
if (data==acc) {
ax = acc->value(ACC_VALUE_X);
ay = acc->value(ACC_VALUE_Y);
az = acc->value(ACC_VALUE_Z);
} else if (data==mag) {
mx = magcal(mag,MAG_VALUE_X);
my = magcal(mag,MAG_VALUE_Y);
mz = magcal(mag,MAG_VALUE_Z);
} else if (data==gyr) {
gx = gyro2rad(gyr->value(GYRO_VALUE_X));
gy = gyro2rad(gyr->value(GYRO_VALUE_Y));
gz = gyro2rad(gyr->value(GYRO_VALUE_Z));
}
}
PROCESS_BEGIN();
/* initialize sensors */
gyr = sensors_find(GYRO_SENSOR);
acc = sensors_find(ACC_SENSOR);
mag = sensors_find(MAG_SENSOR);
if (gyr==NULL) {
strncpy(msg, "gyro not found\n",sizeof(msg));
process_exit(&ahrs_process);
} else if (acc==NULL) {
strncpy(msg, "accelerometer not found\n",sizeof(msg));
process_exit(&ahrs_process);
} else if (mag==NULL) {
strncpy(msg, "compass not found\n",sizeof(msg));
process_exit(&ahrs_process);
} else {
gyr->configure(SENSORS_ACTIVE,1);
acc->configure(SENSORS_ACTIVE,1);
mag->configure(SENSORS_ACTIVE,1);
}
/* configure sensors, gyro to 2000dps@200Hz with bandpass at 15-70Hz, bandpass
* for zero-rate removal! acc to 100Hz and 1Hz high-pass */
gyr->configure(GYRO_L3G_RANGE,GYRO_L3GVAL_2000DPS);
gyr->configure(GYRO_L3G_DRATE,GYRO_L3GVAL_200_70HZ);
gyr->configure(GYRO_L3G_HIGHPASS,GYRO_L3GVAL_HP0); // 15Hz@200Hz with ref
gyr->configure(GYRO_L3G_FILTER,GYRO_L3GVAL_BOTH);
acc->configure(ACC_LSM303_RANGE,ACC_LSM303VAL_2G);
acc->configure(ACC_LSM303_DRATE,ACC_LSM303VAL_100HZ);
acc->configure(ACC_LSM303_HIGHPASS,ACC_LSM303VAL_HP1);
acc->configure(ACC_LSM303_FILTER,ACC_LSM303VAL_BOTH);
/* XXX: ignore first measurements */
while (gx==0 || mx==0 || ax==0)
PROCESS_YIELD_UNTIL(ev==sensors_event);
/* reset quaternion and compass calibration */
q0=1; q1=q2=q3=exInt=eyInt=ezInt=0;
magcal(NULL,0);
while (1) { /* estimate continously and write into msg */
timestamp = clock_hrtime();
PROCESS_YIELD_UNTIL(ev==sensors_event);
/* calculate sampling time */
td = (clock_hrtime()-timestamp) / 1.e6;
/* update estimate */
AHRSupdate(gx,gy,gz,ax,ay,az,mx,my,mz,td);
//IMUupdate(gx,gy,gz,ax,ay,az,td);
/* rebuild msg string */
{
char *s = msg;
s += ftoa(td,s);
*s++ = ' ';
s += ftoa(q0,s);
*s++ = ' ';
s += ftoa(q1,s);
*s++ = ' ';
s += ftoa(q2,s);
*s++ = ' ';
s += ftoa(q3,s);
*s++ = ' ';
s += ftoa(exInt,s);
*s++ = ' ';
s += ftoa(eyInt,s);
*s++ = ' ';
s += ftoa(ezInt,s);
*s++ = '\n';
*s = '\0';
}
}
PROCESS_END();
}
