mbedgroup / Mbed 2 deprecated 3TestBeaconDECAWAVE_plus_others

j

Dependencies:   General C12832 FatFileSystemCpp mbed

Fork of 1TestDECAWAVE_plus_others by U of Calegary - Okeef

main.cpp@2:e28f4414ca2c, 2016-08-19 (annotated)

Committer:
Kekehoho
Date:
Fri Aug 19 17:38:43 2016 +0000
Revision:
2:e28f4414ca2c
Parent:
1:4523d7cda75e 
k

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 2:e28f4414ca2c 53 DW1000 dw(p5, p6, p7, p11, p15); // 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 2:e28f4414ca2c 88 if (!(dw.getEUI() == 0xFAEDCD01FAEDCD01 && dw.getDeviceID() == 0xDECA0130)) //&& dw2.getEUI() == 0xFAEDCD01FAEDCD01 && dw2.getDeviceID() == 0xDECA0130))
Kekehoho 2:e28f4414ca2c 89 { myprintf("\n\rDWM1000 Identification error:\n DeviceID1 = %X\n EUI1 = %016llX", dw.getDeviceID(), dw.getEUI()); //dw2.getDeviceID(), dw2.getEUI());
Kekehoho 1:4523d7cda75e 90 while(1);}
Kekehoho 1:4523d7cda75e 91 else
Kekehoho 1:4523d7cda75e 92 {myprintf("DWM100 Connection established\n\r");}
Kekehoho 2:e28f4414ca2c 93
Kekehoho 1:4523d7cda75e 94 // Anchor or Beacon?
Kekehoho 2:e28f4414ca2c 95
Kekehoho 1:4523d7cda75e 96 node.isAnchor = false;
Kekehoho 2:e28f4414ca2c 97 node.address = 1;
Kekehoho 2:e28f4414ca2c 98 myprintf("This node is Beacon1 address: %d \r\n", node.address);
Kekehoho 1:4523d7cda75e 99
onehorse 0:39935bb3c1a1 100 // Read the WHO_AM_I register, this is a good test of communication
onehorse 0:39935bb3c1a1 101 uint8_t whoami = MPU9150.readByte(MPU9150_ADDRESS, WHO_AM_I_MPU9150); // Read WHO_AM_I register for MPU-9250
onehorse 0:39935bb3c1a1 102
onehorse 0:39935bb3c1a1 103 if (whoami == 0x68) // WHO_AM_I should be 0x68
Kekehoho 1:4523d7cda75e 104 {
Kekehoho 1:4523d7cda75e 105 myprintf("IMU Connection established, Initialization...\n\r");
Kekehoho 1:4523d7cda75e 106 MPU9150.MPU9150SelfTest(SelfTest); // Accelerometer and gyroscope self test; check calibration wrt factory settings
Kekehoho 1:4523d7cda75e 107 //lcd.printf("x-axis self test: acceleration trim within %f % of factory value\n\r", SelfTest[0]);
onehorse 0:39935bb3c1a1 108
onehorse 0:39935bb3c1a1 109 MPU9150.resetMPU9150(); // Reset registers to default in preparation for device calibration
onehorse 0:39935bb3c1a1 110 MPU9150.calibrateMPU9150(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
Kekehoho 1:4523d7cda75e 111 //pc.printf("x gyro bias = %f\n\r", gyroBias[0]);
Kekehoho 1:4523d7cda75e 112
onehorse 0:39935bb3c1a1 113 MPU9150.initMPU9150();
Kekehoho 1:4523d7cda75e 114 myprintf("MPU9150 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
onehorse 0:39935bb3c1a1 115 MPU9150.initAK8975A(magCalibration);
Kekehoho 1:4523d7cda75e 116 myprintf("AK8975 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer
onehorse 0:39935bb3c1a1 117 }
onehorse 0:39935bb3c1a1 118 else
onehorse 0:39935bb3c1a1 119 {
Kekehoho 1:4523d7cda75e 120 myprintf("Could not connect to MPU9150: \n\r");
Kekehoho 1:4523d7cda75e 121 myprintf("%#x \n", whoami);
onehorse 0:39935bb3c1a1 122 while(1) ; // Loop forever if communication doesn't happen
onehorse 0:39935bb3c1a1 123 }
onehorse 0:39935bb3c1a1 124
Kekehoho 1:4523d7cda75e 125 bmp180.Initialize(64, BMP180_OSS_ULTRA_LOW_POWER); // 64m altitude compensation and low power oversampling
Kekehoho 1:4523d7cda75e 126
onehorse 0:39935bb3c1a1 127 uint8_t MagRate = 10; // set magnetometer read rate in Hz; 10 to 100 (max) Hz are reasonable values
onehorse 0:39935bb3c1a1 128 MPU9150.getAres(); // Get accelerometer sensitivity
onehorse 0:39935bb3c1a1 129 MPU9150.getGres(); // Get gyro sensitivity
Kekehoho 1:4523d7cda75e 130 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 131 // So far, magnetometer bias is calculated and subtracted here manually, should construct an algorithm to do it automatically
onehorse 0:39935bb3c1a1 132 // like the gyro and accelerometer biases
onehorse 0:39935bb3c1a1 133 magbias[0] = -5.; // User environmental x-axis correction in milliGauss
onehorse 0:39935bb3c1a1 134 magbias[1] = -95.; // User environmental y-axis correction in milliGauss
onehorse 0:39935bb3c1a1 135 magbias[2] = -260.; // User environmental z-axis correction in milliGauss
Kekehoho 2:e28f4414ca2c 136
Kekehoho 1:4523d7cda75e 137 FILE *fic= fopen("/fs/test.txt","w");
Kekehoho 2:e28f4414ca2c 138 int button = 0;
Kekehoho 2:e28f4414ca2c 139 int a = -2;
Kekehoho 2:e28f4414ca2c 140 int First_Start = 1;
Kekehoho 2:e28f4414ca2c 141 float Start;
Kekehoho 2:e28f4414ca2c 142 float tnodetoanchor2, tnodetoanchor3, tnodetoanchor4, tnode2toanchor4;
Kekehoho 2:e28f4414ca2c 143
Kekehoho 1:4523d7cda75e 144 int Points = 0;
Kekehoho 1:4523d7cda75e 145 while(joystick.read() != 4) {
Kekehoho 1:4523d7cda75e 146 // Get Ranging measurements
Kekehoho 2:e28f4414ca2c 147 node.requestRanging(2);
Kekehoho 2:e28f4414ca2c 148 tnodetoanchor2 = t.read();
Kekehoho 2:e28f4414ca2c 149
Kekehoho 2:e28f4414ca2c 150 node.requestRanging(3);
Kekehoho 2:e28f4414ca2c 151 tnodetoanchor3 = t.read();
Kekehoho 2:e28f4414ca2c 152
Kekehoho 2:e28f4414ca2c 153 node.requestRanging(4);
Kekehoho 2:e28f4414ca2c 154 tnodetoanchor4 = t.read();
Kekehoho 2:e28f4414ca2c 155
Kekehoho 2:e28f4414ca2c 156 //node2.requestRanging(2);
Kekehoho 2:e28f4414ca2c 157 //tnode2toanchor2 = t.read();
Kekehoho 2:e28f4414ca2c 158
Kekehoho 2:e28f4414ca2c 159 //node2.requestRanging(4);
Kekehoho 2:e28f4414ca2c 160 //tnode2toanchor4 = t.read();
Kekehoho 2:e28f4414ca2c 161
Kekehoho 2:e28f4414ca2c 162 float temps;
onehorse 0:39935bb3c1a1 163
onehorse 0:39935bb3c1a1 164 // If intPin goes high, all data registers have new data
onehorse 0:39935bb3c1a1 165 if(MPU9150.readByte(MPU9150_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt
onehorse 0:39935bb3c1a1 166
onehorse 0:39935bb3c1a1 167 MPU9150.readAccelData(accelCount); // Read the x/y/z adc values
onehorse 0:39935bb3c1a1 168 // Now we'll calculate the accleration value into actual g's
Kekehoho 1:4523d7cda75e 169 ax = (float)accelCount[0]*aRes; - accelBias[0]; // get actual g value, this depends on scale being set
Kekehoho 1:4523d7cda75e 170 ay = (float)accelCount[1]*aRes; - accelBias[1];
Kekehoho 1:4523d7cda75e 171 az = (float)accelCount[2]*aRes; - accelBias[2];
Kekehoho 2:e28f4414ca2c 172 temps = t.read();
onehorse 0:39935bb3c1a1 173
onehorse 0:39935bb3c1a1 174 MPU9150.readGyroData(gyroCount); // Read the x/y/z adc values
onehorse 0:39935bb3c1a1 175 // Calculate the gyro value into actual degrees per second
onehorse 0:39935bb3c1a1 176 gx = (float)gyroCount[0]*gRes; // - gyroBias[0]; // get actual gyro value, this depends on scale being set
onehorse 0:39935bb3c1a1 177 gy = (float)gyroCount[1]*gRes; // - gyroBias[1];
onehorse 0:39935bb3c1a1 178 gz = (float)gyroCount[2]*gRes; // - gyroBias[2];
onehorse 0:39935bb3c1a1 179
onehorse 0:39935bb3c1a1 180 mcount++;
Kekehoho 2:e28f4414ca2c 181 //if (mcount > 200/MagRate) { // this is a poor man's way of setting the magnetometer read rate (see below)
onehorse 0:39935bb3c1a1 182 MPU9150.readMagData(magCount); // Read the x/y/z adc values
onehorse 0:39935bb3c1a1 183 // Calculate the magnetometer values in milliGauss
onehorse 0:39935bb3c1a1 184 // Include factory calibration per data sheet and user environmental corrections
onehorse 0:39935bb3c1a1 185 mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0]; // get actual magnetometer value, this depends on scale being set
onehorse 0:39935bb3c1a1 186 my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];
onehorse 0:39935bb3c1a1 187 mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];
onehorse 0:39935bb3c1a1 188 mcount = 0;
Kekehoho 2:e28f4414ca2c 189 //}
onehorse 0:39935bb3c1a1 190 }
Kekehoho 1:4523d7cda75e 191
onehorse 0:39935bb3c1a1 192
Kekehoho 2:e28f4414ca2c 193 Now = t.read_us();
Kekehoho 2:e28f4414ca2c 194 deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
Kekehoho 2:e28f4414ca2c 195 lastUpdate = Now;
onehorse 0:39935bb3c1a1 196
Kekehoho 2:e28f4414ca2c 197 sum += deltat;
Kekehoho 2:e28f4414ca2c 198 sumCount++;
onehorse 0:39935bb3c1a1 199
onehorse 0:39935bb3c1a1 200 // if(lastUpdate - firstUpdate > 10000000.0f) {
onehorse 0:39935bb3c1a1 201 // beta = 0.04; // decrease filter gain after stabilized
onehorse 0:39935bb3c1a1 202 // zeta = 0.015; // increasey bias drift gain after stabilized
onehorse 0:39935bb3c1a1 203 // }
onehorse 0:39935bb3c1a1 204
onehorse 0:39935bb3c1a1 205 // Pass gyro rate as rad/s
Kekehoho 2:e28f4414ca2c 206 //MPU9150.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
Kekehoho 1:4523d7cda75e 207 //MPU9150.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
onehorse 0:39935bb3c1a1 208
onehorse 0:39935bb3c1a1 209 // Serial print and/or display at 0.5 s rate independent of data rates
Kekehoho 1:4523d7cda75e 210
Kekehoho 1:4523d7cda75e 211 //myprintf("\r\n CIR= %u, %016llX", MID(dw.getCIR_PWR(),48,64), dw.getCIR_PWR());
Kekehoho 1:4523d7cda75e 212 //myprintf("\r\n RXPACC = %u, %X", MID(dw.getRXPACC(), 20,32), dw.getRXPACC());
onehorse 0:39935bb3c1a1 213
onehorse 0:39935bb3c1a1 214
Kekehoho 1:4523d7cda75e 215 //while(1);
onehorse 0:39935bb3c1a1 216
Kekehoho 1:4523d7cda75e 217 delt_t = t.read_ms() - count;
Kekehoho 1:4523d7cda75e 218 timer.start();
Kekehoho 2:e28f4414ca2c 219 if (fire)
Kekehoho 1:4523d7cda75e 220 { Start = timer.read();
Kekehoho 2:e28f4414ca2c 221 myprintf("#");
Kekehoho 2:e28f4414ca2c 222 fprintf(fic, "%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d\n", a, a, a, a, a, a, a, a, a, a, a, a, a);
Kekehoho 2:e28f4414ca2c 223 wait(0.4);}
Kekehoho 1:4523d7cda75e 224
Kekehoho 2:e28f4414ca2c 225 if (Start>0.01)
Kekehoho 2:e28f4414ca2c 226 { button = 1;
Kekehoho 2:e28f4414ca2c 227 Points = 0;
Kekehoho 2:e28f4414ca2c 228 myprintf("\n\rNouvelle Aquisition");
Kekehoho 2:e28f4414ca2c 229 wait(1);}
Kekehoho 1:4523d7cda75e 230
Kekehoho 2:e28f4414ca2c 231 if (fic != NULL && button && Points < 2000)//&& bmp180.ReadData(&temperature, &pressure)
Kekehoho 1:4523d7cda75e 232 {
Kekehoho 2:e28f4414ca2c 233 fprintf(fic, "%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f\n",temps, 1000*ax, 1000*ay, 1000*az, gx, gy, gz,tnodetoanchor2, node.distances[2],tnodetoanchor3, node.distances[3],tnodetoanchor4,node.distances[4],mx,my,mz);
Kekehoho 2:e28f4414ca2c 234 myprintf("%f %f %f \n\r",node.distances[2], node.distances[3], node.distances[4]);// Points, node.distances[2],node.distances[3],node.distances[4],1000*ax, 1000*ay, 1000*az );
Kekehoho 2:e28f4414ca2c 235 //fprintf(fic, ";%u;%u;%u", MID(dw.getRXPACC(), 20,32),dw.getFP_AMPL1(), MID(dw.getCIR_PWR(), 16, 32));
Kekehoho 2:e28f4414ca2c 236 //fprintf(fic, ";%u;%.2f", MID(dw.getCIR_PWR(), 32, 48), node.distances[2]);
Kekehoho 1:4523d7cda75e 237 Start = 0;
Kekehoho 1:4523d7cda75e 238 Points ++;}
Kekehoho 2:e28f4414ca2c 239 else
Kekehoho 2:e28f4414ca2c 240 {myprintf(".");
Kekehoho 2:e28f4414ca2c 241 Start = 0;}
Kekehoho 1:4523d7cda75e 242
Kekehoho 1:4523d7cda75e 243 if (delt_t > 500) { // update LCD once per half-second independent of read rate
onehorse 0:39935bb3c1a1 244
onehorse 0:39935bb3c1a1 245 tempCount = MPU9150.readTempData(); // Read the adc values
onehorse 0:39935bb3c1a1 246 temperature = ((float) tempCount) / 340.0f + 36.53f; // Temperature in degrees Centigrade
Kekehoho 1:4523d7cda75e 247 //myprintf(" temperature = %f C", temperature);
Kekehoho 1:4523d7cda75e 248 //myprintf("Ranging = %f\n\r ", Distance);
onehorse 0:39935bb3c1a1 249
Kekehoho 2:e28f4414ca2c 250 //myprintf("q0 = %f\n\r", q[0]);
Kekehoho 2:e28f4414ca2c 251 //myprintf("q1 = %f\n\r", q[1]);
Kekehoho 2:e28f4414ca2c 252 //myprintf("q2 = %f\n\r", q[2]);
Kekehoho 2:e28f4414ca2c 253 //myprintf("q3 = %f\n\r", q[3]);
onehorse 0:39935bb3c1a1 254
onehorse 0:39935bb3c1a1 255 /* lcd.clear();
onehorse 0:39935bb3c1a1 256 lcd.printString("MPU9150", 0, 0);
onehorse 0:39935bb3c1a1 257 lcd.printString("x y z", 0, 1);
onehorse 0:39935bb3c1a1 258 sprintf(buffer, "%d %d %d mg", (int)(1000.0f*ax), (int)(1000.0f*ay), (int)(1000.0f*az));
onehorse 0:39935bb3c1a1 259 lcd.printString(buffer, 0, 2);
onehorse 0:39935bb3c1a1 260 sprintf(buffer, "%d %d %d deg/s", (int)gx, (int)gy, (int)gz);
onehorse 0:39935bb3c1a1 261 lcd.printString(buffer, 0, 3);
onehorse 0:39935bb3c1a1 262 sprintf(buffer, "%d %d %d mG", (int)mx, (int)my, (int)mz);
onehorse 0:39935bb3c1a1 263 lcd.printString(buffer, 0, 4);
onehorse 0:39935bb3c1a1 264 */
onehorse 0:39935bb3c1a1 265 // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
onehorse 0:39935bb3c1a1 266 // In this coordinate system, the positive z-axis is down toward Earth.
onehorse 0:39935bb3c1a1 267 // 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 268 // 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 269 // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
onehorse 0:39935bb3c1a1 270 // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
onehorse 0:39935bb3c1a1 271 // 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 272 // applied in the correct order which for this configuration is yaw, pitch, and then roll.
onehorse 0:39935bb3c1a1 273 // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
Kekehoho 1:4523d7cda75e 274 //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 275 //pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
Kekehoho 1:4523d7cda75e 276 //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 277 //pitch *= 180.0f / PI;
Kekehoho 1:4523d7cda75e 278 //yaw *= 180.0f / PI;
Kekehoho 1:4523d7cda75e 279 //yaw -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
Kekehoho 1:4523d7cda75e 280 //roll *= 180.0f / PI;
onehorse 0:39935bb3c1a1 281
Kekehoho 2:e28f4414ca2c 282 //myprintf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
Kekehoho 2:e28f4414ca2c 283 //myprintf("average rate = %f\n\r", (float) sumCount/sum);
onehorse 0:39935bb3c1a1 284 // sprintf(buffer, "YPR: %f %f %f", yaw, pitch, roll);
onehorse 0:39935bb3c1a1 285 // lcd.printString(buffer, 0, 4);
onehorse 0:39935bb3c1a1 286 // sprintf(buffer, "rate = %f", (float) sumCount/sum);
onehorse 0:39935bb3c1a1 287 // lcd.printString(buffer, 0, 5);
onehorse 0:39935bb3c1a1 288
onehorse 0:39935bb3c1a1 289 myled= !myled;
onehorse 0:39935bb3c1a1 290 count = t.read_ms();
onehorse 0:39935bb3c1a1 291
onehorse 0:39935bb3c1a1 292 if(count > 1<<21) {
onehorse 0:39935bb3c1a1 293 t.start(); // start the timer over again if ~30 minutes has passed
onehorse 0:39935bb3c1a1 294 count = 0;
onehorse 0:39935bb3c1a1 295 deltat= 0;
onehorse 0:39935bb3c1a1 296 lastUpdate = t.read_us();
Kekehoho 2:e28f4414ca2c 297
Kekehoho 1:4523d7cda75e 298 }
Kekehoho 2:e28f4414ca2c 299 sum = 0;
Kekehoho 2:e28f4414ca2c 300 sumCount = 0;
onehorse 0:39935bb3c1a1 301 }
onehorse 0:39935bb3c1a1 302 }
Kekehoho 2:e28f4414ca2c 303 fprintf(fic, "%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d", a, a, a, a, a, a, a, a, a, a, a, a, a);
Kekehoho 1:4523d7cda75e 304 fclose(fic);
onehorse 0:39935bb3c1a1 305
onehorse 0:39935bb3c1a1 306 }