C++ MPU6050类代码示例

// FUNCION MAIN
int main () {

    // Parar con control-c
    signal(SIGINT, my_function);

    // Declaramos las dos estructuras y la clase acelerometro para su control
    Espacio aceleracion, gyroscocion;
    MPU6050 acelerometro;

    // Conectamos con el acelerometro y pedimos datos
    acelerometro.conectamos_acelerometro();


    while (true){
        if(ctrlc){
            whiler = false;
        }
        aceleracion = acelerometro.get_aceleraciones();
        gyroscocion = acelerometro.get_giroscopio();
        printf("Aceleracion: [%d, %d, %d]\n", aceleracion.x, aceleracion.y, aceleracion.z);
        printf("Giroscopo: [%d, %d, %d]\n", gyroscocion.x, gyroscocion.y, gyroscocion.z); 
    }


}

void setup() {

  Wire.begin();  

  if (debugSerial){
    Serial.begin(115200);
    Serial.println(F("===================  SETUP ================="));
  } 

  // initialize device
  if (debugSerial && debugMPU6050) Serial.println(F("Initializing I2C devices..."));
  accelgyro.initialize();

  // verify connection
  if (debugSerial && debugMPU6050) {
    Serial.println("Testing device connections...");
    boolean OK = accelgyro.testConnection() ;
    ( OK )? 
      Serial.println(F("MPU6050 connection successful")): 
      Serial.println(F("MPU6050 connection failed"));
  }

  if (debugSerial){
    Serial.println(F("=============== FIM  SETUP ================="));
  } 

}

void setup() {
    // initialize device
    printf("Initializing I2C devices...\n");
    accelgyro.initialize();
    gettimeofday(&tv0, NULL);

    // verify connection
    printf("Testing device connections...\n");
    printf(accelgyro.testConnection() ? "MPU6050 connection successful\n" : "MPU6050 connection failed\n");
}

void setup() {
  Serial.begin(38400);

  Serial.print("Device type:   0x");Serial.println(HW_TYPE,HEX,8);
  Serial.print("Device serial: 0x");Serial.println(HW_SERIAL,HEX,8);
  //Dump source package to serial
  int len=source_end-source_start;
  char* base=source_start;
  Serial.print("Source package length: ");
  Serial.println(len,DEC);
  Serial.print("Source package start: 0x");
  Serial.println((int)base,DEC,8);
  d.begin();
  while(len>0) {
    d.line(base,0,len>120?120:len);
    base+=120;
    len-=120;
  }
  d.end();

  Wire1.begin();
  SPI1.begin(1000000,1,1);

  bool worked=sd.begin();
  Serial.print("sd");    Serial.print(".begin ");Serial.print(worked?"Worked":"didn't work");Serial.print(". Status code ");Serial.println(sd.errnum);

  worked=p.begin(1);
  Serial.print("p");     Serial.print(".begin ");Serial.print(worked?"Worked":"didn't work");Serial.print(". Status code ");Serial.println(p.errnum);

  worked=fs.begin();  
  Serial.print("fs");    Serial.print(".begin ");Serial.print(worked?"Worked":"didn't work");Serial.print(". Status code ");Serial.println(fs.errnum);
  sector.begin();
  fs.print(Serial,sector);

  mpu6050.begin();
  Serial.print("MPU6050 identifier (should be 0x68): 0x");
  Serial.println(mpu6050.whoami(),HEX);

  hmc5883.begin();
  char HMCid[4];
  hmc5883.whoami(HMCid);
  Serial.print("HMC5883L identifier (should be 'H43'): ");
  Serial.println(HMCid);

  worked=ad799x.begin(0xB); //Turn on channels 0, 1, and 3
  Serial.print("ad799x");Serial.print(".begin ");Serial.print(worked?"Worked":"didn't work");Serial.print(". Status code ");Serial.println(0);

  worked=bmp180.begin();
  Serial.print("bmp180");Serial.print(".begin ");Serial.print(worked?"Worked":"didn't work");Serial.print(". Status code ");Serial.println(0);
  Serial.print("BMP180 identifier (should be 0x55): 0x");
  Serial.println(bmp180.whoami(),HEX);

  bmp180.printCalibration(&Serial);

}

bool i2cSetupGyro()
{
    //wake up gyro
	//return I2Cdev::writeBit( gyroAddr, 0x6b, 6, 0b0);
	gyroscope.initialize();
	return gyroscope.testConnection();
}

void setup() {
  Wire.begin();
  Serial.begin(57600);
  I2Cdev::writeByte(0x68, 0x6B, 0x01); //PWR_MGMT1 for clock source as X-gyro
  uint8_t temp[8] = {8, 0};//GYRO range:250, ACCEL range:2g | Refer the Datasheet if you want to change these.
  I2Cdev::writeBytes(0x68, 0x1B, 2, temp); //GYRO_CFG and ACCEL_CFG

  accelgyro.setRate(4);
  accelgyro.setDLPFMode(0x03);
  accelgyro.setFIFOEnabled(true);
  I2Cdev::writeByte(0x68, 0x23, 0x78); //FIFO_EN for ACCEL,GYRO
  accelgyro.setDMPEnabled(false);
  I2Cdev::writeByte(0x68, 0x38, 0x11); //INT_EN
  
  attachInterrupt(0, mpu_interrupt, RISING);
  attachInterrupt(1, compass_interrupt, FALLING);  

  //Put the HMC5883 IC into the correct operating mode
  Wire.beginTransmission(0x1E); //open communication with HMC5883
  Wire.write(0x00); //select Config_Register_A: 
  Wire.write(0x58); //4-point avg. and 75Hz rate
  Wire.write(0x02); //In Config_Register_B: +-1.9G (820LSB/G)
  Wire.write(0x00); //In Mode_Register: continuous measurement mode
  Wire.endTransmission();
  pinMode(13, INPUT);
  #ifdef CAL_DEBUG
    Serial.print("Calibrating Gyros and Accel! Hold Still and Level!");
  #endif
  calibrate_gyros();
  calibrate_accel();
  accelgyro.resetFIFO();
}

