aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorJamie Sanson <jamie.sanson@outlook.com>2016-06-07 18:21:44 +1200
committerJamie Sanson <jamie.sanson@outlook.com>2016-06-07 18:21:44 +1200
commitfaaf16be3d2f7436b911cbaf38def6483ce23174 (patch)
tree96b7b18af39fa72697d9a1603974ebf4f7e0bc87
parente955022dfb751769f7551ea20b7889876577ea3f (diff)
downloadmetis-1-onboard-faaf16be3d2f7436b911cbaf38def6483ce23174.tar.gz
metis-1-onboard-faaf16be3d2f7436b911cbaf38def6483ce23174.tar.bz2
metis-1-onboard-faaf16be3d2f7436b911cbaf38def6483ce23174.zip
initialise and calibrate magnetometer
-rw-r--r--test_flight_basic_logging/libraries/MPU9250_helper/MPU9250_helper.cpp25
-rw-r--r--test_flight_basic_logging/libraries/MPU9250_helper/MPU9250_helper.h2
-rw-r--r--test_flight_basic_logging/test_flight_basic_logging.ino15
3 files changed, 37 insertions, 5 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 3d0aaec..642212b 100644
--- a/test_flight_basic_logging/libraries/MPU9250_helper/MPU9250_helper.cpp
+++ b/test_flight_basic_logging/libraries/MPU9250_helper/MPU9250_helper.cpp
@@ -9,6 +9,27 @@ MPU9250_helper::MPU9250_helper(uint8_t AScale, uint8_t GScale, uint8_t MScale, u
_mmode = Mmode;
}
+void MPU9250_helper::initAK8963(float * destination)
+{
+ // First extract the factory calibration for each magnetometer axis
+ uint8_t rawData[3]; // x/y/z gyro calibration data stored here
+ writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer
+ delay(10);
+ writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x0F); // Enter Fuse ROM access mode
+ delay(10);
+ readBytes(AK8963_ADDRESS, AK8963_ASAX, 3, &rawData[0]); // Read the x-, y-, and z-axis calibration values
+ destination[0] = (float)(rawData[0] - 128)/256. + 1.; // Return x-axis sensitivity adjustment values, etc.
+ destination[1] = (float)(rawData[1] - 128)/256. + 1.;
+ destination[2] = (float)(rawData[2] - 128)/256. + 1.;
+ writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer
+ delay(10);
+ // Configure the magnetometer for continuous read and highest resolution
+ // set Mscale bit 4 to 1 (0) to enable 16 (14) bit resolution in CNTL register,
+ // and enable continuous mode data acquisition Mmode (bits [3:0]), 0010 for 8 Hz and 0110 for 100 Hz sample rates
+ writeByte(AK8963_ADDRESS, AK8963_CNTL, _mscale << 4 | _mmode); // Set magnetometer data resolution and sample ODR
+ delay(10);
+}
+
void MPU9250_helper::initMPU9250()
{
// wake up device
@@ -243,8 +264,8 @@ uint8_t MPU9250_helper::readByte(uint8_t address, uint8_t subAddress)
uint8_t data; // `data` will store the register data
Wire1.beginTransmission(address); // Initialize the Tx buffer
Wire1.write(subAddress); // Put slave register address in Tx buffer
- Wire1.endTransmission(I2C_NOSTOP); // Send the Tx buffer, but send a restart to keep connection alive
- Wire1.requestFrom(address, (size_t) 1); // Read one byte from slave register address
+ Wire1.endTransmission(I2C_NOSTOP, 50); // Send the Tx buffer, but send a restart to keep connection alive
+ Wire1.requestFrom(address, (size_t) 1, 50); // Read one byte from slave register address
data = Wire1.read(); // Fill Rx buffer with result
return data; // Return data read from slave register
}
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 be02364..2406381 100644
--- a/test_flight_basic_logging/libraries/MPU9250_helper/MPU9250_helper.h
+++ b/test_flight_basic_logging/libraries/MPU9250_helper/MPU9250_helper.h
@@ -190,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 initAK8963(float * destination);
void calibrateMPU9250(float * dest1, float * dest2);
// utility functions
@@ -200,6 +201,7 @@ class MPU9250_helper
// data specific functions
void readAccelData(int16_t * destination);
float getAccelRes();
+
private:
uint8_t _ascale, _gscale, _mscale, _mmode;
};
diff --git a/test_flight_basic_logging/test_flight_basic_logging.ino b/test_flight_basic_logging/test_flight_basic_logging.ino
index 4ff2ad7..09b5138 100644
--- a/test_flight_basic_logging/test_flight_basic_logging.ino
+++ b/test_flight_basic_logging/test_flight_basic_logging.ino
@@ -41,6 +41,7 @@ float ax,ay,az;
// Biases resulting from calibration
float gyroBias[3] = {0, 0, 0}, accelBias[3] = {0, 0, 0};
+float magCalibration[3] = {0, 0, 0}, magbias[3] = {0, 0, 0};
MPU9250_helper helper(Ascale, Gscale, Mscale, Mmode);
@@ -68,14 +69,14 @@ void loop() {
}
void setupMPU9250() {
- Serial.println("Reading whoami byte of MPU9250");
+ Serial.println("Reading who-am-i 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...");
+ Serial.println("Calibrating...\n");
helper.calibrateMPU9250(gyroBias, accelBias);
@@ -91,8 +92,16 @@ void setupMPU9250() {
}
void setupAK8963() {
- Serial.println("Reading whoami byte of magnetometer");
+ Serial.println("Reading who-am-i 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);
+
+ helper.initAK8963(magCalibration);
+
+ Serial.print("Calibrating...\n");
+ Serial.print("X-Axis sensitivity adjustment value "); Serial.println(magCalibration[0], 2);
+ Serial.print("Y-Axis sensitivity adjustment value "); Serial.println(magCalibration[1], 2);
+ Serial.print("Z-Axis sensitivity adjustment value "); Serial.println(magCalibration[2], 2);
+ Serial.println("\nAK8963 initialized for active data mode....");
}