[automerger skipped] [ImuCal] Minimal calibration info logging am: 98f1a92c3f am: 8051945f94 -s ours
am skip reason: Change-Id I476e04be50b8f324cab5fc0ccd72a825db514e42 with SHA-1 ba4f3429b6 is in history
Change-Id: I2b3a101b598e94ef0fa4e6d11a8857558a13784b
diff --git a/firmware/os/algos/calibration/accelerometer/accel_cal.c b/firmware/os/algos/calibration/accelerometer/accel_cal.c
index 99e96ef..d171638 100644
--- a/firmware/os/algos/calibration/accelerometer/accel_cal.c
+++ b/firmware/os/algos/calibration/accelerometer/accel_cal.c
@@ -22,7 +22,9 @@
#include <stdio.h>
#include <string.h>
+#if defined(ACCEL_CAL_DBG_ENABLED) || defined(IMU_TEMP_DBG_ENABLED)
#include "calibration/util/cal_log.h"
+#endif // ACCEL_CAL_DBG_ENABLED || IMU_TEMP_DBG_ENABLED
// clang-format off
#define KSCALE \
diff --git a/firmware/os/algos/calibration/gyroscope/gyro_cal.c b/firmware/os/algos/calibration/gyroscope/gyro_cal.c
index 90b2544..adc9efc 100644
--- a/firmware/os/algos/calibration/gyroscope/gyro_cal.c
+++ b/firmware/os/algos/calibration/gyroscope/gyro_cal.c
@@ -21,7 +21,10 @@
#include <math.h>
#include <string.h>
+#ifdef GYRO_CAL_DBG_ENABLED
#include "calibration/util/cal_log.h"
+#endif // GYRO_CAL_DBG_ENABLED
+
#include "common/math/macros.h"
/////// DEFINITIONS AND MACROS ///////////////////////////////////////
@@ -195,10 +198,6 @@
#endif // GYRO_CAL_DBG_ENABLED
}
-// Void pointer in the gyro calibration data structure (doesn't do anything
-// except prevent compiler warnings).
-void gyroCalDestroy(struct GyroCal* gyro_cal) { (void)gyro_cal; }
-
// Get the most recent bias calibration value.
void gyroCalGetBias(struct GyroCal* gyro_cal, float* bias_x, float* bias_y,
float* bias_z, float* temperature_celsius,
diff --git a/firmware/os/algos/calibration/gyroscope/gyro_cal.h b/firmware/os/algos/calibration/gyroscope/gyro_cal.h
index 1f17254..7b5c1d3 100644
--- a/firmware/os/algos/calibration/gyroscope/gyro_cal.h
+++ b/firmware/os/algos/calibration/gyroscope/gyro_cal.h
@@ -213,9 +213,6 @@
void gyroCalInit(struct GyroCal* gyro_cal,
const struct GyroCalParameters* parameters);
-// Void all pointers in the gyro calibration data structure.
-void gyroCalDestroy(struct GyroCal* gyro_cal);
-
// Get the most recent bias calibration value.
void gyroCalGetBias(struct GyroCal* gyro_cal, float* bias_x, float* bias_y,
float* bias_z, float* temperature_celsius,
diff --git a/firmware/os/algos/calibration/magnetometer/mag_cal/mag_cal.c b/firmware/os/algos/calibration/magnetometer/mag_cal/mag_cal.c
index c3f12ae..fda5170 100644
--- a/firmware/os/algos/calibration/magnetometer/mag_cal/mag_cal.c
+++ b/firmware/os/algos/calibration/magnetometer/mag_cal/mag_cal.c
@@ -20,7 +20,9 @@
#include <inttypes.h>
#include <string.h>
+#ifdef MAG_CAL_DEBUG_ENABLE
#include "calibration/util/cal_log.h"
+#endif // MAG_CAL_DEBUG_ENABLE
// Local helper macro for printing log messages.
#ifdef MAG_CAL_DEBUG_ENABLE
diff --git a/firmware/os/algos/calibration/magnetometer/mag_sphere_fit_cal/mag_sphere_fit.c b/firmware/os/algos/calibration/magnetometer/mag_sphere_fit_cal/mag_sphere_fit.c
index 93c2ac6..0a6f799 100644
--- a/firmware/os/algos/calibration/magnetometer/mag_sphere_fit_cal/mag_sphere_fit.c
+++ b/firmware/os/algos/calibration/magnetometer/mag_sphere_fit_cal/mag_sphere_fit.c
@@ -60,8 +60,6 @@
magCalSphereOdrUpdate(mocs, default_odr_in_hz);
}
-void magCalSphereDestroy(struct MagCalSphere *mocs) { (void)mocs; }
-
void magCalSphereOdrUpdate(struct MagCalSphere *mocs, float odr_in_hz) {
// Calculate the numbers of samples to be dropped, in order to fill up
// the data set.
diff --git a/firmware/os/algos/calibration/magnetometer/mag_sphere_fit_cal/mag_sphere_fit.h b/firmware/os/algos/calibration/magnetometer/mag_sphere_fit_cal/mag_sphere_fit.h
index 3ae4687..0d7ffa6 100644
--- a/firmware/os/algos/calibration/magnetometer/mag_sphere_fit_cal/mag_sphere_fit.h
+++ b/firmware/os/algos/calibration/magnetometer/mag_sphere_fit_cal/mag_sphere_fit.h
@@ -57,8 +57,6 @@
const struct DiversityCheckerParameters *diverse_parameters,
float default_odr_in_hz);
-void magCalSphereDestroy(struct MagCalSphere *mocs);
-
enum MagUpdate magCalSphereUpdate(struct MagCalSphere *mocs,
uint64_t sample_time_us, float x, float y,
float z);
diff --git a/firmware/os/algos/calibration/nano_calibration/aosp_nano_cal_parameters.h b/firmware/os/algos/calibration/nano_calibration/aosp_nano_cal_parameters.h
deleted file mode 100644
index 12b8470..0000000
--- a/firmware/os/algos/calibration/nano_calibration/aosp_nano_cal_parameters.h
+++ /dev/null
@@ -1,125 +0,0 @@
-/*
- * Copyright (C) 2018 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.
- */
-
-#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_NANO_CALIBRATION_AOSP_NANO_CAL_PARAMETERS_H_
-#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_NANO_CALIBRATION_AOSP_NANO_CAL_PARAMETERS_H_
-
-#ifdef ACCEL_CAL_ENABLED
-#include "calibration/accelerometer/accel_cal.h"
-#endif // ACCEL_CAL_ENABLED
-
-#ifdef GYRO_CAL_ENABLED
-#include "calibration/gyroscope/gyro_cal.h"
-#include "calibration/over_temp/over_temp_cal.h"
-#endif // GYRO_CAL_ENABLED
-
-#ifdef MAG_CAL_ENABLED
-#include "calibration/diversity_checker/diversity_checker.h"
-#include "calibration/magnetometer/mag_cal/mag_cal.h"
-#endif // MAG_CAL_ENABLED
-
-#include "common/math/macros.h"
-
-namespace nano_calibration {
-
-//////// ACCELEROMETER CALIBRATION ////////
-#ifdef ACCEL_CAL_ENABLED
-constexpr AccelCalParameters kAccelCalParameters{
- MSEC_TO_NANOS(800), // t0
- 5, // n_s
- 15, // fx
- 15, // fxb
- 15, // fy
- 15, // fyb
- 15, // fz
- 15, // fzb
- 15, // fle
- 0.00025f // th
-};
-#endif // ACCEL_CAL_ENABLED
-
-//////// GYROSCOPE CALIBRATION ////////
-#ifdef GYRO_CAL_ENABLED
-constexpr GyroCalParameters kGyroCalParameters{
- SEC_TO_NANOS(1.4), // min_still_duration_nanos
- SEC_TO_NANOS(1.4), // max_still_duration_nanos [see, NOTE 1]
- 0, // calibration_time_nanos
- MSEC_TO_NANOS(500), // window_time_duration_nanos
- 0, // bias_x
- 0, // bias_y
- 0, // bias_z
- 0.95f, // stillness_threshold
- MDEG_TO_RAD * 60.0f, // stillness_mean_delta_limit [rad/sec]
- 3.0e-5f, // gyro_var_threshold [rad/sec]^2
- 3.0e-6f, // gyro_confidence_delta [rad/sec]^2
- 4.5e-3f, // accel_var_threshold [m/sec^2]^2
- 9.0e-4f, // accel_confidence_delta [m/sec^2]^2
- 5.0f, // mag_var_threshold [uTesla]^2
- 1.0f, // mag_confidence_delta [uTesla]^2
- 1.5f, // temperature_delta_limit_celsius
- true // gyro_calibration_enable
-};
-// [NOTE 1]: 'max_still_duration_nanos' 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).
-
-constexpr OverTempCalParameters kGyroOtcParameters{
- MSEC_TO_NANOS(100), // min_temp_update_period_nanos
- DAYS_TO_NANOS(2), // age_limit_nanos
- 0.75f, // delta_temp_per_bin
- 40.0f * MDEG_TO_RAD, // jump_tolerance
- 100.0f * MDEG_TO_RAD, // outlier_limit
- 250.0f * MDEG_TO_RAD, // temp_sensitivity_limit
- 8.0e3f * MDEG_TO_RAD, // sensor_intercept_limit
- 0.1f * MDEG_TO_RAD, // significant_offset_change
- 5, // min_num_model_pts
- true // over_temp_enable
-};
-#endif // GYRO_CAL_ENABLED
-
-//////// MAGNETOMETER CALIBRATION ////////
-#ifdef MAG_CAL_ENABLED
-constexpr MagCalParameters kMagCalParameters{
- 3000000, // min_batch_window_in_micros
- 0.0f, // x_bias
- 0.0f, // y_bias
- 0.0f, // z_bias
- 1.0f, // c00
- 0.0f, // c01
- 0.0f, // c02
- 0.0f, // c10
- 1.0f, // c11
- 0.0f, // c12
- 0.0f, // c20
- 0.0f, // c21
- 1.0f // c22
-};
-
-constexpr DiversityCheckerParameters kMagDiversityParameters{
- 6.0f, // var_threshold
- 10.0f, // max_min_threshold
- 48.0f, // local_field
- 0.5f, // threshold_tuning_param
- 2.552f, // max_distance_tuning_param
- 8, // min_num_diverse_vectors
- 1 // max_num_max_distance
-};
-#endif // MAG_CAL_ENABLED
-
-} // namespace nano_calibration
-
-#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_NANO_CALIBRATION_AOSP_NANO_CAL_PARAMETERS_H_
diff --git a/firmware/os/algos/calibration/nano_calibration/nano_calibration.cc b/firmware/os/algos/calibration/nano_calibration/nano_calibration.cc
index 8dcbda7..a8daaeb 100644
--- a/firmware/os/algos/calibration/nano_calibration/nano_calibration.cc
+++ b/firmware/os/algos/calibration/nano_calibration/nano_calibration.cc
@@ -16,6 +16,7 @@
#include "calibration/nano_calibration/nano_calibration.h"
+#include <cstdint>
#include <cstring>
#include "chre/util/nanoapp/log.h"
@@ -23,6 +24,28 @@
namespace nano_calibration {
namespace {
+// Common log message sensor-specific identifiers.
+constexpr char kAccelTag[] = {"[NanoSensorCal:ACCEL_MPS2]"};
+constexpr char kGyroTag[] = {"[NanoSensorCal:GYRO_RPS]"};
+constexpr char kMagTag[] = {"[NanoSensorCal:MAG_UT]"};
+
+// Defines a plan for limiting log messages so that upon initialization there
+// begins a period set by 'duration_of_rapid_messages_min' where log messages
+// appear at a rate set by 'rapid_message_interval_sec'. Afterwards, log
+// 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 duration_of_rapid_messages_min;
+};
+
+constexpr LogMessageRegimen kGyroscopeMessagePlan = {
+ /*rapid_message_interval_sec*/ 20,
+ /*slow_message_interval_min*/ 5,
+ /*duration_of_rapid_messages_min*/ 3
+};
+
using ::online_calibration::CalibrationDataThreeAxis;
using ::online_calibration::CalibrationTypeFlags;
using ::online_calibration::SensorData;
@@ -90,6 +113,9 @@
NANO_CAL_LOGE(kMagTag, "Failed to initialize: wrong sensor type.");
}
}
+
+ // Resets the initialization timestamp. Set below in HandleSensorSamples.
+ initialization_start_time_nanos_ = 0;
}
void NanoSensorCal::HandleSensorSamples(
@@ -123,6 +149,13 @@
memcpy(sample.data, data[i].v, sizeof(sample.data));
ProcessSample(sample);
}
+
+ // Starts tracking the time after initialization to help rate limit gyro log
+ // messaging.
+ if (initialization_start_time_nanos_ == 0) {
+ initialization_start_time_nanos_ = header.baseTimestamp;
+ gyro_notification_time_nanos_ = 0;
+ }
}
void NanoSensorCal::HandleTemperatureSamples(
@@ -177,15 +210,7 @@
if (NotifyAshCalibration(CHRE_SENSOR_TYPE_GYROSCOPE,
gyro_cal_->GetSensorCalibration(),
gyro_cal_update_flags_, kGyroTag)) {
- // Limits the log messaging update rate for the gyro calibrations since
- // these can occur frequently with rapid temperature changes.
- if (NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(
- sample.timestamp_nanos, gyro_notification_time_nanos_,
- kNanoSensorCalMessageIntervalNanos)) {
- gyro_notification_time_nanos_ = sample.timestamp_nanos;
- PrintCalibration(gyro_cal_->GetSensorCalibration(),
- gyro_cal_update_flags_, kGyroTag);
- }
+ HandleGyroLogMessage(sample.timestamp_nanos);
}
}
}
@@ -312,7 +337,9 @@
}
}
} else {
- NANO_CAL_LOGE(sensor_tag, "ASH failed to recall calibration data.");
+ // This is not necessarily an error since there may not be any previously
+ // stored runtime calibration data to load yet (e.g., first device boot).
+ NANO_CAL_LOGW(sensor_tag, "ASH did not recall calibration data.");
}
return false;
@@ -416,4 +443,25 @@
}
}
+void 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 =
+ (NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(
+ timestamp_nanos, initialization_start_time_nanos_,
+ MIN_TO_NANOS(kGyroscopeMessagePlan.duration_of_rapid_messages_min)))
+ ? MIN_TO_NANOS(kGyroscopeMessagePlan.slow_message_interval_min)
+ : 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);
+
+ if (print_gyro_log) {
+ gyro_notification_time_nanos_ = timestamp_nanos;
+ PrintCalibration(gyro_cal_->GetSensorCalibration(), gyro_cal_update_flags_,
+ kGyroTag);
+ }
+}
+
} // 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 d56d034..5dad0f6 100644
--- a/firmware/os/algos/calibration/nano_calibration/nano_calibration.h
+++ b/firmware/os/algos/calibration/nano_calibration/nano_calibration.h
@@ -47,6 +47,7 @@
#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"
@@ -56,14 +57,6 @@
namespace nano_calibration {
-// Common log message sensor-specific identifiers.
-constexpr char kAccelTag[] = {"[NanoSensorCal:ACCEL_MPS2]"};
-constexpr char kGyroTag[] = {"[NanoSensorCal:GYRO_RPS]"};
-constexpr char kMagTag[] = {"[NanoSensorCal:MAG_UT]"};
-
-// Limits NanoSensorCal notifications to once every minute.
-constexpr uint64_t kNanoSensorCalMessageIntervalNanos = MIN_TO_NANOS(1);
-
/*
* NanoSensorCal is a container class for dynamic runtime calibration sensor
* algorithms used by the IMU_Cal CHRE nanoapp. The main purpose of this class
@@ -133,6 +126,8 @@
const online_calibration::CalibrationDataThreeAxis &cal_data,
online_calibration::CalibrationTypeFlags flags, const char *sensor_tag);
+ void HandleGyroLogMessage(uint64_t timestamp_nanos);
+
// Pointer to the accelerometer runtime calibration object.
OnlineCalibrationThreeAxis *accel_cal_ = nullptr;
@@ -142,6 +137,7 @@
// Limits the log messaging update rate for the gyro calibrations since these
// can occur frequently with rapid temperature changes.
uint64_t gyro_notification_time_nanos_ = 0;
+ uint64_t initialization_start_time_nanos_ = 0;
// Pointer to the magnetometer runtime calibration object.
OnlineCalibrationThreeAxis *mag_cal_ = nullptr;
diff --git a/firmware/os/algos/calibration/online_calibration/accelerometer/accel_offset_cal/accel_offset_cal.cc b/firmware/os/algos/calibration/online_calibration/accelerometer/accel_offset_cal/accel_offset_cal.cc
index e9def4e..66d8fc5 100644
--- a/firmware/os/algos/calibration/online_calibration/accelerometer/accel_offset_cal/accel_offset_cal.cc
+++ b/firmware/os/algos/calibration/online_calibration/accelerometer/accel_offset_cal/accel_offset_cal.cc
@@ -16,7 +16,9 @@
#include "calibration/online_calibration/accelerometer/accel_offset_cal/accel_offset_cal.h"
+#ifdef ACCEL_CAL_DBG_ENABLED
#include "calibration/util/cal_log.h"
+#endif // ACCEL_CAL_DBG_ENABLED
namespace online_calibration {
@@ -71,8 +73,10 @@
const CalibrationDataThreeAxis& input_cal_data) {
// Checks that the input calibration type matches the algorithm type.
if (input_cal_data.type != get_sensor_type()) {
+#ifdef ACCEL_CAL_DBG_ENABLED
CAL_DEBUG_LOG("[AccelOffsetCal]",
"SetInitialCalibration failed due to wrong sensor type.");
+#endif // ACCEL_CAL_DBG_ENABLED
return false;
}
diff --git a/firmware/os/algos/calibration/online_calibration/accelerometer/accel_offset_cal/accel_offset_cal.h b/firmware/os/algos/calibration/online_calibration/accelerometer/accel_offset_cal/accel_offset_cal.h
index 0256495..60b59df 100644
--- a/firmware/os/algos/calibration/online_calibration/accelerometer/accel_offset_cal/accel_offset_cal.h
+++ b/firmware/os/algos/calibration/online_calibration/accelerometer/accel_offset_cal/accel_offset_cal.h
@@ -68,6 +68,9 @@
return SensorType::kAccelerometerMps2;
};
+ // Accessor for the runtime calibration object.
+ const AccelCal& get_accel_cal() const { return accel_cal_; }
+
private:
// Accelerometer offset calibration algorithm data structure.
AccelCal accel_cal_;
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 0404936..7d66f75 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
@@ -131,10 +131,9 @@
SensorType type = SensorType::kUndefined;
// Optional pointer to an array of over-temperature model data (null when not
- // used). For initialization, populating a model dataset will take precedence
- // over the linear model parameters provided in the calibration data.
+ // used).
OverTempModelThreeAxis* otc_model_data = nullptr;
- int16_t num_model_pts = 0;
+ int16_t num_model_pts = 0; // Length of otc_model_data array.
// Helper function that resets the calibration data to a set of neutral
// reference values where no calibration correction would be applied if used.
@@ -203,10 +202,9 @@
SensorType type = SensorType::kUndefined;
// Optional pointer to an array of over-temperature model data (null when not
- // used). For initialization, populating a model dataset will take precedence
- // over the linear model parameters provided in the calibration data.
+ // used).
OverTempModelSingleAxis* otc_model_data = nullptr;
- int16_t num_model_pts = 0;
+ int16_t num_model_pts = 0; // Length of otc_model_data array.
// Helper function that resets the calibration data to a set of neutral
// reference values where no calibration correction would be applied if used.
diff --git a/firmware/os/algos/calibration/online_calibration/common_data/sensor_data.h b/firmware/os/algos/calibration/online_calibration/common_data/sensor_data.h
index f4c1613..23b63c9 100644
--- a/firmware/os/algos/calibration/online_calibration/common_data/sensor_data.h
+++ b/firmware/os/algos/calibration/online_calibration/common_data/sensor_data.h
@@ -46,7 +46,8 @@
kMagnetometerUt = 3, // 3-axis sensor (units = micro-Tesla).
kTemperatureCelsius = 4, // 1-axis sensor (units = degrees Celsius).
kBarometerHpa = 5, // 1-axis sensor (units = hecto-Pascal).
- kWifiM = 6 // 3-axis sensor (units = meter).
+ kWifiM = 6, // 3-axis sensor (units = meter).
+ kProximity = 7, // 1-axis sensor (units = ?).
};
// Helper function for determining if a sensor type is 3-axis, otherwise it's
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 26eef57..d73ed3b 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
@@ -16,7 +16,9 @@
#include "calibration/online_calibration/gyroscope/gyro_offset_over_temp_cal/gyro_offset_over_temp_cal.h"
+#if defined(GYRO_CAL_DBG_ENABLED) || defined(OVERTEMPCAL_DBG_ENABLED)
#include "calibration/util/cal_log.h"
+#endif // GYRO_CAL_DBG_ENABLED || OVERTEMPCAL_DBG_ENABLE
namespace online_calibration {
@@ -131,8 +133,10 @@
const CalibrationDataThreeAxis& input_cal_data) {
// Checks that the input calibration type matches the algorithm type.
if (input_cal_data.type != get_sensor_type()) {
+#if defined(GYRO_CAL_DBG_ENABLED) || defined(OVERTEMPCAL_DBG_ENABLED)
CAL_DEBUG_LOG("[GyroOffsetOtcCal]",
"SetInitialCalibration failed due to wrong sensor type.");
+#endif // GYRO_CAL_DBG_ENABLED || OVERTEMPCAL_DBG_ENABLE
return false;
}
diff --git a/firmware/os/algos/calibration/online_calibration/gyroscope/gyro_offset_over_temp_cal/gyro_offset_over_temp_cal.h b/firmware/os/algos/calibration/online_calibration/gyroscope/gyro_offset_over_temp_cal/gyro_offset_over_temp_cal.h
index e21007b..9b8962a 100644
--- a/firmware/os/algos/calibration/online_calibration/gyroscope/gyro_offset_over_temp_cal/gyro_offset_over_temp_cal.h
+++ b/firmware/os/algos/calibration/online_calibration/gyroscope/gyro_offset_over_temp_cal/gyro_offset_over_temp_cal.h
@@ -74,6 +74,10 @@
return SensorType::kGyroscopeRps;
};
+ // Accessors for the runtime calibration objects.
+ const GyroCal& get_gyro_cal() const { return gyro_cal_; }
+ const OverTempCal& get_over_temp_cal() const { return over_temp_cal_; }
+
private:
// GyroCal algorithm data structure.
GyroCal gyro_cal_;
diff --git a/firmware/os/algos/calibration/online_calibration/magnetometer/mag_diverse_cal/mag_diverse_cal.cc b/firmware/os/algos/calibration/online_calibration/magnetometer/mag_diverse_cal/mag_diverse_cal.cc
index fe787b1..2453dcd 100644
--- a/firmware/os/algos/calibration/online_calibration/magnetometer/mag_diverse_cal/mag_diverse_cal.cc
+++ b/firmware/os/algos/calibration/online_calibration/magnetometer/mag_diverse_cal/mag_diverse_cal.cc
@@ -16,12 +16,13 @@
#include "calibration/online_calibration/magnetometer/mag_diverse_cal/mag_diverse_cal.h"
+#ifdef MAG_CAL_DEBUG_ENABLE
#include "calibration/util/cal_log.h"
+#endif // MAG_CAL_DEBUG_ENABLE
namespace online_calibration {
// Empirically estimated upper bounds on offset error.
-constexpr float MagDiverseCal::kLowQualityUt;
constexpr float MagDiverseCal::kHighQualityUt;
void MagDiverseCal::Initialize(
@@ -71,8 +72,10 @@
const CalibrationDataThreeAxis& input_cal_data) {
// Checks that the input calibration type matches the algorithm type.
if (input_cal_data.type != get_sensor_type()) {
+#ifdef MAG_CAL_DEBUG_ENABLE
CAL_DEBUG_LOG("[MagDiverseCal]",
"SetInitialCalibration failed due to wrong sensor type.");
+#endif // MAG_CAL_DEBUG_ENABLE
return false;
}
diff --git a/firmware/os/algos/calibration/online_calibration/magnetometer/mag_diverse_cal/mag_diverse_cal.h b/firmware/os/algos/calibration/online_calibration/magnetometer/mag_diverse_cal/mag_diverse_cal.h
index 11ede3d..9b60a7f 100644
--- a/firmware/os/algos/calibration/online_calibration/magnetometer/mag_diverse_cal/mag_diverse_cal.h
+++ b/firmware/os/algos/calibration/online_calibration/magnetometer/mag_diverse_cal/mag_diverse_cal.h
@@ -43,8 +43,7 @@
class MagDiverseCal final : public OnlineCalibration<CalibrationDataThreeAxis> {
public:
// Empirically estimated upper bounds on offset error.
- static constexpr float kLowQualityUt = 1000.0f; // Units of micro Tesla
- static constexpr float kHighQualityUt = 5.0f; // Units of micro Tesla
+ static constexpr float kHighQualityUt = 5.0f; // Units of micro Tesla
MagDiverseCal() = default;
@@ -72,6 +71,9 @@
return SensorType::kMagnetometerUt;
};
+ // Accessor for the runtime calibration object.
+ const MagCal& get_mag_cal() const { return mag_cal_; }
+
private:
// MagCal algorithm data structure.
MagCal mag_cal_;
diff --git a/firmware/os/algos/calibration/over_temp/over_temp_cal.c b/firmware/os/algos/calibration/over_temp/over_temp_cal.c
index 84d7411..6f887f7 100644
--- a/firmware/os/algos/calibration/over_temp/over_temp_cal.c
+++ b/firmware/os/algos/calibration/over_temp/over_temp_cal.c
@@ -21,7 +21,10 @@
#include <math.h>
#include <string.h>
+#if defined(OVERTEMPCAL_DBG_ENABLED) || defined(OVERTEMPCAL_DBG_LOG_TEMP)
#include "calibration/util/cal_log.h"
+#endif // OVERTEMPCAL_DBG_ENABLED || OVERTEMPCAL_DBG_LOG_TEMP
+
#include "util/nano_assert.h"
/////// DEFINITIONS AND MACROS ////////////////////////////////////////////////
@@ -603,7 +606,7 @@
// Computes the temperature bin range data.
const int32_t bin_num =
- CAL_FLOOR(temperature_celsius / over_temp_cal->delta_temp_per_bin);
+ NANO_FLOOR(temperature_celsius / over_temp_cal->delta_temp_per_bin);
const float temp_lo_check = bin_num * over_temp_cal->delta_temp_per_bin;
const float temp_hi_check = (bin_num + 1) * over_temp_cal->delta_temp_per_bin;
@@ -746,31 +749,6 @@
over_temp_cal->last_offset_update_nanos = timestamp_nanos;
}
-void overTempGetModelError(const struct OverTempCal *over_temp_cal,
- const float *temp_sensitivity,
- const float *sensor_intercept, float *max_error) {
- ASSERT_NOT_NULL(over_temp_cal);
- ASSERT_NOT_NULL(temp_sensitivity);
- ASSERT_NOT_NULL(sensor_intercept);
- ASSERT_NOT_NULL(max_error);
-
- float max_error_test;
- memset(max_error, 0, 3 * sizeof(float));
-
- for (size_t i = 0; i < over_temp_cal->num_model_pts; i++) {
- for (size_t j = 0; j < 3; j++) {
- max_error_test =
- NANO_ABS(over_temp_cal->model_data[i].offset[j] -
- (temp_sensitivity[j] *
- over_temp_cal->model_data[i].offset_temp_celsius +
- sensor_intercept[j]));
- if (max_error_test > max_error[j]) {
- max_error[j] = max_error_test;
- }
- }
- }
-}
-
bool overTempValidateAndSetWeight(
struct OverTempCal *over_temp_cal, size_t index,
const struct OverTempCalWeight *new_otc_weight) {
@@ -1311,8 +1289,8 @@
// This defines the minimum contiguous set of points to allow a model update
// when the next offset estimate is received. They are placed at a common
// temperature range that is likely to get replaced with actual data soon.
- const int32_t start_bin_num = CAL_FLOOR(JUMPSTART_START_TEMP_CELSIUS /
- over_temp_cal->delta_temp_per_bin);
+ const int32_t start_bin_num = NANO_FLOOR(JUMPSTART_START_TEMP_CELSIUS /
+ over_temp_cal->delta_temp_per_bin);
float offset_temp_celsius =
(start_bin_num + 0.5f) * over_temp_cal->delta_temp_per_bin;
@@ -1771,4 +1749,29 @@
over_temp_cal->otc_unit_conversion = otc_unit_conversion;
}
+void overTempGetModelError(const struct OverTempCal *over_temp_cal,
+ const float *temp_sensitivity,
+ const float *sensor_intercept, float *max_error) {
+ ASSERT_NOT_NULL(over_temp_cal);
+ ASSERT_NOT_NULL(temp_sensitivity);
+ ASSERT_NOT_NULL(sensor_intercept);
+ ASSERT_NOT_NULL(max_error);
+
+ float max_error_test;
+ memset(max_error, 0, 3 * sizeof(float));
+
+ for (size_t i = 0; i < over_temp_cal->num_model_pts; i++) {
+ for (size_t j = 0; j < 3; j++) {
+ max_error_test =
+ NANO_ABS(over_temp_cal->model_data[i].offset[j] -
+ (temp_sensitivity[j] *
+ over_temp_cal->model_data[i].offset_temp_celsius +
+ sensor_intercept[j]));
+ if (max_error_test > max_error[j]) {
+ max_error[j] = max_error_test;
+ }
+ }
+ }
+}
+
#endif // OVERTEMPCAL_DBG_ENABLED
diff --git a/firmware/os/algos/calibration/over_temp/over_temp_cal.h b/firmware/os/algos/calibration/over_temp/over_temp_cal.h
index 8f6f0a4..446d0a2 100644
--- a/firmware/os/algos/calibration/over_temp/over_temp_cal.h
+++ b/firmware/os/algos/calibration/over_temp/over_temp_cal.h
@@ -505,28 +505,6 @@
float temperature_celsius);
/*
- * Computes the maximum absolute error between the 'model_data' estimates and
- * the estimate determined by the input model parameters.
- * max_error (over all i)
- * |model_data[i]->offset_xyz -
- * getCompensatedOffset(model_data[i]->offset_temp_celsius,
- * temp_sensitivity, sensor_intercept)|
- *
- * INPUTS:
- * over_temp_cal: Over-temp data structure.
- * temp_sensitivity: Model temperature sensitivity to test (array).
- * sensor_intercept: Model intercept to test (array).
- * OUTPUTS:
- * max_error: Maximum absolute error for the candidate model (array).
- *
- * NOTE 1: Arrays are all 3-dimensional with indices: 0=x, 1=y, 2=z.
- * NOTE 2: This function is provided for testing purposes.
- */
-void overTempGetModelError(const struct OverTempCal *over_temp_cal,
- const float *temp_sensitivity,
- const float *sensor_intercept, float *max_error);
-
-/*
* Defines an element in the weighting function that is used to control the
* fitting behavior of the simple linear model regression used in this module.
* The total number of weighting levels that define this functionality is set by
@@ -571,6 +549,28 @@
const char *otc_sensor_tag,
const char *otc_unit_tag,
float otc_unit_conversion);
+
+/*
+ * Computes the maximum absolute error between the 'model_data' estimates and
+ * the estimate determined by the input model parameters.
+ * max_error (over all i)
+ * |model_data[i]->offset_xyz -
+ * getCompensatedOffset(model_data[i]->offset_temp_celsius,
+ * temp_sensitivity, sensor_intercept)|
+ *
+ * INPUTS:
+ * over_temp_cal: Over-temp data structure.
+ * temp_sensitivity: Model temperature sensitivity to test (array).
+ * sensor_intercept: Model intercept to test (array).
+ * OUTPUTS:
+ * max_error: Maximum absolute error for the candidate model (array).
+ *
+ * NOTE 1: Arrays are all 3-dimensional with indices: 0=x, 1=y, 2=z.
+ * NOTE 2: This function is provided for testing purposes.
+ */
+void overTempGetModelError(const struct OverTempCal *over_temp_cal,
+ const float *temp_sensitivity,
+ const float *sensor_intercept, float *max_error);
#endif // OVERTEMPCAL_DBG_ENABLED
#ifdef __cplusplus
diff --git a/firmware/os/algos/calibration/sphere_fit/sphere_fit_calibration.c b/firmware/os/algos/calibration/sphere_fit/sphere_fit_calibration.c
index 853a73d..8729e0d 100644
--- a/firmware/os/algos/calibration/sphere_fit/sphere_fit_calibration.c
+++ b/firmware/os/algos/calibration/sphere_fit/sphere_fit_calibration.c
@@ -21,7 +21,10 @@
#include <stdio.h>
#include <string.h>
+#ifdef SPHERE_FIT_DBG_ENABLED
#include "calibration/util/cal_log.h"
+#endif // SPHERE_FIT_DBG_ENABLED
+
#include "common/math/mat.h"
#include "common/math/vec.h"
diff --git a/firmware/os/algos/calibration/sphere_fit/sphere_fit_calibration.h b/firmware/os/algos/calibration/sphere_fit/sphere_fit_calibration.h
index d3bbf7f..6dff2ad 100644
--- a/firmware/os/algos/calibration/sphere_fit/sphere_fit_calibration.h
+++ b/firmware/os/algos/calibration/sphere_fit/sphere_fit_calibration.h
@@ -141,4 +141,4 @@
}
#endif
-#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_SPHERE_FIT_SPHERE_FIT_CALIBRATION_H_
+#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_SPHERE_FIT_SPHERE_FIT_CALIBRATION_H_
diff --git a/firmware/os/algos/calibration/util/cal_log.h b/firmware/os/algos/calibration/util/cal_log.h
index 46297db..57179a7 100644
--- a/firmware/os/algos/calibration/util/cal_log.h
+++ b/firmware/os/algos/calibration/util/cal_log.h
@@ -22,6 +22,8 @@
#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_UTIL_CAL_LOG_H_
#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_UTIL_CAL_LOG_H_
+#include "common/math/macros.h"
+
// clang-format off
#ifdef GCC_DEBUG_LOG
# include <stdio.h>
@@ -55,10 +57,6 @@
extern "C" {
#endif
-// Floor macro implementation for platforms that do not supply the standard
-// floorf() math function.
-#define CAL_FLOOR(x) ((int)(x) - ((x) < (int)(x))) // NOLINT
-
/*
* On some embedded software platforms numerical string formatting is not fully
* supported. Defining CAL_NO_FLOAT_FORMAT_STRINGS will enable a workaround that
@@ -71,8 +69,8 @@
#ifdef CAL_NO_FLOAT_FORMAT_STRINGS
// Macro used to print floating point numbers with a specified number of digits.
# define CAL_ENCODE_FLOAT(x, num_digits) \
- ((x < 0) ? "-" : ""), (int)CAL_FLOOR(fabsf(x)), \
- (int)((fabsf(x) - CAL_FLOOR(fabsf(x))) * \
+ ((x < 0) ? "-" : ""), (int)NANO_FLOOR(fabsf(x)), \
+ (int)((fabsf(x) - NANO_FLOOR(fabsf(x))) * \
powf(10, num_digits)) // NOLINT
// Helper definitions for CAL_ENCODE_FLOAT to specify the print format with
diff --git a/firmware/os/algos/common/math/levenberg_marquardt.c b/firmware/os/algos/common/math/levenberg_marquardt.c
index 66e423f..8998680 100644
--- a/firmware/os/algos/common/math/levenberg_marquardt.c
+++ b/firmware/os/algos/common/math/levenberg_marquardt.c
@@ -39,10 +39,6 @@
solver->num_iter = 0;
}
-void lmSolverDestroy(struct LmSolver *solver) {
- (void)solver;
-}
-
void lmSolverSetData(struct LmSolver *solver, struct LmData *data) {
ASSERT_NOT_NULL(solver);
ASSERT_NOT_NULL(data);
diff --git a/firmware/os/algos/common/math/levenberg_marquardt.h b/firmware/os/algos/common/math/levenberg_marquardt.h
index d62308d..996a5bb 100644
--- a/firmware/os/algos/common/math/levenberg_marquardt.h
+++ b/firmware/os/algos/common/math/levenberg_marquardt.h
@@ -111,8 +111,6 @@
void lmSolverInit(struct LmSolver *solver, const struct LmParams *params,
ResidualAndJacobianFunction func);
-void lmSolverDestroy(struct LmSolver *solver);
-
// Sets pointer for temporary data needed for an individual LM solve.
// Note, this must be called prior to calling lmSolverSolve().
void lmSolverSetData(struct LmSolver *solver, struct LmData *data);
diff --git a/firmware/os/algos/common/math/macros.h b/firmware/os/algos/common/math/macros.h
index cb75595..060dc0f 100644
--- a/firmware/os/algos/common/math/macros.h
+++ b/firmware/os/algos/common/math/macros.h
@@ -31,25 +31,29 @@
#define NANO_MIN(a, b) ((a) < (b)) ? (a) : (b)
#define SIGMOID(x) (1 / (1 + expf(-x)))
+// Floor macro implementation for platforms that do not supply the standard
+// floorf() math function.
+#define NANO_FLOOR(x) ((int)(x) - ((x) < (int)(x))) // NOLINT
+
// Timestamp conversion macros.
#ifdef __cplusplus
-#define MSEC_TO_NANOS(x) (static_cast<uint64_t>(x * UINT64_C(1000000)))
+#define MSEC_TO_NANOS(x) (static_cast<uint64_t>((x) * UINT64_C(1000000)))
#else
-#define MSEC_TO_NANOS(x) ((uint64_t)(x * UINT64_C(1000000))) // NOLINT
+#define MSEC_TO_NANOS(x) ((uint64_t)((x) * UINT64_C(1000000))) // NOLINT
#endif
-#define SEC_TO_NANOS(x) MSEC_TO_NANOS(x * UINT64_C(1000))
-#define MIN_TO_NANOS(x) SEC_TO_NANOS (x * UINT64_C(60))
-#define HRS_TO_NANOS(x) MIN_TO_NANOS (x * UINT64_C(60))
-#define DAYS_TO_NANOS(x) HRS_TO_NANOS (x * UINT64_C(24))
+#define SEC_TO_NANOS(x) MSEC_TO_NANOS((x) * UINT64_C(1000))
+#define MIN_TO_NANOS(x) SEC_TO_NANOS ((x) * UINT64_C(60))
+#define HRS_TO_NANOS(x) MIN_TO_NANOS ((x) * UINT64_C(60))
+#define DAYS_TO_NANOS(x) HRS_TO_NANOS ((x) * UINT64_C(24))
// Sample rate to period conversion.
#ifdef __cplusplus
#define HZ_TO_PERIOD_NANOS(hz) \
- (SEC_TO_NANOS(1024) / (static_cast<uint64_t>(hz * 1024)))
+ (SEC_TO_NANOS(1024) / (static_cast<uint64_t>((hz) * 1024)))
#else
#define HZ_TO_PERIOD_NANOS(hz) \
- (SEC_TO_NANOS(1024) / ((uint64_t)(hz * 1024))) // NOLINT
+ (SEC_TO_NANOS(1024) / ((uint64_t)((hz) * 1024))) // NOLINT
#endif
// Unit conversion: nanoseconds to seconds.
diff --git a/firmware/os/algos/common/math/mat.h b/firmware/os/algos/common/math/mat.h
index 9d69405..d5391f9 100644
--- a/firmware/os/algos/common/math/mat.h
+++ b/firmware/os/algos/common/math/mat.h
@@ -45,6 +45,7 @@
float elem[3][3];
};
+// Note: Keep this code to preserve Android codebase dependencies.
struct Size3 {
uint32_t elem[3];
};
@@ -109,6 +110,7 @@
void mat33MultiplyTransposed(struct Mat33 *out, const struct Mat33 *A,
const struct Mat33 *B);
+// Note: Keep this code to preserve Android codebase dependencies.
// Updates out with the multiplication of A with B's transpose, i.e.:
// out = A B^T
void mat33MultiplyTransposed2(struct Mat33 *out, const struct Mat33 *A,
