STM-ify I2C and get just enough communication working to configure the devices

Drivers/EM7180/Inc/em7180.h

@@ -101,6 +101,8 @@ typedef struct em7180_s
 {
 	I2C_HandleTypeDef *hi2c;
 	lsm6dsm_t *lsm6dsm;
+	lis2mdl_t *lis2mdl;
+	lps22hb_t *lps22hb;
 	uint16_t acc_fs;
 	uint16_t gyro_fs;
 	uint16_t mag_fs;
@@ -112,13 +114,15 @@ typedef struct em7180_s
 } em7180_t;
 
 /* Function Prototypes */
-void em7180_init(em7180_t *em7180, lsm6dsm_t *lsm6dsm, I2C_HandleTypeDef *hi2c1,
-                 uint16_t acc_fs, uint16_t gyro_fs, uint16_t mag_fs,
-                 uint8_t q_rate_div, uint8_t mag_rate, uint8_t acc_rate,
-                 uint8_t gyro_rate, uint8_t baro_rate);
+void em7180_init(em7180_t *em7180, I2C_HandleTypeDef *hi2c, lsm6dsm_t *lsm6dsm,
+                 lis2mdl_t *lis2mdl, lps22hb_t *lps22hb, uint16_t acc_fs,
+                 uint16_t gyro_fs, uint16_t mag_fs, uint8_t q_rate_div,
+                 uint8_t mag_rate, uint8_t acc_rate, uint8_t gyro_rate,
+                 uint8_t baro_rate);
 void em7180_config(em7180_t *em7180);
-void em7180_gyro_set_fs(uint16_t gyro_fs);
-void em7180_mag_acc_set_fs(uint16_t mag_fs, uint16_t acc_fs);
-void em7180_set_integer_param(uint8_t param, uint32_t param_val);
+void em7180_gyro_set_fs(em7180_t *em7180, uint16_t gyro_fs);
+void em7180_mag_acc_set_fs(em7180_t *em7180, uint16_t mag_fs, uint16_t acc_fs);
+void em7180_set_integer_param(em7180_t *em7180, uint8_t param,
+                              uint32_t param_val);
 
 #endif /* EM7180_H_ */

Drivers/EM7180/Inc/em7180_common.h

@@ -12,11 +12,42 @@
 
 /* Includes */
 #include <stdint.h>
+#include "i2c.h"
+
+/* Definitions */
+#define return_if_fail(cond) \
+	if(!(cond))              \
+	{                        \
+		return;              \
+	}
+#define return_val_if_fail(cond, val) \
+	if(!(cond))                       \
+	{                                 \
+		return (val);                 \
+	}
 
 /* Function Prototypes */
-void lsm6dsm_write_byte(uint8_t address, uint8_t subAddress, uint8_t data);
-uint8_t lsm6dsm_read_byte(uint8_t address, uint8_t subAddress);
-void lsm6dsm_read(uint8_t address, uint8_t subAddress, uint8_t count,
-                  uint8_t *dest);
+inline __attribute__((always_inline))  HAL_StatusTypeDef i2c_write_byte(
+    I2C_HandleTypeDef *hi2c, uint16_t addr, uint16_t sub_addr, uint8_t data)
+{
+	return HAL_I2C_Mem_Write(hi2c, addr << 1, sub_addr, 1, &data, 1,
+	                         HAL_MAX_DELAY);
+}
+inline __attribute__((always_inline))  uint8_t i2c_read_byte(
+    I2C_HandleTypeDef *hi2c, uint16_t addr, uint16_t sub_addr)
+{
+	uint8_t temp;
+
+	HAL_I2C_Mem_Read(hi2c, addr << 1, sub_addr, 1, &temp, 1, HAL_MAX_DELAY);
+
+	return temp;
+}
+inline __attribute__((always_inline))  HAL_StatusTypeDef i2c_read(
+    I2C_HandleTypeDef *hi2c, uint16_t addr, uint16_t sub_addr, uint8_t *data,
+    uint16_t len)
+{
+	return HAL_I2C_Mem_Read(hi2c, addr << 1, sub_addr, 1, data, len,
+	                        HAL_MAX_DELAY);
+}
 
 #endif /* EM7180_COMMON_h */

Drivers/EM7180/Inc/lis2mdl.h

@@ -61,12 +61,11 @@
 /* Data Structures */
 typedef struct lis2mdl_s
 {
-	I2C_HandleTypeDef *hi2c;
 	uint8_t m_odr;
 } lis2mdl_t;
 
 /* Function Prototypes */
-void lis2mdl_init(lis2mdl_t *lis2mdl, I2C_HandleTypeDef *hi2c, uint8_t m_odr);
-void lis2mdl_config(lis2mdl_t *lis2mdl);
+void lis2mdl_init(lis2mdl_t *lis2mdl, uint8_t m_odr);
+void lis2mdl_config(lis2mdl_t *lis2mdl, I2C_HandleTypeDef *hi2c);
 
 #endif

Drivers/EM7180/Inc/lps22hb.h

@@ -65,7 +65,7 @@ typedef struct lps22hb_s
 } lps22hb_t;
 
 /* Function Prototypes */
-void lps22hb_init(lps22hb_t *lps22hb, I2C_HandleTypeDef *hi2c, uint8_t p_odr);
-void lps22hb_config(lps22hb_t *lps22hb);
+void lps22hb_init(lps22hb_t *lps22hb, uint8_t p_odr);
+void lps22hb_config(lps22hb_t *lps22hb, I2C_HandleTypeDef *hi2c);
 
 #endif

Drivers/EM7180/Inc/lsm6dsm.h

@@ -170,8 +170,8 @@ typedef struct lsm6dsm_s
 } lsm6dsm_t;
 
 /* Function Prototypes */
-void lsm6dsm_init(lsm6dsm_t *lsm6dsm, I2C_HandleTypeDef *hi2c, uint8_t ascale,
-                  uint8_t gscale, uint8_t a_odr, uint8_t g_odr);
-void lsm6dsm_config(lsm6dsm_t *lsm6dsm);
+void lsm6dsm_init(lsm6dsm_t *lsm6dsm, uint8_t ascale, uint8_t gscale,
+                  uint8_t a_odr, uint8_t g_odr);
+void lsm6dsm_config(lsm6dsm_t *lsm6dsm, I2C_HandleTypeDef *hi2c);
 
 #endif

Drivers/EM7180/Src/em7180.c

@@ -25,31 +25,23 @@
 /* Private Global Variables */
 
 /* Function Prototypes */
-static void em7180_float_to_bytes(float param_val, uint8_t *buf);
-static void m24512dfm_write_byte(uint8_t device_address, uint8_t data_address1,
-                                 uint8_t data_address2, uint8_t data);
-static void m24512dfm_write(uint8_t device_address, uint8_t data_address1,
-                            uint8_t data_address2, uint8_t count, uint8_t *dest);
-static uint8_t m24512dfm_read_byte(uint8_t device_address,
-                                   uint8_t data_address1, uint8_t data_address2);
-static void m24512dfm_read(uint8_t device_address, uint8_t data_address1,
-                           uint8_t data_address2, uint8_t count, uint8_t *dest);
-static uint8_t em7180_read_byte(uint8_t address, uint8_t subAddress);
-static void em7180_read(uint8_t address, uint8_t subAddress, uint8_t count,
-                        uint8_t *dest);
+static void em7180_passthrough(em7180_t *em7180);
+static float uint32_reg_to_float(uint8_t *buf);
+static void float_to_bytes(float param_val, uint8_t *buf);
 
 /* Function Definitions */
