Sensor data verwerking

Dependencies:   SEGGER_RTT mbed

Fork of MPU9250AHRS by Kris Winer

Files at this revision

API Documentation at this revision

Comitter:
MarijnJ
Date:
Mon Feb 19 12:43:16 2018 +0000
Parent:
2:4e59a37182df
Commit message:
Simpele sensor data verwerking zonder callibratie

Changed in this revision

MPU9250.h Show annotated file Show diff for this revision Revisions of this file
N5110.lib Show diff for this revision Revisions of this file
SEGGER_RTT.lib Show annotated file Show diff for this revision Revisions of this file
ST_401_84MHZ.lib Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
diff -r 4e59a37182df -r d53674889db3 MPU9250.h
--- a/MPU9250.h	Tue Aug 05 01:37:23 2014 +0000
+++ b/MPU9250.h	Mon Feb 19 12:43:16 2018 +0000
@@ -156,7 +156,7 @@
 // Using the MSENSR-9250 breakout board, ADO is set to 0 
 // Seven-bit device address is 110100 for ADO = 0 and 110101 for ADO = 1
 //mbed uses the eight-bit device address, so shift seven-bit addresses left by one!
-#define ADO 0
+#define ADO 1
 #if ADO
 #define MPU9250_ADDRESS 0x69<<1  // Device address when ADO = 1
 #else
@@ -166,16 +166,16 @@
 // Set initial input parameters
 enum Ascale {
   AFS_2G = 0,
-  AFS_4G,
-  AFS_8G,
-  AFS_16G
+  AFS_4G = 1,
+  AFS_8G = 2,
+  AFS_16G = 3
 };
 
 enum Gscale {
   GFS_250DPS = 0,
-  GFS_500DPS,
-  GFS_1000DPS,
-  GFS_2000DPS
+  GFS_500DPS = 1,
+  GFS_1000DPS = 2,
+  GFS_2000DPS = 3
 };
 
 enum Mscale {
@@ -183,19 +183,17 @@
   MFS_16BITS      // 0.15 mG per LSB
 };
 
-uint8_t Ascale = AFS_2G;     // AFS_2G, AFS_4G, AFS_8G, AFS_16G
-uint8_t Gscale = GFS_250DPS; // GFS_250DPS, GFS_500DPS, GFS_1000DPS, GFS_2000DPS
+uint8_t Ascale = AFS_8G;     // AFS_2G, AFS_4G, AFS_8G, AFS_16G
+uint8_t Gscale = GFS_1000DPS; // GFS_250DPS, GFS_500DPS, GFS_1000DPS, GFS_2000DPS
 uint8_t Mscale = MFS_16BITS; // MFS_14BITS or MFS_16BITS, 14-bit or 16-bit magnetometer resolution
 uint8_t Mmode = 0x06;        // Either 8 Hz 0x02) or 100 Hz (0x06) magnetometer data ODR  
 float aRes, gRes, mRes;      // scale resolutions per LSB for the sensors
 
 //Set up I2C, (SDA,SCL)
-I2C i2c(I2C_SDA, I2C_SCL);
-
-DigitalOut myled(LED1);
+I2C i2c(P0_8, P0_9);
     
 // Pin definitions
-int intPin = 12;  // These can be changed, 2 and 3 are the Arduinos ext int pins
+int intPin = 16;  // These can be changed, 2 and 3 are the Arduinos ext int pins
 
 int16_t accelCount[3];  // Stores the 16-bit signed accelerometer sensor output
 int16_t gyroCount[3];   // Stores the 16-bit signed gyro sensor output
@@ -625,7 +623,7 @@
 // Configure the accelerometer for self-test
    writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0xE0); // Enable self test on all three axes and set accelerometer range to +/- 2 g
    writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0xE0); // Enable self test on all three axes and set gyro range to +/- 250 degrees/s
-   delay(25); // Delay a while to let the device stabilize
+   wait(0.1); // Delay a while to let the device stabilize
 
   for( int ii = 0; ii < 200; ii++) { // get average self-test values of gyro and acclerometer
   
@@ -648,7 +646,7 @@
  // Configure the gyro and accelerometer for normal operation
    writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00);
    writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0x00);
-   delay(25); // Delay a while to let the device stabilize
+   wait(0.1); // Delay a while to let the device stabilize
    
    // Retrieve accelerometer and gyro factory Self-Test Code from USR_Reg
    selfTest[0] = readByte(MPU9250_ADDRESS, SELF_TEST_X_ACCEL); // X-axis accel self-test results
