U of Calegary - Okeef / Mbed 2 deprecated 1TestDECAWAVE_plus_others

d

Dependencies:   General C12832 FatFileSystemCpp mbed

Fork of MPU9150AHRS by Kris Winer

main.cpp@1:4523d7cda75e, 2016-06-17 (annotated)

Committer:
Kekehoho
Date:
Fri Jun 17 20:35:28 2016 +0000
Revision:
1:4523d7cda75e
Parent:
0:39935bb3c1a1 
2nd try

Who changed what in which revision?

UserRevisionLine numberNew contents of line
onehorse 0:39935bb3c1a1 1 /* MPU9150 Basic Example Code
onehorse 0:39935bb3c1a1 2 by: Kris Winer
onehorse 0:39935bb3c1a1 3 date: April 1, 2014
onehorse 0:39935bb3c1a1 4 license: Beerware - Use this code however you'd like. If you
onehorse 0:39935bb3c1a1 5 find it useful you can buy me a beer some time.
onehorse 0:39935bb3c1a1 6
onehorse 0:39935bb3c1a1 7 Demonstrate basic MPU-9150 functionality including parameterizing the register addresses, initializing the sensor,
onehorse 0:39935bb3c1a1 8 getting properly scaled accelerometer, gyroscope, and magnetometer data out. Added display functions to
onehorse 0:39935bb3c1a1 9 allow display to on breadboard monitor. Addition of 9 DoF sensor fusion using open source Madgwick and
onehorse 0:39935bb3c1a1 10 Mahony filter algorithms. Sketch runs on the 3.3 V 8 MHz Pro Mini and the Teensy 3.1.
onehorse 0:39935bb3c1a1 11
onehorse 0:39935bb3c1a1 12 SDA and SCL should have external pull-up resistors (to 3.3V).
onehorse 0:39935bb3c1a1 13 10k resistors are on the EMSENSR-9250 breakout board.
onehorse 0:39935bb3c1a1 14
onehorse 0:39935bb3c1a1 15 Hardware setup:
onehorse 0:39935bb3c1a1 16 MPU9150 Breakout --------- Arduino
onehorse 0:39935bb3c1a1 17 VDD ---------------------- 3.3V
onehorse 0:39935bb3c1a1 18 VDDI --------------------- 3.3V
onehorse 0:39935bb3c1a1 19 SDA ----------------------- A4
onehorse 0:39935bb3c1a1 20 SCL ----------------------- A5
onehorse 0:39935bb3c1a1 21 GND ---------------------- GND
onehorse 0:39935bb3c1a1 22
onehorse 0:39935bb3c1a1 23 Note: The MPU9150 is an I2C sensor and uses the Arduino Wire library.
onehorse 0:39935bb3c1a1 24 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.
onehorse 0:39935bb3c1a1 25 We have disabled the internal pull-ups used by the Wire library in the Wire.h/twi.c utility file.
onehorse 0:39935bb3c1a1 26 We are also using the 400 kHz fast I2C mode by setting the TWI_FREQ to 400000L /twi.h utility file.
onehorse 0:39935bb3c1a1 27 */
Kekehoho 1:4523d7cda75e 28
onehorse 0:39935bb3c1a1 29 #include "mbed.h"
onehorse 0:39935bb3c1a1 30 #include "MPU9150.h"
Kekehoho 1:4523d7cda75e 31 #include "C12832.h"
Kekehoho 1:4523d7cda75e 32 #include "MSCFileSystem.h"
Kekehoho 1:4523d7cda75e 33 #include "BMP180.h"
Kekehoho 1:4523d7cda75e 34 #include "DW1000.h"
Kekehoho 1:4523d7cda75e 35 #include "MM2WayRanging.h"
Kekehoho 1:4523d7cda75e 36 #include "stdlib.h"
onehorse 0:39935bb3c1a1 37
Kekehoho 1:4523d7cda75e 38 #define LAST(k, n) ((k)&((1<<(n))-1))
Kekehoho 1:4523d7cda75e 39 #define MID(k,m,n) LAST((k)>>(m),((n)-(m)))
Kekehoho 1:4523d7cda75e 40 #define myprintf pc.printf
onehorse 0:39935bb3c1a1 41
onehorse 0:39935bb3c1a1 42 float sum = 0;
onehorse 0:39935bb3c1a1 43 uint32_t sumCount = 0, mcount = 0;
onehorse 0:39935bb3c1a1 44 char buffer[14];
onehorse 0:39935bb3c1a1 45
Kekehoho 1:4523d7cda75e 46 //IMU
Kekehoho 1:4523d7cda75e 47 MPU9150 MPU9150;
Kekehoho 1:4523d7cda75e 48
Kekehoho 1:4523d7cda75e 49 //BARO
Kekehoho 1:4523d7cda75e 50 BMP180 bmp180(p28, p27); // sda, scl
Kekehoho 1:4523d7cda75e 51
Kekehoho 1:4523d7cda75e 52 // DWM1000
Kekehoho 1:4523d7cda75e 53 DW1000 dw(p5, p6, p7, p11, p17); // Device driver instanceSPI pins: (MOSI, MISO, SCLK, CS, IRQ)
Kekehoho 1:4523d7cda75e 54 MM2WayRanging node(dw); // Ranging algorithm
Kekehoho 1:4523d7cda75e 55
Kekehoho 1:4523d7cda75e 56 //USB
Kekehoho 1:4523d7cda75e 57 MSCFileSystem fs("fs");
Kekehoho 1:4523d7cda75e 58
Kekehoho 1:4523d7cda75e 59 //Timer
Kekehoho 1:4523d7cda75e 60 Timer t, timer;
onehorse 0:39935bb3c1a1 61
Kekehoho 1:4523d7cda75e 62 // Application System
Kekehoho 1:4523d7cda75e 63 Serial pc(USBTX, USBRX); // tx, rx
Kekehoho 1:4523d7cda75e 64
Kekehoho 1:4523d7cda75e 65 // joystick
Kekehoho 1:4523d7cda75e 66 BusIn joystick(p15,p12,p13,p16);
onehorse 0:39935bb3c1a1 67
Kekehoho 1:4523d7cda75e 68 // LCD display
Kekehoho 1:4523d7cda75e 69 //C12832 lcd(p5, p7, p6, p8, p11);
Kekehoho 1:4523d7cda75e 70
Kekehoho 1:4523d7cda75e 71 // joystick
Kekehoho 1:4523d7cda75e 72 DigitalIn fire(p14);
onehorse 0:39935bb3c1a1 73
onehorse 0:39935bb3c1a1 74 int main()
onehorse 0:39935bb3c1a1 75 {
onehorse 0:39935bb3c1a1 76 pc.baud(9600);
onehorse 0:39935bb3c1a1 77
onehorse 0:39935bb3c1a1 78 //Set up I2C
onehorse 0:39935bb3c1a1 79 i2c.frequency(400000); // use fast (400 kHz) I2C
onehorse 0:39935bb3c1a1 80
Kekehoho 1:4523d7cda75e 81 //lcd.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);
onehorse 0:39935bb3c1a1 82
onehorse 0:39935bb3c1a1 83 t.start();
onehorse 0:39935bb3c1a1 84
Kekehoho 1:4523d7cda75e 85 // Connection Test of the DecaWave Ranging Measurement Unit
Kekehoho 1:4523d7cda75e 86
Kekehoho 1:4523d7cda75e 87 dw.setEUI(0xFAEDCD01FAEDCD01); // basic methods called to check if we have a working SPI connection
Kekehoho 1:4523d7cda75e 88 if (!(dw.getEUI() == 0xFAEDCD01FAEDCD01 && dw.getDeviceID() == 0xDECA0130))
Kekehoho 1:4523d7cda75e 89 { myprintf("DWM1000 Identification error:\n DeviceID = %d\n EUI = %d", dw.getDeviceID(), dw.getEUI());
Kekehoho 1:4523d7cda75e 90 while(1);}
Kekehoho 1:4523d7cda75e 91 else
Kekehoho 1:4523d7cda75e 92 {myprintf("DWM100 Connection established\n\r");}
onehorse 0:39935bb3c1a1 93
Kekehoho 1:4523d7cda75e 94 // Anchor or Beacon?
Kekehoho 1:4523d7cda75e 95 myprintf("Set the Chip as an Anchor: Up\n\r Set the Chip as a Beacon: Down\n\r");
Kekehoho 1:4523d7cda75e 96 while (joystick.read() != 1 && joystick.read() != 2){wait(0.001);}
Kekehoho 1:4523d7cda75e 97
Kekehoho 1:4523d7cda75e 98 if (joystick.read() == 1) {
Kekehoho 1:4523d7cda75e 99 node.isAnchor = true;
Kekehoho 1:4523d7cda75e 100 node.address = 2;
Kekehoho 1:4523d7cda75e 101 myprintf("This node is Anchor node %d \r\n", node.address);
Kekehoho 1:4523d7cda75e 102 while(1);
Kekehoho 1:4523d7cda75e 103 } else {
Kekehoho 1:4523d7cda75e 104 node.isAnchor = false;
Kekehoho 1:4523d7cda75e 105 node.address = 0;
Kekehoho 1:4523d7cda75e 106 myprintf("This node is a Beacon.\n\r ");
Kekehoho 1:4523d7cda75e 107 }
Kekehoho 1:4523d7cda75e 108
onehorse 0:39935bb3c1a1 109 // Read the WHO_AM_I register, this is a good test of communication
onehorse 0:39935bb3c1a1 110 uint8_t whoami = MPU9150.readByte(MPU9150_ADDRESS, WHO_AM_I_MPU9150); // Read WHO_AM_I register for MPU-9250
onehorse 0:39935bb3c1a1 111
onehorse 0:39935bb3c1a1 112 if (whoami == 0x68) // WHO_AM_I should be 0x68
Kekehoho 1:4523d7cda75e 113 {
Kekehoho 1:4523d7cda75e 114 myprintf("IMU Connection established, Initialization...\n\r");
Kekehoho 1:4523d7cda75e 115 MPU9150.MPU9150SelfTest(SelfTest); // Accelerometer and gyroscope self test; check calibration wrt factory settings
Kekehoho 1:4523d7cda75e 116 //lcd.printf("x-axis self test: acceleration trim within %f % of factory value\n\r", SelfTest[0]);
onehorse 0:39935bb3c1a1 117
onehorse 0:39935bb3c1a1 118 MPU9150.resetMPU9150(); // Reset registers to default in preparation for device calibration
onehorse 0:39935bb3c1a1 119 MPU9150.calibrateMPU9150(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
Kekehoho 1:4523d7cda75e 120 //pc.printf("x gyro bias = %f\n\r", gyroBias[0]);
Kekehoho 1:4523d7cda75e 121
onehorse 0:39935bb3c1a1 122 MPU9150.initMPU9150();
Kekehoho 1:4523d7cda75e 123 myprintf("MPU9150 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
onehorse 0:39935bb3c1a1 124 MPU9150.initAK8975A(magCalibration);
Kekehoho 1:4523d7cda75e 125 myprintf("AK8975 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer
onehorse 0:39935bb3c1a1 126 }
onehorse 0:39935bb3c1a1 127 else
onehorse 0:39935bb3c1a1 128 {
Kekehoho 1:4523d7cda75e 129 myprintf("Could not connect to MPU9150: \n\r");
Kekehoho 1:4523d7cda75e 130 myprintf("%#x \n", whoami);
onehorse 0:39935bb3c1a1 131 while(1) ; // Loop forever if communication doesn't happen
onehorse 0:39935bb3c1a1 132 }
onehorse 0:39935bb3c1a1 133
Kekehoho 1:4523d7cda75e 134 bmp180.Initialize(64, BMP180_OSS_ULTRA_LOW_POWER); // 64m altitude compensation and low power oversampling
Kekehoho 1:4523d7cda75e 135
onehorse 0:39935bb3c1a1 136 uint8_t MagRate = 10; // set magnetometer read rate in Hz; 10 to 100 (max) Hz are reasonable values
onehorse 0:39935bb3c1a1 137 MPU9150.getAres(); // Get accelerometer sensitivity
onehorse 0:39935bb3c1a1 138 MPU9150.getGres(); // Get gyro sensitivity
Kekehoho 1:4523d7cda75e 139 mRes = 10.*1229./4096.; // Conversion from binary to microtesla and from 1229 microTesla full scale (4096) to 12.29 Gauss full scale
onehorse 0:39935bb3c1a1 140 // So far, magnetometer bias is calculated and subtracted here manually, should construct an algorithm to do it automatically
onehorse 0:39935bb3c1a1 141 // like the gyro and accelerometer biases
onehorse 0:39935bb3c1a1 142 magbias[0] = -5.; // User environmental x-axis correction in milliGauss
onehorse 0:39935bb3c1a1 143 magbias[1] = -95.; // User environmental y-axis correction in milliGauss
onehorse 0:39935bb3c1a1 144 magbias[2] = -260.; // User environmental z-axis correction in milliGauss
Kekehoho 1:4523d7cda75e 145 FILE *fic= fopen("/fs/test.txt","w");
Kekehoho 1:4523d7cda75e 146 int button = 0;
Kekehoho 1:4523d7cda75e 147 float Start;
Kekehoho 1:4523d7cda75e 148 float pressure, temperature;
Kekehoho 1:4523d7cda75e 149 float Distance[3] = {7,8,9};
Kekehoho 1:4523d7cda75e 150 int Points = 0;
Kekehoho 1:4523d7cda75e 151 while(joystick.read() != 4) {
Kekehoho 1:4523d7cda75e 152 // Get Ranging measurements
Kekehoho 1:4523d7cda75e 153 (Distance[0], Distance[1], Distance[2]) = node.rangeAndDisplayAll();
onehorse 0:39935bb3c1a1 154
onehorse 0:39935bb3c1a1 155 // If intPin goes high, all data registers have new data
onehorse 0:39935bb3c1a1 156 if(MPU9150.readByte(MPU9150_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt
onehorse 0:39935bb3c1a1 157
onehorse 0:39935bb3c1a1 158 MPU9150.readAccelData(accelCount); // Read the x/y/z adc values
onehorse 0:39935bb3c1a1 159 // Now we'll calculate the accleration value into actual g's
Kekehoho 1:4523d7cda75e 160 ax = (float)accelCount[0]*aRes; - accelBias[0]; // get actual g value, this depends on scale being set
Kekehoho 1:4523d7cda75e 161 ay = (float)accelCount[1]*aRes; - accelBias[1];
Kekehoho 1:4523d7cda75e 162 az = (float)accelCount[2]*aRes; - accelBias[2];
onehorse 0:39935bb3c1a1 163
onehorse 0:39935bb3c1a1 164 MPU9150.readGyroData(gyroCount); // Read the x/y/z adc values
onehorse 0:39935bb3c1a1 165 // Calculate the gyro value into actual degrees per second
onehorse 0:39935bb3c1a1 166 gx = (float)gyroCount[0]*gRes; // - gyroBias[0]; // get actual gyro value, this depends on scale being set
onehorse 0:39935bb3c1a1 167 gy = (float)gyroCount[1]*gRes; // - gyroBias[1];
onehorse 0:39935bb3c1a1 168 gz = (float)gyroCount[2]*gRes; // - gyroBias[2];
onehorse 0:39935bb3c1a1 169
onehorse 0:39935bb3c1a1 170 mcount++;
onehorse 0:39935bb3c1a1 171 if (mcount > 200/MagRate) { // this is a poor man's way of setting the magnetometer read rate (see below)
onehorse 0:39935bb3c1a1 172 MPU9150.readMagData(magCount); // Read the x/y/z adc values
onehorse 0:39935bb3c1a1 173 // Calculate the magnetometer values in milliGauss
onehorse 0:39935bb3c1a1 174 // Include factory calibration per data sheet and user environmental corrections
onehorse 0:39935bb3c1a1 175 mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0]; // get actual magnetometer value, this depends on scale being set
onehorse 0:39935bb3c1a1 176 my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];
onehorse 0:39935bb3c1a1 177 mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];
onehorse 0:39935bb3c1a1 178 mcount = 0;
onehorse 0:39935bb3c1a1 179 }
onehorse 0:39935bb3c1a1 180 }
Kekehoho 1:4523d7cda75e 181
onehorse 0:39935bb3c1a1 182
Kekehoho 1:4523d7cda75e 183 //Now = t.read_us();
Kekehoho 1:4523d7cda75e 184 //deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
Kekehoho 1:4523d7cda75e 185 //lastUpdate = Now;
onehorse 0:39935bb3c1a1 186
Kekehoho 1:4523d7cda75e 187 //sum += deltat;
Kekehoho 1:4523d7cda75e 188 //sumCount++;
onehorse 0:39935bb3c1a1 189
onehorse 0:39935bb3c1a1 190 // if(lastUpdate - firstUpdate > 10000000.0f) {
onehorse 0:39935bb3c1a1 191 // beta = 0.04; // decrease filter gain after stabilized
onehorse 0:39935bb3c1a1 192 // zeta = 0.015; // increasey bias drift gain after stabilized
onehorse 0:39935bb3c1a1 193 // }
onehorse 0:39935bb3c1a1 194
onehorse 0:39935bb3c1a1 195 // Pass gyro rate as rad/s
onehorse 0:39935bb3c1a1 196 // MPU9150.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
Kekehoho 1:4523d7cda75e 197 //MPU9150.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
onehorse 0:39935bb3c1a1 198
onehorse 0:39935bb3c1a1 199 // Serial print and/or display at 0.5 s rate independent of data rates
Kekehoho 1:4523d7cda75e 200
Kekehoho 1:4523d7cda75e 201 //myprintf("\r\n CIR= %u, %016llX", MID(dw.getCIR_PWR(),48,64), dw.getCIR_PWR());
Kekehoho 1:4523d7cda75e 202 //myprintf("\r\n RXPACC = %u, %X", MID(dw.getRXPACC(), 20,32), dw.getRXPACC());
onehorse 0:39935bb3c1a1 203
onehorse 0:39935bb3c1a1 204
Kekehoho 1:4523d7cda75e 205 //while(1);
onehorse 0:39935bb3c1a1 206
Kekehoho 1:4523d7cda75e 207 delt_t = t.read_ms() - count;
Kekehoho 1:4523d7cda75e 208 timer.start();
Kekehoho 1:4523d7cda75e 209 while (fire)
Kekehoho 1:4523d7cda75e 210 { Start = timer.read();
Kekehoho 1:4523d7cda75e 211 myprintf("#");
Kekehoho 1:4523d7cda75e 212 fprintf(fic, "#\n");
Kekehoho 1:4523d7cda75e 213 wait(0.3);}
Kekehoho 1:4523d7cda75e 214
Kekehoho 1:4523d7cda75e 215 if (Start>0.1)
Kekehoho 1:4523d7cda75e 216 { button = !button;
Kekehoho 1:4523d7cda75e 217 Points = 0;}
Kekehoho 1:4523d7cda75e 218
Kekehoho 1:4523d7cda75e 219 if (fic != NULL && button && Points < 501)
Kekehoho 1:4523d7cda75e 220 {
Kekehoho 1:4523d7cda75e 221 //fprintf(fic, "%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f\n", t.read(),1000*ax, 1000*ay, 1000*az, gx, gy, gz, mx, my, mz, pressure, temperature, Distance);
Kekehoho 1:4523d7cda75e 222 myprintf("%d", Points);
Kekehoho 1:4523d7cda75e 223 fprintf(fic, "%.2f %.2f %.2f %u %u\n", Distance[0], Distance[1], Distance[2], MID(dw.getRXPACC(), 20,32), MID(dw.getCIR_PWR(),48,64));
Kekehoho 1:4523d7cda75e 224 myprintf(" Distance 2 = %.2f Distance 3 = %.2f Distance 4 = %.2f %u %u\n\r", node.distances[2], node.distances[3], node.distances[4], MID(dw.getRXPACC(), 20,32), MID(dw.getCIR_PWR(),48,64));
Kekehoho 1:4523d7cda75e 225 Start = 0;
Kekehoho 1:4523d7cda75e 226 wait(0.0005);
Kekehoho 1:4523d7cda75e 227 Points ++;}
Kekehoho 1:4523d7cda75e 228 else
Kekehoho 1:4523d7cda75e 229 //{if (fic != NULL && button)
Kekehoho 1:4523d7cda75e 230 //{fprintf(fic, "%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;No data;No data\n",t.read(),1000*ax, 1000*ay, 1000*az, gx, gy, gz, mx, my, mz);
Kekehoho 1:4523d7cda75e 231 // Start = 0;}
Kekehoho 1:4523d7cda75e 232 // else {
Kekehoho 1:4523d7cda75e 233 {
Kekehoho 1:4523d7cda75e 234 myprintf(".");
Kekehoho 1:4523d7cda75e 235 Start = 0;}
Kekehoho 1:4523d7cda75e 236
Kekehoho 1:4523d7cda75e 237 if (delt_t > 500) { // update LCD once per half-second independent of read rate
Kekehoho 1:4523d7cda75e 238
Kekehoho 1:4523d7cda75e 239
Kekehoho 1:4523d7cda75e 240
Kekehoho 1:4523d7cda75e 241 //lcd.locate(0,0);
Kekehoho 1:4523d7cda75e 242 //lcd.printf("ax = %.3f", 1000*ax);
Kekehoho 1:4523d7cda75e 243 //myprintf(" ay = %f", 1000*ay);
Kekehoho 1:4523d7cda75e 244 //pc.printf(" az = %f mg\n\r", 1000*az);
Kekehoho 1:4523d7cda75e 245 //lcd.locate(0,9);
Kekehoho 1:4523d7cda75e 246 //lcd.printf("gx = %f", gx);
Kekehoho 1:4523d7cda75e 247 // myprintf(" gy = %f", gy);
Kekehoho 1:4523d7cda75e 248 //pc.printf(" gz = %f deg/s\n\r", gz);
Kekehoho 1:4523d7cda75e 249 //lcd.locate(0,18);
Kekehoho 1:4523d7cda75e 250 //lcd.printf("gx = %f", mx);
Kekehoho 1:4523d7cda75e 251 //myprintf(" gy = %f", my);
Kekehoho 1:4523d7cda75e 252 //pc.printf(" gz = %f mG\n\r", mz);
onehorse 0:39935bb3c1a1 253
onehorse 0:39935bb3c1a1 254 tempCount = MPU9150.readTempData(); // Read the adc values
onehorse 0:39935bb3c1a1 255 temperature = ((float) tempCount) / 340.0f + 36.53f; // Temperature in degrees Centigrade
Kekehoho 1:4523d7cda75e 256 //myprintf(" temperature = %f C", temperature);
Kekehoho 1:4523d7cda75e 257 //myprintf("Ranging = %f\n\r ", Distance);
onehorse 0:39935bb3c1a1 258
Kekehoho 1:4523d7cda75e 259 //pc.printf("q0 = %f\n\r", q[0]);
Kekehoho 1:4523d7cda75e 260 //pc.printf("q1 = %f\n\r", q[1]);
Kekehoho 1:4523d7cda75e 261 //pc.printf("q2 = %f\n\r", q[2]);
Kekehoho 1:4523d7cda75e 262 //pc.printf("q3 = %f\n\r", q[3]);
onehorse 0:39935bb3c1a1 263
onehorse 0:39935bb3c1a1 264 /* lcd.clear();
onehorse 0:39935bb3c1a1 265 lcd.printString("MPU9150", 0, 0);
onehorse 0:39935bb3c1a1 266 lcd.printString("x y z", 0, 1);
onehorse 0:39935bb3c1a1 267 sprintf(buffer, "%d %d %d mg", (int)(1000.0f*ax), (int)(1000.0f*ay), (int)(1000.0f*az));
onehorse 0:39935bb3c1a1 268 lcd.printString(buffer, 0, 2);
onehorse 0:39935bb3c1a1 269 sprintf(buffer, "%d %d %d deg/s", (int)gx, (int)gy, (int)gz);
onehorse 0:39935bb3c1a1 270 lcd.printString(buffer, 0, 3);
onehorse 0:39935bb3c1a1 271 sprintf(buffer, "%d %d %d mG", (int)mx, (int)my, (int)mz);
onehorse 0:39935bb3c1a1 272 lcd.printString(buffer, 0, 4);
onehorse 0:39935bb3c1a1 273 */
onehorse 0:39935bb3c1a1 274 // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
onehorse 0:39935bb3c1a1 275 // In this coordinate system, the positive z-axis is down toward Earth.
onehorse 0:39935bb3c1a1 276 // 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.
onehorse 0:39935bb3c1a1 277 // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
onehorse 0:39935bb3c1a1 278 // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
onehorse 0:39935bb3c1a1 279 // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
onehorse 0:39935bb3c1a1 280 // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
onehorse 0:39935bb3c1a1 281 // applied in the correct order which for this configuration is yaw, pitch, and then roll.
onehorse 0:39935bb3c1a1 282 // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
Kekehoho 1:4523d7cda75e 283 //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]);
Kekehoho 1:4523d7cda75e 284 //pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
Kekehoho 1:4523d7cda75e 285 //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]);
Kekehoho 1:4523d7cda75e 286 //pitch *= 180.0f / PI;
Kekehoho 1:4523d7cda75e 287 //yaw *= 180.0f / PI;
Kekehoho 1:4523d7cda75e 288 //yaw -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
Kekehoho 1:4523d7cda75e 289 //roll *= 180.0f / PI;
onehorse 0:39935bb3c1a1 290
Kekehoho 1:4523d7cda75e 291 //pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
Kekehoho 1:4523d7cda75e 292 //pc.printf("average rate = %f\n\r", (float) sumCount/sum);
onehorse 0:39935bb3c1a1 293 // sprintf(buffer, "YPR: %f %f %f", yaw, pitch, roll);
onehorse 0:39935bb3c1a1 294 // lcd.printString(buffer, 0, 4);
onehorse 0:39935bb3c1a1 295 // sprintf(buffer, "rate = %f", (float) sumCount/sum);
onehorse 0:39935bb3c1a1 296 // lcd.printString(buffer, 0, 5);
onehorse 0:39935bb3c1a1 297
onehorse 0:39935bb3c1a1 298 myled= !myled;
onehorse 0:39935bb3c1a1 299 count = t.read_ms();
onehorse 0:39935bb3c1a1 300
onehorse 0:39935bb3c1a1 301 if(count > 1<<21) {
onehorse 0:39935bb3c1a1 302 t.start(); // start the timer over again if ~30 minutes has passed
onehorse 0:39935bb3c1a1 303 count = 0;
onehorse 0:39935bb3c1a1 304 deltat= 0;
onehorse 0:39935bb3c1a1 305 lastUpdate = t.read_us();
Kekehoho 1:4523d7cda75e 306 }
Kekehoho 1:4523d7cda75e 307 //sum = 0;
Kekehoho 1:4523d7cda75e 308 //sumCount = 0; *
onehorse 0:39935bb3c1a1 309 }
onehorse 0:39935bb3c1a1 310 }
Kekehoho 1:4523d7cda75e 311 fclose(fic);
onehorse 0:39935bb3c1a1 312
onehorse 0:39935bb3c1a1 313 }