diff --git a/src/ICM_20948.cpp b/src/ICM_20948.cpp
index 10220d8..3e6afac 100644
--- a/src/ICM_20948.cpp
+++ b/src/ICM_20948.cpp
@@ -18,11 +18,6 @@ ICM_20948_AGMT_t ICM_20948::getAGMT(void)
 {
     status = ICM_20948_get_agmt(&_device, &agmt);
 
-    if (_has_magnetometer)
-    {
-        getMagnetometerData(&agmt);
-    }
-
     return agmt;
 }
 
@@ -527,7 +522,6 @@ ICM_20948_Status_e ICM_20948::intEnableRawDataReady(bool enable)
     }
     if (en.RAW_DATA_0_RDY_EN != enable)
     {
-        Serial.println("mismatch error");
         status = ICM_20948_Stat_Err;
         return status;
     }
@@ -589,6 +583,12 @@ ICM_20948_Status_e ICM_20948::i2cMasterEnable(bool enable)
     return status;
 }
 
+ICM_20948_Status_e ICM_20948::i2cMasterReset()
+{
+    status = ICM_20948_i2c_master_reset(&_device);
+    return status;
+}
+
 ICM_20948_Status_e ICM_20948::i2cMasterConfigureSlave(uint8_t slave, uint8_t addr, uint8_t reg, uint8_t len, bool Rw, bool enable, bool data_only, bool grp, bool swap)
 {
     status = ICM_20948_i2c_master_configure_slave(&_device, slave, addr, reg, len, Rw, enable, data_only, grp, swap);
@@ -600,6 +600,7 @@ ICM_20948_Status_e ICM_20948::i2cMasterSLV4Transaction(uint8_t addr, uint8_t reg
     status = ICM_20948_i2c_master_slv4_txn(&_device, addr, reg, data, len, Rw, send_reg_addr);
     return status;
 }
+
 ICM_20948_Status_e ICM_20948::i2cMasterSingleW(uint8_t addr, uint8_t reg, uint8_t data)
 {
     status = ICM_20948_i2c_master_single_w(&_device, addr, reg, &data);
@@ -684,49 +685,39 @@ ICM_20948_Status_e ICM_20948::startupDefault(void)
         status = retval;
         return status;
     }
-
-    _has_magnetometer = true;
     retval = startupMagnetometer();
-    if ((retval != ICM_20948_Stat_Ok) && (retval != ICM_20948_Stat_NotImpl))
+    if (retval != ICM_20948_Stat_Ok)
     {
         status = retval;
         return status;
     }
-    if (retval == ICM_20948_Stat_NotImpl)
-    {
-        // This is a temporary fix.
-        // Ultimately we *should* be able to configure the I2C master to handle the
-        // magnetometer no matter what interface (SPI / I2C) we are using.
-
-        // Should try testing I2C master functionality on a bare ICM chip w/o TXS0108 level shifter...
-
-        _has_magnetometer = false;
-        retval = ICM_20948_Stat_Ok; // reset the retval because we handled it in this cases
-    }
 
-    status = retval;
     return status;
 }
 
-ICM_20948_Status_e ICM_20948::startupMagnetometer(void)
+// direct read/write
+ICM_20948_Status_e ICM_20948::read(uint8_t reg, uint8_t *pdata, uint32_t len)
 {
-    return ICM_20948_Stat_NotImpl; // By default we assume that we cannot access the magnetometer
+    status = ICM_20948_execute_r(&_device, reg, pdata, len);
+    return (status);
 }
 
-ICM_20948_Status_e ICM_20948::getMagnetometerData(ICM_20948_AGMT_t *pagmt)
+ICM_20948_Status_e ICM_20948::write(uint8_t reg, uint8_t *pdata, uint32_t len)
 {
-    return ICM_20948_Stat_NotImpl; // By default we assume that we cannot access the magnetometer
+    status = ICM_20948_execute_w(&_device, reg, pdata, len);
+    return (status);
 }
 
-// direct read/write
-ICM_20948_Status_e ICM_20948::read(uint8_t reg, uint8_t *pdata, uint32_t len)
+uint8_t ICM_20948::readMag(AK09916_Reg_Addr_e reg)
 {
-    status = ICM_20948_execute_r(&_device, reg, pdata, len);
+    uint8_t data = i2cMasterSingleR(MAG_AK09916_I2C_ADDR, reg);
+    return data;
 }
 
-ICM_20948_Status_e ICM_20948::write(uint8_t reg, uint8_t *pdata, uint32_t len)
+ICM_20948_Status_e ICM_20948::writeMag(AK09916_Reg_Addr_e reg, uint8_t *pdata)
 {
-    status = ICM_20948_execute_w(&_device, reg, pdata, len);
+    status = i2cMasterSingleW(MAG_AK09916_I2C_ADDR, reg, pdata);
+    return status;
 }
 
 // I2C
@@ -778,103 +769,78 @@ ICM_20948_Status_e ICM_20948_I2C::begin(TwoWire &wirePort, bool ad0val, uint8_t
     return status;
 }
 
-ICM_20948_Status_e ICM_20948_I2C::startupMagnetometer(void)
+ICM_20948_Status_e ICM_20948::startupMagnetometer(void)
 {
-    // If using the magnetometer through passthrough:
-    i2cMasterPassthrough(true); // Set passthrough mode to try to access the magnetometer (by default I2C master is disabled but you still have to enable the passthrough)
+    ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
 
-    // Try to set up magnetometer
-    AK09916_CNTL2_Reg_t reg;
-    reg.MODE = AK09916_mode_cont_100hz;
+    i2cMasterPassthrough(false); //Do not connect the SDA/SCL pins to AUX_DA/AUX_CL
+    i2cMasterEnable(true);
 
-    ICM_20948_Status_e retval = writeMag(AK09916_REG_CNTL2, (uint8_t *)&reg, sizeof(AK09916_CNTL2_Reg_t));
-    status = retval;
-    if (status == ICM_20948_Stat_Ok)
+    //After a ICM reset the Mag sensor may stop responding over the I2C master
+    //Reset the Master I2C until it responds
+    uint8_t tries = 0;
+    uint8_t maxTries = 5;
+    while (tries < maxTries)
     {
-        _has_magnetometer = true;
-    }
-    return status;
-}
-
-ICM_20948_Status_e ICM_20948_I2C::magWhoIAm(void)
-{
-    ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
+        //See if we can read the WhoIAm register correctly
+        retval = magWhoIAm();
+        if (retval == ICM_20948_Stat_Ok)
+            break; //WIA matched!
 
-    const uint8_t len = 2;
-    uint8_t whoiam[len];
-    retval = readMag(AK09916_REG_WIA1, whoiam, len);
-    status = retval;
-    if (retval != ICM_20948_Stat_Ok)
-    {
-        return retval;
+        i2cMasterReset(); //Otherwise, reset the master I2C and try again
+        tries++;
     }
 
-    if ((whoiam[0] == (MAG_AK09916_WHO_AM_I >> 8)) && (whoiam[1] == (MAG_AK09916_WHO_AM_I & 0xFF)))
+    if (tries == maxTries)
     {
-        retval = ICM_20948_Stat_Ok;
-        status = retval;
+        status = ICM_20948_Stat_WrongID;
         return status;
     }
-    retval = ICM_20948_Stat_WrongID;
-    status = retval;
-    return status;
-}
 
-bool ICM_20948_I2C::magIsConnected(void)
-{
-    if (magWhoIAm() != ICM_20948_Stat_Ok)
+    //Serial.printf("Mag connected tries: %d\n", tries);
+
+    //Set up magnetometer
+    AK09916_CNTL2_Reg_t reg;
+    reg.MODE = AK09916_mode_cont_100hz;
+    retval = writeMag(AK09916_REG_CNTL2, (uint8_t *)&reg);
+    if (retval != ICM_20948_Stat_Ok)
     {
-        return false;
+        status = retval;
+        return status;
     }
-    return true;
-}
-
-ICM_20948_Status_e ICM_20948_I2C::getMagnetometerData(ICM_20948_AGMT_t *pagmt)
-{
-
-    const uint8_t reqd_len = 9; // you must read all the way through the status2 register to re-enable the next measurement
-    uint8_t buff[reqd_len];
 
-    status = readMag(AK09916_REG_ST1, buff, reqd_len);
-    if (status != ICM_20948_Stat_Ok)
+    retval = i2cMasterConfigureSlave(0, MAG_AK09916_I2C_ADDR, AK09916_REG_ST1, 9, true, true, false, false, false);
+    if (retval != ICM_20948_Stat_Ok)
     {
+        status = retval;
         return status;
     }
 
-    pagmt->mag.axes.x = ((buff[2] << 8) | (buff[1] & 0xFF));
-    pagmt->mag.axes.y = ((buff[4] << 8) | (buff[3] & 0xFF));
-    pagmt->mag.axes.z = ((buff[6] << 8) | (buff[5] & 0xFF));
-
     return status;
 }
 
-ICM_20948_Status_e ICM_20948_I2C::readMag(uint8_t reg, uint8_t *pdata, uint8_t len)
+ICM_20948_Status_e ICM_20948::magWhoIAm(void)
 {
-    _i2c->beginTransmission(MAG_AK09916_I2C_ADDR);
-    _i2c->write(reg);
-    _i2c->endTransmission(false);
+    ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
 
-    uint8_t num_received = _i2c->requestFrom((uint8_t)MAG_AK09916_I2C_ADDR, (uint8_t)len);
-    if (num_received != len)
+    uint8_t whoiam1, whoiam2;
+    whoiam1 = readMag(AK09916_REG_WIA1);
+    whoiam2 = readMag(AK09916_REG_WIA2);
+    status = retval;
+    if (retval != ICM_20948_Stat_Ok)
     {
-        return ICM_20948_Stat_NoData;
+        return retval;
     }
 
-    for (uint8_t indi = 0; indi < len; indi++)
+    if ((whoiam1 == (MAG_AK09916_WHO_AM_I >> 8)) && (whoiam2 == (MAG_AK09916_WHO_AM_I & 0xFF)))
     {
-        *(pdata + indi) = _i2c->read();
+        retval = ICM_20948_Stat_Ok;
+        status = retval;
+        return status;
     }
-
-    return ICM_20948_Stat_Ok;
-}
-
-ICM_20948_Status_e ICM_20948_I2C::writeMag(uint8_t reg, uint8_t *pdata, uint8_t len)
-{
-    _i2c->beginTransmission(MAG_AK09916_I2C_ADDR);
-    _i2c->write(reg);
-    _i2c->write(pdata, len);
-    _i2c->endTransmission();
-    return ICM_20948_Stat_Ok; // todo: check return of 'endTransmission' to verify all bytes sent w/ ACK
+    retval = ICM_20948_Stat_WrongID;
+    status = retval;
+    return status;
 }
 
 // SPI
@@ -923,8 +889,6 @@ ICM_20948_Status_e ICM_20948_SPI::begin(uint8_t csPin, SPIClass &spiPort, uint32
         return status;
     }
 
-    // todo: disable I2C interface to prevent accidents
-
     return ICM_20948_Stat_Ok;
 }
 
diff --git a/src/ICM_20948.h b/src/ICM_20948.h
index 29bcb3a..df5357e 100644
--- a/src/ICM_20948.h
+++ b/src/ICM_20948.h
@@ -8,6 +8,7 @@ A C++ interface to the ICM-20948
 #define _ICM_20948_H_
 
 #include "util/ICM_20948_C.h" // The C backbone
+#include "util/AK09916_REGISTERS.h"
 
 #include "Arduino.h" // Arduino support
 #include "Wire.h"
@@ -21,7 +22,6 @@ class ICM_20948
 private:
 protected:
     ICM_20948_Device_t _device;
-    bool _has_magnetometer;
 
     float getTempC(int16_t val);
     float getGyrDPS(int16_t axis_val);
@@ -92,20 +92,28 @@ class ICM_20948
     // Interface Options
     ICM_20948_Status_e i2cMasterPassthrough(bool passthrough = true);
     ICM_20948_Status_e i2cMasterEnable(bool enable = true);
-    ICM_20948_Status_e i2cMasterConfigureSlave(uint8_t slave, uint8_t addr, uint8_t reg, uint8_t len, bool Rw = true, bool enable = true, bool data_only = false, bool grp = false, bool swap = false);
+    ICM_20948_Status_e i2cMasterReset();
 
+    //Used for configuring slaves 0-3
+    ICM_20948_Status_e i2cMasterConfigureSlave(uint8_t slave, uint8_t addr, uint8_t reg, uint8_t len, bool Rw = true, bool enable = true, bool data_only = false, bool grp = false, bool swap = false);
     ICM_20948_Status_e i2cMasterSLV4Transaction(uint8_t addr, uint8_t reg, uint8_t *data, uint8_t len, bool Rw, bool send_reg_addr = true);
