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