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