P R / Mbed 2 deprecated Xadow-M0_Xadow-OLED_Accelerometer

A quick and dirty demo of the Xadow M0 acceleromoeter values displayed on the Xadow OLED 0.96" (using the SSD1308 128x64 OLED Driver with I2C interface library).

Dependencies:   mbed SSD1308_128x64_I2C_opt XadowGPS BMP180 ADXL345_I2C MPU9250 USBDevice

main.cpp@9:310663c014d8, 2019-03-01 (annotated)

Committer:
ruevs
Date:
Fri Mar 01 09:48:20 2019 +0000
Revision:
9:310663c014d8
Parent:
8:4e8991196bb8 
Implement online magnetometer calibration - offset and scale.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ruevs 2:7abdbccdf0c8 1 #define __STDC_LIMIT_MACROS 1
ruevs 2:7abdbccdf0c8 2 #include <stdint.h>
seeed 0:b017c907d53d 3
ruevs 2:7abdbccdf0c8 4 #include "ADXL345_I2C.h"
ruevs 2:7abdbccdf0c8 5 #include "SSD1308.h"
ruevs 5:50051611b5bd 6 #include "MPU9250.h"
ruevs 5:50051611b5bd 7 #include "BMP180.h"
ruevs 2:7abdbccdf0c8 8
ruevs 5:50051611b5bd 9 #define DEBUG
seeed 0:b017c907d53d 10
seeed 0:b017c907d53d 11 #ifdef DEBUG
seeed 0:b017c907d53d 12 #include "USBSerial.h" // To use USB virtual serial, a driver is needed, check http://mbed.org/handbook/USBSerial
seeed 0:b017c907d53d 13 #define LOG(args...) pc.printf(args)
seeed 0:b017c907d53d 14 USBSerial pc;
seeed 0:b017c907d53d 15 #else
seeed 0:b017c907d53d 16 #define LOG(args...)
seeed 0:b017c907d53d 17 #endif
seeed 0:b017c907d53d 18
ruevs 2:7abdbccdf0c8 19 #define MAX_ACC_AXIS 3
seeed 0:b017c907d53d 20 ADXL345_I2C accelerometer(P0_5, P0_4);
seeed 0:b017c907d53d 21
ruevs 2:7abdbccdf0c8 22 // Xhadow - OLED 128x64 is connected with I2C
ruevs 2:7abdbccdf0c8 23 I2C i2c(P0_5, P0_4); // SDA, SCL
ruevs 2:7abdbccdf0c8 24 SSD1308 oled = SSD1308(i2c, SSD1308_SA0);
ruevs 5:50051611b5bd 25 SSD1308 oled2 = SSD1308(i2c, SSD1308_SA1);
ruevs 5:50051611b5bd 26
ruevs 5:50051611b5bd 27 // MPU9250 9DOF IMU - accelerometer, gyroscope, magnetometer access class
ruevs 5:50051611b5bd 28 MPU9250 mpu9250(i2c);
ruevs 5:50051611b5bd 29
ruevs 5:50051611b5bd 30 // BMP180 pressure and temperature sensor access class
ruevs 5:50051611b5bd 31 BMP180 bmp180(&i2c);
ruevs 2:7abdbccdf0c8 32
ruevs 2:7abdbccdf0c8 33 AnalogIn AD00(P0_11);
ruevs 2:7abdbccdf0c8 34 AnalogIn AD01(P0_12);
ruevs 2:7abdbccdf0c8 35 AnalogIn ChargerStatus(P0_13); // ADC input from VCC through a 200K/200K divider with extara 100K pull down on !DONE and 49.9K on !CHRG
ruevs 2:7abdbccdf0c8 36
ruevs 2:7abdbccdf0c8 37 DigitalOut blue_led(P0_20, 0);
ruevs 2:7abdbccdf0c8 38 DigitalOut white_led(P0_23, 1);
ruevs 2:7abdbccdf0c8 39 DigitalOut Bus3V3En(P0_14, 1); // Pin that controls the BUS-3V3 (3.3V) regulator output to the Xadow Bus
ruevs 2:7abdbccdf0c8 40
ruevs 2:7abdbccdf0c8 41 InterruptIn test_int(P0_7);
ruevs 2:7abdbccdf0c8 42
ruevs 2:7abdbccdf0c8 43 void test_pin_int(void)
ruevs 2:7abdbccdf0c8 44 {
ruevs 2:7abdbccdf0c8 45 white_led = !white_led;
ruevs 2:7abdbccdf0c8 46 Bus3V3En = !Bus3V3En;
ruevs 2:7abdbccdf0c8 47 }
ruevs 2:7abdbccdf0c8 48
ruevs 5:50051611b5bd 49 void MPU9250Test()
ruevs 5:50051611b5bd 50 {
ruevs 5:50051611b5bd 51 Timer t;
ruevs 5:50051611b5bd 52 float sum = 0;
ruevs 5:50051611b5bd 53 uint32_t sumCount = 0;
ruevs 5:50051611b5bd 54
ruevs 5:50051611b5bd 55 char buffer[14];
ruevs 5:50051611b5bd 56
ruevs 5:50051611b5bd 57 uint8_t whoami;
ruevs 5:50051611b5bd 58
ruevs 5:50051611b5bd 59 //___ Set up I2C: use fast (400 kHz) I2C ___
ruevs 5:50051611b5bd 60 i2c.frequency(400000);
ruevs 5:50051611b5bd 61
ruevs 5:50051611b5bd 62 wait(10); // to allow terminal to cooect on PC
ruevs 5:50051611b5bd 63
ruevs 5:50051611b5bd 64 while(1) {
ruevs 5:50051611b5bd 65 if (bmp180.init() != 0) {
ruevs 5:50051611b5bd 66 LOG("Error communicating with BMP180r\n");
ruevs 5:50051611b5bd 67 } else {
ruevs 7:4931dbfbc042 68 LOG("Initialized BMP180\r\n");
ruevs 5:50051611b5bd 69 break;
ruevs 5:50051611b5bd 70 }
ruevs 5:50051611b5bd 71 wait(1);
ruevs 5:50051611b5bd 72 }
ruevs 5:50051611b5bd 73
ruevs 5:50051611b5bd 74 LOG("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);
ruevs 5:50051611b5bd 75
ruevs 5:50051611b5bd 76 t.start(); // Timer ON
ruevs 5:50051611b5bd 77
ruevs 5:50051611b5bd 78 // Read the WHO_AM_I register, this is a good test of communication
ruevs 5:50051611b5bd 79 whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);
ruevs 5:50051611b5bd 80
ruevs 5:50051611b5bd 81 LOG("I AM 0x%x\r\n", whoami); LOG("I SHOULD BE 0x71\r\n");
ruevs 5:50051611b5bd 82 if (I2Cstate != 0) // error on I2C
ruevs 5:50051611b5bd 83 LOG("I2C failure while reading WHO_AM_I register");
ruevs 5:50051611b5bd 84
ruevs 5:50051611b5bd 85 if (whoami == 0x71) // WHO_AM_I should always be 0x71
ruevs 5:50051611b5bd 86 {
ruevs 5:50051611b5bd 87 LOG("MPU9250 WHO_AM_I is 0x%x\r\n", whoami);
ruevs 5:50051611b5bd 88 LOG("MPU9250 is online...\r\n");
ruevs 5:50051611b5bd 89 sprintf(buffer, "0x%x", whoami);
ruevs 5:50051611b5bd 90 wait(1);
ruevs 5:50051611b5bd 91
ruevs 5:50051611b5bd 92 mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
ruevs 5:50051611b5bd 93
ruevs 5:50051611b5bd 94 mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values (accelerometer and gyroscope self test)
ruevs 5:50051611b5bd 95 LOG("x-axis self test: acceleration trim within : %f % of factory value\r\n", SelfTest[0]);
ruevs 5:50051611b5bd 96 LOG("y-axis self test: acceleration trim within : %f % of factory value\r\n", SelfTest[1]);
ruevs 5:50051611b5bd 97 LOG("z-axis self test: acceleration trim within : %f % of factory value\r\n", SelfTest[2]);
ruevs 5:50051611b5bd 98 LOG("x-axis self test: gyration trim within : %f % of factory value\r\n", SelfTest[3]);
ruevs 5:50051611b5bd 99 LOG("y-axis self test: gyration trim within : %f % of factory value\r\n", SelfTest[4]);
ruevs 5:50051611b5bd 100 LOG("z-axis self test: gyration trim within : %f % of factory value\r\n", SelfTest[5]);
ruevs 5:50051611b5bd 101
ruevs 5:50051611b5bd 102 mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometer, load biases in bias registers
ruevs 5:50051611b5bd 103 LOG("x gyro bias = %f\r\n", gyroBias[0]);
ruevs 5:50051611b5bd 104 LOG("y gyro bias = %f\r\n", gyroBias[1]);
ruevs 5:50051611b5bd 105 LOG("z gyro bias = %f\r\n", gyroBias[2]);
ruevs 5:50051611b5bd 106 LOG("x accel bias = %f\r\n", accelBias[0]);
ruevs 5:50051611b5bd 107 LOG("y accel bias = %f\r\n", accelBias[1]);
ruevs 5:50051611b5bd 108 LOG("z accel bias = %f\r\n", accelBias[2]);
ruevs 5:50051611b5bd 109 wait(2);
ruevs 5:50051611b5bd 110
ruevs 5:50051611b5bd 111 // Initialize device for active mode read of acclerometer, gyroscope, and temperature
ruevs 5:50051611b5bd 112 mpu9250.initMPU9250();
ruevs 5:50051611b5bd 113 LOG("MPU9250 initialized for active data mode....\r\n");
ruevs 5:50051611b5bd 114
ruevs 5:50051611b5bd 115 // Initialize device for active mode read of magnetometer, 16 bit resolution, 100Hz.
ruevs 5:50051611b5bd 116 mpu9250.initAK8963(magCalibration);
ruevs 5:50051611b5bd 117 LOG("AK8963 initialized for active data mode....\r\n");
ruevs 5:50051611b5bd 118 LOG("Accelerometer full-scale range = %f g\r\n", 2.0f*(float)(1<<Ascale));
ruevs 5:50051611b5bd 119 LOG("Gyroscope full-scale range = %f deg/s\r\n", 250.0f*(float)(1<<Gscale));
ruevs 5:50051611b5bd 120 if(Mscale == 0) LOG("Magnetometer resolution = 14 bits\r\n");
ruevs 5:50051611b5bd 121 if(Mscale == 1) LOG("Magnetometer resolution = 16 bits\r\n");
ruevs 5:50051611b5bd 122 if(Mmode == 2) LOG("Magnetometer ODR = 8 Hz\r\n");
ruevs 5:50051611b5bd 123 if(Mmode == 6) LOG("Magnetometer ODR = 100 Hz\r\n");
ruevs 5:50051611b5bd 124 wait(1);
ruevs 5:50051611b5bd 125 }
ruevs 5:50051611b5bd 126
ruevs 5:50051611b5bd 127 else // Connection failure
ruevs 5:50051611b5bd 128 {
ruevs 5:50051611b5bd 129 LOG("Could not connect to MPU9250: \r\n");
ruevs 5:50051611b5bd 130 LOG("%#x \n", whoami);
ruevs 5:50051611b5bd 131 sprintf(buffer, "WHO_AM_I 0x%x", whoami);
ruevs 5:50051611b5bd 132 while(1) ; // Loop forever if communication doesn't happen
ruevs 5:50051611b5bd 133 }
ruevs 5:50051611b5bd 134
ruevs 5:50051611b5bd 135 mpu9250.getAres(); // Get accelerometer sensitivity
ruevs 5:50051611b5bd 136 mpu9250.getGres(); // Get gyro sensitivity
ruevs 5:50051611b5bd 137 mpu9250.getMres(); // Get magnetometer sensitivity
ruevs 5:50051611b5bd 138 LOG("Accelerometer sensitivity is %f LSB/g \r\n", 1.0f/aRes);
ruevs 5:50051611b5bd 139 LOG("Gyroscope sensitivity is %f LSB/deg/s \r\n", 1.0f/gRes);
ruevs 5:50051611b5bd 140 LOG("Magnetometer sensitivity is %f LSB/G \r\n", 1.0f/mRes);
ruevs 9:310663c014d8 141 /*
ruevs 5:50051611b5bd 142 magbias[0] = +470.; // User environmental x-axis correction in milliGauss, should be automatically calculated
ruevs 5:50051611b5bd 143 magbias[1] = +120.; // User environmental x-axis correction in milliGauss
ruevs 5:50051611b5bd 144 magbias[2] = +125.; // User environmental x-axis correction in milliGauss
ruevs 9:310663c014d8 145 */
ruevs 5:50051611b5bd 146
ruevs 5:50051611b5bd 147 while(1) {
ruevs 5:50051611b5bd 148
ruevs 5:50051611b5bd 149 // If intPin goes high, all data registers have new data
ruevs 5:50051611b5bd 150 if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt
ruevs 5:50051611b5bd 151
ruevs 5:50051611b5bd 152 mpu9250.readAccelData(accelCount); // Read the x/y/z adc values
ruevs 5:50051611b5bd 153 // Now we'll calculate the accleration value into actual g's
ruevs 5:50051611b5bd 154 if (I2Cstate != 0) //error on I2C
ruevs 5:50051611b5bd 155 LOG("I2C error ocurred while reading accelerometer data. I2Cstate = %d \r\n", I2Cstate);
ruevs 5:50051611b5bd 156 else{ // I2C read or write ok
ruevs 5:50051611b5bd 157 I2Cstate = 1;
ruevs 5:50051611b5bd 158 ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
ruevs 5:50051611b5bd 159 ay = (float)accelCount[1]*aRes - accelBias[1];
ruevs 5:50051611b5bd 160 az = (float)accelCount[2]*aRes - accelBias[2];
ruevs 5:50051611b5bd 161 }
ruevs 5:50051611b5bd 162
ruevs 5:50051611b5bd 163 mpu9250.readGyroData(gyroCount); // Read the x/y/z adc values
ruevs 5:50051611b5bd 164 // Calculate the gyro value into actual degrees per second
ruevs 5:50051611b5bd 165 if (I2Cstate != 0) //error on I2C
ruevs 5:50051611b5bd 166 LOG("I2C error ocurred while reading gyrometer data. I2Cstate = %d \r\n", I2Cstate);
ruevs 5:50051611b5bd 167 else{ // I2C read or write ok
ruevs 5:50051611b5bd 168 I2Cstate = 1;
ruevs 5:50051611b5bd 169 gx = (float)gyroCount[0]*gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set
ruevs 5:50051611b5bd 170 gy = (float)gyroCount[1]*gRes - gyroBias[1];
ruevs 5:50051611b5bd 171 gz = (float)gyroCount[2]*gRes - gyroBias[2];
ruevs 5:50051611b5bd 172 }
ruevs 5:50051611b5bd 173
ruevs 5:50051611b5bd 174 mpu9250.readMagData(magCount); // Read the x/y/z adc values
ruevs 5:50051611b5bd 175 // Calculate the magnetometer values in milliGauss
ruevs 5:50051611b5bd 176 // Include factory calibration per data sheet and user environmental corrections
ruevs 5:50051611b5bd 177 if (I2Cstate != 0) //error on I2C
ruevs 5:50051611b5bd 178 LOG("I2C error ocurred while reading magnetometer data. I2Cstate = %d \r\n", I2Cstate);
ruevs 5:50051611b5bd 179 else{ // I2C read or write ok
ruevs 5:50051611b5bd 180 I2Cstate = 1;
ruevs 9:310663c014d8 181 /* Online magnetometer calibration */
ruevs 9:310663c014d8 182 float magAverageXYZRange = 0;
ruevs 9:310663c014d8 183 for(int i=0;i<3;++i) {
ruevs 9:310663c014d8 184 if (magCount[i]<minMagCount[i]) {
ruevs 9:310663c014d8 185 minMagCount[i] = magCount[i];
ruevs 9:310663c014d8 186 magbias[i] = (minMagCount[i]+maxMagCount[i])*mRes*magCalibration[1]/2.;
ruevs 9:310663c014d8 187 }
ruevs 9:310663c014d8 188 if (magCount[i]>maxMagCount[i]) {
ruevs 9:310663c014d8 189 maxMagCount[i] = magCount[i];
ruevs 9:310663c014d8 190 magbias[i] = (minMagCount[i]+maxMagCount[i])*mRes*magCalibration[1]/2.;
ruevs 9:310663c014d8 191 }
ruevs 9:310663c014d8 192 magAverageXYZRange += maxMagCount[i]-minMagCount[i];
ruevs 9:310663c014d8 193 }
ruevs 9:310663c014d8 194
ruevs 9:310663c014d8 195 magAverageXYZRange /= 3.;
ruevs 9:310663c014d8 196
ruevs 9:310663c014d8 197 for(int i=0;i<3;++i) {
ruevs 9:310663c014d8 198 magScale[i] = magAverageXYZRange / (maxMagCount[i]-minMagCount[i]);
ruevs 9:310663c014d8 199 }
ruevs 9:310663c014d8 200
ruevs 9:310663c014d8 201 mx = (float)magCount[0]*mRes*magCalibration[0]*magScale[0] - magbias[0]; // get actual magnetometer value, this depends on scale being set
ruevs 9:310663c014d8 202 my = (float)magCount[1]*mRes*magCalibration[1]*magScale[1] - magbias[1];
ruevs 9:310663c014d8 203 mz = (float)magCount[2]*mRes*magCalibration[2]*magScale[2] - magbias[2];
ruevs 5:50051611b5bd 204 }
ruevs 5:50051611b5bd 205
ruevs 5:50051611b5bd 206 mpu9250.getCompassOrientation(orientation);
ruevs 5:50051611b5bd 207 }
ruevs 5:50051611b5bd 208
ruevs 5:50051611b5bd 209 Now = t.read_us();
ruevs 5:50051611b5bd 210 deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
ruevs 5:50051611b5bd 211 lastUpdate = Now;
ruevs 5:50051611b5bd 212 sum += deltat;
ruevs 5:50051611b5bd 213 sumCount++;
ruevs 5:50051611b5bd 214
ruevs 8:4e8991196bb8 215 // Sensors x (y)-axis of the accelerometer/gyro is aligned with the y (x)-axis of the magnetometer;
ruevs 8:4e8991196bb8 216 // the magnetometer z-axis (+ down) is misaligned with z-axis (+ up) of accelerometer and gyro!
ruevs 8:4e8991196bb8 217 // We have to make some allowance for this orientation mismatch in feeding the output to the quaternion filter.
ruevs 8:4e8991196bb8 218 // We will assume that +y accel/gyro is North, then x accel/gyro is East. So if we want te quaternions properly aligned
ruevs 8:4e8991196bb8 219 // we need to feed into the madgwick function Ay, Ax, -Az, Gy, Gx, -Gz, Mx, My, and Mz. But because gravity is by convention
ruevs 8:4e8991196bb8 220 // positive down, we need to invert the accel data, so we pass -Ay, -Ax, Az, Gy, Gx, -Gz, Mx, My, and Mz into the Madgwick
ruevs 8:4e8991196bb8 221 // function to get North along the accel +y-axis, East along the accel +x-axis, and Down along the accel -z-axis.
ruevs 8:4e8991196bb8 222 // This orientation choice can be modified to allow any convenient (non-NED) orientation convention.
ruevs 8:4e8991196bb8 223 // This is ok by aircraft orientation standards!
ruevs 5:50051611b5bd 224 // Pass gyro rate as rad/s
ruevs 8:4e8991196bb8 225 // mpu9250.MadgwickQuaternionUpdate(-ay, -ax, az, gy*PI/180.0f, gx*PI/180.0f, -gz*PI/180.0f, mx, my, mz);
ruevs 8:4e8991196bb8 226 mpu9250.MahonyQuaternionUpdate(-ay, -ax, az, gy*PI/180.0f, gx*PI/180.0f, -gz*PI/180.0f, mx, my, mz);
ruevs 5:50051611b5bd 227
ruevs 9:310663c014d8 228 white_led= !white_led;
ruevs 5:50051611b5bd 229
ruevs 5:50051611b5bd 230 // Serial print and/or display at 1.5 s rate independent of data rates
ruevs 5:50051611b5bd 231 delt_t = t.read_ms() - count;
ruevs 5:50051611b5bd 232 if (delt_t > 1500) { // update LCD once per half-second independent of read rate
ruevs 7:4931dbfbc042 233 LOG("\033[2J"); // ANSI clear screen ESC[2J
ruevs 5:50051611b5bd 234 LOG("ax = %f", 1000*ax);
ruevs 5:50051611b5bd 235 LOG(" ay = %f", 1000*ay);
ruevs 5:50051611b5bd 236 LOG(" az = %f mg\r\n", 1000*az);
ruevs 5:50051611b5bd 237 LOG("gx = %f", gx);
ruevs 5:50051611b5bd 238 LOG(" gy = %f", gy);
ruevs 5:50051611b5bd 239 LOG(" gz = %f deg/s\r\n", gz);
ruevs 5:50051611b5bd 240 LOG("mx = %f", mx);
ruevs 5:50051611b5bd 241 LOG(" my = %f", my);
ruevs 5:50051611b5bd 242 LOG(" mz = %f mG\r\n", mz);
ruevs 9:310663c014d8 243 LOG("minmx = %i", minMagCount[0]);
ruevs 9:310663c014d8 244 LOG(" minmy = %i", minMagCount[1]);
ruevs 9:310663c014d8 245 LOG(" minmz = %i\r\n", minMagCount[2]);
ruevs 9:310663c014d8 246 LOG("maxmx = %i", maxMagCount[0]);
ruevs 9:310663c014d8 247 LOG(" maxmy = %i", maxMagCount[1]);
ruevs 9:310663c014d8 248 LOG(" maxmz = %i\r\n", maxMagCount[2]);
ruevs 9:310663c014d8 249 LOG("magbiasx = %f", magbias[0]);
ruevs 9:310663c014d8 250 LOG(" magbiasy = %f", magbias[1]);
ruevs 9:310663c014d8 251 LOG(" magbiasz = %f mG\r\n", magbias[2]);
ruevs 9:310663c014d8 252 LOG("magscalex = %f", magScale[0]);
ruevs 9:310663c014d8 253 LOG(" magscaley = %f", magScale[1]);
ruevs 9:310663c014d8 254 LOG(" magscalez = %f mG\r\n", magScale[2]);
ruevs 5:50051611b5bd 255
ruevs 5:50051611b5bd 256
ruevs 5:50051611b5bd 257 tempCount = mpu9250.readTempData(); // Read the adc values
ruevs 5:50051611b5bd 258 if (I2Cstate != 0) //error on I2C
ruevs 5:50051611b5bd 259 LOG("I2C error ocurred while reading sensor temp. I2Cstate = %d \r\n", I2Cstate);
ruevs 5:50051611b5bd 260 else{ // I2C read or write ok
ruevs 5:50051611b5bd 261 I2Cstate = 1;
ruevs 5:50051611b5bd 262 temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
ruevs 5:50051611b5bd 263 LOG(" temperature = %f C\r\n", temperature);
ruevs 5:50051611b5bd 264 }
ruevs 5:50051611b5bd 265 LOG("q0 = %f\r\n", q[0]);
ruevs 5:50051611b5bd 266 LOG("q1 = %f\r\n", q[1]);
ruevs 5:50051611b5bd 267 LOG("q2 = %f\r\n", q[2]);
ruevs 5:50051611b5bd 268 LOG("q3 = %f\r\n", q[3]);
ruevs 5:50051611b5bd 269
ruevs 5:50051611b5bd 270 LOG("Compass orientation: %f\r\n", orientation[0]);
ruevs 5:50051611b5bd 271
ruevs 5:50051611b5bd 272
ruevs 5:50051611b5bd 273
ruevs 5:50051611b5bd 274
ruevs 5:50051611b5bd 275 // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
ruevs 5:50051611b5bd 276 // In this coordinate system, the positive z-axis is down toward Earth.
ruevs 5:50051611b5bd 277 // 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.
ruevs 5:50051611b5bd 278 // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
ruevs 5:50051611b5bd 279 // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
ruevs 5:50051611b5bd 280 // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
ruevs 5:50051611b5bd 281 // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
ruevs 5:50051611b5bd 282 // applied in the correct order which for this configuration is yaw, pitch, and then roll.
ruevs 5:50051611b5bd 283 // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
ruevs 5:50051611b5bd 284
ruevs 5:50051611b5bd 285 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]);
ruevs 5:50051611b5bd 286 pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
ruevs 5:50051611b5bd 287 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]);
ruevs 5:50051611b5bd 288 pitch *= 180.0f / PI;
ruevs 5:50051611b5bd 289 yaw *= 180.0f / PI;
ruevs 5:50051611b5bd 290 yaw -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
ruevs 5:50051611b5bd 291 roll *= 180.0f / PI;
ruevs 5:50051611b5bd 292
ruevs 5:50051611b5bd 293
ruevs 5:50051611b5bd 294 LOG("Yaw, Pitch, Roll: %f %f %f\r\n", yaw, pitch, roll);
ruevs 5:50051611b5bd 295 LOG("average rate = %f\r\n", (float) sumCount/sum);
ruevs 5:50051611b5bd 296
ruevs 5:50051611b5bd 297
ruevs 5:50051611b5bd 298
ruevs 5:50051611b5bd 299 myled= !myled;
ruevs 5:50051611b5bd 300 count = t.read_ms();
ruevs 5:50051611b5bd 301
ruevs 5:50051611b5bd 302 if(count > 1<<21) {
ruevs 5:50051611b5bd 303 t.start(); // start the timer over again if ~30 minutes has passed
ruevs 5:50051611b5bd 304 count = 0;
ruevs 5:50051611b5bd 305 deltat= 0;
ruevs 5:50051611b5bd 306 lastUpdate = t.read_us();
ruevs 5:50051611b5bd 307 }
ruevs 5:50051611b5bd 308 sum = 0;
ruevs 5:50051611b5bd 309 sumCount = 0;
ruevs 5:50051611b5bd 310
ruevs 5:50051611b5bd 311 //BMP180
ruevs 5:50051611b5bd 312 bmp180.startTemperature();
ruevs 5:50051611b5bd 313 wait_ms(5); // Wait for conversion to complete
ruevs 5:50051611b5bd 314 float temp;
ruevs 5:50051611b5bd 315 if(bmp180.getTemperature(&temp) != 0) {
ruevs 5:50051611b5bd 316 LOG("Error getting temperature\n");
ruevs 5:50051611b5bd 317 continue;
ruevs 5:50051611b5bd 318 }
ruevs 5:50051611b5bd 319
ruevs 5:50051611b5bd 320 bmp180.startPressure(BMP180::ULTRA_LOW_POWER);
ruevs 5:50051611b5bd 321 wait_ms(10); // Wait for conversion to complete
ruevs 5:50051611b5bd 322 int pressure;
ruevs 5:50051611b5bd 323 if(bmp180.getPressure(&pressure) != 0) {
ruevs 5:50051611b5bd 324 LOG("Error getting pressure\n");
ruevs 5:50051611b5bd 325 continue;
ruevs 5:50051611b5bd 326 }
ruevs 5:50051611b5bd 327
ruevs 5:50051611b5bd 328 LOG("Pressure = %d Pa Temperature = %f C\r\n", pressure, temp);
ruevs 5:50051611b5bd 329
ruevs 5:50051611b5bd 330 }
ruevs 5:50051611b5bd 331 }
ruevs 5:50051611b5bd 332 }
ruevs 5:50051611b5bd 333
seeed 0:b017c907d53d 334 int main()
seeed 0:b017c907d53d 335 {
ruevs 2:7abdbccdf0c8 336 int readings[MAX_ACC_AXIS] = {0, 0, 0};
ruevs 2:7abdbccdf0c8 337 int MAXreadings[MAX_ACC_AXIS] = {INT16_MIN, INT16_MIN, INT16_MIN};
ruevs 2:7abdbccdf0c8 338 int MINreadings[MAX_ACC_AXIS] = {INT16_MAX, INT16_MAX, INT16_MAX};
ruevs 2:7abdbccdf0c8 339
ruevs 2:7abdbccdf0c8 340 char display_buf[17];
ruevs 2:7abdbccdf0c8 341
ruevs 2:7abdbccdf0c8 342 test_int.rise(test_pin_int);
seeed 0:b017c907d53d 343
ruevs 5:50051611b5bd 344 MPU9250Test();
ruevs 5:50051611b5bd 345
seeed 0:b017c907d53d 346 LOG("Starting ADXL345 test...\n");
seeed 0:b017c907d53d 347 LOG("Device ID is: 0x%02x\n", accelerometer.getDeviceID());
ruevs 2:7abdbccdf0c8 348
ruevs 5:50051611b5bd 349 /* // Simple OLED speed test
ruevs 5:50051611b5bd 350 oled.fillDisplay(0xAA);
ruevs 5:50051611b5bd 351 oled.writeBigChar(1,0,'6');
ruevs 5:50051611b5bd 352
ruevs 5:50051611b5bd 353 uint8_t cnt=0;
ruevs 5:50051611b5bd 354 int cycl=0;
ruevs 5:50051611b5bd 355 while(1){
ruevs 5:50051611b5bd 356 snprintf(display_buf, sizeof(display_buf), "%5i", cnt++);
ruevs 5:50051611b5bd 357 oled.writeString(0, 0, display_buf);
ruevs 5:50051611b5bd 358 if (0==cnt) {
ruevs 5:50051611b5bd 359 ++cycl;
ruevs 5:50051611b5bd 360 snprintf(display_buf, sizeof(display_buf), "%5i", cycl);
ruevs 5:50051611b5bd 361 oled.writeString(1, 0, display_buf);
ruevs 5:50051611b5bd 362 }
ruevs 5:50051611b5bd 363 }
ruevs 5:50051611b5bd 364 */
ruevs 5:50051611b5bd 365 oled2.writeString(0, 0, "OLED 2:");
ruevs 5:50051611b5bd 366 oled2.writeString(1, 0, "GEEKCREIT");
ruevs 5:50051611b5bd 367 oled2.writeString(2, 0, "Banggood 0x7A");
ruevs 5:50051611b5bd 368
ruevs 2:7abdbccdf0c8 369 oled.writeString(0, 0, "Accelerometer:");
ruevs 2:7abdbccdf0c8 370 oled.writeString(1, 0, " Curr Min Max");
seeed 0:b017c907d53d 371
seeed 0:b017c907d53d 372 //Go into standby mode to configure the device.
seeed 0:b017c907d53d 373 accelerometer.setPowerControl(0x00);
seeed 0:b017c907d53d 374
seeed 0:b017c907d53d 375 //Full resolution, +/-16g, 4mg/LSB.
seeed 0:b017c907d53d 376 accelerometer.setDataFormatControl(0x0B);
seeed 0:b017c907d53d 377
seeed 0:b017c907d53d 378 //3.2kHz data rate.
seeed 0:b017c907d53d 379 accelerometer.setDataRate(ADXL345_3200HZ);
seeed 0:b017c907d53d 380
seeed 0:b017c907d53d 381 //Measurement mode.
seeed 0:b017c907d53d 382 accelerometer.setPowerControl(0x08);
seeed 0:b017c907d53d 383
seeed 0:b017c907d53d 384 while (1) {
seeed 0:b017c907d53d 385 accelerometer.getOutput(readings);
ruevs 2:7abdbccdf0c8 386
ruevs 2:7abdbccdf0c8 387 for (int i=0; i<MAX_ACC_AXIS; ++i) {
ruevs 2:7abdbccdf0c8 388 if ((int16_t)readings[i] > (int16_t)MAXreadings[i]) {
ruevs 2:7abdbccdf0c8 389 MAXreadings[i] = readings[i];
ruevs 2:7abdbccdf0c8 390 }
ruevs 2:7abdbccdf0c8 391 if ((int16_t)readings[i] < (int16_t)MINreadings[i]) {
ruevs 2:7abdbccdf0c8 392 MINreadings[i] = readings[i];
ruevs 2:7abdbccdf0c8 393 }
ruevs 2:7abdbccdf0c8 394
ruevs 2:7abdbccdf0c8 395 snprintf(display_buf, sizeof(display_buf), "%c%5i%5i%5i",
ruevs 2:7abdbccdf0c8 396 'X'+i, (int16_t)readings[i], (int16_t)MINreadings[i], (int16_t)MAXreadings[i] );
ruevs 2:7abdbccdf0c8 397 oled.writeString(2+i, 0, display_buf);
ruevs 2:7abdbccdf0c8 398 LOG("%c:%i|%i|%i\r\n", 'X'+i, (int16_t)readings[i], (int16_t)MINreadings[i], (int16_t)MAXreadings[i] );
ruevs 2:7abdbccdf0c8 399 }
ruevs 2:7abdbccdf0c8 400
ruevs 2:7abdbccdf0c8 401 /* snprintf(display_buf, sizeof(display_buf), "Ch: %u", ChargerStatus.read_u16());
ruevs 2:7abdbccdf0c8 402 oled.writeString(2+MAX_ACC_AXIS, 0, display_buf);
ruevs 2:7abdbccdf0c8 403
ruevs 2:7abdbccdf0c8 404 snprintf(display_buf, sizeof(display_buf), "Ch: %f", 3.3*(float)ChargerStatus.read_u16()/UINT16_MAX);
ruevs 2:7abdbccdf0c8 405 oled.writeString(2+MAX_ACC_AXIS+1, 0, display_buf);
ruevs 2:7abdbccdf0c8 406 */
ruevs 2:7abdbccdf0c8 407 snprintf(display_buf, sizeof(display_buf), "Bat:%.2fV", 3.3*ChargerStatus);
ruevs 2:7abdbccdf0c8 408 oled.writeString(2+MAX_ACC_AXIS+2, 0, display_buf);
ruevs 2:7abdbccdf0c8 409
ruevs 2:7abdbccdf0c8 410 LOG("Ch: %f\r\n", ChargerStatus.read());
ruevs 2:7abdbccdf0c8 411 LOG("Chu: %u\r\n", ChargerStatus.read_u16());
ruevs 2:7abdbccdf0c8 412 LOG("A0: %f\r\n", (float)AD00.read());
ruevs 2:7abdbccdf0c8 413 LOG("A1: %f\r\n", AD01.read());
ruevs 2:7abdbccdf0c8 414
ruevs 2:7abdbccdf0c8 415 blue_led = !blue_led;
ruevs 2:7abdbccdf0c8 416 // white_led = !white_led; // toggled by pin P0_7 interrupt
ruevs 2:7abdbccdf0c8 417
ruevs 2:7abdbccdf0c8 418 // deepsleep(); // Deep sleep until external interrupt // The wakeup pin on Xadow is on the sme buton at RESET - no good.
seeed 0:b017c907d53d 419
seeed 0:b017c907d53d 420 wait(1);
seeed 0:b017c907d53d 421 }
seeed 0:b017c907d53d 422
seeed 0:b017c907d53d 423 }