1 /* 2 * Copyright (C) 2018 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/online_calibration/magnetometer/mag_diverse_cal/mag_diverse_cal.h" 18 19 #include "calibration/util/cal_log.h" 20 21 namespace online_calibration { 22 23 // Empirically estimated upper bounds on offset error. 24 constexpr float MagDiverseCal::kLowQualityUt; 25 constexpr float MagDiverseCal::kHighQualityUt; 26 27 void MagDiverseCal::Initialize( 28 const MagCalParameters& mag_cal_parameters, 29 const DiversityCheckerParameters& diversity_parameters) { 30 initMagCal(&mag_cal_, &mag_cal_parameters, &diversity_parameters); 31 InitializeCalData(); 32 } 33 34 CalibrationTypeFlags MagDiverseCal::SetMeasurement(const SensorData& sample) { 35 // Routes the input sensor sample to the calibration algorithm. 36 MagUpdate new_calibration_update = MagUpdate::NO_UPDATE; 37 switch (sample.type) { 38 case SensorType::kMagnetometerUt: 39 new_calibration_update = magCalUpdate( 40 &mag_cal_, NanoToMicroseconds(sample.timestamp_nanos), 41 sample.data[SensorIndex::kXAxis], sample.data[SensorIndex::kYAxis], 42 sample.data[SensorIndex::kZAxis]); 43 break; 44 45 case SensorType::kTemperatureCelsius: 46 temperature_celsius_ = sample.data[SensorIndex::kSingleAxis]; 47 break; 48 49 default: 50 // This sample is not required. 51 return cal_update_polling_flags_; 52 } 53 54 // Checks for a new offset estimate, and updates the calibration data. 55 if (MagUpdate::UPDATE_BIAS & new_calibration_update) { 56 magCalGetBias(&mag_cal_, &cal_data_.offset[0], &cal_data_.offset[1], 57 &cal_data_.offset[2]); 58 59 cal_data_.calibration_quality.level = CalibrationQualityLevel::HIGH_QUALITY; 60 cal_data_.calibration_quality.value = kHighQualityUt; 61 cal_data_.offset_temp_celsius = temperature_celsius_; 62 cal_data_.cal_update_time_nanos = sample.timestamp_nanos; 63 cal_update_polling_flags_ = CalibrationTypeFlags::BIAS; 64 OnNotifyCalibrationUpdate(CalibrationTypeFlags::BIAS); 65 } 66 67 return cal_update_polling_flags_; 68 } 69 70 bool MagDiverseCal::SetInitialCalibration( 71 const CalibrationDataThreeAxis& input_cal_data) { 72 // Checks that the input calibration type matches the algorithm type. 73 if (input_cal_data.type != get_sensor_type()) { 74 CAL_DEBUG_LOG("[MagDiverseCal]", 75 "SetInitialCalibration failed due to wrong sensor type."); 76 return false; 77 } 78 79 // Sets the magnetometer algorithm's calibration data. 80 magCalReset(&mag_cal_); // Resets the magnetometer's offset vector. 81 magCalAddBias(&mag_cal_, input_cal_data.offset[0], input_cal_data.offset[1], 82 input_cal_data.offset[2]); 83 84 // Sync's all initial calibration data. 85 cal_data_ = input_cal_data; 86 87 // Sets the calibration quality to undetermined (uncertain magnetic history 88 // makes the usefulness of the input calibration value unknown). 89 cal_data_.calibration_quality.reset(); 90 91 return true; 92 } 93 94 } // namespace online_calibration 95
