common: Implement accelerometer calibration
This change implements the hybrid accelerometer calibration algorithm described in https://drive.google.com/corp/drive/u/0/folders/13k8AWvVkCg8KUr1HhD2qv6_ob1ixgCbE 1. Waits until the device is still 2. Adds a still sample to an orientation accumulator - If the new sample is close to an existing one, they're merged. - If the new sample is unique, it is added to the list of orientations in a FIFO manner (may be evicting an older sample). - Once enough orientations have been gathered, run the kasa algorithm. - The kasa algorithm should yield a radius that's near 1g, since all the samples were added when still. If this isn't the case, we fall back on newton's method (which takes longer). BUG=b:138303429,chromium:1023858 BRANCH=None TEST=buildall with new unit tests Change-Id: I98bb0d365017d8a916b008c7c0c263345a9cddac Signed-off-by: Yuval Peress <peress@chromium.org> Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/1879716 Reviewed-by: Gwendal Grignou <gwendal@chromium.org>
This commit is contained in:
Yuval Peress
Commit Bot
parent
0fffe36e79
commit
dc77f10578
|
|
@ -0,0 +1,81 @@
|
|||
/* Copyright 2020 The Chromium OS Authors. All rights reserved.
|
||||
* Use of this source code is governed by a BSD-style license that can be
|
||||
* found in the LICENSE file.
|
||||
*/
|
||||
|
||||
#include "common.h"
|
||||
#include "console.h"
|
||||
#include "accel_cal.h"
|
||||
|
||||
#define CPRINTS(format, args...) cprints(CC_MOTION_SENSE, format, ##args)
|
||||
|
||||
#define TEMP_RANGE (CONFIG_ACCEL_CAL_MAX_TEMP - CONFIG_ACCEL_CAL_MIN_TEMP)
|
||||
|
||||
void accel_cal_reset(struct accel_cal *cal)
|
||||
{
|
||||
int i;
|
||||
|
||||
for (i = 0; i < cal->num_temp_windows; ++i) {
|
||||
kasa_reset(&(cal->algos[i].kasa_fit));
|
||||
newton_fit_reset(&(cal->algos[i].newton_fit));
|
||||
}
|
||||
}
|
||||
|
||||
static inline int compute_temp_gate(const struct accel_cal *cal, fp_t temp)
|
||||
{
|
||||
int gate = (int) fp_div(fp_mul(temp - CONFIG_ACCEL_CAL_MIN_TEMP,
|
||||
INT_TO_FP(cal->num_temp_windows)),
|
||||
TEMP_RANGE);
|
||||
|
||||
return gate < cal->num_temp_windows
|
||||
? gate : (cal->num_temp_windows - 1);
|
||||
}
|
||||
|
||||
bool accel_cal_accumulate(struct accel_cal *cal, uint32_t sample_time, fp_t x,
|
||||
fp_t y, fp_t z, fp_t temp)
|
||||
{
|
||||
struct accel_cal_algo *algo;
|
||||
|
||||
/* Test that we're within the temperature range. */
|
||||
if (temp >= CONFIG_ACCEL_CAL_MAX_TEMP ||
|
||||
temp <= CONFIG_ACCEL_CAL_MIN_TEMP)
|
||||
return false;
|
||||
|
||||
/* Test that we have a still sample. */
|
||||
if (!still_det_update(&cal->still_det, sample_time, x, y, z))
|
||||
return false;
|
||||
|
||||
/* We have a still sample, update x, y, and z to the mean. */
|
||||
x = cal->still_det.mean_x;
|
||||
y = cal->still_det.mean_y;
|
||||
z = cal->still_det.mean_z;
|
||||
|
||||
/* Compute the temp gate. */
|
||||
algo = &cal->algos[compute_temp_gate(cal, temp)];
|
||||
|
||||
kasa_accumulate(&algo->kasa_fit, x, y, z);
|
||||
if (newton_fit_accumulate(&algo->newton_fit, x, y, z)) {
|
||||
fp_t radius;
|
||||
|
||||
kasa_compute(&algo->kasa_fit, cal->bias, &radius);
|
||||
if (ABS(radius - FLOAT_TO_FP(1.0f)) <
|
||||
CONFIG_ACCEL_CAL_KASA_RADIUS_THRES)
|
||||
goto accel_cal_accumulate_success;
|
||||
|
||||
newton_fit_compute(&algo->newton_fit, cal->bias, &radius);
|
||||
if (ABS(radius - FLOAT_TO_FP(1.0f)) <
|
||||
CONFIG_ACCEL_CAL_NEWTON_RADIUS_THRES)
|
||||
goto accel_cal_accumulate_success;
|
||||
}
|
||||
|
||||
return false;
|
||||
|
||||
accel_cal_accumulate_success:
|
||||
cal->bias[X] = cal->bias[X];
|
||||
cal->bias[Y] = cal->bias[Y];
|
||||
cal->bias[Z] = cal->bias[Z];
|
||||
|
||||
accel_cal_reset(cal);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
|
@ -121,7 +121,7 @@ common-$(CONFIG_RWSIG)+=rwsig.o vboot/common.o
|
|||
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
|
||||
mat44.o vec3.o newton_fit.o accel_cal.o
|
||||
common-$(CONFIG_SHA1)+= sha1.o
|
||||
common-$(CONFIG_SHA256)+=sha256.o
|
||||
common-$(CONFIG_SOFTWARE_CLZ)+=clz.o
|
||||
|
|
|
|||
|
|
@ -0,0 +1,51 @@
|
|||
/* Copyright 2020 The Chromium OS Authors. All rights reserved.
|
||||
* Use of this source code is governed by a BSD-style license that can be
|
||||
* found in the LICENSE file.
|
||||
*/
|
||||
|
||||
/* Online accelerometer calibration */
|
||||
|
||||
#ifndef __CROS_EC_ACCEL_CAL_H
|
||||
#define __CROS_EC_ACCEL_CAL_H
|
||||
|
||||
#include "kasa.h"
|
||||
#include "newton_fit.h"
|
||||
#include "stdbool.h"
|
||||
#include "stillness_detector.h"
|
||||
|
||||
struct accel_cal_algo {
|
||||
struct kasa_fit kasa_fit;
|
||||
struct newton_fit newton_fit;
|
||||
};
|
||||
|
||||
struct accel_cal {
|
||||
struct still_det still_det;
|
||||
struct accel_cal_algo *algos;
|
||||
uint8_t num_temp_windows;
|
||||
fpv3_t bias;
|
||||
};
|
||||
|
||||
/**
|
||||
* Reset the accelerometer calibration object. This should only be called
|
||||
* once. The struct will reset automatically in accel_cal_accumulate when
|
||||
* a new calibration is computed.
|
||||
*
|
||||
* @param cal Pointer to the accel_cal struct to reset.
|
||||
*/
|
||||
void accel_cal_reset(struct accel_cal *cal);
|
||||
|
||||
/**
|
||||
* Add new reading to the accelerometer calibration.
|
||||
*
|
||||
* @param cal Pointer to the accel_cal struct to update.
|
||||
* @param sample_time The timestamp when the sample was taken.
|
||||
* @param x X component of the new reading.
|
||||
* @param y Y component of the new reading.
|
||||
* @param z Z component of the new reading.
|
||||
* @param temp The sensor's internal temperature in degrees C.
|
||||
* @return True if a new bias is available.
|
||||
*/
|
||||
bool accel_cal_accumulate(struct accel_cal *cal, uint32_t sample_time, fp_t x,
|
||||
fp_t y, fp_t z, fp_t temp);
|
||||
|
||||
#endif /* __CROS_EC_ACCEL_CAL_H */
|
||||
|
|
@ -2765,6 +2765,22 @@
|
|||
*/
|
||||
#undef CONFIG_TEMP_CACHE_STALE_THRES
|
||||
|
||||
/* Set minimum temperature for accelerometer calibration. */
|
||||
#undef CONFIG_ACCEL_CAL_MIN_TEMP
|
||||
|
||||
/* Set maximum temperature for accelerometer calibration. */
|
||||
#undef CONFIG_ACCEL_CAL_MAX_TEMP
|
||||
|
||||
/* Set threshold radius for using the Kasa algorithm in accelerometer bias
|
||||
* calculation (g).
|
||||
*/
|
||||
#undef CONFIG_ACCEL_CAL_KASA_RADIUS_THRES
|
||||
|
||||
/* Set threshold radius for using the Newton fit algorithm in accelerometer
|
||||
* bias calculation (g).
|
||||
*/
|
||||
#undef CONFIG_ACCEL_CAL_NEWTON_RADIUS_THRES
|
||||
|
||||
/* Include code to do online compass calibration */
|
||||
#undef CONFIG_MAG_CALIBRATE
|
||||
|
||||
|
|
@ -5251,4 +5267,23 @@
|
|||
#error "Online calibration requires CONFIG_FPU"
|
||||
#endif
|
||||
|
||||
/* Set default values for accelerometer calibration if not defined. */
|
||||
#ifdef CONFIG_ONLINE_CALIB
|
||||
#ifndef CONFIG_ACCEL_CAL_MIN_TEMP
|
||||
#define CONFIG_ACCEL_CAL_MIN_TEMP 0.0f
|
||||
#endif
|
||||
|
||||
#ifndef CONFIG_ACCEL_CAL_MAX_TEMP
|
||||
#define CONFIG_ACCEL_CAL_MAX_TEMP 45.0f
|
||||
#endif
|
||||
|
||||
#ifndef CONFIG_ACCEL_CAL_KASA_RADIUS_THRES
|
||||
#define CONFIG_ACCEL_CAL_KASA_RADIUS_THRES 0.001f
|
||||
#endif
|
||||
|
||||
#ifndef CONFIG_ACCEL_CAL_NEWTON_RADIUS_THRES
|
||||
#define CONFIG_ACCEL_CAL_NEWTON_RADIUS_THRES 0.001f
|
||||
#endif
|
||||
#endif /* CONFIG_ONLINE_CALIB */
|
||||
|
||||
#endif /* __CROS_EC_CONFIG_H */
|
||||
|
|
|
|||
|
|
@ -0,0 +1,137 @@
|
|||
/* Copyright 2020 The Chromium OS Authors. All rights reserved.
|
||||
* Use of this source code is governed by a BSD-style license that can be
|
||||
* found in the LICENSE file.
|
||||
*/
|
||||
|
||||
#include "common.h"
|
||||
#include "accel_cal.h"
|
||||
#include "test_util.h"
|
||||
#include "motion_sense.h"
|
||||
#include <math.h>
|
||||
|
||||
struct motion_sensor_t motion_sensors[] = {};
|
||||
const unsigned int motion_sensor_count = ARRAY_SIZE(motion_sensors);
|
||||
|
||||
struct accel_cal_algo algos[2] = {
|
||||
{
|
||||
.newton_fit = NEWTON_FIT(8, 1, 0.01f, 0.25f, 1.0e-8f, 100),
|
||||
},
|
||||
{
|
||||
.newton_fit = NEWTON_FIT(8, 1, 0.01f, 0.25f, 1.0e-8f, 100),
|
||||
}
|
||||
};
|
||||
|
||||
struct accel_cal cal = {
|
||||
.still_det = STILL_DET(0.00025f, 800 * MSEC, 1200 * MSEC, 5),
|
||||
.algos = algos,
|
||||
.num_temp_windows = ARRAY_SIZE(algos),
|
||||
};
|
||||
|
||||
static bool accumulate(float x, float y, float z, float temperature)
|
||||
{
|
||||
return accel_cal_accumulate(&cal, 0, x, y, z, temperature)
|
||||
| accel_cal_accumulate(&cal, 200 * MSEC, x, y, z, temperature)
|
||||
| accel_cal_accumulate(&cal, 400 * MSEC, x, y, z, temperature)
|
||||
| accel_cal_accumulate(&cal, 600 * MSEC, x, y, z, temperature)
|
||||
| accel_cal_accumulate(&cal, 800 * MSEC, x, y, z, temperature)
|
||||
| accel_cal_accumulate(&cal, 1000 * MSEC, x, y, z, temperature);
|
||||
}
|
||||
|
||||
static int test_calibrated_correctly_with_kasa(void)
|
||||
{
|
||||
bool has_bias;
|
||||
|
||||
accumulate(1.01f, 0.01f, 0.01f, 21.0f);
|
||||
accumulate(-0.99f, 0.01f, 0.01f, 21.0f);
|
||||
accumulate(0.01f, 1.01f, 0.01f, 21.0f);
|
||||
accumulate(0.01f, -0.99f, 0.01f, 21.0f);
|
||||
accumulate(0.01f, 0.01f, 1.01f, 21.0f);
|
||||
accumulate(0.01f, 0.01f, -0.99f, 21.0f);
|
||||
accumulate(0.7171f, 0.7171f, 0.7171f, 21.0f);
|
||||
has_bias = accumulate(-0.6971f, -0.6971f, -0.6971f, 21.0f);
|
||||
|
||||
TEST_EQ(has_bias, true, "%d");
|
||||
TEST_NEAR(cal.bias[X], 0.01f, 0.0001f, "%f");
|
||||
TEST_NEAR(cal.bias[Y], 0.01f, 0.0001f, "%f");
|
||||
TEST_NEAR(cal.bias[Z], 0.01f, 0.0001f, "%f");
|
||||
|
||||
return EC_SUCCESS;
|
||||
}
|
||||
|
||||
static int test_calibrated_correctly_with_newton(void)
|
||||
{
|
||||
bool has_bias = false;
|
||||
struct kasa_fit kasa;
|
||||
fpv3_t kasa_bias;
|
||||
float kasa_radius;
|
||||
int i;
|
||||
float data[] = {
|
||||
1.00290f, 0.09170f, 0.09649f,
|
||||
0.95183f, 0.23626f, 0.25853f,
|
||||
0.95023f, 0.15387f, 0.31865f,
|
||||
0.97374f, 0.01639f, 0.27675f,
|
||||
0.88521f, 0.30212f, 0.39558f,
|
||||
0.92787f, 0.35157f, 0.21209f,
|
||||
0.95162f, 0.33173f, 0.10924f,
|
||||
0.98397f, 0.22644f, 0.07737f,
|
||||
};
|
||||
|
||||
kasa_reset(&kasa);
|
||||
for (i = 0; i < ARRAY_SIZE(data); i += 3) {
|
||||
TEST_EQ(has_bias, false, "%d");
|
||||
kasa_accumulate(&kasa, data[i], data[i + 1], data[i + 2]);
|
||||
has_bias = accumulate(data[i], data[i + 1], data[i + 2], 21.0f);
|
||||
}
|
||||
|
||||
kasa_compute(&kasa, kasa_bias, &kasa_radius);
|
||||
TEST_EQ(has_bias, true, "%d");
|
||||
/* Check that the bias is right */
|
||||
TEST_NEAR(cal.bias[X], 0.01f, 0.001f, "%f");
|
||||
TEST_NEAR(cal.bias[Y], 0.01f, 0.001f, "%f");
|
||||
TEST_NEAR(cal.bias[Z], 0.01f, 0.001f, "%f");
|
||||
/* Demonstrate that we got a better bias compared to kasa */
|
||||
TEST_LT(sqrtf(powf(cal.bias[X] - 0.01f, 2.0f) +
|
||||
powf(cal.bias[Y] - 0.01f, 2.0f) +
|
||||
powf(cal.bias[Z] - 0.01f, 2.0f)),
|
||||
sqrtf(powf(kasa_bias[X] - 0.01f, 2.0f) +
|
||||
powf(kasa_bias[Y] - 0.01f, 2.0f) +
|
||||
powf(kasa_bias[Z] - 0.01f, 2.0f)),
|
||||
"%f");
|
||||
|
||||
return EC_SUCCESS;
|
||||
}
|
||||
|
||||
static int test_temperature_gates(void)
|
||||
{
|
||||
bool has_bias;
|
||||
|
||||
accumulate(1.01f, 0.01f, 0.01f, 21.0f);
|
||||
accumulate(-0.99f, 0.01f, 0.01f, 21.0f);
|
||||
accumulate(0.01f, 1.01f, 0.01f, 21.0f);
|
||||
accumulate(0.01f, -0.99f, 0.01f, 21.0f);
|
||||
accumulate(0.01f, 0.01f, 1.01f, 21.0f);
|
||||
accumulate(0.01f, 0.01f, -0.99f, 21.0f);
|
||||
accumulate(0.7171f, 0.7171f, 0.7171f, 21.0f);
|
||||
has_bias = accumulate(-0.6971f, -0.6971f, -0.6971f, 31.0f);
|
||||
|
||||
TEST_EQ(has_bias, false, "%d");
|
||||
|
||||
return EC_SUCCESS;
|
||||
}
|
||||
|
||||
void before_test(void)
|
||||
{
|
||||
cal.still_det = STILL_DET(0.00025f, 800 * MSEC, 1200 * MSEC, 5);
|
||||
accel_cal_reset(&cal);
|
||||
}
|
||||
|
||||
void run_test(void)
|
||||
{
|
||||
test_reset();
|
||||
|
||||
RUN_TEST(test_calibrated_correctly_with_kasa);
|
||||
RUN_TEST(test_calibrated_correctly_with_newton);
|
||||
RUN_TEST(test_temperature_gates);
|
||||
|
||||
test_print_result();
|
||||
}
|
||||
|
|
@ -0,0 +1,10 @@
|
|||
/* Copyright 2019 The Chromium OS Authors. All rights reserved.
|
||||
* Use of this source code is governed by a BSD-style license that can be
|
||||
* found in the LICENSE file.
|
||||
*/
|
||||
|
||||
/**
|
||||
* See CONFIG_TASK_LIST in config.h for details.
|
||||
*/
|
||||
#define CONFIG_TEST_TASK_LIST \
|
||||
TASK_TEST(MOTIONSENSE, motion_sense_task, NULL, TASK_STACK_SIZE)
|
||||
|
|
@ -11,7 +11,8 @@ test-list-y ?= pingpong timer_calib timer_dos timer_jump mutex utils utils_str
|
|||
ifneq ($(TEST_LIST_HOST),)
|
||||
test-list-host=$(TEST_LIST_HOST)
|
||||
else
|
||||
test-list-host = aes
|
||||
test-list-host = accel_cal
|
||||
test-list-host += aes
|
||||
test-list-host += base32
|
||||
test-list-host += battery_get_params_smart
|
||||
test-list-host += bklight_lid
|
||||
|
|
@ -97,6 +98,7 @@ test-list-host += x25519
|
|||
test-list-host += stillness_detector
|
||||
endif
|
||||
|
||||
accel_cal-y=accel_cal.o
|
||||
aes-y=aes.o
|
||||
base32-y=base32.o
|
||||
battery_get_params_smart-y=battery_get_params_smart.o
|
||||
|
|
|
|||
|
|
@ -105,6 +105,15 @@
|
|||
#define CONFIG_ONLINE_CALIB
|
||||
#endif
|
||||
|
||||
#ifdef TEST_ACCEL_CAL
|
||||
#define CONFIG_FPU
|
||||
#define CONFIG_ONLINE_CALIB
|
||||
#define CONFIG_ACCEL_CAL_MIN_TEMP 20.0f
|
||||
#define CONFIG_ACCEL_CAL_MAX_TEMP 40.0f
|
||||
#define CONFIG_ACCEL_CAL_KASA_RADIUS_THRES 0.1f
|
||||
#define CONFIG_ACCEL_CAL_NEWTON_RADIUS_THRES 0.1f
|
||||
#endif
|
||||
|
||||
#ifdef TEST_NEWTON_FIT
|
||||
#define CONFIG_FPU
|
||||
#define CONFIG_ONLINE_CALIB
|
||||
|
|
|
|||
Loading…
Reference in New Issue