This program is designed to run on a set of Xadow M0 modules to create a Hotshoe IMU which outputs GPS and Orientation data to Nikon cameras, as well as triggering the camera at set intervals.
Dependencies: MBed_Adafruit-GPS-Library SC16IS750 SDFileSystem SSD1308_128x64_I2C USBDevice mbed BMP085
Fork of MPU9150AHRS by
Diff: MPU9150.h
- Revision:
- 9:b7062c55d36c
- Parent:
- 0:39935bb3c1a1
--- a/MPU9150.h Tue Dec 01 02:03:48 2015 +0000 +++ b/MPU9150.h Tue Dec 01 04:42:30 2015 +0000 @@ -2,6 +2,16 @@ #define MPU9150_H #include "mbed.h" + +//#define DEBUG + +#ifdef DEBUG +#include "USBSerial.h" // To use USB virtual serial, a driver is needed, check http://mbed.org/handbook/USBSerial +#define LOG(args...) pc.printf(args) +USBSerial pc; +#else +#define LOG(args...) +#endif // Define registers per MPU6050, Register Map and Descriptions, Rev 4.2, 08/19/2013 6 DOF Motion sensor fusion device // Invensense Inc., www.invensense.com @@ -183,7 +193,7 @@ 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}; ; // Bias corrections for gyro, accelerometer and mag 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; @@ -337,6 +347,45 @@ destination[2] = (float)(rawData[2] - 128)/256.0f + 1.0f; } +void magcalMPU9150(float * dest1) +{ + uint16_t ii = 0, sample_count = 0; + int32_t mag_bias[3] = {0, 0, 0}; + int16_t mag_max[3] = {0, 0, 0}, mag_min[3] = {0, 0, 0}, mag_temp[3] = {0, 0, 0}; + + LOG("Mag Calibration: Wave device in a figure eight until done!"); + wait(4); + + sample_count = 64; + for(ii = 0; ii < sample_count; ii++) { + readMagData(mag_temp); // Read the mag data + for (int jj = 0; jj < 3; jj++) { + if (ii == 0) { + mag_max[jj] = mag_temp[jj]; // Offsets may be large enough that mag_temp[i] may not be bipolar! + mag_min[jj] = mag_temp[jj]; // This prevents max or min being pinned to 0 if the values are unipolar... + } else { + 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]; + } + } + wait_ms(135); // at 8 Hz ODR, new mag data is available every 125 ms + } + +// Serial.println("mag x min/max:"); Serial.println(mag_max[0]); Serial.println(mag_min[0]); +// Serial.println("mag y min/max:"); Serial.println(mag_max[1]); Serial.println(mag_min[1]); +// Serial.println("mag z min/max:"); Serial.println(mag_max[2]); Serial.println(mag_min[2]); + + 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]; + + LOG("Mag Calibration done!"); +} + int16_t readTempData() { uint8_t rawData[2]; // x/y/z gyro register data stored here