-void em7180_init(em7180_t *em7180, lsm6dsm_t *lsm6dsm, I2C_HandleTypeDef *hi2c1,
-                 uint16_t acc_fs, uint16_t gyro_fs, uint16_t mag_fs,
-                 uint8_t q_rate_div, uint8_t mag_rate, uint8_t acc_rate,
-                 uint8_t gyro_rate, uint8_t baro_rate)
+void em7180_init(em7180_t *em7180, I2C_HandleTypeDef *hi2c, lsm6dsm_t *lsm6dsm,
+                 lis2mdl_t *lis2mdl, lps22hb_t *lps22hb, uint16_t acc_fs,
+                 uint16_t gyro_fs, uint16_t mag_fs, uint8_t q_rate_div,
+                 uint8_t mag_rate, uint8_t acc_rate, uint8_t gyro_rate,
+                 uint8_t baro_rate)
 {
-	if(!em7180)
-	{
-		return;
-	}
+	return_if_fail(em7180);
 
+	em7180->hi2c = hi2c;
 	em7180->lsm6dsm = lsm6dsm;
+	em7180->lis2mdl = lis2mdl;
+	em7180->lps22hb = lps22hb;
 	em7180->acc_fs = acc_fs;
 	em7180->gyro_fs = gyro_fs;
 	em7180->mag_fs = mag_fs;
@@ -59,13 +51,27 @@ void em7180_init(em7180_t *em7180, lsm6dsm_t *lsm6dsm, I2C_HandleTypeDef *hi2c1,
 	em7180->gyro_rate = gyro_rate;
 	em7180->baro_rate = baro_rate;
 
+	/* configure the EM7180 */
 	em7180_config(em7180);
+	/* enter passthrough mode */
+	em7180_passthrough(em7180);
+	/* and configure the devices on the slave bus */
+	if(em7180->lsm6dsm)
+	{
+		lsm6dsm_config(em7180->lsm6dsm, em7180->hi2c);
+	}
+	if(em7180->lis2mdl)
+	{
+		lis2mdl_config(em7180->lis2mdl, em7180->hi2c);
+	}
+	if(em7180->lps22hb)
+	{
+		lps22hb_config(em7180->lps22hb, em7180->hi2c);
+	}
 }
 
 void em7180_config(em7180_t *em7180)
 {
-	uint8_t param[4];
-	uint8_t param_xfer;
 	uint8_t runStatus;
 	uint8_t algoStatus;
 	uint8_t passthruStatus;
@@ -73,50 +79,54 @@ void em7180_config(em7180_t *em7180)
 	uint8_t sensorStatus;
 
 	// Enter EM7180 initialized state
-	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_HostControl, 0x00); // set SENtral in initialized state to configure registers
-	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_PassThruControl, 0x00); // make sure pass through mode is off
-	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_HostControl, 0x01); // Force initialize
-	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_HostControl, 0x00); // set SENtral in initialized state to configure registers
+	i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_HostControl, 0x00); // set SENtral in initialized state to configure registers
+	i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_PassThruControl, 0x00); // make sure pass through mode is off
+	i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_HostControl, 0x01); // Force initialize
+	i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_HostControl, 0x00); // set SENtral in initialized state to configure registers
 
 	/* Legacy MPU6250 stuff, it seems
 	 // Setup LPF bandwidth (BEFORE setting ODR's)
-	 lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ACC_LPF_BW, accBW); // accBW = 3 = 41Hz
-	 lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_GYRO_LPF_BW, gyroBW); // gyroBW = 3 = 41Hz */
+	 i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_ACC_LPF_BW, accBW); // accBW = 3 = 41Hz
+	 i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_GYRO_LPF_BW, gyroBW); // gyroBW = 3 = 41Hz */
 	// Set accel/gyro/mag desired ODR rates
-	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_QRateDivisor, em7180->q_rate_div); // quat rate = gyroRt/(1 QRTDiv)
-	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_MagRate, em7180->mag_rate); // 0x64 = 100 Hz
-	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AccelRate, em7180->acc_rate); // 200/10 Hz, 0x14 = 200 Hz
-	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_GyroRate, em7180->gyro_rate); // 200/10 Hz, 0x14 = 200 Hz
-	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_BaroRate,
+	i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_QRateDivisor,
+	               em7180->q_rate_div); // quat rate = gyroRt/(1 QRTDiv)
+	i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_MagRate,
+	               em7180->mag_rate); // 0x64 = 100 Hz
+	i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_AccelRate,
+	               em7180->acc_rate); // 200/10 Hz, 0x14 = 200 Hz
+	i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_GyroRate,
+	               em7180->gyro_rate); // 200/10 Hz, 0x14 = 200 Hz
+	i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_BaroRate,
 	               0x80 | em7180->baro_rate); // set enable bit and set Baro rate to 25 Hz, rate = baroRt/2, 0x32 = 25 Hz
 
 	// Configure operating mode
-	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // read scale sensor data
+	i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // read scale sensor data
 	// Enable interrupt to host upon certain events
 	// choose host interrupts when any sensor updated (0x40), new gyro data (0x20), new accel data (0x10),
 	// new mag data (0x08), quaternions updated (0x04), an error occurs (0x02), or the SENtral needs to be reset(0x01)
-	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_EnableEvents, 0x07);
+	i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_EnableEvents, 0x07);
 	// Enable EM7180 run mode
-	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_HostControl, 0x01); // set SENtral in normal run mode
+	i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_HostControl, 0x01); // set SENtral in normal run mode
 	HAL_Delay(100);
 
 	// EM7180 parameter adjustments
 	/* Serial.println("Beginning Parameter Adjustments"); */
 
 	// Disable stillness mode for balancing robot application
-	em7180_set_integer_param(0x49, 0x00);
+	em7180_set_integer_param(em7180, 0x49, 0x00);
 
 	// Write desired sensor full scale ranges to the EM7180
-	em7180_mag_acc_set_fs(em7180->mag_fs, em7180->acc_fs); // 1000 uT == 0x3E8, 8 g == 0x08
-	em7180_gyro_set_fs(em7180->gyro_fs); // 2000 dps == 0x7D0
+	em7180_mag_acc_set_fs(em7180, em7180->mag_fs, em7180->acc_fs); // 1000 uT == 0x3E8, 8 g == 0x08
+	em7180_gyro_set_fs(em7180, em7180->gyro_fs); // 2000 dps == 0x7D0
 
 	// Read EM7180 status
-	runStatus = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_RunStatus);
+	runStatus = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_RunStatus);
 	if(runStatus & 0x01)
 	{
 		/* Serial.println(" EM7180 run status = normal mode"); */
 	}
-	algoStatus = lsm6dsm_read_byte(EM7180_ADDRESS,
+	algoStatus = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS,
 	EM7180_AlgorithmStatus);
 	if(algoStatus & 0x01)
 	{
@@ -142,13 +152,14 @@ void em7180_config(em7180_t *em7180)
 	{
 		/* Serial.println(" EM7180 unreliable sensor data"); */
 	}
-	passthruStatus = lsm6dsm_read_byte(EM7180_ADDRESS,
+	passthruStatus = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS,
 	EM7180_PassThruStatus);
 	if(passthruStatus & 0x01)
 	{
 		/* Serial.print(" EM7180 in passthru mode!"); */
 	}
-	eventStatus = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_EventStatus);
+	eventStatus = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS,
+	EM7180_EventStatus);
 	if(eventStatus & 0x01)
 	{
 		/* Serial.println(" EM7180 CPU reset"); */
@@ -177,7 +188,8 @@ void em7180_config(em7180_t *em7180)
 	HAL_Delay(1000); // give some time to read the screen
 
 	// Check sensor status
-	sensorStatus = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SensorStatus);
+	sensorStatus = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS,
+	EM7180_SensorStatus);
 	/* Serial.print(" EM7180 sensor status = "); */
 	/* Serial.println(sensorStatus); */
 	if(sensorStatus & 0x01)
@@ -206,47 +218,49 @@ void em7180_config(em7180_t *em7180)
 	}
 
 	/* Serial.print("Actual MagRate = "); */
-	/* Serial.print(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ActualMagRate)); */
+	/* Serial.print(i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_ActualMagRate)); */
 	/* Serial.println(" Hz"); */
 	/* Serial.print("Actual AccelRate = "); */
-	/* Serial.print(10 * lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ActualAccelRate)); */
+	/* Serial.print(10 * i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_ActualAccelRate)); */
 	/* Serial.println(" Hz"); */
 	/* Serial.print("Actual GyroRate = "); */
-	/* Serial.print(10 * lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ActualGyroRate)); */
+	/* Serial.print(10 * i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_ActualGyroRate)); */
 	/* Serial.println(" Hz"); */
 	/* Serial.print("Actual BaroRate = "); */
-	/* Serial.print(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ActualBaroRate)); */
+	/* Serial.print(i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_ActualBaroRate)); */
 	/* Serial.println(" Hz"); */
 }
 
-void em7180_chip_id_get()
+#if(0)
+void em7180_chip_id_get(em7180_t *em7180)
 {
 	// Read SENtral device information
-	uint16_t ROM1 = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ROMVersion1);
-	uint16_t ROM2 = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ROMVersion2);
+	uint16_t ROM1 = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_ROMVersion1);
+	uint16_t ROM2 = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_ROMVersion2);
 	/* Serial.print("EM7180 ROM Version: 0x"); */
 	/* Serial.print(ROM1, HEX); */
 	/* Serial.println(ROM2, HEX); */
 	/* Serial.println("Should be: 0xE609"); */
-	uint16_t RAM1 = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_RAMVersion1);
-	uint16_t RAM2 = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_RAMVersion2);
+	uint16_t RAM1 = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_RAMVersion1);
+	uint16_t RAM2 = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_RAMVersion2);
 	/* Serial.print("EM7180 RAM Version: 0x"); */
 	/* Serial.print(RAM1); */
 	/* Serial.println(RAM2); */
