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 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 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 63 void magCalSphereDestroy(struct MagCalSphere *mocs) { (void)mocs; } 64 65 void magCalSphereOdrUpdate(struct MagCalSphere *mocs, float odr_in_hz) { 66 // Calculate the numbers of samples to be dropped, in order to fill up 67 // the data set. 68 float sample_drop = odr_in_hz * mocs->batch_time_in_sec * mocs->inv_data_size; 69 mocs->sample_drop = (uint32_t)floorf(sample_drop); 70 } 71 72 // Updates the sphere fit data set, by calculating the numbers 73 // of samples to be dropped, based on odr_in_hz, to fill up the available memory 74 // in the given batch size window. 75 void magCalSphereDataUpdate(struct MagCalSphere *mocs, float x, float y, 76 float z) { 77 // build a vector. 78 const float vec[3] = {x, y, z}; 79 80 // sample_counter for the down sampling. 81 mocs->sample_counter++; 82 83 // checking if sample_count >= sample_drop, if yes we store the mag sample in 84 // the data set. 85 if (mocs->sample_counter >= mocs->sample_drop) { 86 if (mocs->number_of_data_samples < NUM_SPHERE_FIT_DATA) { 87 memcpy(&mocs->sphere_data[mocs->number_of_data_samples * 88 THREE_AXIS_DATA_DIM], 89 vec, sizeof(float) * THREE_AXIS_DATA_DIM); 90 // counting the numbers of samples in the data set. 91 mocs->number_of_data_samples++; 92 } 93 // resetting the sample_counter. 94 mocs->sample_counter = 0; 95 } 96 } 97 98 // Runs the Sphere Fit. 99 enum MagUpdate magCalSphereFit(struct MagCalSphere *mocs, 100 uint64_t sample_time_us) { 101 // Setting up sphere fit data. 102 struct SphereFitData data = {&mocs->sphere_data[0], NULL, 103 mocs->number_of_data_samples, mocs->moc.radius}; 104 float initial_bias[3] = {mocs->moc.x_bias, mocs->moc.y_bias, 105 mocs->moc.z_bias}; 106 107 // Setting initial bias values based on the KASA fit. 108 sphereFitSetInitialBias(&mocs->sphere_fit.sphere_cal, initial_bias); 109 110 // Running the sphere fit and checking if successful. 111 if (sphereFitRunCal(&mocs->sphere_fit.sphere_cal, &data, sample_time_us)) { 112 // Updating sphere parameters. 113 sphereFitGetLatestCal(&mocs->sphere_fit.sphere_cal, 114 &mocs->sphere_fit.sphere_param); 115 116 // Updating that a full sphere fit is available. 117 return UPDATE_SPHERE_FIT; 118 } 119 return NO_UPDATE; 120 } 121 122 enum MagUpdate magCalSphereUpdate(struct MagCalSphere *mocs, 123 uint64_t sample_time_us, float x, float y, 124 float z) { 125 enum MagUpdate new_cal = NO_UPDATE; 126 127 // Saving data for sphere fit. 128 magCalSphereDataUpdate(mocs, x, y, z); 129 130 // Checking if KASA found a bias, if yes can run the sphere fit. 131 if (UPDATE_BIAS == magCalUpdate(&mocs->moc, sample_time_us, x, y, z)) { 132 // Running the sphere fit algo. 133 new_cal = magCalSphereFit(mocs, sample_time_us); 134 135 // Resetting. 136 sphereFitReset(&mocs->sphere_fit.sphere_cal); 137 magCalSphereReset(mocs); 138 139 // If moc.kasa_batching is false, ran into a time out, hence the sphere 140 // algo has to be reset as well. 141 } else if (!mocs->moc.kasa_batching) { 142 magCalSphereReset(mocs); 143 } 144 145 // Return which update has happened. 146 return new_cal; 147 } 148
