/* * Updates function * */ extern "C" void sf_MPU6050_2xDriver_GxAyz_Update_wrapper(const int16_T *x_vel, const int16_T *y_acc, const int16_T *z_acc, const int16_T *x_vel_2, const int16_T *y_acc_2, const int16_T *z_acc_2, real_T *xD) { /* %%%-SFUNWIZ_wrapper_Update_Changes_BEGIN --- EDIT HERE TO _END */ if(xD[0] != 1){ # ifndef MATLAB_MEX_FILE Wire.begin(); accelgyro.initialize(); accelgyro.setDLPFMode(MPU6050_DLPF_BW_42); accelgyro.setFullScaleGyroRange(MPU6050_GYRO_FS_1000); accelgyro2.initialize(); accelgyro2.setDLPFMode(MPU6050_DLPF_BW_42); accelgyro2.setFullScaleGyroRange(MPU6050_GYRO_FS_1000); #endif //done with initialization xD[0] = 1; } /* %%%-SFUNWIZ_wrapper_Update_Changes_END --- EDIT HERE TO _BEGIN */ }
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(); }
void initialize_imu() { // join I2C bus (I2Cdev library doesn't do this automatically) #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE Wire.begin(); // ************************************************************** // It is best to configure I2C to 400 kHz. // If you are using an Arduino DUE, modify the variable TWI_CLOCK to 400000, defined in the file: // c:/Program Files/Arduino/hardware/arduino/sam/libraries/Wire/Wire.h // If you are using any other Arduino instead of the DUE, uncomment the following line: //TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz) //This line should be commented if you are using Arduino DUE // ************************************************************** #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE Fastwire::setup(400, true); #endif // initialize serial communication Serial.begin(250000); // initialize device Serial.println(F("Initializing I2C devices...")); mpu.initialize(); // verify connection Serial.println(F("Testing device connections...")); Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed")); // TODO: Compute these parameters // mpu.setXAccelOffset(-1600); // mpu.setYAccelOffset(-180); // mpu.setZAccelOffset(650); // mpu.setXGyroOffset(0); // mpu.setYGyroOffset(0); // mpu.setZGyroOffset(0); mpu.setFullScaleGyroRange(0); calibrate_imu(); // Magnetometer configuration mpu.setI2CMasterModeEnabled(0); mpu.setI2CBypassEnabled(1); Wire.beginTransmission(HMC5883L_DEFAULT_ADDRESS); Wire.write(0x02); Wire.write(0x00); // Set continuous mode Wire.endTransmission(); delay(5); Wire.beginTransmission(HMC5883L_DEFAULT_ADDRESS); Wire.write(0x00); Wire.write(B00011000); // 75Hz Wire.endTransmission(); delay(5); mpu.setI2CBypassEnabled(0); // X axis word mpu.setSlaveAddress(0, HMC5883L_DEFAULT_ADDRESS | 0x80); // 0x80 turns 7th bit ON, according to datasheet, 7th bit controls Read/Write direction mpu.setSlaveRegister(0, HMC5883L_RA_DATAX_H); mpu.setSlaveEnabled(0, true); mpu.setSlaveWordByteSwap(0, false); mpu.setSlaveWriteMode(0, false); mpu.setSlaveWordGroupOffset(0, false); mpu.setSlaveDataLength(0, 2); // Y axis word mpu.setSlaveAddress(1, HMC5883L_DEFAULT_ADDRESS | 0x80); mpu.setSlaveRegister(1, HMC5883L_RA_DATAY_H); mpu.setSlaveEnabled(1, true); mpu.setSlaveWordByteSwap(1, false); mpu.setSlaveWriteMode(1, false); mpu.setSlaveWordGroupOffset(1, false); mpu.setSlaveDataLength(1, 2); // Z axis word mpu.setSlaveAddress(2, HMC5883L_DEFAULT_ADDRESS | 0x80); mpu.setSlaveRegister(2, HMC5883L_RA_DATAZ_H); mpu.setSlaveEnabled(2, true); mpu.setSlaveWordByteSwap(2, false); mpu.setSlaveWriteMode(2, false); mpu.setSlaveWordGroupOffset(2, false); mpu.setSlaveDataLength(2, 2); mpu.setI2CMasterModeEnabled(1); mpu.setDLPFMode(6); }
//PROGRAM FUNCTIONS void setup_mpu6050(){ clear_i2c(); Wire.begin(); SERIAL_OUT.println("Initializing gyro..."); accelgyro.initialize(); //accelgyro.reset(); accelgyro.setSleepEnabled(false); // thanks to Jack Elston for pointing this one out! // verify connection SERIAL_OUT.println("Testing device connections..."); SERIAL_OUT.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed"); SERIAL_OUT.println(F("Setting clock source to Z Gyro...")); accelgyro.setClockSource(MPU6050_CLOCK_PLL_ZGYRO); //SERIAL_OUT.println(accelgyro.getClockSource(MPU6050_CLOCK_PLL_ZGYRO); SERIAL_OUT.println(F("Setting sample rate to 200Hz...")); accelgyro.setRate(0); // 1khz / (1 + 4) = 200 Hz // * | ACCELEROMETER | GYROSCOPE // * DLPF_CFG | Bandwidth | Delay | Bandwidth | Delay | Sample Rate // * ---------+-----------+--------+-----------+--------+------------- // * 0 | 260Hz | 0ms | 256Hz | 0.98ms | 8kHz // * 1 | 184Hz | 2.0ms | 188Hz | 1.9ms | 1kHz // * 2 | 94Hz | 3.0ms | 98Hz | 2.8ms | 1kHz // * 3 | 44Hz | 4.9ms | 42Hz | 4.8ms | 1kHz // * 4 | 21Hz | 8.5ms | 20Hz | 8.3ms | 1kHz // * 5 | 10Hz | 13.8ms | 10Hz | 13.4ms | 1kHz // * 6 | 5Hz | 19.0ms | 5Hz | 18.6ms | 1kHz // * 7 | -- Reserved -- | -- Reserved -- | Reserved SERIAL_OUT.println(F("Setting DLPF bandwidth")); accelgyro.setDLPFMode(MPU6050_DLPF_BW_42); SERIAL_OUT.println(F("Setting gyro sensitivity to +/- 250 deg/sec...")); accelgyro.setFullScaleGyroRange(0); //accelgyro.setFullScaleGyroRange(MPU6050_GYRO_FS_250); //accelgyro.setFullScaleGyroRange(0); // 0=250, 1=500, 2=1000, 3=2000 deg/sec //SERIAL_OUT.println(F("Resetting FIFO...")); //accelgyro.resetFIFO(); // use the code below to change accel/gyro offset values accelgyro.setXGyroOffset(XGYROOFFSET); accelgyro.setYGyroOffset(YGYROOFFSET); accelgyro.setZGyroOffset(ZGYROOFFSET); SERIAL_OUT.print(accelgyro.getXAccelOffset()); SERIAL_OUT.print("\t"); // SERIAL_OUT.print(accelgyro.getYAccelOffset()); SERIAL_OUT.print("\t"); // SERIAL_OUT.print(accelgyro.getZAccelOffset()); SERIAL_OUT.print("\t"); // SERIAL_OUT.print(accelgyro.getXGyroOffset()); SERIAL_OUT.print("\t"); // SERIAL_OUT.print(accelgyro.getYGyroOffset()); SERIAL_OUT.print("\t"); // SERIAL_OUT.print(accelgyro.getZGyroOffset()); SERIAL_OUT.print("\t"); // SERIAL_OUT.print("\n"); SERIAL_OUT.println(F("Enabling FIFO...")); accelgyro.setFIFOEnabled(true); accelgyro.setZGyroFIFOEnabled(true); accelgyro.setXGyroFIFOEnabled(false); accelgyro.setYGyroFIFOEnabled(false); accelgyro.setAccelFIFOEnabled(false); SERIAL_OUT.print("Z axis enabled?\t"); SERIAL_OUT.println(accelgyro.getZGyroFIFOEnabled()); SERIAL_OUT.print("x axis enabled?\t"); SERIAL_OUT.println(accelgyro.getXGyroFIFOEnabled()); SERIAL_OUT.print("y axis enabled?\t"); SERIAL_OUT.println(accelgyro.getYGyroFIFOEnabled()); SERIAL_OUT.print("accel enabled?\t"); SERIAL_OUT.println(accelgyro.getAccelFIFOEnabled()); accelgyro.resetFIFO(); return ; }