diff -r 4e59a37182df -r d53674889db3 N5110.lib
--- a/N5110.lib	Tue Aug 05 01:37:23 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://mbed.org/users/onehorse/code/Adfs/#28c629d0b0d0
diff -r 4e59a37182df -r d53674889db3 SEGGER_RTT.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SEGGER_RTT.lib	Mon Feb 19 12:43:16 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/teams/anyThing-Connected/code/SEGGER_RTT/#7dcd871d726b
diff -r 4e59a37182df -r d53674889db3 ST_401_84MHZ.lib
--- a/ST_401_84MHZ.lib	Tue Aug 05 01:37:23 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://mbed.org/users/dreschpe/code/ST_401_84MHZ/#b9343c8b85ec
diff -r 4e59a37182df -r d53674889db3 main.cpp
--- a/main.cpp	Tue Aug 05 01:37:23 2014 +0000
+++ b/main.cpp	Mon Feb 19 12:43:16 2018 +0000
@@ -1,254 +1,205 @@
 /* MPU9250 Basic Example Code
- by: Kris Winer
- date: April 1, 2014
- license: Beerware - Use this code however you'd like. If you 
- find it useful you can buy me a beer some time.
- 
- Demonstrate basic MPU-9250 functionality including parameterizing the register addresses, initializing the sensor, 
- getting properly scaled accelerometer, gyroscope, and magnetometer data out. Added display functions to 
- allow display to on breadboard monitor. Addition of 9 DoF sensor fusion using open source Madgwick and 
- Mahony filter algorithms. Sketch runs on the 3.3 V 8 MHz Pro Mini and the Teensy 3.1.
- 
- SDA and SCL should have external pull-up resistors (to 3.3V).
- 10k resistors are on the EMSENSR-9250 breakout board.
- 
- Hardware setup:
- MPU9250 Breakout --------- Arduino
- VDD ---------------------- 3.3V
- VDDI --------------------- 3.3V
- SDA ----------------------- A4
- SCL ----------------------- A5
- GND ---------------------- GND
- 
- Note: The MPU9250 is an I2C sensor and uses the Arduino Wire library. 
- Because the sensor is not 5V tolerant, we are using a 3.3 V 8 MHz Pro Mini or a 3.3 V Teensy 3.1.
- We have disabled the internal pull-ups used by the Wire library in the Wire.h/twi.c utility file.
- We are also using the 400 kHz fast I2C mode by setting the TWI_FREQ  to 400000L /twi.h utility file.
- */
- 
-//#include "ST_F401_84MHZ.h" 
-//F401_init84 myinit(0);
+by: Kris Winer
+date: April 1, 2014
+license: Beerware - Use this code however you'd like. If you 
+find it useful you can buy me a beer some time.
+
+Demonstrate basic MPU-9250 functionality including parameterizing the register addresses, initializing the sensor, 
+getting properly scaled accelerometer, gyroscope, and magnetometer data out. Added display functions to 
+allow display to on breadboard monitor. Addition of 9 DoF sensor fusion using open source Madgwick and 
+Mahony filter algorithms. Sketch runs on the 3.3 V 8 MHz Pro Mini and the Teensy 3.1.
+
+-----------------------------------------------------------------------
+
+Adapted by Marijn Jeurissen for the anyThing Connected Sensor Sticker based on Nordic nRF51822
+date: February 16, 2018
+*/
+
 #include "mbed.h"
 #include "MPU9250.h"
-#include "N5110.h"
+#include "SEGGER_RTT.h"
+#include "SEGGER_RTT.c"
+#include "SEGGER_RTT_printf.c"
 
-// Using NOKIA 5110 monochrome 84 x 48 pixel display
-// pin 9 - Serial clock out (SCLK)
-// pin 8 - Serial data out (DIN)
-// pin 7 - Data/Command select (D/C)
-// pin 5 - LCD chip select (CS)
-// pin 6 - LCD reset (RST)
-//Adafruit_PCD8544 display = Adafruit_PCD8544(9, 8, 7, 5, 6);
-
-float sum = 0;
+float sum = 0, shift = 0;
 uint32_t sumCount = 0;
+int cycle = 0;
 char buffer[14];
 
-   MPU9250 mpu9250;
+MPU9250 mpu9250;
    
-   Timer t;
-
-   Serial pc(USBTX, USBRX); // tx, rx
+Timer t;
 
