Mired in code
Check-in [1885651ed3]
Not logged in
Public Repositories
mwm's Repositories

Many hyperlinks are disabled.
Use anonymous login to enable hyperlinks.

Overview
Comment:Move calibration to a separate sketch.
Timelines: family | ancestors | descendants | both | trunk
Files: files | file ages | folders
SHA1:1885651ed3536c46ed4088e3e11ae9f682db1cc1
User & Date: mwm 2015-08-28 14:15:47
Context
2015-08-28
14:34
Add a note about running Calibrate to the calibration load failure message. check-in: bd54efccc5 user: mwm tags: trunk
14:15
Move calibration to a separate sketch. check-in: 1885651ed3 user: mwm tags: trunk
13:31
Save calibration data to eeprom after calibrating. check-in: 2c269f1a5e user: mwm tags: trunk
Changes
Hide Diffs Side-by-Side Diffs Ignore Whitespace Patch

Added Arduino/FloatController/Calibrate/Calibrate.ino.

            1  +#define SERIAL_DEBUG
            2  +// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v2.0)
            3  +// 6/21/2012 by Jeff Rowberg <jeff@rowberg.net>
            4  +// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
            5  +//
            6  +// Modified 2015 as initial version of FloatController by Mike Meyer <mwm@mired.org>
            7  +//
            8  +
            9  +/* ============================================
           10  +I2Cdev device library code is placed under the MIT license
           11  +Copyright (c) 2012 Jeff Rowberg
           12  +Copyright (c) 2015 Mike Meyer
           13  +
           14  +===============================================
           15  +*/
           16  +
           17  +// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
           18  +// for both classes must be in the include path of your project
           19  +#include "I2Cdev.h"
           20  +
           21  +#include "MPU6050_6Axis_MotionApps20.h"
           22  +
           23  +// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
           24  +// is used in I2Cdev.h
           25  +#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
           26  +    #include <Wire.h>
           27  +#endif
           28  +
           29  +#include <EEPROM.h>
           30  +
           31  +// class default I2C address is 0x68
           32  +// specific I2C addresses may be passed as a parameter here
           33  +// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
           34  +// AD0 high = 0x69
           35  +MPU6050 mpu;
           36  +//MPU6050 mpu(0x69); // <-- use for AD0 high
           37  +
           38  +
           39  +#define LED_PIN 13  // We turn the LED on if everything is OK, off otherwise
           40  +#define ROLL_PIN 5  // PWM output for roll value
           41  +
           42  +// MPU control/status vars
           43  +bool dmpReady = false;  // set true if DMP init was successful
           44  +uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
           45  +uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
           46  +uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
           47  +uint16_t fifoCount;     // count of all bytes currently in FIFO
           48  +uint8_t fifoBuffer[64]; // FIFO storage buffer
           49  +
           50  +// orientation/motion vars
           51  +Quaternion q;           // [w, x, y, z]         quaternion container
           52  +VectorFloat gravity;    // [x, y, z]            gravity vector
           53  +float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
           54  +
           55  +
           56  +// ================================================================
           57  +// ===               INTERRUPT DETECTION ROUTINE                ===
           58  +// ================================================================
           59  +
           60  +volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
           61  +void dmpDataReady() {
           62  +    mpuInterrupt = true;
           63  +}
           64  +
           65  +
           66  +// ================================================================
           67  +// ===                      INITIAL SETUP                       ===
           68  +// ================================================================
           69  +bool calibrated;
           70  +void calibrate();
           71  +
           72  +void setup() {
           73  +    // join I2C bus (I2Cdev library doesn't do this automatically)
           74  +    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
           75  +        Wire.begin();
           76  +        TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
           77  +    #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
           78  +        Fastwire::setup(400, true);
           79  +    #endif
           80  +
           81  +    // Set up the output pins.
           82  +    pinMode(LED_PIN, OUTPUT);
           83  +    pinMode(ROLL_PIN, OUTPUT);
           84  +
           85  +#ifdef	SERIAL_DEBUG
           86  +    // initialize serial communication
           87  +    Serial.begin(115200);
           88  +    while (!Serial); // wait for Leonardo enumeration, others continue immediately
           89  +#endif
           90  +
           91  +    // initialize device
           92  +#ifdef	SERIAL_DEBUG
           93  +    Serial.println(F("Initializing I2C devices..."));
           94  +#endif
           95  +    mpu.initialize();
           96  +
           97  +    // verify connection
           98  +#ifdef	SERIAL_DEBUG
           99  +    Serial.println(F("Testing device connections..."));
          100  +    Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
          101  +#else
          102  +    mpu.testConnection();
          103  +#endif
          104  +
          105  +    // wait for ready
          106  +#ifdef	SERIAL_DEBUG
          107  +    Serial.println(F("\nSend any character to begin DMP programming and demo: "));
          108  +    while (Serial.available() && Serial.read()); // empty buffer
          109  +    while (!Serial.available());                 // wait for data
          110  +    while (Serial.available() && Serial.read()); // empty buffer again
          111  +
          112  +    // load and configure the DMPf
          113  +    Serial.println(F("Initializing DMP..."));
          114  +#endif
          115  +    devStatus = mpu.dmpInitialize();
          116  +
          117  +    // make sure it worked (returns 0 if so)
          118  +    if (devStatus == 0) {
          119  +        // turn on the DMP, now that it's ready
          120  +#ifdef	SERIAL_DEBUG
          121  +        Serial.println(F("Enabling DMP..."));
          122  +#endif
          123  +        mpu.setDMPEnabled(true);
          124  +
          125  +        // enable Arduino interrupt detection
          126  +#ifdef	SERIAL_DEBUG
          127  +        Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
          128  +#endif
          129  +        attachInterrupt(0, dmpDataReady, RISING);
          130  +        mpuIntStatus = mpu.getIntStatus();
          131  +
          132  +        // set our DMP Ready flag so the main loop() function knows it's okay to use it
          133  +#ifdef	SERIAL_DEBUG
          134  +        Serial.println(F("DMP ready! Waiting for first interrupt..."));
          135  +#endif
          136  +        dmpReady = true;
          137  +
          138  +        // get expected DMP packet size for later comparison
          139  +        packetSize = mpu.dmpGetFIFOPacketSize();
          140  +        calibrate();
          141  +        digitalWrite(LED_PIN, HIGH);
          142  +    } else {
          143  +        // ERROR!
          144  +        // 1 = initial memory load failed
          145  +        // 2 = DMP configuration updates failed
          146  +        // (if it's going to break, usually the code will be 1)
          147  +#ifdef	SERIAL_DEBUG
          148  +        Serial.print(F("DMP Initialization failed (code "));
          149  +        Serial.print(devStatus);
          150  +        Serial.println(F(")"));
          151  +#endif
          152  +        digitalWrite(LED_PIN, LOW);
          153  +    }
          154  +}
          155  +
          156  +void loop() { }
          157  +
          158  +/**********************************************
          159  + * Additional calibration code by luisrodenas *
          160  + **********************************************/
          161  +// Change this 3 variables if you want to fine tune the skecth to your needs.
          162  +int buffersize = 1000;   //Amount of readings used to average, make it higher to get more precision but sketch will be slower  (default:1000)
          163  +int acel_deadzone = 8;   //Acelerometer error allowed, make it lower to get more precision, but sketch may not converge  (default:8)
          164  +int giro_deadzone = 1;   //Giro error allowed, make it lower to get more precision, but sketch may not converge  (default:1)
          165  +int16_t ax, ay, az, gx, gy, gz;
          166  +
          167  +int mean_ax, mean_ay, mean_az, mean_gx, mean_gy, mean_gz;
          168  +int ax_offset, ay_offset, az_offset, gx_offset, gy_offset, gz_offset;
          169  +
          170  +void calibrate()
          171  +{
          172  +  void calibration(), meansensors(), saveCalibration(), setCalibration();
          173  +
          174  +#ifdef SERIAL_DEBUG
          175  +  Serial.print(F("Calibrating..."));
          176  +#endif
          177  +
          178  +  // Shut things down.
          179  +  mpu.setDMPEnabled(false);
          180  +  detachInterrupt(0);
          181  +
          182  +  // Reset offsets
          183  +  mpu.setXAccelOffset(0);
          184  +  mpu.setYAccelOffset(0);
          185  +  mpu.setZAccelOffset(0);
          186  +  mpu.setXGyroOffset(0);
          187  +  mpu.setYGyroOffset(0);
          188  +  mpu.setZGyroOffset(0);
          189  +
          190  +  delay(100);
          191  +
          192  +#ifdef SERIAL_DEBUG
          193  +  Serial.print(F("state 0..."));
          194  +#endif
          195  +  meansensors();
          196  +  delay(1000);
          197  +
          198  +#ifdef SERIAL_DEBUG
          199  +  Serial.print(F("state 1..."));
          200  +#endif
          201  +  calibration();
          202  +  delay(1000);
          203  +
          204  +  meansensors();
          205  +  setCalibration();
          206  +  calibrated = true;
          207  +  saveCalibration();
          208  +
          209  +#ifdef SERIAL_DEBUG
          210  +  Serial.println("done!");
          211  +  Serial.print("\nSensor readings with offsets:\t");
          212  +  Serial.print(mean_ax); 
          213  +  Serial.print("\t");
          214  +  Serial.print(mean_ay); 
          215  +  Serial.print("\t");
          216  +  Serial.print(mean_az); 
          217  +  Serial.print("\t");
          218  +  Serial.print(mean_gx); 
          219  +  Serial.print("\t");
          220  +  Serial.print(mean_gy); 
          221  +  Serial.print("\t");
          222  +  Serial.println(mean_gz);
          223  +  Serial.println("Values should be close to 0 0 16384 0 0 0");
          224  +#endif
          225  +
          226  +  // Start us up again.
          227  +  mpu.setDMPEnabled(true);
          228  +  attachInterrupt(0, dmpDataReady, RISING);
          229  +  mpuIntStatus = mpu.getIntStatus();
          230  +
          231  +  digitalWrite(LED_PIN, HIGH);
          232  +}
          233  +
          234  +///////////////////////////////////   FUNCTIONS   ////////////////////////////////////
          235  +void meansensors() {
          236  +  long i = 0, buff_ax = 0, buff_ay = 0, buff_az = 0, buff_gx = 0, buff_gy = 0, buff_gz = 0;
          237  +  static int blinkstate = 0 ;
          238  +
          239  +  while (i < (buffersize + 101)) {
          240  +    analogWrite(LED_PIN, blinkstate += 1);
          241  +    if (blinkstate > 255) blinkstate = 0;
          242  +
          243  +    // read raw accel/gyro measurements from device
          244  +    mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
          245  +
          246  +    if (i > 100 && i <= (buffersize + 100)) { //First 100 measures are discarded
          247  +      buff_ax = buff_ax + ax;
          248  +      buff_ay = buff_ay + ay;
          249  +      buff_az = buff_az + az;
          250  +      buff_gx = buff_gx + gx;
          251  +      buff_gy = buff_gy + gy;
          252  +      buff_gz = buff_gz + gz;
          253  +    }
          254  +    if (i == (buffersize + 100)) {
          255  +      mean_ax = buff_ax / buffersize;
          256  +      mean_ay = buff_ay / buffersize;
          257  +      mean_az = buff_az / buffersize;
          258  +      mean_gx = buff_gx / buffersize;
          259  +      mean_gy = buff_gy / buffersize;
          260  +      mean_gz = buff_gz / buffersize;
          261  +    }
          262  +    i++;
          263  +    delay(2); //Needed so we don't get repeated measures
          264  +  }
          265  +}
          266  +
          267  +void calibration() {
          268  +  ax_offset = -mean_ax / 8;
          269  +  ay_offset = -mean_ay / 8;
          270  +  az_offset = (16384 - mean_az) / 8;
          271  +
          272  +  gx_offset = -mean_gx / 4;
          273  +  gy_offset = -mean_gy / 4;
          274  +  gz_offset = -mean_gz / 4;
          275  +  while (1) {
          276  +    int ready = 0;
          277  +    mpu.setXAccelOffset(ax_offset);
          278  +    mpu.setYAccelOffset(ay_offset);
          279  +    mpu.setZAccelOffset(az_offset);
          280  +
          281  +    mpu.setXGyroOffset(gx_offset);
          282  +    mpu.setYGyroOffset(gy_offset);
          283  +    mpu.setZGyroOffset(gz_offset);
          284  +
          285  +    meansensors();
          286  +
          287  +    if (abs(mean_ax) <= acel_deadzone) ready++;
          288  +    else ax_offset = ax_offset - mean_ax / acel_deadzone;
          289  +
          290  +    if (abs(mean_ay) <= acel_deadzone) ready++;
          291  +    else ay_offset = ay_offset - mean_ay / acel_deadzone;
          292  +
          293  +    if (abs(16384 - mean_az) <= acel_deadzone) ready++;
          294  +    else az_offset = az_offset + (16384 - mean_az) / acel_deadzone;
          295  +
          296  +    if (abs(mean_gx) <= giro_deadzone) ready++;
          297  +    else gx_offset = gx_offset - mean_gx / (giro_deadzone + 1);
          298  +
          299  +    if (abs(mean_gy) <= giro_deadzone) ready++;
          300  +    else gy_offset = gy_offset - mean_gy / (giro_deadzone + 1);
          301  +
          302  +    if (abs(mean_gz) <= giro_deadzone) ready++;
          303  +    else gz_offset = gz_offset - mean_gz / (giro_deadzone + 1);
          304  +
          305  +    if (ready == 6) break;
          306  +  }
          307  +}
          308  +
          309  +// Save/restore our EEPROM values.
          310  +void
          311  +saveCalibration() {
          312  +  EEPROM.write(0, 0x01);	// Version number
          313  +  EEPROM.write(1, ax_offset & 0xFF);
          314  +  EEPROM.write(2, ax_offset >> 8);
          315  +  EEPROM.write(3, ay_offset & 0xFF);
          316  +  EEPROM.write(4, ay_offset >> 8);
          317  +  EEPROM.write(5, az_offset & 0xFF);
          318  +  EEPROM.write(6, az_offset >> 8);
          319  +  EEPROM.write(7, gx_offset & 0xFF);
          320  +  EEPROM.write(8, gx_offset >> 8);
          321  +  EEPROM.write(9, gy_offset & 0xFF);
          322  +  EEPROM.write(10, gy_offset >> 8);
          323  +  EEPROM.write(11, gz_offset & 0xFF);
          324  +  EEPROM.write(12, gz_offset >> 8);
          325  +}
          326  +
          327  +void
          328  +setCalibration() {
          329  +  mpu.setXGyroOffset(gx_offset);
          330  +  mpu.setYGyroOffset(gy_offset);
          331  +  mpu.setZGyroOffset(gz_offset);
          332  +  mpu.setXAccelOffset(ax_offset);
          333  +  mpu.setYAccelOffset(ay_offset);
          334  +  mpu.setZAccelOffset(az_offset);
          335  +}

