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(); }