diff options
-rw-r--r-- | common/build.mk | 3 | ||||
-rw-r--r-- | common/online_calibration.c | 44 | ||||
-rw-r--r-- | include/mag_cal.h | 8 | ||||
-rw-r--r-- | test/online_calibration.c | 36 |
4 files changed, 79 insertions, 12 deletions
diff --git a/common/build.mk b/common/build.mk index d21baf5aa7..8bb9197596 100644 --- a/common/build.mk +++ b/common/build.mk @@ -119,7 +119,8 @@ common-$(CONFIG_RWSIG_TYPE_RWSIG)+=vboot/vb21_lib.o common-$(CONFIG_MATH_UTIL)+=math_util.o common-$(CONFIG_ONLINE_CALIB)+=stillness_detector.o kasa.o math_util.o \ mat44.o vec3.o newton_fit.o accel_cal.o online_calibration.o \ - mkbp_event.o + mkbp_event.o mag_cal.o math_util.o mat33.o +common-$(CONFIG_SHA1)+= sha1.o common-$(CONFIG_SHA256)+=sha256.o common-$(CONFIG_SOFTWARE_CLZ)+=clz.o common-$(CONFIG_SOFTWARE_CTZ)+=ctz.o diff --git a/common/online_calibration.c b/common/online_calibration.c index dd2c154260..6c090a6abc 100644 --- a/common/online_calibration.c +++ b/common/online_calibration.c @@ -8,6 +8,7 @@ #include "hwtimer.h" #include "online_calibration.h" #include "common.h" +#include "mag_cal.h" #include "util.h" #include "vec3.h" #include "task.h" @@ -108,8 +109,11 @@ void online_calibration_init(void) switch (s->type) { case MOTIONSENSE_TYPE_ACCEL: { - accel_cal_reset((struct accel_cal *) - type_specific_data); + accel_cal_reset((struct accel_cal *)type_specific_data); + break; + } + case MOTIONSENSE_TYPE_MAG: { + init_mag_cal((struct mag_cal_t *)type_specific_data); break; } default: @@ -153,14 +157,11 @@ int online_calibration_process_data( struct motion_sensor_t *sensor, uint32_t timestamp) { + size_t sensor_num = motion_sensors - sensor; int rc; int temperature; struct online_calib_data *calib_data; - rc = get_temperature(sensor, &temperature); - if (rc != EC_SUCCESS) - return rc; - calib_data = sensor->online_calib_data; switch (sensor->type) { case MOTIONSENSE_TYPE_ACCEL: { @@ -168,11 +169,14 @@ int online_calibration_process_data( (struct accel_cal *)(calib_data->type_specific_data); fpv3_t fdata; + /* Temperature is required for accelerometer calibration. */ + rc = get_temperature(sensor, &temperature); + if (rc != EC_SUCCESS) + return rc; + data_int16_to_fp(sensor, data->data, fdata); if (accel_cal_accumulate(cal, timestamp, fdata[X], fdata[Y], fdata[Z], temperature)) { - int sensor_num = motion_sensors - sensor; - mutex_lock(&g_calib_cache_mutex); /* Convert result to the right scale. */ data_fp_to_int16(sensor, cal->bias, calib_data->cache); @@ -185,6 +189,30 @@ int online_calibration_process_data( } break; } + case MOTIONSENSE_TYPE_MAG: { + struct mag_cal_t *cal = + (struct mag_cal_t *) (calib_data->type_specific_data); + int idata[] = { + (int)data->data[X], + (int)data->data[Y], + (int)data->data[Z], + }; + + if (mag_cal_update(cal, idata)) { + mutex_lock(&g_calib_cache_mutex); + /* Copy the values */ + calib_data->cache[X] = cal->bias[X]; + calib_data->cache[Y] = cal->bias[Y]; + calib_data->cache[Z] = cal->bias[Z]; + /* Set valid and dirty. */ + sensor_calib_cache_valid_map |= BIT(sensor_num); + sensor_calib_cache_dirty_map |= BIT(sensor_num); + mutex_unlock(&g_calib_cache_mutex); + /* Notify the AP. */ + mkbp_send_event(EC_MKBP_EVENT_ONLINE_CALIBRATION); + } + break; + } default: break; } diff --git a/include/mag_cal.h b/include/mag_cal.h index a730fcbe59..61b24c7da9 100644 --- a/include/mag_cal.h +++ b/include/mag_cal.h @@ -29,5 +29,13 @@ struct mag_cal_t { void init_mag_cal(struct mag_cal_t *moc); +/** + * Update the magnetometer calibration structure and possibly compute the new + * bias. + * + * @param moc Pointer to the magnetometer struct to update. + * @param v The new data. + * @return 1 if a new calibration value is available, 0 otherwise. + */ int mag_cal_update(struct mag_cal_t *moc, const intv3_t v); #endif /* __CROS_EC_MAG_CAL_H */ diff --git a/test/online_calibration.c b/test/online_calibration.c index 622ea6232d..e9fff2430e 100644 --- a/test/online_calibration.c +++ b/test/online_calibration.c @@ -6,6 +6,7 @@ #include "accel_cal.h" #include "accelgyro.h" #include "hwtimer.h" +#include "mag_cal.h" #include "online_calibration.h" #include "test_util.h" #include "timer.h" @@ -69,8 +70,6 @@ static struct accelgyro_drv mock_sensor_driver = { .get_range = mock_get_range, }; -static struct accelgyro_drv empty_sensor_driver = {}; - static struct accel_cal_algo base_accel_cal_algos[] = { { .newton_fit = NEWTON_FIT(4, 15, FLOAT_TO_FP(0.01f), @@ -86,6 +85,8 @@ static struct accel_cal base_accel_cal_data = { .num_temp_windows = ARRAY_SIZE(base_accel_cal_algos), }; +static struct mag_cal_t lid_mag_cal_data; + static bool next_accel_cal_accumulate_result; static fpv3_t next_accel_cal_bias; @@ -110,7 +111,11 @@ struct motion_sensor_t motion_sensors[] = { }, }, [LID] = { - .drv = &empty_sensor_driver, + .type = MOTIONSENSE_TYPE_MAG, + .drv = &mock_sensor_driver, + .online_calib_data[0] = { + .type_specific_data = &lid_mag_cal_data, + } }, }; @@ -216,6 +221,30 @@ static int test_new_calibration_value(void) return EC_SUCCESS; } +int test_mag_reading_updated_cal(void) +{ + struct mag_cal_t expected_results; + struct ec_response_motion_sensor_data data; + int rc; + int test_values[] = { 207, -17, -37 }; + + data.sensor_num = LID; + data.data[X] = test_values[X]; + data.data[Y] = test_values[Y]; + data.data[Z] = test_values[Z]; + + init_mag_cal(&expected_results); + mag_cal_update(&expected_results, test_values); + + rc = online_calibration_process_data( + &data, &motion_sensors[LID], __hw_clock_source_read()); + TEST_EQ(rc, EC_SUCCESS, "%d"); + TEST_EQ(expected_results.kasa_fit.nsamples, + lid_mag_cal_data.kasa_fit.nsamples, "%d"); + + return EC_SUCCESS; +} + void before_test(void) { mock_read_temp_results = NULL; @@ -230,6 +259,7 @@ void run_test(int argc, char **argv) RUN_TEST(test_read_temp_from_cache_on_stage); RUN_TEST(test_read_temp_twice_after_cache_stale); RUN_TEST(test_new_calibration_value); + RUN_TEST(test_mag_reading_updated_cal); test_print_result(); } |