Exemple #1
0
void *imuUpdate(void *arg) {
    
    //struct timeval t0, t1;
    //float elapsed;
    
    struct sched_param param = {.sched_priority = 15};
    sched_setscheduler(0, SCHED_RR, &param);
    
    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);

    }

}
Exemple #2
0
void FreeIMU1::getYawPitchRoll(float * ypr) {
  int i;
  double dYPR[3];
  
  getYawPitchRoll(dYPR);
  for (i = 0; i < 3; i++)
    ypr[i] = (float)dYPR[i];
}
Exemple #3
0
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];
}
Exemple #4
0
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;
}