+
+    //Used for configuring the Magnetometer
     ICM_20948_Status_e i2cMasterSingleW(uint8_t addr, uint8_t reg, uint8_t data);
     uint8_t i2cMasterSingleR(uint8_t addr, uint8_t reg);
 
     // Default Setup
     ICM_20948_Status_e startupDefault(void);
-    virtual ICM_20948_Status_e startupMagnetometer(void);
-    virtual ICM_20948_Status_e getMagnetometerData(ICM_20948_AGMT_t *pagmt);
 
     // direct read/write
     ICM_20948_Status_e read(uint8_t reg, uint8_t *pdata, uint32_t len);
     ICM_20948_Status_e write(uint8_t reg, uint8_t *pdata, uint32_t len);
+
+    //Mag specific
+    ICM_20948_Status_e startupMagnetometer(void);
+    ICM_20948_Status_e magWhoIAm(void);
+    uint8_t readMag(AK09916_Reg_Addr_e reg);
+    ICM_20948_Status_e writeMag(AK09916_Reg_Addr_e reg, uint8_t *pdata);
 };
 
 // I2C
@@ -128,13 +136,6 @@ class ICM_20948_I2C : public ICM_20948
     ICM_20948_I2C(); // Constructor
 
     virtual ICM_20948_Status_e begin(TwoWire &wirePort = Wire, bool ad0val = true, uint8_t ad0pin = ICM_20948_ARD_UNUSED_PIN);
-    virtual ICM_20948_Status_e readMag(uint8_t reg, uint8_t *pdata, uint8_t len);
-    virtual ICM_20948_Status_e writeMag(uint8_t reg, uint8_t *pdata, uint8_t len);
-
-    ICM_20948_Status_e startupMagnetometer(void);
-    ICM_20948_Status_e magWhoIAm(void);
-    bool magIsConnected(void);
-    ICM_20948_Status_e getMagnetometerData(ICM_20948_AGMT_t *pagmt);
 };
 
 // SPI
diff --git a/src/util/ICM_20948_C.c b/src/util/ICM_20948_C.c
index 4ff663c..85ceb40 100644
--- a/src/util/ICM_20948_C.c
+++ b/src/util/ICM_20948_C.c
@@ -2,57 +2,69 @@
 #include "ICM_20948_REGISTERS.h"
 #include "AK09916_REGISTERS.h"
 
-
-
 const ICM_20948_Serif_t NullSerif = {
-	NULL,	// write
-	NULL,	// read
-	NULL,	// user
+	NULL, // write
+	NULL, // read
+	NULL, // user
 };
 
 // Private function prototypes
 
-
-
-
-
-
 // Function definitions
