icm20948.h

00001 
00002 #define SERIAL_DEBUG true
00003 #include <stdint.h>
00004 #include <inttypes.h>
00005 #include <I2C.h>
00006 #include "mbed.h"
00007 #include "mbed.h"
00008 #include <math.h>
00009 #include <stdint.h>
00010 #include <inttypes.h>
00011 #define DEG_TO_RAD (1/57.2957795)
00012 #define RAD_TO_DEG 57.2957795
00013 using namespace std::chrono;
00014 Timer t;
00015 I2C i2c(PB_7, PB_6);
00016 //static BufferedSerial pc(USBTX, USBRX);
00017 
00018 /*float clock_s() { return us_ticker_read() / 1000000.0f; }
00019 uint64_t clock_ms() { return us_ticker_read() / 1000; }
00020 uint64_t clock_us() { return us_ticker_read(); }
00021 */
00022 // See also ICM-20948 Datasheet, Register Map and Descriptions, Revision 1.3,
00023 // https://www.invensense.com/wp-content/uploads/2016/06/DS-000189-ICM-20948-v1.3.pdf
00024 // and AK09916 Datasheet and Register Map
00025 // https://www.akm.com/akm/en/file/datasheet/AK09916C.pdf
00026 
00027 //Magnetometer Registers
00028 #define AK09916_ADDRESS  0x0C 
00029 #define WHO_AM_I_AK09916 0x01 // (AKA WIA2) should return 0x09
00030 #define AK09916_ST1      0x10  // data ready status bit 0
00031 #define AK09916_XOUT_L   0x11  // data
00032 #define AK09916_XOUT_H   0x12
00033 #define AK09916_YOUT_L   0x13
00034 #define AK09916_YOUT_H   0x14
00035 #define AK09916_ZOUT_L   0x15
00036 #define AK09916_ZOUT_H   0x16
00037 #define AK09916_ST2      0x18  // Data overflow bit 3 and data read error status bit 2
00038 #define AK09916_CNTL     0x30  // Power down (0000), single-measurement (0001), self-test (1000) and Fuse ROM (1111) modes on bits 3:0
00039 #define AK09916_CNTL2    0x31  // Normal (0), Reset (1)
00040 
00041 // ICM-20948
00042 
00043 // USER BANK 0 REGISTER MAP
00044 #define WHO_AM_I_ICM20948  0x00 // Should return 0xEA
00045 #define USER_CTRL          0x03  // Bit 7 enable DMP, bit 3 reset DMP
00046 #define LP_CONFIG          0x05 // Not found in MPU-9250
00047 #define PWR_MGMT_1         0x06 // Device defaults to the SLEEP mode
00048 #define PWR_MGMT_2         0x07
00049 #define INT_PIN_CFG        0x0F
00050 #define INT_ENABLE         0x10
00051 #define INT_ENABLE_1       0x11 // Not found in MPU-9250
00052 #define INT_ENABLE_2       0x12 // Not found in MPU-9250
00053 #define INT_ENABLE_3       0x13 // Not found in MPU-9250
00054 #define I2C_MST_STATUS     0x17
00055 #define INT_STATUS         0x19
00056 #define INT_STATUS_1       0x1A // Not found in MPU-9250
00057 #define INT_STATUS_2       0x1B // Not found in MPU-9250
00058 #define INT_STATUS_3       0x1C // Not found in MPU-9250
00059 #define DELAY_TIMEH        0x28 // Not found in MPU-9250
00060 #define DELAY_TIMEL        0x29 // Not found in MPU-9250
00061 #define ACCEL_XOUT_H       0x2D
00062 #define ACCEL_XOUT_L       0x2E
00063 #define ACCEL_YOUT_H       0x2F
00064 #define ACCEL_YOUT_L       0x30
00065 #define ACCEL_ZOUT_H       0x31
00066 #define ACCEL_ZOUT_L       0x32
00067 #define GYRO_XOUT_H        0x33
00068 #define GYRO_XOUT_L        0x34
00069 #define GYRO_YOUT_H        0x35
00070 #define GYRO_YOUT_L        0x36
00071 #define GYRO_ZOUT_H        0x37
00072 #define GYRO_ZOUT_L        0x38
00073 #define TEMP_OUT_H         0x39
00074 #define TEMP_OUT_L         0x3A
00075 #define EXT_SENS_DATA_00   0x3B
00076 #define EXT_SENS_DATA_01   0x3C
00077 #define EXT_SENS_DATA_02   0x3D
00078 #define EXT_SENS_DATA_03   0x3E
00079 #define EXT_SENS_DATA_04   0x3F
00080 #define EXT_SENS_DATA_05   0x40
00081 #define EXT_SENS_DATA_06   0x41
00082 #define EXT_SENS_DATA_07   0x42
00083 #define EXT_SENS_DATA_08   0x43
00084 #define EXT_SENS_DATA_09   0x44
00085 #define EXT_SENS_DATA_10   0x45
00086 #define EXT_SENS_DATA_11   0x46
00087 #define EXT_SENS_DATA_12   0x47
00088 #define EXT_SENS_DATA_13   0x48
00089 #define EXT_SENS_DATA_14   0x49
00090 #define EXT_SENS_DATA_15   0x4A
00091 #define EXT_SENS_DATA_16   0x4B
00092 #define EXT_SENS_DATA_17   0x4C
00093 #define EXT_SENS_DATA_18   0x4D
00094 #define EXT_SENS_DATA_19   0x4E
00095 #define EXT_SENS_DATA_20   0x4F
00096 #define EXT_SENS_DATA_21   0x50
00097 #define EXT_SENS_DATA_22   0x51
00098 #define EXT_SENS_DATA_23   0x52
00099 #define FIFO_EN_1          0x66
00100 #define FIFO_EN_2          0x67 // Not found in MPU-9250
00101 #define FIFO_RST           0x68 // Not found in MPU-9250
00102 #define FIFO_MODE          0x69 // Not found in MPU-9250
00103 #define FIFO_COUNTH        0x70
00104 #define FIFO_COUNTL        0x71
00105 #define FIFO_R_W           0x72
00106 #define DATA_RDY_STATUS    0x74 // Not found in MPU-9250
00107 #define FIFO_CFG           0x76 // Not found in MPU-9250
00108 #define REG_BANK_SEL       0x7F // Not found in MPU-9250
00109 
00110 // USER BANK 1 REGISTER MAP
00111 #define SELF_TEST_X_GYRO            0x02
00112 #define SELF_TEST_Y_GYRO            0x03
00113 #define SELF_TEST_Z_GYRO            0x04
00114 #define SELF_TEST_X_ACCEL           0x0E
00115 #define SELF_TEST_Y_ACCEL           0x0F
00116 #define SELF_TEST_Z_ACCEL           0x10
00117 #define XA_OFFSET_H                 0x14
00118 #define XA_OFFSET_L                 0x15
00119 #define YA_OFFSET_H                 0x17
00120 #define YA_OFFSET_L                 0x18
00121 #define ZA_OFFSET_H                 0x1A
00122 #define ZA_OFFSET_L                 0x1B
00123 #define TIMEBASE_CORRECTION_PLL     0x28
00124 
00125 // USER BANK 2 REGISTER MAP
00126 #define GYRO_SMPLRT_DIV         0x00 // Not found in MPU-9250
00127 #define GYRO_CONFIG_1           0x01 // Not found in MPU-9250
00128 #define GYRO_CONFIG_2           0x02 // Not found in MPU-9250
00129 #define XG_OFFSET_H             0x03  // User-defined trim values for gyroscope
00130 #define XG_OFFSET_L             0x04
00131 #define YG_OFFSET_H             0x05
00132 #define YG_OFFSET_L             0x06
00133 #define ZG_OFFSET_H             0x07
00134 #define ZG_OFFSET_L             0x08
00135 #define ODR_ALIGN_EN            0x09 // Not found in MPU-9250
00136 #define ACCEL_SMPLRT_DIV_1      0x10 // Not found in MPU-9250
00137 #define ACCEL_SMPLRT_DIV_2      0x11 // Not found in MPU-9250
00138 #define ACCEL_INTEL_CTRL        0x12 // Not found in MPU-9250
00139 #define ACCEL_WOM_THR           0x13 // Not found in MPU-9250 (could be WOM_THR)
00140 #define ACCEL_CONFIG            0x14
00141 #define ACCEL_CONFIG_2          0x15 // Not found in MPU-9250 (could be ACCEL_CONFIG2)
00142 #define FSYNC_CONFIG            0x52 // Not found in MPU-9250
00143 #define TEMP_CONFIG             0x53 // Not found in MPU-9250
00144 #define MOD_CTRL_USR            0x54 // Not found in MPU-9250
00145 
00146 // USER BANK 3 REGISTER MAP
00147 #define I2C_MST_ODR_CONFIG      0x00 // Not found in MPU-9250
00148 #define I2C_MST_CTRL            0x01
00149 #define I2C_MST_DELAY_CTRL      0x02
00150 #define I2C_SLV0_ADDR           0x03
00151 #define I2C_SLV0_REG            0x04
00152 #define I2C_SLV0_CTRL           0x05
00153 #define I2C_SLV0_DO             0x06
00154 #define I2C_SLV1_ADDR           0x07
00155 #define I2C_SLV1_REG            0x08
00156 #define I2C_SLV1_CTRL           0x09
00157 #define I2C_SLV1_DO             0x0A
00158 #define I2C_SLV2_ADDR           0x0B
00159 #define I2C_SLV2_REG            0x0C
00160 #define I2C_SLV2_CTRL           0x0D
00161 #define I2C_SLV2_DO             0x0E
00162 #define I2C_SLV3_ADDR           0x0F
00163 #define I2C_SLV3_REG            0x10
00164 #define I2C_SLV3_CTRL           0x11
00165 #define I2C_SLV3_DO             0x12
00166 #define I2C_SLV4_ADDR           0x13
00167 #define I2C_SLV4_REG            0x14
00168 #define I2C_SLV4_CTRL           0x15
00169 #define I2C_SLV4_DO             0x16
00170 #define I2C_SLV4_DI             0x17
00171 
00172 // Using the ICM-20948 breakout board, ADO is set to 1
00173 // Seven-bit device address is 1000100 for ADO = 0 and 1000101 for ADO = 1
00174 #define ADO 0 
00175 #if ADO
00176 #define ICM20948_ADDRESS 0x69<<1  // Device address when ADO = 1
00177 #else
00178 #define ICM20948_ADDRESS 0x68<<1  // Device address when ADO = 0
00179 #define AK09916_ADDRESS  0x0C   // Address of magnetometer
00180 #endif // AD0
00181 
00182 #define READ_FLAGS 0x80
00183 
00184     enum Ascale
00185     {
00186       AFS_2G = 0,
00187       AFS_4G,
00188       AFS_8G,
00189       AFS_16G
00190     };
00191 
00192     enum Gscale {
00193       GFS_250DPS = 0,
00194       GFS_500DPS,
00195       GFS_1000DPS,
00196       GFS_2000DPS
00197     };
00198 
00199     enum Mscale {
00200       MFS_14BITS = 0, // 0.6 mG per LSB
00201       MFS_16BITS      // 0.15 mG per LSB
00202     };
00203 
00204     enum M_MODE {
00205       M_8HZ = 0x02,  // 8 Hz update
00206       M_100HZ = 0x06 // 100 Hz continuous magnetometer
00207     };
00208 
00209     // TODO: Add setter methods for this hard coded stuff
00210     // Specify sensor full scale
00211     uint8_t Gscale = GFS_250DPS;
00212     uint8_t Ascale = AFS_2G;
00213 
00214     // 2 for 8 Hz, 6 for 100 Hz continuous magnetometer data read
00215     uint8_t Mmode = M_100HZ;
00216 
00217     uint8_t writeByteWire(uint8_t, uint8_t, uint8_t);
00218  
00219     uint8_t readByteWire(uint8_t address, uint8_t subAddress);
00220     
00221 
00222 
00223     float pitch, yaw, roll;
00224     float temperature;   // Stores the real internal chip temperature in Celsius
00225     int16_t tempCount;   // Temperature raw count output
00226     uint32_t delt_t = 0; // Used to control display output rate
00227 
00228     uint32_t counts = 0, sumCount = 0; // used to control display output rate
00229     float deltat = 0.0f, sum = 0.0f;  // integration interval for both filter schemes
00230     uint32_t lastUpdate = 0, firstUpdate = 0; // used to calculate integration interval
00231     uint32_t Now = 0;        // used to calculate integration interval
00232 
00233     int16_t gyroCount[3];   // Stores the 16-bit signed gyro sensor output
00234     int16_t magCount[3];    // Stores the 16-bit signed magnetometer sensor output
00235     // Scale resolutions per LSB for the sensors
00236     float aRes, gRes, mRes;
00237     // Variables to hold latest sensor data values
00238     float ax, ay, az, gx, gy, gz, mx, my, mz;
00239     // Factory mag calibration and mag bias
00240     float factoryMagCalibration[3] = {0, 0, 0}, factoryMagBias[3] = {0, 0, 0};
00241     // Bias corrections for gyro, accelerometer, and magnetometer
00242     float gyroBias[3]  = {0, 0, 0},
00243           accelBias[3] = {0, 0, 0},
00244           magBias[3]   = {0, 0, 0},
00245           magScale[3]  = {0, 0, 0};
00246   //  float selfTest[6];
00247     // Stores the 16-bit signed accelerometer sensor output
00248     int16_t accelCount[3];
00249 
00250     // Public method declarations
00251     void getMres();
00252     void getGres();
00253     void getAres();
00254     void readAccelData(int16_t *);
00255     void readGyroData(int16_t *);
00256     void readMagData(int16_t *);
00257     int16_t readTempData();
00258     void updateTime();
00259     void initAK09916();
00260     void initICM20948();
00261     void calibrateICM20948(float * gyroBias, float * accelBias);
00262     void ICM20948SelfTest(float * destination);
00263     void magCalICM20948(float * dest1, float * dest2);
00264     uint8_t writeByte(uint8_t, uint8_t, uint8_t);
00265     uint8_t readByte(uint8_t, uint8_t);
00266     uint8_t readBytes(uint8_t, uint8_t, uint8_t, uint8_t *);
00267     uint8_t readBytesWire(uint8_t, uint8_t, uint8_t, uint8_t *);
00268     bool begin();
00269 
00270 
00271 bool begin(void)
00272 {
00273     i2c.frequency(400000);  // use fast (400 kHz) I2C 
00274     t.start();
00275     return true;
00276 }
00277 
00278 void getMres()
00279 {
00280     mRes = 10.0f * 4912.0f / 32760.0f; // Proper scale to return milliGauss
00281 }
00282 
00283 void getGres()
00284 {
00285   switch (Gscale)
00286   {
00287     // Possible gyro scales (and their register bit settings) are:
00288     // 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS (11).
00289     // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that
00290     // 2-bit value:
00291     case GFS_250DPS:
00292       gRes = 250.0f / 32768.0f;
00293       break;
00294     case GFS_500DPS:
00295       gRes = 500.0f / 32768.0f;
00296       break;
00297     case GFS_1000DPS:
00298       gRes = 1000.0f / 32768.0f;
00299       break;
00300     case GFS_2000DPS:
00301       gRes = 2000.0f / 32768.0f;
00302       break;
00303   }
00304 }
00305 
00306 void getAres()
00307 {
00308   switch (Ascale)
00309   {
00310     // Possible accelerometer scales (and their register bit settings) are:
00311     // 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs  (11).
00312     // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that
00313     // 2-bit value:
00314     case AFS_2G:
00315       aRes = 2.0f / 32768.0f;
00316       break;
00317     case AFS_4G:
00318       aRes = 4.0f / 32768.0f;
00319       break;
00320     case AFS_8G:
00321       aRes = 8.0f / 32768.0f;
00322       break;
00323     case AFS_16G:
00324       aRes = 16.0f / 32768.0f;
00325       break;
00326   }
00327 }
00328 
00329 
00330 void readAccelData(int16_t * destination)
00331 {
00332   uint8_t rawData[6];  // x/y/z accel register data stored here
00333   // Read the six raw data registers into data array
00334  // readBytes(ICM20948_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]);
00335     for(int z=0;z<6;z++)
00336     {
00337         rawData[z]=readByte(ICM20948_ADDRESS, ACCEL_XOUT_H+z);
00338     }
00339   // Turn the MSB and LSB into a signed 16-bit value
00340   destination[0] = (int16_t)(rawData[0] << 8) | rawData[1];
00341   destination[1] = (int16_t)(rawData[2] << 8) | rawData[3];
00342   destination[2] = (int16_t)(rawData[4] << 8) | rawData[5];
00343 }
00344 
00345 
00346 void readGyroData(int16_t * destination)
00347 {
00348   uint8_t rawData[6];  // x/y/z gyro register data stored here
00349   // Read the six raw data registers sequentially into data array
00350   readBytes(ICM20948_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]);
00351 
00352   // Turn the MSB and LSB into a signed 16-bit value
00353   destination[0] = (int16_t)(rawData[0] << 8) | rawData[1];
00354   destination[1] = (int16_t)(rawData[2] << 8) | rawData[3];
00355   destination[2] = (int16_t)(rawData[4] << 8) | rawData[5];
00356 }
00357 
00358 void readMagData(int16_t * destination)
00359 {
00360   // x/y/z gyro register data, ST2 register stored here, must read ST2 at end
00361   // of data acquisition
00362   uint8_t rawData[8];
00363   // thread_sleep_for for magnetometer data ready bit to be set
00364   if (readByte(AK09916_ADDRESS, AK09916_ST1) & 0x01)
00365   {
00366       
00367     // Read the six raw data and ST2 registers sequentially into data array
00368     readBytes(AK09916_ADDRESS, AK09916_XOUT_L, 8, &rawData[0]);
00369     uint8_t c = rawData[7]; // End data read by reading ST2 register
00370     // Check if magnetic sensor overflow set, if not then report data
00371       // Remove once finished
00372     
00373     if (!(c & 0x08))
00374     {
00375       // Turn the MSB and LSB into a signed 16-bit value
00376       destination[0] = ((int16_t)rawData[1] << 8) | rawData[0];
00377       // Data stored as little Endian
00378       destination[1] = ((int16_t)rawData[3] << 8) | rawData[2];
00379       destination[2] = ((int16_t)rawData[5] << 8) | rawData[4];
00380     }
00381   }
00382 }
00383 
00384 int16_t readTempData()
00385 {
00386   uint8_t rawData[2]; // x/y/z gyro register data stored here
00387   // Read the two raw data registers sequentially into data array
00388   readBytes(ICM20948_ADDRESS, TEMP_OUT_H, 2, &rawData[0]);
00389   // Turn the MSB and LSB into a 16-bit value
00390   return ((int16_t)rawData[0] << 8) | rawData[1];
00391 }
00392 
00393 // Calculate the time the last update took for use in the quaternion filters
00394 // TODO: This doesn't really belong in this class.
00395 void updateTime()
00396 {
00397   Now = t.elapsed_time().count();;
00398 
00399   // Set integration time by time elapsed since last filter update
00400   deltat = ((Now - lastUpdate) / 1000000.0f);
00401   lastUpdate = Now;
00402 
00403   sum += deltat; // sum for averaging filter update rate
00404   sumCount++;
00405 }
00406 
00407 void initAK09916()
00408 {
00409     
00410     // Write code to initialise magnetometer
00411     // Bypass I2C master interface and turn on magnetometer
00412  // writeByte(ICM20948_ADDRESS, INT_PIN_CFG, 0x02); //Already set in initICM20948
00413 
00414   // Configure the magnetometer for continuous read and highest resolution.
00415   // Enable continuous mode data acquisition Mmode (bits [3:0]),
00416   // 0010 for 8 Hz and 0110 for 100 Hz sample rates.
00417 
00418   // Set magnetometer data resolution and sample ODR
00419   writeByte(AK09916_ADDRESS, AK09916_CNTL2, 0x08);
00420   thread_sleep_for(10);
00421 }
00422 
00423 void initICM20948()
00424 {
00425     // Get stable time source
00426   // Auto select clock source to be PLL gyroscope reference if ready else
00427   writeByte(ICM20948_ADDRESS, PWR_MGMT_1, 0x01);
00428   thread_sleep_for(200);
00429   // Switch to user bank 2
00430   writeByte(ICM20948_ADDRESS, REG_BANK_SEL, 0x20);
00431   // Configure Gyro and Thermometer
00432   // Disable FSYNC and set gyro bandwidth to 51.2 Hz,
00433   // respectively;
00434   // minimum delay time for this setting is 5.9 ms, which means sensor fusion
00435   // update rates cannot be higher than 1 / 0.0059 = 170 Hz
00436   // DLPF_CFG = bits 2:0 = 011; this limits the sample rate to 1000 Hz for both
00437   // With the ICM20948, it is possible to get gyro sample rates of 32 kHz (!),
00438   // 8 kHz, or 1 kHz
00439   // Set gyroscope full scale range to 250 dps
00440   writeByte(ICM20948_ADDRESS, GYRO_CONFIG_1, 0x19);
00441   writeByte(ICM20948_ADDRESS, TEMP_CONFIG, 0x03);
00442 
00443   // Set sample rate = gyroscope output rate/(1 + GYRO_SMPLRT_DIV)
00444   // Use a 220 Hz rate; a rate consistent with the filter update rate
00445   // determined inset in CONFIG above.
00446   
00447   writeByte(ICM20948_ADDRESS, GYRO_SMPLRT_DIV, 0x04);
00448 
00449   // Set gyroscope full scale range
00450   // Range selects FS_SEL and AFS_SEL are 0 - 3, so 2-bit values are
00451   // left-shifted into positions 4:3
00452 
00453   // Set accelerometer full-scale range configuration
00454   // Get current ACCEL_CONFIG register value
00455   uint8_t c = readByte(ICM20948_ADDRESS, ACCEL_CONFIG);
00456   // c = c & ~0xE0; // Clear self-test bits [7:5]
00457   c = c & ~0x06;  // Clear AFS bits [4:3]
00458   c = c | Ascale << 1; // Set full scale range for the accelerometer
00459   c = c | 0x01; // Set enable accel DLPF for the accelerometer
00460   c = c | 0x18; // and set DLFPFCFG to 50.4 hz
00461   // Write new ACCEL_CONFIG register value
00462   writeByte(ICM20948_ADDRESS, ACCEL_CONFIG, c);
00463 
00464   // Set accelerometer sample rate configuration
00465   // It is possible to get a 4 kHz sample rate from the accelerometer by
00466   // choosing 1 for accel_fchoice_b bit [3]; in this case the bandwidth is
00467   // 1.13 kHz
00468   writeByte(ICM20948_ADDRESS, ACCEL_SMPLRT_DIV_2, 0x04);
00469   // The accelerometer, gyro, and thermometer are set to 1 kHz sample rates,
00470   // but all these rates are further reduced by a factor of 5 to 200 Hz because
00471   // of the GYRO_SMPLRT_DIV setting
00472 
00473   // Switch to user bank 0
00474   writeByte(ICM20948_ADDRESS, REG_BANK_SEL, 0x00);
00475 
00476   // Configure Interrupts and Bypass Enable
00477   // Set interrupt pin active high, push-pull, hold interrupt pin level HIGH
00478   // until interrupt cleared, clear on read of INT_STATUS, and enable
00479   // I2C_BYPASS_EN so additional chips can join the I2C bus and all can be
00480   // controlled by the Arduino as master.
00481   writeByte(ICM20948_ADDRESS, INT_PIN_CFG, 0x22);
00482   // Enable data ready (bit 0) interrupt
00483   writeByte(ICM20948_ADDRESS, INT_ENABLE_1, 0x01);
00484 }
00485 
00486 
00487 // Function which accumulates gyro and accelerometer data after device
00488 // initialization. It calculates the average of the at-rest readings and then
00489 // loads the resulting offsets into accelerometer and gyro bias registers.
00490 void calibrateICM20948(float * gyroBias, float * accelBias)
00491 {
00492   uint8_t data[12]; // data array to hold accelerometer and gyro x, y, z, data
00493   uint16_t ii, packet_count, fifo_count;
00494   int32_t gyro_bias[3]  = {0, 0, 0}, accel_bias[3] = {0, 0, 0}; 
00495   // reset device
00496   // Write a one to bit 7 reset bit; toggle reset device
00497   writeByte(ICM20948_ADDRESS, PWR_MGMT_1, READ_FLAGS);
00498   thread_sleep_for(200);
00499 
00500   // get stable time source; Auto select clock source to be PLL gyroscope
00501   // reference if ready else use the internal oscillator, bits 2:0 = 001
00502   writeByte(ICM20948_ADDRESS, PWR_MGMT_1, 0x01);
00503   thread_sleep_for(200);
00504 
00505   // Configure device for bias calculation
00506   // Disable all interrupts
00507   writeByte(ICM20948_ADDRESS, INT_ENABLE, 0x00);
00508   // Disable FIFO
00509   writeByte(ICM20948_ADDRESS, FIFO_EN_1, 0x00);
00510   writeByte(ICM20948_ADDRESS, FIFO_EN_2, 0x00);
00511   // Turn on internal clock source
00512   writeByte(ICM20948_ADDRESS, PWR_MGMT_1, 0x00);
00513   // Disable I2C master
00514   //writeByte(ICM20948_ADDRESS, I2C_MST_CTRL, 0x00); Already disabled
00515   // Disable FIFO and I2C master modes
00516   writeByte(ICM20948_ADDRESS, USER_CTRL, 0x00);
00517   // Reset FIFO and DMP
00518   writeByte(ICM20948_ADDRESS, USER_CTRL, 0x08);
00519   writeByte(ICM20948_ADDRESS, FIFO_RST, 0x1F);
00520   thread_sleep_for(10);
00521   writeByte(ICM20948_ADDRESS, FIFO_RST, 0x00);
00522   thread_sleep_for(15);
00523 
00524   // Set FIFO mode to snapshot
00525   writeByte(ICM20948_ADDRESS, FIFO_MODE, 0x1F);
00526   // Switch to user bank 2
00527   writeByte(ICM20948_ADDRESS, REG_BANK_SEL, 0x20);
00528   // Configure ICM20948 gyro and accelerometer for bias calculation
00529   // Set low-pass filter to 188 Hz
00530   writeByte(ICM20948_ADDRESS, GYRO_CONFIG_1, 0x01);
00531   // Set sample rate to 1 kHz
00532   writeByte(ICM20948_ADDRESS, GYRO_SMPLRT_DIV, 0x00);
00533   // Set gyro full-scale to 250 degrees per second, maximum sensitivity
00534   writeByte(ICM20948_ADDRESS, GYRO_CONFIG_1, 0x00);
00535   // Set accelerometer full-scale to 2 g, maximum sensitivity
00536   writeByte(ICM20948_ADDRESS, ACCEL_CONFIG, 0x00);
00537 
00538   uint16_t  gyrosensitivity  = 131;   // = 131 LSB/degrees/sec
00539   uint16_t  accelsensitivity = 16384; // = 16384 LSB/g
00540 
00541   // Switch to user bank 0
00542   writeByte(ICM20948_ADDRESS, REG_BANK_SEL, 0x00);
00543   // Configure FIFO to capture accelerometer and gyro data for bias calculation
00544   writeByte(ICM20948_ADDRESS, USER_CTRL, 0x40);  // Enable FIFO
00545   // Enable gyro and accelerometer sensors for FIFO  (max size 512 bytes in
00546   // ICM20948)
00547   writeByte(ICM20948_ADDRESS, FIFO_EN_2, 0x1E);
00548   thread_sleep_for(40);  // accumulate 40 samples in 40 milliseconds = 480 bytes
00549 
00550   // At end of sample accumulation, turn off FIFO sensor read
00551   // Disable gyro and accelerometer sensors for FIFO
00552   writeByte(ICM20948_ADDRESS, FIFO_EN_2, 0x00);
00553   // Read FIFO sample count
00554   readBytes(ICM20948_ADDRESS, FIFO_COUNTH, 2, &data[0]);
00555   fifo_count = ((uint16_t)data[0] << 8) | data[1];
00556   // How many sets of full gyro and accelerometer data for averaging
00557   packet_count = fifo_count/12;
00558 
00559   for (ii = 0; ii < packet_count; ii++)
00560   {
00561     int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0};
00562     // Read data for averaging
00563     readBytes(ICM20948_ADDRESS, FIFO_R_W, 12, &data[0]);
00564     // Form signed 16-bit integer for each sample in FIFO
00565     accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1]  );
00566     accel_temp[1] = (int16_t) (((int16_t)data[2] << 8) | data[3]  );
00567     accel_temp[2] = (int16_t) (((int16_t)data[4] << 8) | data[5]  );
00568     gyro_temp[0]  = (int16_t) (((int16_t)data[6] << 8) | data[7]  );
00569     gyro_temp[1]  = (int16_t) (((int16_t)data[8] << 8) | data[9]  );
00570     gyro_temp[2]  = (int16_t) (((int16_t)data[10] << 8) | data[11]);
00571 
00572     // Sum individual signed 16-bit biases to get accumulated signed 32-bit
00573     // biases.
00574     accel_bias[0] += (int32_t) accel_temp[0];
00575     accel_bias[1] += (int32_t) accel_temp[1];
00576     accel_bias[2] += (int32_t) accel_temp[2];
00577     gyro_bias[0]  += (int32_t) gyro_temp[0];
00578     gyro_bias[1]  += (int32_t) gyro_temp[1];
00579     gyro_bias[2]  += (int32_t) gyro_temp[2];
00580   }
00581   // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
00582   accel_bias[0] /= (int32_t) packet_count;
00583   accel_bias[1] /= (int32_t) packet_count;
00584   accel_bias[2] /= (int32_t) packet_count;
00585   gyro_bias[0]  /= (int32_t) packet_count;
00586   gyro_bias[1]  /= (int32_t) packet_count;
00587   gyro_bias[2]  /= (int32_t) packet_count;
00588 
00589   // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
00590   if (accel_bias[2] > 0L)
00591   {
00592     accel_bias[2] -= (int32_t) accelsensitivity;
00593   }
00594   else
00595   {
00596     accel_bias[2] += (int32_t) accelsensitivity;
00597   }
00598 
00599   // Construct the gyro biases for push to the hardware gyro bias registers,
00600   // which are reset to zero upon device startup.
00601   // Divide by 4 to get 32.9 LSB per deg/s to conform to expected bias input
00602   // format.
00603   data[0] = (-gyro_bias[0]/4  >> 8) & 0xFF;
00604   // Biases are additive, so change sign on calculated average gyro biases
00605   data[1] = (-gyro_bias[0]/4)       & 0xFF;
00606   data[2] = (-gyro_bias[1]/4  >> 8) & 0xFF;
00607   data[3] = (-gyro_bias[1]/4)       & 0xFF;
00608   data[4] = (-gyro_bias[2]/4  >> 8) & 0xFF;
00609   data[5] = (-gyro_bias[2]/4)       & 0xFF;
00610   
00611   // Switch to user bank 2
00612   writeByte(ICM20948_ADDRESS, REG_BANK_SEL, 0x20);
00613 
00614   // Push gyro biases to hardware registers
00615   writeByte(ICM20948_ADDRESS, XG_OFFSET_H, data[0]);
00616   writeByte(ICM20948_ADDRESS, XG_OFFSET_L, data[1]);
00617   writeByte(ICM20948_ADDRESS, YG_OFFSET_H, data[2]);
00618   writeByte(ICM20948_ADDRESS, YG_OFFSET_L, data[3]);
00619   writeByte(ICM20948_ADDRESS, ZG_OFFSET_H, data[4]);
00620   writeByte(ICM20948_ADDRESS, ZG_OFFSET_L, data[5]);
00621 
00622   // Output scaled gyro biases for display in the main program
00623   gyroBias[0] = (float) gyro_bias[0]/(float) gyrosensitivity;
00624   gyroBias[1] = (float) gyro_bias[1]/(float) gyrosensitivity;
00625   gyroBias[2] = (float) gyro_bias[2]/(float) gyrosensitivity;
00626 
00627   // Construct the accelerometer biases for push to the hardware accelerometer
00628   // bias registers. These registers contain factory trim values which must be
00629   // added to the calculated accelerometer biases; on boot up these registers
00630   // will hold non-zero values. In addition, bit 0 of the lower byte must be
00631   // preserved since it is used for temperature compensation calculations.
00632   // Accelerometer bias registers expect bias input as 2048 LSB per g, so that
00633   // the accelerometer biases calculated above must be divided by 8.
00634   
00635   // Switch to user bank 1
00636   writeByte(ICM20948_ADDRESS, REG_BANK_SEL, 0x10);
00637   // A place to hold the factory accelerometer trim biases
00638   int32_t accel_bias_reg[3] = {0, 0, 0};
00639   // Read factory accelerometer trim values
00640   readBytes(ICM20948_ADDRESS, XA_OFFSET_H, 2, &data[0]);
00641   accel_bias_reg[0] = (int32_t) (((int16_t)data[0] << 8) | data[1]);
00642   readBytes(ICM20948_ADDRESS, YA_OFFSET_H, 2, &data[0]);
00643   accel_bias_reg[1] = (int32_t) (((int16_t)data[0] << 8) | data[1]);
00644   readBytes(ICM20948_ADDRESS, ZA_OFFSET_H, 2, &data[0]);
00645   accel_bias_reg[2] = (int32_t) (((int16_t)data[0] << 8) | data[1]);
00646 
00647   // Define mask for temperature compensation bit 0 of lower byte of
00648   // accelerometer bias registers
00649   uint32_t mask = 1uL;
00650   // Define array to hold mask bit for each accelerometer bias axis
00651   uint8_t mask_bit[3] = {0, 0, 0};
00652 
00653   for (ii = 0; ii < 3; ii++)
00654   {
00655     // If temperature compensation bit is set, record that fact in mask_bit
00656     if ((accel_bias_reg[ii] & mask))
00657     {
00658       mask_bit[ii] = 0x01;
00659     }
00660   }
00661 
00662   // Construct total accelerometer bias, including calculated average
00663   // accelerometer bias from above
00664   // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g
00665   // (16 g full scale)
00666   accel_bias_reg[0] -= (accel_bias[0]/8);
00667   accel_bias_reg[1] -= (accel_bias[1]/8);
00668   accel_bias_reg[2] -= (accel_bias[2]/8);
00669 
00670   data[0] = (accel_bias_reg[0] >> 8) & 0xFF;
00671   data[1] = (accel_bias_reg[0]) & 0xFF;
00672   // preserve temperature compensation bit when writing back to accelerometer
00673   // bias registers
00674   data[1] = data[1] | mask_bit[0];
00675   data[2] = (accel_bias_reg[1] >> 8) & 0xFF;
00676   data[3] = (accel_bias_reg[1]) & 0xFF;
00677   // Preserve temperature compensation bit when writing back to accelerometer
00678   // bias registers
00679   data[3] = data[3] | mask_bit[1];
00680   data[4] = (accel_bias_reg[2] >> 8) & 0xFF;
00681   data[5] = (accel_bias_reg[2]) & 0xFF;
00682   // Preserve temperature compensation bit when writing back to accelerometer
00683   // bias registers
00684   data[5] = data[5] | mask_bit[2];
00685 
00686   // Apparently this is not working for the acceleration biases in the ICM-20948
00687   // Are we handling the temperature correction bit properly?
00688   // Push accelerometer biases to hardware registers
00689   writeByte(ICM20948_ADDRESS, XA_OFFSET_H, data[0]);
00690   writeByte(ICM20948_ADDRESS, XA_OFFSET_L, data[1]);
00691   writeByte(ICM20948_ADDRESS, YA_OFFSET_H, data[2]);
00692   writeByte(ICM20948_ADDRESS, YA_OFFSET_L, data[3]);
00693   writeByte(ICM20948_ADDRESS, ZA_OFFSET_H, data[4]);
00694   writeByte(ICM20948_ADDRESS, ZA_OFFSET_L, data[5]);
00695 
00696   // Output scaled accelerometer biases for display in the main program
00697   accelBias[0] = (float)accel_bias[0]/(float)accelsensitivity;
00698   accelBias[1] = (float)accel_bias[1]/(float)accelsensitivity;
00699   accelBias[2] = (float)accel_bias[2]/(float)accelsensitivity;
00700   // Switch to user bank 0
00701   writeByte(ICM20948_ADDRESS, REG_BANK_SEL, 0x00);
00702 }
00703 
00704 
00705 // Accelerometer and gyroscope self test; check calibration wrt factory settings
00706 // Should return percent deviation from factory trim values, +/- 14 or less
00707 // deviation is a pass.
00708 void ICM20948SelfTest(float * destination)
00709 {
00710   uint8_t rawData[6] = {0, 0, 0, 0, 0, 0};
00711   uint8_t selfTest[6];
00712   int32_t gAvg[3] = {0}, aAvg[3] = {0}, aSTAvg[3] = {0}, gSTAvg[3] = {0};
00713   float factoryTrim[6];
00714   uint8_t FS = 0;
00715   // Get stable time source
00716   // Auto select clock source to be PLL gyroscope reference if ready else
00717   writeByte(ICM20948_ADDRESS, PWR_MGMT_1, 0x01);
00718   thread_sleep_for(200);
00719   // Switch to user bank 2
00720   writeByte(ICM20948_ADDRESS, REG_BANK_SEL, 0x20);
00721   // Set gyro sample rate to 1 kHz
00722   writeByte(ICM20948_ADDRESS, GYRO_SMPLRT_DIV, 0x00);
00723   // Set gyro sample rate to 1 kHz, DLPF to 119.5 Hz and FSR to 250 dps
00724   writeByte(ICM20948_ADDRESS, GYRO_CONFIG_1, 0x11);
00725   // Set accelerometer rate to 1 kHz and bandwidth to 111.4 Hz
00726   // Set full scale range for the accelerometer to 2 g
00727   writeByte(ICM20948_ADDRESS, ACCEL_CONFIG, 0x11);
00728   // Switch to user bank 0
00729   writeByte(ICM20948_ADDRESS, REG_BANK_SEL, 0x00);
00730   // Get average current values of gyro and acclerometer
00731   for (int ii = 0; ii < 200; ii++)
00732   {
00733 
00734     // Read the six raw data registers into data array
00735     for(int z=0;z<6;z++)
00736     {
00737         rawData[z]=readByte(ICM20948_ADDRESS, ACCEL_XOUT_H+z);
00738     }
00739     // Turn the MSB and LSB into a signed 16-bit value
00740     aAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ;
00741     aAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
00742     aAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
00743 
00744     // Read the six raw data registers sequentially into data array
00745     for(int z=0;z<6;z++)
00746     {
00747         rawData[z]=readByte(ICM20948_ADDRESS, GYRO_XOUT_H+z);
00748     }
00749     // Turn the MSB and LSB into a signed 16-bit value
00750     gAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ;
00751     gAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
00752     gAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
00753   }
00754 
00755   // Get average of 200 values and store as average current readings
00756   for (int ii =0; ii < 3; ii++)
00757   {
00758     aAvg[ii] /= 200;
00759     gAvg[ii] /= 200;
00760   }
00761   // Switch to user bank 2
00762   writeByte(ICM20948_ADDRESS, REG_BANK_SEL, 0x20);
00763   // Configure the accelerometer for self-test
00764   // Enable self test on all three axes and set accelerometer range to +/- 2 g
00765   writeByte(ICM20948_ADDRESS, ACCEL_CONFIG_2, 0x1C);
00766   // Enable self test on all three axes and set gyro range to +/- 250 degrees/s
00767   writeByte(ICM20948_ADDRESS, GYRO_CONFIG_2,  0x38);
00768   thread_sleep_for(25);  // Delay a while to let the device stabilize
00769   // Switch to user bank 0
00770   writeByte(ICM20948_ADDRESS, REG_BANK_SEL, 0x00);
00771   // Get average self-test values of gyro and acclerometer
00772   for (int ii = 0; ii < 200; ii++)
00773   {
00774     // Read the six raw data registers into data array
00775    // readBytes(ICM20948_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]);
00776    for(int z=0;z<6;z++)
00777     {
00778         rawData[z]=readByte(ICM20948_ADDRESS, ACCEL_XOUT_H+z);
00779     }
00780     // Turn the MSB and LSB into a signed 16-bit value
00781     aSTAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ;
00782     aSTAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
00783     aSTAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
00784 
00785     // Read the six raw data registers sequentially into data array
00786     //readBytes(ICM20948_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]);
00787     for(int z=0;z<6;z++)
00788     {
00789         rawData[z]=readByte(ICM20948_ADDRESS, GYRO_XOUT_H+z);
00790     }
00791     // Turn the MSB and LSB into a signed 16-bit value
00792     gSTAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ;
00793     gSTAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
00794     gSTAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
00795   }
00796 
00797   // Get average of 200 values and store as average self-test readings
00798   for (int ii =0; ii < 3; ii++)
00799   {
00800     aSTAvg[ii] /= 200;
00801     gSTAvg[ii] /= 200;
00802   }
00803   
00804   // Switch to user bank 2
00805   writeByte(ICM20948_ADDRESS, REG_BANK_SEL, 0x20);
00806   // Configure the gyro and accelerometer for normal operation
00807   writeByte(ICM20948_ADDRESS, ACCEL_CONFIG_2, 0x00);
00808   writeByte(ICM20948_ADDRESS, GYRO_CONFIG_2,  0x00);
00809   thread_sleep_for(25);  // Delay a while to let the device stabilize
00810   // Switch to user bank 1
00811   writeByte(ICM20948_ADDRESS, REG_BANK_SEL, 0x10);
00812   // Retrieve accelerometer and gyro factory Self-Test Code from USR_Reg
00813   // X-axis accel self-test results
00814   selfTest[0] = readByte(ICM20948_ADDRESS, SELF_TEST_X_ACCEL);
00815   // Y-axis accel self-test results
00816   selfTest[1] = readByte(ICM20948_ADDRESS, SELF_TEST_Y_ACCEL);
00817   // Z-axis accel self-test results
00818   selfTest[2] = readByte(ICM20948_ADDRESS, SELF_TEST_Z_ACCEL);
00819   // X-axis gyro self-test results
00820   selfTest[3] = readByte(ICM20948_ADDRESS, SELF_TEST_X_GYRO);
00821   // Y-axis gyro self-test results
00822   selfTest[4] = readByte(ICM20948_ADDRESS, SELF_TEST_Y_GYRO);
00823   // Z-axis gyro self-test results
00824   selfTest[5] = readByte(ICM20948_ADDRESS, SELF_TEST_Z_GYRO);
00825   // Switch to user bank 0
00826   writeByte(ICM20948_ADDRESS, REG_BANK_SEL, 0x00);
00827   // Retrieve factory self-test value from self-test code reads
00828   // FT[Xa] factory trim calculation
00829   factoryTrim[0] = (float)(2620/1<<FS)*(pow(1.01 ,((float)selfTest[0] - 1.0) ));
00830   // FT[Ya] factory trim calculation
00831   factoryTrim[1] = (float)(2620/1<<FS)*(pow(1.01 ,((float)selfTest[1] - 1.0) ));
00832   // FT[Za] factory trim calculation
00833   factoryTrim[2] = (float)(2620/1<<FS)*(pow(1.01 ,((float)selfTest[2] - 1.0) ));
00834   // FT[Xg] factory trim calculation
00835   factoryTrim[3] = (float)(2620/1<<FS)*(pow(1.01 ,((float)selfTest[3] - 1.0) ));
00836   // FT[Yg] factory trim calculation
00837   factoryTrim[4] = (float)(2620/1<<FS)*(pow(1.01 ,((float)selfTest[4] - 1.0) ));
00838   // FT[Zg] factory trim calculation
00839   factoryTrim[5] = (float)(2620/1<<FS)*(pow(1.01 ,((float)selfTest[5] - 1.0) ));
00840 
00841   // Report results as a ratio of (STR - FT)/FT; the change from Factory Trim
00842   // of the Self-Test Response
00843   // To get percent, must multiply by 100
00844   for (int i = 0; i < 3; i++)
00845   {
00846     // Report percent differences
00847     destination[i] = 100.0 * ((float)(aSTAvg[i] - aAvg[i]))/factoryTrim[i]- 100./*selfTest[i]*/;
00848     // Report percent differences
00849     destination[i+3] =100.0*((float)(gSTAvg[i] - gAvg[i]))/factoryTrim[i+3]- 100./*selfTest[i+3]*/;
00850   }
00851 }
00852 
00853 // Function which accumulates magnetometer data after device initialization.
00854 // It calculates the bias and scale in the x, y, and z axes.
00855 void magCalICM20948(float * bias_dest, float * scale_dest)
00856 {
00857   uint16_t ii = 0, sample_count = 0;
00858   int32_t mag_bias[3]  = {0, 0, 0},
00859           mag_scale[3] = {0, 0, 0};
00860   int32_t mag_max[3]  = {0x8000, 0x8000, 0x8000},
00861           mag_min[3]  = {0x7FFF, 0x7FFF, 0x7FFF},
00862           mag_temp[3] = {0, 0, 0};
00863 
00864   // Make sure resolution has been calculated
00865   getMres();
00866   thread_sleep_for(4000);
00867 
00868   // shoot for ~fifteen seconds of mag data
00869   // at 8 Hz ODR, new mag data is available every 125 ms
00870   if (Mmode == M_8HZ)
00871   {
00872     sample_count = 128;
00873   }
00874   // at 100 Hz ODR, new mag data is available every 10 ms
00875   if (Mmode == M_100HZ)
00876   {
00877     sample_count = 1500;
00878   }
00879 
00880   for (ii = 0; ii < sample_count; ii++)
00881   {
00882     readMagData((int16_t *) mag_temp);  // Read the mag data
00883 
00884     for (int jj = 0; jj < 3; jj++)
00885     {
00886       if (mag_temp[jj] > mag_max[jj])
00887       {
00888         mag_max[jj] = mag_temp[jj];
00889       }
00890       if (mag_temp[jj] < mag_min[jj])
00891       {
00892         mag_min[jj] = mag_temp[jj];
00893       }
00894     }
00895 
00896     if (Mmode == M_8HZ)
00897     {
00898       thread_sleep_for(135); // At 8 Hz ODR, new mag data is available every 125 ms
00899     }
00900     if (Mmode == M_100HZ)
00901     {
00902       thread_sleep_for(12);  // At 100 Hz ODR, new mag data is available every 10 ms
00903     }
00904   }
00905 
00906   // pc.println("mag x min/max:"); pc.println(mag_max[0]); pc.println(mag_min[0]);
00907   // pc.println("mag y min/max:"); pc.println(mag_max[1]); pc.println(mag_min[1]);
00908   // pc.println("mag z min/max:"); pc.println(mag_max[2]); pc.println(mag_min[2]);
00909 
00910   // Get hard iron correction
00911   // Get 'average' x mag bias in counts
00912   mag_bias[0]  = (mag_max[0] + mag_min[0]) / 2;
00913   // Get 'average' y mag bias in counts
00914   mag_bias[1]  = (mag_max[1] + mag_min[1]) / 2;
00915   // Get 'average' z mag bias in counts
00916   mag_bias[2]  = (mag_max[2] + mag_min[2]) / 2;
00917 
00918   // Save mag biases in G for main program
00919   bias_dest[0] = (float)mag_bias[0] * mRes;// * factoryMagCalibration[0];
00920   bias_dest[1] = (float)mag_bias[1] * mRes;// * factoryMagCalibration[1];
00921   bias_dest[2] = (float)mag_bias[2] * mRes;// * factoryMagCalibration[2];
00922 
00923   // Get soft iron correction estimate
00924   // Get average x axis max chord length in counts
00925   mag_scale[0]  = (mag_max[0] - mag_min[0]) / 2;
00926   // Get average y axis max chord length in counts
00927   mag_scale[1]  = (mag_max[1] - mag_min[1]) / 2;
00928   // Get average z axis max chord length in counts
00929   mag_scale[2]  = (mag_max[2] - mag_min[2]) / 2;
00930 
00931   float avg_rad = mag_scale[0] + mag_scale[1] + mag_scale[2];
00932   avg_rad /= 3.0;
00933 
00934   scale_dest[0] = avg_rad / ((float)mag_scale[0]);
00935   scale_dest[1] = avg_rad / ((float)mag_scale[1]);
00936   scale_dest[2] = avg_rad / ((float)mag_scale[2]);
00937 }
00938 
00939 // Wire.h read and write protocols
00940 uint8_t writeByte(uint8_t deviceAddress, uint8_t registerAddress,uint8_t data)
00941 {
00942   //writeByteWire(deviceAddress,registerAddress, data);
00943     char tmp[2];
00944     tmp[0]=registerAddress;
00945     tmp[1]=data;
00946     i2c.write(deviceAddress,  tmp, 2, 0); // no stop
00947     return NULL;
00948 }
00949 
00950 uint8_t writeByteOne(uint8_t deviceAddress, uint8_t registerAddress)
00951 {
00952   char tmp[2];
00953     tmp[0]=registerAddress;
00954     i2c.write(deviceAddress,  tmp, 1, 1);
00955   return NULL;
00956 }
00957 
00958 /*
00959 uint8_t writeByteWire(uint8_t deviceAddress, uint8_t registerAddress,
00960                             uint8_t data)
00961 { // i2c.write(address, data_write, 1, 1); // no stop
00962     char tmp[2];
00963     tmp[0]=registerAddress;
00964     i2c.write(deviceAddress,  tmp, 1, 1); // no stop
00965     tmp[0]=data;
00966     i2c.write(deviceAddress,  tmp, 1, 0); // stop
00967   // TODO: Fix this to return something meaningful
00968   return NULL;
00969 }
00970 */
00971 // Read a byte from given register on device. Calls necessary SPI or I2C
00972 // implementation. This was configured in the constructor.
00973 uint8_t readByte(uint8_t deviceAddress, uint8_t registerAddress)
00974 {
00975  char tmp[1];
00976     tmp[0]=registerAddress;
00977   i2c.write(deviceAddress,tmp, 1, 1); // no stop
00978   //tmp[0]=data;
00979   i2c.read(deviceAddress, tmp, 1, 0);//stop
00980   // Return data read from slave register
00981   return (uint8_t) tmp[0];
00982 }
00983 /*
00984 
00985 uint8_t readByteWire(uint8_t deviceAddress, uint8_t registerAddress)
00986 {
00987   uint8_t data; // `data` will store the register data
00988 // i2c.write(address, data_write, 1, 1); // no stop
00989 //    i2c.read(address, data, count, 0); 
00990   // Initialize the Tx buffer
00991   char tmp[2];
00992     tmp[0]=registerAddress;
00993   i2c.write(deviceAddress,tmp, 1, 0); // no stop
00994   //tmp[0]=data;
00995   i2c.read(deviceAddress, tmp, 1, 0);//stop
00996   // Return data read from slave register
00997   return tmp[0];
00998 }
00999 
01000 */
01001 // Read 1 or more bytes from given register and device using I2C
01002 uint8_t readBytesWire(uint8_t deviceAddress, uint8_t registerAddress,
01003                         uint8_t count, uint8_t * dest)
01004 {
01005     char tmp[2];
01006     tmp[0]=registerAddress;
01007   i2c.write(deviceAddress, tmp, 1, 1); // no stop
01008   i2c.read(deviceAddress,(char *) dest, count, 0);//stop
01009   // Initialize the Tx buffer
01010 /*  Wire.beginTransmission(deviceAddress);
01011   // Put slave register address in Tx buffer
01012   Wire.write(registerAddress);
01013   // Send the Tx buffer, but send a restart to keep connection alive
01014   Wire.endTransmission(false);
01015 
01016   uint8_t i = 0;
01017   // Read bytes from slave register address
01018   Wire.requestFrom(deviceAddress, count);
01019   while (Wire.available())
01020   {
01021     // Put read results in the Rx buffer
01022     dest[i++] = Wire.read();
01023   }
01024 */
01025   return count; // Return number of bytes written
01026 }
01027 
01028 uint8_t readBytes(uint8_t deviceAddress, uint8_t registerAddress,
01029                         uint8_t count, uint8_t * dest)
01030 {
01031  
01032     return readBytesWire(deviceAddress, registerAddress, count, dest);
01033   
01034 }