handle yas530 offsets and stored cal better.
yas530 has offset registers that need to be persisted along with any
compass bias information. These changes, along with the associated
kernel update make that possible. Note that this change breaks
compatibility with previous versions of libmpl.so.
Change-Id: I2e4e6e0cf6330397f0a9e2e0536ff00dcc6d7a3a
diff --git a/libsensors/MPLSensor.cpp b/libsensors/MPLSensor.cpp
index 9ae2254..1cbe121 100644
--- a/libsensors/MPLSensor.cpp
+++ b/libsensors/MPLSensor.cpp
@@ -372,6 +372,12 @@
}
if (!mDmpStarted) {
+ if (mHaveGoodMpuCal) {
+ rv = inv_store_calibration();
+ LOGE_IF(rv != INV_SUCCESS,
+ "error: unable to store MPL calibration file");
+ mHaveGoodMpuCal = false;
+ }
LOGV("Starting DMP");
rv = inv_dmp_start();
LOGE_IF(rv != INV_SUCCESS, "unable to start dmp");
@@ -389,12 +395,6 @@
ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_STOP, 0);
}
clearIrqData(irq_set);
- if (mHaveGoodMpuCal) {
- rv = inv_store_calibration();
- LOGE_IF(rv != INV_SUCCESS,
- "error: unable to store MPL calibration file");
- mHaveGoodMpuCal = false;
- }
mDmpStarted = false;
mPollTime = -1;
@@ -532,7 +532,7 @@
//after the first no motion, the gyro should be calibrated well
if (val == 2) {
mMpuAccuracy = SENSOR_STATUS_ACCURACY_HIGH;
- if (mEnabled & (1 << MPLSensor::Gyro)) {
+ if ((inv_get_dl_config()->requested_sensors) & INV_THREE_AXIS_GYRO) {
//if gyros are on and we got a no motion, set a flag
// indicating that the cal file can be written.
mHaveGoodMpuCal = true;
diff --git a/mlsdk/mllite/compass.c b/mlsdk/mllite/compass.c
index 9716afd..e1a91f3 100644
--- a/mlsdk/mllite/compass.c
+++ b/mlsdk/mllite/compass.c
@@ -371,6 +371,75 @@
return result;
}
+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];
+ inv_reset_compass_calibration();
+ }
+ }
+
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+
+ return result;
+}
+
+inv_error_t inv_compass_check_range(void)
+{
+ struct ext_slave_config config;
+ unsigned char data[3];
+ inv_error_t result;
+
+ config.key = MPU_SLAVE_RANGE_CHECK;
+ config.len = 3;
+ config.apply = TRUE;
+ config.data = data;
+
+ result = inv_mpu_get_compass_config(inv_get_dl_config(),
+ inv_get_serial_handle(),
+ inv_get_serial_handle(), &config);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+
+ if(data[0] || data[1] || data[2]) {
+ /* some value clipped */
+ return INV_ERROR_COMPASS_DATA_ERROR;
+ }
+ return INV_SUCCESS;
+}
+
/**
* @}
- */
\ No newline at end of file
+ */
diff --git a/mlsdk/mllite/compass.h b/mlsdk/mllite/compass.h
index c5dffa4..ebdc816 100644
--- a/mlsdk/mllite/compass.h
+++ b/mlsdk/mllite/compass.h
@@ -50,6 +50,8 @@
inv_error_t inv_get_compass_data(long *data);
inv_error_t inv_set_compass_bias(long *bias);
unsigned short inv_get_compass_id(void);
+ inv_error_t inv_set_compass_offset(void);
+ inv_error_t inv_compass_check_range(void);
inv_error_t inv_compass_write_reg(unsigned char reg, unsigned char val);
inv_error_t inv_compass_read_reg(unsigned char reg, unsigned char *val);
inv_error_t inv_compass_read_scale(long *val);
diff --git a/mlsdk/mllite/ml.c b/mlsdk/mllite/ml.c
index 02ba2b3..cb995a0 100644
--- a/mlsdk/mllite/ml.c
+++ b/mlsdk/mllite/ml.c
@@ -480,6 +480,7 @@
return result;
}
+ inv_set_motion_state(INV_MOTION);
return result;
}
diff --git a/mlsdk/mllite/ml.h b/mlsdk/mllite/ml.h
index 14ba9db..4d35fd7 100644
--- a/mlsdk/mllite/ml.h
+++ b/mlsdk/mllite/ml.h
@@ -239,6 +239,7 @@
#define INV_GOT_GESTURE 0x0004
#define INV_MOTION_STATE_CHANGE 0x0006
+#define INV_COMPASS_OFFSET_VALID 0x0007
/*************************************************************************/
/* General */
@@ -307,6 +308,7 @@
long compass_test_bias[3];
long compass_test_scale[3];
long compass_asa[3];
+ long compass_offsets[3];
long compass_bias_error[3];
@@ -378,7 +380,7 @@
inv_error_t (*external_slave_callback)(struct inv_obj_t *);
int compass_accuracy;
- unsigned short flags[7];
+ unsigned short flags[8];
unsigned short suspend;
long no_motion_threshold;
diff --git a/mlsdk/mllite/ml_stored_data.c b/mlsdk/mllite/ml_stored_data.c
index f66fe72..31bf592 100644
--- a/mlsdk/mllite/ml_stored_data.c
+++ b/mlsdk/mllite/ml_stored_data.c
@@ -777,11 +777,12 @@
unsigned long long ll;
} dToLL;
- const unsigned int expLen = 2777;
+ const unsigned int expLen = 2782;
long bias[3];
int ptr = INV_CAL_HDR_LEN;
int i, j;
long long tmp;
+ inv_error_t result;
LOADCAL_LOG("Entering inv_load_cal_V4\n");
@@ -995,6 +996,25 @@
LOADCAL_LOG("compass_prev_m[%d] = %f\n", i, dToLL.db);
}
+ /* Load the compass offset flag and values */
+ inv_obj.flags[INV_COMPASS_OFFSET_VALID] = calData[ptr++];
+ inv_obj.compass_offsets[0] = calData[ptr++];
+ inv_obj.compass_offsets[1] = calData[ptr++];
+ inv_obj.compass_offsets[2] = calData[ptr++];
+
+ /* push the compass offset values to the device */
+ result = inv_set_compass_offset();
+
+ if (result == INV_SUCCESS) {
+ if (inv_compass_check_range() != INV_SUCCESS) {
+ MPL_LOGI("range check fail");
+ inv_obj.flags[INV_COMPASS_OFFSET_VALID] = 0;
+ inv_set_compass_offset();
+ }
+ }
+
+ /* load the compass accuracy */
+ inv_obj.compass_accuracy = calData[ptr++];
inv_obj.got_no_motion_bias = TRUE;
LOADCAL_LOG("got_no_motion_bias = 1\n");
inv_obj.cal_loaded_flag = TRUE;
@@ -1434,6 +1454,15 @@
inv_obj.compass_prev_m[i]);
}
+ /* store the compass offsets and validity flag */
+ calData[ptr++] = inv_obj.flags[INV_COMPASS_OFFSET_VALID];
+ calData[ptr++] = inv_obj.compass_offsets[0];
+ calData[ptr++] = inv_obj.compass_offsets[1];
+ calData[ptr++] = inv_obj.compass_offsets[2];
+
+ /* store the compass accuracy */
+ calData[ptr++] = (unsigned char)(inv_obj.compass_accuracy);
+
/* add a checksum */
chk = inv_checksum(calData + INV_CAL_HDR_LEN,
length - (INV_CAL_HDR_LEN + INV_CAL_CHK_LEN));
diff --git a/mlsdk/mllite/ml_stored_data.h b/mlsdk/mllite/ml_stored_data.h
index 02634d1..74c2b7c 100644
--- a/mlsdk/mllite/ml_stored_data.h
+++ b/mlsdk/mllite/ml_stored_data.h
@@ -39,7 +39,7 @@
Defines
*/
#define INV_CAL_ACCEL_LEN (12)
-#define INV_CAL_COMPASS_LEN (555)
+#define INV_CAL_COMPASS_LEN (555 + 5)
#define INV_CAL_HDR_LEN (6)
#define INV_CAL_CHK_LEN (4)
diff --git a/mlsdk/platform/include/linux/mpu.h b/mlsdk/platform/include/linux/mpu.h
index af60215..04fa7b6 100644
--- a/mlsdk/platform/include/linux/mpu.h
+++ b/mlsdk/platform/include/linux/mpu.h
@@ -76,6 +76,9 @@
MPU_SLAVE_SEARCHOFFSET,
/* AKM specific config keys */
MPU_SLAVE_READ_SCALE,
+ /* YAS specific config keys */
+ MPU_SLAVE_OFFSET_VALS,
+ MPU_SLAVE_RANGE_CHECK,
MPU_SLAVE_CONFIG_NUM_CONFIG_KEYS,
};