aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorJamie Sanson <jamie.sanson@outlook.com>2016-06-07 15:22:26 +1200
committerJamie Sanson <jamie.sanson@outlook.com>2016-06-07 15:22:26 +1200
commit2d931969428a1d184be92f486e405f0f87744b33 (patch)
tree3411f6232e7fd26438c6abafddd55c6774bd7ed5
parent4446ab49ff24fc79af54367d5f279ab21fcad79f (diff)
downloadmetis-1-onboard-2d931969428a1d184be92f486e405f0f87744b33.tar.gz
metis-1-onboard-2d931969428a1d184be92f486e405f0f87744b33.tar.bz2
metis-1-onboard-2d931969428a1d184be92f486e405f0f87744b33.zip
Move MPU9250 code to library
-rw-r--r--test_flight_basic_logging/libraries/MPU9250_helper/MPU9250_helper.cpp163
-rw-r--r--test_flight_basic_logging/libraries/MPU9250_helper/MPU9250_helper.h152
-rw-r--r--test_flight_basic_logging/test_flight_basic_logging.ino300
3 files changed, 324 insertions, 291 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
new file mode 100644
index 0000000..ba8b1a3
--- /dev/null
+++ b/test_flight_basic_logging/libraries/MPU9250_helper/MPU9250_helper.cpp
@@ -0,0 +1,163 @@
+#include "Arduino.h"
+#include "MPU9250_helper.h"
+#include "i2c_t3.h"
+
+// Set initial input parameters
+enum Ascale {
+ AFS_2G = 0,
+ AFS_4G,
+ AFS_8G,
+ AFS_16G
+};
+
+enum Gscale {
+ GFS_250DPS = 0,
+ GFS_500DPS,
+ GFS_1000DPS,
+ GFS_2000DPS
+};
+
+enum Mscale {
+ MFS_14BITS = 0, // 0.6 mG per LSB
+ MFS_16BITS // 0.15 mG per LSB
+};
+
+uint8_t Gscale = GFS_250DPS;
+uint8_t Ascale = AFS_8G;
+uint8_t Mscale = MFS_16BITS; // Choose either 14-bit or 16-bit magnetometer resolution
+uint8_t Mmode = 0x02; // 2 for 8 Hz, 6 for 100 Hz continuous magnetometer data read
+
+
+MPU9250_helper::MPU9250_helper(){
+
+}
+
+void MPU9250_helper::initMPU9250()
+{
+ // wake up device
+ writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Clear sleep mode bit (6), enable all sensors
+ delay(100); // Wait for all registers to reset
+
+ // get stable time source
+ writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01); // Auto select clock source to be PLL gyroscope reference if ready else
+ delay(200);
+
+ // Configure Gyro and Thermometer
+ // Disable FSYNC and set thermometer and gyro bandwidth to 41 and 42 Hz, respectively;
+ // minimum delay time for this setting is 5.9 ms, which means sensor fusion update rates cannot
+ // be higher than 1 / 0.0059 = 170 Hz
+ // DLPF_CFG = bits 2:0 = 011; this limits the sample rate to 1000 Hz for both
+ // With the MPU9250, it is possible to get gyro sample rates of 32 kHz (!), 8 kHz, or 1 kHz
+ writeByte(MPU9250_ADDRESS, CONFIG, 0x03);
+
+ // Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV)
+ writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x04); // Use a 200 Hz rate; a rate consistent with the filter update rate
+ // determined inset in CONFIG above
+
+ // Set gyroscope full scale range
+ // Range selects FS_SEL and AFS_SEL are 0 - 3, so 2-bit values are left-shifted into positions 4:3
+ uint8_t c = readByte(MPU9250_ADDRESS, GYRO_CONFIG); // get current GYRO_CONFIG register value
+
+ // c = c & ~0xE0; // Clear self-test bits [7:5]
+ c = c & ~0x02; // Clear Fchoice bits [1:0]
+ c = c & ~0x18; // Clear AFS bits [4:3]
+ c = c | Gscale << 3; // Set full scale range for the gyro
+ // c =| 0x00; // Set Fchoice for the gyro to 11 by writing its inverse to bits 1:0 of GYRO_CONFIG
+ writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c ); // Write new GYRO_CONFIG value to register
+
+ // Set accelerometer full-scale range configuration
+ c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG); // get current ACCEL_CONFIG register value
+ // c = c & ~0xE0; // Clear self-test bits [7:5]
+ c = c & ~0x18; // Clear AFS bits [4:3]
+ c = c | Ascale << 3; // Set full scale range for the accelerometer
+ writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c); // Write new ACCEL_CONFIG register value
+
+ // Set accelerometer sample rate configuration
+ // It is possible to get a 4 kHz sample rate from the accelerometer by choosing 1 for
+ // accel_fchoice_b bit [3]; in this case the bandwidth is 1.13 kHz
+ c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG2); // get current ACCEL_CONFIG2 register value
+ c = c & ~0x0F; // Clear accel_fchoice_b (bit 3) and A_DLPFG (bits [2:0])
+ c = c | 0x03; // Set accelerometer rate to 1 kHz and bandwidth to 41 Hz
+ writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, c); // Write new ACCEL_CONFIG2 register value
+
+ // The accelerometer, gyro, and thermometer are set to 1 kHz sample rates,
+ // but all these rates are further reduced by a factor of 5 to 200 Hz because of the SMPLRT_DIV setting
+
+ // Configure Interrupts and Bypass Enable
+ // Set interrupt pin active high, push-pull, hold interrupt pin level HIGH until interrupt cleared,
+ // clear on read of INT_STATUS, and 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
+ delay(100);
+}
+
+void MPU9250_helper::waitForInput()
+{
+ while(!Serial.available()){};
+
+ while(Serial.available()){
+ Serial.read();
+ };
+}
+
+void MPU9250_helper::writeByte(uint8_t address, uint8_t subAddress, uint8_t data)
+{
+ Wire1.beginTransmission(address); // Initialize the Tx buffer
+ Wire1.write(subAddress); // Put slave register address in Tx buffer
+ Wire1.write(data); // Put data in Tx buffer
+ Wire1.endTransmission(); // Send the Tx buffer
+}
+
+void MPU9250_helper::readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest)
+{
+ 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
+
+ uint8_t i = 0;
+ Wire1.requestFrom(address, (size_t) count); // Read bytes from slave register address
+ while (Wire1.available()) {
+ dest[i++] = Wire1.read(); // Put read results in the Rx buffer
+ }
+}
+
+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
+ data = Wire1.read(); // Fill Rx buffer with result
+ return data; // Return data read from slave register
+}
+
+void MPU9250_helper::readAccelData(int16_t * destination)
+{
+ uint8_t rawData[6]; // x/y/z accel register data stored here
+ readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // Read the six 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] ;
+}
+
+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:
+ case AFS_2G:
+ return 2.0/32768.0;
+ case AFS_4G:
+ return 4.0/32768.0;
+ case AFS_8G:
+ return 8.0/32768.0;
+ case AFS_16G:
+ return 16.0/32768.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
new file mode 100644
index 0000000..86463df
--- /dev/null
+++ b/test_flight_basic_logging/libraries/MPU9250_helper/MPU9250_helper.h
@@ -0,0 +1,152 @@
+/*
+ helper library for using the MPU9250 sensor package on the rear of
+ a Teensy 3.1
+*/
+#ifndef MPU9250_helper_h
+#define MPU9250_helper_h
+
+#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
+
+
+#define SELF_TEST_X_ACCEL 0x0D
+#define SELF_TEST_Y_ACCEL 0x0E
+#define SELF_TEST_Z_ACCEL 0x0F
+
+#define SELF_TEST_A 0x10
+
+#define XG_OFFSET_H 0x13 // User-defined trim values for gyroscope
+#define XG_OFFSET_L 0x14
+#define YG_OFFSET_H 0x15
+#define YG_OFFSET_L 0x16
+#define ZG_OFFSET_H 0x17
+#define ZG_OFFSET_L 0x18
+#define SMPLRT_DIV 0x19
+#define CONFIG 0x1A
+#define GYRO_CONFIG 0x1B
+#define ACCEL_CONFIG 0x1C
+#define ACCEL_CONFIG2 0x1D
+#define LP_ACCEL_ODR 0x1E
+#define WOM_THR 0x1F
+
+#define MOT_DUR 0x20 // Duration counter threshold for motion interrupt generation, 1 kHz rate, LSB = 1 ms
+#define ZMOT_THR 0x21 // Zero-motion detection threshold bits [7:0]
+#define ZRMOT_DUR 0x22 // Duration counter threshold for zero motion interrupt generation, 16 Hz rate, LSB = 64 ms
+
+#define FIFO_EN 0x23
+#define I2C_MST_CTRL 0x24
+#define I2C_SLV0_ADDR 0x25
+#define I2C_SLV0_REG 0x26
+#define I2C_SLV0_CTRL 0x27
+#define I2C_SLV1_ADDR 0x28
+#define I2C_SLV1_REG 0x29
+#define I2C_SLV1_CTRL 0x2A
+#define I2C_SLV2_ADDR 0x2B
+#define I2C_SLV2_REG 0x2C
+#define I2C_SLV2_CTRL 0x2D
+#define I2C_SLV3_ADDR 0x2E
+#define I2C_SLV3_REG 0x2F
+#define I2C_SLV3_CTRL 0x30
+#define I2C_SLV4_ADDR 0x31
+#define I2C_SLV4_REG 0x32
+#define I2C_SLV4_DO 0x33
+#define I2C_SLV4_CTRL 0x34
+#define I2C_SLV4_DI 0x35
+#define I2C_MST_STATUS 0x36
+#define INT_PIN_CFG 0x37
+#define INT_ENABLE 0x38
+#define DMP_INT_STATUS 0x39 // Check DMP interrupt
+#define INT_STATUS 0x3A
+#define ACCEL_XOUT_H 0x3B
+#define ACCEL_XOUT_L 0x3C
+#define ACCEL_YOUT_H 0x3D
+#define ACCEL_YOUT_L 0x3E
+#define ACCEL_ZOUT_H 0x3F
+#define ACCEL_ZOUT_L 0x40
+#define TEMP_OUT_H 0x41
+#define TEMP_OUT_L 0x42
+#define GYRO_XOUT_H 0x43
+#define GYRO_XOUT_L 0x44
+#define GYRO_YOUT_H 0x45
+#define GYRO_YOUT_L 0x46
+#define GYRO_ZOUT_H 0x47
+#define GYRO_ZOUT_L 0x48
+#define EXT_SENS_DATA_00 0x49
+#define EXT_SENS_DATA_01 0x4A
+#define EXT_SENS_DATA_02 0x4B
+#define EXT_SENS_DATA_03 0x4C
+#define EXT_SENS_DATA_04 0x4D
+#define EXT_SENS_DATA_05 0x4E
+#define EXT_SENS_DATA_06 0x4F
+#define EXT_SENS_DATA_07 0x50
+#define EXT_SENS_DATA_08 0x51
+#define EXT_SENS_DATA_09 0x52
+#define EXT_SENS_DATA_10 0x53
+#define EXT_SENS_DATA_11 0x54
+#define EXT_SENS_DATA_12 0x55
+#define EXT_SENS_DATA_13 0x56
+#define EXT_SENS_DATA_14 0x57
+#define EXT_SENS_DATA_15 0x58
+#define EXT_SENS_DATA_16 0x59
+#define EXT_SENS_DATA_17 0x5A
+#define EXT_SENS_DATA_18 0x5B
+#define EXT_SENS_DATA_19 0x5C
+#define EXT_SENS_DATA_20 0x5D
+#define EXT_SENS_DATA_21 0x5E
+#define EXT_SENS_DATA_22 0x5F
+#define EXT_SENS_DATA_23 0x60
+#define MOT_DETECT_STATUS 0x61
+#define I2C_SLV0_DO 0x63
+#define I2C_SLV1_DO 0x64
+#define I2C_SLV2_DO 0x65
+#define I2C_SLV3_DO 0x66
+#define I2C_MST_DELAY_CTRL 0x67
+#define SIGNAL_PATH_RESET 0x68
+#define MOT_DETECT_CTRL 0x69
+#define USER_CTRL 0x6A // Bit 7 enable DMP, bit 3 reset DMP
+#define PWR_MGMT_1 0x6B // Device defaults to the SLEEP mode
+#define PWR_MGMT_2 0x6C
+#define DMP_BANK 0x6D // Activates a specific bank in the DMP
+#define DMP_RW_PNT 0x6E // Set read/write pointer to a specific start address in specified DMP bank
+#define DMP_REG 0x6F // Register in DMP from which to read or to which to write
+#define DMP_REG_1 0x70
+#define DMP_REG_2 0x71
+#define FIFO_COUNTH 0x72
+#define FIFO_COUNTL 0x73
+#define FIFO_R_W 0x74
+#define WHO_AM_I_MPU9250 0x75 // Should return 0x71
+#define XA_OFFSET_H 0x77
+#define XA_OFFSET_L 0x78
+#define YA_OFFSET_H 0x7A
+#define YA_OFFSET_L 0x7B
+#define ZA_OFFSET_H 0x7D
+#define ZA_OFFSET_L 0x7E
+
+// Library function prototypes
+
+class MPU9250_helper
+{
+ public:
+ MPU9250_helper();
+ // init and basic helpers
+ void initMPU9250();
+ void waitForInput();
+
+ // utility functions
+ void writeByte(uint8_t address, uint8_t subAddress, uint8_t data);
+ void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest);
+ uint8_t readByte(uint8_t address, uint8_t subAddress);
+
+ // data specific functions
+ void readAccelData(int16_t * destination);
+ float getAccelRes();
+};
+
+#endif
diff --git a/test_flight_basic_logging/test_flight_basic_logging.ino b/test_flight_basic_logging/test_flight_basic_logging.ino
index 46d2946..2d71754 100644
--- a/test_flight_basic_logging/test_flight_basic_logging.ino
+++ b/test_flight_basic_logging/test_flight_basic_logging.ino
@@ -26,183 +26,32 @@
#include <i2c_t3.h>
#include <SPI.h>
-
-#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 SELF_TEST_X_ACCEL 0x0D
-#define SELF_TEST_Y_ACCEL 0x0E
-#define SELF_TEST_Z_ACCEL 0x0F
-
-#define SELF_TEST_A 0x10
-
-#define XG_OFFSET_H 0x13 // User-defined trim values for gyroscope
-#define XG_OFFSET_L 0x14
-#define YG_OFFSET_H 0x15
-#define YG_OFFSET_L 0x16
-#define ZG_OFFSET_H 0x17
-#define ZG_OFFSET_L 0x18
-#define SMPLRT_DIV 0x19
-#define CONFIG 0x1A
-#define GYRO_CONFIG 0x1B
-#define ACCEL_CONFIG 0x1C
-#define ACCEL_CONFIG2 0x1D
-#define LP_ACCEL_ODR 0x1E
-#define WOM_THR 0x1F
-
-#define MOT_DUR 0x20 // Duration counter threshold for motion interrupt generation, 1 kHz rate, LSB = 1 ms
-#define ZMOT_THR 0x21 // Zero-motion detection threshold bits [7:0]
-#define ZRMOT_DUR 0x22 // Duration counter threshold for zero motion interrupt generation, 16 Hz rate, LSB = 64 ms
-
-#define FIFO_EN 0x23
-#define I2C_MST_CTRL 0x24
-#define I2C_SLV0_ADDR 0x25
-#define I2C_SLV0_REG 0x26
-#define I2C_SLV0_CTRL 0x27
-#define I2C_SLV1_ADDR 0x28
-#define I2C_SLV1_REG 0x29
-#define I2C_SLV1_CTRL 0x2A
-#define I2C_SLV2_ADDR 0x2B
-#define I2C_SLV2_REG 0x2C
-#define I2C_SLV2_CTRL 0x2D
-#define I2C_SLV3_ADDR 0x2E
-#define I2C_SLV3_REG 0x2F
-#define I2C_SLV3_CTRL 0x30
-#define I2C_SLV4_ADDR 0x31
-#define I2C_SLV4_REG 0x32
-#define I2C_SLV4_DO 0x33
-#define I2C_SLV4_CTRL 0x34
-#define I2C_SLV4_DI 0x35
-#define I2C_MST_STATUS 0x36
-#define INT_PIN_CFG 0x37
-#define INT_ENABLE 0x38
-#define DMP_INT_STATUS 0x39 // Check DMP interrupt
-#define INT_STATUS 0x3A
-#define ACCEL_XOUT_H 0x3B
-#define ACCEL_XOUT_L 0x3C
-#define ACCEL_YOUT_H 0x3D
-#define ACCEL_YOUT_L 0x3E
-#define ACCEL_ZOUT_H 0x3F
-#define ACCEL_ZOUT_L 0x40
-#define TEMP_OUT_H 0x41
-#define TEMP_OUT_L 0x42
-#define GYRO_XOUT_H 0x43
-#define GYRO_XOUT_L 0x44
-#define GYRO_YOUT_H 0x45
-#define GYRO_YOUT_L 0x46
-#define GYRO_ZOUT_H 0x47
-#define GYRO_ZOUT_L 0x48
-#define EXT_SENS_DATA_00 0x49
-#define EXT_SENS_DATA_01 0x4A
-#define EXT_SENS_DATA_02 0x4B
-#define EXT_SENS_DATA_03 0x4C
-#define EXT_SENS_DATA_04 0x4D
-#define EXT_SENS_DATA_05 0x4E
-#define EXT_SENS_DATA_06 0x4F
-#define EXT_SENS_DATA_07 0x50
-#define EXT_SENS_DATA_08 0x51
-#define EXT_SENS_DATA_09 0x52
-#define EXT_SENS_DATA_10 0x53
-#define EXT_SENS_DATA_11 0x54
-#define EXT_SENS_DATA_12 0x55
-#define EXT_SENS_DATA_13 0x56
-#define EXT_SENS_DATA_14 0x57
-#define EXT_SENS_DATA_15 0x58
-#define EXT_SENS_DATA_16 0x59
-#define EXT_SENS_DATA_17 0x5A
-#define EXT_SENS_DATA_18 0x5B
-#define EXT_SENS_DATA_19 0x5C
-#define EXT_SENS_DATA_20 0x5D
-#define EXT_SENS_DATA_21 0x5E
-#define EXT_SENS_DATA_22 0x5F
-#define EXT_SENS_DATA_23 0x60
-#define MOT_DETECT_STATUS 0x61
-#define I2C_SLV0_DO 0x63
-#define I2C_SLV1_DO 0x64
-#define I2C_SLV2_DO 0x65
-#define I2C_SLV3_DO 0x66
-#define I2C_MST_DELAY_CTRL 0x67
-#define SIGNAL_PATH_RESET 0x68
-#define MOT_DETECT_CTRL 0x69
-#define USER_CTRL 0x6A // Bit 7 enable DMP, bit 3 reset DMP
-#define PWR_MGMT_1 0x6B // Device defaults to the SLEEP mode
-#define PWR_MGMT_2 0x6C
-#define DMP_BANK 0x6D // Activates a specific bank in the DMP
-#define DMP_RW_PNT 0x6E // Set read/write pointer to a specific start address in specified DMP bank
-#define DMP_REG 0x6F // Register in DMP from which to read or to which to write
-#define DMP_REG_1 0x70
-#define DMP_REG_2 0x71
-#define FIFO_COUNTH 0x72
-#define FIFO_COUNTL 0x73
-#define FIFO_R_W 0x74
-#define WHO_AM_I_MPU9250 0x75 // Should return 0x71
-#define XA_OFFSET_H 0x77
-#define XA_OFFSET_L 0x78
-#define YA_OFFSET_H 0x7A
-#define YA_OFFSET_L 0x7B
-#define ZA_OFFSET_H 0x7D
-#define ZA_OFFSET_L 0x7E
-
-
-// Set initial input parameters
-enum Ascale {
- AFS_2G = 0,
- AFS_4G,
- AFS_8G,
- AFS_16G
-};
-
-enum Gscale {
- GFS_250DPS = 0,
- GFS_500DPS,
- GFS_1000DPS,
- GFS_2000DPS
-};
-
-enum Mscale {
- MFS_14BITS = 0, // 0.6 mG per LSB
- MFS_16BITS // 0.15 mG per LSB
-};
-
-#define WAITFORINPUT(){ \
- while(!Serial.available()){};\
- while(Serial.available()){ \
- Serial.read(); \
- }; \
-}
-
-uint8_t Gscale = GFS_250DPS;
-uint8_t Ascale = AFS_2G;
-uint8_t Mscale = MFS_16BITS; // Choose either 14-bit or 16-bit magnetometer resolution
-uint8_t Mmode = 0x02; // 2 for 8 Hz, 6 for 100 Hz continuous magnetometer data read
+#include <MPU9250_helper.h>
int16_t accelCount[3]; // Stores the 16-bit signed accelerometer sensor output
float aRes;
float ax,ay,az;
+MPU9250_helper sensor_helper;
+
void setup() {
Wire1.begin(I2C_MASTER, 0x00, I2C_PINS_29_30, I2C_PULLUP_EXT, I2C_RATE_400);
Serial.begin(38400);
- WAITFORINPUT();
+ sensor_helper.waitForInput();
Serial.println("Reading whoami byte (using wire1)");
- byte c = readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250
+ byte c = sensor_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);
delay(1000);
- initMPU9250();
+ sensor_helper.initMPU9250();
}
void loop() {
- readAccelData(accelCount); // Read the x/y/z adc values
- getAres();
+ sensor_helper.readAccelData(accelCount); // Read the x/y/z adc values
+ aRes = sensor_helper.getAccelRes();
// 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
@@ -213,138 +62,7 @@ void loop() {
Serial.print(" Y: "); Serial.print(ay, DEC);
Serial.print(" Z: "); Serial.println(az, DEC);
- delay(20);
+ delay(100);
}
-// -----------------------------------------------------------------------------
-// Useful functions region (Move these to header later)
-// -----------------------------------------------------------------------------
-
-
-void initMPU9250()
-{
- // wake up device
- writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Clear sleep mode bit (6), enable all sensors
- delay(100); // Wait for all registers to reset
-
- // get stable time source
- writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01); // Auto select clock source to be PLL gyroscope reference if ready else
- delay(200);
-
- // Configure Gyro and Thermometer
- // Disable FSYNC and set thermometer and gyro bandwidth to 41 and 42 Hz, respectively;
- // minimum delay time for this setting is 5.9 ms, which means sensor fusion update rates cannot
- // be higher than 1 / 0.0059 = 170 Hz
- // DLPF_CFG = bits 2:0 = 011; this limits the sample rate to 1000 Hz for both
- // With the MPU9250, it is possible to get gyro sample rates of 32 kHz (!), 8 kHz, or 1 kHz
- writeByte(MPU9250_ADDRESS, CONFIG, 0x03);
-
- // Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV)
- writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x04); // Use a 200 Hz rate; a rate consistent with the filter update rate
- // determined inset in CONFIG above
-
-// Set gyroscope full scale range
- // Range selects FS_SEL and AFS_SEL are 0 - 3, so 2-bit values are left-shifted into positions 4:3
- uint8_t c = readByte(MPU9250_ADDRESS, GYRO_CONFIG); // get current GYRO_CONFIG register value
- // c = c & ~0xE0; // Clear self-test bits [7:5]
- c = c & ~0x02; // Clear Fchoice bits [1:0]
- c = c & ~0x18; // Clear AFS bits [4:3]
- c = c | Gscale << 3; // Set full scale range for the gyro
- // c =| 0x00; // Set Fchoice for the gyro to 11 by writing its inverse to bits 1:0 of GYRO_CONFIG
- writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c ); // Write new GYRO_CONFIG value to register
-
- // Set accelerometer full-scale range configuration
- c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG); // get current ACCEL_CONFIG register value
- // c = c & ~0xE0; // Clear self-test bits [7:5]
- c = c & ~0x18; // Clear AFS bits [4:3]
- c = c | Ascale << 3; // Set full scale range for the accelerometer
- writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c); // Write new ACCEL_CONFIG register value
-
- // Set accelerometer sample rate configuration
- // It is possible to get a 4 kHz sample rate from the accelerometer by choosing 1 for
- // accel_fchoice_b bit [3]; in this case the bandwidth is 1.13 kHz
- c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG2); // get current ACCEL_CONFIG2 register value
- c = c & ~0x0F; // Clear accel_fchoice_b (bit 3) and A_DLPFG (bits [2:0])
- c = c | 0x03; // Set accelerometer rate to 1 kHz and bandwidth to 41 Hz
- writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, c); // Write new ACCEL_CONFIG2 register value
-
- // The accelerometer, gyro, and thermometer are set to 1 kHz sample rates,
- // but all these rates are further reduced by a factor of 5 to 200 Hz because of the SMPLRT_DIV setting
-
- // Configure Interrupts and Bypass Enable
- // Set interrupt pin active high, push-pull, hold interrupt pin level HIGH until interrupt cleared,
- // clear on read of INT_STATUS, and 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
- delay(100);
-}
-
-
-void readAccelData(int16_t * destination)
-{
- uint8_t rawData[6]; // x/y/z accel register data stored here
- readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // Read the six 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] ;
-}
-
-void writeByte(uint8_t address, uint8_t subAddress, uint8_t data)
-{
- Wire1.beginTransmission(address); // Initialize the Tx buffer
- Wire1.write(subAddress); // Put slave register address in Tx buffer
- Wire1.write(data); // Put data in Tx buffer
- Wire1.endTransmission(); // Send the Tx buffer
-}
-
-
-// For reading a single byte from an address
-uint8_t 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
- data = Wire1.read(); // Fill Rx buffer with result
- return data; // Return data read from slave register
-}
-
-// For reading multiple bytes
-void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest)
-{
- 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
-
- uint8_t i = 0;
- Wire1.requestFrom(address, (size_t) count); // Read bytes from slave register address
- while (Wire1.available()) {
- dest[i++] = Wire1.read(); // Put read results in the Rx buffer
- }
-}
-
-// Get accelerometer resolution
-void getAres() {
- 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:
- case AFS_2G:
- aRes = 2.0/32768.0;
- break;
- case AFS_4G:
- aRes = 4.0/32768.0;
- break;
- case AFS_8G:
- aRes = 8.0/32768.0;
- break;
- case AFS_16G:
- aRes = 16.0/32768.0;
- break;
- }
-}
-