Added ICM-20948 callback to state struct

imu/icm20948.c

@@ -34,9 +34,6 @@ static void terminal_read_reg(int argc, const char **argv);
 static uint8_t read_single_reg(ICM20948_STATE *s, uint8_t reg);
 static bool write_single_reg(ICM20948_STATE *s, uint8_t reg, uint8_t value);
 
-// Function pointers
-static void(*m_read_callback)(float *accel, float *gyro, float *mag) = 0;
-
 // Private variables
 static ICM20948_STATE *m_terminal_state = 0;
 
@@ -45,6 +42,7 @@ void icm20948_init(ICM20948_STATE *s, i2c_bb_state *i2c_state, int ad0_val,
 
 	s->i2cs = i2c_state;
 	s->i2c_address = ad0_val ? 0x69 : 0x68;
+	s->read_callback = 0;
 
 	if (reset_init_icm(s)) {
 		chThdCreateStatic(work_area, work_area_size, NORMALPRIO, icm_thread, s);
@@ -61,8 +59,8 @@ void icm20948_init(ICM20948_STATE *s, i2c_bb_state *i2c_state, int ad0_val,
 	}
 }
 
-void icm20948_set_read_callback(void(*func)(float *accel, float *gyro, float *mag)) {
-	m_read_callback = func;
+void icm20948_set_read_callback(ICM20948_STATE *s, void(*func)(float *accel, float *gyro, float *mag)) {
+	s->read_callback = func;
 }
 
 static void terminal_read_reg(int argc, const char **argv) {
@@ -169,8 +167,8 @@ static THD_FUNCTION(icm_thread, arg) {
 			// TODO: Read magnetometer as well
 			memset(mag, 0, sizeof(mag));
 
-			if (m_read_callback) {
-				m_read_callback(accel, gyro, mag);
+			if (s->read_callback) {
+				s->read_callback(accel, gyro, mag);
 			}
 		} else {
 			reset_init_icm(s);

imu/icm20948.h

@@ -31,11 +31,12 @@
 typedef struct {
 	i2c_bb_state *i2cs;
 	uint8_t i2c_address;
+	void(*read_callback)(float *accel, float *gyro, float *mag);
 } ICM20948_STATE;
 
 void icm20948_init(ICM20948_STATE *s, i2c_bb_state *i2c_state, int ad0_val,
 		stkalign_t *work_area, size_t work_area_size);
-void icm20948_set_read_callback(void(*func)(float *accel, float *gyro, float *mag));
+void icm20948_set_read_callback(ICM20948_STATE *s, void(*func)(float *accel, float *gyro, float *mag));
 
 // All banks
 #define ICM20948_BANK_SEL						0x7F

imu/imu.c

@@ -85,7 +85,7 @@ void imu_init_icm20948(stm32_gpio_t *sda_gpio, int sda_pin,
 	icm20948_init(&m_icm20948_state,
 			&m_i2c_bb, ad0_val,
 			m_thd_work_area, sizeof(m_thd_work_area));
-	icm20948_set_read_callback(imu_read_callback);
+	icm20948_set_read_callback(&m_icm20948_state, imu_read_callback);
 }
 
 float imu_get_roll(void) {