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

/* Get readings from IMU and compute them to get the angles */
void computeIMU(){
    
    /* Get Accel and Gyro readings */
    IMU.getMotion6(&accel[0], &accel[1], &accel[2], &gyro[0], &gyro[1], &gyro[2]);    
    
    /* Apply offsets */
    for( int i=0 ; i<3 ; i++ ){
        accel[i] += accelOffsets[i];
        gyro[i] += gyroOffsets[i];
    }
    
    /* Refresh time variables */
    delta = micros() - auxtime;
    auxtime = delta + auxtime;
    
    if( delta > maxdelta ) maxdelta = delta;
    compute += delta;
    times++;
    if( times == 100 ){
        avgDelta = compute/100;
        times = 0;
        compute = 0;
    }    
    
    /* Compute offsets if needed */
    if( !gyroOffReady[0] || !gyroOffReady[1] || !gyroOffReady[2] )
        computeGyroOffsets();
    
    if( !accelOffReady[0] || !accelOffReady[1] || !accelOffReady[2] )
        computeAccelOffsets();
}

void loop() {
    // read raw accel/gyro measurements from device
    accelgyro.getMotion6(&accData[0], &accData[1], &accData[2], &gyrData[0], &gyrData[1], &gyrData[2]);
    CalculatedT();
    ComplementaryFilter(accData, gyrData, &final_pitch, &final_roll);
    printf("pitch = %6f  roll %6f dt = %lu\n", final_pitch, final_roll, elapsed);
}

void CalculateOffsets(uint8_t gyroSamplingRate)
{
	int numReadings = 0;
	long base_x_gyro = 0;
	long base_y_gyro = 0;
	long base_z_gyro = 0;
	long base_x_accel = 0;
	long base_y_accel = 0;
	long base_z_accel = 0;

	int16_t ax, ay, az, gx, gy, gz;

	delay(100);
	// Discard the first few reading
	unsigned long before = millis();
	unsigned long now;
	do
	{
		mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
		now = millis();
	}
	while(now - before < 500/(gyroSamplingRate+1)); // ignore 500 frames of data

	before = millis();

	do
	{
		mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
	//	PrintMotion6Data(ax, ay, az, gx, gy, gz);
		base_x_gyro += gx;
		base_y_gyro += gy;
		base_z_gyro += gz;
		base_x_accel += ax;
		base_y_accel += ay;
		base_z_accel += az;
		numReadings++;
		now = millis();
	}
	while(now - before < 1000/(gyroSamplingRate+1)); // collect average over 1000 frames of data
	base_x_gyro /= numReadings; 	gXOffset = base_x_gyro;
	base_y_gyro /= numReadings;		gYOffset = base_y_gyro;
	base_z_gyro /= numReadings;		gZOffset = base_z_gyro;
	base_x_accel /= numReadings;	aXOffset = base_x_accel;
	base_y_accel /= numReadings;	aYOffset = base_y_accel;
	base_z_accel /= numReadings;	aZOffset = base_z_accel;

}

bool GetMotion6(float angles[])
{
	int16_t 	accX, accY, accZ;
	int16_t		gyroX, gyroY, gyroZ;
	mpu.getMotion6(&accX, &accY, &accZ, &gyroX, &gyroY, &gyroZ);  //400µs
	angles[0] = accX; angles[1] = accY; angles[2] = accZ; angles[3] = gyroX; angles[4] = gyroY; angles[5] = gyroZ;
	return true;
}

void loop() {
    // read raw accel/gyro measurements from device
    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

    // these methods (and a few others) are also available
    //accelgyro.getAcceleration(&ax, &ay, &az);
    //accelgyro.getRotation(&gx, &gy, &gz);

    // display accel/gyro x/y/z values
    printf("a/g: %6hd %6hd %6hd   %6hd %6hd %6hd\n",ax,ay,az,gx,gy,gz);
}

/* Initialize the IMU and I2C communication */
void IMUInit(){

    /* Intialize communication I2C */
    Wire.begin();
    
    /* Initialize IMU. Default resolution is set to minimum, no offsets */
    IMU.initialize();

    /* Get first values for accel and gyro */
    IMU.getMotion6(&accel[0], &accel[1], &accel[2], &gyro[0], &gyro[1], &gyro[2]);
}

