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 <cinttypes> 18 19extern "C" { 20 21#include "fixed_point.h" 22#include "sns_smgr_api_v01.h" 23 24} // extern "C" 25 26#include "ash_api/ash.h" 27#include "chre/platform/assert.h" 28#include "chre/platform/log.h" 29#include "chre/platform/memory.h" 30#include "chre/platform/slpi/smgr_client.h" 31#include "chre_api/chre/sensor.h" 32 33using chre::getSmrHelper; 34using chre::getSensorServiceSmrClientHandle; 35using chre::MakeUnique; 36using chre::MakeUniqueZeroFill; 37using chre::memoryAlloc; 38using chre::memoryFree; 39using chre::UniquePtr; 40 41namespace { 42 43//! The constant to convert magnetometer readings from uT in Android to Gauss 44//! in SMGR. 45constexpr float kGaussPerMicroTesla = 0.01f; 46 47/** 48 * @param sensorType One of the CHRE_SENSOR_TYPE_* constants. 49 * @return true if runtime sensor calibration is supported on this platform. 50 */ 51bool isCalibrationSupported(uint8_t sensorType) { 52 switch (sensorType) { 53 case CHRE_SENSOR_TYPE_ACCELEROMETER: 54 case CHRE_SENSOR_TYPE_GYROSCOPE: 55 case CHRE_SENSOR_TYPE_GEOMAGNETIC_FIELD: 56 return true; 57 default: 58 return false; 59 } 60} 61 62/** 63 * @param sensorType One of the CHRE_SENSOR_TYPE_* constants. 64 * @return The sensor ID of the sensor type as defined in the SMGR API. 65 */ 66uint8_t getSensorId(uint8_t sensorType) { 67 switch (sensorType) { 68 case CHRE_SENSOR_TYPE_ACCELEROMETER: 69 return SNS_SMGR_ID_ACCEL_V01; 70 case CHRE_SENSOR_TYPE_GYROSCOPE: 71 return SNS_SMGR_ID_GYRO_V01; 72 case CHRE_SENSOR_TYPE_GEOMAGNETIC_FIELD: 73 return SNS_SMGR_ID_MAG_V01; 74 default: 75 return 0; 76 } 77} 78 79/** 80 * Populates the calibration request mesasge. 81 * 82 * @param sensorType One of the CHRE_SENSOR_TYPE_* constants. 83 * @param calInfo The sensor calibraion info supplied by the user. 84 * @param calRequest The SMGR cal request message to be populated. 85 */ 86void populateCalRequest(uint8_t sensorType, const ashCalInfo *calInfo, 87 sns_smgr_sensor_cal_req_msg_v01 *calRequest) { 88 CHRE_ASSERT(calInfo); 89 CHRE_ASSERT(calRequest); 90 91 calRequest->usage = SNS_SMGR_CAL_DYNAMIC_V01; 92 calRequest->SensorId = getSensorId(sensorType); 93 calRequest->DataType = SNS_SMGR_DATA_TYPE_PRIMARY_V01; 94 95 // Convert from micro Tesla to Gauss for magnetometer bias 96 float scaling = 1.0f; 97 if (sensorType == CHRE_SENSOR_TYPE_GEOMAGNETIC_FIELD) { 98 scaling = kGaussPerMicroTesla; 99 } 100 101 // Convert from Android to SMGR's NED coordinate and invert the sign as SMGR 102 // defines Sc = CM * (Su + Bias) in sns_rh_calibrate_cm_and_bias(). 103 calRequest->ZeroBias_len = 3; 104 calRequest->ZeroBias[0] = FX_FLTTOFIX_Q16(-calInfo->bias[1] * scaling); 105 calRequest->ZeroBias[1] = FX_FLTTOFIX_Q16(-calInfo->bias[0] * scaling); 106 calRequest->ZeroBias[2] = FX_FLTTOFIX_Q16(calInfo->bias[2] * scaling); 107 108 // ScaleFactor will be over-written by compensation matrix. 109 calRequest->ScaleFactor_len = 3; 110 calRequest->ScaleFactor[0] = FX_FLTTOFIX_Q16(1.0); 111 calRequest->ScaleFactor[1] = FX_FLTTOFIX_Q16(1.0); 112 calRequest->ScaleFactor[2] = FX_FLTTOFIX_Q16(1.0); 113 114 // Convert from Android to SMGR's NED coordinate. 115 calRequest->CompensationMatrix_valid = true; 116 calRequest->CompensationMatrix_len = 9; 117 calRequest->CompensationMatrix[0] = FX_FLTTOFIX_Q16(calInfo->compMatrix[4]); 118 calRequest->CompensationMatrix[1] = FX_FLTTOFIX_Q16(calInfo->compMatrix[3]); 119 calRequest->CompensationMatrix[2] = FX_FLTTOFIX_Q16(-calInfo->compMatrix[5]); 120 calRequest->CompensationMatrix[3] = FX_FLTTOFIX_Q16(calInfo->compMatrix[1]); 121 calRequest->CompensationMatrix[4] = FX_FLTTOFIX_Q16(calInfo->compMatrix[0]); 122 calRequest->CompensationMatrix[5] = FX_FLTTOFIX_Q16(-calInfo->compMatrix[2]); 123 calRequest->CompensationMatrix[6] = FX_FLTTOFIX_Q16(-calInfo->compMatrix[7]); 124 calRequest->CompensationMatrix[7] = FX_FLTTOFIX_Q16(-calInfo->compMatrix[6]); 125 calRequest->CompensationMatrix[8] = FX_FLTTOFIX_Q16(calInfo->compMatrix[8]); 126 127 calRequest->CalibrationAccuracy_valid = true; 128 calRequest->CalibrationAccuracy = calInfo->accuracy; 129} 130 131} // namespace 132 133bool ashSetCalibration(uint8_t sensorType, const struct ashCalInfo *calInfo) { 134 bool success = false; 135 if (!isCalibrationSupported(sensorType)) { 136 LOGE("Attempting to set calibration of sensor %" PRIu8, sensorType); 137 } else { 138 // Allocate request and response for sensor calibraton. 139 auto calRequest = MakeUniqueZeroFill<sns_smgr_sensor_cal_req_msg_v01>(); 140 auto calResponse = MakeUnique<sns_smgr_sensor_cal_resp_msg_v01>(); 141 if (calRequest.isNull() || calResponse.isNull()) { 142 LOGE("Failed to allocated sensor cal memory"); 143 } else { 144 populateCalRequest(sensorType, calInfo, calRequest.get()); 145 146 smr_err status = getSmrHelper()->sendReqSync( 147 getSensorServiceSmrClientHandle(), SNS_SMGR_CAL_REQ_V01, 148 &calRequest, &calResponse); 149 150 if (status != SMR_NO_ERR) { 151 LOGE("Error setting sensor calibration: status %d", status); 152 } else if (calResponse->Resp.sns_result_t != SNS_RESULT_SUCCESS_V01) { 153 LOGE("Setting sensor calibration failed with error: %" PRIu8, 154 calResponse->Resp.sns_err_t); 155 } else { 156 success = true; 157 } 158 } 159 } 160 return success; 161} 162