-	uint8_t PID = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ProductID);
+	uint8_t PID = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_ProductID);
 	/* Serial.print("EM7180 ProductID: 0x"); */
 	/* Serial.print(PID, HEX); */
 	/* Serial.println(" Should be: 0x80"); */
-	uint8_t RID = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_RevisionID);
+	uint8_t RID = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_RevisionID);
 	/* Serial.print("EM7180 RevisionID: 0x"); */
 	/* Serial.print(RID, HEX); */
 	/* Serial.println(" Should be: 0x02"); */
 }
+#endif
 
-void em7180_load_fw_from_eeprom()
+void em7180_load_fw_from_eeprom(em7180_t *em7180)
 {
 	// Check which sensors can be detected by the EM7180
-	uint8_t featureflag = lsm6dsm_read_byte(EM7180_ADDRESS,
+	uint8_t featureflag = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS,
 	EM7180_FeatureFlags);
 	if(featureflag & 0x01)
 	{
@@ -276,52 +290,55 @@ void em7180_load_fw_from_eeprom()
 	HAL_Delay(1000); // give some time to read the screen
 
 	// Check SENtral status, make sure EEPROM upload of firmware was accomplished
-	uint8_t STAT = (lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus)
+	uint8_t STAT = (i2c_read_byte(em7180->hi2c, EM7180_ADDRESS,
+	EM7180_SentralStatus)
 	                & 0x01);
-	if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x01)
+	if(i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_SentralStatus) & 0x01)
 	{
 		/* Serial.println("EEPROM detected on the sensor bus!"); */
 	}
-	if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x02)
+	if(i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_SentralStatus) & 0x02)
 	{
 		/* Serial.println("EEPROM uploaded config file!"); */
 	}
-	if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x04)
+	if(i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_SentralStatus) & 0x04)
 	{
 		/* Serial.println("EEPROM CRC incorrect!"); */
 	}
-	if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x08)
+	if(i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_SentralStatus) & 0x08)
 	{
 		/* Serial.println("EM7180 in initialized state!"); */
 	}
-	if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x10)
+	if(i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_SentralStatus) & 0x10)
 	{
 		/* Serial.println("No EEPROM detected!"); */
 	}
 	int count = 0;
 	while(!STAT)
 	{
-		lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ResetRequest, 0x01);
+		i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_ResetRequest, 0x01);
 		HAL_Delay(500);
 		count++;
-		STAT = (lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x01);
-		if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x01)
+		STAT = (i2c_read_byte(em7180->hi2c, EM7180_ADDRESS,
+		EM7180_SentralStatus)
+		        & 0x01);
+		if(i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_SentralStatus) & 0x01)
 		{
 			/* Serial.println("EEPROM detected on the sensor bus!"); */
 		}
-		if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x02)
+		if(i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_SentralStatus) & 0x02)
 		{
 			/* Serial.println("EEPROM uploaded config file!"); */
 		}
-		if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x04)
+		if(i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_SentralStatus) & 0x04)
 		{
 			/* Serial.println("EEPROM CRC incorrect!"); */
 		}
-		if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x08)
+		if(i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_SentralStatus) & 0x08)
 		{
 			/* Serial.println("EM7180 in initialized state!"); */
 		}
-		if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x10)
+		if(i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_SentralStatus) & 0x10)
 		{
 			/* Serial.println("No EEPROM detected!"); */
 		}
@@ -331,117 +348,85 @@ void em7180_load_fw_from_eeprom()
 		}
 	}
 
-	if(!(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x04))
+	if(!(i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_SentralStatus)
+	    & 0x04))
 	{
 		/* Serial.println("EEPROM upload successful!"); */
 	}
 }
 
-uint8_t em7180_status()
+uint8_t em7180_status(em7180_t *em7180)
 {
 	// Check event status register, way to check data ready by polling rather than interrupt
-	uint8_t c = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_EventStatus); // reading clears the register and interrupt
-	return c;
-}
+	uint8_t c = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_EventStatus); // reading clears the register and interrupt
 
-uint8_t em7180_errors()
-{
-	uint8_t c = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ErrorRegister); // check error register
 	return c;
 }
 
-float em7180_uint32_reg_to_float(uint8_t *buf)
+uint8_t em7180_errors(em7180_t *em7180)
 {
-	union
-	{
-		uint32_t ui32;
-		float f;
-	} u;
-
-	u.ui32 = (((uint32_t) buf[0]) + (((uint32_t) buf[1]) << 8)
-	          + (((uint32_t) buf[2]) << 16) + (((uint32_t) buf[3]) << 24));
-	return u.f;
-}
+	uint8_t c = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS,
+	EM7180_ErrorRegister); // check error register
 
-float em7180_int32_reg_to_float(uint8_t *buf)
-{
-	union
-	{
-		int32_t i32;
-		float f;
-	} u;
-
-	u.i32 = (((int32_t) buf[0]) + (((int32_t) buf[1]) << 8)
-	         + (((int32_t) buf[2]) << 16) + (((int32_t) buf[3]) << 24));
-	return u.f;
-}
-
-static void em7180_float_to_bytes(float param_val, uint8_t *buf)
-{
-	union
-	{
-		float f;
-		uint8_t u8[sizeof(float)];
-	} u;
-
-	u.f = param_val;
-	for(uint8_t i = 0; i < sizeof(float); i++)
-	{
-		buf[i] = u.u8[i];
-	}
-	// Convert to LITTLE ENDIAN
-	/* FIXME: What the hell? */
-	for(uint8_t i = 0; i < sizeof(float); i++)
-	{
-		buf[i] = buf[(sizeof(float) - 1) - i];
-	}
+	return c;
 }
 
-void em7180_gyro_set_fs(uint16_t gyro_fs)
+void em7180_gyro_set_fs(em7180_t *em7180, uint16_t gyro_fs)
 {
 	uint8_t bytes[4], STAT;
 	bytes[0] = gyro_fs & (0xFF);
 	bytes[1] = (gyro_fs >> 8) & (0xFF);
 	bytes[2] = 0x00;
 	bytes[3] = 0x00;
-	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte0, bytes[0]); // Gyro LSB
-	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte1, bytes[1]); // Gyro MSB
-	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte2, bytes[2]); // Unused
-	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte3, bytes[3]); // Unused
-	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0xCB); // Parameter 75; 0xCB is 75 decimal with the MSB set high to indicate a parameter write process
-	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); // Request parameter transfer procedure
-	STAT = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge); // Check the parameter acknowledge register and loop until the result matches parameter request byte
+	i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_LoadParamByte0,
+	               bytes[0]); // Gyro LSB
+	i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_LoadParamByte1,
+	               bytes[1]); // Gyro MSB
+	i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_LoadParamByte2,
+	               bytes[2]); // Unused
+	i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_LoadParamByte3,
+	               bytes[3]); // Unused
+	i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_ParamRequest, 0xCB); // Parameter 75; 0xCB is 75 decimal with the MSB set high to indicate a parameter write process
+	i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); // Request parameter transfer procedure
+	STAT = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_ParamAcknowledge); // Check the parameter acknowledge register and loop until the result matches parameter request byte
 	while(!(STAT == 0xCB))
 	{
-		STAT = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
+		STAT = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS,
+		EM7180_ParamAcknowledge);
 	}
-	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0x00); // Parameter request = 0 to end parameter transfer process
-	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // Re-start algorithm
+	i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_ParamRequest, 0x00); // Parameter request = 0 to end parameter transfer process
+	i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // Re-start algorithm
 }
 
-void em7180_mag_acc_set_fs(uint16_t mag_fs, uint16_t acc_fs)
+void em7180_mag_acc_set_fs(em7180_t *em7180, uint16_t mag_fs, uint16_t acc_fs)
 {
 	uint8_t bytes[4], STAT;
 	bytes[0] = mag_fs & (0xFF);
 	bytes[1] = (mag_fs >> 8) & (0xFF);
 	bytes[2] = acc_fs & (0xFF);
 	bytes[3] = (acc_fs >> 8) & (0xFF);
-	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte0, bytes[0]); // Mag LSB
-	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte1, bytes[1]); // Mag MSB
-	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte2, bytes[2]); // Acc LSB
-	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte3, bytes[3]); // Acc MSB
-	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0xCA); // Parameter 74; 0xCA is 74 decimal with the MSB set high to indicate a paramter write processs
-	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); // Request parameter transfer procedure
-	STAT = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge); // Check the parameter acknowledge register and loop until the result matches parameter request byte
+	i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_LoadParamByte0,
+	               bytes[0]); // Mag LSB
+	i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_LoadParamByte1,
+	               bytes[1]); // Mag MSB
+	i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_LoadParamByte2,
+	               bytes[2]); // Acc LSB
+	i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_LoadParamByte3,
+	               bytes[3]); // Acc MSB
+	i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_ParamRequest, 0xCA); // Parameter 74; 0xCA is 74 decimal with the MSB set high to indicate a paramter write processs
+	i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); // Request parameter transfer procedure
+	STAT = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_ParamAcknowledge); // Check the parameter acknowledge register and loop until the result matches parameter request byte
 	while(!(STAT == 0xCA))
 	{
-		STAT = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
+		STAT = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS,
+		EM7180_ParamAcknowledge);
 	}