Added Arduino/FloatController/Calibrate/Makefile.

            1  +ARDUINO_LIBS = I2Cdev MPU6050 Wire Wire/utility EEPROM
            2  +
            3  +include ../../Makefile

Changes to Arduino/FloatController/FloatController.ino.

     1         -// #define SERIAL_DEBUG
            1  +#define SERIAL_DEBUG
     2      2   // I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v2.0)
     3      3   // 6/21/2012 by Jeff Rowberg <jeff@rowberg.net>
     4      4   // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
     5      5   //
     6      6   // Modified 2015 as initial version of FloatController by Mike Meyer <mwm@mired.org>
     7      7   //
     8      8   
................................................................................
    58     58   // ================================================================
    59     59   
    60     60   volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
    61     61   void dmpDataReady() {
    62     62       mpuInterrupt = true;
    63     63   }
    64     64   
    65         -
    66     65   // ================================================================
    67     66   // ===                      INITIAL SETUP                       ===
    68     67   // ================================================================
    69         -bool calibrated;
    70         -
    71     68   void setup() {
    72         -    void loadCalibration(), setCalibration();
    73         -
    74         -    loadCalibration();
           69  +    bool loadCalibration();
           70  +    void setCalibration();
    75     71   
    76     72       // join I2C bus (I2Cdev library doesn't do this automatically)
    77         -    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    78         -        Wire.begin();
    79         -        TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
    80         -    #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
    81         -        Fastwire::setup(400, true);
    82         -    #endif
           73  +#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
           74  +    Wire.begin();
           75  +    TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
           76  +#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
           77  +    Fastwire::setup(400, true);
           78  +#endif
    83     79   
    84     80       // Set up the output pins.
    85     81       pinMode(LED_PIN, OUTPUT);
    86     82       pinMode(ROLL_PIN, OUTPUT);
    87     83   
    88     84   #ifdef	SERIAL_DEBUG
    89     85       // initialize serial communication
................................................................................
   113    109       while (Serial.available() && Serial.read()); // empty buffer again
   114    110   
   115    111       // load and configure the DMPf
   116    112       Serial.println(F("Initializing DMP..."));
   117    113   #endif
   118    114       devStatus = mpu.dmpInitialize();
   119    115   
   120         -    setCalibration();
          116  +    if (!loadCalibration()) {
          117  +#ifdef SERIAL_DEBUG
          118  +      Serial.println(F("MPU not calibrated, or calibrated for wrong firmware."));
          119  +#endif
          120  +      digitalWrite(LED_PIN, LOW);
          121  +    } else {
          122  +      setCalibration();
   121    123   
   122         -    // make sure it worked (returns 0 if so)
   123         -    if (devStatus == 0) {
          124  +      // make sure it worked (returns 0 if so)
          125  +      if (devStatus != 0) {
          126  +        // ERROR!
          127  +        // 1 = initial memory load failed
          128  +        // 2 = DMP configuration updates failed
          129  +        // (if it's going to break, usually the code will be 1)
          130  +#ifdef	SERIAL_DEBUG
          131  +        Serial.print(F("DMP Initialization failed (code "));
          132  +        Serial.print(devStatus);
          133  +        Serial.println(F(")"));
          134  +#endif
          135  +        digitalWrite(LED_PIN, LOW);
          136  +      } else {
   124    137           // turn on the DMP, now that it's ready
   125    138   #ifdef	SERIAL_DEBUG
   126    139           Serial.println(F("Enabling DMP..."));
   127    140   #endif
   128    141           mpu.setDMPEnabled(true);
   129    142   
   130    143           // enable Arduino interrupt detection
................................................................................
   139    152           Serial.println(F("DMP ready! Waiting for first interrupt..."));
   140    153   #endif
   141    154           dmpReady = true;
   142    155   
   143    156           // get expected DMP packet size for later comparison
   144    157           packetSize = mpu.dmpGetFIFOPacketSize();
   145    158           digitalWrite(LED_PIN, HIGH);
   146         -    } else {
   147         -        // ERROR!
   148         -        // 1 = initial memory load failed
   149         -        // 2 = DMP configuration updates failed
   150         -        // (if it's going to break, usually the code will be 1)
   151         -#ifdef	SERIAL_DEBUG
   152         -        Serial.print(F("DMP Initialization failed (code "));
   153         -        Serial.print(devStatus);
   154         -        Serial.println(F(")"));
   155         -#endif
   156         -        digitalWrite(LED_PIN, LOW);
          159  +      }
   157    160       }
   158    161   }
   159    162   
   160    163   
   161    164   
   162    165   // ================================================================
   163    166   // ===                    MAIN PROGRAM LOOP                     ===
   164    167   // ================================================================
   165    168   void loop() {
   166         -    void calibrate();
   167         -
   168         -    // For now, we just do this at install time. Add some way to control it later.
   169         -    if (!calibrated) calibrate();
   170         -
   171    169       // if programming failed, don't try to do anything
   172    170       if (!dmpReady) return;
   173    171   
   174    172       // wait for MPU interrupt or extra packet(s) available
   175    173       while (!mpuInterrupt && fifoCount < packetSize) {
   176    174           // other program behavior stuff here
   177    175           // .
................................................................................
   228    226           Serial.print("\t");
   229    227           Serial.println(min(abs(ypr[1]) * 510 / M_PI, 1023));
   230    228           /* Serial.println(ypr[2] * 180/M_PI); */
   231    229   #endif
   232    230       }
   233    231   }
   234    232   
   235         -/**********************************************
   236         - * Additional calibration code by luisrodenas *
   237         - **********************************************/
   238         -// Change this 3 variables if you want to fine tune the skecth to your needs.
   239         -int buffersize = 1000;   //Amount of readings used to average, make it higher to get more precision but sketch will be slower  (default:1000)
   240         -int acel_deadzone = 8;   //Acelerometer error allowed, make it lower to get more precision, but sketch may not converge  (default:8)
   241         -int giro_deadzone = 1;   //Giro error allowed, make it lower to get more precision, but sketch may not converge  (default:1)
   242         -int16_t ax, ay, az, gx, gy, gz;
   243         -
   244         -int mean_ax, mean_ay, mean_az, mean_gx, mean_gy, mean_gz;
   245         -int ax_offset, ay_offset, az_offset, gx_offset, gy_offset, gz_offset;
   246         -
   247         -void calibrate()
   248         -{
   249         -  void calibration(), meansensors(), saveCalibration(), setCalibration();
   250         -
   251         -#ifdef SERIAL_DEBUG
   252         -  Serial.print(F("Calibrating..."));
   253         -#endif
   254         -
   255         -  // Shut things down.
   256         -  mpu.setDMPEnabled(false);
   257         -  detachInterrupt(0);
   258         -
   259         -  // Reset offsets
   260         -  mpu.setXAccelOffset(0);
   261         -  mpu.setYAccelOffset(0);
   262         -  mpu.setZAccelOffset(0);
   263         -  mpu.setXGyroOffset(0);
   264         -  mpu.setYGyroOffset(0);
   265         -  mpu.setZGyroOffset(0);
   266         -
   267         -  delay(100);
   268         -
   269         -#ifdef SERIAL_DEBUG
   270         -  Serial.print(F("state 0..."));
   271         -#endif
   272         -  meansensors();
   273         -  delay(1000);
   274         -
   275         -#ifdef SERIAL_DEBUG
   276         -  Serial.print(F("state 1..."));
   277         -#endif
   278         -  calibration();
   279         -  delay(1000);
   280         -
   281         -  meansensors();
   282         -  setCalibration();
   283         -  calibrated = true;
   284         -  saveCalibration();
   285         -
   286         -#ifdef SERIAL_DEBUG
   287         -  Serial.println("done!");
   288         -  Serial.print("\nSensor readings with offsets:\t");
   289         -  Serial.print(mean_ax); 
   290         -  Serial.print("\t");
   291         -  Serial.print(mean_ay); 
   292         -  Serial.print("\t");
   293         -  Serial.print(mean_az); 
   294         -  Serial.print("\t");
   295         -  Serial.print(mean_gx); 
   296         -  Serial.print("\t");
   297         -  Serial.print(mean_gy); 
   298         -  Serial.print("\t");
   299         -  Serial.println(mean_gz);
   300         -  Serial.println("Values should be close to 0 0 16384 0 0 0");
   301         -#endif
   302         -
   303         -  // Start us up again.
   304         -  mpu.setDMPEnabled(true);
   305         -  attachInterrupt(0, dmpDataReady, RISING);
   306         -  mpuIntStatus = mpu.getIntStatus();
   307         -
   308         -  digitalWrite(LED_PIN, HIGH);
   309         -}
   310         -
   311         -///////////////////////////////////   FUNCTIONS   ////////////////////////////////////
   312         -void meansensors() {
   313         -  long i = 0, buff_ax = 0, buff_ay = 0, buff_az = 0, buff_gx = 0, buff_gy = 0, buff_gz = 0;
   314         -  static int blinkstate = 0 ;
   315         -
   316         -  while (i < (buffersize + 101)) {
   317         -    analogWrite(LED_PIN, blinkstate += 1);
   318         -    if (blinkstate > 255) blinkstate = 0;
   319         -
   320         -    // read raw accel/gyro measurements from device
   321         -    mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
   322         -
   323         -    if (i > 100 && i <= (buffersize + 100)) { //First 100 measures are discarded
   324         -      buff_ax = buff_ax + ax;
   325         -      buff_ay = buff_ay + ay;
   326         -      buff_az = buff_az + az;
   327         -      buff_gx = buff_gx + gx;
   328         -      buff_gy = buff_gy + gy;
   329         -      buff_gz = buff_gz + gz;
   330         -    }
   331         -    if (i == (buffersize + 100)) {
   332         -      mean_ax = buff_ax / buffersize;
   333         -      mean_ay = buff_ay / buffersize;
   334         -      mean_az = buff_az / buffersize;
   335         -      mean_gx = buff_gx / buffersize;
   336         -      mean_gy = buff_gy / buffersize;
   337         -      mean_gz = buff_gz / buffersize;
   338         -    }
   339         -    i++;
   340         -    delay(2); //Needed so we don't get repeated measures
   341         -  }
   342         -}
   343         -
   344         -void calibration() {
   345         -  ax_offset = -mean_ax / 8;
   346         -  ay_offset = -mean_ay / 8;
   347         -  az_offset = (16384 - mean_az) / 8;
   348         -
   349         -  gx_offset = -mean_gx / 4;
   350         -  gy_offset = -mean_gy / 4;
   351         -  gz_offset = -mean_gz / 4;
   352         -  while (1) {
   353         -    int ready = 0;
   354         -    mpu.setXAccelOffset(ax_offset);
   355         -    mpu.setYAccelOffset(ay_offset);
   356         -    mpu.setZAccelOffset(az_offset);
   357         -
   358         -    mpu.setXGyroOffset(gx_offset);
   359         -    mpu.setYGyroOffset(gy_offset);
   360         -    mpu.setZGyroOffset(gz_offset);
   361         -
   362         -    meansensors();
   363         -
   364         -    if (abs(mean_ax) <= acel_deadzone) ready++;
   365         -    else ax_offset = ax_offset - mean_ax / acel_deadzone;
   366         -
   367         -    if (abs(mean_ay) <= acel_deadzone) ready++;
   368         -    else ay_offset = ay_offset - mean_ay / acel_deadzone;
   369         -
   370         -    if (abs(16384 - mean_az) <= acel_deadzone) ready++;
   371         -    else az_offset = az_offset + (16384 - mean_az) / acel_deadzone;
   372         -
   373         -    if (abs(mean_gx) <= giro_deadzone) ready++;
   374         -    else gx_offset = gx_offset - mean_gx / (giro_deadzone + 1);
   375         -
   376         -    if (abs(mean_gy) <= giro_deadzone) ready++;
   377         -    else gy_offset = gy_offset - mean_gy / (giro_deadzone + 1);
   378         -
   379         -    if (abs(mean_gz) <= giro_deadzone) ready++;
   380         -    else gz_offset = gz_offset - mean_gz / (giro_deadzone + 1);
   381         -
   382         -    if (ready == 6) break;
   383         -  }
   384         -}
   385         -
   386         -// Save/restore our EEPROM values.
   387         -void
   388         -saveCalibration() {
   389         -  EEPROM.write(0, calibrated);
   390         -  EEPROM.write(1, ax_offset & 0xFF);
   391         -  EEPROM.write(2, ax_offset >> 8);
   392         -  EEPROM.write(3, ay_offset & 0xFF);
   393         -  EEPROM.write(4, ay_offset >> 8);
   394         -  EEPROM.write(5, az_offset & 0xFF);
   395         -  EEPROM.write(6, az_offset >> 8);
   396         -  EEPROM.write(7, gx_offset & 0xFF);
   397         -  EEPROM.write(8, gx_offset >> 8);
   398         -  EEPROM.write(9, gy_offset & 0xFF);
   399         -  EEPROM.write(10, gy_offset >> 8);
   400         -  EEPROM.write(11, gz_offset & 0xFF);
   401         -  EEPROM.write(12, gz_offset >> 8);
   402         -}
   403         -
   404         -void
          233  +static int ax_offset, ay_offset, az_offset, gx_offset, gy_offset, gz_offset;
          234  +bool
   405    235   loadCalibration() {
   406         -  calibrated = EEPROM.read(0);
          236  +  if (EEPROM.read(0) != 0x01) return false;
   407    237     ax_offset = EEPROM.read(1) | (EEPROM.read(2) << 8);
   408    238     ay_offset = EEPROM.read(3) | (EEPROM.read(4) << 8);
   409    239     az_offset = EEPROM.read(5) | (EEPROM.read(6) << 8);
   410    240     gx_offset = EEPROM.read(7) | (EEPROM.read(8) << 8);
   411    241     gy_offset = EEPROM.read(9) | (EEPROM.read(10) << 8);
   412    242     gz_offset = EEPROM.read(11) | (EEPROM.read(12) << 8);
          243  +  return true;
   413    244   }
   414    245   
   415    246   void
   416    247   setCalibration() {
   417    248     mpu.setXGyroOffset(gx_offset);
   418    249     mpu.setYGyroOffset(gy_offset);
   419    250     mpu.setZGyroOffset(gz_offset);
   420    251     mpu.setXAccelOffset(ax_offset);
   421    252     mpu.setYAccelOffset(ay_offset);
   422    253     mpu.setZAccelOffset(az_offset);
   423    254   }

Changes to Arduino/FloatController/Makefile.

     1      1   ARDUINO_LIBS = I2Cdev MPU6050 Wire Wire/utility EEPROM
     2      2   
     3         -include ../Makefile
            3  +include ../../Makefile