Diff: icm20948.h

Revision:

1:8459e28d77a1

Parent:

0:efb1550773f1

--- a/icm20948.h	Fri Mar 19 21:59:25 2021 +0000
+++ b/icm20948.h	Mon Mar 29 21:16:25 2021 +0000
@@ -1,14 +1,24 @@
-/*
- Note: The ICM-20948 is an I2C sensor and uses the Arduino Wire library.
- */
-#ifndef _ICM20948_H_
-#define _ICM20948_H_
-
-#include <SPI.h>
-#include <Wire.h>
 
 #define SERIAL_DEBUG true
+#include <stdint.h>
+#include <inttypes.h>
+#include <I2C.h>
+#include "mbed.h"
+#include "mbed.h"
+#include <math.h>
+#include <stdint.h>
+#include <inttypes.h>
+#define DEG_TO_RAD (1/57.2957795)
+#define RAD_TO_DEG 57.2957795
+using namespace std::chrono;
+Timer t;
+I2C i2c(PB_7, PB_6);
+//static BufferedSerial pc(USBTX, USBRX);
 
+/*float clock_s() { return us_ticker_read() / 1000000.0f; }
+uint64_t clock_ms() { return us_ticker_read() / 1000; }
+uint64_t clock_us() { return us_ticker_read(); }
+*/
 // See also ICM-20948 Datasheet, Register Map and Descriptions, Revision 1.3,
 // https://www.invensense.com/wp-content/uploads/2016/06/DS-000189-ICM-20948-v1.3.pdf
 // and AK09916 Datasheet and Register Map
@@ -159,27 +169,18 @@
 #define I2C_SLV4_DO             0x16
 #define I2C_SLV4_DI             0x17
 
-
 // Using the ICM-20948 breakout board, ADO is set to 1
 // Seven-bit device address is 1000100 for ADO = 0 and 1000101 for ADO = 1
-#define ADO 1
+#define ADO 0 
 #if ADO
-#define ICM20948_ADDRESS 0x69  // Device address when ADO = 1
+#define ICM20948_ADDRESS 0x69<<1  // Device address when ADO = 1
 #else
-#define ICM20948_ADDRESS 0x68  // Device address when ADO = 0
+#define ICM20948_ADDRESS 0x68<<1  // Device address when ADO = 0
 #define AK09916_ADDRESS  0x0C   // Address of magnetometer
 #endif // AD0
 
-#define READ_FLAG 0x80
-#define NOT_SPI -1
-#define SPI_DATA_RATE 1000000 // 1MHz is the max speed of the ICM-20948
-//#define SPI_DATA_RATE 1000000 // 1MHz is the max speed of the ICM-20948
-#define SPI_MODE SPI_MODE3
+#define READ_FLAGS 0x80
 