-   //        VCC,   SCE,  RST,  D/C,  MOSI,S CLK, LED
-   N5110 lcd(PA_8, PB_10, PA_9, PA_6, PA_7, PA_5, PC_7);
-   
-
-        
+short toInt(float x)
+{
+    return (x >= 0) ? (int)(x + 0.5) : (int)(x - 0.5);
+}
+int getTime(int counter)
+{
+    return (int)((counter/60000.0f)+shift);
+}
+       
 int main()
 {
-  pc.baud(9600);  
-
-  //Set up I2C
-  i2c.frequency(400000);  // use fast (400 kHz) I2C  
-  
-  pc.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);   
-  
-  t.start();        
-  
-  lcd.init();
-//  lcd.setBrightness(0.05);
-  
+    //Set up I2C
+    i2c.frequency(400000);  // use fast (400 kHz) I2C    
+    SEGGER_RTT_printf(0, "CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);
+    t.start();
     
-  // Read the WHO_AM_I register, this is a good test of communication
-  uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);  // Read WHO_AM_I register for MPU-9250
-  pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x71\n\r");
-  
-  if (whoami == 0x71) // WHO_AM_I should always be 0x68
-  {  
-    pc.printf("MPU9250 WHO_AM_I is 0x%x\n\r", whoami);
-    pc.printf("MPU9250 is online...\n\r");
-    lcd.clear();
-    lcd.printString("MPU9250 is", 0, 0);
-    sprintf(buffer, "0x%x", whoami);
-    lcd.printString(buffer, 0, 1);
-    lcd.printString("shoud be 0x71", 0, 2);  
-    wait(1);
+    // Read the WHO_AM_I register, this is a good test of communication
+    uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);  // Read WHO_AM_I register for MPU-9250
     