-	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0x00); // Parameter request = 0 to end parameter transfer process
-	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // Re-start algorithm
+	i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_ParamRequest, 0x00); // Parameter request = 0 to end parameter transfer process
+	i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // Re-start algorithm
 }
 
-void em7180_set_integer_param(uint8_t param, uint32_t param_val)
+void em7180_set_integer_param(em7180_t *em7180, uint8_t param,
+                              uint32_t param_val)
 {
 	uint8_t bytes[4], STAT;
 	bytes[0] = param_val & (0xFF);
@@ -449,77 +434,90 @@ void em7180_set_integer_param(uint8_t param, uint32_t param_val)
 	bytes[2] = (param_val >> 16) & (0xFF);
 	bytes[3] = (param_val >> 24) & (0xFF);
 	param = param | 0x80; // Parameter is the decimal value with the MSB set high to indicate a paramter write processs
-	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte0, bytes[0]); // Param LSB
-	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte1, bytes[1]);
-	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte2, bytes[2]);
-	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte3, bytes[3]); // Param MSB
-	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, param);
-	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); // Request parameter transfer procedure
-	STAT = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge); // Check the parameter acknowledge register and loop until the result matches parameter request byte
+	i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_LoadParamByte0,
+	               bytes[0]); // Param LSB
+	i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_LoadParamByte1,
+	               bytes[1]);
+	i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_LoadParamByte2,
+	               bytes[2]);
+	i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_LoadParamByte3,
+	               bytes[3]); // Param MSB
+	i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_ParamRequest, param);
+	i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); // Request parameter transfer procedure
+	STAT = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_ParamAcknowledge); // Check the parameter acknowledge register and loop until the result matches parameter request byte
 	while(!(STAT == param))
 	{
-		STAT = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
+		STAT = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS,
+		EM7180_ParamAcknowledge);
 	}
-	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0x00); // Parameter request = 0 to end parameter transfer process
-	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // Re-start algorithm
+	i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_ParamRequest, 0x00); // Parameter request = 0 to end parameter transfer process
+	i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // Re-start algorithm
 }
 
-void em7180_param_set_float(uint8_t param, float param_val)
+void em7180_param_set_float(em7180_t *em7180, uint8_t param, float param_val)
 {
 	uint8_t bytes[4], STAT;
-	em7180_float_to_bytes(param_val, &bytes[0]);
+	float_to_bytes(param_val, &bytes[0]);
 	param = param | 0x80; // Parameter is the decimal value with the MSB set high to indicate a paramter write processs
-	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte0, bytes[0]); // Param LSB
-	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte1, bytes[1]);
-	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte2, bytes[2]);
-	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte3, bytes[3]); // Param MSB
-	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, param);
-	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); // Request parameter transfer procedure
-	STAT = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge); // Check the parameter acknowledge register and loop until the result matches parameter request byte
+	i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_LoadParamByte0,
+	               bytes[0]); // Param LSB
+	i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_LoadParamByte1,
+	               bytes[1]);
+	i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_LoadParamByte2,
+	               bytes[2]);
+	i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_LoadParamByte3,
+	               bytes[3]); // Param MSB
+	i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_ParamRequest, param);
+	i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); // Request parameter transfer procedure
+	STAT = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_ParamAcknowledge); // Check the parameter acknowledge register and loop until the result matches parameter request byte
 	while(!(STAT == param))
 	{
-		STAT = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
+		STAT = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS,
+		EM7180_ParamAcknowledge);
 	}
-	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0x00); // Parameter request = 0 to end parameter transfer process
-	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // Re-start algorithm
+	i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_ParamRequest, 0x00); // Parameter request = 0 to end parameter transfer process
+	i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // Re-start algorithm
 }
 
-void em7180_quatdata_get(float *destination)
+void em7180_quatdata_get(em7180_t *em7180, float *destination)
 {
-	uint8_t rawData[16];  // x/y/z quaternion register data stored here
-	em7180_read(EM7180_ADDRESS, EM7180_QX, 16, &rawData[0]); // Read the sixteen raw data registers into data array
-	destination[1] = uint32_reg_to_float(&rawData[0]);
-	destination[2] = uint32_reg_to_float(&rawData[4]);
-	destination[3] = uint32_reg_to_float(&rawData[8]);
-	destination[0] = uint32_reg_to_float(&rawData[12]); // SENtral stores quats as qx, qy, qz, q0!
+	uint8_t data[16];  // x/y/z quaternion register data stored here
 
+	i2c_read(em7180->hi2c, EM7180_ADDRESS, EM7180_QX, data, 16); // Read the sixteen raw data registers into data array
+	destination[1] = uint32_reg_to_float(&data[0]);
+	destination[2] = uint32_reg_to_float(&data[4]);
+	destination[3] = uint32_reg_to_float(&data[8]);
+	destination[0] = uint32_reg_to_float(&data[12]); // SENtral stores quats as qx, qy, qz, q0!
 }
 
-void em7180_acceldata_get(int16_t *destination)
+void em7180_acceldata_get(em7180_t *em7180, int16_t *destination)
 {
-	uint8_t rawData[6];  // x/y/z accel register data stored here
-	em7180_read(EM7180_ADDRESS, EM7180_AX, 6, &rawData[0]); // Read the six raw data registers into data array
-	destination[0] = (int16_t) (((int16_t) rawData[1] << 8) | rawData[0]); // Turn the MSB and LSB into a signed 16-bit value
-	destination[1] = (int16_t) (((int16_t) rawData[3] << 8) | rawData[2]);
-	destination[2] = (int16_t) (((int16_t) rawData[5] << 8) | rawData[4]);
+	uint8_t data[6];  // x/y/z accel register data stored here
+
+	i2c_read(em7180->hi2c, EM7180_ADDRESS, EM7180_AX, data, 6); // Read the six raw data registers into data array
+	destination[0] = (int16_t) (((int16_t) data[1] << 8) | data[0]); // Turn the MSB and LSB into a signed 16-bit value
+	destination[1] = (int16_t) (((int16_t) data[3] << 8) | data[2]);
+	destination[2] = (int16_t) (((int16_t) data[5] << 8) | data[4]);
 }
 
-void em7180_gyrodata_get(int16_t *destination)
+void em7180_gyrodata_get(em7180_t *em7180, int16_t *destination)
 {
-	uint8_t rawData[6];  // x/y/z gyro register data stored here
-	em7180_read(EM7180_ADDRESS, EM7180_GX, 6, &rawData[0]); // Read the six raw data registers sequentially into data array
-	destination[0] = (int16_t) (((int16_t) rawData[1] << 8) | rawData[0]); // Turn the MSB and LSB into a signed 16-bit value
-	destination[1] = (int16_t) (((int16_t) rawData[3] << 8) | rawData[2]);
-	destination[2] = (int16_t) (((int16_t) rawData[5] << 8) | rawData[4]);
+	uint8_t data[6];  // x/y/z gyro register data stored here
+
+	i2c_read(em7180->hi2c, EM7180_ADDRESS, EM7180_GX, data, 6); // Read the six raw data registers sequentially into data array
+	destination[0] = (int16_t) (((int16_t) data[1] << 8) | data[0]); // Turn the MSB and LSB into a signed 16-bit value
+	destination[1] = (int16_t) (((int16_t) data[3] << 8) | data[2]);
+	destination[2] = (int16_t) (((int16_t) data[5] << 8) | data[4]);
 }
 