int ImuTester::run()
{
	// Default is fail unless pass critera met
	m_pass = TEST_FAIL;

	// Register the driver
	int ret = m_sensor.init();

	// Open the IMU sensor
	DevHandle h;
	DevMgr::getHandle(IMU_DEVICE_PATH, h);

	if (!h.isValid()) {
		DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)",
			    IMU_DEVICE_PATH, h.getError());
		m_done = true;

	} else {
		m_done = false;
	}

	while (!m_done) {
		++m_read_attempts;

		struct imu_sensor_data data;

		ret = ImuSensor::getSensorData(h, data, true);

		if (ret == 0) {
			uint32_t count = data.read_counter;
			DF_LOG_INFO("count: %d", count);

			if (m_read_counter != count) {
				m_read_counter = count;
				ImuSensor::printImuValues(h, data);
			}

		} else {
			DF_LOG_INFO("error: unable to read the IMU sensor device.");
		}

		if (m_read_counter >= num_read_attempts) {
			// Done test - PASSED
			m_pass = TEST_PASS;
			m_done = true;

		} else if (m_read_attempts > num_read_attempts) {
			DF_LOG_INFO("error: unable to read the IMU sensor device.");
			m_done = true;
		}
	}

	DevMgr::releaseHandle(h);

	DF_LOG_INFO("Closing IMU sensor");
	m_sensor.stop();
	return m_pass;
}

void setup()
{
    // join I2C bus (I2Cdev library doesn't do this automatically)
    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
        Wire.begin();
        //Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
    #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
        Fastwire::setup(400, true);
    #endif

    // initialize serial communication
    Serial.begin(38400);
    while (!Serial);

    mpu.initialize();
    pinMode(INTERRUPT_PIN, INPUT);

	send_status(MPU_INITIALIZE, STATUS_OK);

    // verify connection
	send_status(MPU_CONNECTION, mpu.testConnection() ? STATUS_OK : STATUS_FAIL);

    // load and configure the DMP
    // 0 = DMP OK
    // 1 = initial memory load failed
    // 2 = DMP configuration updates failed
    ua_dev_status = mpu.dmpInitialize();
	send_status(DMP_INITIALIZE, ua_dev_status);

    // supply your own gyro offsets here, scaled for min sensitivity
    mpu.setXGyroOffset(120);
    mpu.setYGyroOffset(76);
    mpu.setZGyroOffset(-185);
    mpu.setZAccelOffset(1688); // 1688 factory default for my test chip

    // make sure it worked (returns 0 if so)
    if (ua_dev_status == 0)
	{
        // turn on the DMP, now that it's ready
        mpu.setDMPEnabled(true);

        // enable Arduino interrupt detection
        attachPinChangeInterrupt(INTERRUPT_PIN, dmpDataReady, RISING);
        ua_mpu_interrupt_status = mpu.getIntStatus();
        send_status(DMP_INTERRUPT, ua_mpu_interrupt_status);

        b_dmp_ready = true;

        // get expected DMP packet size for later comparison
        uh_packet_size = mpu.dmpGetFIFOPacketSize();
    }

    // configure LED for output
    pinMode(LED_PIN, OUTPUT);
}

/* 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]);
}

void setup_IMU() 
{
	I2CInitialize();
	mpu.initialize();
	devStatus = mpu.dmpInitialize();
	if (devStatus == 0) 
	{
		mpu.setDMPEnabled(true);
		mpuIntStatus = mpu.getIntStatus();
		dmpReady = true;
		packetSize = mpu.dmpGetFIFOPacketSize();
	} 
}

void update_IMU()
{
	mpuIntStatus = mpu.getIntStatus();
	// get current FIFO count
	fifoCount = mpu.getFIFOCount();
	// check for overflow (this should never happen unless our code is too inefficient)
	if ((mpuIntStatus & 0x10) || fifoCount == 1024)	mpu.resetFIFO();
	// otherwise, check for DMP data ready interrupt (this should happen frequently)
	else if (mpuIntStatus & 0x02) 
	{
		// wait for correct available data length, should be a VERY short wait
		while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
		// read a packet from FIFO
		mpu.getFIFOBytes(fifoBuffer, packetSize);
		// track FIFO count here in case there is > 1 packet available
		// (this lets us immediately read more without waiting for an interrupt)
		fifoCount -= packetSize;
		mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetEuler(euler, &q);
		mpu.dmpGetGyro(gyroRate,fifoBuffer);
		gyro_rate_float[0] = (float)gyroRate[0]/2147483648*2000*0.41;
		gyro_rate_float[1] = (float)gyroRate[1]/2147483648*2000*0.41;
		gyro_rate_float[2] = (float)gyroRate[2]/2147483648*2000*0.41;
	}
}

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 setup_IMU() 
{
	tmObjectInit(&lagedatalogsync_tmup);
	I2CInitialize();
	mpu.initialize();
	devStatus = mpu.dmpInitialize();
	if (devStatus == 0) 
	{
		//chThdCreateStatic(LageSyncThreadWorkingArea, sizeof(LageSyncThreadWorkingArea), NORMALPRIO, LageSyncthread, NULL);
		mpu.setDMPEnabled(true);
		mpuIntStatus = mpu.getIntStatus();
		dmpReady = true;
		packetSize = mpu.dmpGetFIFOPacketSize();
	} 
}

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

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