sandy
/
PL286
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
1 Commit
1 Branch
0 Tags
18 GiB
Tree: 99da0f2c72
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from '99da0f2c72'
${ noResults }
PL286/system/chre/apps/imu_cal/nano_calibration.cc

1134 lines
46 KiB
Raw Blame History

/*
* 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, &timestamp_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