-void em7180_magdata_get(int16_t *destination)
+void em7180_magdata_get(em7180_t *em7180, int16_t *destination)
 {
-	uint8_t rawData[6];  // x/y/z gyro register data stored here
-	em7180_read(EM7180_ADDRESS, EM7180_MX, 6, &rawData[0]); // Read the six raw data registers sequentially into data array
-	destination[0] = (int16_t) (((int16_t) rawData[1] << 8) | rawData[0]); // Turn the MSB and LSB into a signed 16-bit value
-	destination[1] = (int16_t) (((int16_t) rawData[3] << 8) | rawData[2]);
-	destination[2] = (int16_t) (((int16_t) rawData[5] << 8) | rawData[4]);
+	uint8_t data[6];  // x/y/z mag register data stored here
+
+	i2c_read(em7180->hi2c, EM7180_ADDRESS, EM7180_MX, data, 6); // Read the six raw data registers sequentially into data array
+	destination[0] = (int16_t) (((int16_t) data[1] << 8) | data[0]); // Turn the MSB and LSB into a signed 16-bit value
+	destination[1] = (int16_t) (((int16_t) data[3] << 8) | data[2]);
+	destination[2] = (int16_t) (((int16_t) data[5] << 8) | data[4]);
 }
 
 float em7180_mres_get(uint8_t Mscale)
@@ -608,33 +606,39 @@ float em7180_ares_get(uint8_t ascale)
 	return a_res;
 }
 
-int16_t em7180_baro_get()
+int16_t em7180_baro_get(em7180_t *em7180)
 {
-	uint8_t rawData[2];  // x/y/z gyro register data stored here
-	em7180_read(EM7180_ADDRESS, EM7180_Baro, 2, &rawData[0]); // Read the two raw data registers sequentially into data array
-	return (int16_t) (((int16_t) rawData[1] << 8) | rawData[0]); // Turn the MSB and LSB into a signed 16-bit value
+	uint8_t data[2];  // baro register data stored here
+
+	i2c_read(em7180->hi2c, EM7180_ADDRESS, EM7180_Baro, data, 2); // Read the two raw data registers sequentially into data array
+
+	return (((int16_t) data[1] << 8) | data[0]); // Turn the MSB and LSB into a signed 16-bit value
 }
 
-int16_t em7180_temp_get()
+int16_t em7180_temp_get(em7180_t *em7180)
 {
-	uint8_t rawData[2];  // x/y/z gyro register data stored here
-	em7180_read(EM7180_ADDRESS, EM7180_Temp, 2, &rawData[0]); // Read the two raw data registers sequentially into data array
-	return (int16_t) (((int16_t) rawData[1] << 8) | rawData[0]); // Turn the MSB and LSB into a signed 16-bit value
+	uint8_t data[2];  // temp register data stored here
+
+	i2c_read(em7180->hi2c, EM7180_ADDRESS, EM7180_Temp, data, 2); // Read the two raw data registers sequentially into data array
+
+	return (((int16_t) data[1] << 8) | data[0]); // Turn the MSB and LSB into a signed 16-bit value
 }
 
-void em7180_passthrough()
+static void em7180_passthrough(em7180_t *em7180)
 {
 	// First put SENtral in standby mode
-	uint8_t c = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_AlgorithmControl);
-	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, c | 0x01);
+	uint8_t c = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS,
+	EM7180_AlgorithmControl);
+	i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_AlgorithmControl,
+	               c | 0x01);
 	//  c = readByte(EM7180_ADDRESS, EM7180_AlgorithmStatus);
 	/*	//  Serial.print("c = "); Serial.println(c); */
 	// Verify standby status
 	// if(readByte(EM7180_ADDRESS, EM7180_AlgorithmStatus) & 0x01) {
 	/* Serial.println("SENtral in standby mode"); */
 	// Place SENtral in pass-through mode
-	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_PassThruControl, 0x01);
-	if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_PassThruStatus) & 0x01)
+	i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_PassThruControl, 0x01);
+	if(i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_PassThruStatus) & 0x01)
 	{
 		/* Serial.println("SENtral in pass-through mode"); */
 	}
@@ -644,67 +648,37 @@ void em7180_passthrough()
 	}
 }
 
-// I2C communication with the M24512DFM EEPROM is a little different from I2C communication with the usual motion sensor
-// since the address is defined by two bytes
-static void m24512dfm_write_byte(uint8_t device_address, uint8_t data_address1,
-                                 uint8_t data_address2, uint8_t data)
+static float uint32_reg_to_float(uint8_t *buf)
 {
-	uint8_t temp[2] = { data_address1, data_address2 };
-	/* Wire.transfer(device_address, &temp[0], 2, NULL, 0); */
-	/* Wire.transfer(device_address, &data, 1, NULL, 0); */
-}
-
-static void m24512dfm_write(uint8_t device_address, uint8_t data_address1,
-                            uint8_t data_address2, uint8_t count, uint8_t *dest)
-{
-	if(count > 128)
+	union
 	{
-		count = 128;
-		/* Serial.print("Page count cannot be more than 128 bytes!"); */
-	}
-	uint8_t temp[2] = { data_address1, data_address2 };
-	/* Wire.transfer(device_address, &temp[0], 2, NULL, 0); */
-	/* Wire.transfer(device_address, &dest[0], count, NULL, 0); */
-}
-
-static uint8_t m24512dfm_read_byte(uint8_t device_address,
-                                   uint8_t data_address1, uint8_t data_address2)
-{
-	uint8_t data; // `data` will store the register data
-	/* Wire.beginTransmission(device_address);         // Initialize the Tx buffer */
-	/* Wire.write(data_address1);        // Put slave register address in Tx buffer */
-	/* Wire.write(data_address2);        // Put slave register address in Tx buffer */
-	/* Wire.endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive */
-	/* Wire.requestFrom(device_address, 1); // Read one byte from slave register address */
-	/*	data = Wire.read();                      // Fill Rx buffer with result */
-	return data;                         // Return data read from slave register
-}
+		uint32_t ui32;
+		float f;
+	} u;
 
-static void m24512dfm_read(uint8_t device_address, uint8_t data_address1,
-                           uint8_t data_address2, uint8_t count, uint8_t *dest)
-{
-	uint8_t temp[2] = { data_address1, data_address2 };
-	/* Wire.transfer(device_address, &temp[0], 2, dest, count); */
-}
+	u.ui32 = (((uint32_t) buf[0]) + (((uint32_t) buf[1]) << 8)
+	          + (((uint32_t) buf[2]) << 16) + (((uint32_t) buf[3]) << 24));
 
-// I2C read/write functions for the EM7180
-void em7180_write_byte(uint8_t address, uint8_t subAddress, uint8_t data)
-{
-	uint8_t temp[2];
-	temp[0] = subAddress;
-	temp[1] = data;
-	/* Wire.transfer(address, &temp[0], 2, NULL, 0); */
+	return u.f;
 }
 
-static uint8_t em7180_read_byte(uint8_t address, uint8_t subAddress)
+static void float_to_bytes(float param_val, uint8_t *buf)
 {
-	uint8_t temp[1];
-	/* Wire.transfer(address, &subAddress, 1, &temp[0], 1); */
-	return temp[0];
-}
+	union
+	{
+		float f;
+		uint8_t u8[sizeof(float)];
+	} u;
 
-static void em7180_read(uint8_t address, uint8_t subAddress, uint8_t count,
-                        uint8_t *dest)
-{
-	/* Wire.transfer(address, &subAddress, 1, dest, count); */
+	u.f = param_val;
+	for(uint8_t i = 0; i < sizeof(float); i++)
+	{
+		buf[i] = u.u8[i];
+	}
+	// Convert to LITTLE ENDIAN
+	/* FIXME: What the hell? */
+	for(uint8_t i = 0; i < sizeof(float); i++)
+	{
+		buf[i] = buf[(sizeof(float) - 1) - i];
+	}
 }

Drivers/EM7180/Src/lis2mdl.c

@@ -22,83 +22,78 @@
 /* Private Global Variables */
 
 /* Function Prototypes */
-static void lis2mdl_write_byte(uint8_t address, uint8_t subAddress,
-                               uint8_t data);
-static uint8_t lis2mdl_read_byte(uint8_t address, uint8_t subAddress);
-static void lis2mdl_read(uint8_t address, uint8_t subAddress, uint8_t count,
-                         uint8_t *dest);
 
 /* Function Definitions */
-void lis2mdl_init(lis2mdl_t *lis2mdl, I2C_HandleTypeDef *hi2c, uint8_t m_odr)
+void lis2mdl_init(lis2mdl_t *lis2mdl, uint8_t m_odr)
 {
 	if(!lis2mdl)
 	{
 		return;
 	}
 
-	lis2mdl->hi2c = hi2c;
 	lis2mdl->m_odr = m_odr;
 }
 
-void lis2mdl_config(lis2mdl_t *lis2mdl)
+void lis2mdl_config(lis2mdl_t *lis2mdl, I2C_HandleTypeDef *hi2c)
 {
 	// enable temperature compensation (bit 7 == 1), continuous mode (bits 0:1 == 00)
-	lis2mdl_write_byte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A,
+	i2c_write_byte(hi2c, LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A,
 	               0x80 | lis2mdl->m_odr << 2);
 
 	// enable low pass filter (bit 0 == 1), set to ODR/4
-	lis2mdl_write_byte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_B, 0x01);
+	i2c_write_byte(hi2c, LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_B, 0x01);
 
 	// enable data ready on interrupt pin (bit 0 == 1), enable block data read (bit 4 == 1)
