Diff: FreeIMU.cpp

Revision:

1:794e9cdbc2a0

Parent:

0:21840c01d3d7

Child:

2:5c419926dcd7

--- a/FreeIMU.cpp	Sat Nov 02 17:25:51 2013 +0000
+++ b/FreeIMU.cpp	Tue Nov 05 11:31:39 2013 +0000
@@ -40,12 +40,9 @@
 
 I2C i2c(I2C_SDA,I2C_SCL);
 
-FreeIMU::FreeIMU() {
+FreeIMU::FreeIMU() : accgyro(MPU6050(i2c)), magn(HMC58X3(i2c)), baro(MS561101BA(i2c)){
 
    i2c.frequency(400000);
-   accgyro = MPU6050(i2c); // I2C
-   magn = HMC58X3(i2c);
-   baro = MS561101BA(i2c);
 
   // initialize quaternion
   q0 = 1.0f;
@@ -59,7 +56,6 @@
   twoKi = twoKiDef;
   integralFBx = 0.0f,  integralFBy = 0.0f, integralFBz = 0.0f;
   
-  
   update.start();
   dt_us=0;
   /*
@@ -110,43 +106,26 @@
 
 void FreeIMU::init(int accgyro_addr, bool fastmode) {
 
-  wait_ms(5);
-  /*
-  // disable internal pullups of the ATMEGA which Wire enable by default
-  #if defined(__AVR_ATmega168__) || defined(__AVR_ATmega8__) || defined(__AVR_ATmega328P__)
-    // deactivate internal pull-ups for twi
-    // as per note from atmega8 manual pg167
-    cbi(PORTC, 4);
-    cbi(PORTC, 5);
-  #else
-    // deactivate internal pull-ups for twi
-    // as per note from atmega128 manual pg204
-    cbi(PORTD, 0);
-    cbi(PORTD, 1);
-  #endif
-  */
-  
-  /*
-  if(fastmode) { // switch to 400KHz I2C - eheheh
-    TWBR = ((F_CPU / 400000L) - 16) / 2; // see twi_init in Wire/utility/twi.c
-  }
-*/
-  //accgyro = MPU6050(false, accgyro_addr);
+  wait_ms(10);
+
   accgyro = MPU6050(0x68);
   accgyro.initialize();
   accgyro.setI2CMasterModeEnabled(0);
   accgyro.setI2CBypassEnabled(1);
-  accgyro.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
-  wait_ms(5);
+  accgyro.setFullScaleGyroRange(MPU6050_GYRO_FS_1000);
+  accgyro.setDLPFMode(1);
+  accgyro.setRate(0);
+  wait_ms(10);
 
 
   // init HMC5843
   magn.init(false); // Don't set mode yet, we'll do that later on.
+  magn.setGain(0);
   // Calibrate HMC using self test, not recommended to change the gain after calibration.
-  magn.calibrate(1); // Use gain 1=default, valid 0-7, 7 not recommended.
+  magn.calibrate(0, 8); // Use gain 1=default, valid 0-7, 7 not recommended.
   // Single mode conversion was used in calibration, now set continuous mode
   magn.setMode(0);
-  wait_ms(10);
+  wait_ms(20);
   magn.setDOR(6);
   
   baro.init(FIMU_BARO_ADDR);
@@ -195,19 +174,6 @@
           q[3] = q3;
   }
 }
-/*
-#ifndef CALIBRATION_H
-
-static uint8_t location; // assuming ordered reads
-
-void eeprom_read_var(uint8_t size, byte * var) {
-  for(uint8_t i = 0; i<size; i++) {
-    var[i] = EEPROM.read(location + i);
-  }
-  location += size;
-}
-*/
-
 
 /**
  * Populates raw_values with the raw_values from the sensors
@@ -245,7 +211,7 @@
         values[i] = (float) accgyroval[i];
       }
       else {
-        values[i] = ((float) accgyroval[i]) / 16.4f; // NOTE: this depends on the sensitivity chosen
+        values[i] = ((float) accgyroval[i]) / 32.8f; // NOTE: this depends on the sensitivity chosen
       }
     }