JNIEXPORT void JNICALL Java_com_nhl_spindp_sensors_I2C_loopI2c
  (JNIEnv *env, jobject thisObj)
{
	jclass dataCls = env->FindClass("com/nhl/spindp/sensors/I2C$I2CData");
	if (env->ExceptionCheck()) return;
	jfieldID dataField = env->GetFieldID( env->GetObjectClass(thisObj), "data", "Lcom/nhl/spindp/sensors/I2C$I2CData;");
	if (env->ExceptionCheck()) return;
	jobject dataObj = env->GetObjectField( thisObj, dataField);
	if (env->ExceptionCheck()) return;

	if(adc)
	{
		jfieldID adcVal0Field = env->GetFieldID( dataCls, "adcVal0", "S");
		jfieldID adcVal1Field = env->GetFieldID( dataCls, "adcVal1", "S");
		if (env->ExceptionCheck()) return;
		short spanning = readADC(0); //spanning
		short stroom = readADC(1); //stroom
		env->SetShortField( dataObj, adcVal0Field, spanning);
		env->SetShortField( dataObj, adcVal1Field, stroom);
		if (env->ExceptionCheck()) return;
	}

	if(gyro)
	{
		jfieldID accDataXField = env->GetFieldID( dataCls, "accDataX", "S");
		jfieldID accDataYField = env->GetFieldID( dataCls, "accDataY", "S");
		jfieldID accDataZField = env->GetFieldID( dataCls, "accDataZ", "S");
		//jfieldID tmpField      = env->GetFieldID(dataCls, "tmp", "S");
		jfieldID gyroXField    = env->GetFieldID( dataCls, "gyroX", "S");
		jfieldID gyroYField    = env->GetFieldID( dataCls, "gyroY", "S");
		jfieldID gyroZField    = env->GetFieldID( dataCls, "gyroZ", "S");
		if (env->ExceptionCheck()) return;
		
		int16_t gx, gy, gz, ax, ay, az;

		gyroscope.getMotion6( &ax, &ay, &az, &gx, &gy, &gz);

		env->SetShortField( dataObj, accDataXField, (short) ax);//readWord(0x68,0x3B)
		env->SetShortField( dataObj, accDataYField, (short) ay);//readWord(0x68,0x3D)
		env->SetShortField( dataObj, accDataZField, (short) az);//readWord(0x68,0x3F)
		//env->SetShortField(dataObj,      tmpField, (result[ 6] << 8) | result[ 7]);
		env->SetShortField( dataObj,    gyroXField, (short) gx);//readWord(0x68,0x43)
		env->SetShortField( dataObj,    gyroYField, (short) gy);//readWord(0x68,0x45)
		env->SetShortField( dataObj,    gyroZField, (short) gz);//readWord(0x68,0x47)
		if (env->ExceptionCheck()) return;
	}
	env->SetObjectField( thisObj, dataField, dataObj);
	if (env->ExceptionCheck()) return;
}

void compute_imu(float _looptime) {
  looptime = _looptime;
  // get sensor readings
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  gx -= imu_offset[3];
  gy -= imu_offset[4];
  gz -= imu_offset[5];

  mx = mpu.getExternalSensorWord(0);
  my = mpu.getExternalSensorWord(2);
  mz = mpu.getExternalSensorWord(4);

  /* computations */
  compute_state_imu();
}