-	lis2mdl_write_byte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C, 0x01 | 0x10);
+	i2c_write_byte(hi2c, LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C, 0x01 | 0x10);
 
 }
 
-uint8_t lis2mdl_chip_id_get()
+uint8_t lis2mdl_chip_id_get(I2C_HandleTypeDef *hi2c)
 {
-	uint8_t c = lis2mdl_read_byte(LIS2MDL_ADDRESS, LIS2MDL_WHO_AM_I);
+	uint8_t c = i2c_read_byte(hi2c, LIS2MDL_ADDRESS, LIS2MDL_WHO_AM_I);
+
 	return c;
 }
 
-void lis2mdl_reset()
+void lis2mdl_reset(I2C_HandleTypeDef *hi2c)
 {
 	// reset device
-	uint8_t temp = lis2mdl_read_byte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A);
-	lis2mdl_write_byte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A, temp | 0x20); // Set bit 5 to 1 to reset LIS2MDL
+	uint8_t temp = i2c_read_byte(hi2c, LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A);
+
+	i2c_write_byte(hi2c, LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A, temp | 0x20); // Set bit 5 to 1 to reset LIS2MDL
 	HAL_Delay(1);
-	lis2mdl_write_byte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A, temp | 0x40); // Set bit 6 to 1 to boot LIS2MDL
+	i2c_write_byte(hi2c, LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A, temp | 0x40); // Set bit 6 to 1 to boot LIS2MDL
 	HAL_Delay(100); // Wait for all registers to reset
 }
 
-uint8_t lis2mdl_status()
+uint8_t lis2mdl_status(I2C_HandleTypeDef *hi2c)
 {
 	// Read the status register of the altimeter  
-	uint8_t temp = lis2mdl_read_byte(LIS2MDL_ADDRESS, LIS2MDL_STATUS_REG);
+	uint8_t temp = i2c_read_byte(hi2c, LIS2MDL_ADDRESS, LIS2MDL_STATUS_REG);
 	return temp;
 }
 
-void lis2mdl_data_get(int16_t *destination)
+void lis2mdl_data_get(I2C_HandleTypeDef *hi2c, int16_t *destination)
 {
-	uint8_t rawData[6];  // x/y/z mag register data stored here
-	lis2mdl_read_bytes(LIS2MDL_ADDRESS, (0x80 | LIS2MDL_OUTX_L_REG), 8,
-	                   &rawData[0]); // Read the 6 raw data registers into data array
+	uint8_t data[6];  // x/y/z mag register data stored here
+	i2c_read(hi2c, LIS2MDL_ADDRESS, (0x80 | LIS2MDL_OUTX_L_REG), data, 8); // Read the 6 raw data registers into data array
 
-	destination[0] = ((int16_t) rawData[1] << 8) | rawData[0]; // Turn the MSB and LSB into a signed 16-bit value
-	destination[1] = ((int16_t) rawData[3] << 8) | rawData[2];
-	destination[2] = ((int16_t) rawData[5] << 8) | rawData[4];
+	destination[0] = ((int16_t) data[1] << 8) | data[0]; // Turn the MSB and LSB into a signed 16-bit value
+	destination[1] = ((int16_t) data[3] << 8) | data[2];
+	destination[2] = ((int16_t) data[5] << 8) | data[4];
 }
 
 int16_t lis2mdl_temp_get()
 {
-	uint8_t rawData[2];  // x/y/z mag register data stored here
+	uint8_t data[2];  // x/y/z mag register data stored here
 	lis2mdl_read_bytes(LIS2MDL_ADDRESS, (0x80 | LIS2MDL_TEMP_OUT_L_REG), 2,
-	                   &rawData[0]); // Read the 8 raw data registers into data array
+	                   &data[0]); // Read the 8 raw data registers into data array
 
-	int16_t temp = ((int16_t) rawData[1] << 8) | rawData[0]; // Turn the MSB and LSB into a signed 16-bit value
+	int16_t temp = ((int16_t) data[1] << 8) | data[0]; // Turn the MSB and LSB into a signed 16-bit value
 	return temp;
 }
 
