1134 lines
46 KiB
C++
1134 lines
46 KiB
C++
/*
|
|
* Copyright (C) 2017 The Android Open Source Project
|
|
*
|
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
|
* you may not use this file except in compliance with the License.
|
|
* You may obtain a copy of the License at
|
|
*
|
|
* http://www.apache.org/licenses/LICENSE-2.0
|
|
*
|
|
* Unless required by applicable law or agreed to in writing, software
|
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
* See the License for the specific language governing permissions and
|
|
* limitations under the License.
|
|
*/
|
|
#include "nano_calibration.h"
|
|
|
|
#include <cmath>
|
|
#include <cstring>
|
|
|
|
#ifdef DIVERSITY_CHECK_ENABLED
|
|
#include "calibration/common/diversity_checker.h"
|
|
#endif // DIVERSITY_CHECK_ENABLED
|
|
|
|
#include "calibration/util/cal_log.h"
|
|
#include "chre/util/nanoapp/log.h"
|
|
#include "common/math/macros.h"
|
|
|
|
namespace nano_calibration {
|
|
|
|
namespace {
|
|
|
|
// Nano calibration log macros.
|
|
#ifdef NANO_SENSOR_CAL_DBG_ENABLED
|
|
#define NANO_CAL_LOGD(tag, format, ...) \
|
|
chreLog(CHRE_LOG_DEBUG, "%s " format, tag, ##__VA_ARGS__)
|
|
|
|
#define NANO_CAL_LOGI(tag, format, ...) \
|
|
chreLog(CHRE_LOG_INFO, "%s " format, tag, ##__VA_ARGS__)
|
|
|
|
#define NANO_CAL_LOGW(tag, format, ...) \
|
|
chreLog(CHRE_LOG_WARN, "%s " format, tag, ##__VA_ARGS__)
|
|
|
|
#define NANO_CAL_LOGE(tag, format, ...) \
|
|
chreLog(CHRE_LOG_ERROR, "%s " format, tag, ##__VA_ARGS__)
|
|
#else
|
|
#define NANO_CAL_LOGD(tag, format, ...) \
|
|
chreLogNull(format, ##__VA_ARGS__)
|
|
|
|
#define NANO_CAL_LOGI(tag, format, ...) \
|
|
chreLogNull(format, ##__VA_ARGS__)
|
|
|
|
#define NANO_CAL_LOGW(tag, format, ...) \
|
|
chreLogNull(format, ##__VA_ARGS__)
|
|
|
|
#define NANO_CAL_LOGE(tag, format, ...) \
|
|
chreLogNull(format, ##__VA_ARGS__)
|
|
#endif // NANO_SENSOR_CAL_DBG_ENABLED
|
|
|
|
// Indicates and invalid sensor temperature.
|
|
constexpr float kInvalidTemperatureCelsius = -274.0f;
|
|
|
|
#ifdef GYRO_CAL_ENABLED
|
|
// Limits NanoSensorCal gyro notifications to once every minute.
|
|
constexpr uint64_t kNanoSensorCalMessageIntervalNanos = MIN_TO_NANOS(1);
|
|
#endif // GYRO_CAL_ENABLED
|
|
|
|
#ifdef MAG_CAL_ENABLED
|
|
// Unit conversion from nanoseconds to microseconds.
|
|
constexpr float kNanoToMicroseconds = 1e-3f;
|
|
#endif // MAG_CAL_ENABLED
|
|
|
|
#ifdef SPHERE_FIT_ENABLED
|
|
constexpr size_t kSamplesToAverageForOdrEstimateMag = 10;
|
|
|
|
// Helper function that estimates the ODR based on the incoming data timestamp.
|
|
void SamplingRateEstimate(struct SampleRateData *sample_rate_data,
|
|
float *mean_sampling_rate_hz,
|
|
uint64_t timestamp_nanos,
|
|
bool reset_stats) {
|
|
// If 'mean_sampling_rate_hz' is not nullptr then this function just reads
|
|
// out the estimate of the sampling rate.
|
|
if (mean_sampling_rate_hz != nullptr) {
|
|
if (sample_rate_data->num_samples > 1 &&
|
|
sample_rate_data->time_delta_accumulator > 0) {
|
|
// Computes the final mean calculation.
|
|
*mean_sampling_rate_hz =
|
|
sample_rate_data->num_samples /
|
|
(static_cast<float>(sample_rate_data->time_delta_accumulator) *
|
|
NANOS_TO_SEC);
|
|
} else {
|
|
// Not enough samples to compute a valid sample rate estimate. Indicate
|
|
// this with a -1 value.
|
|
*mean_sampling_rate_hz = -1.0f;
|
|
}
|
|
reset_stats = true;
|
|
}
|
|
|
|
// Resets the sampling rate mean estimator data.
|
|
if (reset_stats) {
|
|
sample_rate_data->last_timestamp_nanos = 0;
|
|
sample_rate_data->time_delta_accumulator = 0;
|
|
sample_rate_data->num_samples = 0;
|
|
return;
|
|
}
|
|
|
|
// Skip adding this data to the accumulator if:
|
|
// 1. A bad timestamp was received (i.e., time not monotonic).
|
|
// 2. 'last_timestamp_nanos' is zero.
|
|
if (timestamp_nanos <= sample_rate_data->last_timestamp_nanos ||
|
|
sample_rate_data->last_timestamp_nanos == 0) {
|
|
sample_rate_data->last_timestamp_nanos = timestamp_nanos;
|
|
return;
|
|
}
|
|
|
|
// Increments the number of samples.
|
|
sample_rate_data->num_samples++;
|
|
|
|
// Accumulate the time steps.
|
|
sample_rate_data->time_delta_accumulator += timestamp_nanos -
|
|
sample_rate_data->last_timestamp_nanos;
|
|
sample_rate_data->last_timestamp_nanos = timestamp_nanos;
|
|
}
|
|
#endif // SPHERE_FIT_ENABLED
|
|
|
|
// Helper function that resets calibration data to a known initial state.
|
|
void ResetCalParams(struct ashCalParams *cal_params) {
|
|
// Puts 'cal_params' into a known "default" pass-through state (i.e.,
|
|
// calibration data will not influence sensor streams).
|
|
memset(cal_params, 0, sizeof(struct ashCalParams));
|
|
|
|
// Sets 'scaleFactor' to unity.
|
|
cal_params->scaleFactor[0] = 1.0f;
|
|
cal_params->scaleFactor[1] = 1.0f;
|
|
cal_params->scaleFactor[2] = 1.0f;
|
|
}
|
|
|
|
// Helper function that resets calibration info to a known initial state.
|
|
void ResetCalInfo(struct ashCalInfo *cal_info) {
|
|
// Puts 'cal_info' into a known "default" pass-through state (i.e.,
|
|
// calibration info will not influence sensor streams).
|
|
memset(cal_info, 0, sizeof(struct ashCalInfo));
|
|
|
|
// Sets 'compMatrix' to the Identity matrix.
|
|
cal_info->compMatrix[0] = 1.0f;
|
|
cal_info->compMatrix[4] = 1.0f;
|
|
cal_info->compMatrix[8] = 1.0f;
|
|
|
|
cal_info->accuracy = ASH_CAL_ACCURACY_MEDIUM;
|
|
}
|
|
|
|
// Helper function to print out calibration data.
|
|
void PrintAshCalParams(const struct ashCalParams &cal_params, const char *tag) {
|
|
NANO_CAL_LOGI(tag, "Offset | Temp [Celsius]: %.6f, %.6f, %.6f | %.6f",
|
|
cal_params.offset[0], cal_params.offset[1],
|
|
cal_params.offset[2], cal_params.offsetTempCelsius);
|
|
|
|
NANO_CAL_LOGI(tag, "Temp Sensitivity [rad/sec/C]: %.6f, %.6f, %.6f",
|
|
cal_params.tempSensitivity[0], cal_params.tempSensitivity[1],
|
|
cal_params.tempSensitivity[2]);
|
|
NANO_CAL_LOGI(tag, "Temp Intercept [rad/sec]: %.6f, %.6f, %.6f",
|
|
cal_params.tempIntercept[0], cal_params.tempIntercept[1],
|
|
cal_params.tempIntercept[2]);
|
|
|
|
NANO_CAL_LOGI(tag, "Scale Factor: %.6f, %.6f, %.6f",
|
|
cal_params.scaleFactor[0], cal_params.scaleFactor[1],
|
|
cal_params.scaleFactor[2]);
|
|
|
|
NANO_CAL_LOGI(tag, "Cross-Axis in [yx, zx, zy] order: %.6f, %.6f, %.6f",
|
|
cal_params.crossAxis[0], cal_params.crossAxis[1],
|
|
cal_params.crossAxis[2]);
|
|
}
|
|
|
|
// Detects and converts Factory Calibration data into a format consumable by the
|
|
// runtime accelerometer calibration algorithm.
|
|
#ifdef ACCEL_CAL_ENABLED
|
|
void HandleAccelFactoryCalibration(struct ashCalParams *cal_params) {
|
|
// Checks for factory calibration data and performs any processing on the
|
|
// input to make it compatible with this runtime algorithm. NOTE: Factory
|
|
// calibrations are distinguished by 'offsetSource'=ASH_CAL_PARAMS_SOURCE_NONE
|
|
// and 'offsetTempCelsiusSource'=ASH_CAL_PARAMS_SOURCE_FACTORY.
|
|
bool factory_cal_detected =
|
|
cal_params->offsetSource == ASH_CAL_PARAMS_SOURCE_NONE &&
|
|
cal_params->offsetTempCelsiusSource == ASH_CAL_PARAMS_SOURCE_FACTORY;
|
|
|
|
if (factory_cal_detected) {
|
|
// Prints the received factory data.
|
|
PrintAshCalParams(*cal_params,"[NanoSensorCal:ACCEL_FACTORY_CAL]");
|
|
|
|
// Sets the parameter source to runtime calibration.
|
|
cal_params->offsetSource = ASH_CAL_PARAMS_SOURCE_RUNTIME;
|
|
cal_params->offsetTempCelsiusSource = ASH_CAL_PARAMS_SOURCE_RUNTIME;
|
|
|
|
// Ensures that the offset vector is zero in case it has been overwritten by
|
|
// mistake.
|
|
memset(cal_params->offset, 0, sizeof(cal_params->offset));
|
|
|
|
//TODO: Incorporate over-temperature offset compensation.
|
|
}
|
|
}
|
|
#endif // ACCEL_CAL_ENABLED
|
|
|
|
// Detects and converts Factory Calibration data into a format consumable by the
|
|
// runtime gyroscope calibration algorithm.
|
|
#ifdef GYRO_CAL_ENABLED
|
|
void HandleGyroFactoryCalibration(struct ashCalParams *cal_params) {
|
|
#ifdef OVERTEMPCAL_GYRO_ENABLED
|
|
// Checks for factory calibration data and performs any processing on the
|
|
// input to make it compatible with this runtime algorithm. NOTE: Factory
|
|
// calibrations are distinguished by 'offsetSource'=ASH_CAL_PARAMS_SOURCE_NONE
|
|
// and 'offsetTempCelsiusSource'=ASH_CAL_PARAMS_SOURCE_FACTORY
|
|
bool factory_cal_detected =
|
|
cal_params->offsetSource == ASH_CAL_PARAMS_SOURCE_NONE &&
|
|
cal_params->offsetTempCelsiusSource == ASH_CAL_PARAMS_SOURCE_FACTORY &&
|
|
cal_params->tempSensitivitySource == ASH_CAL_PARAMS_SOURCE_FACTORY &&
|
|
cal_params->tempInterceptSource == ASH_CAL_PARAMS_SOURCE_FACTORY;
|
|
|
|
if (factory_cal_detected) {
|
|
// Prints the received factory data.
|
|
PrintAshCalParams(*cal_params, "[NanoSensorCal:OTC_GYRO_FACTORY_CAL]");
|
|
|
|
#ifdef GYRO_OTC_FACTORY_CAL_ENABLED
|
|
// Factory OTC calibration initialization is ENABLED.
|
|
// Since the Factory-Cal OTC model is computed from raw measured data and
|
|
// the 'offset' at 'offsetTempCelsius' is removed from the input sensor
|
|
// stream, the intercept must be adjusted so that the runtime OTC produces a
|
|
// zero offset vector at 'offsetTempCelsius'.
|
|
for (size_t i = 0; i < 3; i++) {
|
|
// Shifts the OTC linear model intercept by 'offset_at_offsetTempCelsius'.
|
|
float offset_at_offsetTempCelsius =
|
|
cal_params->tempSensitivity[i] * cal_params->offsetTempCelsius +
|
|
cal_params->tempIntercept[i];
|
|
cal_params->tempIntercept[i] -= offset_at_offsetTempCelsius;
|
|
}
|
|
#else
|
|
// Factory OTC calibration initialization is DISABLED. This resets the
|
|
// AshCalParams and invalidates factory initialization. No factory
|
|
// initialized model data will be loaded.
|
|
ResetCalParams(cal_params);
|
|
#endif // GYRO_OTC_FACTORY_CAL_ENABLED
|
|
|
|
// Sets the parameter source to runtime calibration.
|
|
cal_params->offsetSource = ASH_CAL_PARAMS_SOURCE_RUNTIME;
|
|
cal_params->offsetTempCelsiusSource = ASH_CAL_PARAMS_SOURCE_RUNTIME;
|
|
cal_params->tempSensitivitySource = ASH_CAL_PARAMS_SOURCE_RUNTIME;
|
|
cal_params->tempInterceptSource = ASH_CAL_PARAMS_SOURCE_RUNTIME;
|
|
|
|
// Ensures that the offset vector is zero in case it has been overwritten by
|
|
// mistake.
|
|
memset(cal_params->offset, 0, sizeof(cal_params->offset));
|
|
}
|
|
#else
|
|
// Checks for factory calibration data and performs any processing on the
|
|
// input to make it compatible with this runtime algorithm.
|
|
bool factory_cal_detected =
|
|
cal_params->offsetSource == ASH_CAL_PARAMS_SOURCE_NONE &&
|
|
cal_params->offsetTempCelsiusSource == ASH_CAL_PARAMS_SOURCE_FACTORY;
|
|
|
|
if (factory_cal_detected) {
|
|
// Prints the received factory data.
|
|
PrintAshCalParams(*cal_params,"[NanoSensorCal:GYRO_FACTORY_CAL]");
|
|
|
|
// Sets the parameter source to runtime calibration.
|
|
cal_params->offsetSource = ASH_CAL_PARAMS_SOURCE_RUNTIME;
|
|
cal_params->offsetTempCelsiusSource = ASH_CAL_PARAMS_SOURCE_RUNTIME;
|
|
|
|
// Ensures that the offset vector is zero in case it has been overwritten by
|
|
// mistake.
|
|
memset(cal_params->offset, 0, sizeof(cal_params->offset));
|
|
}
|
|
#endif // OVERTEMPCAL_GYRO_ENABLED
|
|
}
|
|
#endif // GYRO_CAL_ENABLED
|
|
|
|
// Detects and converts Factory Calibration data into a format consumable by the
|
|
// runtime magnetometer calibration algorithm.
|
|
#ifdef MAG_CAL_ENABLED
|
|
void HandleMagFactoryCalibration(struct ashCalParams *cal_params) {
|
|
// Checks for factory calibration data and performs any processing on the
|
|
// input to make it compatible with this runtime algorithm.
|
|
bool factory_cal_detected =
|
|
cal_params->offsetSource == ASH_CAL_PARAMS_SOURCE_NONE &&
|
|
cal_params->offsetTempCelsiusSource == ASH_CAL_PARAMS_SOURCE_FACTORY;
|
|
|
|
if (factory_cal_detected) {
|
|
// Prints the received factory data.
|
|
PrintAshCalParams(*cal_params,"[NanoSensorCal:MAG_FACTORY_CAL]");
|
|
|
|
// Sets the parameter source to runtime calibration.
|
|
cal_params->offsetSource = ASH_CAL_PARAMS_SOURCE_RUNTIME;
|
|
cal_params->offsetTempCelsiusSource = ASH_CAL_PARAMS_SOURCE_RUNTIME;
|
|
|
|
// Ensures that the offset vector is zero in case it has been overwritten by
|
|
// mistake.
|
|
memset(cal_params->offset, 0, sizeof(cal_params->offset));
|
|
}
|
|
}
|
|
#endif // MAG_CAL_ENABLED
|
|
|
|
} // anonymous namespace
|
|
|
|
NanoSensorCal::NanoSensorCal() {
|
|
// Initializes the calibration data to a known default state.
|
|
ResetCalParams(&accel_cal_params_);
|
|
ResetCalParams(&gyro_cal_params_);
|
|
ResetCalParams(&mag_cal_params_);
|
|
|
|
// Initializes sensor temperature.
|
|
temperature_celsius_ = kInvalidTemperatureCelsius;
|
|
}
|
|
|
|
void NanoSensorCal::Initialize() {
|
|
NANO_CAL_LOGI("[NanoSensorCal]", "Initialized.");
|
|
|
|
#ifdef ACCEL_CAL_ENABLED
|
|
// Initializes the accelerometer offset calibration algorithm.
|
|
accelCalInit(&accel_cal_,
|
|
800000000, // Stillness Time in ns (0.8s)
|
|
5, // Minimum Sample Number
|
|
0.00025f, // Threshold
|
|
15, // nx bucket count
|
|
15, // nxb bucket count
|
|
15, // ny bucket count
|
|
15, // nyb bucket count
|
|
15, // nz bucket count
|
|
15, // nzb bucket count
|
|
15); // nle bucket count
|
|
|
|
// Retrieves stored calibration data using the ASH API.
|
|
LoadAshAccelCal();
|
|
#endif // ACCEL_CAL_ENABLED
|
|
|
|
#ifdef GYRO_CAL_ENABLED
|
|
// Initializes the gyroscope offset calibration algorithm.
|
|
gyroCalInit(
|
|
&gyro_cal_,
|
|
SEC_TO_NANOS(1.4f), // Min stillness period = 1.4 seconds
|
|
SEC_TO_NANOS(1.4f), // Max stillness period = 1.5 seconds (NOTE 1)
|
|
0, 0, 0, // Initial bias offset calibration
|
|
0, // Time stamp of initial bias calibration
|
|
SEC_TO_NANOS(0.5f), // Analysis window length = 0.5 seconds
|
|
3.0e-5f, // Gyroscope variance threshold [rad/sec]^2
|
|
3.0e-6f, // Gyroscope confidence delta [rad/sec]^2
|
|
4.5e-3f, // Accelerometer variance threshold [m/sec^2]^2
|
|
9.0e-4f, // Accelerometer confidence delta [m/sec^2]^2
|
|
5.0f, // Magnetometer variance threshold [uT]^2
|
|
1.0f, // Magnetometer confidence delta [uT]^2
|
|
0.95f, // Stillness threshold [0,1]
|
|
60.0f * MDEG_TO_RAD, // Stillness mean variation limit [rad/sec]
|
|
1.5f, // Max temperature delta during stillness [C]
|
|
true); // Gyro calibration enable
|
|
// NOTE 1: This parameter is set to 1.4 seconds to achieve a max stillness
|
|
// period of 1.5 seconds and avoid buffer boundary conditions that could push
|
|
// the max stillness to the next multiple of the analysis window length
|
|
// (i.e., 2.0 seconds).
|
|
|
|
#ifdef OVERTEMPCAL_GYRO_ENABLED
|
|
// Initializes the over-temperature compensated gyroscope (OTC-Gyro) offset
|
|
// calibration algorithm.
|
|
overTempCalInit(
|
|
&over_temp_gyro_cal_,
|
|
5, // Min num of points to enable model update
|
|
SEC_TO_NANOS(0.1f), // Min temperature update interval [nsec]
|
|
0.75f, // Temperature span of bin method [C]
|
|
40.0f * MDEG_TO_RAD, // Jump tolerance [rad/sec]
|
|
100.0f * MDEG_TO_RAD, // Outlier rejection tolerance [rad/sec]
|
|
DAYS_TO_NANOS(2), // Model data point age limit [nsec]
|
|
250.0f * MDEG_TO_RAD, // Limit for temp. sensitivity [rad/sec/C]
|
|
8.0e3f * MDEG_TO_RAD, // Limit for model intercept parameter [rad/sec]
|
|
0.1f * MDEG_TO_RAD, // Significant offset change [rad/sec]
|
|
true); // Over-temp compensation enable
|
|
#endif // OVERTEMPCAL_GYRO_ENABLED
|
|
|
|
// Retrieves stored calibration data using the ASH API.
|
|
#ifdef OVERTEMPCAL_GYRO_ENABLED
|
|
LoadAshOtcGyroCal();
|
|
#else
|
|
LoadAshGyroCal();
|
|
#endif // OVERTEMPCAL_GYRO_ENABLED
|
|
#endif // GYRO_CAL_ENABLED
|
|
|
|
#ifdef MAG_CAL_ENABLED
|
|
#ifdef DIVERSITY_CHECK_ENABLED
|
|
#ifdef SPHERE_FIT_ENABLED
|
|
// Full Sphere Fit.
|
|
// TODO: Replace function parameters with a struct, to avoid swapping them per
|
|
// accident.
|
|
initMagCalSphere(&mag_cal_sphere_,
|
|
0.0f, 0.0f, 0.0f, // Bias x, y, z
|
|
1.0f, 0.0f, 0.0f, // c00, c01, c02
|
|
0.0f, 1.0f, 0.0f, // c10, c11, c12
|
|
0.0f, 0.0f, 1.0f, // c20, c21, c22
|
|
7357000, // min_batch_window_in_micros
|
|
15, // min_num_diverse_vectors
|
|
1, // max_num_max_distance
|
|
5.0f, // var_threshold
|
|
8.0f, // max_min_threshold
|
|
48.f, // local_field
|
|
0.49f, // threshold_tuning_param
|
|
2.5f); // max_distance_tuning_param
|
|
magCalSphereOdrUpdate(&mag_cal_sphere_, 50 /* Default sample rate Hz */);
|
|
|
|
// ODR init.
|
|
memset(&mag_sample_rate_data_, 0, sizeof(SampleRateData));
|
|
#endif // SPHERE_FIT_ENABLED
|
|
|
|
// Initializes the magnetometer offset calibration algorithm (with diversity
|
|
// checker).
|
|
initMagCal(&mag_cal_,
|
|
0.0f, 0.0f, 0.0f, // bias x, y, z
|
|
1.0f, 0.0f, 0.0f, // c00, c01, c02
|
|
0.0f, 1.0f, 0.0f, // c10, c11, c12
|
|
0.0f, 0.0f, 1.0f, // c20, c21, c22
|
|
3000000, // min_batch_window_in_micros
|
|
8, // min_num_diverse_vectors
|
|
1, // max_num_max_distance
|
|
6.0f, // var_threshold
|
|
10.0f, // max_min_threshold
|
|
48.f, // local_field
|
|
0.49f, // threshold_tuning_param
|
|
2.5f); // max_distance_tuning_param
|
|
#else
|
|
// Initializes the magnetometer offset calibration algorithm.
|
|
initMagCal(&mag_cal_,
|
|
0.0f, 0.0f, 0.0f, // bias x, y, z
|
|
1.0f, 0.0f, 0.0f, // c00, c01, c02
|
|
0.0f, 1.0f, 0.0f, // c10, c11, c12
|
|
0.0f, 0.0f, 1.0f, // c20, c21, c22
|
|
3000000); // min_batch_window_in_micros
|
|
#endif // DIVERSITY_CHECK_ENABLED
|
|
|
|
// Retrieves stored calibration data using the ASH API.
|
|
LoadAshMagCal();
|
|
#endif // MAG_CAL_ENABLED
|
|
|
|
// Resets the calibration ready flags.
|
|
accel_calibration_ready_ = false;
|
|
gyro_calibration_ready_ = false;
|
|
mag_calibration_ready_ = false;
|
|
|
|
// NanoSensorCal algorithms have been initialized.
|
|
nanosensorcal_initialized_ = true;
|
|
}
|
|
|
|
// TODO: Evaluate the impact of sensor batching on the performance of the
|
|
// calibration algorithms (versus processing on a per-sample basis). For
|
|
// example, some of the internal algorithms rely on the temperature signal to
|
|
// determine when temperature variation is too high to perform calibrations.
|
|
void NanoSensorCal::HandleSensorSamples(
|
|
uint16_t event_type, const chreSensorThreeAxisData *event_data) {
|
|
if (nanosensorcal_initialized_) {
|
|
HandleSensorSamplesAccelCal(event_type, event_data);
|
|
HandleSensorSamplesGyroCal(event_type, event_data);
|
|
HandleSensorSamplesMagCal(event_type, event_data);
|
|
}
|
|
}
|
|
|
|
void NanoSensorCal::HandleTemperatureSamples(
|
|
uint16_t event_type, const chreSensorFloatData *event_data) {
|
|
if (!nanosensorcal_initialized_)
|
|
return;
|
|
|
|
// Takes the mean of the batched temperature samples and delivers it to the
|
|
// calibration algorithms. The latency setting determines the minimum update
|
|
// interval.
|
|
if (event_type == CHRE_EVENT_SENSOR_ACCELEROMETER_TEMPERATURE_DATA &&
|
|
event_data->header.readingCount > 0) {
|
|
const auto header = event_data->header;
|
|
const auto *data = event_data->readings;
|
|
uint64_t timestamp_nanos = header.baseTimestamp;
|
|
float mean_temperature_celsius = 0.0f;
|
|
for (size_t i = 0; i < header.readingCount; i++) {
|
|
timestamp_nanos += data[i].timestampDelta;
|
|
mean_temperature_celsius += data[i].value;
|
|
}
|
|
mean_temperature_celsius /= header.readingCount;
|
|
temperature_celsius_ = mean_temperature_celsius;
|
|
|
|
#ifdef GYRO_CAL_ENABLED
|
|
#ifdef OVERTEMPCAL_GYRO_ENABLED
|
|
// Updates the OTC gyro temperature.
|
|
overTempCalSetTemperature(&over_temp_gyro_cal_, timestamp_nanos,
|
|
temperature_celsius_);
|
|
#endif // OVERTEMPCAL_GYRO_ENABLED
|
|
#endif // GYRO_CAL_ENABLED
|
|
}
|
|
}
|
|
|
|
void NanoSensorCal::HandleSensorSamplesAccelCal(
|
|
uint16_t event_type, const chreSensorThreeAxisData *event_data) {
|
|
#ifdef ACCEL_CAL_ENABLED
|
|
if (event_type == CHRE_EVENT_SENSOR_UNCALIBRATED_ACCELEROMETER_DATA) {
|
|
const auto header = event_data->header;
|
|
const auto *data = event_data->readings;
|
|
uint64_t timestamp_nanos = header.baseTimestamp;
|
|
for (size_t i = 0; i < header.readingCount; i++) {
|
|
timestamp_nanos += data[i].timestampDelta;
|
|
accelCalRun(&accel_cal_, timestamp_nanos,
|
|
data[i].v[0], // x-axis data [m/sec^2]
|
|
data[i].v[1], // y-axis data [m/sec^2]
|
|
data[i].v[2], // z-axis data [m/sec^2]
|
|
temperature_celsius_);
|
|
}
|
|
// Checks for an accelerometer bias calibration change.
|
|
float offset[3] = {0.0f, 0.0f, 0.0f};
|
|
if (accelCalUpdateBias(&accel_cal_, &offset[0], &offset[1], &offset[2])) {
|
|
// Provides a new accelerometer calibration update.
|
|
accel_calibration_ready_ = true;
|
|
NotifyAshAccelCal();
|
|
}
|
|
|
|
#ifdef ACCEL_CAL_DBG_ENABLED
|
|
// Prints debug data report.
|
|
accelCalDebPrint(&accel_cal_, temperature_celsius_);
|
|
#endif
|
|
}
|
|
#endif // ACCEL_CAL_ENABLED
|
|
}
|
|
|
|
// TODO: Factor common code to shorten function and improve readability.
|
|
void NanoSensorCal::HandleSensorSamplesGyroCal(
|
|
uint16_t event_type, const chreSensorThreeAxisData *event_data) {
|
|
#ifdef GYRO_CAL_ENABLED
|
|
uint64_t timestamp_nanos = 0;
|
|
// Only updates the gyroscope calibration algorithm when measured
|
|
// temperature is valid.
|
|
if (temperature_celsius_ <= kInvalidTemperatureCelsius) {
|
|
return;
|
|
}
|
|
|
|
switch (event_type) {
|
|
case CHRE_EVENT_SENSOR_UNCALIBRATED_ACCELEROMETER_DATA: {
|
|
const auto header = event_data->header;
|
|
const auto *data = event_data->readings;
|
|
timestamp_nanos = header.baseTimestamp;
|
|
for (size_t i = 0; i < header.readingCount; i++) {
|
|
timestamp_nanos += data[i].timestampDelta;
|
|
gyroCalUpdateAccel(&gyro_cal_, timestamp_nanos,
|
|
data[i].v[0], // x-axis data [m/sec^2]
|
|
data[i].v[1], // y-axis data [m/sec^2]
|
|
data[i].v[2]); // z-axis data [m/sec^2]
|
|
}
|
|
break;
|
|
}
|
|
|
|
case CHRE_EVENT_SENSOR_UNCALIBRATED_GYROSCOPE_DATA: {
|
|
const auto header = event_data->header;
|
|
const auto *data = event_data->readings;
|
|
timestamp_nanos = header.baseTimestamp;
|
|
for (size_t i = 0; i < header.readingCount; i++) {
|
|
timestamp_nanos += data[i].timestampDelta;
|
|
gyroCalUpdateGyro(&gyro_cal_, timestamp_nanos,
|
|
data[i].v[0], // x-axis data [rad/sec]
|
|
data[i].v[1], // y-axis data [rad/sec]
|
|
data[i].v[2], // z-axis data [rad/sec]
|
|
temperature_celsius_);
|
|
}
|
|
|
|
if (gyroCalNewBiasAvailable(&gyro_cal_)) {
|
|
#ifdef OVERTEMPCAL_GYRO_ENABLED
|
|
// Sends new GyroCal offset estimate to the OTC-Gyro.
|
|
float offset[3] = {0.0f, 0.0f, 0.0f};
|
|
float offset_temperature_celsius = 0.0f;
|
|
gyroCalGetBias(&gyro_cal_, &offset[0], &offset[1], &offset[2],
|
|
&offset_temperature_celsius);
|
|
overTempCalUpdateSensorEstimate(&over_temp_gyro_cal_, timestamp_nanos,
|
|
offset, offset_temperature_celsius);
|
|
#else
|
|
// Provides a new gyroscope calibration update.
|
|
gyro_calibration_ready_ = true;
|
|
NotifyAshGyroCal();
|
|
#endif // OVERTEMPCAL_GYRO_ENABLED
|
|
}
|
|
|
|
#ifdef OVERTEMPCAL_GYRO_ENABLED
|
|
// Checks OTC for new calibration model update.
|
|
bool new_otc_model_update =
|
|
overTempCalNewModelUpdateAvailable(&over_temp_gyro_cal_);
|
|
|
|
// Checks for a change in the OTC-Gyro temperature compensated offset
|
|
// estimate.
|
|
bool new_otc_offset = overTempCalNewOffsetAvailable(&over_temp_gyro_cal_);
|
|
|
|
if (new_otc_model_update || new_otc_offset) {
|
|
// Provides a temperature compensated gyroscope calibration update.
|
|
gyro_calibration_ready_ = true;
|
|
NotifyAshGyroCal();
|
|
}
|
|
#endif // OVERTEMPCAL_GYRO_ENABLED
|
|
break;
|
|
}
|
|
|
|
case CHRE_EVENT_SENSOR_UNCALIBRATED_GEOMAGNETIC_FIELD_DATA: {
|
|
const auto header = event_data->header;
|
|
const auto *data = event_data->readings;
|
|
timestamp_nanos = header.baseTimestamp;
|
|
for (size_t i = 0; i < header.readingCount; i++) {
|
|
timestamp_nanos += data[i].timestampDelta;
|
|
gyroCalUpdateMag(&gyro_cal_, timestamp_nanos,
|
|
data[i].v[0], // x-axis data [uT]
|
|
data[i].v[1], // y-axis data [uT]
|
|
data[i].v[2]); // z-axis data [uT]
|
|
}
|
|
break;
|
|
}
|
|
|
|
default:
|
|
break;
|
|
}
|
|
|
|
if (timestamp_nanos > 0) {
|
|
#ifdef GYRO_CAL_DBG_ENABLED
|
|
// Prints debug data report.
|
|
gyroCalDebugPrint(&gyro_cal_, timestamp_nanos);
|
|
#endif // GYRO_CAL_DBG_ENABLED
|
|
|
|
#if defined(OVERTEMPCAL_GYRO_ENABLED) && defined(OVERTEMPCAL_DBG_ENABLED)
|
|
// Prints debug data report.
|
|
overTempCalDebugPrint(&over_temp_gyro_cal_, timestamp_nanos);
|
|
#endif // OVERTEMPCAL_GYRO_ENABLED && OVERTEMPCAL_DBG_ENABLED
|
|
}
|
|
#endif // GYRO_CAL_ENABLED
|
|
}
|
|
|
|
void NanoSensorCal::HandleSensorSamplesMagCal(
|
|
uint16_t event_type, const chreSensorThreeAxisData *event_data) {
|
|
#ifdef MAG_CAL_ENABLED
|
|
if (event_type == CHRE_EVENT_SENSOR_UNCALIBRATED_GEOMAGNETIC_FIELD_DATA) {
|
|
const auto header = event_data->header;
|
|
const auto *data = event_data->readings;
|
|
uint64_t timestamp_nanos = header.baseTimestamp;
|
|
MagUpdateFlags new_calibration_update_mag_cal = MagUpdate::NO_UPDATE;
|
|
|
|
for (size_t i = 0; i < header.readingCount; i++) {
|
|
timestamp_nanos += data[i].timestampDelta;
|
|
|
|
// Sets the flag to indicate a new calibration update.
|
|
new_calibration_update_mag_cal |= magCalUpdate(
|
|
&mag_cal_,
|
|
static_cast<uint64_t>(timestamp_nanos * kNanoToMicroseconds),
|
|
data[i].v[0], // x-axis data [uT]
|
|
data[i].v[1], // y-axis data [uT]
|
|
data[i].v[2]); // z-axis data [uT]
|
|
|
|
#ifdef SPHERE_FIT_ENABLED
|
|
// Sphere Fit Algo Part.
|
|
|
|
// getting ODR.
|
|
if (mag_sample_rate_data_.num_samples <
|
|
kSamplesToAverageForOdrEstimateMag) {
|
|
SamplingRateEstimate(&mag_sample_rate_data_, nullptr, timestamp_nanos,
|
|
false);
|
|
} else {
|
|
SamplingRateEstimate(&mag_sample_rate_data_, &mag_odr_estimate_hz_,
|
|
0, true);
|
|
|
|
// Sphere fit ODR update.
|
|
magCalSphereOdrUpdate(&mag_cal_sphere_, mag_odr_estimate_hz_);
|
|
}
|
|
|
|
// Running Sphere fit, and getting trigger.
|
|
new_calibration_update_mag_cal |= magCalSphereUpdate(
|
|
&mag_cal_sphere_,
|
|
static_cast<uint64_t>(timestamp_nanos * kNanoToMicroseconds),
|
|
data[i].v[0], // x-axis data [uT]
|
|
data[i].v[1], // y-axis data [uT]
|
|
data[i].v[2]); // z-axis data [uT]
|
|
#endif // SPHERE_FIT_ENABLED
|
|
}
|
|
|
|
if ((MagUpdate::UPDATE_BIAS & new_calibration_update_mag_cal) ||
|
|
(MagUpdate::UPDATE_SPHERE_FIT & new_calibration_update_mag_cal)) {
|
|
// Sets the flag to indicate a new calibration update is pending.
|
|
mag_calibration_ready_ = true;
|
|
NotifyAshMagCal(new_calibration_update_mag_cal);
|
|
}
|
|
}
|
|
#endif // MAG_CAL_ENABLED
|
|
}
|
|
|
|
void NanoSensorCal::GetAccelerometerCalibration(
|
|
struct ashCalParams *accel_cal_params) const {
|
|
// Resets the calibration ready flag; and returns the calibration data.
|
|
accel_calibration_ready_ = false;
|
|
memcpy(accel_cal_params, &accel_cal_params_, sizeof(struct ashCalParams));
|
|
}
|
|
|
|
void NanoSensorCal::GetGyroscopeCalibration(
|
|
struct ashCalParams *gyro_cal_params) const {
|
|
// Resets the calibration ready flag; and returns the calibration data.
|
|
gyro_calibration_ready_ = false;
|
|
memcpy(gyro_cal_params, &gyro_cal_params_, sizeof(struct ashCalParams));
|
|
}
|
|
|
|
void NanoSensorCal::GetMagnetometerCalibration(
|
|
struct ashCalParams *mag_cal_params) const {
|
|
// Resets the calibration ready flag; and returns the calibration data.
|
|
mag_calibration_ready_ = false;
|
|
memcpy(mag_cal_params, &mag_cal_params_, sizeof(struct ashCalParams));
|
|
}
|
|
|
|
void NanoSensorCal::UpdateAccelCalParams() {
|
|
#ifdef ACCEL_CAL_ENABLED
|
|
// Gets the accelerometer's offset vector and temperature.
|
|
accelCalUpdateBias(&accel_cal_, &accel_cal_params_.offset[0],
|
|
&accel_cal_params_.offset[1],
|
|
&accel_cal_params_.offset[2]);
|
|
accel_cal_params_.offsetTempCelsius = temperature_celsius_;
|
|
|
|
// Sets the parameter source to runtime calibration.
|
|
accel_cal_params_.offsetSource = ASH_CAL_PARAMS_SOURCE_RUNTIME;
|
|
accel_cal_params_.offsetTempCelsiusSource = ASH_CAL_PARAMS_SOURCE_RUNTIME;
|
|
#endif // ACCEL_CAL_ENABLED
|
|
}
|
|
|
|
void NanoSensorCal::UpdateGyroCalParams() {
|
|
#ifdef GYRO_CAL_ENABLED
|
|
#ifdef OVERTEMPCAL_GYRO_ENABLED
|
|
// Gets the gyroscope's offset vector and temperature; and OTC linear model
|
|
// parameters.
|
|
uint64_t timestamp_nanos = 0;
|
|
overTempCalGetModel(&over_temp_gyro_cal_, gyro_cal_params_.offset,
|
|
&gyro_cal_params_.offsetTempCelsius, ×tamp_nanos,
|
|
gyro_cal_params_.tempSensitivity,
|
|
gyro_cal_params_.tempIntercept);
|
|
|
|
// Sets the parameter source to runtime calibration.
|
|
gyro_cal_params_.offsetSource = ASH_CAL_PARAMS_SOURCE_RUNTIME;
|
|
gyro_cal_params_.offsetTempCelsiusSource = ASH_CAL_PARAMS_SOURCE_RUNTIME;
|
|
gyro_cal_params_.tempSensitivitySource = ASH_CAL_PARAMS_SOURCE_RUNTIME;
|
|
gyro_cal_params_.tempInterceptSource = ASH_CAL_PARAMS_SOURCE_RUNTIME;
|
|
#else
|
|
// Gets the gyroscope's offset vector and temperature.
|
|
gyroCalGetBias(&gyro_cal_, &gyro_cal_params_.offset[0],
|
|
&gyro_cal_params_.offset[1], &gyro_cal_params_.offset[2],
|
|
&gyro_cal_params_.offsetTempCelsius);
|
|
|
|
// Sets the parameter source to runtime calibration.
|
|
gyro_cal_params_.offsetSource = ASH_CAL_PARAMS_SOURCE_RUNTIME;
|
|
gyro_cal_params_.offsetTempCelsiusSource = ASH_CAL_PARAMS_SOURCE_RUNTIME;
|
|
#endif // OVERTEMPCAL_GYRO_ENABLED
|
|
#endif // GYRO_CAL_ENABLED
|
|
}
|
|
|
|
void NanoSensorCal::UpdateMagCalParams(MagUpdateFlags new_update) {
|
|
#ifdef MAG_CAL_ENABLED
|
|
if (MagUpdate::UPDATE_SPHERE_FIT & new_update) {
|
|
#ifdef SPHERE_FIT_ENABLED
|
|
// Updating the mag offset from sphere fit.
|
|
mag_cal_params_.offset[0] = mag_cal_sphere_.sphere_fit.sphere_param.bias[0];
|
|
mag_cal_params_.offset[1] = mag_cal_sphere_.sphere_fit.sphere_param.bias[1];
|
|
mag_cal_params_.offset[2] = mag_cal_sphere_.sphere_fit.sphere_param.bias[2];
|
|
|
|
// Updating the Sphere Param.
|
|
mag_cal_params_.scaleFactor[0] =
|
|
mag_cal_sphere_.sphere_fit.sphere_param.scale_factor_x;
|
|
mag_cal_params_.scaleFactor[1] =
|
|
mag_cal_sphere_.sphere_fit.sphere_param.scale_factor_y;
|
|
mag_cal_params_.scaleFactor[2] =
|
|
mag_cal_sphere_.sphere_fit.sphere_param.scale_factor_z;
|
|
mag_cal_params_.crossAxis[0] =
|
|
mag_cal_sphere_.sphere_fit.sphere_param.skew_yx;
|
|
mag_cal_params_.crossAxis[1] =
|
|
mag_cal_sphere_.sphere_fit.sphere_param.skew_zx;
|
|
mag_cal_params_.crossAxis[2] =
|
|
mag_cal_sphere_.sphere_fit.sphere_param.skew_zy;
|
|
|
|
// Updating the temperature.
|
|
mag_cal_params_.offsetTempCelsius = temperature_celsius_;
|
|
|
|
// Sets the parameter source to runtime calibration.
|
|
mag_cal_params_.offsetSource = ASH_CAL_PARAMS_SOURCE_RUNTIME;
|
|
mag_cal_params_.scaleFactorSource = ASH_CAL_PARAMS_SOURCE_RUNTIME;
|
|
mag_cal_params_.crossAxisSource = ASH_CAL_PARAMS_SOURCE_RUNTIME;
|
|
mag_cal_params_.offsetTempCelsiusSource = ASH_CAL_PARAMS_SOURCE_RUNTIME;
|
|
#endif // SPHERE_FIT_ENABLED
|
|
} else if (MagUpdate::UPDATE_BIAS & new_update) {
|
|
// Gets the magnetometer's offset vector and temperature.
|
|
magCalGetBias(&mag_cal_, &mag_cal_params_.offset[0],
|
|
&mag_cal_params_.offset[1], &mag_cal_params_.offset[2]);
|
|
mag_cal_params_.offsetTempCelsius = temperature_celsius_;
|
|
|
|
// Sets the parameter source to runtime calibration.
|
|
mag_cal_params_.offsetSource = ASH_CAL_PARAMS_SOURCE_RUNTIME;
|
|
mag_cal_params_.offsetTempCelsiusSource = ASH_CAL_PARAMS_SOURCE_RUNTIME;
|
|
}
|
|
#endif // MAG_CAL_ENABLED
|
|
}
|
|
|
|
void NanoSensorCal::LoadAshAccelCal() {
|
|
#ifdef ACCEL_CAL_ENABLED
|
|
struct ashCalParams cal_params;
|
|
if (!ashLoadCalibrationParams(CHRE_SENSOR_TYPE_ACCELEROMETER,
|
|
ASH_CAL_STORAGE_ASH, &cal_params)) {
|
|
NANO_CAL_LOGE("[NanoSensorCal:RECALL ACCEL]",
|
|
"ASH failed to recall accelerometer calibration data from "
|
|
"persistent memory.");
|
|
} else {
|
|
// Checks for and performs required processing on input factory cal data.
|
|
HandleAccelFactoryCalibration(&cal_params);
|
|
|
|
// Checks for valid calibration data.
|
|
bool runtime_cal_detected =
|
|
cal_params.offsetSource == ASH_CAL_PARAMS_SOURCE_RUNTIME &&
|
|
cal_params.offsetTempCelsiusSource == ASH_CAL_PARAMS_SOURCE_RUNTIME;
|
|
|
|
if (!runtime_cal_detected) {
|
|
NANO_CAL_LOGW("[NanoSensorCal:RECALL ACCEL]",
|
|
"No valid calibration data found.");
|
|
} else {
|
|
// On a successful load, copies the new set of calibration parameters.
|
|
memcpy(&accel_cal_params_, &cal_params, sizeof(struct ashCalParams));
|
|
|
|
// Sets the accelerometer algorithm's calibration data.
|
|
accelCalBiasSet(&accel_cal_, accel_cal_params_.offset[0],
|
|
accel_cal_params_.offset[1], accel_cal_params_.offset[2]);
|
|
|
|
// Prints recalled calibration data.
|
|
NANO_CAL_LOGI(
|
|
"[NanoSensorCal:RECALL ACCEL]",
|
|
"Offset [m/sec^2] | Temp [Celsius]: %.6f, %.6f, %.6f | %.6f",
|
|
accel_cal_params_.offset[0], accel_cal_params_.offset[1],
|
|
accel_cal_params_.offset[2], accel_cal_params_.offsetTempCelsius);
|
|
|
|
// Updates the calibration data using ASH.
|
|
NotifyAshAccelCal();
|
|
}
|
|
}
|
|
#endif // ACCEL_CAL_ENABLED
|
|
}
|
|
|
|
void NanoSensorCal::LoadAshGyroCal() {
|
|
#ifdef GYRO_CAL_ENABLED
|
|
struct ashCalParams cal_params;
|
|
if (!ashLoadCalibrationParams(CHRE_SENSOR_TYPE_GYROSCOPE, ASH_CAL_STORAGE_ASH,
|
|
&cal_params)) {
|
|
NANO_CAL_LOGE("[NanoSensorCal:RECALL GYRO]",
|
|
"ASH failed to recall gyroscope calibration data from "
|
|
"persistent memory.");
|
|
} else {
|
|
// Checks for and performs required processing on input factory cal data.
|
|
HandleGyroFactoryCalibration(&cal_params);
|
|
|
|
// Gyroscope offset calibration parameters were recalled.
|
|
bool runtime_cal_detected =
|
|
cal_params.offsetSource == ASH_CAL_PARAMS_SOURCE_RUNTIME &&
|
|
cal_params.offsetTempCelsiusSource == ASH_CAL_PARAMS_SOURCE_RUNTIME;
|
|
|
|
if (!runtime_cal_detected) {
|
|
NANO_CAL_LOGW("[NanoSensorCal:RECALL GYRO]",
|
|
"No valid calibration data found.");
|
|
} else {
|
|
// On a successful load, copies the new set of calibration parameters.
|
|
memcpy(&gyro_cal_params_, &cal_params, sizeof(struct ashCalParams));
|
|
|
|
// Sets the gyroscope algorithm's calibration data.
|
|
gyroCalSetBias(&gyro_cal_, gyro_cal_params_.offset[0],
|
|
gyro_cal_params_.offset[1], gyro_cal_params_.offset[2],
|
|
/*calibration_time_nanos=*/0);
|
|
|
|
// Prints recalled calibration data.
|
|
NANO_CAL_LOGI(
|
|
"[NanoSensorCal:RECALL GYRO]",
|
|
"Offset [rad/sec] | Temp [Celsius]: %.6f, %.6f, %.6f | %.6f",
|
|
gyro_cal_params_.offset[0], gyro_cal_params_.offset[1],
|
|
gyro_cal_params_.offset[2], gyro_cal_params_.offsetTempCelsius);
|
|
|
|
// Updates the calibration data using ASH.
|
|
NotifyAshGyroCal();
|
|
}
|
|
}
|
|
#endif // GYRO_CAL_ENABLED
|
|
}
|
|
|
|
void NanoSensorCal::LoadAshOtcGyroCal() {
|
|
#ifdef GYRO_CAL_ENABLED
|
|
#ifdef OVERTEMPCAL_GYRO_ENABLED
|
|
struct ashCalParams cal_params;
|
|
if (!ashLoadCalibrationParams(CHRE_SENSOR_TYPE_GYROSCOPE, ASH_CAL_STORAGE_ASH,
|
|
&cal_params)) {
|
|
NANO_CAL_LOGE("[NanoSensorCal:RECALL OTC-GYRO]",
|
|
"ASH failed to recall gyroscope calibration data from "
|
|
"persistent memory.");
|
|
} else {
|
|
// Checks for and performs required processing on input factory cal data.
|
|
HandleGyroFactoryCalibration(&cal_params);
|
|
|
|
// Gyroscope offset calibration with over-temperature compensation (OTC)
|
|
// parameters were recalled.
|
|
bool runtime_cal_detected =
|
|
cal_params.offsetSource == ASH_CAL_PARAMS_SOURCE_RUNTIME &&
|
|
cal_params.offsetTempCelsiusSource == ASH_CAL_PARAMS_SOURCE_RUNTIME &&
|
|
cal_params.tempSensitivitySource == ASH_CAL_PARAMS_SOURCE_RUNTIME &&
|
|
cal_params.tempInterceptSource == ASH_CAL_PARAMS_SOURCE_RUNTIME;
|
|
|
|
if (!runtime_cal_detected) {
|
|
NANO_CAL_LOGW("[NanoSensorCal:RECALL OTC-GYRO]",
|
|
"No valid calibration data found.");
|
|
} else {
|
|
// On a successful load, copies the new set of calibration parameters.
|
|
memcpy(&gyro_cal_params_, &cal_params, sizeof(struct ashCalParams));
|
|
|
|
// Sets the gyroscope algorithm's calibration data.
|
|
const uint64_t timestamp_nanos = chreGetTime();
|
|
gyroCalSetBias(&gyro_cal_, gyro_cal_params_.offset[0],
|
|
gyro_cal_params_.offset[1], gyro_cal_params_.offset[2],
|
|
timestamp_nanos);
|
|
overTempCalSetModel(&over_temp_gyro_cal_, gyro_cal_params_.offset,
|
|
gyro_cal_params_.offsetTempCelsius, timestamp_nanos,
|
|
gyro_cal_params_.tempSensitivity,
|
|
gyro_cal_params_.tempIntercept,
|
|
/*jump_start_model=*/false);
|
|
|
|
// Prints recalled calibration data.
|
|
NANO_CAL_LOGI(
|
|
"[NanoSensorCal:RECALL OTC-GYRO]",
|
|
"Offset [rad/sec] | Temp [Celsius]: %.6f, %.6f, %.6f | %.6f",
|
|
gyro_cal_params_.offset[0], gyro_cal_params_.offset[1],
|
|
gyro_cal_params_.offset[2], gyro_cal_params_.offsetTempCelsius);
|
|
NANO_CAL_LOGI("[NanoSensorCal:RECALL OTC-GYRO]",
|
|
"Temp Sensitivity [rad/sec/C]: %.6f, %.6f, %.6f",
|
|
gyro_cal_params_.tempSensitivity[0],
|
|
gyro_cal_params_.tempSensitivity[1],
|
|
gyro_cal_params_.tempSensitivity[2]);
|
|
NANO_CAL_LOGI("[NanoSensorCal:RECALL OTC-GYRO]",
|
|
"Temp Intercept [rad/sec]: %.6f, %.6f, %.6f",
|
|
gyro_cal_params_.tempIntercept[0],
|
|
gyro_cal_params_.tempIntercept[1],
|
|
gyro_cal_params_.tempIntercept[2]);
|
|
|
|
// Updates the calibration data using ASH.
|
|
NotifyAshGyroCal();
|
|
}
|
|
}
|
|
#endif // OVERTEMPCAL_GYRO_ENABLED
|
|
#endif // GYRO_CAL_ENABLED
|
|
}
|
|
|
|
void NanoSensorCal::LoadAshMagCal() {
|
|
#ifdef MAG_CAL_ENABLED
|
|
struct ashCalParams cal_params;
|
|
if (!ashLoadCalibrationParams(CHRE_SENSOR_TYPE_GEOMAGNETIC_FIELD,
|
|
ASH_CAL_STORAGE_ASH, &cal_params)) {
|
|
NANO_CAL_LOGE("[NanoSensorCal:RECALL MAG]",
|
|
"ASH failed to recall Magnetometer calibration data from "
|
|
"persistent memory.");
|
|
} else {
|
|
// Checks for and performs required processing on input factory cal data.
|
|
HandleMagFactoryCalibration(&cal_params);
|
|
|
|
// Checks for valid calibration data.
|
|
bool runtime_cal_detected =
|
|
cal_params.offsetSource == ASH_CAL_PARAMS_SOURCE_RUNTIME &&
|
|
cal_params.offsetTempCelsiusSource == ASH_CAL_PARAMS_SOURCE_RUNTIME;
|
|
|
|
if (runtime_cal_detected) {
|
|
// On a successful load, copies the new set of calibration parameters.
|
|
memcpy(&mag_cal_params_, &cal_params, sizeof(struct ashCalParams));
|
|
|
|
// Sets the magnetometer algorithm's calibration data.
|
|
magCalReset(&mag_cal_); // Resets the magnetometer's offset vector.
|
|
magCalAddBias(&mag_cal_, mag_cal_params_.offset[0],
|
|
mag_cal_params_.offset[1], mag_cal_params_.offset[2]);
|
|
|
|
#ifdef SPHERE_FIT_ENABLED
|
|
// Sets Sphere Fit calibration data.
|
|
mag_cal_sphere_.sphere_fit.sphere_param.scale_factor_x =
|
|
mag_cal_params_.scaleFactor[0];
|
|
mag_cal_sphere_.sphere_fit.sphere_param.scale_factor_y =
|
|
mag_cal_params_.scaleFactor[1];
|
|
mag_cal_sphere_.sphere_fit.sphere_param.scale_factor_z =
|
|
mag_cal_params_.scaleFactor[2];
|
|
mag_cal_sphere_.sphere_fit.sphere_param.skew_yx =
|
|
mag_cal_params_.crossAxis[0];
|
|
mag_cal_sphere_.sphere_fit.sphere_param.skew_zx =
|
|
mag_cal_params_.crossAxis[1];
|
|
mag_cal_sphere_.sphere_fit.sphere_param.skew_zy =
|
|
mag_cal_params_.crossAxis[2];
|
|
mag_cal_sphere_.sphere_fit.sphere_param.bias[0] =
|
|
mag_cal_params_.offset[0];
|
|
mag_cal_sphere_.sphere_fit.sphere_param.bias[1] =
|
|
mag_cal_params_.offset[1];
|
|
mag_cal_sphere_.sphere_fit.sphere_param.bias[2] =
|
|
mag_cal_params_.offset[2];
|
|
#endif // SPHERE_FIT_ENABLED
|
|
|
|
// Prints recalled calibration data.
|
|
NANO_CAL_LOGI("[NanoSensorCal:RECALL MAG]",
|
|
"Offset [uT] | Temp [Celsius]: %.3f, %.3f, %.3f | %.3f",
|
|
mag_cal_params_.offset[0], mag_cal_params_.offset[1],
|
|
mag_cal_params_.offset[2],
|
|
mag_cal_params_.offsetTempCelsius);
|
|
#ifdef SPHERE_FIT_ENABLED
|
|
NANO_CAL_LOGI(
|
|
"[NanoSensorCal:RECALL MAG]",
|
|
"Scale Factor [%] | Cross Axis [%]: %.3f, %.3f, %.3f |"
|
|
" %.3f, %.3f, %.3f",
|
|
mag_cal_params_.scaleFactor[0], mag_cal_params_.scaleFactor[1],
|
|
mag_cal_params_.scaleFactor[2], mag_cal_params_.crossAxis[0],
|
|
mag_cal_params_.crossAxis[1], mag_cal_params_.crossAxis[2]);
|
|
#endif // SPHERE_FIT_ENABLED
|
|
|
|
// Updates the calibration data using ASH.
|
|
#ifdef SPHERE_FIT_ENABLED
|
|
NotifyAshMagCal(MagUpdate::UPDATE_SPHERE_FIT);
|
|
#else
|
|
NotifyAshMagCal(MagUpdate::UPDATE_BIAS);
|
|
#endif // SPHERE_FIT_ENABLED
|
|
} else {
|
|
NANO_CAL_LOGW("[NanoSensorCal:RECALL MAG]",
|
|
"No valid calibration data found.");
|
|
}
|
|
}
|
|
#endif // MAG_CAL_ENABLED
|
|
}
|
|
|
|
void NanoSensorCal::NotifyAshAccelCal() {
|
|
#ifdef ACCEL_CAL_ENABLED
|
|
// Update ASH with the latest calibration data.
|
|
UpdateAccelCalParams();
|
|
struct ashCalInfo cal_info;
|
|
ResetCalInfo(&cal_info);
|
|
memcpy(cal_info.bias, accel_cal_params_.offset, sizeof(cal_info.bias));
|
|
cal_info.accuracy = ASH_CAL_ACCURACY_HIGH;
|
|
if (!ashSetCalibration(CHRE_SENSOR_TYPE_ACCELEROMETER, &cal_info)) {
|
|
NANO_CAL_LOGE("[NanoSensorCal:UPDATE ACCEL]",
|
|
"ASH failed to apply calibration update.");
|
|
} else {
|
|
NANO_CAL_LOGD("[NanoSensorCal:UPDATE ACCEL]",
|
|
"Offset [m/sec^2] | Temp [Celsius]: %.6f, %.6f, %.6f | %.2f",
|
|
accel_cal_params_.offset[0], accel_cal_params_.offset[1],
|
|
accel_cal_params_.offset[2],
|
|
accel_cal_params_.offsetTempCelsius);
|
|
}
|
|
|
|
// Store the calibration parameters using the ASH API.
|
|
if (!ashSaveCalibrationParams(CHRE_SENSOR_TYPE_ACCELEROMETER,
|
|
&accel_cal_params_)) {
|
|
NANO_CAL_LOGE("[NanoSensorCal:STORE ACCEL]",
|
|
"ASH failed to write calibration update.");
|
|
}
|
|
#endif // ACCEL_CAL_ENABLED
|
|
}
|
|
|
|
void NanoSensorCal::NotifyAshGyroCal() {
|
|
#ifdef GYRO_CAL_ENABLED
|
|
// Update ASH with the latest calibration data.
|
|
UpdateGyroCalParams();
|
|
struct ashCalInfo cal_info;
|
|
ResetCalInfo(&cal_info);
|
|
memcpy(cal_info.bias, gyro_cal_params_.offset, sizeof(cal_info.bias));
|
|
cal_info.accuracy = ASH_CAL_ACCURACY_HIGH;
|
|
if (!ashSetCalibration(CHRE_SENSOR_TYPE_GYROSCOPE, &cal_info)) {
|
|
NANO_CAL_LOGE("[NanoSensorCal:UPDATE GYRO]",
|
|
"ASH failed to apply calibration update.");
|
|
} else {
|
|
const uint64_t timestamp_nanos = chreGetTime();
|
|
if (timestamp_nanos >=
|
|
gyro_notification_time_check_ + kNanoSensorCalMessageIntervalNanos) {
|
|
gyro_notification_time_check_ = timestamp_nanos;
|
|
#ifdef OVERTEMPCAL_GYRO_ENABLED
|
|
NANO_CAL_LOGD(
|
|
"[NanoSensorCal:UPDATE OTC-GYRO]",
|
|
"Offset [rad/sec] | Temp [Celsius]: %.6f, %.6f, %.6f | %.2f",
|
|
gyro_cal_params_.offset[0], gyro_cal_params_.offset[1],
|
|
gyro_cal_params_.offset[2], gyro_cal_params_.offsetTempCelsius);
|
|
NANO_CAL_LOGD("[NanoSensorCal:UPDATE OTC-GYRO]",
|
|
"Temp Sensitivity [rad/sec/C]: %.6f, %.6f, %.6f",
|
|
gyro_cal_params_.tempSensitivity[0],
|
|
gyro_cal_params_.tempSensitivity[1],
|
|
gyro_cal_params_.tempSensitivity[2]);
|
|
NANO_CAL_LOGD("[NanoSensorCal:UPDATE OTC-GYRO]",
|
|
"Temp Intercept [rad/sec]: %.6f, %.6f, %.6f",
|
|
gyro_cal_params_.tempIntercept[0],
|
|
gyro_cal_params_.tempIntercept[1],
|
|
gyro_cal_params_.tempIntercept[2]);
|
|
#else
|
|
NANO_CAL_LOGD(
|
|
"[NanoSensorCal:UPDATE GYRO]",
|
|
"Offset [rad/sec] | Temp [Celsius]: %.6f, %.6f, %.6f | %.2f",
|
|
gyro_cal_params_.offset[0], gyro_cal_params_.offset[1],
|
|
gyro_cal_params_.offset[2], gyro_cal_params_.offsetTempCelsius);
|
|
#endif // OVERTEMPCAL_GYRO_ENABLED
|
|
}
|
|
}
|
|
|
|
// Store the calibration parameters using the ASH API.
|
|
if (!ashSaveCalibrationParams(CHRE_SENSOR_TYPE_GYROSCOPE,
|
|
&gyro_cal_params_)) {
|
|
NANO_CAL_LOGE("[NanoSensorCal:STORE GYRO]",
|
|
"ASH failed to write calibration update.");
|
|
}
|
|
#endif // GYRO_CAL_ENABLED
|
|
}
|
|
|
|
void NanoSensorCal::NotifyAshMagCal(MagUpdateFlags new_update) {
|
|
#ifdef MAG_CAL_ENABLED
|
|
// Update ASH with the latest calibration data.
|
|
UpdateMagCalParams(new_update);
|
|
struct ashCalInfo cal_info;
|
|
ResetCalInfo(&cal_info);
|
|
memcpy(cal_info.bias, mag_cal_params_.offset, sizeof(cal_info.bias));
|
|
|
|
// TODO: Adding Sphere Parameters to compensation matrix.
|
|
cal_info.accuracy = ASH_CAL_ACCURACY_HIGH;
|
|
if (!ashSetCalibration(CHRE_SENSOR_TYPE_GEOMAGNETIC_FIELD, &cal_info)) {
|
|
NANO_CAL_LOGE("[NanoSensorCal:UPDATE MAG]",
|
|
"ASH failed to apply calibration update.");
|
|
} else {
|
|
NANO_CAL_LOGD("[NanoSensorCal:UPDATE MAG]",
|
|
"Offset [uT] | Temp [Celsius]: %.6f, %.6f, %.6f | %.2f",
|
|
mag_cal_params_.offset[0], mag_cal_params_.offset[1],
|
|
mag_cal_params_.offset[2], mag_cal_params_.offsetTempCelsius);
|
|
#ifdef SPHERE_FIT_ENABLED
|
|
NANO_CAL_LOGD("[NanoSensorCal:UPDATE MAG]",
|
|
"Scale Factor [%] | Cross Axis [%]: %.3f, %.3f, %.3f | "
|
|
" %.3f, %.3f, %.3f",
|
|
mag_cal_params_.scaleFactor[0],
|
|
mag_cal_params_.scaleFactor[1],
|
|
mag_cal_params_.scaleFactor[2], mag_cal_params_.crossAxis[0],
|
|
mag_cal_params_.crossAxis[1], mag_cal_params_.crossAxis[2]);
|
|
#endif // SPHERE_FIT_ENABLED
|
|
}
|
|
|
|
// Store the calibration parameters using the ASH API.
|
|
if (!ashSaveCalibrationParams(CHRE_SENSOR_TYPE_GEOMAGNETIC_FIELD,
|
|
&mag_cal_params_)) {
|
|
NANO_CAL_LOGE("[NanoSensorCal:STORE MAG]",
|
|
"ASH failed to write calibration update.");
|
|
}
|
|
#endif // MAG_CAL_ENABLED
|
|
}
|
|
|
|
} // namespace nano_calibration
|