-class ICM20948
-{
-  protected:
-    // Set initial input parameters
     enum Ascale
     {
       AFS_2G = 0,
@@ -213,28 +214,18 @@
     // 2 for 8 Hz, 6 for 100 Hz continuous magnetometer data read
     uint8_t Mmode = M_100HZ;
 
-    // SPI chip select pin
-    int8_t _csPin;
-
     uint8_t writeByteWire(uint8_t, uint8_t, uint8_t);
-    uint8_t writeByteSPI(uint8_t, uint8_t);
-    uint8_t readByteSPI(uint8_t subAddress);
+ 
     uint8_t readByteWire(uint8_t address, uint8_t subAddress);
-    bool magInit();
-    void kickHardware();
-    void select();
-    void deselect();
-// TODO: Remove this next line
-public:
-    uint8_t ak09916WhoAmI_SPI();
+    
 
-  public:
+
     float pitch, yaw, roll;
     float temperature;   // Stores the real internal chip temperature in Celsius
     int16_t tempCount;   // Temperature raw count output
     uint32_t delt_t = 0; // Used to control display output rate
 
-    uint32_t count = 0, sumCount = 0; // used to control display output rate
+    uint32_t counts = 0, sumCount = 0; // used to control display output rate
     float deltat = 0.0f, sum = 0.0f;  // integration interval for both filter schemes
     uint32_t lastUpdate = 0, firstUpdate = 0; // used to calculate integration interval
     uint32_t Now = 0;        // used to calculate integration interval
@@ -252,12 +243,11 @@
           accelBias[3] = {0, 0, 0},
           magBias[3]   = {0, 0, 0},
           magScale[3]  = {0, 0, 0};
-    float selfTest[6];
+  //  float selfTest[6];
     // Stores the 16-bit signed accelerometer sensor output
     int16_t accelCount[3];
 
     // Public method declarations
-    ICM20948(int8_t csPin=NOT_SPI);
     void getMres();
     void getGres();
     void getAres();
@@ -274,11 +264,771 @@
     uint8_t writeByte(uint8_t, uint8_t, uint8_t);
     uint8_t readByte(uint8_t, uint8_t);
     uint8_t readBytes(uint8_t, uint8_t, uint8_t, uint8_t *);
-    // TODO: make SPI/Wire private
-    uint8_t readBytesSPI(uint8_t, uint8_t, uint8_t *);
     uint8_t readBytesWire(uint8_t, uint8_t, uint8_t, uint8_t *);
-    bool isInI2cMode() { return _csPin == -1; }
     bool begin();
-};  // class ICM20948
+
+
+bool begin(void)
+{
+    i2c.frequency(400000);  // use fast (400 kHz) I2C 
+    t.start();
+    return true;
+}
+
+void getMres()
+{
+    mRes = 10.0f * 4912.0f / 32760.0f; // Proper scale to return milliGauss
+}
+
+void getGres()
+{
+  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:
+      gRes = 250.0f / 32768.0f;
+      break;
+    case GFS_500DPS:
+      gRes = 500.0f / 32768.0f;
+      break;
+    case GFS_1000DPS:
+      gRes = 1000.0f / 32768.0f;
+      break;
+    case GFS_2000DPS:
+      gRes = 2000.0f / 32768.0f;
+      break;
+  }
+}
+
+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.0f / 32768.0f;
+      break;
+    case AFS_4G:
+      aRes = 4.0f / 32768.0f;
+      break;
+    case AFS_8G:
+      aRes = 8.0f / 32768.0f;
+      break;
+    case AFS_16G:
+      aRes = 16.0f / 32768.0f;
+      break;
+  }
+}
+
+
+void readAccelData(int16_t * destination)
+{
+  uint8_t rawData[6];  // x/y/z accel register data stored here
+  // Read the six raw data registers into data array
+ // readBytes(ICM20948_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]);
+    for(int z=0;z<6;z++)
+    {
+        rawData[z]=readByte(ICM20948_ADDRESS, ACCEL_XOUT_H+z);
+    }
+  // Turn the MSB and LSB into a signed 16-bit value
+  destination[0] = (int16_t)(rawData[0] << 8) | rawData[1];
+  destination[1] = (int16_t)(rawData[2] << 8) | rawData[3];
+  destination[2] = (int16_t)(rawData[4] << 8) | rawData[5];
+}
+
+
+void readGyroData(int16_t * destination)
+{
+  uint8_t rawData[6];  // x/y/z gyro register data stored here
+  // Read the six raw data registers sequentially into data array
+  readBytes(ICM20948_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]);
+
+  // Turn the MSB and LSB into a signed 16-bit value
+  destination[0] = (int16_t)(rawData[0] << 8) | rawData[1];
+  destination[1] = (int16_t)(rawData[2] << 8) | rawData[3];
+  destination[2] = (int16_t)(rawData[4] << 8) | rawData[5];
+}
+
+void readMagData(int16_t * destination)
+{
+  // x/y/z gyro register data, ST2 register stored here, must read ST2 at end
+  // of data acquisition
+  uint8_t rawData[8];
+  // thread_sleep_for for magnetometer data ready bit to be set
+  if (readByte(AK09916_ADDRESS, AK09916_ST1) & 0x01)
+  {
+      
+    // Read the six raw data and ST2 registers sequentially into data array
+    readBytes(AK09916_ADDRESS, AK09916_XOUT_L, 8, &rawData[0]);
+    uint8_t c = rawData[7]; // End data read by reading ST2 register
+    // Check if magnetic sensor overflow set, if not then report data
+      // Remove once finished
+    
+    if (!(c & 0x08))
+    {
+      // Turn the MSB and LSB into a signed 16-bit value
+      destination[0] = ((int16_t)rawData[1] << 8) | rawData[0];
+      // Data stored as little Endian
+      destination[1] = ((int16_t)rawData[3] << 8) | rawData[2];
+      destination[2] = ((int16_t)rawData[5] << 8) | rawData[4];
+    }
+  }
+}
+
+int16_t readTempData()
+{
+  uint8_t rawData[2]; // x/y/z gyro register data stored here
+  // Read the two raw data registers sequentially into data array
+  readBytes(ICM20948_ADDRESS, TEMP_OUT_H, 2, &rawData[0]);
+  // Turn the MSB and LSB into a 16-bit value
+  return ((int16_t)rawData[0] << 8) | rawData[1];
+}
+
+// Calculate the time the last update took for use in the quaternion filters
+// TODO: This doesn't really belong in this class.
+void updateTime()
+{
+  Now = t.elapsed_time().count();;
+
+  // Set integration time by time elapsed since last filter update
+  deltat = ((Now - lastUpdate) / 1000000.0f);
+  lastUpdate = Now;
+
+  sum += deltat; // sum for averaging filter update rate
+  sumCount++;
+}
+
+void initAK09916()
+{
+    
+    // Write code to initialise magnetometer
+    // Bypass I2C master interface and turn on magnetometer
+ // writeByte(ICM20948_ADDRESS, INT_PIN_CFG, 0x02); //Already set in initICM20948
+
+  // Configure the magnetometer for continuous read and highest resolution.
+  // Enable continuous mode data acquisition Mmode (bits [3:0]),
+  // 0010 for 8 Hz and 0110 for 100 Hz sample rates.
+
+  // Set magnetometer data resolution and sample ODR
+  writeByte(AK09916_ADDRESS, AK09916_CNTL2, 0x08);
+  thread_sleep_for(10);
+}
+
+void initICM20948()
+{
+    // Get stable time source
+  // Auto select clock source to be PLL gyroscope reference if ready else
+  writeByte(ICM20948_ADDRESS, PWR_MGMT_1, 0x01);
+  thread_sleep_for(200);
+  // Switch to user bank 2
+  writeByte(ICM20948_ADDRESS, REG_BANK_SEL, 0x20);
+  // Configure Gyro and Thermometer
+  // Disable FSYNC and set gyro bandwidth to 51.2 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 ICM20948, it is possible to get gyro sample rates of 32 kHz (!),
+  // 8 kHz, or 1 kHz
+  // Set gyroscope full scale range to 250 dps
+  writeByte(ICM20948_ADDRESS, GYRO_CONFIG_1, 0x19);
+  writeByte(ICM20948_ADDRESS, TEMP_CONFIG, 0x03);
+
+  // Set sample rate = gyroscope output rate/(1 + GYRO_SMPLRT_DIV)
+  // Use a 220 Hz rate; a rate consistent with the filter update rate
+  // determined inset in CONFIG above.
+  
+  writeByte(ICM20948_ADDRESS, GYRO_SMPLRT_DIV, 0x04);
+
+  // 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
+
+  // Set accelerometer full-scale range configuration
+  // Get current ACCEL_CONFIG register value
+  uint8_t c = readByte(ICM20948_ADDRESS, ACCEL_CONFIG);
+  // c = c & ~0xE0; // Clear self-test bits [7:5]
+  c = c & ~0x06;  // Clear AFS bits [4:3]
+  c = c | Ascale << 1; // Set full scale range for the accelerometer
+  c = c | 0x01; // Set enable accel DLPF for the accelerometer
+  c = c | 0x18; // and set DLFPFCFG to 50.4 hz
+  // Write new ACCEL_CONFIG register value
+  writeByte(ICM20948_ADDRESS, ACCEL_CONFIG, c);
+
+  // 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
+  writeByte(ICM20948_ADDRESS, ACCEL_SMPLRT_DIV_2, 0x04);
+  // 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 GYRO_SMPLRT_DIV setting
+
+  // Switch to user bank 0
+  writeByte(ICM20948_ADDRESS, REG_BANK_SEL, 0x00);
+
+  // 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(ICM20948_ADDRESS, INT_PIN_CFG, 0x22);
+  // Enable data ready (bit 0) interrupt
+  writeByte(ICM20948_ADDRESS, INT_ENABLE_1, 0x01);
+}
+
+
+// Function which accumulates gyro and accelerometer data after device
+// initialization. It calculates the average of the at-rest readings and then
+// loads the resulting offsets into accelerometer and gyro bias registers.
+void calibrateICM20948(float * gyroBias, float * accelBias)
+{
+  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
+  // Write a one to bit 7 reset bit; toggle reset device
+  writeByte(ICM20948_ADDRESS, PWR_MGMT_1, READ_FLAGS);
+  thread_sleep_for(200);
+
+  // 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(ICM20948_ADDRESS, PWR_MGMT_1, 0x01);
+  thread_sleep_for(200);
+
+  // Configure device for bias calculation
+  // Disable all interrupts
+  writeByte(ICM20948_ADDRESS, INT_ENABLE, 0x00);
+  // Disable FIFO
+  writeByte(ICM20948_ADDRESS, FIFO_EN_1, 0x00);
+  writeByte(ICM20948_ADDRESS, FIFO_EN_2, 0x00);
+  // Turn on internal clock source
+  writeByte(ICM20948_ADDRESS, PWR_MGMT_1, 0x00);
+  // Disable I2C master
+  //writeByte(ICM20948_ADDRESS, I2C_MST_CTRL, 0x00); Already disabled
+  // Disable FIFO and I2C master modes
+  writeByte(ICM20948_ADDRESS, USER_CTRL, 0x00);
+  // Reset FIFO and DMP
+  writeByte(ICM20948_ADDRESS, USER_CTRL, 0x08);
+  writeByte(ICM20948_ADDRESS, FIFO_RST, 0x1F);
+  thread_sleep_for(10);
+  writeByte(ICM20948_ADDRESS, FIFO_RST, 0x00);
+  thread_sleep_for(15);
+
+  // Set FIFO mode to snapshot
+  writeByte(ICM20948_ADDRESS, FIFO_MODE, 0x1F);
+  // Switch to user bank 2
+  writeByte(ICM20948_ADDRESS, REG_BANK_SEL, 0x20);
+  // Configure ICM20948 gyro and accelerometer for bias calculation
+  // Set low-pass filter to 188 Hz
+  writeByte(ICM20948_ADDRESS, GYRO_CONFIG_1, 0x01);
+  // Set sample rate to 1 kHz
+  writeByte(ICM20948_ADDRESS, GYRO_SMPLRT_DIV, 0x00);
+  // Set gyro full-scale to 250 degrees per second, maximum sensitivity
+  writeByte(ICM20948_ADDRESS, GYRO_CONFIG_1, 0x00);
+  // Set accelerometer full-scale to 2 g, maximum sensitivity
+  writeByte(ICM20948_ADDRESS, ACCEL_CONFIG, 0x00);
+
+  uint16_t  gyrosensitivity  = 131;   // = 131 LSB/degrees/sec
+  uint16_t  accelsensitivity = 16384; // = 16384 LSB/g
+
+  // Switch to user bank 0
+  writeByte(ICM20948_ADDRESS, REG_BANK_SEL, 0x00);
+  // Configure FIFO to capture accelerometer and gyro data for bias calculation
+  writeByte(ICM20948_ADDRESS, USER_CTRL, 0x40);  // Enable FIFO
+  // Enable gyro and accelerometer sensors for FIFO  (max size 512 bytes in
+  // ICM20948)
+  writeByte(ICM20948_ADDRESS, FIFO_EN_2, 0x1E);
+  thread_sleep_for(40);  // accumulate 40 samples in 40 milliseconds = 480 bytes
+
+  // At end of sample accumulation, turn off FIFO sensor read
+  // Disable gyro and accelerometer sensors for FIFO
+  writeByte(ICM20948_ADDRESS, FIFO_EN_2, 0x00);
+  // Read FIFO sample count
+  readBytes(ICM20948_ADDRESS, FIFO_COUNTH, 2, &data[0]);
+  fifo_count = ((uint16_t)data[0] << 8) | data[1];
+  // How many sets of full gyro and accelerometer data for averaging
+  packet_count = fifo_count/12;
+
+  for (ii = 0; ii < packet_count; ii++)
+  {
+    int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0};
+    // Read data for averaging
+    readBytes(ICM20948_ADDRESS, FIFO_R_W, 12, &data[0]);
+    // Form signed 16-bit integer for each sample in FIFO
+    accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1]  );
+    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]);
+
+    // Sum individual signed 16-bit biases to get accumulated signed 32-bit
+    // biases.
+    accel_bias[0] += (int32_t) accel_temp[0];
+    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];
+  }
+  // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
+  accel_bias[0] /= (int32_t) packet_count;
+  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;
+
+  // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
+  if (accel_bias[2] > 0L)
+  {
+    accel_bias[2] -= (int32_t) accelsensitivity;
+  }
+  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.
+  // Divide by 4 to get 32.9 LSB per deg/s to conform to expected bias input
+  // format.
+  data[0] = (-gyro_bias[0]/4  >> 8) & 0xFF;
+  // Biases are additive, so change sign on calculated average gyro biases
+  data[1] = (-gyro_bias[0]/4)       & 0xFF;
+  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;
+  
+  // Switch to user bank 2
+  writeByte(ICM20948_ADDRESS, REG_BANK_SEL, 0x20);
+
+  // Push gyro biases to hardware registers
+  writeByte(ICM20948_ADDRESS, XG_OFFSET_H, data[0]);
+  writeByte(ICM20948_ADDRESS, XG_OFFSET_L, data[1]);
+  writeByte(ICM20948_ADDRESS, YG_OFFSET_H, data[2]);
+  writeByte(ICM20948_ADDRESS, YG_OFFSET_L, data[3]);
+  writeByte(ICM20948_ADDRESS, ZG_OFFSET_H, data[4]);
+  writeByte(ICM20948_ADDRESS, ZG_OFFSET_L, data[5]);
+
+  // Output scaled gyro biases for display in the main program
+  gyroBias[0] = (float) gyro_bias[0]/(float) gyrosensitivity;
+  gyroBias[1] = (float) gyro_bias[1]/(float) gyrosensitivity;
+  gyroBias[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.
+  
+  // Switch to user bank 1
+  writeByte(ICM20948_ADDRESS, REG_BANK_SEL, 0x10);
+  // A place to hold the factory accelerometer trim biases
+  int32_t accel_bias_reg[3] = {0, 0, 0};
+  // Read factory accelerometer trim values
+  readBytes(ICM20948_ADDRESS, XA_OFFSET_H, 2, &data[0]);
+  accel_bias_reg[0] = (int32_t) (((int16_t)data[0] << 8) | data[1]);
+  readBytes(ICM20948_ADDRESS, YA_OFFSET_H, 2, &data[0]);
+  accel_bias_reg[1] = (int32_t) (((int16_t)data[0] << 8) | data[1]);
+  readBytes(ICM20948_ADDRESS, ZA_OFFSET_H, 2, &data[0]);
+  accel_bias_reg[2] = (int32_t) (((int16_t)data[0] << 8) | data[1]);
 
-#endif // _ICM20948_H_
\ No newline at end of file
+  // Define mask for temperature compensation bit 0 of lower byte of
+  // accelerometer bias registers
+  uint32_t mask = 1uL;
+  // Define array to hold mask bit for each accelerometer bias axis
+  uint8_t mask_bit[3] = {0, 0, 0};
+
+  for (ii = 0; ii < 3; ii++)
+  {
+    // If temperature compensation bit is set, record that fact in mask_bit
+    if ((accel_bias_reg[ii] & mask))
+    {
+      mask_bit[ii] = 0x01;
+    }
+  }
+
+  // Construct total accelerometer bias, including calculated average
+  // accelerometer bias from above
+  // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g
+  // (16 g full scale)
+  accel_bias_reg[0] -= (accel_bias[0]/8);
+  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;
+  // preserve temperature compensation bit when writing back to accelerometer
+  // bias registers
+  data[1] = data[1] | mask_bit[0];
+  data[2] = (accel_bias_reg[1] >> 8) & 0xFF;
+  data[3] = (accel_bias_reg[1]) & 0xFF;
+  // Preserve temperature compensation bit when writing back to accelerometer
+  // bias registers
+  data[3] = data[3] | mask_bit[1];
+  data[4] = (accel_bias_reg[2] >> 8) & 0xFF;
+  data[5] = (accel_bias_reg[2]) & 0xFF;
+  // Preserve temperature compensation bit when writing back to accelerometer
+  // bias registers
+  data[5] = data[5] | mask_bit[2];
+
+  // Apparently this is not working for the acceleration biases in the ICM-20948
+  // Are we handling the temperature correction bit properly?
+  // Push accelerometer biases to hardware registers
+  writeByte(ICM20948_ADDRESS, XA_OFFSET_H, data[0]);
+  writeByte(ICM20948_ADDRESS, XA_OFFSET_L, data[1]);
+  writeByte(ICM20948_ADDRESS, YA_OFFSET_H, data[2]);
+  writeByte(ICM20948_ADDRESS, YA_OFFSET_L, data[3]);
+  writeByte(ICM20948_ADDRESS, ZA_OFFSET_H, data[4]);
+  writeByte(ICM20948_ADDRESS, ZA_OFFSET_L, data[5]);
+
+  // Output scaled accelerometer biases for display in the main program
+  accelBias[0] = (float)accel_bias[0]/(float)accelsensitivity;
+  accelBias[1] = (float)accel_bias[1]/(float)accelsensitivity;
+  accelBias[2] = (float)accel_bias[2]/(float)accelsensitivity;
+  // Switch to user bank 0
+  writeByte(ICM20948_ADDRESS, REG_BANK_SEL, 0x00);
+}
+
+
+// Accelerometer and gyroscope self test; check calibration wrt factory settings
+// Should return percent deviation from factory trim values, +/- 14 or less
+// deviation is a pass.
+void ICM20948SelfTest(float * destination)
+{
+  uint8_t rawData[6] = {0, 0, 0, 0, 0, 0};
+  uint8_t selfTest[6];
+  int32_t gAvg[3] = {0}, aAvg[3] = {0}, aSTAvg[3] = {0}, gSTAvg[3] = {0};
+  float factoryTrim[6];
+  uint8_t FS = 0;
+  // Get stable time source
+  // Auto select clock source to be PLL gyroscope reference if ready else
+  writeByte(ICM20948_ADDRESS, PWR_MGMT_1, 0x01);
+  thread_sleep_for(200);
+  // Switch to user bank 2
+  writeByte(ICM20948_ADDRESS, REG_BANK_SEL, 0x20);
+  // Set gyro sample rate to 1 kHz
+  writeByte(ICM20948_ADDRESS, GYRO_SMPLRT_DIV, 0x00);
+  // Set gyro sample rate to 1 kHz, DLPF to 119.5 Hz and FSR to 250 dps
+  writeByte(ICM20948_ADDRESS, GYRO_CONFIG_1, 0x11);
+  // Set accelerometer rate to 1 kHz and bandwidth to 111.4 Hz
+  // Set full scale range for the accelerometer to 2 g
+  writeByte(ICM20948_ADDRESS, ACCEL_CONFIG, 0x11);
+  // Switch to user bank 0
+  writeByte(ICM20948_ADDRESS, REG_BANK_SEL, 0x00);
+  // Get average current values of gyro and acclerometer
+  for (int ii = 0; ii < 200; ii++)
+  {
+
+    // Read the six raw data registers into data array
+    for(int z=0;z<6;z++)
+    {
+        rawData[z]=readByte(ICM20948_ADDRESS, ACCEL_XOUT_H+z);
+    }
+    // Turn the MSB and LSB into a signed 16-bit value
+    aAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ;
+    aAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
+    aAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
+
+    // Read the six raw data registers sequentially into data array
+    for(int z=0;z<6;z++)
+    {
+        rawData[z]=readByte(ICM20948_ADDRESS, GYRO_XOUT_H+z);
+    }
+    // Turn the MSB and LSB into a signed 16-bit value
+    gAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ;
+    gAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
+    gAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
+  }
+
+  // Get average of 200 values and store as average current readings
+  for (int ii =0; ii < 3; ii++)
+  {
+    aAvg[ii] /= 200;
+    gAvg[ii] /= 200;
+  }
+  // Switch to user bank 2
+  writeByte(ICM20948_ADDRESS, REG_BANK_SEL, 0x20);
+  // Configure the accelerometer for self-test
+  // Enable self test on all three axes and set accelerometer range to +/- 2 g
+  writeByte(ICM20948_ADDRESS, ACCEL_CONFIG_2, 0x1C);
+  // Enable self test on all three axes and set gyro range to +/- 250 degrees/s
+  writeByte(ICM20948_ADDRESS, GYRO_CONFIG_2,  0x38);
+  thread_sleep_for(25);  // Delay a while to let the device stabilize
+  // Switch to user bank 0
+  writeByte(ICM20948_ADDRESS, REG_BANK_SEL, 0x00);
+  // Get average self-test values of gyro and acclerometer
+  for (int ii = 0; ii < 200; ii++)
+  {
+    // Read the six raw data registers into data array
+   // readBytes(ICM20948_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]);
+   for(int z=0;z<6;z++)
+    {
+        rawData[z]=readByte(ICM20948_ADDRESS, ACCEL_XOUT_H+z);
+    }
+    // Turn the MSB and LSB into a signed 16-bit value
+    aSTAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ;
+    aSTAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
+    aSTAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
+
+    // Read the six raw data registers sequentially into data array
+    //readBytes(ICM20948_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]);
+    for(int z=0;z<6;z++)
+    {
+        rawData[z]=readByte(ICM20948_ADDRESS, GYRO_XOUT_H+z);
+    }
+    // Turn the MSB and LSB into a signed 16-bit value
+    gSTAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ;
+    gSTAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
+    gSTAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
+  }
+
+  // Get average of 200 values and store as average self-test readings
+  for (int ii =0; ii < 3; ii++)
+  {
+    aSTAvg[ii] /= 200;
+    gSTAvg[ii] /= 200;
+  }
+  
+  // Switch to user bank 2
+  writeByte(ICM20948_ADDRESS, REG_BANK_SEL, 0x20);
+  // Configure the gyro and accelerometer for normal operation
+  writeByte(ICM20948_ADDRESS, ACCEL_CONFIG_2, 0x00);
+  writeByte(ICM20948_ADDRESS, GYRO_CONFIG_2,  0x00);
+  thread_sleep_for(25);  // Delay a while to let the device stabilize
+  // Switch to user bank 1
+  writeByte(ICM20948_ADDRESS, REG_BANK_SEL, 0x10);
+  // Retrieve accelerometer and gyro factory Self-Test Code from USR_Reg
+  // X-axis accel self-test results
+  selfTest[0] = readByte(ICM20948_ADDRESS, SELF_TEST_X_ACCEL);
+  // Y-axis accel self-test results
+  selfTest[1] = readByte(ICM20948_ADDRESS, SELF_TEST_Y_ACCEL);
+  // Z-axis accel self-test results
+  selfTest[2] = readByte(ICM20948_ADDRESS, SELF_TEST_Z_ACCEL);
+  // X-axis gyro self-test results
+  selfTest[3] = readByte(ICM20948_ADDRESS, SELF_TEST_X_GYRO);
+  // Y-axis gyro self-test results
+  selfTest[4] = readByte(ICM20948_ADDRESS, SELF_TEST_Y_GYRO);
+  // Z-axis gyro self-test results
+  selfTest[5] = readByte(ICM20948_ADDRESS, SELF_TEST_Z_GYRO);
+  // Switch to user bank 0
+  writeByte(ICM20948_ADDRESS, REG_BANK_SEL, 0x00);
+  // Retrieve factory self-test value from self-test code reads
+  // FT[Xa] factory trim calculation
+  factoryTrim[0] = (float)(2620/1<<FS)*(pow(1.01 ,((float)selfTest[0] - 1.0) ));
+  // FT[Ya] factory trim calculation
+  factoryTrim[1] = (float)(2620/1<<FS)*(pow(1.01 ,((float)selfTest[1] - 1.0) ));
+  // FT[Za] factory trim calculation
+  factoryTrim[2] = (float)(2620/1<<FS)*(pow(1.01 ,((float)selfTest[2] - 1.0) ));
+  // FT[Xg] factory trim calculation
+  factoryTrim[3] = (float)(2620/1<<FS)*(pow(1.01 ,((float)selfTest[3] - 1.0) ));
+  // FT[Yg] factory trim calculation
+  factoryTrim[4] = (float)(2620/1<<FS)*(pow(1.01 ,((float)selfTest[4] - 1.0) ));
+  // FT[Zg] factory trim calculation
+  factoryTrim[5] = (float)(2620/1<<FS)*(pow(1.01 ,((float)selfTest[5] - 1.0) ));
+
+  // Report results as a ratio of (STR - FT)/FT; the change from Factory Trim
+  // of the Self-Test Response
+  // To get percent, must multiply by 100
+  for (int i = 0; i < 3; i++)
+  {
+    // Report percent differences
+    destination[i] = 100.0 * ((float)(aSTAvg[i] - aAvg[i]))/factoryTrim[i]- 100./*selfTest[i]*/;
+    // Report percent differences
+    destination[i+3] =100.0*((float)(gSTAvg[i] - gAvg[i]))/factoryTrim[i+3]- 100./*selfTest[i+3]*/;
+  }
+}
+
+// Function which accumulates magnetometer data after device initialization.
+// It calculates the bias and scale in the x, y, and z axes.
+void magCalICM20948(float * bias_dest, float * scale_dest)
+{
+  uint16_t ii = 0, sample_count = 0;
+  int32_t mag_bias[3]  = {0, 0, 0},
+          mag_scale[3] = {0, 0, 0};
+  int32_t mag_max[3]  = {0x8000, 0x8000, 0x8000},
+          mag_min[3]  = {0x7FFF, 0x7FFF, 0x7FFF},
+          mag_temp[3] = {0, 0, 0};
+
+  // Make sure resolution has been calculated
+  getMres();
+  thread_sleep_for(4000);
+
+  // shoot for ~fifteen seconds of mag data
+  // at 8 Hz ODR, new mag data is available every 125 ms
+  if (Mmode == M_8HZ)
+  {
+    sample_count = 128;
+  }
+  // at 100 Hz ODR, new mag data is available every 10 ms
+  if (Mmode == M_100HZ)
+  {
+    sample_count = 1500;
+  }
+
+  for (ii = 0; ii < sample_count; ii++)
+  {
+    readMagData((int16_t *) mag_temp);  // Read the mag data
+
+    for (int jj = 0; jj < 3; jj++)
+    {
+      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];
+      }
+    }
+
+    if (Mmode == M_8HZ)
+    {
+      thread_sleep_for(135); // At 8 Hz ODR, new mag data is available every 125 ms
+    }
+    if (Mmode == M_100HZ)
+    {
+      thread_sleep_for(12);  // At 100 Hz ODR, new mag data is available every 10 ms
+    }
+  }
+
+  // pc.println("mag x min/max:"); pc.println(mag_max[0]); pc.println(mag_min[0]);
+  // pc.println("mag y min/max:"); pc.println(mag_max[1]); pc.println(mag_min[1]);
+  // pc.println("mag z min/max:"); pc.println(mag_max[2]); pc.println(mag_min[2]);
+
+  // Get hard iron correction
+  // Get 'average' x mag bias in counts
+  mag_bias[0]  = (mag_max[0] + mag_min[0]) / 2;
+  // Get 'average' y mag bias in counts
+  mag_bias[1]  = (mag_max[1] + mag_min[1]) / 2;
+  // Get 'average' z mag bias in counts
+  mag_bias[2]  = (mag_max[2] + mag_min[2]) / 2;
+
+  // Save mag biases in G for main program
+  bias_dest[0] = (float)mag_bias[0] * mRes;// * factoryMagCalibration[0];
+  bias_dest[1] = (float)mag_bias[1] * mRes;// * factoryMagCalibration[1];
+  bias_dest[2] = (float)mag_bias[2] * mRes;// * factoryMagCalibration[2];
+
+  // Get soft iron correction estimate
+  // Get average x axis max chord length in counts
+  mag_scale[0]  = (mag_max[0] - mag_min[0]) / 2;
+  // Get average y axis max chord length in counts
+  mag_scale[1]  = (mag_max[1] - mag_min[1]) / 2;
+  // Get average z axis max chord length in counts
+  mag_scale[2]  = (mag_max[2] - mag_min[2]) / 2;
+
+  float avg_rad = mag_scale[0] + mag_scale[1] + mag_scale[2];
+  avg_rad /= 3.0;
+
+  scale_dest[0] = avg_rad / ((float)mag_scale[0]);
+  scale_dest[1] = avg_rad / ((float)mag_scale[1]);
+  scale_dest[2] = avg_rad / ((float)mag_scale[2]);
+}
+
+// Wire.h read and write protocols
+uint8_t writeByte(uint8_t deviceAddress, uint8_t registerAddress,uint8_t data)
+{
+  //writeByteWire(deviceAddress,registerAddress, data);
+    char tmp[2];
+    tmp[0]=registerAddress;
+    tmp[1]=data;
+    i2c.write(deviceAddress,  tmp, 2, 0); // no stop
+    return NULL;
+}
+
+uint8_t writeByteOne(uint8_t deviceAddress, uint8_t registerAddress)
+{
+  char tmp[2];
+    tmp[0]=registerAddress;
+    i2c.write(deviceAddress,  tmp, 1, 1);
+  return NULL;
+}
+
+/*
+uint8_t writeByteWire(uint8_t deviceAddress, uint8_t registerAddress,
+                            uint8_t data)
+{ // i2c.write(address, data_write, 1, 1); // no stop
+    char tmp[2];
+    tmp[0]=registerAddress;
+    i2c.write(deviceAddress,  tmp, 1, 1); // no stop
+    tmp[0]=data;
+    i2c.write(deviceAddress,  tmp, 1, 0); // stop
+  // TODO: Fix this to return something meaningful
+  return NULL;
+}
+*/
+// Read a byte from given register on device. Calls necessary SPI or I2C
+// implementation. This was configured in the constructor.
+uint8_t readByte(uint8_t deviceAddress, uint8_t registerAddress)
+{
+ char tmp[1];
+    tmp[0]=registerAddress;
+  i2c.write(deviceAddress,tmp, 1, 1); // no stop
+  //tmp[0]=data;
+  i2c.read(deviceAddress, tmp, 1, 0);//stop
+  // Return data read from slave register
+  return (uint8_t) tmp[0];
+}
+/*
+
+uint8_t readByteWire(uint8_t deviceAddress, uint8_t registerAddress)
+{
+  uint8_t data; // `data` will store the register data
+// i2c.write(address, data_write, 1, 1); // no stop
+//    i2c.read(address, data, count, 0); 
+  // Initialize the Tx buffer
+  char tmp[2];
+    tmp[0]=registerAddress;
+  i2c.write(deviceAddress,tmp, 1, 0); // no stop
+  //tmp[0]=data;
+  i2c.read(deviceAddress, tmp, 1, 0);//stop
+  // Return data read from slave register
+  return tmp[0];
+}
+
+*/
+// Read 1 or more bytes from given register and device using I2C
+uint8_t readBytesWire(uint8_t deviceAddress, uint8_t registerAddress,
+                        uint8_t count, uint8_t * dest)
+{
+    char tmp[2];
+    tmp[0]=registerAddress;
+  i2c.write(deviceAddress, tmp, 1, 1); // no stop
+  i2c.read(deviceAddress,(char *) dest, count, 0);//stop
+  // Initialize the Tx buffer
+/*  Wire.beginTransmission(deviceAddress);
+  // Put slave register address in Tx buffer
+  Wire.write(registerAddress);
+  // Send the Tx buffer, but send a restart to keep connection alive
+  Wire.endTransmission(false);
+
+  uint8_t i = 0;
+  // Read bytes from slave register address
+  Wire.requestFrom(deviceAddress, count);
+  while (Wire.available())
+  {
+    // Put read results in the Rx buffer
+    dest[i++] = Wire.read();
+  }
+*/
+  return count; // Return number of bytes written
+}
+
+uint8_t readBytes(uint8_t deviceAddress, uint8_t registerAddress,
+                        uint8_t count, uint8_t * dest)
+{
+ 
+    return readBytesWire(deviceAddress, registerAddress, count, dest);
+  
+}