-void lis2mdl_offset_bias(float *dest1, float *dest2)
+void lis2mdl_offset_bias(I2C_HandleTypeDef *hi2c, float *dest1, float *dest2)
 {
 	int32_t mag_bias[3] = { 0, 0, 0 }, mag_scale[3] = { 0, 0, 0 };
 	int16_t mag_max[3] = { -32767, -32767, -32767 }, mag_min[3] =
@@ -110,7 +105,7 @@ void lis2mdl_offset_bias(float *dest1, float *dest2)
 
 	for(int ii = 0; ii < 4000; ii++)
 	{
-		lis2mdl_data_get(mag_temp);
+		lis2mdl_data_get(hi2c, mag_temp);
 		for(int jj = 0; jj < 3; jj++)
 		{
 			if(mag_temp[jj] > mag_max[jj])
@@ -150,7 +145,7 @@ void lis2mdl_offset_bias(float *dest1, float *dest2)
 	/* Serial.println("Mag Calibration done!"); */
 }
 
-void lis2mdl_self_test()
+void lis2mdl_self_test(I2C_HandleTypeDef *hi2c)
 {
 	int16_t temp[3] = { 0, 0, 0 };
 	float magTest[3] = { 0., 0., 0. };
@@ -161,7 +156,7 @@ void lis2mdl_self_test()
 	// first, get average response with self test disabled
 	for(int ii = 0; ii < 50; ii++)
 	{
-		lis2mdl_data_get(temp);
+		lis2mdl_data_get(hi2c, temp);
 		sum[0] += temp[0];
 		sum[1] += temp[1];
 		sum[2] += temp[2];
@@ -172,8 +167,8 @@ void lis2mdl_self_test()
 	magNom[1] = (float) sum[1] / 50.0f;
 	magNom[2] = (float) sum[2] / 50.0f;
 
-	uint8_t c = lis2mdl_read_byte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C);
-	lis2mdl_write_byte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C, c | 0x02); // enable self test
+	uint8_t c = i2c_read_byte(hi2c, LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C);
+	i2c_write_byte(hi2c, LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C, c | 0x02); // enable self test
 	HAL_Delay(100); // let mag respond
 
 	sum[0] = 0;
@@ -181,7 +176,7 @@ void lis2mdl_self_test()
 	sum[2] = 0;
 	for(int ii = 0; ii < 50; ii++)
 	{
-		lis2mdl_data_get(temp);
+		lis2mdl_data_get(hi2c, temp);
 		sum[0] += temp[0];
 		sum[1] += temp[1];
 		sum[2] += temp[2];
@@ -192,7 +187,7 @@ void lis2mdl_self_test()
 	magTest[1] = (float) sum[1] / 50.0f;
 	magTest[2] = (float) sum[2] / 50.0f;
 
-	lis2mdl_write_byte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C, c); // return to previous settings/normal mode
+	i2c_write_byte(hi2c, LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C, c); // return to previous settings/normal mode
 	HAL_Delay(100); // let mag respond
 
 	/* Serial.println("Mag Self Test:"); */
@@ -206,27 +201,3 @@ void lis2mdl_self_test()
 	/* Serial.println("Should be between 15 and 500 mG"); */
 	HAL_Delay(2000);  // give some time to read the screen
 }
-
-// I2C read/write functions for the LIS2MDL
-
-static void lis2mdl_write_byte(uint8_t address, uint8_t subAddress,
-                               uint8_t data)
-{
-	uint8_t temp[2];
-	temp[0] = subAddress;
-	temp[1] = data;
-	/* Wire.transfer(address, &temp[0], 2, NULL, 0); */
-}
-
-static uint8_t lis2mdl_read_byte(uint8_t address, uint8_t subAddress)
-{
-	uint8_t temp[1];
-	/* Wire.transfer(address, &subAddress, 1, &temp[0], 1); */
-	return temp[0];
-}
-
-static void lis2mdl_read(uint8_t address, uint8_t subAddress, uint8_t count,
-                         uint8_t *dest)
-{
-	/* Wire.transfer(address, &subAddress, 1, dest, count); */
-}

Drivers/EM7180/Src/lps22hb.c

@@ -22,91 +22,56 @@
 /* Private Global Variables */
 
 /* Function Prototypes */
-static void lps22hb_write_byte(uint8_t address, uint8_t subAddress,
-                               uint8_t data);
-static uint8_t lps22hb_read_byte(uint8_t address, uint8_t subAddress);
-static void lps22hb_read(uint8_t address, uint8_t subAddress, uint8_t count,
-                         uint8_t *dest);
 
 /* Function Definitions */
-void lps22hb_init(lps22hb_t *lps22hb, I2C_HandleTypeDef *hi2c, uint8_t p_odr)
+void lps22hb_init(lps22hb_t *lps22hb, uint8_t p_odr)
 {
-	lps22hb->hi2c = hi2c;
+	return_if_fail(lps22hb);
+
 	lps22hb->p_odr = p_odr;
 }
 
-void lps22hb_config(lps22hb_t *lps22hb)
+void lps22hb_config(lps22hb_t *lps22hb, I2C_HandleTypeDef *hi2c)
 {
 	// set sample rate by setting bits 6:4
 	// enable low-pass filter by setting bit 3 to one
 	// bit 2 == 0 means bandwidth is odr/9, bit 2 == 1 means bandwidth is odr/20
 	// make sure data not updated during read by setting block data udate (bit 1) to 1
-	lps22hb_write_byte(LPS22H_ADDRESS, LPS22H_CTRL_REG1,
+	i2c_write_byte(hi2c, LPS22H_ADDRESS, LPS22H_CTRL_REG1,
 	               lps22hb->p_odr << 4 | 0x08 | 0x02);
-	lps22hb_write_byte(LPS22H_ADDRESS, LPS22H_CTRL_REG3, 0x04); // enable data ready as interrupt source
+	i2c_write_byte(hi2c, LPS22H_ADDRESS, LPS22H_CTRL_REG3, 0x04); // enable data ready as interrupt source
 }
 
-uint8_t lps22hb_chip_id_get()
+uint8_t lps22hb_chip_id_get(I2C_HandleTypeDef *hi2c)
 {
 	// Read the WHO_AM_I register of the altimeter this is a good test of communication
-	uint8_t temp = lps22hb_read_byte(LPS22H_ADDRESS, LPS22H_WHOAMI); // Read WHO_AM_I register for LPS22H
+	uint8_t temp = i2c_read_byte(hi2c, LPS22H_ADDRESS, LPS22H_WHOAMI); // Read WHO_AM_I register for LPS22H
+
 	return temp;
 }
 
-uint8_t lps22hb_status()
+uint8_t lps22hb_status(I2C_HandleTypeDef *hi2c)
 {
 	// Read the status register of the altimeter  
-	uint8_t temp = lps22hb_read_byte(LPS22H_ADDRESS, LPS22H_STATUS);
+	uint8_t temp = i2c_read_byte(hi2c, LPS22H_ADDRESS, LPS22H_STATUS);
+
 	return temp;
 }
 
-int32_t lps22hb_pressure_get()
+int32_t lps22hb_pressure_get(I2C_HandleTypeDef *hi2c)
 {
-	uint8_t rawData[3];  // 24-bit pressure register data stored here
-	lps22hb_read(LPS22H_ADDRESS, (LPS22H_PRESS_OUT_XL | 0x80), 3, &rawData[0]); // bit 7 must be one to read multiple bytes
-	return (int32_t) ((int32_t) rawData[2] << 16 | (int32_t) rawData[1] << 8
-	                  | rawData[0]);
-}
+	uint8_t data[3];  // 24-bit pressure register data stored here
 
-int16_t lps22hb_temp_get()
-{
-	uint8_t rawData[2];  // 16-bit pressure register data stored here
-	lps22hb_read(LPS22H_ADDRESS, (LPS22H_TEMP_OUT_L | 0x80), 2, &rawData[0]); // bit 7 must be one to read multiple bytes
-	return (int16_t) ((int16_t) rawData[1] << 8 | rawData[0]);
-}
+	i2c_read(hi2c, LPS22H_ADDRESS, (LPS22H_PRESS_OUT_XL | 0x80), data, 3); // bit 7 must be one to read multiple bytes
 
-static void lps22hb_write_byte(uint8_t address, uint8_t subAddress,
-                               uint8_t data)
-{
-	/* Wire.beginTransmission(address);  // Initialize the Tx buffer */
-	/* Wire.write(subAddress);           // Put slave register address in Tx buffer */
-	/* Wire.write(data);                 // Put data in Tx buffer */
-	/* Wire.endTransmission();           // Send the Tx buffer */
+	return ((int32_t) data[2] << 16 | (int32_t) data[1] << 8 | data[0]);
 }
 
-static uint8_t lps22hb_read_byte(uint8_t address, uint8_t subAddress)
+int16_t lps22hb_temp_get(I2C_HandleTypeDef *hi2c)
 {
-	uint8_t data; // `data` will store the register data   
-	/* Wire.beginTransmission(address);         // Initialize the Tx buffer */
-	/* Wire.write(subAddress);           // Put slave register address in Tx buffer */
-	/* Wire.endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive */
-	/* Wire.requestFrom(address, (size_t) 1); // Read one uint8_t from slave register address  */
-	/*	data = Wire.read();                      // Fill Rx buffer with result */
-	return data;                 // Return data lps22hb_read from slave register
-}
+	uint8_t data[2];  // 16-bit pressure register data stored here
 
-static void lps22hb_read(uint8_t address, uint8_t subAddress, uint8_t count,
-                         uint8_t *dest)
-{
-	/* Wire.beginTransmission(address);   // Initialize the Tx buffer */
-	/* Wire.write(subAddress);           // Put slave register address in Tx buffer */
-	/* Wire.endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive */
-	uint8_t i = 0;
-	/* Wire.requestFrom(address, (size_t) count); // Read bytes from slave register address  */
-	/*
-	 while(Wire.available())
-	 {
-	 dest[i++] = Wire.lps22hb_read();
-	 }         // Put lps22hb_read results in the Rx buffer
-	 */
+	i2c_read(hi2c, LPS22H_ADDRESS, (LPS22H_TEMP_OUT_L | 0x80), data, 2); // bit 7 must be one to read multiple bytes
+
+	return ((int16_t) data[1] << 8 | data[0]);
 }

Drivers/EM7180/Src/lsm6dsm.c

@@ -23,49 +23,48 @@
 /* Private Global Variables */
 
 /* Function Prototypes */
+static void lsm6dsm_read_data(lsm6dsm_t *lsm6dsm, I2C_HandleTypeDef *hi2c,
+                              int16_t *destination);
 
 /* Function Definitions */
-void lsm6dsm_init(lsm6dsm_t *lsm6dsm, I2C_HandleTypeDef *hi2c, uint8_t ascale,
-                  uint8_t gscale, uint8_t a_odr, uint8_t g_odr)
+void lsm6dsm_init(lsm6dsm_t *lsm6dsm, uint8_t ascale, uint8_t gscale,
+                  uint8_t a_odr, uint8_t g_odr)
 {
-	if(!lsm6dsm)
-	{
-		return;
-	}
+	return_if_fail(lsm6dsm);
 
-	lsm6dsm->hi2c = hi2c;
 	lsm6dsm->ascale = ascale;
 	lsm6dsm->gscale = gscale;
 	lsm6dsm->a_odr = a_odr;
 	lsm6dsm->g_odr = g_odr;
 }
 
-void lsm6dsm_config(lsm6dsm_t *lsm6dsm)
+void lsm6dsm_config(lsm6dsm_t *lsm6dsm, I2C_HandleTypeDef *hi2c)
 {
-	lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL1_XL,
+	i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL1_XL,
 	               lsm6dsm->a_odr << 4 | lsm6dsm->ascale << 2);
 
-	lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL2_G,
+	i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL2_G,
 	               lsm6dsm->g_odr << 4 | lsm6dsm->gscale << 2);
 
-	uint8_t temp = lsm6dsm_read_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL3_C);
+	uint8_t temp = i2c_read_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL3_C);
 	// enable block update (bit 6 = 1), auto-increment registers (bit 2 = 1)
-	lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL3_C, temp | 0x40 | 0x04);
+	i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL3_C, temp | 0x40 | 0x04);
 	// by default, interrupts active HIGH, push pull, little endian data
 	// (can be changed by writing to bits 5, 4, and 1, resp to above register)
 
 	// enable accel LP2 (bit 7 = 1), set LP2 tp ODR/9 (bit 6 = 1), enable input_composite (bit 3) for low noise
-	lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL8_XL, 0x80 | 0x40 | 0x08);
+	i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL8_XL, 0x80 | 0x40 | 0x08);
 
 	// interrupt handling
