aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorJamie Sanson <jamie.sanson@outlook.com>2016-06-07 17:44:35 +1200
committerJamie Sanson <jamie.sanson@outlook.com>2016-06-07 17:44:35 +1200
commite955022dfb751769f7551ea20b7889876577ea3f (patch)
tree17cb301d4f1a1f172d83e9d46fd29c582e2ca08b
parentc2ee9af63d07e1a846e99121101e8026e754bb7f (diff)
downloadmetis-1-onboard-e955022dfb751769f7551ea20b7889876577ea3f.tar.gz
metis-1-onboard-e955022dfb751769f7551ea20b7889876577ea3f.tar.bz2
metis-1-onboard-e955022dfb751769f7551ea20b7889876577ea3f.zip
Add calibration and begin initialise of magnetometer
-rw-r--r--test_flight_basic_logging/libraries/MPU9250_helper/MPU9250_helper.cpp148
-rw-r--r--test_flight_basic_logging/libraries/MPU9250_helper/MPU9250_helper.h53
-rw-r--r--test_flight_basic_logging/test_flight_basic_logging.ino45
3 files changed, 231 insertions, 15 deletions
diff --git a/test_flight_basic_logging/libraries/MPU9250_helper/MPU9250_helper.cpp b/test_flight_basic_logging/libraries/MPU9250_helper/MPU9250_helper.cpp
index f3e5b54..3d0aaec 100644
--- a/test_flight_basic_logging/libraries/MPU9250_helper/MPU9250_helper.cpp
+++ b/test_flight_basic_logging/libraries/MPU9250_helper/MPU9250_helper.cpp
@@ -69,6 +69,154 @@ void MPU9250_helper::initMPU9250()
delay(100);
}
+void MPU9250_helper::calibrateMPU9250(float * dest1, float * dest2)
+{
+ uint8_t data[12]; // data array to hold accelerometer and gyro x, y, z, data
+ uint16_t ii, packet_count, fifo_count;
+ int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0};
+
+ // reset device
+ writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device
+ delay(100);
+
+ // get stable time source; Auto select clock source to be PLL gyroscope reference if ready
+ // else use the internal oscillator, bits 2:0 = 001
+ writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01);
+ writeByte(MPU9250_ADDRESS, PWR_MGMT_2, 0x00);
+ delay(200);
+
+ // Configure device for bias calculation
+ writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x00); // Disable all interrupts
+ writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); // Disable FIFO
+ writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Turn on internal clock source
+ writeByte(MPU9250_ADDRESS, I2C_MST_CTRL, 0x00); // Disable I2C master
+ writeByte(MPU9250_ADDRESS, USER_CTRL, 0x00); // Disable FIFO and I2C master modes
+ writeByte(MPU9250_ADDRESS, USER_CTRL, 0x0C); // Reset FIFO and DMP
+ delay(15);
+
+ // Configure MPU6050 gyro and accelerometer for bias calculation
+ writeByte(MPU9250_ADDRESS, CONFIG, 0x01); // Set low-pass filter to 188 Hz
+ writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00); // Set sample rate to 1 kHz
+ writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0x00); // Set gyro full-scale to 250 degrees per second, maximum sensitivity
+ writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity
+
+ uint16_t gyrosensitivity = 131; // = 131 LSB/degrees/sec
+ uint16_t accelsensitivity = 16384; // = 16384 LSB/g
+
+ // Configure FIFO to capture accelerometer and gyro data for bias calculation
+ writeByte(MPU9250_ADDRESS, USER_CTRL, 0x40); // Enable FIFO
+ writeByte(MPU9250_ADDRESS, FIFO_EN, 0x78); // Enable gyro and accelerometer sensors for FIFO (max size 512 bytes in MPU-9150)
+ delay(40); // accumulate 40 samples in 40 milliseconds = 480 bytes
+
+ // At end of sample accumulation, turn off FIFO sensor read
+ writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); // Disable gyro and accelerometer sensors for FIFO
+ readBytes(MPU9250_ADDRESS, FIFO_COUNTH, 2, &data[0]); // read FIFO sample count
+ fifo_count = ((uint16_t)data[0] << 8) | data[1];
+ packet_count = fifo_count/12;// How many sets of full gyro and accelerometer data for averaging
+
+ for (ii = 0; ii < packet_count; ii++) {
+ int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0};
+ readBytes(MPU9250_ADDRESS, FIFO_R_W, 12, &data[0]); // read data for averaging
+ accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1] ) ; // Form signed 16-bit integer for each sample in FIFO
+ accel_temp[1] = (int16_t) (((int16_t)data[2] << 8) | data[3] ) ;
+ accel_temp[2] = (int16_t) (((int16_t)data[4] << 8) | data[5] ) ;
+ gyro_temp[0] = (int16_t) (((int16_t)data[6] << 8) | data[7] ) ;
+ gyro_temp[1] = (int16_t) (((int16_t)data[8] << 8) | data[9] ) ;
+ gyro_temp[2] = (int16_t) (((int16_t)data[10] << 8) | data[11]) ;
+
+ accel_bias[0] += (int32_t) accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
+ accel_bias[1] += (int32_t) accel_temp[1];
+ accel_bias[2] += (int32_t) accel_temp[2];
+ gyro_bias[0] += (int32_t) gyro_temp[0];
+ gyro_bias[1] += (int32_t) gyro_temp[1];
+ gyro_bias[2] += (int32_t) gyro_temp[2];
+
+ }
+
+ accel_bias[0] /= (int32_t) packet_count; // Normalize sums to get average count biases
+ accel_bias[1] /= (int32_t) packet_count;
+ accel_bias[2] /= (int32_t) packet_count;
+ gyro_bias[0] /= (int32_t) packet_count;
+ gyro_bias[1] /= (int32_t) packet_count;
+ gyro_bias[2] /= (int32_t) packet_count;
+
+ if(accel_bias[2] > 0L) {accel_bias[2] -= (int32_t) accelsensitivity;} // Remove gravity from the z-axis accelerometer bias calculation
+ else {accel_bias[2] += (int32_t) accelsensitivity;}
+
+ // Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup
+ data[0] = (-gyro_bias[0]/4 >> 8) & 0xFF; // Divide by 4 to get 32.9 LSB per deg/s to conform to expected bias input format
+ data[1] = (-gyro_bias[0]/4) & 0xFF; // Biases are additive, so change sign on calculated average gyro biases
+ data[2] = (-gyro_bias[1]/4 >> 8) & 0xFF;
+ data[3] = (-gyro_bias[1]/4) & 0xFF;
+ data[4] = (-gyro_bias[2]/4 >> 8) & 0xFF;
+ data[5] = (-gyro_bias[2]/4) & 0xFF;
+
+ // Push gyro biases to hardware registers
+ writeByte(MPU9250_ADDRESS, XG_OFFSET_H, data[0]);
+ writeByte(MPU9250_ADDRESS, XG_OFFSET_L, data[1]);
+ writeByte(MPU9250_ADDRESS, YG_OFFSET_H, data[2]);
+ writeByte(MPU9250_ADDRESS, YG_OFFSET_L, data[3]);
+ writeByte(MPU9250_ADDRESS, ZG_OFFSET_H, data[4]);
+ writeByte(MPU9250_ADDRESS, ZG_OFFSET_L, data[5]);
+
+ // Output scaled gyro biases for display in the main program
+ dest1[0] = (float) gyro_bias[0]/(float) gyrosensitivity;
+ dest1[1] = (float) gyro_bias[1]/(float) gyrosensitivity;
+ dest1[2] = (float) gyro_bias[2]/(float) gyrosensitivity;
+
+ // Construct the accelerometer biases for push to the hardware accelerometer bias registers. These registers contain
+ // factory trim values which must be added to the calculated accelerometer biases; on boot up these registers will hold
+ // non-zero values. In addition, bit 0 of the lower byte must be preserved since it is used for temperature
+ // compensation calculations. Accelerometer bias registers expect bias input as 2048 LSB per g, so that
+ // the accelerometer biases calculated above must be divided by 8.
+
+ int32_t accel_bias_reg[3] = {0, 0, 0}; // A place to hold the factory accelerometer trim biases
+ readBytes(MPU9250_ADDRESS, XA_OFFSET_H, 2, &data[0]); // Read factory accelerometer trim values
+ accel_bias_reg[0] = (int32_t) (((int16_t)data[0] << 8) | data[1]);
+ readBytes(MPU9250_ADDRESS, YA_OFFSET_H, 2, &data[0]);
+ accel_bias_reg[1] = (int32_t) (((int16_t)data[0] << 8) | data[1]);
+ readBytes(MPU9250_ADDRESS, ZA_OFFSET_H, 2, &data[0]);
+ accel_bias_reg[2] = (int32_t) (((int16_t)data[0] << 8) | data[1]);
+
+ uint32_t mask = 1uL; // Define mask for temperature compensation bit 0 of lower byte of accelerometer bias registers
+ uint8_t mask_bit[3] = {0, 0, 0}; // Define array to hold mask bit for each accelerometer bias axis
+
+ for(ii = 0; ii < 3; ii++) {
+ if((accel_bias_reg[ii] & mask)) mask_bit[ii] = 0x01; // If temperature compensation bit is set, record that fact in mask_bit
+ }
+
+ // Construct total accelerometer bias, including calculated average accelerometer bias from above
+ accel_bias_reg[0] -= (accel_bias[0]/8); // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g (16 g full scale)
+ accel_bias_reg[1] -= (accel_bias[1]/8);
+ accel_bias_reg[2] -= (accel_bias[2]/8);
+
+ data[0] = (accel_bias_reg[0] >> 8) & 0xFF;
+ data[1] = (accel_bias_reg[0]) & 0xFF;
+ data[1] = data[1] | mask_bit[0]; // preserve temperature compensation bit when writing back to accelerometer bias registers
+ data[2] = (accel_bias_reg[1] >> 8) & 0xFF;
+ data[3] = (accel_bias_reg[1]) & 0xFF;
+ data[3] = data[3] | mask_bit[1]; // preserve temperature compensation bit when writing back to accelerometer bias registers
+ data[4] = (accel_bias_reg[2] >> 8) & 0xFF;
+ data[5] = (accel_bias_reg[2]) & 0xFF;
+ data[5] = data[5] | mask_bit[2]; // preserve temperature compensation bit when writing back to accelerometer bias registers
+
+ // Apparently this is not working for the acceleration biases in the MPU-9250
+ // Are we handling the temperature correction bit properly?
+ // Push accelerometer biases to hardware registers
+ writeByte(MPU9250_ADDRESS, XA_OFFSET_H, data[0]);
+ writeByte(MPU9250_ADDRESS, XA_OFFSET_L, data[1]);
+ writeByte(MPU9250_ADDRESS, YA_OFFSET_H, data[2]);
+ writeByte(MPU9250_ADDRESS, YA_OFFSET_L, data[3]);
+ writeByte(MPU9250_ADDRESS, ZA_OFFSET_H, data[4]);
+ writeByte(MPU9250_ADDRESS, ZA_OFFSET_L, data[5]);
+
+ // Output scaled accelerometer biases for display in the main program
+ dest2[0] = (float)accel_bias[0]/(float)accelsensitivity;
+ dest2[1] = (float)accel_bias[1]/(float)accelsensitivity;
+ dest2[2] = (float)accel_bias[2]/(float)accelsensitivity;
+
+}
+
void MPU9250_helper::writeByte(uint8_t address, uint8_t subAddress, uint8_t data)
{
Wire1.beginTransmission(address); // Initialize the Tx buffer
diff --git a/test_flight_basic_logging/libraries/MPU9250_helper/MPU9250_helper.h b/test_flight_basic_logging/libraries/MPU9250_helper/MPU9250_helper.h
index 729777e..be02364 100644
--- a/test_flight_basic_logging/libraries/MPU9250_helper/MPU9250_helper.h
+++ b/test_flight_basic_logging/libraries/MPU9250_helper/MPU9250_helper.h
@@ -8,13 +8,43 @@
#include "Arduino.h"
// Register definitions
-#if ADO
-#define MPU9250_ADDRESS 0x69 // Device address when ADO = 1
-#else
-#define MPU9250_ADDRESS 0x68 // Device address when ADO = 0
-#define AK8963_ADDRESS 0x0C // Address of magnetometer
-#endif
+// See also MPU-9250 Register Map and Descriptions, Revision 4.0, RM-MPU-9250A-00, Rev. 1.4, 9/9/2013 for registers not listed in
+// above document; the MPU9250 and MPU9150 are virtually identical but the latter has a different register map
+//
+
+//Magnetometer Registers
+#define AK8963_ADDRESS 0x0C
+#define WHO_AM_I_AK8963 0x00 // should return 0x48
+#define INFO 0x01
+#define AK8963_ST1 0x02 // data ready status bit 0
+#define AK8963_XOUT_L 0x03 // data
+#define AK8963_XOUT_H 0x04
+#define AK8963_YOUT_L 0x05
+#define AK8963_YOUT_H 0x06
+#define AK8963_ZOUT_L 0x07
+#define AK8963_ZOUT_H 0x08
+#define AK8963_ST2 0x09 // Data overflow bit 3 and data read error status bit 2
+#define AK8963_CNTL 0x0A // Power down (0000), single-measurement (0001), self-test (1000) and Fuse ROM (1111) modes on bits 3:0
+#define AK8963_ASTC 0x0C // Self test control
+#define AK8963_I2CDIS 0x0F // I2C disable
+#define AK8963_ASAX 0x10 // Fuse ROM x-axis sensitivity adjustment value
+#define AK8963_ASAY 0x11 // Fuse ROM y-axis sensitivity adjustment value
+#define AK8963_ASAZ 0x12 // Fuse ROM z-axis sensitivity adjustment value
+
+#define SELF_TEST_X_GYRO 0x00
+#define SELF_TEST_Y_GYRO 0x01
+#define SELF_TEST_Z_GYRO 0x02
+
+/*#define X_FINE_GAIN 0x03 // [7:0] fine gain
+#define Y_FINE_GAIN 0x04
+#define Z_FINE_GAIN 0x05
+#define XA_OFFSET_H 0x06 // User-defined trim values for accelerometer
+#define XA_OFFSET_L_TC 0x07
+#define YA_OFFSET_H 0x08
+#define YA_OFFSET_L_TC 0x09
+#define ZA_OFFSET_H 0x0A
+#define ZA_OFFSET_L_TC 0x0B */
#define SELF_TEST_X_ACCEL 0x0D
#define SELF_TEST_Y_ACCEL 0x0E
@@ -129,6 +159,16 @@
#define ZA_OFFSET_H 0x7D
#define ZA_OFFSET_L 0x7E
+// Using the MSENSR-9250 breakout board, ADO is set to 0
+// Seven-bit device address is 110100 for ADO = 0 and 110101 for ADO = 1
+//#define ADO 0
+#if ADO
+#define MPU9250_ADDRESS 0x69 // Device address when ADO = 1
+#else
+#define MPU9250_ADDRESS 0x68 // Device address when ADO = 0
+#define AK8963_ADDRESS 0x0C // Address of magnetometer
+#endif
+
#define AFS_2G 0
#define AFS_4G 1
#define AFS_8G 2
@@ -150,6 +190,7 @@ class MPU9250_helper
MPU9250_helper(uint8_t AScale, uint8_t GScale, uint8_t MScale, uint8_t Mmode);
// init and basic helpers
void initMPU9250();
+ void calibrateMPU9250(float * dest1, float * dest2);
// utility functions
void writeByte(uint8_t address, uint8_t subAddress, uint8_t data);
diff --git a/test_flight_basic_logging/test_flight_basic_logging.ino b/test_flight_basic_logging/test_flight_basic_logging.ino
index af9b48e..4ff2ad7 100644
--- a/test_flight_basic_logging/test_flight_basic_logging.ino
+++ b/test_flight_basic_logging/test_flight_basic_logging.ino
@@ -39,6 +39,9 @@ int16_t accelCount[3];
float aRes;
float ax,ay,az;
+// Biases resulting from calibration
+float gyroBias[3] = {0, 0, 0}, accelBias[3] = {0, 0, 0};
+
MPU9250_helper helper(Ascale, Gscale, Mscale, Mmode);
void setup() {
@@ -46,13 +49,9 @@ void setup() {
Serial.begin(115200);
while(!Serial.available()){}
-
- Serial.println("Reading whoami byte (using wire1)");
- byte c = helper.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250
-
- Serial.print("MPU9250 "); Serial.print("I AM "); Serial.print(c, HEX); Serial.print(" I should be "); Serial.println(0x71, HEX);
- helper.initMPU9250();
+ setupMPU9250();
+ setupAK8963();
}
void loop() {
@@ -63,9 +62,37 @@ void loop() {
ay = (float)accelCount[1]*aRes;
az = (float)accelCount[2]*aRes;
- Serial.print("X: "); Serial.print(ax, DEC);
- Serial.print(" Y: "); Serial.print(ay, DEC);
- Serial.print(" Z: "); Serial.println(az, DEC);
+ //Serial.print("X: "); Serial.print(ax, DEC);
+ //Serial.print(" Y: "); Serial.print(ay, DEC);
+ //Serial.print(" Z: "); Serial.println(az, DEC);
+}
+
+void setupMPU9250() {
+ Serial.println("Reading whoami byte of MPU9250");
+ byte c = helper.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250
+
+ Serial.print("MPU9250 "); Serial.print("I AM "); Serial.print(c, HEX); Serial.print(", I should be "); Serial.println(0x71, HEX);
+
+ if (c == 0x71) {
+ Serial.println("MPU9250 online");
+ Serial.println("Calibrating...");
+
+ helper.calibrateMPU9250(gyroBias, accelBias);
+
+ Serial.println("Accelerometer bias: (mg)");
+ Serial.println("X: " + (String)(1000*accelBias[0]) + " Y: " + (String)(1000*accelBias[1]) + " Z: " + (String)(1000*accelBias[2]));
+
+ Serial.println("Gyro bias: (o/s)");
+ Serial.println("X: " + (String)gyroBias[0] + " Y: " + (String)gyroBias[1] + " Z: " + (String)gyroBias[2]);
+
+ helper.initMPU9250();
+ Serial.println("\nMPU9250 initialized for active data mode....\n");
+ }
}
+void setupAK8963() {
+ Serial.println("Reading whoami byte of magnetometer");
+ byte d = helper.readByte(AK8963_ADDRESS, WHO_AM_I_AK8963); // Read WHO_AM_I register for AK8963
+ Serial.print("AK8963 "); Serial.print("I AM "); Serial.print(d, HEX); Serial.print(", I should be "); Serial.println(0x48, HEX);
+}