C++ MPU6050::getRotation方法代码示例

void calibrate_gyros(){
  byte i=0;
  while (i < ITERATIONS){ //hope that offsets converge in 6 iterations
    accelgyro.getRotation(&gx, &gy, &gz);
    if (count == SAMPLE_COUNT){
      xoff += int(gxm/-3);
      yoff += int(gym/-3);
      zoff += int(gzm/-3);
      accelgyro.setXGyroOffset(xoff);
      accelgyro.setYGyroOffset(yoff);
      accelgyro.setZGyroOffset(zoff);
      #ifdef CAL_DEBUG
        Serial.print(gxm); Serial.print(" ");
        Serial.print(gym); Serial.print(" ");
        Serial.println(gzm);
        Serial.print(xoff); Serial.print(" ");
        Serial.print(yoff); Serial.print(" ");
        Serial.println(zoff);
        Serial.println("*********************");
      #endif
      count = 0;
      i++; //iteration++
      #ifdef CAL_DEBUG
        Serial.print(".");
      #endif
    }
    else{
      gxm = (gxm*count + gx)/(count+1.0);
      gym = (gym*count + gy)/(count+1.0);
      gzm = (gzm*count + gz)/(count+1.0);
      count++;
    }
  }
  #ifdef CAL_DEBUG
    Serial.println(" Done.");
  #endif
}

// PUBLIC METHOD TO PROVIDE DATA ACQUIRED FROM MPU TO DEPENDENT SYSTEMS
uint8_t mpuAcquire(int16_t *accelOffsetX, int16_t *accelOffsetY,int16_t *accelOffsetZ, int16_t *currAccelX, int16_t *currAccelY, int16_t *currAccelZ, int16_t *currYaw, int16_t *currPitch, int16_t *currRoll){ /*Changed to int16*/

int16_t rot[3]; //Equivalent to uint8_t yaw; uint8_t pitch; uint8_t roll;
int16_t accel[3];//Equivalent to uint8_t accelX; uint8_t accelY; uint8_t accelZ;
//Error flag
uint8_t errorCode;

//DEFAULT SEND 0's
for(uint8_t j=0; j<4;j++){
  accel[j]=0;
}

for(uint8_t k=0; k<4;k++){
  rot[k]=0;
}

//Monitor MPU
errorCode=mpuMonitor(currAccelX,currAccelY,currAccelZ);

//IF PROBLEM, SEND PREVIOUS VALUES
    if(errorCode!=0){
       accel[0]=*currAccelX;
       accel[1]=*currAccelY;
       accel[2]=*currAccelZ;

       rot[0]=*currYaw;
       rot[1]=*currPitch;
       rot[2]=*currRoll;
    }

//IF GOT VALUES, SUPPLY NEW VALUES
    else if(errorCode==0){

  // display raw acceleration values
    // Chosen getAcceleration method because only one showing approximately independent dimensions
    // ALL others having problems; linearAccelInWorld in particular only giving zeroes, and linearAccel giving all ~same
    #ifdef MPU_RAW_ACCEL
    mpu.getAcceleration(accel,accel+1,accel+2);
    accel[0]=(accel[0] - *accelOffsetX); // Components with offset applied
    accel[1]=(accel[1] - *accelOffsetY); // No scaling factor 2048, to maximize resolution in integer rep.
    accel[2]=(accel[2] - *accelOffsetZ);
    #endif

  // TO DO: WILL DETERMINE AXIS OF MAX DIFFERENCE BETWEEN accel AND currAccel VALUES...
  // AND DIVIDE OTHER AXES' VALUES BY DERIVATIVE (USING TIMER COUNT)
  
  // -OR- TAKE TIME AVERAGE OF BUFFERED VALUES AND DIVIDE OTHERS BY AXIS OF MAX RATE OF CHANGE
  
  // -OR- MAX/MIN HOLD VALUES ON AXIS AND FIND RATE OF CHANGE OF PEAKS. DIVIDE OTHER AXES BY MAX RATE OF CHANGE
  
  
    // display raw gyroscope values
    //Took raw values because all others not independent (and dmpGetGyro in particular not sensitive)
    #ifdef MPU_RAW_GYRO 
    mpu.getRotation(rot,rot+1,rot+2);
    /* rot[0]=(rot[0]/16.4); // No scaling of rotation, so as maximize resolution with integer
    rot[1]=(rot[1]/16.4);
    rot[2]=(rot[2]/16.4); */

      //Return absolute angle values, if desired
      /* #ifdef ABSOLUTE_ANGLE
      //Apply complementary filter to get
      Complementary_Filter(accel[0],accel[1],accel[2],rot[0],rot[1],rot[2]);
      #endif */
    
    #endif

    }


//Print out and update "previous" values to current ones
#ifdef MPU_RAW_ACCEL

/*  FOR DEBUG, SERIAL PRINT
  #ifdef accelAsComponents
    Serial.print(accel[0] + ", " + accel[1] + ", " + accel[2]));
  #endif
  #ifdef accelAsMag
        Serial.print(sqrt((abs(accel[0]))^2+(abs(accel[1]))^2+(abs(accel[2]))^2));
  #endif
*/

  *currAccelX=accel[0];
  *currAccelY=accel[1];
  *currAccelZ=accel[2];
#endif


#ifdef MPU_RAW_GYRO
  *currYaw=rot[0];
  *currPitch=rot[1];
  *currRoll=rot[2];
#endif


/* #ifdef GET_TAPS
    if(&(devAddr+TAPXYZ)){ //If tap received register goes high, send 1 tap indication
      Serial.print(", 1");
    }
//.........这里部分代码省略.........