void IMU_Init()
{
	int8_t rslt = BMI160_OK;
	
	motionSensor.id = 0;
	motionSensor.interface = BMI160_SPI_INTF;
	motionSensor.read = IMU_spi_read;
	motionSensor.write = IMU_spi_write;
	motionSensor.delay_ms = IMU_delay_ms;
	
	// so we can use bmm150 magnetometer compensation code
	fakeSensor.dev_id = BMM150_DEFAULT_I2C_ADDRESS;
	fakeSensor.intf = BMM150_I2C_INTF;
	fakeSensor.read = bmm150_aux_read;
	fakeSensor.write = bmm150_aux_write;
	fakeSensor.delay_ms = IMU_delay_ms;
  
	bmi160_init(&motionSensor);
	
	HAL_Delay(20);
	/*! Chip Id */
	// re check with direct register read
	uint8_t chipID;
	bmi160_get_regs(BMI160_CHIP_ID_ADDR, &chipID, 1, &motionSensor);
	if ((chipID != 0xD8) | (motionSensor.chip_id != 0xD8))
		DBG(" IMU ID ERROR - ID should be 0xD8", chipID);
	DBG(" IMU returned: 0x%x\r\n", chipID);


	motionSensor.aux_cfg.aux_sensor_enable = BMI160_ENABLE;
	motionSensor.aux_cfg.aux_i2c_addr = fakeSensor.dev_id;
	motionSensor.aux_cfg.manual_enable = BMI160_ENABLE;
	motionSensor.aux_cfg.aux_rd_burst_len = BMI160_AUX_READ_LEN_3;

	bmi160_aux_init(&motionSensor);
	bmm150_init(&fakeSensor);

	
	// configure some sensor params 
	motionSensor.accel_cfg.odr = BMI160_ACCEL_ODR_200HZ;
	motionSensor.accel_cfg.range = BMI160_ACCEL_RANGE_2G;
	motionSensor.accel_cfg.bw = BMI160_ACCEL_BW_NORMAL_AVG4;
	motionSensor.accel_cfg.power = BMI160_ACCEL_NORMAL_MODE;

	motionSensor.gyro_cfg.odr = BMI160_GYRO_ODR_200HZ;
	motionSensor.gyro_cfg.range = BMI160_GYRO_RANGE_2000_DPS;
	motionSensor.gyro_cfg.bw = BMI160_GYRO_BW_NORMAL_MODE;
	motionSensor.gyro_cfg.power = BMI160_GYRO_NORMAL_MODE;

	rslt = bmi160_set_sens_conf(&motionSensor);
	if (rslt != BMI160_OK) { sprintf(g_errInfo, "IMU config ERROR"); Error_Handler(); }


	fakeSensor.settings.preset_mode = BMM150_PRESETMODE_REGULAR;
	bmm150_set_presetmode(&fakeSensor);
	fakeSensor.settings.pwr_mode = BMM150_FORCED_MODE;
	bmm150_set_op_mode(&fakeSensor);


	uint8_t aux_addr = 0x42;	


	motionSensor.aux_cfg.aux_odr = 8;
	bmi160_config_aux_mode(&motionSensor);
	
	bmi160_set_aux_auto_mode(&aux_addr, &motionSensor);
	
	uint8_t intReg = 0x87;
	bmi160_set_regs( BMI160_INT_ENABLE_0_ADDR, &intReg, 1, &motionSensor );
	intReg = 0x07;
	bmi160_set_regs( BMI160_INT_ENABLE_2_ADDR, &intReg, 1, &motionSensor );
	intReg = 0x07;
	bmi160_set_regs( BMI160_INT_MOTION_3_ADDR, &intReg, 1, &motionSensor );
	
}