Snap for 7339742 from 846b602d3936757bf722778b355a2d97100efccd to sc-release
Change-Id: I30e761cf1cc715bcb2237ecf6e22fed3a20a495c
diff --git a/firmware/os/algos/calibration/nano_calibration/nano_calibration.cc b/firmware/os/algos/calibration/nano_calibration/nano_calibration.cc
index a8daaeb..122af48 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 <cstdint>
#include <cstring>
-#include "chre/util/nanoapp/log.h"
+#include "common/techeng_log_util.h"
namespace nano_calibration {
namespace {
@@ -35,16 +35,15 @@
// 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 @@
#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 @@
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 @@
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 @@
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::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::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 @@
: 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 5dad0f6..82a8396 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 <ash.h>
+#include <chre.h>
#include <stdbool.h>
#include <stdint.h>
-#include <ash.h>
-#include <chre.h>
#include <cstdint>
#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 @@
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 @@
// 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 @@
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 @@
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 7d66f75..f21108e 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 @@
* 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 @@
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 0000000..ca54f2f
--- /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 d73ed3b..02c2670 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 @@
}
// 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 @@
&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 @@
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 @@
// 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 a24a31b..911afba 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 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 @@
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 e9652d6..d3504b6 100644
--- a/firmware/os/algos/common/math/kasa.h
+++ b/firmware/os/algos/common/math/kasa.h
@@ -20,6 +20,7 @@
#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;
