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
19 extern "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/smgr_client.h"
31 #include "chre_api/chre/sensor.h"
32
33 using chre::getSmrHelper;
34 using chre::getSensorServiceSmrClientHandle;
35 using chre::MakeUnique;
36 using chre::MakeUniqueZeroFill;
37 using chre::memoryAlloc;
38 using chre::memoryFree;
39 using chre::UniquePtr;
40
41 namespace {
42
43 //! The constant to convert magnetometer readings from uT in Android to Gauss
44 //! in SMGR.
45 constexpr 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 */
isCalibrationSupported(uint8_t sensorType)51 bool 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 */
getSensorId(uint8_t sensorType)66 uint8_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 */
populateCalRequest(uint8_t sensorType,const ashCalInfo * calInfo,sns_smgr_sensor_cal_req_msg_v01 * calRequest)86 void 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.0f);
111 calRequest->ScaleFactor[1] = FX_FLTTOFIX_Q16(1.0f);
112 calRequest->ScaleFactor[2] = FX_FLTTOFIX_Q16(1.0f);
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
ashSetCalibration(uint8_t sensorType,const struct ashCalInfo * calInfo)133 DLL_EXPORT bool ashSetCalibration(uint8_t sensorType,
134 const struct ashCalInfo *calInfo) {
135 bool success = false;
136 if (!isCalibrationSupported(sensorType)) {
137 LOGE("Attempting to set calibration of sensor %" PRIu8, sensorType);
138 } else {
139 // Allocate request and response for sensor calibraton.
140 auto calRequest = MakeUniqueZeroFill<sns_smgr_sensor_cal_req_msg_v01>();
141 auto calResponse = MakeUnique<sns_smgr_sensor_cal_resp_msg_v01>();
142 if (calRequest.isNull() || calResponse.isNull()) {
143 LOGE("Failed to allocated sensor cal memory");
144 } else {
145 populateCalRequest(sensorType, calInfo, calRequest.get());
146
147 smr_err status = getSmrHelper()->sendReqSync(
148 getSensorServiceSmrClientHandle(), SNS_SMGR_CAL_REQ_V01,
149 &calRequest, &calResponse);
150
151 if (status != SMR_NO_ERR) {
152 LOGE("Error setting sensor calibration: status %d", status);
153 } else if (calResponse->Resp.sns_result_t != SNS_RESULT_SUCCESS_V01) {
154 LOGE("Setting sensor calibration failed with error: %" PRIu8,
155 calResponse->Resp.sns_err_t);
156 } else {
157 success = true;
158 }
159 }
160 }
161 return success;
162 }
163