void atualizaSensors(int deltaT){

  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  /* Calcula os angulos (em graus) de X e Y  */
  accYangle = (atan2(ax,az)+PI)*RAD_TO_DEG;
  accXangle = (atan2(ay,az)+PI)*RAD_TO_DEG; 
  
  double gyroXrate =  (double)gx/131.0;
  double gyroYrate = -((double)gy/131.0);
  
  /* Calcula os angulos X e Y do Giroscopio*/  
  gyroXangle += gyroXrate*((double)deltaT/1000);  
  gyroYangle += gyroYrate*((double)deltaT/1000);
  
  /* Aplica o Filtro Complementar*/
  compAngleX = (0.93*(compAngleX+(gyroXrate*(double)deltaT/1000)))+(0.07*accXangle); 
  compAngleY = (0.93*(compAngleY+(gyroYrate*(double)deltaT/1000)))+(0.07*accYangle);  

  // DEBUG //
  if (debugSerial && debugMPU6050){
    Serial.print(accXangle);
    Serial.print("\t");
    Serial.print(accYangle);
    Serial.print("\t"); 

    Serial.print(gyroXangle);
    Serial.print("\t");
    Serial.print(gyroYangle);
    Serial.print("\t");

    Serial.print(compAngleX);
    Serial.print("\t");
    Serial.print(compAngleY); 
    Serial.print("\t");
  }
  delay(1);
}

// This function reads acceleration form the MPU 6050 and returns impacts
// 255-1000;
void Accel_read(int *Accel)
{
	int16_t ax, ay, az;
	int16_t gx, gy, gz;
	static int x;
	
	static int lax, lay, laz;	
	int s1,s2,s3;
	
    // read raw accel/gyro measurements from device
    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

    // display accel/gyro x/y/z values
	s1= abs(ax-lax) / 100;
	s2 = abs(ay-lay) / 100;
	s3 = abs(az-laz) / 100 ;

	//get maxval
	if((s1 > s2) && (s1 > s3))	
		*Accel = s1;
	else if((s2 > s1) && (s1 > s3))	
		*Accel = s2;
	else
		*Accel =s3;

	//fflush(stdout);		
	//if(max > 254)
	//	printf("\n%d",max);
	
	//update last vals
	laz = az;
	lay = ay;
	lax = ax;
	
	// > 255 would be a servere jolt
	LimitInt(Accel,0,1000);
	
}//END ACCEL_Read

void calibrate_imu(){
   delay(1000); // allow IMU to settle
   
   int total = 300;
   long imu_total[6] = {0, 0, 0, 0, 0, 0};
   int count = 0;
   int i;
   while(count <= total){
      count++;
      mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
      imu_total[0] += ax;
      imu_total[1] += ay;
      imu_total[2] += az;
      imu_total[3] += gx;
      imu_total[4] += gy;
      imu_total[5] += gz;
   }
   Serial.print("Calibrated offsets :");
   for(i = 0; i < 6; i++){
    imu_offset[i] = imu_total[i] / total;
    Serial.print(imu_offset[i]); Serial.print(" ");
   }
   Serial.println("");
}

