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 "chre/platform/slpi/see/see_cal_helper.h"
18 
19 #include "chre/core/sensor_type_helpers.h"
20 #include "chre/platform/assert.h"
21 #include "chre/platform/log.h"
22 #include "chre/platform/slpi/see/see_helper.h"
23 #include "chre/util/lock_guard.h"
24 #include "chre/util/macros.h"
25 
26 namespace chre {
27 
28 void SeeCalHelper::applyCalibration(uint8_t sensorType, const float input[3],
29                                     float output[3]) const {
30   bool applied = false;
31   size_t index = getCalIndexFromSensorType(sensorType);
32   if (index < ARRAY_SIZE(mCalInfo)) {
33     LockGuard<Mutex> lock(mMutex);
34 
35     // TODO: Support compensation matrix and scaling factor calibration
36     if (mCalInfo[index].cal.hasBias) {
37       for (size_t i = 0; i < 3; i++) {
38         output[i] = input[i] - mCalInfo[index].cal.bias[i];
39       }
40       applied = true;
41     }
42   }
43 
44   if (!applied) {
45     for (size_t i = 0; i < 3; i++) {
46       output[i] = input[i];
47     }
48   }
49 }
50 
51 bool SeeCalHelper::getBias(uint8_t sensorType,
52                            struct chreSensorThreeAxisData *biasData) const {
53   CHRE_ASSERT(biasData != nullptr);
54 
55   bool success = false;
56   if (biasData != nullptr) {
57     size_t index = getCalIndexFromSensorType(sensorType);
58     if (index < ARRAY_SIZE(mCalInfo)) {
59       LockGuard<Mutex> lock(mMutex);
60 
61       if (mCalInfo[index].cal.hasBias) {
62         biasData->header.baseTimestamp = mCalInfo[index].cal.timestamp;
63         biasData->header.readingCount = 1;
64         biasData->header.accuracy = mCalInfo[index].cal.accuracy;
65         biasData->header.reserved = 0;
66 
67         for (size_t i = 0; i < 3; i++) {
68           biasData->readings[0].bias[i] = mCalInfo[index].cal.bias[i];
69         }
70         biasData->readings[0].timestampDelta = 0;
71         success = true;
72       }
73     }
74   }
75 
76   return success;
77 }
78 
79 bool SeeCalHelper::areCalUpdatesEnabled(const sns_std_suid &suid) const {
80   size_t index = getCalIndexFromSuid(suid);
81   if (index < ARRAY_SIZE(mCalInfo)) {
82     return mCalInfo[index].enabled;
83   }
84   return false;
85 }
86 
87 bool SeeCalHelper::configureCalUpdates(const sns_std_suid &suid, bool enable,
88                                        SeeHelper &helper) {
89   bool success = false;
90 
91   size_t index = getCalIndexFromSuid(suid);
92   if (index >= ARRAY_SIZE(mCalInfo)) {
93     CHRE_ASSERT(false);
94   } else if ((mCalInfo[index].enabled == enable) ||
95              helper.configureOnChangeSensor(suid, enable)) {
96     success = true;
97     mCalInfo[index].enabled = enable;
98   }
99 
100   return success;
101 }
102 
103 const sns_std_suid *SeeCalHelper::getCalSuidFromSensorType(
104     uint8_t sensorType) const {
105   // Mutex not needed, SUID is not modified after init
106   size_t calIndex = getCalIndexFromSensorType(sensorType);
107   if (calIndex < ARRAY_SIZE(mCalInfo) && mCalInfo[calIndex].suid.has_value()) {
108     return &mCalInfo[calIndex].suid.value();
109   }
110   return nullptr;
111 }
112 
113 bool SeeCalHelper::registerForCalibrationUpdates(SeeHelper &seeHelper) {
114   bool success = true;
115 
116   // Find the cal sensor's SUID, assign it to mCalInfo, and make cal sensor data
117   // request.
118   DynamicVector<sns_std_suid> suids;
119   for (size_t i = 0; i < ARRAY_SIZE(mCalInfo); i++) {
120     const char *calType = getDataTypeForCalSensorIndex(i);
121     if (!seeHelper.findSuidSync(calType, &suids)) {
122       success = false;
123       LOGE("Failed to find sensor '%s'", calType);
124     } else {
125       mCalInfo[i].suid = suids[0];
126 
127 #ifndef CHRE_SLPI_DEFAULT_BUILD
128       if (!seeHelper.configureOnChangeSensor(suids[0], true /* enable */)) {
129         success = false;
130         LOGE("Failed to request '%s' data", calType);
131       }
132 #endif
133     }
134   }
135 
136   return success;
137 }
138 
139 void SeeCalHelper::updateCalibration(const sns_std_suid &suid, bool hasBias,
140                                      float bias[3], bool hasScale,
141                                      float scale[3], bool hasMatrix,
142                                      float matrix[9], uint8_t accuracy,
143                                      uint64_t timestamp) {
144   size_t index = getCalIndexFromSuid(suid);
145   if (index < ARRAY_SIZE(mCalInfo)) {
146     LockGuard<Mutex> lock(mMutex);
147     SeeCalData &calData = mCalInfo[index].cal;
148 
149     calData.hasBias = hasBias;
150     if (hasBias) {
151       memcpy(calData.bias, bias, sizeof(calData.bias));
152     }
153 
154     calData.hasScale = hasScale;
155     if (hasScale) {
156       memcpy(calData.scale, scale, sizeof(calData.scale));
157     }
158 
159     calData.hasMatrix = hasMatrix;
160     if (hasMatrix) {
161       memcpy(calData.matrix, matrix, sizeof(calData.matrix));
162     }
163 
164     calData.accuracy = accuracy;
165     calData.timestamp = timestamp;
166   }
167 }
168 
169 bool SeeCalHelper::getSensorTypeFromSuid(const sns_std_suid &suid,
170                                          uint8_t *sensorType) const {
171   size_t calSensorIndex = getCalIndexFromSuid(suid);
172   bool found = true;
173   switch (static_cast<SeeCalSensor>(calSensorIndex)) {
174 #ifdef CHRE_ENABLE_ACCEL_CAL
175     case SeeCalSensor::AccelCal:
176       *sensorType = CHRE_SENSOR_TYPE_ACCELEROMETER;
177       break;
178 #endif  // CHRE_ENABLE_ACCEL_CAL
179     case SeeCalSensor::GyroCal:
180       *sensorType = CHRE_SENSOR_TYPE_GYROSCOPE;
181       break;
182     case SeeCalSensor::MagCal:
183       *sensorType = CHRE_SENSOR_TYPE_GEOMAGNETIC_FIELD;
184       break;
185     default:
186       // Don't assert here as SEE may send us calibration updates for other
187       // sensors even if CHRE doesn't request them.
188       found = false;
189       break;
190   }
191   return found;
192 }
193 
194 size_t SeeCalHelper::getCalIndexFromSensorType(uint8_t sensorType) {
195   SeeCalSensor index;
196   switch (sensorType) {
197 #ifdef CHRE_ENABLE_ACCEL_CAL
198     case CHRE_SENSOR_TYPE_ACCELEROMETER:
199       index = SeeCalSensor::AccelCal;
200       break;
201 #endif  // CHRE_ENABLE_ACCEL_CAL
202     case CHRE_SENSOR_TYPE_GYROSCOPE:
203       index = SeeCalSensor::GyroCal;
204       break;
205     case CHRE_SENSOR_TYPE_GEOMAGNETIC_FIELD:
206       index = SeeCalSensor::MagCal;
207       break;
208     default:
209       index = SeeCalSensor::NumCalSensors;
210   }
211   return static_cast<size_t>(index);
212 }
213 
214 const char *SeeCalHelper::getDataTypeForCalSensorIndex(size_t calSensorIndex) {
215   switch (static_cast<SeeCalSensor>(calSensorIndex)) {
216 #ifdef CHRE_ENABLE_ACCEL_CAL
217     case SeeCalSensor::AccelCal:
218       return "accel_cal";
219 #endif  // CHRE_ENABLE_ACCEL_CAL
220     case SeeCalSensor::GyroCal:
221       return "gyro_cal";
222     case SeeCalSensor::MagCal:
223       return "mag_cal";
224     default:
225       CHRE_ASSERT(false);
226   }
227   return nullptr;
228 }
229 
230 size_t SeeCalHelper::getCalIndexFromSuid(const sns_std_suid &suid) const {
231   size_t i = 0;
232   for (; i < ARRAY_SIZE(mCalInfo); i++) {
233     if (mCalInfo[i].suid.has_value() &&
234         suidsMatch(suid, mCalInfo[i].suid.value())) {
235       break;
236     }
237   }
238   return i;
239 }
240 
241 }  // namespace chre
242