-    mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
-    mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values
-    pc.printf("x-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[0]);  
-    pc.printf("y-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[1]);  
-    pc.printf("z-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[2]);  
-    pc.printf("x-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[3]);  
-    pc.printf("y-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[4]);  
-    pc.printf("z-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[5]);  
-    mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers  
-    pc.printf("x gyro bias = %f\n\r", gyroBias[0]);
-    pc.printf("y gyro bias = %f\n\r", gyroBias[1]);
-    pc.printf("z gyro bias = %f\n\r", gyroBias[2]);
-    pc.printf("x accel bias = %f\n\r", accelBias[0]);
-    pc.printf("y accel bias = %f\n\r", accelBias[1]);
-    pc.printf("z accel bias = %f\n\r", accelBias[2]);
-    wait(2);
-    mpu9250.initMPU9250(); 
-    pc.printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
-    mpu9250.initAK8963(magCalibration);
-    pc.printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer
-    pc.printf("Accelerometer full-scale range = %f  g\n\r", 2.0f*(float)(1<<Ascale));
-    pc.printf("Gyroscope full-scale range = %f  deg/s\n\r", 250.0f*(float)(1<<Gscale));
-    if(Mscale == 0) pc.printf("Magnetometer resolution = 14  bits\n\r");
-    if(Mscale == 1) pc.printf("Magnetometer resolution = 16  bits\n\r");
-    if(Mmode == 2) pc.printf("Magnetometer ODR = 8 Hz\n\r");
-    if(Mmode == 6) pc.printf("Magnetometer ODR = 100 Hz\n\r");
-    wait(1);
-   }
-   else
-   {
-    pc.printf("Could not connect to MPU9250: \n\r");
-    pc.printf("%#x \n",  whoami);
- 
-    lcd.clear();
-    lcd.printString("MPU9250", 0, 0);
-    lcd.printString("no connection", 0, 1);
-    sprintf(buffer, "WHO_AM_I 0x%x", whoami);
-    lcd.printString(buffer, 0, 2); 
- 
-    while(1) ; // Loop forever if communication doesn't happen
+    if (whoami == 0x71) // WHO_AM_I should always be 0x68
+    { 
+        SEGGER_RTT_WriteString(0, "MPU9250 is online...\n\n");
+        wait(1);
+        
+        mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
+        mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values
+        SEGGER_RTT_printf(0, "x-axis self test: acceleration trim within : %d % of factory value\n", toInt(SelfTest[0]));  
+        SEGGER_RTT_printf(0, "y-axis self test: acceleration trim within : %d % of factory value\n", toInt(SelfTest[1]));  
+        SEGGER_RTT_printf(0, "z-axis self test: acceleration trim within : %d % of factory value\n", toInt(SelfTest[2]));  
+        SEGGER_RTT_printf(0, "x-axis self test: gyration trim within : %d % of factory value\n", toInt(SelfTest[3]));  
+        SEGGER_RTT_printf(0, "y-axis self test: gyration trim within : %d % of factory value\n", toInt(SelfTest[4]));  
+        SEGGER_RTT_printf(0, "z-axis self test: gyration trim within : %d % of factory value\n\n", toInt(SelfTest[5]));
+        
+        mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers  
+        SEGGER_RTT_printf(0, "x gyro bias = %d\n", toInt(gyroBias[0]));
+        SEGGER_RTT_printf(0, "y gyro bias = %d\n", toInt(gyroBias[1]));
+        SEGGER_RTT_printf(0, "z gyro bias = %d\n", toInt(gyroBias[2]));
+        SEGGER_RTT_printf(0, "x accel bias = %d\n", toInt(accelBias[0]));
+        SEGGER_RTT_printf(0, "y accel bias = %d\n", toInt(accelBias[1]));
+        SEGGER_RTT_printf(0, "z accel bias = %d\n\n", toInt(accelBias[2]));
+        wait(2);
+        
+        mpu9250.initMPU9250(); 
+        SEGGER_RTT_WriteString(0, "MPU9250 initialized for active data mode....\n"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
+        mpu9250.initAK8963(magCalibration);
+        SEGGER_RTT_WriteString(0, "AK8963 initialized for active data mode....\n"); // Initialize device for active mode read of magnetometer
+        SEGGER_RTT_printf(0, "Accelerometer full-scale range = %d  g\n", toInt(2.0f*(float)(1<<Ascale)));
+        SEGGER_RTT_printf(0, "Gyroscope full-scale range = %d  deg/s\n", toInt(250.0f*(float)(1<<Gscale)));
+        if(Mscale == 0) SEGGER_RTT_WriteString(0, "Magnetometer resolution = 14  bits\n");
+        if(Mscale == 1) SEGGER_RTT_WriteString(0, "Magnetometer resolution = 16  bits\n");
+        if(Mmode == 2) SEGGER_RTT_WriteString(0, "Magnetometer ODR = 8 Hz\n");
+        if(Mmode == 6) SEGGER_RTT_WriteString(0, "Magnetometer ODR = 100 Hz\n");
+        SEGGER_RTT_WriteString(0, "\n");
+        wait(1);
+    }
+    else {
+        SEGGER_RTT_printf(0, "Could not connect to MPU9250: 0x%x \n",  whoami); 
+        while(1); // Loop forever if communication doesn't happen
     }
 
     mpu9250.getAres(); // Get accelerometer sensitivity
     mpu9250.getGres(); // Get gyro sensitivity
     mpu9250.getMres(); // Get magnetometer sensitivity
-    pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes);
-    pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes);
-    pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes);
+    SEGGER_RTT_printf(0, "Accelerometer sensitivity is %d LSB/g \n", toInt(1.0f/aRes));
+    SEGGER_RTT_printf(0, "Gyroscope sensitivity is %d LSB/deg/s \n", toInt(1.0f/gRes));
+    SEGGER_RTT_printf(0, "Magnetometer sensitivity is %d LSB/G \n", toInt(1.0f/mRes));
     magbias[0] = +470.;  // User environmental x-axis correction in milliGauss, should be automatically calculated
     magbias[1] = +120.;  // User environmental x-axis correction in milliGauss
     magbias[2] = +125.;  // User environmental x-axis correction in milliGauss
 
- while(1) {
-  
-  // If intPin goes high, all data registers have new data
-  if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) {  // On interrupt, check if data ready interrupt
-
-    mpu9250.readAccelData(accelCount);  // Read the x/y/z adc values   
-    // Now we'll calculate the accleration value into actual g's
-    ax = (float)accelCount[0]*aRes - accelBias[0];  // get actual g value, this depends on scale being set
-    ay = (float)accelCount[1]*aRes - accelBias[1];   
-    az = (float)accelCount[2]*aRes - accelBias[2];  
-   
-    mpu9250.readGyroData(gyroCount);  // Read the x/y/z adc values
-    // Calculate the gyro value into actual degrees per second
-    gx = (float)gyroCount[0]*gRes - gyroBias[0];  // get actual gyro value, this depends on scale being set
-    gy = (float)gyroCount[1]*gRes - gyroBias[1];  
-    gz = (float)gyroCount[2]*gRes - gyroBias[2];   
-  
-    mpu9250.readMagData(magCount);  // Read the x/y/z adc values   
-    // Calculate the magnetometer values in milliGauss
-    // Include factory calibration per data sheet and user environmental corrections
-    mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0];  // get actual magnetometer value, this depends on scale being set
-    my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];  
-    mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];   
-  }
+    while(1)
+    {
+        // If intPin goes high, all data registers have new data
+        if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01)  // On interrupt, check if data ready interrupt
+        {    
+            mpu9250.readAccelData(accelCount);  // Read the x/y/z adc values   
+            // Now we'll calculate the accleration value into actual g's
+            ax = (float)accelCount[0]*aRes - accelBias[0];  // get actual g value, this depends on scale being set
+            ay = (float)accelCount[1]*aRes - accelBias[1];   
+            az = (float)accelCount[2]*aRes - accelBias[2];  
+            
+            mpu9250.readGyroData(gyroCount);  // Read the x/y/z adc values
+            // Calculate the gyro value into actual degrees per second
+            gx = (float)gyroCount[0]*gRes - gyroBias[0];  // get actual gyro value, this depends on scale being set
+            gy = (float)gyroCount[1]*gRes - gyroBias[1];  
+            gz = (float)gyroCount[2]*gRes - gyroBias[2];   
+            
+            mpu9250.readMagData(magCount);  // Read the x/y/z adc values   
+            // Calculate the magnetometer values in milliGauss
+            // Include factory calibration per data sheet and user environmental corrections
+            mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0];  // get actual magnetometer value, this depends on scale being set
+            my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];  
+            mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];   
+        }
    
