Sphere fit was removed from the KASA bias fit
This allows to run both algorithms at the same time
and to be optimized independently. Furthermore the sphere
fit parameters are now stored as well.
This is a G3 sync from CL 157860258.
Memory usage:
Total memory used by MagCal: 544 Bytes
Total memory used by MagCalSphere: 4392 Bytes.
Total memory used by LmData: 3040 Bytes.
Bug: 62413555
Test: The code compiles and was tested on devices. Functionality
was confirmed via several figure 8 triggers and performance was
confirmed by checking offset estimates against true offset measured in
zero gauss chamber. All test pass.
Change-Id: Ib4aad2f0ec0cf11a4ef06d2bfcd04d736307e0d1
diff --git a/firmware/os/algos/calibration/common/diversity_checker.c b/firmware/os/algos/calibration/common/diversity_checker.c
index 5e17da1..d71ad9a 100644
--- a/firmware/os/algos/calibration/common/diversity_checker.c
+++ b/firmware/os/algos/calibration/common/diversity_checker.c
@@ -204,7 +204,7 @@
void diversityCheckerLocalFieldUpdate(struct DiversityChecker* diverse_data,
float local_field) {
- if ( local_field > 0 ) {
+ if (local_field > 0) {
// Updating threshold based on the local field information.
diverse_data->threshold = diverse_data->threshold_tuning_param_sq *
(local_field * local_field);
diff --git a/firmware/os/algos/calibration/magnetometer/mag_cal.c b/firmware/os/algos/calibration/magnetometer/mag_cal.c
index 958e080..1fb7630 100644
--- a/firmware/os/algos/calibration/magnetometer/mag_cal.c
+++ b/firmware/os/algos/calibration/magnetometer/mag_cal.c
@@ -416,7 +416,7 @@
// Sample counter.
static size_t sample_counter = 0;
const float* data_log_ptr =
- &diverse_data->diversity_dbg.diverse_kasa_batchingdata_log[0];
+ &diverse_data->diversity_dbg.diverse_data_log[0];
if (diverse_data->diversity_dbg.new_trigger == 1) {
sample_counter++;
if (sample_counter == 2) {
diff --git a/firmware/os/algos/calibration/magnetometer/mag_cal.h b/firmware/os/algos/calibration/magnetometer/mag_cal.h
index d9fd573..8c9da6f 100644
--- a/firmware/os/algos/calibration/magnetometer/mag_cal.h
+++ b/firmware/os/algos/calibration/magnetometer/mag_cal.h
@@ -43,9 +43,9 @@
};
enum MagUpdate {
- NO_UPDATE = 0,
- UPDATE_BIAS,
- UPDATE_SPHERE_FIT,
+ NO_UPDATE = 0x00,
+ UPDATE_BIAS = 0x01,
+ UPDATE_SPHERE_FIT = 0x02,
};
#ifdef MAG_CAL_DEBUG_ENABLE
diff --git a/firmware/os/algos/calibration/magnetometer/mag_sphere_fit.c b/firmware/os/algos/calibration/magnetometer/mag_sphere_fit.c
new file mode 100644
index 0000000..9d6591c
--- /dev/null
+++ b/firmware/os/algos/calibration/magnetometer/mag_sphere_fit.c
@@ -0,0 +1,153 @@
+/*
+ * 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 "calibration/magnetometer/mag_sphere_fit.h"
+
+#include <errno.h>
+#include <string.h>
+
+#include "calibration/util/cal_log.h"
+
+#define MAX_ITERATIONS 30
+#define INITIAL_U_SCALE 1.0e-4f
+#define GRADIENT_THRESHOLD 1.0e-16f
+#define RELATIVE_STEP_THRESHOLD 1.0e-7f
+#define FROM_MICRO_SEC_TO_SEC 1.0e-6f
+
+void magCalSphereReset(struct MagCalSphere *mocs) {
+ mocs->number_of_data_samples = 0;
+ mocs->sample_counter = 0;
+ memset(&mocs->sphere_data, 0, sizeof(mocs->sphere_data));
+}
+
+void initMagCalSphere(struct MagCalSphere *mocs, float x_bias, float y_bias,
+ float z_bias, float c00, float c01, float c02, float c10,
+ float c11, float c12, float c20, float c21, float c22,
+ uint32_t min_batch_window_in_micros,
+ size_t min_num_diverse_vectors,
+ size_t max_num_max_distance, float var_threshold,
+ float max_min_threshold, float local_field,
+ float threshold_tuning_param,
+ float max_distance_tuning_param) {
+ initMagCal(&mocs->moc, x_bias, y_bias, z_bias, c00, c01, c02, c10, c11, c12,
+ c20, c21, c22, min_batch_window_in_micros, min_num_diverse_vectors,
+ max_num_max_distance, var_threshold, max_min_threshold,
+ local_field, threshold_tuning_param, max_distance_tuning_param);
+ mocs->inv_data_size = 1.0f / (float)NUM_SPHERE_FIT_DATA;
+ mocs->batch_time_in_sec =
+ (float)(min_batch_window_in_micros) * FROM_MICRO_SEC_TO_SEC;
+ // Initialize to take every sample, default setting.
+ mocs->sample_drop = 0;
+ magCalSphereReset(mocs);
+
+ // Setting lm params.
+ mocs->sphere_fit.params.max_iterations = MAX_ITERATIONS;
+ mocs->sphere_fit.params.initial_u_scale = INITIAL_U_SCALE;
+ mocs->sphere_fit.params.gradient_threshold = GRADIENT_THRESHOLD;
+ mocs->sphere_fit.params.relative_step_threshold = RELATIVE_STEP_THRESHOLD;
+ sphereFitInit(&mocs->sphere_fit.sphere_cal, &mocs->sphere_fit.params,
+ MIN_NUM_SPHERE_FIT_POINTS);
+ sphereFitSetSolverData(&mocs->sphere_fit.sphere_cal,
+ &mocs->sphere_fit.lm_data);
+ calDataReset(&mocs->sphere_fit.sphere_param);
+}
+
+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.
+ mocs->sample_drop =
+ floor(odr_in_hz * mocs->batch_time_in_sec * mocs->inv_data_size);
+}
+
+// Updates the sphere fit data set, by calculating the numbers
+// of samples to be dropped, based on odr_in_hz, to fill up the available memory
+// in the given batch size window.
+void magCalSphereDataUpdate(struct MagCalSphere *mocs, float x, float y,
+ float z) {
+ // build a vector.
+ const float vec[3] = {x, y, z};
+
+ // sample_counter for the down sampling.
+ mocs->sample_counter++;
+
+ // checking if sample_count >= sample_drop, if yes we store the mag sample in
+ // the data set.
+ if (mocs->sample_counter >= mocs->sample_drop) {
+ if (mocs->number_of_data_samples < NUM_SPHERE_FIT_DATA) {
+ memcpy(&mocs->sphere_data[mocs->number_of_data_samples *
+ THREE_AXIS_DATA_DIM],
+ vec, sizeof(float) * 3);
+ // counting the numbers of samples in the data set.
+ mocs->number_of_data_samples++;
+ }
+ // resetting the sample_counter.
+ mocs->sample_counter = 0;
+ }
+}
+
+// Runs the Sphere Fit.
+enum MagUpdate magCalSphereFit(struct MagCalSphere *mocs,
+ uint64_t sample_time_us) {
+ // Setting up sphere fit data.
+ struct SphereFitData data = {&mocs->sphere_data[0], NULL,
+ mocs->number_of_data_samples, mocs->moc.radius};
+ float initial_bias[3] = {mocs->moc.x_bias, mocs->moc.y_bias,
+ mocs->moc.z_bias};
+
+ // Setting initial bias values based on the KASA fit.
+ sphereFitSetInitialBias(&mocs->sphere_fit.sphere_cal, initial_bias);
+
+ // Running the sphere fit and checking if successful.
+ if (sphereFitRunCal(&mocs->sphere_fit.sphere_cal, &data, sample_time_us)) {
+ // Updating Sphere parameters. Can use "calDataCorrectData" function to
+ // correct data.
+ sphereFitGetLatestCal(&mocs->sphere_fit.sphere_cal,
+ &mocs->sphere_fit.sphere_param);
+
+ // Updating that a full sphere fit is available.
+ return UPDATE_SPHERE_FIT;
+ }
+ return NO_UPDATE;
+}
+
+enum MagUpdate magCalSphereUpdate(struct MagCalSphere *mocs,
+ uint64_t sample_time_us, float x, float y,
+ float z) {
+ enum MagUpdate new_cal = NO_UPDATE;
+
+ // Saving data for sphere fit.
+ magCalSphereDataUpdate(mocs, x, y, z);
+
+ // Checking if KASA found a bias, if yes can run the sphere fit.
+ if (UPDATE_BIAS == magCalUpdate(&mocs->moc, sample_time_us, x, y, z)) {
+ // Running the sphere fit algo.
+ new_cal = magCalSphereFit(mocs, sample_time_us);
+
+ // Resetting.
+ sphereFitReset(&mocs->sphere_fit.sphere_cal);
+ magCalSphereReset(mocs);
+
+ // If moc.kasa_batching is false, ran into a time out, hence the sphere
+ // algo has to be reset as well.
+ } else if (!mocs->moc.kasa_batching) {
+ magCalSphereReset(mocs);
+ }
+
+ // Return which update has happened.
+ return new_cal;
+}
diff --git a/firmware/os/algos/calibration/magnetometer/mag_sphere_fit.h b/firmware/os/algos/calibration/magnetometer/mag_sphere_fit.h
new file mode 100644
index 0000000..85df48f
--- /dev/null
+++ b/firmware/os/algos/calibration/magnetometer/mag_sphere_fit.h
@@ -0,0 +1,78 @@
+/*
+ * 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.
+ */
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_SPHERE_FIT_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_SPHERE_FIT_H_
+
+#include "calibration/common/sphere_fit_calibration.h"
+#include "calibration/magnetometer/mag_cal.h"
+
+#define NUM_SPHERE_FIT_DATA 50
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct SphereFit {
+ struct LmParams params;
+ struct LmData lm_data;
+ struct SphereFitCal sphere_cal;
+ struct ThreeAxisCalData sphere_param;
+};
+
+struct MagCalSphere {
+ // KASA Fit Struct.
+ struct MagCal moc;
+
+ // Sphere fit Struct.
+ struct SphereFit sphere_fit;
+
+ // down sampler control.
+ uint32_t number_of_data_samples;
+ uint32_t sample_counter;
+ uint32_t sample_drop;
+ float inv_data_size;
+ float batch_time_in_sec;
+
+ // Sphere fit data set.
+ float sphere_data[THREE_AXIS_DATA_DIM * NUM_SPHERE_FIT_DATA];
+};
+
+void initMagCalSphere(struct MagCalSphere *mocs,
+ float x_bias, float y_bias, float z_bias,
+ float c00, float c01, float c02, float c10, float c11,
+ float c12, float c20, float c21, float c22,
+ uint32_t min_batch_window_in_micros,
+ size_t min_num_diverse_vectors,
+ size_t max_num_max_distance,
+ float var_threshold,
+ float max_min_threshold,
+ float local_field,
+ float threshold_tuning_param,
+ float max_distance_tuning_param);
+
+void magCalSphereDestroy(struct MagCalSphere *mocs);
+
+enum MagUpdate magCalSphereUpdate(struct MagCalSphere *mocs,
+ uint64_t sample_time_us,
+ float x, float y, float z);
+
+void magCalSphereOdrUpdate(struct MagCalSphere *mocs, float odr_in_hz);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_SPHERE_FIT_H_