-ICM_20948_Status_e	ICM_20948_link_serif( ICM_20948_Device_t* pdev, const ICM_20948_Serif_t* s ){
-	if(s == NULL){ return ICM_20948_Stat_ParamErr; }
-	if(pdev == NULL){ return ICM_20948_Stat_ParamErr; }
+ICM_20948_Status_e ICM_20948_link_serif(ICM_20948_Device_t *pdev, const ICM_20948_Serif_t *s)
+{
+	if (s == NULL)
+	{
+		return ICM_20948_Stat_ParamErr;
+	}
+	if (pdev == NULL)
+	{
+		return ICM_20948_Stat_ParamErr;
+	}
 	pdev->_serif = s;
 	return ICM_20948_Stat_Ok;
 }
 
-ICM_20948_Status_e	ICM_20948_execute_w( ICM_20948_Device_t* pdev, uint8_t regaddr, uint8_t* pdata, uint32_t len ){
-	if( pdev->_serif->write == NULL ){ return ICM_20948_Stat_NotImpl; }
-	return (*pdev->_serif->write)( regaddr, pdata, len, pdev->_serif->user );
+ICM_20948_Status_e ICM_20948_execute_w(ICM_20948_Device_t *pdev, uint8_t regaddr, uint8_t *pdata, uint32_t len)
+{
+	if (pdev->_serif->write == NULL)
+	{
+		return ICM_20948_Stat_NotImpl;
+	}
+	return (*pdev->_serif->write)(regaddr, pdata, len, pdev->_serif->user);
 }
 
-ICM_20948_Status_e	ICM_20948_execute_r( ICM_20948_Device_t* pdev, uint8_t regaddr, uint8_t* pdata, uint32_t len ){
-	if( pdev->_serif->read == NULL ){ return ICM_20948_Stat_NotImpl; }
-	return (*pdev->_serif->read)( regaddr, pdata, len, pdev->_serif->user );
+ICM_20948_Status_e ICM_20948_execute_r(ICM_20948_Device_t *pdev, uint8_t regaddr, uint8_t *pdata, uint32_t len)
+{
+	if (pdev->_serif->read == NULL)
+	{
+		return ICM_20948_Stat_NotImpl;
+	}
+	return (*pdev->_serif->read)(regaddr, pdata, len, pdev->_serif->user);
 }
 
-
-
-
-// Single-shot I2C on Master IF
-ICM_20948_Status_e	ICM_20948_i2c_master_slv4_txn( ICM_20948_Device_t* pdev, uint8_t addr, uint8_t reg, uint8_t* data, uint8_t len, bool Rw, bool send_reg_addr ){
+//Transact directly with an I2C device, one byte at a time
+//Used to configure a device before it is setup into a normal 0-3 slave slot
+ICM_20948_Status_e ICM_20948_i2c_master_slv4_txn(ICM_20948_Device_t *pdev, uint8_t addr, uint8_t reg, uint8_t *data, uint8_t len, bool Rw, bool send_reg_addr)
+{
 	// Thanks MikeFair! // https://github.com/kriswiner/MPU9250/issues/86
-	
 	ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
-	
-	addr = (((Rw) ? 0x80 : 0x00) | addr );
 
-	retval = ICM_20948_set_bank( pdev, 3 );
-	retval = ICM_20948_execute_w( pdev, AGB3_REG_I2C_SLV4_ADDR, (uint8_t*)&addr, 1 );
-	if( retval != ICM_20948_Stat_Ok ){ return retval; }
+	addr = (((Rw) ? 0x80 : 0x00) | addr);
+
+	retval = ICM_20948_set_bank(pdev, 3);
+	retval = ICM_20948_execute_w(pdev, AGB3_REG_I2C_SLV4_ADDR, (uint8_t *)&addr, 1);
+	if (retval != ICM_20948_Stat_Ok)
+	{
+		return retval;
+	}
 
-	retval = ICM_20948_set_bank( pdev, 3 );
-	retval = ICM_20948_execute_w( pdev, AGB3_REG_I2C_SLV4_REG, (uint8_t*)&reg, 1 );
-	if( retval != ICM_20948_Stat_Ok ){ return retval; }
+	retval = ICM_20948_set_bank(pdev, 3);
+	retval = ICM_20948_execute_w(pdev, AGB3_REG_I2C_SLV4_REG, (uint8_t *)&reg, 1);
+	if (retval != ICM_20948_Stat_Ok)
+	{
+		return retval;
+	}
 
 	ICM_20948_I2C_SLV4_CTRL_t ctrl;
 	ctrl.EN = 1;
@@ -60,192 +72,276 @@ ICM_20948_Status_e	ICM_20948_i2c_master_slv4_txn( ICM_20948_Device_t* pdev, uint
 	ctrl.DLY = 0;
 	ctrl.REG_DIS = !send_reg_addr;
 
-	// ICM_20948_I2C_MST_STATUS_t i2c_mst_status;
-	// bool txn_failed = false;
+	ICM_20948_I2C_MST_STATUS_t i2c_mst_status;
+	bool txn_failed = false;
 	uint16_t nByte = 0;
 
-	while( nByte < len ){
-		if( !Rw ){
-			retval = ICM_20948_set_bank( pdev, 3 );
-			retval = ICM_20948_execute_w( pdev, AGB3_REG_I2C_SLV4_DO, (uint8_t*)&(data[nByte]), 1 );
-			if( retval != ICM_20948_Stat_Ok ){ return retval; }
+	while (nByte < len)
+	{
+		if (!Rw)
+		{
+			retval = ICM_20948_set_bank(pdev, 3);
+			retval = ICM_20948_execute_w(pdev, AGB3_REG_I2C_SLV4_DO, (uint8_t *)&(data[nByte]), 1);
+			if (retval != ICM_20948_Stat_Ok)
+			{
+				return retval;
+			}
 		}
 
 		// Kick off txn
-		retval = ICM_20948_set_bank( pdev, 3 );
-		retval = ICM_20948_execute_w( pdev, AGB3_REG_I2C_SLV4_CTRL, (uint8_t*)&ctrl, sizeof(ICM_20948_I2C_SLV4_CTRL_t) );
-		if( retval != ICM_20948_Stat_Ok ){ return retval; }
-
-	// 	// long tsTimeout = millis() + 3000;  // Emergency timeout for txn (hard coded to 3 secs)
-	// 	uint32_t max_cycles = 1000;
-	// 	uint32_t count = 0;
-	// 	bool slave4Done = false;
-	// 	while (!slave4Done) { 
-	// 		retval = ICM_20948_set_bank( pdev, 0 );
-	// 		retval = ICM_20948_execute_r( pdev, AGB0_REG_I2C_MST_STATUS, &i2c_mst_status, 1 );
-
-	// 		slave4Done = ( i2c_mst_status.I2C_SLV4_DONE /*| (millis() > tsTimeout) */ ); // todo: avoid forever-loops
-	// 		slave4Done |= (count >= max_cycles);
-	// 		count++;
-	// 	}
-	// 	txn_failed = (i2c_mst_status.I2C_SLV4_NACK /* & (1 << I2C_SLV4_NACK_BIT)) | (millis() > tsTimeout) */);
-	// 	txn_failed |= (count >= max_cycles);
-	// 	if (txn_failed) break;
-
-	// 	if ( Rw ){ 
-	// 		retval = ICM_20948_set_bank( pdev, 3 );
-	// 		retval = ICM_20948_execute_r( pdev, AGB3_REG_I2C_SLV4_DI, &data[nByte], 1 );
-	// 	}
+		retval = ICM_20948_set_bank(pdev, 3);
+		retval = ICM_20948_execute_w(pdev, AGB3_REG_I2C_SLV4_CTRL, (uint8_t *)&ctrl, sizeof(ICM_20948_I2C_SLV4_CTRL_t));
+		if (retval != ICM_20948_Stat_Ok)
+		{
+			return retval;
+		}
+
+		// long tsTimeout = millis() + 3000;  // Emergency timeout for txn (hard coded to 3 secs)
+		uint32_t max_cycles = 1000;
+		uint32_t count = 0;
+		bool slave4Done = false;
+		while (!slave4Done)
+		{
+			retval = ICM_20948_set_bank(pdev, 0);
+			retval = ICM_20948_execute_r(pdev, AGB0_REG_I2C_MST_STATUS, (uint8_t *)&i2c_mst_status, 1);
+
+			slave4Done = (i2c_mst_status.I2C_SLV4_DONE /*| (millis() > tsTimeout) */); //Avoid forever-loops
+			slave4Done |= (count >= max_cycles);
+			count++;
+		}
+		txn_failed = (i2c_mst_status.I2C_SLV4_NACK /*| (millis() > tsTimeout) */);
+		txn_failed |= (count >= max_cycles);
+		if (txn_failed)
+			break;
+
+		if (Rw)
+		{
+			retval = ICM_20948_set_bank(pdev, 3);
+			retval = ICM_20948_execute_r(pdev, AGB3_REG_I2C_SLV4_DI, &data[nByte], 1);
+		}
 
 		nByte++;
 	}
-	// if( txn_failed ){ return ICM_20948_Stat_Err; }
+
+	if (txn_failed)
+	{
+		//We often fail here if mag is stuck
+		return ICM_20948_Stat_Err;
+	}
+
 	return retval;
 }
 
-ICM_20948_Status_e	ICM_20948_i2c_master_single_w( ICM_20948_Device_t* pdev, uint8_t addr, uint8_t reg, uint8_t* data ){
-	return ICM_20948_i2c_master_slv4_txn( pdev, addr, reg, data, 1, false, true );
+ICM_20948_Status_e ICM_20948_i2c_master_single_w(ICM_20948_Device_t *pdev, uint8_t addr, uint8_t reg, uint8_t *data)
+{
+	return ICM_20948_i2c_master_slv4_txn(pdev, addr, reg, data, 1, false, true);
 }
 
-ICM_20948_Status_e	ICM_20948_i2c_master_single_r( ICM_20948_Device_t* pdev, uint8_t addr, uint8_t reg, uint8_t* data ){
-	return ICM_20948_i2c_master_slv4_txn( pdev, addr, reg, data, 1, true, true );
+ICM_20948_Status_e ICM_20948_i2c_master_single_r(ICM_20948_Device_t *pdev, uint8_t addr, uint8_t reg, uint8_t *data)
+{
+	return ICM_20948_i2c_master_slv4_txn(pdev, addr, reg, data, 1, true, true);
 }
 
-
-
-
-ICM_20948_Status_e	ICM_20948_set_bank( ICM_20948_Device_t* pdev, uint8_t bank ){
-	if( bank > 3 ){ return ICM_20948_Stat_ParamErr; } // Only 4 possible banks
+ICM_20948_Status_e ICM_20948_set_bank(ICM_20948_Device_t *pdev, uint8_t bank)
+{
+	if (bank > 3)
+	{
+		return ICM_20948_Stat_ParamErr;
+	}						   // Only 4 possible banks
 	bank = (bank << 4) & 0x30; // bits 5:4 of REG_BANK_SEL
-	return ICM_20948_execute_w( pdev, REG_BANK_SEL, &bank, 1 );
+	return ICM_20948_execute_w(pdev, REG_BANK_SEL, &bank, 1);
 }
 
-ICM_20948_Status_e	ICM_20948_sw_reset( ICM_20948_Device_t* pdev ){
+ICM_20948_Status_e ICM_20948_sw_reset(ICM_20948_Device_t *pdev)
+{
 	ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
 	ICM_20948_PWR_MGMT_1_t reg;
 
-	ICM_20948_set_bank(pdev, 0);	// Must be in the right bank
+	ICM_20948_set_bank(pdev, 0); // Must be in the right bank
 
-	retval = ICM_20948_execute_r( pdev, AGB0_REG_PWR_MGMT_1, (uint8_t*)&reg, sizeof(ICM_20948_PWR_MGMT_1_t));
-	if( retval != ICM_20948_Stat_Ok ){ return retval; }
+	retval = ICM_20948_execute_r(pdev, AGB0_REG_PWR_MGMT_1, (uint8_t *)&reg, sizeof(ICM_20948_PWR_MGMT_1_t));
+	if (retval != ICM_20948_Stat_Ok)
+	{
+		return retval;
+	}
 
 	reg.DEVICE_RESET = 1;
 
-	retval = ICM_20948_execute_w( pdev, AGB0_REG_PWR_MGMT_1, (uint8_t*)&reg, sizeof(ICM_20948_PWR_MGMT_1_t));
-	if( retval != ICM_20948_Stat_Ok ){ return retval; }
+	retval = ICM_20948_execute_w(pdev, AGB0_REG_PWR_MGMT_1, (uint8_t *)&reg, sizeof(ICM_20948_PWR_MGMT_1_t));
+	if (retval != ICM_20948_Stat_Ok)
+	{
+		return retval;
+	}
 	return retval;
 }
 
-ICM_20948_Status_e	ICM_20948_sleep				( ICM_20948_Device_t* pdev, bool on ){
+ICM_20948_Status_e ICM_20948_sleep(ICM_20948_Device_t *pdev, bool on)
+{
 	ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
 	ICM_20948_PWR_MGMT_1_t reg;
 
-	ICM_20948_set_bank(pdev, 0);	// Must be in the right bank
+	ICM_20948_set_bank(pdev, 0); // Must be in the right bank
 
-	retval = ICM_20948_execute_r( pdev, AGB0_REG_PWR_MGMT_1, (uint8_t*)&reg, sizeof(ICM_20948_PWR_MGMT_1_t));
-	if( retval != ICM_20948_Stat_Ok ){ return retval; }
+	retval = ICM_20948_execute_r(pdev, AGB0_REG_PWR_MGMT_1, (uint8_t *)&reg, sizeof(ICM_20948_PWR_MGMT_1_t));
+	if (retval != ICM_20948_Stat_Ok)
+	{
+		return retval;
+	}
 
-	if(on){ reg.SLEEP = 1; }
-	else{ reg.SLEEP = 0; }
+	if (on)
+	{
+		reg.SLEEP = 1;
+	}
+	else
+	{
+		reg.SLEEP = 0;
+	}
 
-	retval = ICM_20948_execute_w( pdev, AGB0_REG_PWR_MGMT_1, (uint8_t*)&reg, sizeof(ICM_20948_PWR_MGMT_1_t));
-	if( retval != ICM_20948_Stat_Ok ){ return retval; }
+	retval = ICM_20948_execute_w(pdev, AGB0_REG_PWR_MGMT_1, (uint8_t *)&reg, sizeof(ICM_20948_PWR_MGMT_1_t));
+	if (retval != ICM_20948_Stat_Ok)
+	{
+		return retval;
+	}
 	return retval;
 }
 
-ICM_20948_Status_e	ICM_20948_low_power			( ICM_20948_Device_t* pdev, bool on ){
+ICM_20948_Status_e ICM_20948_low_power(ICM_20948_Device_t *pdev, bool on)
+{
 	ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
 	ICM_20948_PWR_MGMT_1_t reg;
 
-	ICM_20948_set_bank(pdev, 0);	// Must be in the right bank
+	ICM_20948_set_bank(pdev, 0); // Must be in the right bank
 
-	retval = ICM_20948_execute_r( pdev, AGB0_REG_PWR_MGMT_1, (uint8_t*)&reg, sizeof(ICM_20948_PWR_MGMT_1_t));
-	if( retval != ICM_20948_Stat_Ok ){ return retval; }
+	retval = ICM_20948_execute_r(pdev, AGB0_REG_PWR_MGMT_1, (uint8_t *)&reg, sizeof(ICM_20948_PWR_MGMT_1_t));
+	if (retval != ICM_20948_Stat_Ok)
+	{
+		return retval;
+	}
 
-	if(on){ reg.LP_EN = 1; }
-	else{ reg.LP_EN = 0; }
+	if (on)
+	{
+		reg.LP_EN = 1;
+	}
+	else
+	{
+		reg.LP_EN = 0;
+	}
 
-	retval = ICM_20948_execute_w( pdev, AGB0_REG_PWR_MGMT_1, (uint8_t*)&reg, sizeof(ICM_20948_PWR_MGMT_1_t));
-	if( retval != ICM_20948_Stat_Ok ){ return retval; }
+	retval = ICM_20948_execute_w(pdev, AGB0_REG_PWR_MGMT_1, (uint8_t *)&reg, sizeof(ICM_20948_PWR_MGMT_1_t));
+	if (retval != ICM_20948_Stat_Ok)
+	{
+		return retval;
+	}
 	return retval;
 }
 
-ICM_20948_Status_e	ICM_20948_set_clock_source	( ICM_20948_Device_t* pdev, ICM_20948_PWR_MGMT_1_CLKSEL_e source ){
+ICM_20948_Status_e ICM_20948_set_clock_source(ICM_20948_Device_t *pdev, ICM_20948_PWR_MGMT_1_CLKSEL_e source)
+{
 	ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
 	ICM_20948_PWR_MGMT_1_t reg;
 
-	ICM_20948_set_bank(pdev, 0);	// Must be in the right bank
-	
-	retval = ICM_20948_execute_r( pdev, AGB0_REG_PWR_MGMT_1, (uint8_t*)&reg, sizeof(ICM_20948_PWR_MGMT_1_t));
-	if( retval != ICM_20948_Stat_Ok ){ return retval; }
+	ICM_20948_set_bank(pdev, 0); // Must be in the right bank
+
+	retval = ICM_20948_execute_r(pdev, AGB0_REG_PWR_MGMT_1, (uint8_t *)&reg, sizeof(ICM_20948_PWR_MGMT_1_t));
+	if (retval != ICM_20948_Stat_Ok)
+	{
+		return retval;
+	}
 
 	reg.CLKSEL = source;
 
-	retval = ICM_20948_execute_w( pdev, AGB0_REG_PWR_MGMT_1, (uint8_t*)&reg, sizeof(ICM_20948_PWR_MGMT_1_t));
-	if( retval != ICM_20948_Stat_Ok ){ return retval; }
+	retval = ICM_20948_execute_w(pdev, AGB0_REG_PWR_MGMT_1, (uint8_t *)&reg, sizeof(ICM_20948_PWR_MGMT_1_t));
+	if (retval != ICM_20948_Stat_Ok)
+	{
+		return retval;
+	}
 	return retval;
 }
 
-
-
-ICM_20948_Status_e ICM_20948_get_who_am_i( ICM_20948_Device_t* pdev, uint8_t* whoami ){
-	if( whoami == NULL ){ return ICM_20948_Stat_ParamErr; }
-	ICM_20948_set_bank(pdev, 0);	// Must be in the right bank
-	return ICM_20948_execute_r( pdev, AGB0_REG_WHO_AM_I, whoami, 1 );
+ICM_20948_Status_e ICM_20948_get_who_am_i(ICM_20948_Device_t *pdev, uint8_t *whoami)
+{
+	if (whoami == NULL)
+	{
+		return ICM_20948_Stat_ParamErr;
+	}
+	ICM_20948_set_bank(pdev, 0); // Must be in the right bank
+	return ICM_20948_execute_r(pdev, AGB0_REG_WHO_AM_I, whoami, 1);
 }
 
-ICM_20948_Status_e	ICM_20948_check_id( ICM_20948_Device_t* pdev ){
+ICM_20948_Status_e ICM_20948_check_id(ICM_20948_Device_t *pdev)
+{
 	ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
 	uint8_t whoami = 0x00;
-	retval = ICM_20948_get_who_am_i( pdev, &whoami );
-	if( retval != ICM_20948_Stat_Ok){ return retval; }
-	if( whoami != ICM_20948_WHOAMI ){ return ICM_20948_Stat_WrongID; }
+	retval = ICM_20948_get_who_am_i(pdev, &whoami);
+	if (retval != ICM_20948_Stat_Ok)
+	{
+		return retval;
+	}
+	if (whoami != ICM_20948_WHOAMI)
+	{
+		return ICM_20948_Stat_WrongID;
+	}
 	return retval;
 }
 
-ICM_20948_Status_e	ICM_20948_data_ready( ICM_20948_Device_t* pdev ){
+ICM_20948_Status_e ICM_20948_data_ready(ICM_20948_Device_t *pdev)
+{
 	ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
 	ICM_20948_INT_STATUS_1_t reg;
-	retval = ICM_20948_set_bank(pdev, 0);	// Must be in the right bank
-	if( retval != ICM_20948_Stat_Ok){ return retval; }
-	retval = ICM_20948_execute_r( pdev, AGB0_REG_INT_STATUS_1, (uint8_t*)&reg, sizeof(ICM_20948_INT_STATUS_1_t));
-	if( retval != ICM_20948_Stat_Ok){ return retval; }
-	if( !reg.RAW_DATA_0_RDY_INT ){ retval = ICM_20948_Stat_NoData; }
+	retval = ICM_20948_set_bank(pdev, 0); // Must be in the right bank
+	if (retval != ICM_20948_Stat_Ok)
+	{
+		return retval;
+	}
+	retval = ICM_20948_execute_r(pdev, AGB0_REG_INT_STATUS_1, (uint8_t *)&reg, sizeof(ICM_20948_INT_STATUS_1_t));
+	if (retval != ICM_20948_Stat_Ok)
+	{
+		return retval;
+	}
+	if (!reg.RAW_DATA_0_RDY_INT)
+	{
+		retval = ICM_20948_Stat_NoData;
+	}
 	return retval;
 }
 
-
-
-
-
-
-
 // Interrupt Configuration
-ICM_20948_Status_e	ICM_20948_int_pin_cfg		( ICM_20948_Device_t* pdev, ICM_20948_INT_PIN_CFG_t* write, ICM_20948_INT_PIN_CFG_t* read ){
+ICM_20948_Status_e ICM_20948_int_pin_cfg(ICM_20948_Device_t *pdev, ICM_20948_INT_PIN_CFG_t *write, ICM_20948_INT_PIN_CFG_t *read)
+{
 	ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
-	retval = ICM_20948_set_bank(pdev, 0);						// Must be in the right bank
-	if( write != NULL ){										// write first, if available
-		retval = ICM_20948_execute_w( pdev, AGB0_REG_INT_PIN_CONFIG, (uint8_t*)write, sizeof(ICM_20948_INT_PIN_CFG_t));
-		if( retval != ICM_20948_Stat_Ok ){ return retval; }
+	retval = ICM_20948_set_bank(pdev, 0); // Must be in the right bank
+	if (write != NULL)
+	{ // write first, if available
+		retval = ICM_20948_execute_w(pdev, AGB0_REG_INT_PIN_CONFIG, (uint8_t *)write, sizeof(ICM_20948_INT_PIN_CFG_t));
+		if (retval != ICM_20948_Stat_Ok)
+		{
+			return retval;
+		}
 	}
-	if( read != NULL ){											// then read, to allow for verification
-		retval = ICM_20948_execute_r( pdev, AGB0_REG_INT_PIN_CONFIG, (uint8_t*)read, sizeof(ICM_20948_INT_PIN_CFG_t));
-		if( retval != ICM_20948_Stat_Ok ){ return retval; }
+	if (read != NULL)
+	{ // then read, to allow for verification
+		retval = ICM_20948_execute_r(pdev, AGB0_REG_INT_PIN_CONFIG, (uint8_t *)read, sizeof(ICM_20948_INT_PIN_CFG_t));
+		if (retval != ICM_20948_Stat_Ok)
+		{
+			return retval;
+		}
 	}
 	return retval;
 }
 
-ICM_20948_Status_e	ICM_20948_int_enable 		( ICM_20948_Device_t* pdev, ICM_20948_INT_enable_t* write, ICM_20948_INT_enable_t* read ){
+ICM_20948_Status_e ICM_20948_int_enable(ICM_20948_Device_t *pdev, ICM_20948_INT_enable_t *write, ICM_20948_INT_enable_t *read)
+{
 	ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
 
-	ICM_20948_INT_ENABLE_t 		en_0;
-	ICM_20948_INT_ENABLE_1_t 	en_1;
-	ICM_20948_INT_ENABLE_2_t 	en_2;
-	ICM_20948_INT_ENABLE_3_t 	en_3;
+	ICM_20948_INT_ENABLE_t en_0;
+	ICM_20948_INT_ENABLE_1_t en_1;
+	ICM_20948_INT_ENABLE_2_t en_2;
+	ICM_20948_INT_ENABLE_3_t en_3;
 
-	retval = ICM_20948_set_bank(pdev, 0);	// Must be in the right bank
+	retval = ICM_20948_set_bank(pdev, 0); // Must be in the right bank
 
-	if( write != NULL ){	// If the write pointer is not NULL then write to the registers BEFORE reading
+	if (write != NULL)
+	{ // If the write pointer is not NULL then write to the registers BEFORE reading
 		en_0.I2C_MST_INT_EN = write->I2C_MST_INT_EN;
 		en_0.DMP_INT1_EN = write->DMP_INT1_EN;
 		en_0.PLL_READY_EN = write->PLL_RDY_EN;
@@ -263,18 +359,51 @@ ICM_20948_Status_e	ICM_20948_int_enable 		( ICM_20948_Device_t* pdev, ICM_20948_
 		en_3.individual.FIFO_WM_EN_1 = write->FIFO_WM_EN_1;
 		en_3.individual.FIFO_WM_EN_0 = write->FIFO_WM_EN_0;
 
-		retval = ICM_20948_execute_w( pdev, AGB0_REG_INT_ENABLE, (uint8_t*)&en_0, sizeof(ICM_20948_INT_ENABLE_t)); if( retval != ICM_20948_Stat_Ok ){ return retval; }
-		retval = ICM_20948_execute_w( pdev, AGB0_REG_INT_ENABLE_1, (uint8_t*)&en_1, sizeof(ICM_20948_INT_ENABLE_1_t)); if( retval != ICM_20948_Stat_Ok ){ return retval; }
-		retval = ICM_20948_execute_w( pdev, AGB0_REG_INT_ENABLE_2, (uint8_t*)&en_2, sizeof(ICM_20948_INT_ENABLE_2_t)); if( retval != ICM_20948_Stat_Ok ){ return retval; }
-		retval = ICM_20948_execute_w( pdev, AGB0_REG_INT_ENABLE_3, (uint8_t*)&en_3, sizeof(ICM_20948_INT_ENABLE_3_t)); if( retval != ICM_20948_Stat_Ok ){ return retval; }
+		retval = ICM_20948_execute_w(pdev, AGB0_REG_INT_ENABLE, (uint8_t *)&en_0, sizeof(ICM_20948_INT_ENABLE_t));
+		if (retval != ICM_20948_Stat_Ok)
+		{
+			return retval;
+		}
+		retval = ICM_20948_execute_w(pdev, AGB0_REG_INT_ENABLE_1, (uint8_t *)&en_1, sizeof(ICM_20948_INT_ENABLE_1_t));
+		if (retval != ICM_20948_Stat_Ok)
+		{
+			return retval;
+		}
+		retval = ICM_20948_execute_w(pdev, AGB0_REG_INT_ENABLE_2, (uint8_t *)&en_2, sizeof(ICM_20948_INT_ENABLE_2_t));
+		if (retval != ICM_20948_Stat_Ok)
+		{
+			return retval;
+		}
+		retval = ICM_20948_execute_w(pdev, AGB0_REG_INT_ENABLE_3, (uint8_t *)&en_3, sizeof(ICM_20948_INT_ENABLE_3_t));
+		if (retval != ICM_20948_Stat_Ok)
+		{
+			return retval;
+		}
 	}
 
-	if( read != NULL ){	// If read pointer is not NULL then read the registers (if write is not NULL then this should read back the results of write into read)
-		retval = ICM_20948_execute_r( pdev, AGB0_REG_INT_ENABLE, (uint8_t*)&en_0, sizeof(ICM_20948_INT_ENABLE_t)); if( retval != ICM_20948_Stat_Ok ){ return retval; }
-		retval = ICM_20948_execute_r( pdev, AGB0_REG_INT_ENABLE_1, (uint8_t*)&en_1, sizeof(ICM_20948_INT_ENABLE_1_t)); if( retval != ICM_20948_Stat_Ok ){ return retval; }
-		retval = ICM_20948_execute_r( pdev, AGB0_REG_INT_ENABLE_2, (uint8_t*)&en_2, sizeof(ICM_20948_INT_ENABLE_2_t)); if( retval != ICM_20948_Stat_Ok ){ return retval; }
-		retval = ICM_20948_execute_r( pdev, AGB0_REG_INT_ENABLE_3, (uint8_t*)&en_3, sizeof(ICM_20948_INT_ENABLE_3_t)); if( retval != ICM_20948_Stat_Ok ){ return retval; }
-	
+	if (read != NULL)
+	{ // If read pointer is not NULL then read the registers (if write is not NULL then this should read back the results of write into read)
+		retval = ICM_20948_execute_r(pdev, AGB0_REG_INT_ENABLE, (uint8_t *)&en_0, sizeof(ICM_20948_INT_ENABLE_t));
+		if (retval != ICM_20948_Stat_Ok)
+		{
+			return retval;
+		}
+		retval = ICM_20948_execute_r(pdev, AGB0_REG_INT_ENABLE_1, (uint8_t *)&en_1, sizeof(ICM_20948_INT_ENABLE_1_t));
+		if (retval != ICM_20948_Stat_Ok)
+		{
+			return retval;
+		}
+		retval = ICM_20948_execute_r(pdev, AGB0_REG_INT_ENABLE_2, (uint8_t *)&en_2, sizeof(ICM_20948_INT_ENABLE_2_t));
+		if (retval != ICM_20948_Stat_Ok)
+		{
+			return retval;
+		}
+		retval = ICM_20948_execute_r(pdev, AGB0_REG_INT_ENABLE_3, (uint8_t *)&en_3, sizeof(ICM_20948_INT_ENABLE_3_t));
+		if (retval != ICM_20948_Stat_Ok)
+		{
+			return retval;
+		}
+
 		read->I2C_MST_INT_EN = en_0.I2C_MST_INT_EN;
 		read->DMP_INT1_EN = en_0.DMP_INT1_EN;
 		read->PLL_RDY_EN = en_0.PLL_READY_EN;
@@ -296,199 +425,344 @@ ICM_20948_Status_e	ICM_20948_int_enable 		( ICM_20948_Device_t* pdev, ICM_20948_
 	return retval;
 }
 
+ICM_20948_Status_e ICM_20948_set_sample_mode(ICM_20948_Device_t *pdev, ICM_20948_InternalSensorID_bm sensors, ICM_20948_LP_CONFIG_CYCLE_e mode)
+{
+	ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
+	ICM_20948_LP_CONFIG_t reg;
 
+	if (!(sensors & (ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr | ICM_20948_Internal_Mst)))
+	{
+		return ICM_20948_Stat_SensorNotSupported;
+	}
 
+	retval = ICM_20948_set_bank(pdev, 0); // Must be in the right bank
+	if (retval != ICM_20948_Stat_Ok)
+	{
+		return retval;
+	}
+	retval = ICM_20948_execute_r(pdev, AGB0_REG_LP_CONFIG, (uint8_t *)&reg, sizeof(ICM_20948_LP_CONFIG_t));
+	if (retval != ICM_20948_Stat_Ok)
+	{
+		return retval;
+	}
 
+	if (sensors & ICM_20948_Internal_Acc)
+	{
+		reg.ACCEL_CYCLE = mode;
+	} // Set all desired sensors to this setting
+	if (sensors & ICM_20948_Internal_Gyr)
+	{
+		reg.GYRO_CYCLE = mode;
+	}
+	if (sensors & ICM_20948_Internal_Mst)
+	{
+		reg.I2C_MST_CYCLE = mode;
+	}
 
-
-
-
-
-ICM_20948_Status_e	ICM_20948_set_sample_mode( ICM_20948_Device_t* pdev, ICM_20948_InternalSensorID_bm sensors, ICM_20948_LP_CONFIG_CYCLE_e mode ){
-	ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
-	ICM_20948_LP_CONFIG_t reg;
-
-	if( !(sensors & ( ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr | ICM_20948_Internal_Mst ) ) ){ return ICM_20948_Stat_SensorNotSupported; }
-	
-	retval = ICM_20948_set_bank(pdev, 0);				// Must be in the right bank
-	if( retval != ICM_20948_Stat_Ok){ return retval; }
-	retval = ICM_20948_execute_r( pdev, AGB0_REG_LP_CONFIG, (uint8_t*)&reg, sizeof(ICM_20948_LP_CONFIG_t));
-	if( retval != ICM_20948_Stat_Ok){ return retval; }
-	
-	if( sensors & ICM_20948_Internal_Acc ){ reg.ACCEL_CYCLE = mode; }		// Set all desired sensors to this setting
-	if( sensors & ICM_20948_Internal_Gyr ){ reg.GYRO_CYCLE = mode; }
-	if( sensors & ICM_20948_Internal_Mst ){ reg.I2C_MST_CYCLE = mode; }
-
-	retval = ICM_20948_execute_w( pdev, AGB0_REG_LP_CONFIG, (uint8_t*)&reg, sizeof(ICM_20948_LP_CONFIG_t));
-	if( retval != ICM_20948_Stat_Ok ){ return retval; }
+	retval = ICM_20948_execute_w(pdev, AGB0_REG_LP_CONFIG, (uint8_t *)&reg, sizeof(ICM_20948_LP_CONFIG_t));
+	if (retval != ICM_20948_Stat_Ok)
+	{
+		return retval;
+	}
 	return retval;
 }
 
-ICM_20948_Status_e	ICM_20948_set_full_scale 	( ICM_20948_Device_t* pdev, ICM_20948_InternalSensorID_bm sensors, ICM_20948_fss_t fss ){
+ICM_20948_Status_e ICM_20948_set_full_scale(ICM_20948_Device_t *pdev, ICM_20948_InternalSensorID_bm sensors, ICM_20948_fss_t fss)
+{
 	ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
 
-	if( !(sensors & ( ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr ) ) ){ return ICM_20948_Stat_SensorNotSupported; }
+	if (!(sensors & (ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr)))
+	{
+		return ICM_20948_Stat_SensorNotSupported;
+	}
 
-	if( sensors & ICM_20948_Internal_Acc ){
+	if (sensors & ICM_20948_Internal_Acc)
+	{
 		ICM_20948_ACCEL_CONFIG_t reg;
-		retval |= ICM_20948_set_bank(pdev, 2);	// Must be in the right bank
-		retval |= ICM_20948_execute_r( pdev, AGB2_REG_ACCEL_CONFIG, (uint8_t*)&reg, sizeof(ICM_20948_ACCEL_CONFIG_t));
+		retval |= ICM_20948_set_bank(pdev, 2); // Must be in the right bank
+		retval |= ICM_20948_execute_r(pdev, AGB2_REG_ACCEL_CONFIG, (uint8_t *)&reg, sizeof(ICM_20948_ACCEL_CONFIG_t));
 		reg.ACCEL_FS_SEL = fss.a;
-		retval |= ICM_20948_execute_w( pdev, AGB2_REG_ACCEL_CONFIG, (uint8_t*)&reg, sizeof(ICM_20948_ACCEL_CONFIG_t));
+		retval |= ICM_20948_execute_w(pdev, AGB2_REG_ACCEL_CONFIG, (uint8_t *)&reg, sizeof(ICM_20948_ACCEL_CONFIG_t));
 	}
-	if( sensors & ICM_20948_Internal_Gyr ){
+	if (sensors & ICM_20948_Internal_Gyr)
+	{
 		ICM_20948_GYRO_CONFIG_1_t reg;
-		retval |= ICM_20948_set_bank(pdev, 2);	// Must be in the right bank
-		retval |= ICM_20948_execute_r( pdev, AGB2_REG_GYRO_CONFIG_1, (uint8_t*)&reg, sizeof(ICM_20948_GYRO_CONFIG_1_t));
+		retval |= ICM_20948_set_bank(pdev, 2); // Must be in the right bank
+		retval |= ICM_20948_execute_r(pdev, AGB2_REG_GYRO_CONFIG_1, (uint8_t *)&reg, sizeof(ICM_20948_GYRO_CONFIG_1_t));
 		reg.GYRO_FS_SEL = fss.g;
-		retval |= ICM_20948_execute_w( pdev, AGB2_REG_GYRO_CONFIG_1, (uint8_t*)&reg, sizeof(ICM_20948_GYRO_CONFIG_1_t));
+		retval |= ICM_20948_execute_w(pdev, AGB2_REG_GYRO_CONFIG_1, (uint8_t *)&reg, sizeof(ICM_20948_GYRO_CONFIG_1_t));
 	}
 	return retval;
 }
 
-ICM_20948_Status_e	ICM_20948_set_dlpf_cfg		( ICM_20948_Device_t* pdev, ICM_20948_InternalSensorID_bm sensors, ICM_20948_dlpcfg_t cfg ){
+ICM_20948_Status_e ICM_20948_set_dlpf_cfg(ICM_20948_Device_t *pdev, ICM_20948_InternalSensorID_bm sensors, ICM_20948_dlpcfg_t cfg)
+{
 	ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
 
-	if( !(sensors & ( ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr ) ) ){ return ICM_20948_Stat_SensorNotSupported; }
+	if (!(sensors & (ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr)))
+	{
+		return ICM_20948_Stat_SensorNotSupported;
+	}
 
-	if( sensors & ICM_20948_Internal_Acc ){
+	if (sensors & ICM_20948_Internal_Acc)
+	{
 		ICM_20948_ACCEL_CONFIG_t reg;
-		retval |= ICM_20948_set_bank(pdev, 2);	// Must be in the right bank
-		retval |= ICM_20948_execute_r( pdev, AGB2_REG_ACCEL_CONFIG, (uint8_t*)&reg, sizeof(ICM_20948_ACCEL_CONFIG_t));
+		retval |= ICM_20948_set_bank(pdev, 2); // Must be in the right bank
+		retval |= ICM_20948_execute_r(pdev, AGB2_REG_ACCEL_CONFIG, (uint8_t *)&reg, sizeof(ICM_20948_ACCEL_CONFIG_t));
 		reg.ACCEL_DLPFCFG = cfg.a;
-		retval |= ICM_20948_execute_w( pdev, AGB2_REG_ACCEL_CONFIG, (uint8_t*)&reg, sizeof(ICM_20948_ACCEL_CONFIG_t));
+		retval |= ICM_20948_execute_w(pdev, AGB2_REG_ACCEL_CONFIG, (uint8_t *)&reg, sizeof(ICM_20948_ACCEL_CONFIG_t));
 	}
-	if( sensors & ICM_20948_Internal_Gyr ){
+	if (sensors & ICM_20948_Internal_Gyr)
+	{
 		ICM_20948_GYRO_CONFIG_1_t reg;
-		retval |= ICM_20948_set_bank(pdev, 2);	// Must be in the right bank
-		retval |= ICM_20948_execute_r( pdev, AGB2_REG_GYRO_CONFIG_1, (uint8_t*)&reg, sizeof(ICM_20948_GYRO_CONFIG_1_t));
+		retval |= ICM_20948_set_bank(pdev, 2); // Must be in the right bank
+		retval |= ICM_20948_execute_r(pdev, AGB2_REG_GYRO_CONFIG_1, (uint8_t *)&reg, sizeof(ICM_20948_GYRO_CONFIG_1_t));
 		reg.GYRO_DLPFCFG = cfg.g;
-		retval |= ICM_20948_execute_w( pdev, AGB2_REG_GYRO_CONFIG_1, (uint8_t*)&reg, sizeof(ICM_20948_GYRO_CONFIG_1_t));
+		retval |= ICM_20948_execute_w(pdev, AGB2_REG_GYRO_CONFIG_1, (uint8_t *)&reg, sizeof(ICM_20948_GYRO_CONFIG_1_t));
 	}
 	return retval;
-}	
+}
 
-ICM_20948_Status_e	ICM_20948_enable_dlpf		( ICM_20948_Device_t* pdev, ICM_20948_InternalSensorID_bm sensors, bool enable ){
+ICM_20948_Status_e ICM_20948_enable_dlpf(ICM_20948_Device_t *pdev, ICM_20948_InternalSensorID_bm sensors, bool enable)
+{
 	ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
 
-	if( !(sensors & ( ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr ) ) ){ return ICM_20948_Stat_SensorNotSupported; }
+	if (!(sensors & (ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr)))
+	{
+		return ICM_20948_Stat_SensorNotSupported;
+	}
 
-	if( sensors & ICM_20948_Internal_Acc ){
+	if (sensors & ICM_20948_Internal_Acc)
+	{
 		ICM_20948_ACCEL_CONFIG_t reg;
-		retval |= ICM_20948_set_bank(pdev, 2);	// Must be in the right bank
-		retval |= ICM_20948_execute_r( pdev, AGB2_REG_ACCEL_CONFIG, (uint8_t*)&reg, sizeof(ICM_20948_ACCEL_CONFIG_t));
-		if( enable ){ reg.ACCEL_FCHOICE = 1; }
-		else{ reg.ACCEL_FCHOICE = 0; }
-		retval |= ICM_20948_execute_w( pdev, AGB2_REG_ACCEL_CONFIG, (uint8_t*)&reg, sizeof(ICM_20948_ACCEL_CONFIG_t));
+		retval |= ICM_20948_set_bank(pdev, 2); // Must be in the right bank
+		retval |= ICM_20948_execute_r(pdev, AGB2_REG_ACCEL_CONFIG, (uint8_t *)&reg, sizeof(ICM_20948_ACCEL_CONFIG_t));
+		if (enable)
+		{
+			reg.ACCEL_FCHOICE = 1;
+		}
+		else
+		{
+			reg.ACCEL_FCHOICE = 0;
+		}
+		retval |= ICM_20948_execute_w(pdev, AGB2_REG_ACCEL_CONFIG, (uint8_t *)&reg, sizeof(ICM_20948_ACCEL_CONFIG_t));
 	}
-	if( sensors & ICM_20948_Internal_Gyr ){
+	if (sensors & ICM_20948_Internal_Gyr)
+	{
 		ICM_20948_GYRO_CONFIG_1_t reg;
-		retval |= ICM_20948_set_bank(pdev, 2);	// Must be in the right bank
-		retval |= ICM_20948_execute_r( pdev, AGB2_REG_GYRO_CONFIG_1, (uint8_t*)&reg, sizeof(ICM_20948_GYRO_CONFIG_1_t));
-		if( enable ){ reg.GYRO_FCHOICE = 1; }
-		else{ reg.GYRO_FCHOICE = 0; }
-		retval |= ICM_20948_execute_w( pdev, AGB2_REG_GYRO_CONFIG_1, (uint8_t*)&reg, sizeof(ICM_20948_GYRO_CONFIG_1_t));
+		retval |= ICM_20948_set_bank(pdev, 2); // Must be in the right bank
+		retval |= ICM_20948_execute_r(pdev, AGB2_REG_GYRO_CONFIG_1, (uint8_t *)&reg, sizeof(ICM_20948_GYRO_CONFIG_1_t));
+		if (enable)
+		{
+			reg.GYRO_FCHOICE = 1;
+		}
+		else
+		{
+			reg.GYRO_FCHOICE = 0;
+		}
+		retval |= ICM_20948_execute_w(pdev, AGB2_REG_GYRO_CONFIG_1, (uint8_t *)&reg, sizeof(ICM_20948_GYRO_CONFIG_1_t));
 	}
 	return retval;
 }
 
-ICM_20948_Status_e	ICM_20948_set_sample_rate	( ICM_20948_Device_t* pdev, ICM_20948_InternalSensorID_bm sensors, ICM_20948_smplrt_t smplrt ){
+ICM_20948_Status_e ICM_20948_set_sample_rate(ICM_20948_Device_t *pdev, ICM_20948_InternalSensorID_bm sensors, ICM_20948_smplrt_t smplrt)
+{
 	ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
 
-	if( !(sensors & ( ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr ) ) ){ return ICM_20948_Stat_SensorNotSupported; }
+	if (!(sensors & (ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr)))
+	{
+		return ICM_20948_Stat_SensorNotSupported;
+	}
 
-	if( sensors & ICM_20948_Internal_Acc ){
-		retval |= ICM_20948_set_bank(pdev, 2);	// Must be in the right bank
+	if (sensors & ICM_20948_Internal_Acc)
+	{
+		retval |= ICM_20948_set_bank(pdev, 2); // Must be in the right bank
 		uint8_t div1 = (smplrt.a << 8);
 		uint8_t div2 = (smplrt.a & 0xFF);
-		retval |= ICM_20948_execute_w( pdev, AGB2_REG_ACCEL_SMPLRT_DIV_1, &div1, 1);
-		retval |= ICM_20948_execute_w( pdev, AGB2_REG_ACCEL_SMPLRT_DIV_2, &div2, 1);
+		retval |= ICM_20948_execute_w(pdev, AGB2_REG_ACCEL_SMPLRT_DIV_1, &div1, 1);
+		retval |= ICM_20948_execute_w(pdev, AGB2_REG_ACCEL_SMPLRT_DIV_2, &div2, 1);
 	}
-	if( sensors & ICM_20948_Internal_Gyr ){
-		retval |= ICM_20948_set_bank(pdev, 2);	// Must be in the right bank
+	if (sensors & ICM_20948_Internal_Gyr)
+	{
+		retval |= ICM_20948_set_bank(pdev, 2); // Must be in the right bank
 		uint8_t div = (smplrt.g);
-		retval |= ICM_20948_execute_w( pdev, AGB2_REG_GYRO_SMPLRT_DIV, &div, 1);
+		retval |= ICM_20948_execute_w(pdev, AGB2_REG_GYRO_SMPLRT_DIV, &div, 1);
 	}
 	return retval;
 }
 
-
-
-
 // Interface Things
-ICM_20948_Status_e	ICM_20948_i2c_master_passthrough 			( ICM_20948_Device_t* pdev, bool passthrough ){
+ICM_20948_Status_e ICM_20948_i2c_master_passthrough(ICM_20948_Device_t *pdev, bool passthrough)
+{
 	ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
 
 	ICM_20948_INT_PIN_CFG_t reg;
 	retval = ICM_20948_set_bank(pdev, 0);
-	if( retval != ICM_20948_Stat_Ok ){ return retval; }
-	retval = ICM_20948_execute_r( pdev, AGB0_REG_INT_PIN_CONFIG, (uint8_t*)&reg, sizeof(ICM_20948_INT_PIN_CFG_t) );
-	if( retval != ICM_20948_Stat_Ok ){ return retval; }
+	if (retval != ICM_20948_Stat_Ok)
+	{
+		return retval;
+	}
+	retval = ICM_20948_execute_r(pdev, AGB0_REG_INT_PIN_CONFIG, (uint8_t *)&reg, sizeof(ICM_20948_INT_PIN_CFG_t));
+	if (retval != ICM_20948_Stat_Ok)
+	{
+		return retval;
+	}
 	reg.BYPASS_EN = passthrough;
-	retval = ICM_20948_execute_w( pdev, AGB0_REG_INT_PIN_CONFIG, (uint8_t*)&reg, sizeof(ICM_20948_INT_PIN_CFG_t) );
-	if( retval != ICM_20948_Stat_Ok ){ return retval; }
+	retval = ICM_20948_execute_w(pdev, AGB0_REG_INT_PIN_CONFIG, (uint8_t *)&reg, sizeof(ICM_20948_INT_PIN_CFG_t));
+	if (retval != ICM_20948_Stat_Ok)
+	{
+		return retval;
+	}
 
 	return retval;
 }
 
-ICM_20948_Status_e	ICM_20948_i2c_master_enable ( ICM_20948_Device_t* pdev, bool enable ){
+ICM_20948_Status_e ICM_20948_i2c_master_enable(ICM_20948_Device_t *pdev, bool enable)
+{
 	ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
 
 	// Disable BYPASS_EN
-	retval = ICM_20948_i2c_master_passthrough( pdev, false );
-	if( retval != ICM_20948_Stat_Ok ){ return retval; }
+	retval = ICM_20948_i2c_master_passthrough(pdev, false);
+	if (retval != ICM_20948_Stat_Ok)
+	{
+		return retval;
+	}
 
 	ICM_20948_I2C_MST_CTRL_t ctrl;
 	retval = ICM_20948_set_bank(pdev, 3);
-	if( retval != ICM_20948_Stat_Ok ){ return retval; }	
-	retval = ICM_20948_execute_r( pdev, AGB3_REG_I2C_MST_CTRL, (uint8_t*)&ctrl, sizeof(ICM_20948_I2C_MST_CTRL_t) );
-	if( retval != ICM_20948_Stat_Ok ){ return retval; }
+	if (retval != ICM_20948_Stat_Ok)
+	{
+		return retval;
+	}
+	retval = ICM_20948_execute_r(pdev, AGB3_REG_I2C_MST_CTRL, (uint8_t *)&ctrl, sizeof(ICM_20948_I2C_MST_CTRL_t));
+	if (retval != ICM_20948_Stat_Ok)
+	{
+		return retval;
+	}
 	ctrl.I2C_MST_CLK = 0x07; // corresponds to 345.6 kHz, good for up to 400 kHz
 	ctrl.I2C_MST_P_NSR = 1;
-	retval = ICM_20948_execute_w( pdev, AGB3_REG_I2C_MST_CTRL, (uint8_t*)&ctrl, sizeof(ICM_20948_I2C_MST_CTRL_t) );
-	if( retval != ICM_20948_Stat_Ok ){ return retval; }
+	retval = ICM_20948_execute_w(pdev, AGB3_REG_I2C_MST_CTRL, (uint8_t *)&ctrl, sizeof(ICM_20948_I2C_MST_CTRL_t));
+	if (retval != ICM_20948_Stat_Ok)
+	{
+		return retval;
+	}
 
 	ICM_20948_USER_CTRL_t reg;
 	retval = ICM_20948_set_bank(pdev, 0);
-	if( retval != ICM_20948_Stat_Ok ){ return retval; }
-	retval = ICM_20948_execute_r( pdev, AGB0_REG_USER_CTRL, (uint8_t*)&reg, sizeof(ICM_20948_USER_CTRL_t) );
-	if( retval != ICM_20948_Stat_Ok ){ return retval; }
-	if( enable ){ reg.I2C_MST_EN = 1; }
-	else{ reg.I2C_MST_EN = 0; }
-	retval = ICM_20948_execute_w( pdev, AGB0_REG_USER_CTRL, (uint8_t*)&reg, sizeof(ICM_20948_USER_CTRL_t) );
-	if( retval != ICM_20948_Stat_Ok ){ return retval; }
+	if (retval != ICM_20948_Stat_Ok)
+	{
+		return retval;
+	}
+	retval = ICM_20948_execute_r(pdev, AGB0_REG_USER_CTRL, (uint8_t *)&reg, sizeof(ICM_20948_USER_CTRL_t));
+	if (retval != ICM_20948_Stat_Ok)
+	{
+		return retval;
+	}
+	if (enable)
+	{
+		reg.I2C_MST_EN = 1;
+	}
+	else
+	{
+		reg.I2C_MST_EN = 0;
+	}
+	retval = ICM_20948_execute_w(pdev, AGB0_REG_USER_CTRL, (uint8_t *)&reg, sizeof(ICM_20948_USER_CTRL_t));
+	if (retval != ICM_20948_Stat_Ok)
+	{
+		return retval;
+	}
+
+	return retval;
+}
+
+ICM_20948_Status_e ICM_20948_i2c_master_reset(ICM_20948_Device_t *pdev)
+{
+	ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
+
+	ICM_20948_USER_CTRL_t ctrl;
+	retval = ICM_20948_set_bank(pdev, 0);
+	if (retval != ICM_20948_Stat_Ok)
+	{
+		return retval;
+	}
+
+	retval = ICM_20948_execute_r(pdev, AGB0_REG_USER_CTRL, (uint8_t *)&ctrl, sizeof(ICM_20948_USER_CTRL_t));
+	if (retval != ICM_20948_Stat_Ok)
+	{
+		return retval;
+	}
+
+	ctrl.I2C_MST_RST = 1; //Reset!
 
+	retval = ICM_20948_execute_w(pdev, AGB0_REG_USER_CTRL, (uint8_t *)&ctrl, sizeof(ICM_20948_USER_CTRL_t));
+	if (retval != ICM_20948_Stat_Ok)
+	{
+		return retval;
+	}
 	return retval;
 }
 
-ICM_20948_Status_e	ICM_20948_i2c_master_configure_slave 		( ICM_20948_Device_t* pdev, uint8_t slave, uint8_t addr, uint8_t reg, uint8_t len, bool Rw, bool enable, bool data_only, bool grp, bool swap ){
+ICM_20948_Status_e ICM_20948_i2c_master_configure_slave(ICM_20948_Device_t *pdev, uint8_t slave, uint8_t addr, uint8_t reg, uint8_t len, bool Rw, bool enable, bool data_only, bool grp, bool swap)
+{
 	ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
-	
+
 	uint8_t slv_addr_reg;
 	uint8_t slv_reg_reg;
 	uint8_t slv_ctrl_reg;
 
-	switch( slave ){
-		case 0 : slv_addr_reg = AGB3_REG_I2C_SLV0_ADDR; slv_reg_reg = AGB3_REG_I2C_SLV0_REG; slv_ctrl_reg = AGB3_REG_I2C_SLV0_CTRL; break;
-		case 1 : slv_addr_reg = AGB3_REG_I2C_SLV1_ADDR; slv_reg_reg = AGB3_REG_I2C_SLV1_REG; slv_ctrl_reg = AGB3_REG_I2C_SLV1_CTRL; break;
-		case 2 : slv_addr_reg = AGB3_REG_I2C_SLV2_ADDR; slv_reg_reg = AGB3_REG_I2C_SLV2_REG; slv_ctrl_reg = AGB3_REG_I2C_SLV2_CTRL; break;
-		case 3 : slv_addr_reg = AGB3_REG_I2C_SLV3_ADDR; slv_reg_reg = AGB3_REG_I2C_SLV3_REG; slv_ctrl_reg = AGB3_REG_I2C_SLV3_CTRL; break;
-		default :
-			return ICM_20948_Stat_ParamErr;
+	switch (slave)
+	{
+	case 0:
+		slv_addr_reg = AGB3_REG_I2C_SLV0_ADDR;
+		slv_reg_reg = AGB3_REG_I2C_SLV0_REG;
+		slv_ctrl_reg = AGB3_REG_I2C_SLV0_CTRL;
+		break;
+	case 1:
+		slv_addr_reg = AGB3_REG_I2C_SLV1_ADDR;
+		slv_reg_reg = AGB3_REG_I2C_SLV1_REG;
+		slv_ctrl_reg = AGB3_REG_I2C_SLV1_CTRL;
+		break;
+	case 2:
+		slv_addr_reg = AGB3_REG_I2C_SLV2_ADDR;
+		slv_reg_reg = AGB3_REG_I2C_SLV2_REG;
+		slv_ctrl_reg = AGB3_REG_I2C_SLV2_CTRL;
+		break;
+	case 3:
+		slv_addr_reg = AGB3_REG_I2C_SLV3_ADDR;
+		slv_reg_reg = AGB3_REG_I2C_SLV3_REG;
+		slv_ctrl_reg = AGB3_REG_I2C_SLV3_CTRL;
+		break;
+	default:
+		return ICM_20948_Stat_ParamErr;
+	}
+
+	retval = ICM_20948_set_bank(pdev, 3);
+	if (retval != ICM_20948_Stat_Ok)
+	{
+		return retval;
 	}
 
 	// Set the slave address and the Rw flag
 	ICM_20948_I2C_SLVX_ADDR_t address;
 	address.ID = addr;
-	if( Rw ){ address.RNW = 1; }
-	retval = ICM_20948_execute_w( pdev, slv_addr_reg, (uint8_t*)&address, sizeof(ICM_20948_I2C_SLVX_ADDR_t) );
-	if( retval != ICM_20948_Stat_Ok ){ return retval; }
+	if (Rw)
+	{
+		address.RNW = 1;
+	}
+	retval = ICM_20948_execute_w(pdev, slv_addr_reg, (uint8_t *)&address, sizeof(ICM_20948_I2C_SLVX_ADDR_t));
+	if (retval != ICM_20948_Stat_Ok)
+	{
+		return retval;
+	}
 
 	// Set the slave sub-address (reg)
 	ICM_20948_I2C_SLVX_REG_t subaddress;
 	subaddress.REG = reg;
-	retval = ICM_20948_execute_w( pdev, slv_reg_reg, (uint8_t*)&subaddress, sizeof(ICM_20948_I2C_SLVX_REG_t) );
-	if( retval != ICM_20948_Stat_Ok ){ return retval; }
+	retval = ICM_20948_execute_w(pdev, slv_reg_reg, (uint8_t *)&subaddress, sizeof(ICM_20948_I2C_SLVX_REG_t));
+	if (retval != ICM_20948_Stat_Ok)
+	{
+		return retval;
+	}
 
 	// Set up the control info
 	ICM_20948_I2C_SLVX_CTRL_t ctrl;
@@ -497,31 +771,30 @@ ICM_20948_Status_e	ICM_20948_i2c_master_configure_slave 		( ICM_20948_Device_t*
 	ctrl.REG_DIS = data_only;
 	ctrl.GRP = grp;
 	ctrl.BYTE_SW = swap;
-	retval = ICM_20948_execute_w( pdev, slv_ctrl_reg, (uint8_t*)&ctrl, sizeof(ICM_20948_I2C_SLVX_CTRL_t) );
-	if( retval != ICM_20948_Stat_Ok ){ return retval; }
+	retval = ICM_20948_execute_w(pdev, slv_ctrl_reg, (uint8_t *)&ctrl, sizeof(ICM_20948_I2C_SLVX_CTRL_t));
+	if (retval != ICM_20948_Stat_Ok)
+	{
+		return retval;
+	}
 
 	return retval;
 }
 
-
-
-
-
-
-
-
-
 // Higher Level
-ICM_20948_Status_e  ICM_20948_get_agmt          ( ICM_20948_Device_t* pdev, ICM_20948_AGMT_t* pagmt ){
-	if( pagmt == NULL ){ return ICM_20948_Stat_ParamErr; }
+ICM_20948_Status_e ICM_20948_get_agmt(ICM_20948_Device_t *pdev, ICM_20948_AGMT_t *pagmt)
+{
+	if (pagmt == NULL)
+	{
+		return ICM_20948_Stat_ParamErr;
+	}
 
 	ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
-	const uint8_t numbytes = 14;
+	const uint8_t numbytes = 14 + 9; //Read Accel, gyro, temp, and 9 bytes of mag
 	uint8_t buff[numbytes];
 
 	// Get readings
-	retval |= ICM_20948_set_bank( pdev, 0 ); 
-	retval |= ICM_20948_execute_r( pdev, (uint8_t)AGB0_REG_ACCEL_XOUT_H, buff, numbytes );
+	retval |= ICM_20948_set_bank(pdev, 0);
+	retval |= ICM_20948_execute_r(pdev, (uint8_t)AGB0_REG_ACCEL_XOUT_H, buff, numbytes);
 
 	pagmt->acc.axes.x = ((buff[0] << 8) | (buff[1] & 0xFF));
 	pagmt->acc.axes.y = ((buff[2] << 8) | (buff[3] & 0xFF));
@@ -533,43 +806,24 @@ ICM_20948_Status_e  ICM_20948_get_agmt          ( ICM_20948_Device_t* pdev, ICM_
 
 	pagmt->tmp.val = ((buff[12] << 8) | (buff[13] & 0xFF));
 
-	// ToDo: get magnetometer readings
-	//  pagmt->mag.axes.x =
-	//  pagmt->mag.axes.y =
-	//  pagmt->mag.axes.z =
-
+	pagmt->magStat1 = buff[14];
+	pagmt->mag.axes.x = ((buff[16] << 8) | (buff[15] & 0xFF)); //Mag data is read little endian
+	pagmt->mag.axes.y = ((buff[18] << 8) | (buff[17] & 0xFF));
+	pagmt->mag.axes.z = ((buff[20] << 8) | (buff[19] & 0xFF));
+	pagmt->magStat2 = buff[22];
 
 	// Get settings to be able to compute scaled values
-	retval |= ICM_20948_set_bank( pdev, 2 ); 
-	ICM_20948_ACCEL_CONFIG_t acfg; 
-	retval |= ICM_20948_execute_r( pdev, (uint8_t)AGB2_REG_ACCEL_CONFIG, (uint8_t*)&acfg, 1*sizeof(acfg) );
-	pagmt->fss.a = acfg.ACCEL_FS_SEL; 	// Worth noting that without explicitly setting the FS range of the accelerometer it was showing the register value for +/- 2g but the reported values were actually scaled to the +/- 16g range 
-										// Wait a minute... now it seems like this problem actually comes from the digital low-pass filter. When enabled the value is 1/8 what it should be...
-	retval |= ICM_20948_set_bank( pdev, 2 ); 
+	retval |= ICM_20948_set_bank(pdev, 2);
+	ICM_20948_ACCEL_CONFIG_t acfg;
+	retval |= ICM_20948_execute_r(pdev, (uint8_t)AGB2_REG_ACCEL_CONFIG, (uint8_t *)&acfg, 1 * sizeof(acfg));
+	pagmt->fss.a = acfg.ACCEL_FS_SEL; // Worth noting that without explicitly setting the FS range of the accelerometer it was showing the register value for +/- 2g but the reported values were actually scaled to the +/- 16g range
+									  // Wait a minute... now it seems like this problem actually comes from the digital low-pass filter. When enabled the value is 1/8 what it should be...
+	retval |= ICM_20948_set_bank(pdev, 2);
 	ICM_20948_GYRO_CONFIG_1_t gcfg1;
-	retval |= ICM_20948_execute_r( pdev, (uint8_t)AGB2_REG_GYRO_CONFIG_1, (uint8_t*)&gcfg1, 1*sizeof(gcfg1) );
+	retval |= ICM_20948_execute_r(pdev, (uint8_t)AGB2_REG_GYRO_CONFIG_1, (uint8_t *)&gcfg1, 1 * sizeof(gcfg1));
 	pagmt->fss.g = gcfg1.GYRO_FS_SEL;
 	ICM_20948_ACCEL_CONFIG_2_t acfg2;
-	retval |= ICM_20948_execute_r( pdev, (uint8_t)AGB2_REG_ACCEL_CONFIG_2, (uint8_t*)&acfg2, 1*sizeof(acfg2) );
- 
+	retval |= ICM_20948_execute_r(pdev, (uint8_t)AGB2_REG_ACCEL_CONFIG_2, (uint8_t *)&acfg2, 1 * sizeof(acfg2));
+
 	return retval;
 }
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/src/util/ICM_20948_C.h b/src/util/ICM_20948_C.h
index 617de33..402ece2 100644
--- a/src/util/ICM_20948_C.h
+++ b/src/util/ICM_20948_C.h
@@ -3,8 +3,7 @@
 This is a C-compatible interface to the features presented by the ICM 20948 9-axis device
 The imementation of the interface is flexible
 
-*/ 
-
+*/
 
 #ifndef _ICM_20948_C_H_
 #define _ICM_20948_C_H_
@@ -14,123 +13,135 @@ The imementation of the interface is flexible
 #include <stddef.h>
 
 #include "ICM_20948_REGISTERS.h"
-#include "ICM_20948_ENUMERATIONS.h"	// This is to give users access to usable value definiitons
+#include "ICM_20948_ENUMERATIONS.h" // This is to give users access to usable value definiitons
 #include "AK09916_ENUMERATIONS.h"
 
 #ifdef __cplusplus
-extern "C" {
-#endif	/* __cplusplus */
-
-#define ICM_20948_I2C_ADDR_AD0 	0x68 // Or 0x69 when AD0 is high
-#define ICM_20948_I2C_ADDR_AD1	0x69 //
-#define ICM_20948_WHOAMI		0xEA
-
-#define MAG_AK09916_I2C_ADDR 	0x0C
-#define MAG_AK09916_WHO_AM_I	0x4809
-#define MAG_REG_WHO_AM_I		0x00
-
-typedef enum{
-	ICM_20948_Stat_Ok = 0x00,	// The only return code that means all is well
-	ICM_20948_Stat_Err,			// A general error
-	ICM_20948_Stat_NotImpl,		// Returned by virtual functions that are not implemented
-	ICM_20948_Stat_ParamErr,
-	ICM_20948_Stat_WrongID,
-	ICM_20948_Stat_InvalSensor,	// Tried to apply a function to a sensor that does not support it (e.g. DLPF to the temperature sensor)
-	ICM_20948_Stat_NoData,
-	ICM_20948_Stat_SensorNotSupported,
-
-	ICM_20948_Stat_NUM,
-	ICM_20948_Stat_Unknown,
-}ICM_20948_Status_e;
-
-typedef enum{
-	ICM_20948_Internal_Acc = (1 << 0),
-	ICM_20948_Internal_Gyr = (1 << 1),
-	ICM_20948_Internal_Mag = (1 << 2),
-	ICM_20948_Internal_Tmp = (1 << 3),
-	ICM_20948_Internal_Mst = (1 << 4), 		// I2C Master Ineternal
-}ICM_20948_InternalSensorID_bm;			// A bitmask of internal sensor IDs
-
-typedef union{
-  int16_t i16bit[3];
-  uint8_t u8bit[6];
-}ICM_20948_axis3bit16_t;
-
-typedef union{
-  int16_t i16bit;
-  uint8_t u8bit[2];
-}ICM_20948_axis1bit16_t;
-
-typedef struct{								
-	uint8_t				a			: 2;
-	uint8_t 			g			: 2;
-	uint8_t 			reserved_0	: 4;
-}ICM_20948_fss_t;							// Holds full-scale settings to be able to extract measurements with units
-
-typedef struct{								
-	uint8_t	a;
-	uint8_t	g;
-}ICM_20948_dlpcfg_t;						// Holds digital low pass filter settings. Members are type ICM_20948_ACCEL_CONFIG_DLPCFG_e
-
-typedef struct{
-	uint16_t a;
-	uint8_t g;
-}ICM_20948_smplrt_t;
-
-typedef struct{
-	uint8_t		I2C_MST_INT_EN		: 1;
-	uint8_t 	DMP_INT1_EN			: 1;
-	uint8_t		PLL_RDY_EN			: 1;
-	uint8_t 	WOM_INT_EN			: 1;
-	uint8_t		REG_WOF_EN			: 1;
-	uint8_t		RAW_DATA_0_RDY_EN	: 1;
-	uint8_t		FIFO_OVERFLOW_EN_4	: 1;
-	uint8_t		FIFO_OVERFLOW_EN_3	: 1;
-	uint8_t		FIFO_OVERFLOW_EN_2	: 1;
-	uint8_t		FIFO_OVERFLOW_EN_1	: 1;
-	uint8_t		FIFO_OVERFLOW_EN_0	: 1;
-	uint8_t 	FIFO_WM_EN_4		: 1;
-	uint8_t 	FIFO_WM_EN_3		: 1;
-	uint8_t 	FIFO_WM_EN_2		: 1;
-	uint8_t 	FIFO_WM_EN_1		: 1;
-	uint8_t 	FIFO_WM_EN_0		: 1;
-}ICM_20948_INT_enable_t;
-
-typedef union{
-	ICM_20948_axis3bit16_t	raw;
-	struct{
-		int16_t x;
-		int16_t y;
-		int16_t z;
-	}axes;
-}ICM_20948_axis3named_t;
-
-typedef struct{
-	ICM_20948_axis3named_t 	acc;
-	ICM_20948_axis3named_t 	gyr;
-	ICM_20948_axis3named_t 	mag;
-	union{
-		ICM_20948_axis1bit16_t	raw;
-		int16_t 				val;
-	}tmp;
-	ICM_20948_fss_t			fss;			// Full-scale range settings for this measurement
-}ICM_20948_AGMT_t;
-
-typedef struct{
-	ICM_20948_Status_e	(*write)( uint8_t regaddr, uint8_t* pdata, uint32_t len, void* user);
-	ICM_20948_Status_e	(*read)( uint8_t regaddr, uint8_t* pdata, uint32_t len, void* user);
-	// void				(*delay)(uint32_t ms);
-	void*				user;
-}ICM_20948_Serif_t;							// This is the vtable of serial interface functions
-extern const ICM_20948_Serif_t NullSerif;	// Here is a default for initialization (NULL)
-
-typedef struct{
-	const ICM_20948_Serif_t*	_serif;		// Pointer to the assigned Serif (Serial Interface) vtable
-}ICM_20948_Device_t;						// Definition of device struct type
-
-
-// Here's the list of what I want to be able to do:
-/*
+extern "C"
+{
+#endif /* __cplusplus */
+
+#define ICM_20948_I2C_ADDR_AD0 0x68 // Or 0x69 when AD0 is high
+#define ICM_20948_I2C_ADDR_AD1 0x69 //
+#define ICM_20948_WHOAMI 0xEA
+
+#define MAG_AK09916_I2C_ADDR 0x0C
+#define MAG_AK09916_WHO_AM_I 0x4809
+#define MAG_REG_WHO_AM_I 0x00
+
+	typedef enum
+	{
+		ICM_20948_Stat_Ok = 0x00, // The only return code that means all is well
+		ICM_20948_Stat_Err,		  // A general error
+		ICM_20948_Stat_NotImpl,   // Returned by virtual functions that are not implemented
+		ICM_20948_Stat_ParamErr,
+		ICM_20948_Stat_WrongID,
+		ICM_20948_Stat_InvalSensor, // Tried to apply a function to a sensor that does not support it (e.g. DLPF to the temperature sensor)
+		ICM_20948_Stat_NoData,
+		ICM_20948_Stat_SensorNotSupported,
+
+		ICM_20948_Stat_NUM,
+		ICM_20948_Stat_Unknown,
+	} ICM_20948_Status_e;
+
+	typedef enum
+	{
+		ICM_20948_Internal_Acc = (1 << 0),
+		ICM_20948_Internal_Gyr = (1 << 1),
+		ICM_20948_Internal_Mag = (1 << 2),
+		ICM_20948_Internal_Tmp = (1 << 3),
+		ICM_20948_Internal_Mst = (1 << 4), // I2C Master Ineternal
+	} ICM_20948_InternalSensorID_bm;	   // A bitmask of internal sensor IDs
+
+	typedef union {
+		int16_t i16bit[3];
+		uint8_t u8bit[6];
+	} ICM_20948_axis3bit16_t;
+
+	typedef union {
+		int16_t i16bit;
+		uint8_t u8bit[2];
+	} ICM_20948_axis1bit16_t;
+
+	typedef struct
+	{
+		uint8_t a : 2;
+		uint8_t g : 2;
+		uint8_t reserved_0 : 4;
+	} ICM_20948_fss_t; // Holds full-scale settings to be able to extract measurements with units
+
+	typedef struct
+	{
+		uint8_t a;
+		uint8_t g;
+	} ICM_20948_dlpcfg_t; // Holds digital low pass filter settings. Members are type ICM_20948_ACCEL_CONFIG_DLPCFG_e
+
+	typedef struct
+	{
+		uint16_t a;
+		uint8_t g;
+	} ICM_20948_smplrt_t;
+
+	typedef struct
+	{
+		uint8_t I2C_MST_INT_EN : 1;
+		uint8_t DMP_INT1_EN : 1;
+		uint8_t PLL_RDY_EN : 1;
+		uint8_t WOM_INT_EN : 1;
+		uint8_t REG_WOF_EN : 1;
+		uint8_t RAW_DATA_0_RDY_EN : 1;
+		uint8_t FIFO_OVERFLOW_EN_4 : 1;
+		uint8_t FIFO_OVERFLOW_EN_3 : 1;
+		uint8_t FIFO_OVERFLOW_EN_2 : 1;
+		uint8_t FIFO_OVERFLOW_EN_1 : 1;
+		uint8_t FIFO_OVERFLOW_EN_0 : 1;
+		uint8_t FIFO_WM_EN_4 : 1;
+		uint8_t FIFO_WM_EN_3 : 1;
+		uint8_t FIFO_WM_EN_2 : 1;
+		uint8_t FIFO_WM_EN_1 : 1;
+		uint8_t FIFO_WM_EN_0 : 1;
+	} ICM_20948_INT_enable_t;
+
+	typedef union {
+		ICM_20948_axis3bit16_t raw;
+		struct
+		{
+			int16_t x;
+			int16_t y;
+			int16_t z;
+		} axes;
+	} ICM_20948_axis3named_t;
+
+	typedef struct
+	{
+		ICM_20948_axis3named_t acc;
+		ICM_20948_axis3named_t gyr;
+		ICM_20948_axis3named_t mag;
+		union {
+			ICM_20948_axis1bit16_t raw;
+			int16_t val;
+		} tmp;
+		ICM_20948_fss_t fss; // Full-scale range settings for this measurement
+		uint8_t magStat1;
+		uint8_t magStat2;
+	} ICM_20948_AGMT_t;
+
+	typedef struct
+	{
+		ICM_20948_Status_e (*write)(uint8_t regaddr, uint8_t *pdata, uint32_t len, void *user);
+		ICM_20948_Status_e (*read)(uint8_t regaddr, uint8_t *pdata, uint32_t len, void *user);
+		// void				(*delay)(uint32_t ms);
+		void *user;
+	} ICM_20948_Serif_t;					  // This is the vtable of serial interface functions
+	extern const ICM_20948_Serif_t NullSerif; // Here is a default for initialization (NULL)
+
+	typedef struct
+	{
+		const ICM_20948_Serif_t *_serif; // Pointer to the assigned Serif (Serial Interface) vtable
+	} ICM_20948_Device_t;				 // Definition of device struct type
+
+	// Here's the list of what I want to be able to do:
+	/*
 
 perform a generic startup routine that sets most things in the optimal performance range
 Read / check against Who Am I
@@ -154,62 +165,54 @@ callbacks for the user to respond to interrupt events
 
 */
 
-// ICM_20948_Status_e ICM_20948_Startup( ICM_20948_Device_t* pdev ); // For the time being this performs a standardized startup routine
-
-
-ICM_20948_Status_e	ICM_20948_link_serif( ICM_20948_Device_t* pdev, const ICM_20948_Serif_t* s );	// Links a SERIF structure to the device
-
-// use the device's serif to perform a read or write
-ICM_20948_Status_e	ICM_20948_execute_r( ICM_20948_Device_t* pdev, uint8_t regaddr, uint8_t* pdata, uint32_t len ); // Executes a R or W witht he serif vt as long as the pointers are not null
-ICM_20948_Status_e	ICM_20948_execute_w( ICM_20948_Device_t* pdev, uint8_t regaddr, uint8_t* pdata, uint32_t len );
-
-
-// Single-shot I2C on Master IF
-ICM_20948_Status_e 	ICM_20948_i2c_master_slv4_txn( ICM_20948_Device_t* pdev, uint8_t addr, uint8_t reg, uint8_t* data, uint8_t len, bool Rw, bool send_reg_addr );
-ICM_20948_Status_e	ICM_20948_i2c_master_single_w( ICM_20948_Device_t* pdev, uint8_t addr, uint8_t reg, uint8_t* data );
-ICM_20948_Status_e	ICM_20948_i2c_master_single_r( ICM_20948_Device_t* pdev, uint8_t addr, uint8_t reg, uint8_t* data );
-
-
-// Device Level
-ICM_20948_Status_e	ICM_20948_set_bank			( ICM_20948_Device_t* pdev, uint8_t bank );									// Sets the bank
-ICM_20948_Status_e	ICM_20948_sw_reset			( ICM_20948_Device_t* pdev );												// Performs a SW reset
-ICM_20948_Status_e	ICM_20948_sleep				( ICM_20948_Device_t* pdev, bool on );										// Set sleep mode for the chip
-ICM_20948_Status_e	ICM_20948_low_power			( ICM_20948_Device_t* pdev, bool on );										// Set low power mode for the chip
-ICM_20948_Status_e	ICM_20948_set_clock_source	( ICM_20948_Device_t* pdev, ICM_20948_PWR_MGMT_1_CLKSEL_e source ); 		// Choose clock source
-ICM_20948_Status_e	ICM_20948_get_who_am_i		( ICM_20948_Device_t* pdev, uint8_t* whoami );								// Return whoami in out prarmeter
-ICM_20948_Status_e	ICM_20948_check_id			( ICM_20948_Device_t* pdev );												// Return 'ICM_20948_Stat_Ok' if whoami matches ICM_20948_WHOAMI
-ICM_20948_Status_e	ICM_20948_data_ready		( ICM_20948_Device_t* pdev );									// Returns 'Ok' if data is ready
+	// ICM_20948_Status_e ICM_20948_Startup( ICM_20948_Device_t* pdev ); // For the time being this performs a standardized startup routine
 
-// Interrupt Configuration
-ICM_20948_Status_e	ICM_20948_int_pin_cfg		( ICM_20948_Device_t* pdev, ICM_20948_INT_PIN_CFG_t* write, ICM_20948_INT_PIN_CFG_t* read );	// Set the INT pin configuration
-ICM_20948_Status_e	ICM_20948_int_enable 		( ICM_20948_Device_t* pdev, ICM_20948_INT_enable_t* write, ICM_20948_INT_enable_t* read );		// Write and or read the interrupt enable information. If non-null the write operation occurs before the read, so as to verify that the write was successful
+	ICM_20948_Status_e ICM_20948_link_serif(ICM_20948_Device_t *pdev, const ICM_20948_Serif_t *s); // Links a SERIF structure to the device
 
-// Internal Sensor Options
-ICM_20948_Status_e	ICM_20948_set_sample_mode	( ICM_20948_Device_t* pdev, ICM_20948_InternalSensorID_bm sensors, ICM_20948_LP_CONFIG_CYCLE_e mode );	// Use to set accel, gyro, and I2C master into cycled or continuous modes
-ICM_20948_Status_e	ICM_20948_set_full_scale 	( ICM_20948_Device_t* pdev, ICM_20948_InternalSensorID_bm sensors, ICM_20948_fss_t fss );
-ICM_20948_Status_e	ICM_20948_set_dlpf_cfg		( ICM_20948_Device_t* pdev, ICM_20948_InternalSensorID_bm sensors, ICM_20948_dlpcfg_t cfg );			
-ICM_20948_Status_e	ICM_20948_enable_dlpf		( ICM_20948_Device_t* pdev, ICM_20948_InternalSensorID_bm sensors, bool enable );
-ICM_20948_Status_e	ICM_20948_set_sample_rate	( ICM_20948_Device_t* pdev, ICM_20948_InternalSensorID_bm sensors, ICM_20948_smplrt_t smplrt );
+	// use the device's serif to perform a read or write
+	ICM_20948_Status_e ICM_20948_execute_r(ICM_20948_Device_t *pdev, uint8_t regaddr, uint8_t *pdata, uint32_t len); // Executes a R or W witht he serif vt as long as the pointers are not null
+	ICM_20948_Status_e ICM_20948_execute_w(ICM_20948_Device_t *pdev, uint8_t regaddr, uint8_t *pdata, uint32_t len);
 
-// Interface Things
-ICM_20948_Status_e	ICM_20948_i2c_master_passthrough 			( ICM_20948_Device_t* pdev, bool passthrough );
-ICM_20948_Status_e	ICM_20948_i2c_master_enable 				( ICM_20948_Device_t* pdev, bool enable );
-ICM_20948_Status_e	ICM_20948_i2c_master_configure_slave 		( ICM_20948_Device_t* pdev, uint8_t slave, uint8_t addr, uint8_t reg, uint8_t len, bool Rw, bool enable, bool data_only, bool grp, bool swap );
+	// Single-shot I2C on Master IF
+	ICM_20948_Status_e ICM_20948_i2c_master_slv4_txn(ICM_20948_Device_t *pdev, uint8_t addr, uint8_t reg, uint8_t *data, uint8_t len, bool Rw, bool send_reg_addr);
+	ICM_20948_Status_e ICM_20948_i2c_master_single_w(ICM_20948_Device_t *pdev, uint8_t addr, uint8_t reg, uint8_t *data);
+	ICM_20948_Status_e ICM_20948_i2c_master_single_r(ICM_20948_Device_t *pdev, uint8_t addr, uint8_t reg, uint8_t *data);
 
+	// Device Level
+	ICM_20948_Status_e ICM_20948_set_bank(ICM_20948_Device_t *pdev, uint8_t bank);								   // Sets the bank
+	ICM_20948_Status_e ICM_20948_sw_reset(ICM_20948_Device_t *pdev);											   // Performs a SW reset
+	ICM_20948_Status_e ICM_20948_sleep(ICM_20948_Device_t *pdev, bool on);										   // Set sleep mode for the chip
+	ICM_20948_Status_e ICM_20948_low_power(ICM_20948_Device_t *pdev, bool on);									   // Set low power mode for the chip
+	ICM_20948_Status_e ICM_20948_set_clock_source(ICM_20948_Device_t *pdev, ICM_20948_PWR_MGMT_1_CLKSEL_e source); // Choose clock source
+	ICM_20948_Status_e ICM_20948_get_who_am_i(ICM_20948_Device_t *pdev, uint8_t *whoami);						   // Return whoami in out prarmeter
+	ICM_20948_Status_e ICM_20948_check_id(ICM_20948_Device_t *pdev);											   // Return 'ICM_20948_Stat_Ok' if whoami matches ICM_20948_WHOAMI
+	ICM_20948_Status_e ICM_20948_data_ready(ICM_20948_Device_t *pdev);											   // Returns 'Ok' if data is ready
 
+	// Interrupt Configuration
+	ICM_20948_Status_e ICM_20948_int_pin_cfg(ICM_20948_Device_t *pdev, ICM_20948_INT_PIN_CFG_t *write, ICM_20948_INT_PIN_CFG_t *read); // Set the INT pin configuration
+	ICM_20948_Status_e ICM_20948_int_enable(ICM_20948_Device_t *pdev, ICM_20948_INT_enable_t *write, ICM_20948_INT_enable_t *read);	// Write and or read the interrupt enable information. If non-null the write operation occurs before the read, so as to verify that the write was successful
 
-// Higher Level
-ICM_20948_Status_e  ICM_20948_get_agmt          ( ICM_20948_Device_t* pdev, ICM_20948_AGMT_t* p );
+	// Internal Sensor Options
+	ICM_20948_Status_e ICM_20948_set_sample_mode(ICM_20948_Device_t *pdev, ICM_20948_InternalSensorID_bm sensors, ICM_20948_LP_CONFIG_CYCLE_e mode); // Use to set accel, gyro, and I2C master into cycled or continuous modes
+	ICM_20948_Status_e ICM_20948_set_full_scale(ICM_20948_Device_t *pdev, ICM_20948_InternalSensorID_bm sensors, ICM_20948_fss_t fss);
+	ICM_20948_Status_e ICM_20948_set_dlpf_cfg(ICM_20948_Device_t *pdev, ICM_20948_InternalSensorID_bm sensors, ICM_20948_dlpcfg_t cfg);
+	ICM_20948_Status_e ICM_20948_enable_dlpf(ICM_20948_Device_t *pdev, ICM_20948_InternalSensorID_bm sensors, bool enable);
+	ICM_20948_Status_e ICM_20948_set_sample_rate(ICM_20948_Device_t *pdev, ICM_20948_InternalSensorID_bm sensors, ICM_20948_smplrt_t smplrt);
 
+	// Interface Things
+	ICM_20948_Status_e ICM_20948_i2c_master_passthrough(ICM_20948_Device_t *pdev, bool passthrough);
+	ICM_20948_Status_e ICM_20948_i2c_master_enable(ICM_20948_Device_t *pdev, bool enable);
+	ICM_20948_Status_e ICM_20948_i2c_master_reset(ICM_20948_Device_t *pdev);
+	ICM_20948_Status_e ICM_20948_i2c_master_configure_slave(ICM_20948_Device_t *pdev, uint8_t slave, uint8_t addr, uint8_t reg, uint8_t len, bool Rw, bool enable, bool data_only, bool grp, bool swap);
 
+	// Higher Level
+	ICM_20948_Status_e ICM_20948_get_agmt(ICM_20948_Device_t *pdev, ICM_20948_AGMT_t *p);
 
+	ICM_20948_Status_e ICM_20948_get_agmt(ICM_20948_Device_t *pdev, ICM_20948_AGMT_t *p);
 
+	// ToDo:
 
-
-
-// ToDo:
-
-/* 
+	/* 
 	Want to access magnetometer throught the I2C master interface...
 
   // If using the I2C master to read from the magnetometer
@@ -223,15 +226,8 @@ ICM_20948_Status_e  ICM_20948_get_agmt          ( ICM_20948_Device_t* pdev, ICM_
   myICM.setSampleMode( (ICM_20948_Internal_Mst | ICM_20948_Internal_Gyr), ICM_20948_Sample_Mode_Cycled ); // options: ICM_20948_Sample_Mode_Continuous or ICM_20948_Sample_Mode_Cycled
 */
 
-
-
-
-
-
-
-
 #ifdef __cplusplus
 }
-#endif	/* __cplusplus */
+#endif /* __cplusplus */
 
-#endif 	/* _ICM_20948_C_H_ */
\ No newline at end of file
+#endif /* _ICM_20948_C_H_ */
\ No newline at end of file