From 846b602d3936757bf722778b355a2d97100efccd Mon Sep 17 00:00:00 2001 From: "davejacobs: David Jacobs" Date: Sun, 2 May 2021 15:18:50 -0400 Subject: [ImuCal] Add calibration telemetry logging callback. - Integrates logging functions for tracking calibration updates. - Implementation of the telemetry functions will come later. - Syncs code from Google3, PiperOrigin-RevId: 371592964. - Adds 200 bytes; no nanoapp size change with padding. Memory (before): text data bss dec 32053 356 18712 51121 MemSize:0x7e40 Align:0x1000 Padded:0x8000 Padding:448 MemSize:0x4a80 Align:0x1000 Padded:0x5000 Padding:1408 Total Padded MemSize: 0xd000 (53248) Memory (after): text data bss dec 32209 356 18752 51317 MemSize:0x7ed0 Align:0x1000 Padded:0x8000 Padding:304 MemSize:0x4aa8 Align:0x1000 Padded:0x5000 Padding:1368 Total Padded MemSize: 0xd000 (53248) Bug: 186054152 Tested: Built and verified on device. Change-Id: Id3a94674fd394c9ac869eb727f76e691bc717ecb --- .../nano_calibration/nano_calibration.cc | 62 ++++++++++++++++------ .../nano_calibration/nano_calibration.h | 17 ++++-- .../common_data/calibration_data.h | 7 +++ .../common_data/result_callback_interface.h | 32 +++++++++++ .../gyro_offset_over_temp_cal.cc | 5 +- firmware/os/algos/common/math/kasa.c | 21 +++++++- firmware/os/algos/common/math/kasa.h | 1 + 7 files changed, 121 insertions(+), 24 deletions(-) create mode 100644 firmware/os/algos/calibration/online_calibration/common_data/result_callback_interface.h diff --git a/firmware/os/algos/calibration/nano_calibration/nano_calibration.cc b/firmware/os/algos/calibration/nano_calibration/nano_calibration.cc index a8daaeb8..122af489 100644 --- a/firmware/os/algos/calibration/nano_calibration/nano_calibration.cc +++ b/firmware/os/algos/calibration/nano_calibration/nano_calibration.cc @@ -19,7 +19,7 @@ #include #include -#include "chre/util/nanoapp/log.h" +#include "common/techeng_log_util.h" namespace nano_calibration { namespace { @@ -35,16 +35,15 @@ constexpr char kMagTag[] = {"[NanoSensorCal:MAG_UT]"}; // messages will be produced at a rate determined by // 'slow_message_interval_min'. struct LogMessageRegimen { - uint8_t rapid_message_interval_sec; // Assists device verification. - uint8_t slow_message_interval_min; // Avoids long-term log spam. + uint8_t rapid_message_interval_sec; // Assists device verification. + uint8_t slow_message_interval_min; // Avoids long-term log spam. uint8_t duration_of_rapid_messages_min; }; constexpr LogMessageRegimen kGyroscopeMessagePlan = { /*rapid_message_interval_sec*/ 20, /*slow_message_interval_min*/ 5, - /*duration_of_rapid_messages_min*/ 3 -}; + /*duration_of_rapid_messages_min*/ 3}; using ::online_calibration::CalibrationDataThreeAxis; using ::online_calibration::CalibrationTypeFlags; @@ -58,18 +57,22 @@ using ::online_calibration::SensorType; #endif #ifdef NANO_SENSOR_CAL_DBG_ENABLED -#define NANO_CAL_LOGD(tag, format, ...) LOGD("%s " format, tag, ##__VA_ARGS__) -#define NANO_CAL_LOGW(tag, format, ...) LOGW("%s " format, tag, ##__VA_ARGS__) -#define NANO_CAL_LOGE(tag, format, ...) LOGE("%s " format, tag, ##__VA_ARGS__) +#define NANO_CAL_LOGD(tag, format, ...) \ + TECHENG_LOGD("%s " format, tag, ##__VA_ARGS__) +#define NANO_CAL_LOGW(tag, format, ...) \ + TECHENG_LOGW("%s " format, tag, ##__VA_ARGS__) +#define NANO_CAL_LOGE(tag, format, ...) \ + TECHENG_LOGE("%s " format, tag, ##__VA_ARGS__) #else -#define NANO_CAL_LOGD(tag, format, ...) CHRE_LOG_NULL(format, ##__VA_ARGS__) -#define NANO_CAL_LOGW(tag, format, ...) CHRE_LOG_NULL(format, ##__VA_ARGS__) -#define NANO_CAL_LOGE(tag, format, ...) CHRE_LOG_NULL(format, ##__VA_ARGS__) +#define NANO_CAL_LOGD(tag, format, ...) techeng_log_null(format, ##__VA_ARGS__) +#define NANO_CAL_LOGW(tag, format, ...) techeng_log_null(format, ##__VA_ARGS__) +#define NANO_CAL_LOGE(tag, format, ...) techeng_log_null(format, ##__VA_ARGS__) #endif // NANO_SENSOR_CAL_DBG_ENABLED // NOTE: LOGI is defined to ensure calibration updates are always logged for // field diagnosis and verification. -#define NANO_CAL_LOGI(tag, format, ...) LOGI("%s " format, tag, ##__VA_ARGS__) +#define NANO_CAL_LOGI(tag, format, ...) \ + TECHENG_LOGI("%s " format, tag, ##__VA_ARGS__) } // namespace @@ -199,6 +202,12 @@ void NanoSensorCal::ProcessSample(const SensorData &sample) { accel_cal_update_flags_, kAccelTag); PrintCalibration(accel_cal_->GetSensorCalibration(), accel_cal_update_flags_, kAccelTag); + + if (result_callback_ != nullptr) { + result_callback_->SetCalibrationEvent(sample.timestamp_nanos, + SensorType::kAccelerometerMps2, + accel_cal_update_flags_); + } } } @@ -210,7 +219,19 @@ void NanoSensorCal::ProcessSample(const SensorData &sample) { if (NotifyAshCalibration(CHRE_SENSOR_TYPE_GYROSCOPE, gyro_cal_->GetSensorCalibration(), gyro_cal_update_flags_, kGyroTag)) { - HandleGyroLogMessage(sample.timestamp_nanos); + const bool print_gyro_log = + HandleGyroLogMessage(sample.timestamp_nanos); + + if (result_callback_ != nullptr && + (print_gyro_log || + gyro_cal_update_flags_ != CalibrationTypeFlags::BIAS)) { + // Rate-limits OTC gyro telemetry updates since they can happen + // frequently with temperature change. However, all GyroCal stillness + // and OTC model parameter updates will be recorded. + result_callback_->SetCalibrationEvent(sample.timestamp_nanos, + SensorType::kGyroscopeRps, + gyro_cal_update_flags_); + } } } } @@ -224,6 +245,12 @@ void NanoSensorCal::ProcessSample(const SensorData &sample) { mag_cal_update_flags_, kMagTag); PrintCalibration(mag_cal_->GetSensorCalibration(), mag_cal_update_flags_, kMagTag); + + if (result_callback_ != nullptr) { + result_callback_->SetCalibrationEvent(sample.timestamp_nanos, + SensorType::kMagnetometerUt, + mag_cal_update_flags_); + } } } } @@ -294,7 +321,7 @@ bool NanoSensorCal::NotifyAshCalibration( bool NanoSensorCal::LoadAshCalibration(uint8_t chreSensorType, OnlineCalibrationThreeAxis *online_cal, - CalibrationTypeFlags* flags, + CalibrationTypeFlags *flags, const char *sensor_tag) { ashCalParams recalled_ash_cal_parameters; if (ashLoadCalibrationParams(chreSensorType, ASH_CAL_STORAGE_ASH, @@ -443,7 +470,7 @@ void NanoSensorCal::PrintCalibration(const CalibrationDataThreeAxis &cal_data, } } -void NanoSensorCal::HandleGyroLogMessage(uint64_t timestamp_nanos) { +bool NanoSensorCal::HandleGyroLogMessage(uint64_t timestamp_nanos) { // Limits the log messaging update rate for the gyro calibrations since // these can occur frequently with rapid temperature changes. const int64_t next_log_interval_nanos = @@ -454,14 +481,15 @@ void NanoSensorCal::HandleGyroLogMessage(uint64_t timestamp_nanos) { : SEC_TO_NANOS(kGyroscopeMessagePlan.rapid_message_interval_sec); const bool print_gyro_log = NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA( - timestamp_nanos, gyro_notification_time_nanos_, - next_log_interval_nanos); + timestamp_nanos, gyro_notification_time_nanos_, next_log_interval_nanos); if (print_gyro_log) { gyro_notification_time_nanos_ = timestamp_nanos; PrintCalibration(gyro_cal_->GetSensorCalibration(), gyro_cal_update_flags_, kGyroTag); } + + return print_gyro_log; } } // namespace nano_calibration diff --git a/firmware/os/algos/calibration/nano_calibration/nano_calibration.h b/firmware/os/algos/calibration/nano_calibration/nano_calibration.h index 5dad0f68..82a8396d 100644 --- a/firmware/os/algos/calibration/nano_calibration/nano_calibration.h +++ b/firmware/os/algos/calibration/nano_calibration/nano_calibration.h @@ -42,16 +42,17 @@ #ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_NANO_CALIBRATION_NANO_CALIBRATION_H_ #define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_NANO_CALIBRATION_NANO_CALIBRATION_H_ +#include +#include #include #include -#include -#include #include #include "calibration/online_calibration/common_data/calibration_callback.h" #include "calibration/online_calibration/common_data/calibration_data.h" #include "calibration/online_calibration/common_data/online_calibration.h" +#include "calibration/online_calibration/common_data/result_callback_interface.h" #include "calibration/online_calibration/common_data/sensor_data.h" #include "common/math/macros.h" @@ -88,6 +89,11 @@ class NanoSensorCal { void HandleTemperatureSamples(uint16_t event_type, const chreSensorFloatData *event_data); + void set_result_callback( + online_calibration::ResultCallbackInterface *result_callback) { + result_callback_ = result_callback; + } + private: // Passes sensor data to the runtime calibration algorithms. void ProcessSample(const online_calibration::SensorData &sample); @@ -99,7 +105,7 @@ class NanoSensorCal { // which runtime calibration parameters were recalled. bool LoadAshCalibration(uint8_t chreSensorType, OnlineCalibrationThreeAxis *online_cal, - online_calibration::CalibrationTypeFlags* flags, + online_calibration::CalibrationTypeFlags *flags, const char *sensor_tag); // Provides sensor calibration updates using the ASH API for the specified @@ -126,7 +132,7 @@ class NanoSensorCal { const online_calibration::CalibrationDataThreeAxis &cal_data, online_calibration::CalibrationTypeFlags flags, const char *sensor_tag); - void HandleGyroLogMessage(uint64_t timestamp_nanos); + bool HandleGyroLogMessage(uint64_t timestamp_nanos); // Pointer to the accelerometer runtime calibration object. OnlineCalibrationThreeAxis *accel_cal_ = nullptr; @@ -154,6 +160,9 @@ class NanoSensorCal { online_calibration::CalibrationTypeFlags::NONE; online_calibration::CalibrationTypeFlags mag_cal_update_flags_ = online_calibration::CalibrationTypeFlags::NONE; + + // Pointer to telemetry logger. + online_calibration::ResultCallbackInterface *result_callback_ = nullptr; }; } // namespace nano_calibration diff --git a/firmware/os/algos/calibration/online_calibration/common_data/calibration_data.h b/firmware/os/algos/calibration/online_calibration/common_data/calibration_data.h index 7d66f756..f21108e2 100644 --- a/firmware/os/algos/calibration/online_calibration/common_data/calibration_data.h +++ b/firmware/os/algos/calibration/online_calibration/common_data/calibration_data.h @@ -48,6 +48,12 @@ namespace online_calibration { * behavior with temperature (e.g., linear bias sensitivity * model). * QUALITY_DEGRADED - Indicates a degradation in calibration quality. + * OTC_STILL_BIAS - Indicates that a stillness-induced bias update occurred as + * an input to the over-temperature compensation algorithm + * NOTE: Stillness bias values (e.g., GyroCal) may be + * different from the OTC bias. If these bias value are + * desired, they should be retrieved directly (see related + * calibration wrappers for access [e.g., GyroOffsetOtcCal]). */ enum class CalibrationTypeFlags : uint8_t { NONE = 0x00, @@ -56,6 +62,7 @@ enum class CalibrationTypeFlags : uint8_t { CROSS_AXIS = 0x04, OVER_TEMP = 0x08, QUALITY_DEGRADED = 0x10, + OTC_STILL_BIAS = 0x20, ALL = 0xFF }; diff --git a/firmware/os/algos/calibration/online_calibration/common_data/result_callback_interface.h b/firmware/os/algos/calibration/online_calibration/common_data/result_callback_interface.h new file mode 100644 index 00000000..ca54f2fb --- /dev/null +++ b/firmware/os/algos/calibration/online_calibration/common_data/result_callback_interface.h @@ -0,0 +1,32 @@ +#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_RESULT_CALLBACK_INTERFACE_H_ +#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_RESULT_CALLBACK_INTERFACE_H_ + +#include "calibration/online_calibration/common_data/calibration_data.h" +#include "calibration/online_calibration/common_data/sensor_data.h" + +namespace online_calibration { + +// Interface for a results callback implementation (useful for building +// calibration event loggers). +class ResultCallbackInterface { + protected: + // Protected destructor. The implementation can destroy itself, it can't be + // destroyed through this interface. + virtual ~ResultCallbackInterface() = default; + + public: + // Sets a calibration event, such as a magnetometer calibration event. + // + // event_timestamp_nanos: Timestamp in nanoseconds of when the calibration + // event was produced in the sensor timebase. + // sensor_type: Which sensor the calibration was produced for. + // flags: What kind of update the calibration was, e.g. offset, quality + // degradation (like a magnetization event), over temperature, etc. + virtual void SetCalibrationEvent(uint64_t event_timestamp_nanos, + SensorType sensor_type, + CalibrationTypeFlags flags) = 0; +}; + +} // namespace online_calibration + +#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_RESULT_CALLBACK_INTERFACE_H_ diff --git a/firmware/os/algos/calibration/online_calibration/gyroscope/gyro_offset_over_temp_cal/gyro_offset_over_temp_cal.cc b/firmware/os/algos/calibration/online_calibration/gyroscope/gyro_offset_over_temp_cal/gyro_offset_over_temp_cal.cc index d73ed3b4..02c26709 100644 --- a/firmware/os/algos/calibration/online_calibration/gyroscope/gyro_offset_over_temp_cal/gyro_offset_over_temp_cal.cc +++ b/firmware/os/algos/calibration/online_calibration/gyroscope/gyro_offset_over_temp_cal/gyro_offset_over_temp_cal.cc @@ -71,6 +71,7 @@ CalibrationTypeFlags GyroOffsetOtcCal::SetMeasurement( } // Checks for a new calibration, and updates the OTC. + CalibrationTypeFlags cal_update_callback_flags = CalibrationTypeFlags::NONE; if (gyroCalNewBiasAvailable(&gyro_cal_)) { float offset[3]; float temperature_celsius = kInvalidTemperatureCelsius; @@ -79,6 +80,7 @@ CalibrationTypeFlags GyroOffsetOtcCal::SetMeasurement( &temperature_celsius, &calibration_time_nanos); overTempCalUpdateSensorEstimate(&over_temp_cal_, calibration_time_nanos, offset, temperature_celsius); + cal_update_callback_flags |= CalibrationTypeFlags::OTC_STILL_BIAS; } // Checks the OTC for a new calibration model update. @@ -89,7 +91,6 @@ CalibrationTypeFlags GyroOffsetOtcCal::SetMeasurement( const bool new_otc_offset = overTempCalNewOffsetAvailable(&over_temp_cal_); // Sets the new calibration data. - CalibrationTypeFlags cal_update_callback_flags = CalibrationTypeFlags::NONE; if (new_otc_offset) { overTempCalGetOffset(&over_temp_cal_, &cal_data_.offset_temp_celsius, cal_data_.offset); @@ -111,7 +112,7 @@ CalibrationTypeFlags GyroOffsetOtcCal::SetMeasurement( // Sets the new calibration quality, polling flag, and notifies a calibration // callback listener of the new update. - if (new_otc_model_update || new_otc_offset) { + if (cal_update_callback_flags != CalibrationTypeFlags::NONE) { cal_data_.calibration_quality.level = CalibrationQualityLevel::HIGH_QUALITY; cal_data_.calibration_quality.value = kHighQualityRps; cal_update_polling_flags_ |= cal_update_callback_flags; diff --git a/firmware/os/algos/common/math/kasa.c b/firmware/os/algos/common/math/kasa.c index a24a31bf..911afba2 100644 --- a/firmware/os/algos/common/math/kasa.c +++ b/firmware/os/algos/common/math/kasa.c @@ -6,6 +6,7 @@ #include "common/math/mat.h" void kasaReset(struct KasaFit *kasa) { + kasa->acc_mean_x = kasa->acc_mean_y = kasa->acc_mean_z = 0.0f; kasa->acc_x = kasa->acc_y = kasa->acc_z = kasa->acc_w = 0.0f; kasa->acc_xx = kasa->acc_xy = kasa->acc_xz = kasa->acc_xw = 0.0f; kasa->acc_yy = kasa->acc_yz = kasa->acc_yw = 0.0f; @@ -16,6 +17,21 @@ void kasaReset(struct KasaFit *kasa) { void kasaInit(struct KasaFit *kasa) { kasaReset(kasa); } void kasaAccumulate(struct KasaFit *kasa, float x, float y, float z) { + // KASA fit runs into numerical accuracy issues for large offset and small + // radii. Assuming that all points are on an sphere we can substract the + // first x,y,z value from all incoming data, making sure that the sphere will + // always go through 0,0,0 ensuring the highest possible numerical accuracy. + if (kasa->nsamples == 0) { + kasa->acc_mean_x = x; + kasa->acc_mean_y = y; + kasa->acc_mean_z = z; + } + + x = x - kasa->acc_mean_x; + y = y - kasa->acc_mean_y; + z = z - kasa->acc_mean_z; + + // Accumulation. float w = x * x + y * y + z * z; kasa->acc_x += x; @@ -108,7 +124,10 @@ int kasaFit(struct KasaFit *kasa, struct Vec3 *bias, float *radius, float r_square = vec3Dot(&v, &v) - out.w; float r = (r_square > 0) ? sqrtf(r_square) : 0; - initVec3(bias, v.x, v.y, v.z); + // Need to correct the bias with the first sample, which was used to shift + // the sphere in order to have best accuracy. + initVec3(bias, v.x + kasa->acc_mean_x, v.y + kasa->acc_mean_y, + v.z + kasa->acc_mean_z); *radius = r; int success = 0; diff --git a/firmware/os/algos/common/math/kasa.h b/firmware/os/algos/common/math/kasa.h index e9652d60..d3504b6f 100644 --- a/firmware/os/algos/common/math/kasa.h +++ b/firmware/os/algos/common/math/kasa.h @@ -20,6 +20,7 @@ extern "C" { #endif struct KasaFit { + float acc_mean_x, acc_mean_y, acc_mean_z; float acc_x, acc_y, acc_z, acc_w; float acc_xx, acc_xy, acc_xz, acc_xw; float acc_yy, acc_yz, acc_yw, acc_zz, acc_zw; -- cgit v1.2.3