-    Now = t.read_us();
-    deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
-    lastUpdate = Now;
-    
-    sum += deltat;
-    sumCount++;
-    
-//    if(lastUpdate - firstUpdate > 10000000.0f) {
-//     beta = 0.04;  // decrease filter gain after stabilized
-//     zeta = 0.015; // increasey bias drift gain after stabilized
- //   }
-    
-   // Pass gyro rate as rad/s
-//  mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f,  my,  mx, mz);
-  mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
-
-    // Serial print and/or display at 0.5 s rate independent of data rates
-    delt_t = t.read_ms() - count;
-    if (delt_t > 500) { // update LCD once per half-second independent of read rate
-
-    pc.printf("ax = %f", 1000*ax); 
-    pc.printf(" ay = %f", 1000*ay); 
-    pc.printf(" az = %f  mg\n\r", 1000*az); 
-
-    pc.printf("gx = %f", gx); 
-    pc.printf(" gy = %f", gy); 
-    pc.printf(" gz = %f  deg/s\n\r", gz); 
-    
-    pc.printf("gx = %f", mx); 
-    pc.printf(" gy = %f", my); 
-    pc.printf(" gz = %f  mG\n\r", mz); 
+        Now = t.read_us();
+        deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
+        lastUpdate = Now;
+        
+        sum += deltat;
+        sumCount++;
+        
+        if(lastUpdate - firstUpdate > 10000000.0f)
+        {
+            beta = 0.04;  // decrease filter gain after stabilized
+            zeta = 0.015; // increasey bias drift gain after stabilized
+        }
+        
+       // Pass gyro rate as rad/s
+//      mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f,  my,  mx, mz);
+        mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
     
-    tempCount = mpu9250.readTempData();  // Read the adc values
-    temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
-    pc.printf(" temperature = %f  C\n\r", temperature); 
-    
-    pc.printf("q0 = %f\n\r", q[0]);
-    pc.printf("q1 = %f\n\r", q[1]);
-    pc.printf("q2 = %f\n\r", q[2]);
-    pc.printf("q3 = %f\n\r", q[3]);      
-    
-/*    lcd.clear();
-    lcd.printString("MPU9250", 0, 0);
-    lcd.printString("x   y   z", 0, 1);
-    sprintf(buffer, "%d %d %d mg", (int)(1000.0f*ax), (int)(1000.0f*ay), (int)(1000.0f*az));
-    lcd.printString(buffer, 0, 2);
-    sprintf(buffer, "%d %d %d deg/s", (int)gx, (int)gy, (int)gz);
-    lcd.printString(buffer, 0, 3);
-    sprintf(buffer, "%d %d %d mG", (int)mx, (int)my, (int)mz);
-    lcd.printString(buffer, 0, 4); 
- */  
-  // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
-  // In this coordinate system, the positive z-axis is down toward Earth. 
-  // Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise.
-  // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
-  // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
-  // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
-  // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
-  // applied in the correct order which for this configuration is yaw, pitch, and then roll.
-  // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
-    yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);   
-    pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
-    roll  = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
-    pitch *= 180.0f / PI;
-    yaw   *= 180.0f / PI; 
-    yaw   -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
-    roll  *= 180.0f / PI;
-
-    pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
-    pc.printf("average rate = %f\n\r", (float) sumCount/sum);
-//    sprintf(buffer, "YPR: %f %f %f", yaw, pitch, roll);
-//    lcd.printString(buffer, 0, 4);
-//    sprintf(buffer, "rate = %f", (float) sumCount/sum);
-//    lcd.printString(buffer, 0, 5);
-    
-    myled= !myled;
-    count = t.read_ms(); 
-
-    if(count > 1<<21) {
-        t.start(); // start the timer over again if ~30 minutes has passed
-        count = 0;
-        deltat= 0;
-        lastUpdate = t.read_us();
-    }
-    sum = 0;
-    sumCount = 0; 
-}
-}
- 
- }
\ No newline at end of file
+        // Serial print 1 s rate independent of data rates
+        delt_t = t.read_ms() - count;
+        if (delt_t > 1000) // update print once per second independent of read rate
+        {
+            SEGGER_RTT_printf(0, "\n\nax = %d", toInt(1000*ax)); 
+            SEGGER_RTT_printf(0, " ay = %d", toInt(1000*ay)); 
+            SEGGER_RTT_printf(0, " az = %d  mg\n", toInt(1000*az)); 
+        
+            SEGGER_RTT_printf(0, "gx = %d", toInt(gx)); 
+            SEGGER_RTT_printf(0, " gy = %d", toInt(gy)); 
+            SEGGER_RTT_printf(0, " gz = %d  deg/s\n", toInt(gz)); 
+            
+            SEGGER_RTT_printf(0, "mx = %d", toInt(mx)); 
+            SEGGER_RTT_printf(0, " my = %d", toInt(my)); 
+            SEGGER_RTT_printf(0, " mz = %d  mG\n", toInt(mz)); 
+            
+            tempCount = mpu9250.readTempData();  // Read the adc values
+            temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Celsius
+            SEGGER_RTT_printf(0, "Temperature = %d  C\n\n", toInt(temperature)); 
+            
+            //SEGGER_RTT_printf(0, "q0 = %d\n", toInt(q[0]*100));
+            //SEGGER_RTT_printf(0, "q1 = %d\n", toInt(q[1]*100));
+            //SEGGER_RTT_printf(0, "q2 = %d\n", toInt(q[2]*100));
+            //SEGGER_RTT_printf(0, "q3 = %d\n", toInt(q[3]*100));
+        
+            // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
+            // In this coordinate system, the positive z-axis is down toward Earth. 
+            // Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise.
+            // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
+            // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
+            // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
+            // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
+            // applied in the correct order which for this configuration is yaw, pitch, and then roll.
+            // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
+            yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);   
+            pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
+            roll  = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
+            pitch *= 180.0f / PI;
+            yaw   *= 180.0f / PI; 
+            yaw   -= 1.05f; // Declination at Delft is 1 degrees 3 minutes on 2018-02-16
+            roll  *= 180.0f / PI;
+        
+            SEGGER_RTT_printf(0, "Yaw: %d   Pitch: %d   Roll: %d\n\n", toInt(yaw), toInt(pitch), toInt(roll));
+            //SEGGER_RTT_printf(0, "average rate = %d\n\n\n", toInt((float) sumCount/sum));
+        
+            count = t.read_ms();
+            SEGGER_RTT_printf(0, "Time active: %d minutes\n---------------------------------", getTime(count));
+        
+            if(count > 1<<21)
+            {
+                t.start(); // start the timer over again if ~30 minutes has passed
+                count = 0;
+                deltat= 0;
+                lastUpdate = t.read_us();
+                shift = (++cycle * 34.9525f);
+            }
+            sum = 0;
+            sumCount = 0; 
+        }
+    } 
+}
\ No newline at end of file
diff -r 4e59a37182df -r d53674889db3 mbed.bld
--- a/mbed.bld	Tue Aug 05 01:37:23 2014 +0000
+++ b/mbed.bld	Mon Feb 19 12:43:16 2018 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/mbed_official/code/mbed/builds/0b3ab51c8877
\ No newline at end of file
+http://mbed.org/users/mbed_official/code/mbed/builds/7130f322cb7e
\ No newline at end of file