Kris Winer
Added mag calibration and interrupt-based data ready
Dependencies: BLE_API mbed-src nRF51822
Revision 4:8d11bfc7cac0, committed 2016-09-22
- Comitter:
- onehorse
- Date:
- Thu Sep 22 01:21:24 2016 +0000
- Parent:
- 3:fe46f14f5aef
- Commit message:
- Added mag calibration and interrupt-based data ready
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/BMP280.h Thu Sep 22 01:21:24 2016 +0000
@@ -0,0 +1,199 @@
+#ifndef BMP280_H
+#define BMP280_H
+
+//#include "mbed.h"
+
+// BMP280 registers
+#define BMP280_TEMP_XLSB 0xFC
+#define BMP280_TEMP_LSB 0xFB
+#define BMP280_TEMP_MSB 0xFA
+#define BMP280_PRESS_XLSB 0xF9
+#define BMP280_PRESS_LSB 0xF8
+#define BMP280_PRESS_MSB 0xF7
+#define BMP280_CONFIG 0xF5
+#define BMP280_CTRL_MEAS 0xF4
+#define BMP280_STATUS 0xF3
+#define BMP280_RESET 0xE0
+#define BMP280_ID 0xD0 // should be 0x58
+#define BMP280_CALIB00 0x88
+#define BMP280_ADDRESS 0x77<<1
+
+// Set initial input parameters
+
+enum Posr {
+ P_OSR_00 = 0, // no op
+ P_OSR_01,
+ P_OSR_02,
+ P_OSR_04,
+ P_OSR_08,
+ P_OSR_16
+};
+
+enum Tosr {
+ T_OSR_00 = 0, // no op
+ T_OSR_01,
+ T_OSR_02,
+ T_OSR_04,
+ T_OSR_08,
+ T_OSR_16
+};
+
+enum IIRFilter {
+ full = 0, // bandwidth at full sample rate
+ BW0_223ODR,
+ BW0_092ODR,
+ BW0_042ODR,
+ BW0_021ODR // bandwidth at 0.021 x sample rate
+};
+
+enum Mode {
+ BMP280Sleep = 0,
+ forced,
+ forced2,
+ normal
+};
+
+enum SBy {
+ t_00_5ms = 0,
+ t_62_5ms,
+ t_125ms,
+ t_250ms,
+ t_500ms,
+ t_1000ms,
+ t_2000ms,
+ t_4000ms,
+};
+
+// Specify BMP280 configuration
+uint8_t Posr = P_OSR_16, Tosr = T_OSR_02, Mode = normal, IIRFilter = BW0_042ODR, SBy = t_62_5ms; // set pressure amd temperature output data rate
+// t_fine carries fine temperature as global value for BMP280
+int32_t t_fine;
+
+//Set up I2C, (SDA,SCL)
+//I2C i2c(P0_0, P0_1);
+
+// BMP280 compensation parameters
+uint16_t dig_T1, dig_P1;
+int16_t dig_T2, dig_T3, dig_P2, dig_P3, dig_P4, dig_P5, dig_P6, dig_P7, dig_P8, dig_P9;
+
+class BMP280 {
+
+ protected:
+
+ public:
+ //===================================================================================================================
+//====== Set of useful function to access pressure and temperature data
+//===================================================================================================================
+
+
+ void writeByte(uint8_t address, uint8_t subAddress, uint8_t data)
+{
+ char data_write[2];
+ data_write[0] = subAddress;
+ data_write[1] = data;
+ i2c.write(address, data_write, 2, 0);
+}
+
+ char readByte(uint8_t address, uint8_t subAddress)
+{
+ char data[1]; // `data` will store the register data
+ char data_write[1];
+ data_write[0] = subAddress;
+ i2c.write(address, data_write, 1, 1); // no stop
+ i2c.read(address, data, 1, 0);
+ return data[0];
+}
+
+ void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest)
+{
+ char data[24];
+ char data_write[1];
+ data_write[0] = subAddress;
+ i2c.write(address, data_write, 1, 1); // no stop
+ i2c.read(address, data, count, 0);
+ for(int ii = 0; ii < count; ii++) {
+ dest[ii] = data[ii];
+ }
+}
+
+
+int32_t readBMP280Temperature()
+{
+ uint8_t rawData[3]; // 20-bit pressure register data stored here
+ readBytes(BMP280_ADDRESS, BMP280_TEMP_MSB, 3, &rawData[0]);
+ return (int32_t) (((int32_t) rawData[0] << 16 | (int32_t) rawData[1] << 8 | rawData[2]) >> 4);
+}
+
+int32_t readBMP280Pressure()
+{
+ uint8_t rawData[3]; // 20-bit pressure register data stored here
+ readBytes(BMP280_ADDRESS, BMP280_PRESS_MSB, 3, &rawData[0]);
+ return (int32_t) (((int32_t) rawData[0] << 16 | (int32_t) rawData[1] << 8 | rawData[2]) >> 4);
+}
+
+void BMP280Init()
+{
+ // Configure the BMP280
+ // Set T and P oversampling rates and sensor mode
+ writeByte(BMP280_ADDRESS, BMP280_CTRL_MEAS, Tosr << 5 | Posr << 2 | Mode);
+ // Set standby time interval in normal mode and bandwidth
+ writeByte(BMP280_ADDRESS, BMP280_CONFIG, SBy << 5 | IIRFilter << 2);
+ // Read and store calibration data
+ uint8_t calib[24];
+ readBytes(BMP280_ADDRESS, BMP280_CALIB00, 24, &calib[0]);
+ dig_T1 = (uint16_t)(((uint16_t) calib[1] << 8) | calib[0]);
+ dig_T2 = ( int16_t)((( int16_t) calib[3] << 8) | calib[2]);
+ dig_T3 = ( int16_t)((( int16_t) calib[5] << 8) | calib[4]);
+ dig_P1 = (uint16_t)(((uint16_t) calib[7] << 8) | calib[6]);
+ dig_P2 = ( int16_t)((( int16_t) calib[9] << 8) | calib[8]);
+ dig_P3 = ( int16_t)((( int16_t) calib[11] << 8) | calib[10]);
+ dig_P4 = ( int16_t)((( int16_t) calib[13] << 8) | calib[12]);
+ dig_P5 = ( int16_t)((( int16_t) calib[15] << 8) | calib[14]);
+ dig_P6 = ( int16_t)((( int16_t) calib[17] << 8) | calib[16]);
+ dig_P7 = ( int16_t)((( int16_t) calib[19] << 8) | calib[18]);
+ dig_P8 = ( int16_t)((( int16_t) calib[21] << 8) | calib[20]);
+ dig_P9 = ( int16_t)((( int16_t) calib[23] << 8) | calib[22]);
+}
+
+// Returns temperature in DegC, resolution is 0.01 DegC. Output value of
+// “5123” equals 51.23 DegC.
+int32_t bmp280_compensate_T(int32_t adc_T)
+{
+ int32_t var1, var2, T;
+ var1 = ((((adc_T >> 3) - ((int32_t)dig_T1 << 1))) * ((int32_t)dig_T2)) >> 11;
+ var2 = (((((adc_T >> 4) - ((int32_t)dig_T1)) * ((adc_T >> 4) - ((int32_t)dig_T1))) >> 12) * ((int32_t)dig_T3)) >> 14;
+ t_fine = var1 + var2;
+ T = (t_fine * 5 + 128) >> 8;
+ return T;
+}
+
+// Returns pressure in Pa as unsigned 32 bit integer in Q24.8 format (24 integer bits and 8
+//fractional bits).
+//Output value of “24674867” represents 24674867/256 = 96386.2 Pa = 963.862 hPa
+uint32_t bmp280_compensate_P(int32_t adc_P)
+{
+ long long var1, var2, p;
+ var1 = ((long long)t_fine) - 128000;
+ var2 = var1 * var1 * (long long)dig_P6;
+ var2 = var2 + ((var1*(long long)dig_P5)<<17);
+ var2 = var2 + (((long long)dig_P4)<<35);
+ var1 = ((var1 * var1 * (long long)dig_P3)>>8) + ((var1 * (long long)dig_P2)<<12);
+ var1 = (((((long long)1)<<47)+var1))*((long long)dig_P1)>>33;
+ if(var1 == 0)
+ {
+ return 0;
+ // avoid exception caused by division by zero
+ }
+ p = 1048576 - adc_P;
+ p = (((p<<31) - var2)*3125)/var1;
+ var1 = (((long long)dig_P9) * (p>>13) * (p>>13)) >> 25;
+ var2 = (((long long)dig_P8) * p)>> 19;
+ p = ((p + var1 + var2) >> 8) + (((long long)dig_P7)<<4);
+ return (uint32_t)p;
+}
+
+
+
+
+ };
+#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/EM7180.h Thu Sep 22 01:21:24 2016 +0000
@@ -0,0 +1,124 @@
+#ifndef EM7180_H
+#define EM7180_H
+
+//#include "mbed.h"
+
+// EM7180 SENtral register map
+// see http://www.emdeveloper.com/downloads/7180/EMSentral_EM7180_Register_Map_v1_3.pdf
+//
+#define EM7180_QX 0x00 // this is a 32-bit normalized floating point number read from registers 0x00-03
+#define EM7180_QY 0x04 // this is a 32-bit normalized floating point number read from registers 0x04-07
+#define EM7180_QZ 0x08 // this is a 32-bit normalized floating point number read from registers 0x08-0B
+#define EM7180_QW 0x0C // this is a 32-bit normalized floating point number read from registers 0x0C-0F
+#define EM7180_QTIME 0x10 // this is a 16-bit unsigned integer read from registers 0x10-11
+#define EM7180_MX 0x12 // int16_t from registers 0x12-13
+#define EM7180_MY 0x14 // int16_t from registers 0x14-15
+#define EM7180_MZ 0x16 // int16_t from registers 0x16-17
+#define EM7180_MTIME 0x18 // uint16_t from registers 0x18-19
+#define EM7180_AX 0x1A // int16_t from registers 0x1A-1B
+#define EM7180_AY 0x1C // int16_t from registers 0x1C-1D
+#define EM7180_AZ 0x1E // int16_t from registers 0x1E-1F
+#define EM7180_ATIME 0x20 // uint16_t from registers 0x20-21
+#define EM7180_GX 0x22 // int16_t from registers 0x22-23
+#define EM7180_GY 0x24 // int16_t from registers 0x24-25
+#define EM7180_GZ 0x26 // int16_t from registers 0x26-27
+#define EM7180_GTIME 0x28 // uint16_t from registers 0x28-29
+#define EM7180_Baro 0x2A // start of two-byte MS5637 pressure data, 16-bit signed interger
+#define EM7180_BaroTIME 0x2C // start of two-byte MS5637 pressure timestamp, 16-bit unsigned
+#define EM7180_Temp 0x2E // start of two-byte MS5637 temperature data, 16-bit signed interger
+#define EM7180_TempTIME 0x30 // start of two-byte MS5637 temperature timestamp, 16-bit unsigned
+#define EM7180_QRateDivisor 0x32 // uint8_t
+#define EM7180_EnableEvents 0x33
+#define EM7180_HostControl 0x34
+#define EM7180_EventStatus 0x35
+#define EM7180_SensorStatus 0x36
+#define EM7180_SentralStatus 0x37
+#define EM7180_AlgorithmStatus 0x38
+#define EM7180_FeatureFlags 0x39
+#define EM7180_ParamAcknowledge 0x3A
+#define EM7180_SavedParamByte0 0x3B
+#define EM7180_SavedParamByte1 0x3C
+#define EM7180_SavedParamByte2 0x3D
+#define EM7180_SavedParamByte3 0x3E
+#define EM7180_ActualMagRate 0x45
+#define EM7180_ActualAccelRate 0x46
+#define EM7180_ActualGyroRate 0x47
+#define EM7180_ActualBaroRate 0x48
+#define EM7180_ActualTempRate 0x49
+#define EM7180_ErrorRegister 0x50
+#define EM7180_AlgorithmControl 0x54
+#define EM7180_MagRate 0x55
+#define EM7180_AccelRate 0x56
+#define EM7180_GyroRate 0x57
+#define EM7180_BaroRate 0x58
+#define EM7180_TempRate 0x59
+#define EM7180_LoadParamByte0 0x60
+#define EM7180_LoadParamByte1 0x61
+#define EM7180_LoadParamByte2 0x62
+#define EM7180_LoadParamByte3 0x63
+#define EM7180_ParamRequest 0x64
+#define EM7180_ROMVersion1 0x70
+#define EM7180_ROMVersion2 0x71
+#define EM7180_RAMVersion1 0x72
+#define EM7180_RAMVersion2 0x73
+#define EM7180_ProductID 0x90
+#define EM7180_RevisionID 0x91
+#define EM7180_RunStatus 0x92
+#define EM7180_UploadAddress 0x94 // uint16_t registers 0x94 (MSB)-5(LSB)
+#define EM7180_UploadData 0x96
+#define EM7180_CRCHost 0x97 // uint32_t from registers 0x97-9A
+#define EM7180_ResetRequest 0x9B
+#define EM7180_PassThruStatus 0x9E
+#define EM7180_PassThruControl 0xA0
+#define EM7180_ACC_LPF_BW 0x5B //Register GP36
+#define EM7180_GYRO_LPF_BW 0x5C //Register GP37
+#define EM7180_BARO_LPF_BW 0x5D //Register GP38
+
+#define EM7180_ADDRESS 0x28<<1 // Address of the EM7180 SENtral sensor hub
+
+
+class EM7180 {
+
+ protected:
+
+ public:
+ //===================================================================================================================
+//====== Set of useful function to access pressure and temperature data
+//===================================================================================================================
+
+
+ void writeByte(uint8_t address, uint8_t subAddress, uint8_t data)
+{
+ char data_write[2];
+ data_write[0] = subAddress;
+ data_write[1] = data;
+ i2c.write(address, data_write, 2, 0);
+}
+
+ char readByte(uint8_t address, uint8_t subAddress)
+{
+ char data[1]; // `data` will store the register data
+ char data_write[1];
+ data_write[0] = subAddress;
+ i2c.write(address, data_write, 1, 1); // no stop
+ i2c.read(address, data, 1, 0);
+ return data[0];
+}
+
+ void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest)
+{
+ char data[24];
+ char data_write[1];
+ data_write[0] = subAddress;
+ i2c.write(address, data_write, 1, 1); // no stop
+ i2c.read(address, data, count, 0);
+ for(int ii = 0; ii < count; ii++) {
+ dest[ii] = data[ii];
+ }
+}
+
+
+
+
+ };
+#endif
\ No newline at end of file
--- a/MPU9250.h Mon Jun 15 21:34:33 2015 +0000
+++ b/MPU9250.h Thu Sep 22 01:21:24 2016 +0000
@@ -191,18 +191,24 @@
//Set up I2C, (SDA,SCL)
//I2C i2c(I2C_SDA, I2C_SCL);
-I2C i2c(P0_20, P0_22);
+//I2C i2c(P0_0, P0_1); // nRF51 FC
+//I2C i2c(P0_6, P0_7); // nRF52 QFN dev board
+I2C i2c(P0_6, P0_5); // nRF52 CIAA dev board
-DigitalOut myled(LED1);
+//DigitalOut myled(P0_19); // mbed kit
+//DigitalOut myled(P0_18); // nRF51822 module
+DigitalOut myled(P0_24); // nRF52832 module
// Pin definitions
-int intPin = 12; // These can be changed, 2 and 3 are the Arduinos ext int pins
+bool newData = false;
+bool newMagData = false;
+int16_t MPU9250Data[7]; // used to read all 14 bytes at once from the MPU9250 accel/gyro
int16_t accelCount[3]; // Stores the 16-bit signed accelerometer sensor output
int16_t gyroCount[3]; // Stores the 16-bit signed gyro sensor output
int16_t magCount[3]; // Stores the 16-bit signed magnetometer sensor output
float magCalibration[3] = {0, 0, 0}, magbias[3] = {0, 0, 0}; // Factory mag calibration and mag bias
-float gyroBias[3] = {0, 0, 0}, accelBias[3] = {0, 0, 0}; // Bias corrections for gyro and accelerometer
+float gyroBias[3] = {0, 0, 0}, accelBias[3] = {0, 0, 0}, magBias[3] = {0, 0, 0}, magScale[3] = {0, 0, 0}; // Bias corrections for gyro and accelerometer
float ax, ay, az, gx, gy, gz, mx, my, mz; // variables to hold latest sensor data values
int16_t tempCount; // Stores the real internal chip temperature in degrees Celsius
float temperature;
@@ -235,7 +241,7 @@
//====== Set of useful function to access acceleratio, gyroscope, and temperature data
//===================================================================================================================
- void writeByte(uint8_t address, uint8_t subAddress, uint8_t data)
+ void writeByte(uint8_t address, uint8_t subAddress, uint8_t data)
{
char data_write[2];
data_write[0] = subAddress;
@@ -272,10 +278,10 @@
// Possible magnetometer scales (and their register bit settings) are:
// 14 bit resolution (0) and 16 bit resolution (1)
case MFS_14BITS:
- mRes = 10.0*4219.0/8190.0; // Proper scale to return milliGauss
+ mRes = 10.0*4912.0/8190.0; // Proper scale to return milliGauss
break;
case MFS_16BITS:
- mRes = 10.0*4219.0/32760.0; // Proper scale to return milliGauss
+ mRes = 10.0*4912.0/32760.0; // Proper scale to return milliGauss
break;
}
}
@@ -288,16 +294,16 @@
// 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS (11).
// Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value:
case GFS_250DPS:
- gRes = 250.0/32768.0;
+ gRes = 250.0f/32768.0f;
break;
case GFS_500DPS:
- gRes = 500.0/32768.0;
+ gRes = 500.0f/32768.0f;
break;
case GFS_1000DPS:
- gRes = 1000.0/32768.0;
+ gRes = 1000.0f/32768.0f;
break;
case GFS_2000DPS:
- gRes = 2000.0/32768.0;
+ gRes = 2000.0f/32768.0f;
break;
}
}
@@ -310,20 +316,32 @@
// 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs (11).
// Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value:
case AFS_2G:
- aRes = 2.0/32768.0;
+ aRes = 2.0f/32768.0f;
break;
case AFS_4G:
- aRes = 4.0/32768.0;
+ aRes = 4.0f/32768.0f;
break;
case AFS_8G:
- aRes = 8.0/32768.0;
+ aRes = 8.0f/32768.0f;
break;
case AFS_16G:
- aRes = 16.0/32768.0;
+ aRes = 16.0f/32768.0f;
break;
}
}
+void readMPU9250Data(int16_t * destination)
+{
+ uint8_t rawData[14]; // x/y/z accel register data stored here
+ readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 14, &rawData[0]); // Read the 14 raw data registers into data array
+ destination[0] = ((int16_t)rawData[0] << 8) | rawData[1] ; // Turn the MSB and LSB into a signed 16-bit value
+ destination[1] = ((int16_t)rawData[2] << 8) | rawData[3] ;
+ destination[2] = ((int16_t)rawData[4] << 8) | rawData[5] ;
+ destination[3] = ((int16_t)rawData[6] << 8) | rawData[7] ;
+ destination[4] = ((int16_t)rawData[8] << 8) | rawData[9] ;
+ destination[5] = ((int16_t)rawData[10] << 8) | rawData[11] ;
+ destination[6] = ((int16_t)rawData[12] << 8) | rawData[13] ;
+}
void readAccelData(int16_t * destination)
{
@@ -346,15 +364,13 @@
void readMagData(int16_t * destination)
{
uint8_t rawData[7]; // x/y/z gyro register data, ST2 register stored here, must read ST2 at end of data acquisition
- if(readByte(AK8963_ADDRESS, AK8963_ST1) & 0x01) { // wait for magnetometer data ready bit to be set
readBytes(AK8963_ADDRESS, AK8963_XOUT_L, 7, &rawData[0]); // Read the six raw data and ST2 registers sequentially into data array
uint8_t c = rawData[6]; // End data read by reading ST2 register
if(!(c & 0x08)) { // Check if magnetic sensor overflow set, if not then report data
- destination[0] = (int16_t)(((int16_t)rawData[1] << 8) | rawData[0]); // Turn the MSB and LSB into a signed 16-bit value
- destination[1] = (int16_t)(((int16_t)rawData[3] << 8) | rawData[2]) ; // Data stored as little Endian
- destination[2] = (int16_t)(((int16_t)rawData[5] << 8) | rawData[4]) ;
+ destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ; // Turn the MSB and LSB into a signed 16-bit value
+ destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ; // Data stored as little Endian
+ destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ;
}
- }
}
int16_t readTempData()
@@ -374,12 +390,15 @@
void initAK8963(float * destination)
{
// First extract the factory calibration for each magnetometer axis
- uint8_t rawData[3]; // x/y/z gyro calibration data stored here
+ uint8_t rawData[3] = {0, 0, 0}; // x/y/z gyro calibration data stored here
writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer
wait(0.01);
writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x0F); // Enter Fuse ROM access mode
wait(0.01);
- readBytes(AK8963_ADDRESS, AK8963_ASAX, 3, &rawData[0]); // Read the x-, y-, and z-axis calibration values
+ // readBytes(AK8963_ADDRESS, AK8963_ASAX, 3, &rawData[0]); // Read the x-, y-, and z-axis calibration values
+ rawData[0] = readByte(AK8963_ADDRESS, AK8963_ASAX);
+ rawData[1] = readByte(AK8963_ADDRESS, AK8963_ASAY);
+ rawData[2] = readByte(AK8963_ADDRESS, AK8963_ASAZ);
destination[0] = (float)(rawData[0] - 128)/256.0f + 1.0f; // Return x-axis sensitivity adjustment values, etc.
destination[1] = (float)(rawData[1] - 128)/256.0f + 1.0f;
destination[2] = (float)(rawData[2] - 128)/256.0f + 1.0f;
@@ -438,8 +457,8 @@
// Configure Interrupts and Bypass Enable
// Set interrupt pin active high, push-pull, and clear on read of INT_STATUS, enable I2C_BYPASS_EN so additional chips
// can join the I2C bus and all can be controlled by the Arduino as master
- writeByte(MPU9250_ADDRESS, INT_PIN_CFG, 0x22);
- writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x01); // Enable data ready (bit 0) interrupt
+ writeByte(MPU9250_ADDRESS, INT_PIN_CFG, 0x12); // INT is 50 microsecond pulse and any read to clear
+ writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x01); // Enable data ready (bit 0) interrupt
}
// Function which accumulates gyro and accelerometer data after device initialization. It calculates the average
@@ -589,13 +608,57 @@
dest2[2] = (float)accel_bias[2]/(float)accelsensitivity;
}
+void magcalMPU9250(float * dest1, float * dest2)
+{
+ uint16_t ii = 0, sample_count = 0;
+ int32_t mag_bias[3] = {0, 0, 0}, mag_scale[3] = {0, 0, 0};
+ int16_t mag_max[3] = {-32767, -32767, -32767}, mag_min[3] = {32767, 32767, 32767}, mag_temp[3] = {0, 0, 0};
+
+ // shoot for ~fifteen seconds of mag data
+ if(Mmode == 0x02) sample_count = 128; // at 8 Hz ODR, new mag data is available every 125 ms
+ if(Mmode == 0x06) sample_count = 1500; // at 100 Hz ODR, new mag data is available every 10 ms
+
+ for(ii = 0; ii < sample_count; ii++) {
+ readMagData(mag_temp); // Read the mag data
+
+ for (int jj = 0; jj < 3; jj++) {
+ if(mag_temp[jj] > mag_max[jj]) mag_max[jj] = mag_temp[jj];
+ if(mag_temp[jj] < mag_min[jj]) mag_min[jj] = mag_temp[jj];
+ }
+
+ if(Mmode == 0x02) wait(0.135); // at 8 Hz ODR, new mag data is available every 125 ms
+ if(Mmode == 0x06) wait(0.012); // at 100 Hz ODR, new mag data is available every 10 ms
+ }
+
+ // Get hard iron correction
+ mag_bias[0] = (mag_max[0] + mag_min[0])/2; // get average x mag bias in counts
+ mag_bias[1] = (mag_max[1] + mag_min[1])/2; // get average y mag bias in counts
+ mag_bias[2] = (mag_max[2] + mag_min[2])/2; // get average z mag bias in counts
+
+ dest1[0] = (float) mag_bias[0]*mRes*magCalibration[0]; // save mag biases in G for main program
+ dest1[1] = (float) mag_bias[1]*mRes*magCalibration[1];
+ dest1[2] = (float) mag_bias[2]*mRes*magCalibration[2];
+
+ // Get soft iron correction estimate
+ mag_scale[0] = (mag_max[0] - mag_min[0])/2; // get average x axis max chord length in counts
+ mag_scale[1] = (mag_max[1] - mag_min[1])/2; // get average y axis max chord length in counts
+ mag_scale[2] = (mag_max[2] - mag_min[2])/2; // get average z axis max chord length in counts
+
+ float avg_rad = mag_scale[0] + mag_scale[1] + mag_scale[2];
+ avg_rad /= 3.0;
+
+ dest2[0] = avg_rad/((float)mag_scale[0]);
+ dest2[1] = avg_rad/((float)mag_scale[1]);
+ dest2[2] = avg_rad/((float)mag_scale[2]);
+}
+
// Accelerometer and gyroscope self test; check calibration wrt factory settings
void MPU9250SelfTest(float * destination) // Should return percent deviation from factory trim values, +/- 14 or less deviation is a pass
{
uint8_t rawData[6] = {0, 0, 0, 0, 0, 0};
uint8_t selfTest[6];
- int16_t gAvg[3], aAvg[3], aSTAvg[3], gSTAvg[3];
+ int32_t gAvg[3] = {0, 0, 0}, aAvg[3] = {0, 0, 0}, aSTAvg[3] = {0, 0, 0}, gSTAvg[3] = {0, 0, 0};
float factoryTrim[6];
uint8_t FS = 0;
@@ -670,8 +733,8 @@
// Report results as a ratio of (STR - FT)/FT; the change from Factory Trim of the Self-Test Response
// To get percent, must multiply by 100
for (int i = 0; i < 3; i++) {
- destination[i] = 100.0*((float)(aSTAvg[i] - aAvg[i]))/factoryTrim[i]; // Report percent differences
- destination[i+3] = 100.0*((float)(gSTAvg[i] - gAvg[i]))/factoryTrim[i+3]; // Report percent differences
+ destination[i] = 100.0f*((float)(aSTAvg[i] - aAvg[i]))/factoryTrim[i] - 100.0f; // Report percent differences
+ destination[i+3] = 100.0f*((float)(gSTAvg[i] - gAvg[i]))/factoryTrim[i+3] - 100.0f; // Report percent differences
}
}
@@ -717,7 +780,7 @@
float q4q4 = q4 * q4;
// Normalise accelerometer measurement
- norm = sqrt(ax * ax + ay * ay + az * az);
+ norm = sqrtf(ax * ax + ay * ay + az * az);
if (norm == 0.0f) return; // handle NaN
norm = 1.0f/norm;
ax *= norm;
@@ -725,7 +788,7 @@
az *= norm;
// Normalise magnetometer measurement
- norm = sqrt(mx * mx + my * my + mz * mz);
+ norm = sqrtf(mx * mx + my * my + mz * mz);
if (norm == 0.0f) return; // handle NaN
norm = 1.0f/norm;
mx *= norm;
@@ -749,7 +812,7 @@
s2 = _2q4 * (2.0f * q2q4 - _2q1q3 - ax) + _2q1 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q2 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
s3 = -_2q1 * (2.0f * q2q4 - _2q1q3 - ax) + _2q4 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q3 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
s4 = _2q2 * (2.0f * q2q4 - _2q1q3 - ax) + _2q3 * (2.0f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
- norm = sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4); // normalise step magnitude
+ norm = sqrtf(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4); // normalise step magnitude
norm = 1.0f/norm;
s1 *= norm;
s2 *= norm;
@@ -767,7 +830,7 @@
q2 += qDot2 * deltat;
q3 += qDot3 * deltat;
q4 += qDot4 * deltat;
- norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion
+ norm = sqrtf(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion
norm = 1.0f/norm;
q[0] = q1 * norm;
q[1] = q2 * norm;
@@ -802,7 +865,7 @@
float q4q4 = q4 * q4;
// Normalise accelerometer measurement
- norm = sqrt(ax * ax + ay * ay + az * az);
+ norm = sqrtf(ax * ax + ay * ay + az * az);
if (norm == 0.0f) return; // handle NaN
norm = 1.0f / norm; // use reciprocal for division
ax *= norm;
@@ -810,7 +873,7 @@
az *= norm;
// Normalise magnetometer measurement
- norm = sqrt(mx * mx + my * my + mz * mz);
+ norm = sqrtf(mx * mx + my * my + mz * mz);
if (norm == 0.0f) return; // handle NaN
norm = 1.0f / norm; // use reciprocal for division
mx *= norm;
@@ -820,7 +883,7 @@
// Reference direction of Earth's magnetic field
hx = 2.0f * mx * (0.5f - q3q3 - q4q4) + 2.0f * my * (q2q3 - q1q4) + 2.0f * mz * (q2q4 + q1q3);
hy = 2.0f * mx * (q2q3 + q1q4) + 2.0f * my * (0.5f - q2q2 - q4q4) + 2.0f * mz * (q3q4 - q1q2);
- bx = sqrt((hx * hx) + (hy * hy));
+ bx = sqrtf((hx * hx) + (hy * hy));
bz = 2.0f * mx * (q2q4 - q1q3) + 2.0f * my * (q3q4 + q1q2) + 2.0f * mz * (0.5f - q2q2 - q3q3);
// Estimated direction of gravity and magnetic field
@@ -863,7 +926,7 @@
q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * deltat);
// Normalise quaternion
- norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
+ norm = sqrtf(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
norm = 1.0f / norm;
q[0] = q1 * norm;
q[1] = q2 * norm;
--- a/main.cpp Mon Jun 15 21:34:33 2015 +0000
+++ b/main.cpp Thu Sep 22 01:21:24 2016 +0000
@@ -1,98 +1,141 @@
/* MPU9250 Basic Example Code
by: Kris Winer
- date: April 1, 2014
+ date: September 20, 2016
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.
+ Mahony filter algorithms.
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);
#include "mbed.h"
#include "MPU9250.h"
-#include "N5110.h"
+#include "BMP280.h"
+#include "math.h"
-// 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);
+ MPU9250 mpu9250; // Instantiate MPU9250 class
+
+ BMP280 bmp280; // Instantiate BMP280 class
+
+ Timer t;
+
+ InterruptIn myInterrupt(P0_8); // One nRF52 Dev Board variant uses pin 8, one uses pin 10
+
+/* Serial pc(USBTX, USBRX); // tx, rx*/
+ Serial pc(P0_12, P0_14); // tx, rx
float sum = 0;
uint32_t sumCount = 0;
char buffer[14];
+uint8_t whoami = 0;
+double Temperature, Pressure; // stores BMP280 pressures sensor pressure and temperature
+int32_t rawPress, rawTemp; // pressure and temperature raw count output for BMP280
- MPU9250 mpu9250;
-
- Timer t;
+int32_t readBMP280Temperature()
+{
+ uint8_t rawData[3]; // 20-bit pressure register data stored here
+ bmp280.readBytes(BMP280_ADDRESS, BMP280_TEMP_MSB, 3, &rawData[0]);
+ return (int32_t) (((int32_t) rawData[0] << 16 | (int32_t) rawData[1] << 8 | rawData[2]) >> 4);
+}
+
+int32_t readBMP280Pressure()
+{
+ uint8_t rawData[3]; // 20-bit pressure register data stored here
+ bmp280.readBytes(BMP280_ADDRESS, BMP280_PRESS_MSB, 3, &rawData[0]);
+ return (int32_t) (((int32_t) rawData[0] << 16 | (int32_t) rawData[1] << 8 | rawData[2]) >> 4);
+}
+
+// Returns temperature in DegC, resolution is 0.01 DegC. Output value of
+// “5123” equals 51.23 DegC.
+int32_t bmp280_compensate_T(int32_t adc_T)
+{
+ int32_t var1, var2, T;
+ var1 = ((((adc_T >> 3) - ((int32_t)dig_T1 << 1))) * ((int32_t)dig_T2)) >> 11;
+ var2 = (((((adc_T >> 4) - ((int32_t)dig_T1)) * ((adc_T >> 4) - ((int32_t)dig_T1))) >> 12) * ((int32_t)dig_T3)) >> 14;
+ t_fine = var1 + var2;
+ T = (t_fine * 5 + 128) >> 8;
+ return T;
+}
- Serial pc(USBTX, USBRX); // tx, rx
+// Returns pressure in Pa as unsigned 32 bit integer in Q24.8 format (24 integer bits and 8
+//fractional bits).
+//Output value of “24674867” represents 24674867/256 = 96386.2 Pa = 963.862 hPa
+uint32_t bmp280_compensate_P(int32_t adc_P)
+{
+ long long var1, var2, p;
+ var1 = ((long long)t_fine) - 128000;
+ var2 = var1 * var1 * (long long)dig_P6;
+ var2 = var2 + ((var1*(long long)dig_P5)<<17);
+ var2 = var2 + (((long long)dig_P4)<<35);
+ var1 = ((var1 * var1 * (long long)dig_P3)>>8) + ((var1 * (long long)dig_P2)<<12);
+ var1 = (((((long long)1)<<47)+var1))*((long long)dig_P1)>>33;
+ if(var1 == 0)
+ {
+ return 0;
+ // avoid exception caused by division by zero
+ }
+ p = 1048576 - adc_P;
+ p = (((p<<31) - var2)*3125)/var1;
+ var1 = (((long long)dig_P9) * (p>>13) * (p>>13)) >> 25;
+ var2 = (((long long)dig_P8) * p)>> 19;
+ p = ((p + var1 + var2) >> 8) + (((long long)dig_P7)<<4);
+ return (uint32_t)p;
+}
- // VCC, SCE, RST, D/C, MOSI,S CLK, LED
- N5110 lcd(P0_8, P0_10, P0_9, P0_6, P0_7, P0_5, P0_7);
-
-
+void myinthandler() // interrupt handler
+{
+ newData = true;
+}
+
int main()
{
pc.baud(9600);
-
+ myled = 0; // turn off led
+
+ wait(5);
+
//Set up I2C
i2c.frequency(400000); // use fast (400 kHz) I2C
+
+ t.start(); // enable system timer
- pc.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);
-
- t.start();
+ myled = 1; // turn on led
+
+ myInterrupt.rise(&myinthandler); // define interrupt for INT pin output of MPU9250
- lcd.init();
-// lcd.setBrightness(0.05);
+ // Read the WHO_AM_I register, this is a good test of communication
+ 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");
+ myled = 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
- 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
+ if (whoami == 0x71) // WHO_AM_I should always be 0x71
{
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);
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]);
+ pc.printf("x-axis self test: acceleration trim within: %f pct of factory value\n\r", SelfTest[0]);
+ pc.printf("y-axis self test: acceleration trim within: %f pct of factory value\n\r", SelfTest[1]);
+ pc.printf("z-axis self test: acceleration trim within: %f pct of factory value\n\r", SelfTest[2]);
+ pc.printf("x-axis self test: gyration trim within: %f pct of factory value\n\r", SelfTest[3]);
+ pc.printf("y-axis self test: gyration trim within: %f pct of factory value\n\r", SelfTest[4]);
+ pc.printf("z-axis self test: gyration trim within: %f pct of factory value\n\r", SelfTest[5]);
+
+ 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);
+
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]);
@@ -101,8 +144,11 @@
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
+ wait(1);
+
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));
@@ -111,56 +157,117 @@
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);
- }
+
+ pc.printf("Mag Calibration: Wave device in a figure eight until done!");
+ wait(4);
+ mpu9250.magcalMPU9250(magBias, magScale);
+ pc.printf("Mag Calibration done!\n\r");
+ pc.printf("x mag bias = %f\n\r", magBias[0]);
+ pc.printf("y mag bias = %f\n\r", magBias[1]);
+ pc.printf("z mag bias = %f\n\r", magBias[2]);
+ wait(2);
+ }
+
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);
+ myled = 0;
while(1) ; // Loop forever if communication doesn't happen
}
+
+ // Read the WHO_AM_I register of the BMP-280, this is a good test of communication
+ uint8_t c = bmp280.readByte(BMP280_ADDRESS, BMP280_ID);
+ if(c == 0x58) {
+
+ pc.printf("BMP-280 is 0x%x\n\r", c);
+ pc.printf("BMP-280 should be 0x58\n\r");
+ pc.printf("BMP-280 online...\n\r");
+
+ //bmp280.BMP280Init();
+
+ // Set T and P oversampling rates and sensor mode
+ bmp280.writeByte(BMP280_ADDRESS, BMP280_CTRL_MEAS, Tosr << 5 | Posr << 2 | Mode);
+ // Set standby time interval in normal mode and bandwidth
+ bmp280.writeByte(BMP280_ADDRESS, BMP280_CONFIG, SBy << 5 | IIRFilter << 2);
+ uint8_t calib[24];
+ bmp280.readBytes(BMP280_ADDRESS, BMP280_CALIB00, 24, &calib[0]);
+ dig_T1 = (uint16_t)(((uint16_t) calib[1] << 8) | calib[0]);
+ dig_T2 = ( int16_t)((( int16_t) calib[3] << 8) | calib[2]);
+ dig_T3 = ( int16_t)((( int16_t) calib[5] << 8) | calib[4]);
+ dig_P1 = (uint16_t)(((uint16_t) calib[7] << 8) | calib[6]);
+ dig_P2 = ( int16_t)((( int16_t) calib[9] << 8) | calib[8]);
+ dig_P3 = ( int16_t)((( int16_t) calib[11] << 8) | calib[10]);
+ dig_P4 = ( int16_t)((( int16_t) calib[13] << 8) | calib[12]);
+ dig_P5 = ( int16_t)((( int16_t) calib[15] << 8) | calib[14]);
+ dig_P6 = ( int16_t)((( int16_t) calib[17] << 8) | calib[16]);
+ dig_P7 = ( int16_t)((( int16_t) calib[19] << 8) | calib[18]);
+ dig_P8 = ( int16_t)((( int16_t) calib[21] << 8) | calib[20]);
+ dig_P9 = ( int16_t)((( int16_t) calib[23] << 8) | calib[22]);
- 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);
- 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
-
+ pc.printf("dig_T1 is %d\n\r", dig_T1);
+ pc.printf("dig_T2 is %d\n\r", dig_T2);
+ pc.printf("dig_T3 is %d\n\r", dig_T3);
+ pc.printf("dig_P1 is %d\n\r", dig_P1);
+ pc.printf("dig_P2 is %d\n\r", dig_P2);
+ pc.printf("dig_P3 is %d\n\r", dig_P3);
+ pc.printf("dig_P4 is %d\n\r", dig_P4);
+ pc.printf("dig_P5 is %d\n\r", dig_P5);
+ pc.printf("dig_P6 is %d\n\r", dig_P6);
+ pc.printf("dig_P7 is %d\n\r", dig_P7);
+ pc.printf("dig_P8 is %d\n\r", dig_P8);
+ pc.printf("dig_P9 is %d\n\r", dig_P9);
+
+ pc.printf("BMP-280 calibration complete...\n\r");
+
+ }
+
+ else
+
+ {
+ pc.printf("BMP-280 is 0x%x\n\r", c);
+ pc.printf("BMP-280 should be 0x55\n\r");
+ while(1); // idle here forever
+ }
+
+ /* Main Loop*/
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
+ // if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // OUse polling to check for data ready
+ if(newData){ // wait for interrupt for data ready
+ newData = false; // reset newData flag
+
+ mpu9250.readMPU9250Data(MPU9250Data); // INT cleared on any read
- mpu9250.readAccelData(accelCount); // Read the x/y/z adc values
+// 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
+ ax = (float)MPU9250Data[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
+ ay = (float)MPU9250Data[1]*aRes - accelBias[1];
+ az = (float)MPU9250Data[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];
+ gx = (float)MPU9250Data[4]*gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set
+ gy = (float)MPU9250Data[5]*gRes - gyroBias[1];
+ gz = (float)MPU9250Data[6]*gRes - gyroBias[2];
- mpu9250.readMagData(magCount); // Read the x/y/z adc values
+ }
+
+ if(mpu9250.readByte(AK8963_ADDRESS, AK8963_ST1) & 0x01) {
+
+ 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];
- }
+ 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];
+ mx *= magScale[0]; // poor man's soft iron calibration
+ my *= magScale[1];
+ mz *= magScale[2];
+ }
Now = t.read_us();
deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
@@ -169,18 +276,13 @@
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);
+ 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
+ // Serial print and/or display at 1 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
+ if (delt_t > 1000) { // update LCD once per second independent of read rate
pc.printf("ax = %f", 1000*ax);
pc.printf(" ay = %f", 1000*ay);
@@ -190,29 +292,26 @@
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);
+ pc.printf("mx = %f", mx);
+ pc.printf(" my = %f", my);
+ pc.printf(" mz = %f mG\n\r", 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);
+ // tempCount = mpu9250.readTempData(); // Read the adc values
+ temperature = ((float) MPU9250Data[3]) / 333.87f + 21.0f; // Temperature in degrees Centigrade
+ pc.printf("gyro temperature = %f C\n\r", temperature);
+
+ pc.printf("q0, q1, q2, q3 = %f %f %f %f\n\r",q[0], q[1], q[2], q[3]);
- 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);
- */
+ rawPress = readBMP280Pressure();
+ Pressure = (float) bmp280_compensate_P(rawPress)/25600.0f; // Pressure in mbar
+ rawTemp = readBMP280Temperature();
+ Temperature = (float) bmp280_compensate_T(rawTemp)/100.0f;
+
+ float altitude = 145366.45f*(1.0f - powf(Pressure/1013.25f, 0.190284f) );
+ pc.printf("Temperature = %f C\n\r", Temperature);
+ pc.printf("Pressure = %f Pa\n\r", Pressure);
+ pc.printf("Altitude = %f feet\n\r", altitude);
+
// 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.
@@ -222,20 +321,16 @@
// 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]);
+ yaw = atan2f(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 = -asinf(2.0f * (q[1] * q[3] - q[0] * q[2]));
+ roll = atan2f(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
+ 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);
+ pc.printf("average rate = %f Hz \n\r", (float) sumCount/sum);
myled= !myled;
count = t.read_ms();
@@ -249,6 +344,7 @@
sum = 0;
sumCount = 0;
}
+
}
- }
\ No newline at end of file
+}
\ No newline at end of file