diff options
Diffstat (limited to 'common/online_calibration.c')
| -rw-r--r-- | common/online_calibration.c | 156 |
1 files changed, 139 insertions, 17 deletions
diff --git a/common/online_calibration.c b/common/online_calibration.c index 6c090a6abc..ca7a5a620d 100644 --- a/common/online_calibration.c +++ b/common/online_calibration.c @@ -15,8 +15,9 @@ #include "ec_commands.h" #include "accel_cal.h" #include "mkbp_event.h" +#include "gyro_cal.h" -#define CPRINTS(format, args...) cprints(CC_MOTION_SENSE, format, ## args) +#define CPRINTS(format, args...) cprints(CC_MOTION_SENSE, format, ##args) #ifndef CONFIG_MKBP_EVENT #error "Must use CONFIG_MKBP_EVENT for online calibration" @@ -40,7 +41,7 @@ static int get_temperature(struct motion_sensor_t *sensor, int *temp) now = __hw_clock_source_read(); if (entry->last_temperature < 0 || time_until(entry->last_temperature_timestamp, now) > - CONFIG_TEMP_CACHE_STALE_THRES) { + CONFIG_TEMP_CACHE_STALE_THRES) { int t; int rc = sensor->drv->read_temp(sensor, &t); @@ -72,8 +73,8 @@ static void data_int16_to_fp(const struct motion_sensor_t *s, } } -static void data_fp_to_int16(const struct motion_sensor_t *s, - const fpv3_t data, int16_t *out) +static void data_fp_to_int16(const struct motion_sensor_t *s, const fpv3_t data, + int16_t *out) { int i; fp_t range = INT_TO_FP(s->drv->get_range(s)); @@ -82,14 +83,102 @@ static void data_fp_to_int16(const struct motion_sensor_t *s, int32_t iv; fp_t v = fp_div(data[i], range); - v = fp_mul(v, INT_TO_FP( - (data[i] >= INT_TO_FP(0)) ? 0x7fff : 0x8000)); + v = fp_mul(v, INT_TO_FP((data[i] >= INT_TO_FP(0)) ? 0x7fff : + 0x8000)); iv = FP_TO_INT(v); /* Check for overflow */ out[i] = CLAMP(iv, (int32_t)0xffff8000, (int32_t)0x00007fff); } } +/** + * Check a gyroscope for new bias. This function checks a given sensor (must be + * a gyroscope) for new bias values. If found, it will update the appropriate + * caches and notify the AP. + * + * @param sensor Pointer to the gyroscope sensor to check. + */ +static void check_gyro_cal_new_bias(struct motion_sensor_t *sensor) +{ + struct online_calib_data *calib_data = + (struct online_calib_data *)sensor->online_calib_data; + struct gyro_cal_data *data = + (struct gyro_cal_data *)calib_data->type_specific_data; + size_t sensor_num = motion_sensors - sensor; + int temp_out; + fpv3_t bias_out; + uint32_t timestamp_out; + + /* Check that we have a new bias. */ + if (data == NULL || calib_data == NULL || + !gyro_cal_new_bias_available(&data->gyro_cal)) + return; + + /* Read the calibration values. */ + gyro_cal_get_bias(&data->gyro_cal, bias_out, &temp_out, ×tamp_out); + + mutex_lock(&g_calib_cache_mutex); + /* Convert result to the right scale. */ + data_fp_to_int16(sensor, bias_out, calib_data->cache); + /* 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); +} + +/** + * Update the data stream (accel/mag) for a given sensor and data in all + * gyroscopes that are interested. + * + * @param sensor Pointer to the sensor that generated the data. + * @param data 3 floats/fixed point data points generated by the sensor. + * @param timestamp The timestamp at which the data was generated. + */ +static void update_gyro_cal(struct motion_sensor_t *sensor, fpv3_t data, + uint32_t timestamp) +{ + int i; + + /* + * Find gyroscopes, while we don't currently have instance where more + * than one are present in a board, this loop will work with any number + * of them. + */ + for (i = 0; i < SENSOR_COUNT; ++i) { + struct motion_sensor_t *s = motion_sensors + i; + struct gyro_cal_data *gyro_cal_data = + (struct gyro_cal_data *) + s->online_calib_data->type_specific_data; + + /* + * If we're not looking at a gyroscope OR if the calibration + * data is NULL, skip this sensor. + */ + if (s->type != MOTIONSENSE_TYPE_GYRO || gyro_cal_data == NULL) + continue; + + /* + * Update the appropriate data stream (accel/mag) depending on + * which sensors the gyroscope is tracking. + */ + if (sensor->type == MOTIONSENSE_TYPE_ACCEL && + gyro_cal_data->accel_sensor_id == sensor - motion_sensors) { + gyro_cal_update_accel(&gyro_cal_data->gyro_cal, + timestamp, data[X], data[Y], + data[Z]); + check_gyro_cal_new_bias(s); + } else if (sensor->type == MOTIONSENSE_TYPE_MAG && + gyro_cal_data->mag_sensor_id == + sensor - motion_sensors) { + gyro_cal_update_mag(&gyro_cal_data->gyro_cal, timestamp, + data[X], data[Y], data[Z]); + check_gyro_cal_new_bias(s); + } + } +} + void online_calibration_init(void) { size_t i; @@ -116,6 +205,12 @@ void online_calibration_init(void) init_mag_cal((struct mag_cal_t *)type_specific_data); break; } + case MOTIONSENSE_TYPE_GYRO: { + init_gyro_cal( + &((struct gyro_cal_data *)type_specific_data) + ->gyro_cal); + break; + } default: break; } @@ -141,8 +236,7 @@ bool online_calibration_read(int sensor_num, int16_t out[3]) has_valid = (sensor_calib_cache_valid_map & BIT(sensor_num)) != 0; if (has_valid) { /* Update data in out */ - memcpy(out, - motion_sensors[sensor_num].online_calib_data->cache, + memcpy(out, motion_sensors[sensor_num].online_calib_data->cache, sizeof(out)); /* Clear dirty bit */ sensor_calib_cache_dirty_map &= ~(1 << sensor_num); @@ -152,10 +246,9 @@ bool online_calibration_read(int sensor_num, int16_t out[3]) return has_valid; } -int online_calibration_process_data( - struct ec_response_motion_sensor_data *data, - struct motion_sensor_t *sensor, - uint32_t timestamp) +int online_calibration_process_data(struct ec_response_motion_sensor_data *data, + struct motion_sensor_t *sensor, + uint32_t timestamp) { size_t sensor_num = motion_sensors - sensor; int rc; @@ -169,20 +262,25 @@ int online_calibration_process_data( (struct accel_cal *)(calib_data->type_specific_data); fpv3_t fdata; + /* Convert data to fp. */ + data_int16_to_fp(sensor, data->data, fdata); + + /* Possibly update the gyroscope calibration. */ + update_gyro_cal(sensor, fdata, timestamp); + /* 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)) { mutex_lock(&g_calib_cache_mutex); /* Convert result to the right scale. */ data_fp_to_int16(sensor, cal->bias, calib_data->cache); /* Set valid and dirty. */ - sensor_calib_cache_valid_map |= BIT(sensor_num); - sensor_calib_cache_dirty_map |= BIT(sensor_num); + 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); @@ -191,12 +289,19 @@ int online_calibration_process_data( } case MOTIONSENSE_TYPE_MAG: { struct mag_cal_t *cal = - (struct mag_cal_t *) (calib_data->type_specific_data); + (struct mag_cal_t *)(calib_data->type_specific_data); int idata[] = { (int)data->data[X], (int)data->data[Y], (int)data->data[Z], }; + fpv3_t fdata; + + /* Convert data to fp. */ + data_int16_to_fp(sensor, data->data, fdata); + + /* Possibly update the gyroscope calibration. */ + update_gyro_cal(sensor, fdata, timestamp); if (mag_cal_update(cal, idata)) { mutex_lock(&g_calib_cache_mutex); @@ -213,10 +318,27 @@ int online_calibration_process_data( } break; } + case MOTIONSENSE_TYPE_GYRO: { + fpv3_t fdata; + + /* Temperature is required for gyro calibration. */ + rc = get_temperature(sensor, &temperature); + if (rc != EC_SUCCESS) + return rc; + + /* Convert data to fp. */ + data_int16_to_fp(sensor, data->data, fdata); + + /* Update gyroscope calibration. */ + gyro_cal_update_gyro( + &((struct gyro_cal_data *)calib_data->type_specific_data)->gyro_cal, + timestamp, fdata[X], fdata[Y], fdata[Z], temperature); + check_gyro_cal_new_bias(sensor); + break; + } default: break; } return EC_SUCCESS; } - |