inv_error_t inv_set_compass_offset(void)
{
struct ext_slave_config config;
unsigned char data[3];
inv_error_t result;
config.key = MPU_SLAVE_OFFSET_VALS;
config.len = 3;
config.apply = TRUE;
config.data = data;
if(inv_obj.flags[INV_COMPASS_OFFSET_VALID]) {
/* push stored values */
data[0] = (char)inv_obj.compass_offsets[0];
data[1] = (char)inv_obj.compass_offsets[1];
data[2] = (char)inv_obj.compass_offsets[2];
MPL_LOGI("push compass offsets %hhd, %hhd, %hhd", data[0], data[1], data[2]);
result = inv_mpu_config_compass(inv_get_dl_config(),
inv_get_serial_handle(),
inv_get_serial_handle(), &config);
} else {
/* compute new values and store them */
result = inv_mpu_get_compass_config(inv_get_dl_config(),
inv_get_serial_handle(),
inv_get_serial_handle(), &config);
MPL_LOGI("pulled compass offsets %hhd %hhd %hhd", data[0], data[1], data[2]);
if(result == INV_SUCCESS) {
inv_obj.flags[INV_COMPASS_OFFSET_VALID] = 1;
inv_obj.compass_offsets[0] = data[0];
inv_obj.compass_offsets[1] = data[1];
inv_obj.compass_offsets[2] = data[2];
}
}
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
return result;
}
/**
* @brief Write a single register on the compass slave device, regardless
* of the bus it is connected to and the MPU's configuration.
* @param reg
* the register to write to on the slave compass device.
* @param val
* the value to write.
* @return INV_SUCCESS = 0 if successful. A non-zero error code otherwise.
*/
inv_error_t inv_compass_write_reg(unsigned char reg, unsigned char val)
{
struct ext_slave_config config;
unsigned char data[2];
inv_error_t result;
data[0] = reg;
data[1] = val;
config.key = MPU_SLAVE_WRITE_REGISTERS;
config.len = 2;
config.apply = true;
config.data = data;
result = inv_mpu_config_compass(inv_get_dl_config(),
inv_get_serial_handle(),
inv_get_serial_handle(), &config);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
return result;
}
