1 /*
2 * Copyright (C) 2017 The Android Open Source Project
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16
17 #include "calibration/magnetometer/mag_sphere_fit_cal/mag_sphere_fit.h"
18
19 #include <errno.h>
20 #include <string.h>
21
22 #define MAX_ITERATIONS 30
23 #define INITIAL_U_SCALE 1.0e-4f
24 #define GRADIENT_THRESHOLD 1.0e-16f
25 #define RELATIVE_STEP_THRESHOLD 1.0e-7f
26 #define FROM_MICRO_SEC_TO_SEC 1.0e-6f
27
magCalSphereReset(struct MagCalSphere * mocs)28 void magCalSphereReset(struct MagCalSphere *mocs) {
29 mocs->number_of_data_samples = 0;
30 mocs->sample_counter = 0;
31 memset(&mocs->sphere_data, 0, sizeof(mocs->sphere_data));
32 }
33
initMagCalSphere(struct MagCalSphere * mocs,const struct MagCalParameters * mag_cal_parameters,const struct DiversityCheckerParameters * diverse_parameters,float default_odr_in_hz)34 void initMagCalSphere(
35 struct MagCalSphere *mocs,
36 const struct MagCalParameters *mag_cal_parameters,
37 const struct DiversityCheckerParameters *diverse_parameters,
38 float default_odr_in_hz) {
39 initMagCal(&mocs->moc, mag_cal_parameters, diverse_parameters);
40 mocs->inv_data_size = 1.0f / (float)NUM_SPHERE_FIT_DATA;
41 mocs->batch_time_in_sec =
42 (float)(mag_cal_parameters->min_batch_window_in_micros) *
43 FROM_MICRO_SEC_TO_SEC;
44 // Initialize to take every sample, default setting.
45 mocs->sample_drop = 0;
46 magCalSphereReset(mocs);
47
48 // Setting lm params.
49 mocs->sphere_fit.params.max_iterations = MAX_ITERATIONS;
50 mocs->sphere_fit.params.initial_u_scale = INITIAL_U_SCALE;
51 mocs->sphere_fit.params.gradient_threshold = GRADIENT_THRESHOLD;
52 mocs->sphere_fit.params.relative_step_threshold = RELATIVE_STEP_THRESHOLD;
53 sphereFitInit(&mocs->sphere_fit.sphere_cal, &mocs->sphere_fit.params,
54 MIN_NUM_SPHERE_FIT_POINTS);
55 sphereFitSetSolverData(&mocs->sphere_fit.sphere_cal,
56 &mocs->sphere_fit.lm_data);
57 calDataReset(&mocs->sphere_fit.sphere_param);
58
59 // Initializes the starting output data rate estimate.
60 magCalSphereOdrUpdate(mocs, default_odr_in_hz);
61 }
62
magCalSphereOdrUpdate(struct MagCalSphere * mocs,float odr_in_hz)63 void magCalSphereOdrUpdate(struct MagCalSphere *mocs, float odr_in_hz) {
64 // Calculate the numbers of samples to be dropped, in order to fill up
65 // the data set.
66 float sample_drop = odr_in_hz * mocs->batch_time_in_sec * mocs->inv_data_size;
67 mocs->sample_drop = (uint32_t)floorf(sample_drop);
68 }
69
70 // Updates the sphere fit data set, by calculating the numbers
71 // of samples to be dropped, based on odr_in_hz, to fill up the available memory
72 // in the given batch size window.
magCalSphereDataUpdate(struct MagCalSphere * mocs,float x,float y,float z)73 void magCalSphereDataUpdate(struct MagCalSphere *mocs, float x, float y,
74 float z) {
75 // build a vector.
76 const float vec[3] = {x, y, z};
77
78 // sample_counter for the down sampling.
79 mocs->sample_counter++;
80
81 // checking if sample_count >= sample_drop, if yes we store the mag sample in
82 // the data set.
83 if (mocs->sample_counter >= mocs->sample_drop) {
84 if (mocs->number_of_data_samples < NUM_SPHERE_FIT_DATA) {
85 memcpy(&mocs->sphere_data[mocs->number_of_data_samples *
86 THREE_AXIS_DATA_DIM],
87 vec, sizeof(float) * THREE_AXIS_DATA_DIM);
88 // counting the numbers of samples in the data set.
89 mocs->number_of_data_samples++;
90 }
91 // resetting the sample_counter.
92 mocs->sample_counter = 0;
93 }
94 }
95
96 // Runs the Sphere Fit.
magCalSphereFit(struct MagCalSphere * mocs,uint64_t sample_time_us)97 enum MagUpdate magCalSphereFit(struct MagCalSphere *mocs,
98 uint64_t sample_time_us) {
99 // Setting up sphere fit data.
100 struct SphereFitData data = {&mocs->sphere_data[0], NULL,
101 mocs->number_of_data_samples, mocs->moc.radius};
102 float initial_bias[3] = {mocs->moc.x_bias, mocs->moc.y_bias,
103 mocs->moc.z_bias};
104
105 // Setting initial bias values based on the KASA fit.
106 sphereFitSetInitialBias(&mocs->sphere_fit.sphere_cal, initial_bias);
107
108 // Running the sphere fit and checking if successful.
109 if (sphereFitRunCal(&mocs->sphere_fit.sphere_cal, &data, sample_time_us)) {
110 // Updating sphere parameters.
111 sphereFitGetLatestCal(&mocs->sphere_fit.sphere_cal,
112 &mocs->sphere_fit.sphere_param);
113
114 // Updating that a full sphere fit is available.
115 return UPDATE_SPHERE_FIT;
116 }
117 return NO_UPDATE;
118 }
119
magCalSphereUpdate(struct MagCalSphere * mocs,uint64_t sample_time_us,float x,float y,float z)120 enum MagUpdate magCalSphereUpdate(struct MagCalSphere *mocs,
121 uint64_t sample_time_us, float x, float y,
122 float z) {
123 enum MagUpdate new_cal = NO_UPDATE;
124
125 // Saving data for sphere fit.
126 magCalSphereDataUpdate(mocs, x, y, z);
127
128 // Checking if KASA found a bias, if yes can run the sphere fit.
129 if (UPDATE_BIAS == magCalUpdate(&mocs->moc, sample_time_us, x, y, z)) {
130 // Running the sphere fit algo.
131 new_cal = magCalSphereFit(mocs, sample_time_us);
132
133 // Resetting.
134 sphereFitReset(&mocs->sphere_fit.sphere_cal);
135 magCalSphereReset(mocs);
136
137 // If moc.kasa_batching is false, ran into a time out, hence the sphere
138 // algo has to be reset as well.
139 } else if (!mocs->moc.kasa_batching) {
140 magCalSphereReset(mocs);
141 }
142
143 // Return which update has happened.
144 return new_cal;
145 }
146