-	lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_DRDY_PULSE_CFG, 0x80); // latch interrupt until data read
-	lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_INT1_CTRL, 0x40); // enable significant motion interrupts on INT1
-	lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_INT2_CTRL, 0x03); // enable accel/gyro data ready interrupts on INT2
+	i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_DRDY_PULSE_CFG, 0x80); // latch interrupt until data read
+	i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_INT1_CTRL, 0x40); // enable significant motion interrupts on INT1
+	i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_INT2_CTRL, 0x03); // enable accel/gyro data ready interrupts on INT2
 }
 
-uint8_t lsm6dsm_chip_id_get()
+uint8_t lsm6dsm_chip_id_get(lsm6dsm_t *lsm6dsm, I2C_HandleTypeDef *hi2c)
 {
-	uint8_t c = lsm6dsm_read_byte(LSM6DSM_ADDRESS, LSM6DSM_WHO_AM_I);
+	uint8_t c = i2c_read_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_WHO_AM_I);
+
 	return c;
 }
 
@@ -126,22 +125,26 @@ float lsm6dsm_gres_get(lsm6dsm_t *lsm6dsm)
 	return g_res;
 }
 
-void lsm6dsm_reset()
+void lsm6dsm_reset(lsm6dsm_t *lsm6dsm, I2C_HandleTypeDef *hi2c)
 {
 	// reset device
-	uint8_t temp = lsm6dsm_read_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL3_C);
-	lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL3_C, temp | 0x01); // Set bit 0 to 1 to reset LSM6DSM
+	uint8_t temp = i2c_read_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL3_C);
+
+	i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL3_C, temp | 0x01); // Set bit 0 to 1 to reset LSM6DSM
 	HAL_Delay(100); // Wait for all registers to reset
 }
 
-void lsm6dsm_selfTest()
+void lsm6dsm_self_test(lsm6dsm_t *lsm6dsm, I2C_HandleTypeDef *hi2c)
 {
 	int16_t temp[7] = { 0, 0, 0, 0, 0, 0, 0 };
-	int16_t accelPTest[3] = { 0, 0, 0 }, accelNTest[3] = { 0, 0, 0 },
-	    gyroPTest[3] = { 0, 0, 0 }, gyroNTest[3] = { 0, 0, 0 };
-	int16_t accelNom[3] = { 0, 0, 0 }, gyroNom[3] = { 0, 0, 0 };
-
-	lsm6dsm_read_data(temp);
+	int16_t accelPTest[3] = { 0, 0, 0 };
+	int16_t accelNTest[3] = { 0, 0, 0 };
+	int16_t gyroPTest[3] = { 0, 0, 0 };
+	int16_t gyroNTest[3] = { 0, 0, 0 };
+	int16_t accelNom[3] = { 0, 0, 0 };
+	int16_t gyroNom[3] = { 0, 0, 0 };
+
+	lsm6dsm_read_data(lsm6dsm, hi2c, temp);
 	accelNom[0] = temp[4];
 	accelNom[1] = temp[5];
 	accelNom[2] = temp[6];
@@ -149,35 +152,35 @@ void lsm6dsm_selfTest()
 	gyroNom[1] = temp[2];
 	gyroNom[2] = temp[3];
 
-	lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x01); // positive accel self test
+	i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x01); // positive accel self test
 	HAL_Delay(100); // let accel respond
-	lsm6dsm_read_data(temp);
+	lsm6dsm_read_data(lsm6dsm, hi2c, temp);
 	accelPTest[0] = temp[4];
 	accelPTest[1] = temp[5];
 	accelPTest[2] = temp[6];
 
-	lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x03); // negative accel self test
+	i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x03); // negative accel self test
 	HAL_Delay(100); // let accel respond
-	lsm6dsm_read_data(temp);
+	lsm6dsm_read_data(lsm6dsm, hi2c, temp);
 	accelNTest[0] = temp[4];
 	accelNTest[1] = temp[5];
 	accelNTest[2] = temp[6];
 
-	lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x04); // positive gyro self test
+	i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x04); // positive gyro self test
 	HAL_Delay(100); // let gyro respond
-	lsm6dsm_read_data(temp);
+	lsm6dsm_read_data(lsm6dsm, hi2c, temp);
 	gyroPTest[0] = temp[1];
 	gyroPTest[1] = temp[2];
 	gyroPTest[2] = temp[3];
 
-	lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x0C); // negative gyro self test
+	i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x0C); // negative gyro self test
 	HAL_Delay(100); // let gyro respond
-	lsm6dsm_read_data(temp);
+	lsm6dsm_read_data(lsm6dsm, hi2c, temp);
 	gyroNTest[0] = temp[1];
 	gyroNTest[1] = temp[2];
 	gyroNTest[2] = temp[3];
 
-	lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x00); // normal mode
+	i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x00); // normal mode
 	HAL_Delay(100); // let accel and gyro respond
 
 	/* Serial.println("Accel Self Test:"); */
@@ -215,7 +218,8 @@ void lsm6dsm_selfTest()
 
 }
 
-void lsm6dsm_offset_bias(lsm6dsm_t *lsm6dsm, float *dest1, float *dest2)
+void lsm6dsm_offset_bias(lsm6dsm_t *lsm6dsm, I2C_HandleTypeDef *hi2c,
+                         float *dest1, float *dest2)
 {
 	int16_t temp[7] = { 0, 0, 0, 0, 0, 0, 0 };
 	int32_t sum[7] = { 0, 0, 0, 0, 0, 0, 0 };
@@ -227,7 +231,7 @@ void lsm6dsm_offset_bias(lsm6dsm_t *lsm6dsm, float *dest1, float *dest2)
 
 	for(int ii = 0; ii < 128; ii++)
 	{
-		lsm6dsm_read_data(temp);
+		lsm6dsm_read_data(lsm6dsm, hi2c, temp);
 		sum[1] += temp[1];
 		sum[2] += temp[2];
 		sum[3] += temp[3];
@@ -271,36 +275,17 @@ void lsm6dsm_offset_bias(lsm6dsm_t *lsm6dsm, float *dest1, float *dest2)
 
 }
 
-void lsm6dsm_read_data(int16_t *destination)
-{
-	uint8_t rawdata[14];  // x/y/z accel register data stored here
-	lsm6dsm_read(LSM6DSM_ADDRESS, LSM6DSM_OUT_TEMP_L, 14, &rawdata[0]); // Read the 14 raw data registers into data array
-	destination[0] = ((int16_t) rawdata[1] << 8) | rawdata[0]; // Turn the MSB and LSB into a signed 16-bit value
-	destination[1] = ((int16_t) rawdata[3] << 8) | rawdata[2];
-	destination[2] = ((int16_t) rawdata[5] << 8) | rawdata[4];
-	destination[3] = ((int16_t) rawdata[7] << 8) | rawdata[6];
-	destination[4] = ((int16_t) rawdata[9] << 8) | rawdata[8];
-	destination[5] = ((int16_t) rawdata[11] << 8) | rawdata[10];
-	destination[6] = ((int16_t) rawdata[13] << 8) | rawdata[12];
-}
-
-// I2C read/write functions for the LSM6DSM
-void lsm6dsm_write_byte(uint8_t addr, uint8_t sub_addr, uint8_t data)
-{
-	uint8_t temp[2];
-	temp[0] = sub_addr;
-	temp[1] = data;
-	/* Wire.transfer(addr, &temp[0], 2, NULL, 0); */
-}
-
-uint8_t lsm6dsm_read_byte(uint8_t addr, uint8_t sub_addr)
-{
-	uint8_t temp[1];
-	/* Wire.transfer(addr, &sub_addr, 1, &temp[0], 1); */
-	return temp[0];
-}
-
-void lsm6dsm_read(uint8_t addr, uint8_t sub_addr, uint8_t count, uint8_t *dest)
+static void lsm6dsm_read_data(lsm6dsm_t *lsm6dsm, I2C_HandleTypeDef *hi2c,
+                              int16_t *destination)
 {
-	/* Wire.transfer(addr, &sub_addr, 1, dest, count); */
+	uint8_t data[14];  // x/y/z accel register data stored here
+
+	i2c_read(hi2c, LSM6DSM_ADDRESS, LSM6DSM_OUT_TEMP_L, data, 14); // Read the 14 raw data registers into data array
+	destination[0] = ((int16_t) data[1] << 8) | data[0]; // Turn the MSB and LSB into a signed 16-bit value
+	destination[1] = ((int16_t) data[3] << 8) | data[2];
+	destination[2] = ((int16_t) data[5] << 8) | data[4];
+	destination[3] = ((int16_t) data[7] << 8) | data[6];
+	destination[4] = ((int16_t) data[9] << 8) | data[8];
+	destination[5] = ((int16_t) data[11] << 8) | data[10];
+	destination[6] = ((int16_t) data[13] << 8) | data[12];
 }