diff options
| -rw-r--r-- | common/build.mk | 2 | ||||
| -rw-r--r-- | common/motion_sense.c | 2 | ||||
| -rw-r--r-- | common/motion_sense_fifo.c | 107 | ||||
| -rw-r--r-- | common/online_calibration.c | 73 | ||||
| -rw-r--r-- | include/online_calibration.h | 29 | ||||
| -rw-r--r-- | test/build.mk | 2 | ||||
| -rw-r--r-- | test/motion_sense_fifo.c | 99 | ||||
| -rw-r--r-- | test/online_calibration.c | 134 | ||||
| -rw-r--r-- | test/online_calibration.tasklist | 11 | ||||
| -rw-r--r-- | test/test_config.h | 20 |
10 files changed, 329 insertions, 150 deletions
diff --git a/common/build.mk b/common/build.mk index 56608afb88..6f3c8baada 100644 --- a/common/build.mk +++ b/common/build.mk @@ -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 accel_cal.o + mat44.o vec3.o newton_fit.o accel_cal.o online_calibration.o common-$(CONFIG_SHA1)+= sha1.o common-$(CONFIG_SHA256)+=sha256.o common-$(CONFIG_SOFTWARE_CLZ)+=clz.o diff --git a/common/motion_sense.c b/common/motion_sense.c index 5f3f187ff0..8bcad0edcf 100644 --- a/common/motion_sense.c +++ b/common/motion_sense.c @@ -322,6 +322,8 @@ static inline int motion_sense_init(struct motion_sensor_t *sensor) { int ret, cnt = 3; + BUILD_ASSERT(SENSOR_COUNT < 32); + /* Initialize accelerometers. */ do { ret = sensor->drv->init(sensor); diff --git a/common/motion_sense_fifo.c b/common/motion_sense_fifo.c index b1a849d8c2..3ae4317ed2 100644 --- a/common/motion_sense_fifo.c +++ b/common/motion_sense_fifo.c @@ -12,6 +12,7 @@ #include "task.h" #include "util.h" #include "math_util.h" +#include "online_calibration.h" #define CPRINTS(format, args...) cprints(CC_MOTION_SENSE, format, ## args) @@ -52,20 +53,6 @@ static int fifo_lost; static struct fifo_staged fifo_staged; /** - * Entry of the temperature cache - * @temp: The temperature that's cached (-1 if invalid) - * @timestamp: The timestamp at which the temperature was cached - */ -struct temp_cache_entry { - int temp; - uint32_t timestamp; -}; - -/** Cache for internal sensor temperatures. */ -STATIC_IF(CONFIG_ONLINE_CALIB) - struct temp_cache_entry sensor_temp_cache[SENSOR_COUNT]; - -/** * Cached expected timestamp per sensor. If a sensor's timestamp pre-dates this * timestamp it will be fast forwarded. */ @@ -206,6 +193,19 @@ static void fifo_ensure_space(void) } /** + * Test if a given timestamp is the first timestamp seen by a given sensor + * number. + * + * @param sensor_num the sensor index to test. + * @return True if the given sensor index has not seen a timestamp yet. + */ +static inline bool is_new_timestamp(uint8_t sensor_num) +{ + return sensor_num < SENSOR_COUNT && + !(next_timestamp_initialized & BIT(sensor_num)); +} + +/** * Stage a single data unit to the motion sense fifo. Note that for the AP to * see this data, it must be committed. * @@ -226,16 +226,36 @@ static void fifo_stage_unit( for (i = 0; i < valid_data; i++) sensor->xyz[i] = data->data[i]; + /* + * For timestamps, update the next value of the sensor's timestamp + * if this timestamp is considered new. + */ + if (data->flags & MOTIONSENSE_SENSOR_FLAG_TIMESTAMP && + is_new_timestamp(data->sensor_num)) { + next_timestamp[data->sensor_num].next = + next_timestamp[data->sensor_num].prev = data->timestamp; + next_timestamp_initialized |= BIT(data->sensor_num); + } + /* For valid sensors, check if AP really needs this data */ if (valid_data) { - int removed; - - if (sensor->oversampling_ratio == 0) - goto stage_unit_end; - removed = sensor->oversampling++; - sensor->oversampling %= sensor->oversampling_ratio; - if (removed) - goto stage_unit_end; + int removed = 0; + + if (sensor->oversampling_ratio == 0) { + removed = 1; + } else { + removed = sensor->oversampling++; + sensor->oversampling %= sensor->oversampling_ratio; + } + if (removed) { + mutex_unlock(&g_sensor_mutex); + if (IS_ENABLED(CONFIG_ONLINE_CALIB) && + next_timestamp_initialized & BIT(data->sensor_num)) + online_calibration_process_data( + data, sensor, + next_timestamp[data->sensor_num].next); + return; + } } /* Make sure we have room for the data */ @@ -259,7 +279,8 @@ static void fifo_stage_unit( * address 0. Just don't add any data to the queue instead. */ CPRINTS("Failed to get write chunk for new fifo data!"); - goto stage_unit_end; + mutex_unlock(&g_sensor_mutex); + return; } /* @@ -284,7 +305,6 @@ static void fifo_stage_unit( ++fifo_staged.sample_count[data->sensor_num] > 1) fifo_staged.requires_spreading = 1; -stage_unit_end: mutex_unlock(&g_sensor_mutex); } @@ -292,14 +312,16 @@ stage_unit_end: * Stage an entry representing a single timestamp. * * @param timestamp The timestamp to add to the fifo. + * @param sensor_num The sensor number that this timestamp came from (use 0xff + * for unknown). */ -static void fifo_stage_timestamp(uint32_t timestamp) +static void fifo_stage_timestamp(uint32_t timestamp, uint8_t sensor_num) { struct ec_response_motion_sensor_data vector; vector.flags = MOTIONSENSE_SENSOR_FLAG_TIMESTAMP; vector.timestamp = timestamp; - vector.sensor_num = 0; + vector.sensor_num = sensor_num; fifo_stage_unit(&vector, NULL, 0); } @@ -319,11 +341,8 @@ peek_fifo_staged(size_t offset) void motion_sense_fifo_init(void) { - size_t i; - if (IS_ENABLED(CONFIG_ONLINE_CALIB)) - for (i = 0; i < ARRAY_SIZE(sensor_temp_cache); i++) - sensor_temp_cache[i].temp = -1; + online_calibration_init(); } int motion_sense_fifo_wake_up_needed(void) @@ -359,7 +378,7 @@ void motion_sense_fifo_insert_async_event( inline void motion_sense_fifo_add_timestamp(uint32_t timestamp) { - fifo_stage_timestamp(timestamp); + fifo_stage_timestamp(timestamp, 0xff); motion_sense_fifo_commit_data(); } @@ -373,24 +392,7 @@ void motion_sense_fifo_stage_data( /* First entry, save the time for spreading later. */ if (!fifo_staged.count) fifo_staged.read_ts = __hw_clock_source_read(); - fifo_stage_timestamp(time); - } - if (IS_ENABLED(CONFIG_ONLINE_CALIB) && sensor->drv->read_temp) { - struct temp_cache_entry *entry = - &sensor_temp_cache[motion_sensors - sensor]; - uint32_t now = __hw_clock_source_read(); - - if (entry->temp < 0 || - time_until(entry->timestamp, now) > - CONFIG_TEMP_CACHE_STALE_THRES) { - int temp; - int rc = sensor->drv->read_temp(sensor, &temp); - - if (rc == EC_SUCCESS) { - entry->temp = temp; - entry->timestamp = now; - } - } + fifo_stage_timestamp(time, data->sensor_num); } fifo_stage_unit(data, sensor, valid_data); } @@ -494,6 +496,13 @@ commit_data_end: fifo_staged.requires_spreading ? data_periods[sensor_num] : motion_sensors[sensor_num].collection_rate; + + /* Update online calibration if enabled. */ + data = peek_fifo_staged(i); + if (IS_ENABLED(CONFIG_ONLINE_CALIB)) + online_calibration_process_data( + data, &motion_sensors[sensor_num], + next_timestamp[sensor_num].prev); } /* Advance the tail and clear the staged metadata. */ diff --git a/common/online_calibration.c b/common/online_calibration.c new file mode 100644 index 0000000000..5626a70107 --- /dev/null +++ b/common/online_calibration.c @@ -0,0 +1,73 @@ +/* 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 "accelgyro.h" +#include "hwtimer.h" +#include "online_calibration.h" +#include "common.h" + +/** Entry of the temperature cache */ +struct temp_cache_entry { + /** The temperature that's cached (-1 if invalid) */ + int temp; + /** The timestamp at which the temperature was cached */ + uint32_t timestamp; +}; + +/** Cache for internal sensor temperatures. */ +static struct temp_cache_entry sensor_temp_cache[SENSOR_COUNT]; + +static int get_temperature(struct motion_sensor_t *sensor, int *temp) +{ + struct temp_cache_entry *entry = + &sensor_temp_cache[motion_sensors - sensor]; + uint32_t now; + + if (sensor->drv->read_temp == NULL) + return EC_ERROR_UNIMPLEMENTED; + + now = __hw_clock_source_read(); + if (entry->temp < 0 || + time_until(entry->timestamp, now) > CONFIG_TEMP_CACHE_STALE_THRES) { + int t; + int rc = sensor->drv->read_temp(sensor, &t); + + if (rc == EC_SUCCESS) { + entry->temp = t; + entry->timestamp = now; + } else { + return rc; + } + } + + *temp = entry->temp; + return EC_SUCCESS; +} + +void online_calibration_init(void) +{ + size_t i; + + for (i = 0; i < ARRAY_SIZE(sensor_temp_cache); i++) + sensor_temp_cache[i].temp = -1; +} + +int online_calibration_process_data( + struct ec_response_motion_sensor_data *data, + struct motion_sensor_t *sensor, + uint32_t timestamp) +{ + int rc; + int temperature; + + rc = get_temperature(sensor, &temperature); + /* + * TODO actual implementation will come in following CLs. + * The goal of this change is to establish the interface of + * online calibration with the rest of the code base. + */ + return rc; +} + diff --git a/include/online_calibration.h b/include/online_calibration.h new file mode 100644 index 0000000000..5d22ec5d5f --- /dev/null +++ b/include/online_calibration.h @@ -0,0 +1,29 @@ +/* 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. + */ + +#ifndef __CROS_EC_ONLINE_CALIBRATION_H +#define __CROS_EC_ONLINE_CALIBRATION_H + +#include "motion_sense.h" + +/** + * Initialize the online calibration caches. + */ +void online_calibration_init(void); + +/** + * Process a new data measurement from a given sensor. + * + * @param data Pointer to the data that should be processed. + * @param sensor Pointer to the sensor that generated the data. + * @param timestamp The time associated with the sample + * @return EC_SUCCESS when successful. + */ +int online_calibration_process_data( + struct ec_response_motion_sensor_data *data, + struct motion_sensor_t *sensor, + uint32_t timestamp); + +#endif /* __CROS_EC_ONLINE_CALIBRATION_H */ diff --git a/test/build.mk b/test/build.mk index 2b84ffe907..4235274312 100644 --- a/test/build.mk +++ b/test/build.mk @@ -56,6 +56,7 @@ test-list-host += motion_lid test-list-host += motion_sense_fifo test-list-host += mutex test-list-host += newton_fit +test-list-host += online_calibration test-list-host += pingpong test-list-host += pinweaver test-list-host += power_button @@ -136,6 +137,7 @@ motion_angle-y=motion_angle.o motion_angle_data_literals.o motion_common.o motion_angle_tablet-y=motion_angle_tablet.o motion_angle_data_literals_tablet.o motion_common.o motion_lid-y=motion_lid.o motion_sense_fifo-y=motion_sense_fifo.o +online_calibration-y=online_calibration.o kasa-y=kasa.o mutex-y=mutex.o newton_fit-y=newton_fit.o diff --git a/test/motion_sense_fifo.c b/test/motion_sense_fifo.c index 7e436bd404..9762fa61c9 100644 --- a/test/motion_sense_fifo.c +++ b/test/motion_sense_fifo.c @@ -14,46 +14,9 @@ #include "accelgyro.h" #include <sys/types.h> -struct mock_read_temp_result { - void *s; - int temp; - int ret; - int used_count; - struct mock_read_temp_result *next; -}; - -static struct mock_read_temp_result *mock_read_temp_results; - -static int mock_read_temp(const struct motion_sensor_t *s, int *temp) -{ - struct mock_read_temp_result *ptr = mock_read_temp_results; - - while (ptr) { - if (ptr->s == s) { - if (ptr->ret == EC_SUCCESS) - *temp = ptr->temp; - ptr->used_count++; - return ptr->ret; - } - ptr = ptr->next; - } - - return EC_ERROR_UNKNOWN; -} - -static struct accelgyro_drv mock_sensor_driver = { - .read_temp = mock_read_temp, -}; - -static struct accelgyro_drv empty_sensor_driver = {}; - struct motion_sensor_t motion_sensors[] = { - [BASE] = { - .drv = &mock_sensor_driver, - }, - [LID] = { - .drv = &empty_sensor_driver, - }, + [BASE] = {}, + [LID] = {}, }; const unsigned int motion_sensor_count = ARRAY_SIZE(motion_sensors); @@ -351,59 +314,6 @@ static int test_spread_double_commit_same_timestamp(void) return EC_SUCCESS; } -static int test_read_temp_on_stage(void) -{ - struct mock_read_temp_result expected = { &motion_sensors[BASE], 200, - EC_SUCCESS, 0, NULL }; - - mock_read_temp_results = &expected; - motion_sensors[0].oversampling_ratio = 1; - motion_sensors[0].collection_rate = 20000; /* ns */ - motion_sense_fifo_stage_data(data, motion_sensors, 3, - __hw_clock_source_read() - 10000); - - TEST_EQ(expected.used_count, 1, "%d"); - - return EC_SUCCESS; -} - -static int test_read_temp_from_cache_on_stage(void) -{ - struct mock_read_temp_result expected = { &motion_sensors[BASE], 200, - EC_SUCCESS, 0, NULL }; - - mock_read_temp_results = &expected; - motion_sensors[0].oversampling_ratio = 1; - motion_sensors[0].collection_rate = 20000; /* ns */ - motion_sense_fifo_stage_data(data, motion_sensors, 3, - __hw_clock_source_read() - 10000); - motion_sense_fifo_stage_data(data, motion_sensors, 3, - __hw_clock_source_read() - 5000); - - TEST_EQ(expected.used_count, 1, "%d"); - - return EC_SUCCESS; -} - -static int test_read_temp_twice_after_cache_stale(void) -{ - struct mock_read_temp_result expected = { &motion_sensors[BASE], 200, - EC_SUCCESS, 0, NULL }; - - mock_read_temp_results = &expected; - motion_sensors[0].oversampling_ratio = 1; - motion_sensors[0].collection_rate = 20000; /* ns */ - motion_sense_fifo_stage_data(data, motion_sensors, 3, - __hw_clock_source_read() - 10000); - sleep(2); - motion_sense_fifo_stage_data(data, motion_sensors, 3, - __hw_clock_source_read() - 5000); - - TEST_EQ(expected.used_count, 2, "%d"); - - return EC_SUCCESS; -} - void before_test(void) { motion_sense_fifo_commit_data(); @@ -412,13 +322,13 @@ void before_test(void) motion_sense_fifo_reset_wake_up_needed(); memset(data, 0, sizeof(data)); motion_sense_fifo_reset(); - mock_read_temp_results = NULL; } void run_test(void) { test_reset(); motion_sense_fifo_init(); + RUN_TEST(test_insert_async_event); RUN_TEST(test_wake_up_needed); RUN_TEST(test_wake_up_needed_overflow); @@ -432,9 +342,6 @@ void run_test(void) RUN_TEST(test_spread_data_in_window); RUN_TEST(test_spread_data_by_collection_rate); RUN_TEST(test_spread_double_commit_same_timestamp); - RUN_TEST(test_read_temp_on_stage); - RUN_TEST(test_read_temp_from_cache_on_stage); - RUN_TEST(test_read_temp_twice_after_cache_stale); test_print_result(); } diff --git a/test/online_calibration.c b/test/online_calibration.c new file mode 100644 index 0000000000..f9fe0ee068 --- /dev/null +++ b/test/online_calibration.c @@ -0,0 +1,134 @@ +/* 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 "online_calibration.h" +#include "test_util.h" +#include "hwtimer.h" +#include "timer.h" +#include "accelgyro.h" + +struct mock_read_temp_result { + void *s; + int temp; + int ret; + int used_count; + struct mock_read_temp_result *next; +}; + +static struct mock_read_temp_result *mock_read_temp_results; + +static int mock_read_temp(const struct motion_sensor_t *s, int *temp) +{ + struct mock_read_temp_result *ptr = mock_read_temp_results; + + while (ptr) { + if (ptr->s == s) { + if (ptr->ret == EC_SUCCESS) + *temp = ptr->temp; + ptr->used_count++; + return ptr->ret; + } + ptr = ptr->next; + } + + return EC_ERROR_UNKNOWN; +} + +static struct accelgyro_drv mock_sensor_driver = { + .read_temp = mock_read_temp, +}; + +static struct accelgyro_drv empty_sensor_driver = {}; + +struct motion_sensor_t motion_sensors[] = { + [BASE] = { + .drv = &mock_sensor_driver, + }, + [LID] = { + .drv = &empty_sensor_driver, + }, +}; + +const unsigned int motion_sensor_count = ARRAY_SIZE(motion_sensors); + +static int test_read_temp_on_stage(void) +{ + struct mock_read_temp_result expected = { &motion_sensors[BASE], 200, + EC_SUCCESS, 0, NULL }; + struct ec_response_motion_sensor_data data; + int rc; + + mock_read_temp_results = &expected; + data.sensor_num = 0; + rc = online_calibration_process_data( + &data, &motion_sensors[0], __hw_clock_source_read()); + + TEST_EQ(rc, EC_SUCCESS, "%d"); + TEST_EQ(expected.used_count, 1, "%d"); + + return EC_SUCCESS; +} + +static int test_read_temp_from_cache_on_stage(void) +{ + struct mock_read_temp_result expected = { &motion_sensors[BASE], 200, + EC_SUCCESS, 0, NULL }; + struct ec_response_motion_sensor_data data; + int rc; + + mock_read_temp_results = &expected; + data.sensor_num = 0; + rc = online_calibration_process_data( + &data, &motion_sensors[0], __hw_clock_source_read()); + TEST_EQ(rc, EC_SUCCESS, "%d"); + + rc = online_calibration_process_data( + &data, &motion_sensors[0], __hw_clock_source_read()); + TEST_EQ(rc, EC_SUCCESS, "%d"); + + TEST_EQ(expected.used_count, 1, "%d"); + + return EC_SUCCESS; +} + +static int test_read_temp_twice_after_cache_stale(void) +{ + struct mock_read_temp_result expected = { &motion_sensors[BASE], 200, + EC_SUCCESS, 0, NULL }; + struct ec_response_motion_sensor_data data; + int rc; + + mock_read_temp_results = &expected; + data.sensor_num = 0; + rc = online_calibration_process_data( + &data, &motion_sensors[0], __hw_clock_source_read()); + TEST_EQ(rc, EC_SUCCESS, "%d"); + + sleep(2); + rc = online_calibration_process_data( + &data, &motion_sensors[0], __hw_clock_source_read()); + TEST_EQ(rc, EC_SUCCESS, "%d"); + + TEST_EQ(expected.used_count, 2, "%d"); + + return EC_SUCCESS; +} + +void before_test(void) +{ + mock_read_temp_results = NULL; + online_calibration_init(); +} + +void run_test(void) +{ + test_reset(); + + RUN_TEST(test_read_temp_on_stage); + RUN_TEST(test_read_temp_from_cache_on_stage); + RUN_TEST(test_read_temp_twice_after_cache_stale); + + test_print_result(); +} diff --git a/test/online_calibration.tasklist b/test/online_calibration.tasklist new file mode 100644 index 0000000000..5b67239ff8 --- /dev/null +++ b/test/online_calibration.tasklist @@ -0,0 +1,11 @@ +/* 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. + */ + +/** + * See CONFIG_TASK_LIST in config.h for details. + */ +#define CONFIG_TEST_TASK_LIST \ + TASK_TEST(MOTIONSENSE, motion_sense_task, NULL, TASK_STACK_SIZE) + diff --git a/test/test_config.h b/test/test_config.h index 51275afb30..b0dac3deeb 100644 --- a/test/test_config.h +++ b/test/test_config.h @@ -72,6 +72,7 @@ #ifdef TEST_STILLNESS_DETECTOR #define CONFIG_FPU #define CONFIG_ONLINE_CALIB +#define CONFIG_TEMP_CACHE_STALE_THRES (5 * SECOND) #endif #ifdef TEST_FLOAT @@ -96,8 +97,6 @@ #define CONFIG_ACCEL_FIFO #define CONFIG_ACCEL_FIFO_SIZE 256 #define CONFIG_ACCEL_FIFO_THRES 10 -#define CONFIG_ONLINE_CALIB -#define CONFIG_TEMP_CACHE_STALE_THRES (1 * SECOND) #endif #ifdef TEST_KASA @@ -119,8 +118,21 @@ #define CONFIG_ONLINE_CALIB #endif -#if defined(TEST_MOTION_LID) || defined(TEST_MOTION_ANGLE) || \ - defined(TEST_MOTION_ANGLE_TABLET) || defined(TEST_MOTION_SENSE_FIFO) +#ifdef TEST_ONLINE_CALIBRATION +#define CONFIG_GPU +#define CONFIG_ONLINE_CALIB +#endif + +#if defined(CONFIG_ONLINE_CALIB) && \ + !defined(CONFIG_TEMP_CACHE_STALE_THRES) +#define CONFIG_TEMP_CACHE_STALE_THRES (1 * SECOND) +#endif /* CONFIG_ONLINE_CALIB && !CONFIG_TEMP_CACHE_STALE_THRES */ + +#if defined(TEST_MOTION_LID) || \ + defined(TEST_MOTION_ANGLE) || \ + defined(TEST_MOTION_ANGLE_TABLET) || \ + defined(TEST_MOTION_SENSE_FIFO) || \ + defined(CONFIG_ONLINE_CALIB) enum sensor_id { BASE, LID, |