aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorJamie Sanson <jamie.sanson@outlook.com>2016-06-07 18:45:51 +1200
committerJamie Sanson <jamie.sanson@outlook.com>2016-06-07 18:45:51 +1200
commit7d48de8a717cfd953a0428ec2f025d3ceb99ff1b (patch)
tree615f5a1e2daef4001a4b350ea64037cdec6226bc
parentfaaf16be3d2f7436b911cbaf38def6483ce23174 (diff)
downloadmetis-1-onboard-7d48de8a717cfd953a0428ec2f025d3ceb99ff1b.tar.gz
metis-1-onboard-7d48de8a717cfd953a0428ec2f025d3ceb99ff1b.tar.bz2
metis-1-onboard-7d48de8a717cfd953a0428ec2f025d3ceb99ff1b.zip
Add reading of gyro and mag data
-rw-r--r--test_flight_basic_logging/libraries/MPU9250_helper/MPU9250_helper.cpp62
-rw-r--r--test_flight_basic_logging/libraries/MPU9250_helper/MPU9250_helper.h6
-rw-r--r--test_flight_basic_logging/test_flight_basic_logging.ino31
3 files changed, 90 insertions, 9 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 642212b..f828945 100644
--- a/test_flight_basic_logging/libraries/MPU9250_helper/MPU9250_helper.cpp
+++ b/test_flight_basic_logging/libraries/MPU9250_helper/MPU9250_helper.cpp
@@ -279,13 +279,36 @@ void MPU9250_helper::readAccelData(int16_t * destination)
destination[2] = ((int16_t)rawData[4] << 8) | rawData[5] ;
}
+void MPU9250_helper::readGyroData(int16_t * destination)
+{
+ uint8_t rawData[6]; // x/y/z gyro register data stored here
+ readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially 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] ;
+}
+
+void MPU9250_helper::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)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] ;
+ }
+ }
+}
+
float MPU9250_helper::getAccelRes()
{
switch (_ascale)
{
// Possible accelerometer scales (and their register bit settings) are:
- // 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:
+ // 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs (11).
+ // Here's a bit of an algorithm to calculate DPS/(ADC tick) based on that 2-bit value:
case AFS_2G:
return 2.0/32768.0;
case AFS_4G:
@@ -298,3 +321,38 @@ float MPU9250_helper::getAccelRes()
return 0;
}
+
+float MPU9250_helper::getGyroRes()
+{
+ switch (_gscale)
+ {
+ // Possible gyro scales (and their register bit settings) are:
+ // 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:
+ return 250.0/32768.0;
+ case GFS_500DPS:
+ return 500.0/32768.0;
+ case GFS_1000DPS:
+ return 1000.0/32768.0;
+ case GFS_2000DPS:
+ return 2000.0/32768.0;
+ }
+
+ return 0;
+}
+
+float MPU9250_helper::getMagRes()
+{
+ switch (_mscale)
+ {
+ // Possible magnetometer scales (and their register bit settings) are:
+ // 14 bit resolution (0) and 16 bit resolution (1)
+ case MFS_14BITS:
+ return 10.*4912./8190.;
+ case MFS_16BITS:
+ return 10.*4912./32760.0;
+ }
+
+ return 0;
+}
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 2406381..adb9137 100644
--- a/test_flight_basic_logging/libraries/MPU9250_helper/MPU9250_helper.h
+++ b/test_flight_basic_logging/libraries/MPU9250_helper/MPU9250_helper.h
@@ -202,6 +202,12 @@ class MPU9250_helper
void readAccelData(int16_t * destination);
float getAccelRes();
+ void readGyroData(int16_t * destination);
+ float getGyroRes();
+
+ void readMagData(int16_t * destination);
+ float getMagRes();
+
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 09b5138..32fa62c 100644
--- a/test_flight_basic_logging/test_flight_basic_logging.ino
+++ b/test_flight_basic_logging/test_flight_basic_logging.ino
@@ -34,10 +34,12 @@ int8_t Gscale = GFS_250DPS;
int8_t Mscale = MFS_16BITS; // Choose either 14-bit or 16-bit magnetometer resolution
int8_t Mmode = 0x02; // 2 for 8Hz, 6 for 100Hz continuous
-// Stores the 16-bit signed accelerometer sensor output
-int16_t accelCount[3];
-float aRes;
-float ax,ay,az;
+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 aRes, gRes, mRes;
+float ax,ay,az, gx, gy, gz, mx, my, mz;
// Biases resulting from calibration
float gyroBias[3] = {0, 0, 0}, accelBias[3] = {0, 0, 0};
@@ -57,15 +59,30 @@ void setup() {
void loop() {
helper.readAccelData(accelCount); // Read the x/y/z adc values
+ helper.readGyroData(gyroCount);
+ helper.readMagData(magCount);
+
aRes = helper.getAccelRes();
+ gRes = helper.getGyroRes();
+ mRes = helper.getMagRes();
ax = (float)accelCount[0]*aRes;
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);
+ gx = (float)gyroCount[0]*gRes;
+ gy = (float)gyroCount[1]*gRes;
+ gz = (float)gyroCount[2]*gRes;
+
+ // 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];
+
+ Serial.print("X: "); Serial.print(ax, DEC); Serial.print(","); Serial.print(gx, DEC); Serial.print(","); Serial.print(mx,DEC);
+ Serial.print(" Y: "); Serial.print(ay, DEC); Serial.print(","); Serial.print(gy, DEC); Serial.print(","); Serial.print(my,DEC);
+ Serial.print(" Z: "); Serial.println(az, DEC); Serial.print(","); Serial.print(gz, DEC); Serial.print(","); Serial.print(mz,DEC);
}
void setupMPU9250() {