int main(int argc, char const *argv[])
{
    AccelVector gravity;
    MPU6050 accelgyro;
    accelgyro.initialize();
    accelgyro.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
    int16_t 
        x=-3940,
        y=-0110,
        z=00326;
    int16_t 
        gx,
        gy,
        gz;
    // accelgyro.getAcceleration(&x, &y,&z);
    gravity.x = x; gravity.y = y; gravity.z = z;
    float alpha = 0.5f, gyralpha = 0.9f;
    // for(int i=0; i<100; i++){
    //     accelgyro.getAcceleration(&x, &y,&z);
    //     gravity.x = gravity.x * alpha + x * (1 - alpha);
    //     gravity.y = gravity.y * alpha + y * (1 - alpha);
    //     gravity.z = gravity.z * alpha + z * (1 - alpha);
    //     usleep(50000);
    // }
    AccelVector vec, gyr;

        accelgyro.getMotion6(&x, &y,&z, &gx, &gy, &gz);
    gyr.x = gx; gyr.y = gy; gyr.z = gz;
    while(1)
    {
        accelgyro.getMotion6(&x, &y,&z, &gx, &gy, &gz);
        //accelgyro.getAcceleration(&x, &y,&z,);
        vec.x = vec.x * alpha + x * (1 - alpha);
        vec.y = vec.y * alpha + y * (1 - alpha);
        vec.z = vec.z * alpha + z * (1 - alpha);

        gyr.x = gyr.x * gyralpha + gx * (1 - gyralpha);
        gyr.y = gyr.y * gyralpha + gy * (1 - gyralpha);
        gyr.z = gyr.z * gyralpha + gz * (1 - gyralpha);
        
        printf("Total gforce now %.3f g's gyro len %.3f | Now: %05.0f %05.0f %05.0f Gyro: %05.0f %05.0f %05.0f\n", 
            vec.getMagnitude()/16000, gyr.getMagnitude(), vec.x, vec.y, vec.z, gyr.x, gyr.y, gyr.z);

        usleep(50000);
    }

        // MPU control/status vars
	/*bool dmpReady = false;  // set true if DMP init was successful
	uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
	uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
	uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
	uint16_t fifoCount;     // count of all bytes currently in FIFO
	uint8_t fifoBuffer[64]; // FIFO storage buffer

	// orientation/motion vars
	Quaternion q;           // [w, x, y, z]         quaternion container
	VectorInt16 aa;         // [x, y, z]            accel sensor measurements
	VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
	VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
	VectorFloat gravity;    // [x, y, z]            gravity vector
	float euler[3];         // [psi, theta, phi]    Euler angle container
	float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
	MPU6050 accelgyro;
    accelgyro.initialize();
    // load and configure the DMP
    printf("Initializing DMP...\n");
    devStatus = accelgyro.dmpInitialize();
    
    // make sure it worked (returns 0 if so)
    if (devStatus == 0) {
        // turn on the DMP, now that it's ready
        printf("Enabling DMP...\n");
        accelgyro.setDMPEnabled(true);

        // enable Arduino interrupt detection
        //Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
        //attachInterrupt(0, dmpDataReady, RISING);
        mpuIntStatus = accelgyro.getIntStatus();

        // set our DMP Ready flag so the main loop() function knows it's okay to use it
        printf("DMP ready!\n");
        dmpReady = true;

        // get expected DMP packet size for later comparison
        packetSize = accelgyro.dmpGetFIFOPacketSize();
    } else {
        // ERROR!
        // 1 = initial memory load failed
        // 2 = DMP configuration updates failed
        // (if it's going to break, usually the code will be 1)
        printf("DMP Initialization failed (code %d)\n", devStatus);
        return 0;
    }

    while(1){
        fifoCount = accelgyro.getFIFOCount();

        if (fifoCount == 1024) {
            // reset so we can continue cleanly
            accelgyro.resetFIFO();
//.........这里部分代码省略.........

// Acquire a data record.
void acquireData(data_t* data) {
  data->time = micros();
  mpu.getMotion6(&data->ax, &data->ay, &data->az, 
                 &data->gx, &data->gy, &data->gz);
}

bool IntegrateGyro()
{
	// Set the full scale range of the gyro
	uint8_t FS_SEL = 0;
	int16_t 	accX, accY, accZ;
	int16_t		gyroX, gyroY, gyroZ;
	mpu.getMotion6(&accX, &accY, &accZ, &gyroX, &gyroY, &gyroZ);  //Set Starting angles
	unsigned long now = millis();
	float dt =(now - Before)/1000.0;
	Before = now;
	//mpu.setFullScaleGyroRange(FS_SEL);

	// get default full scale value of gyro - may have changed from default
	// function call returns values between 0 and 3
	uint8_t READ_FS_SEL = mpu.getFullScaleGyroRange();
//	Serial.print("FS_SEL = ");
//	Serial.println(READ_FS_SEL);
	GYRO_FACTOR = 131.0/(READ_FS_SEL + 1);


	// get default full scale value of accelerometer - may not be default value.
	// Accelerometer scale factor doesn't reall matter as it divides out
	uint8_t READ_AFS_SEL = mpu.getFullScaleAccelRange();
//	Serial.print("AFS_SEL = ");
//	Serial.println(READ_AFS_SEL);
	//ACCEL_FACTOR = 16384.0/(AFS_SEL + 1);


	// Remove offsets and scale gyro data
	float fgyroX, fgyroY, fgyroZ;

	fgyroX = (gyroX - gXOffset)/GYRO_FACTOR;
	fgyroY = -(gyroY - gYOffset)/GYRO_FACTOR;
	fgyroZ = -(gyroZ - gZOffset)/GYRO_FACTOR;
	accX = accX; // - base_x_accel;
	accY = accY; // - base_y_accel;
	accZ = accZ; // - base_z_accel;

	const double Q_angle = 0.001;

	const double Q_gyroBias = 0.003;

	const double R_angle = 0.03;

	AccelAngleY = atan2(accX, sqrt(pow(accY,2) + pow(accZ,2)))*RADIANS_TO_DEGREES;
	AccelAngleX = atan2(accY, sqrt(pow(accX,2) + pow(accZ,2)))*RADIANS_TO_DEGREES;
	AccelAngleZ = accZ;


#ifdef UNFILTERED
	// Compute the (filtered) gyro angles
	fAngleX = fgyroX*dt + fLastGyroAngleX;
	fAngleY = fgyroY*dt + fLastGyroAngleY;
	fAngleZ = fgyroZ*dt + fLastGyroAngleZ;
#endif

#ifdef FILTERED
	// Apply the complementary filter to figure out the change in angle - choice of alpha is
	// estimated now.  Alpha depends on the sampling rate...
	const float alpha = 0.9;
	float gyroAngleX = fgyroX*dt + fLastGyroAngleX;
	float gyroAngleY = fgyroY*dt + fLastGyroAngleY;
	float gyroAngleZ = fgyroZ*dt + fLastGyroAngleZ;
	fAngleX = alpha*gyroAngleX + (1.0 - alpha)*AccelAngleX;
	fAngleY = alpha*gyroAngleY + (1.0 - alpha)*AccelAngleY;
	fAngleZ = gyroAngleZ;  //Accelerometer doesn't give z-angle
#endif
	fLastGyroAngleX = fAngleX; fLastGyroAngleY = fAngleY; fLastGyroAngleZ = fAngleZ;
	return true;
}

int main(int argc, char **argv) {
  printf("MPU6050 3-axis acceleromter example program\n");
  I2Cdev::initialize();
  MPU6050 accelgyro ;
  int16_t ax, ay, az;
  int16_t gx, gy, gz;
  
  accelgyro.initialize();

  if ( accelgyro.testConnection() ) 
    printf("MPU6050 connection test successful\n") ;
  else {
    fprintf( stderr, "MPU6050 connection test failed! something maybe wrong, continuing anyway though ...\n");
    //return 1;
  }
  // use the code below to change accel/gyro offset values
  /*
  printf("Updating internal sensor offsets...\n");
  // -76	-2359	1688	0	0	0
  printf("%i \t %i \t %i \t %i \t %i \t %i\n", 
	 accelgyro.getXAccelOffset(),
	 accelgyro.getYAccelOffset(),
	 accelgyro.getZAccelOffset(),
	 accelgyro.getXGyroOffset(),
	 accelgyro.getYGyroOffset(),
	 accelgyro.getZGyroOffset());
  accelgyro.setXGyroOffset(220);
  accelgyro.setYGyroOffset(76);
  accelgyro.setZGyroOffset(-85);
  printf("%i \t %i \t %i \t %i \t %i \t %i\n", 
	 accelgyro.getXAccelOffset(),
	 accelgyro.getYAccelOffset(),
	 accelgyro.getZAccelOffset(),
	 accelgyro.getXGyroOffset(),
	 accelgyro.getYGyroOffset(),
	 accelgyro.getZGyroOffset());
  */
  
  printf("\n");
  printf("  ax \t ay \t az \t gx \t gy \t gz:\n");
  while (true) {
    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
//    printf("  %d \t %d \t %d \t %d \t %d \t %d\n", ax, ay, az, gx, gy, gz);

//    accelgyro.getAcceleration(&ax, &ay, &az);
//    printf("  %d \t %d \t %d \r", ax, ay, az);
    float axs = ax/16384.0;
    float ays = ay/16384.0;
    float azs = az/16384.0;

    float dxz = sqrt( axs*axs+azs*azs);
    float dyz = sqrt( ays*ays+azs*azs);

    float rotX = atan2(axs,dyz);
    float rotY = atan2(ays,dxz);
    printf("  %f \t %f \r", 180*rotX/3.14159, 180*rotY/3.14159);

    fflush(stdout);
//    fflush(stdout);
    bcm2835_delay(100);
  }
  return 1; 
}