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 #include <chre.h>
20 #include "chre/util/macros.h"
21 #include "chre/util/nanoapp/log.h"
22 #include "chre/util/nanoapp/sensor.h"
23 #include "chre/util/time.h"
24 #include "nano_calibration.h"
25
26 #define LOG_TAG "[ImuCal]"
27
28 #ifdef CHRE_NANOAPP_INTERNAL
29 namespace chre {
30 namespace {
31 #endif // CHRE_NANOAPP_INTERNAL
32
33 namespace {
34
35 struct SensorState {
36 uint32_t handle;
37 const uint8_t type;
38 bool isInitialized;
39 bool enable;
40 // Sample interval/latency defined in nanoseconds
41 uint64_t interval;
42 uint64_t highPerformanceLatency;
43 uint64_t standByLatency;
44 };
45
46 // Dynamic sensor latency settings.
47 constexpr uint64_t kDefaultHighPerformanceLatency =
48 Milliseconds(500).toRawNanoseconds();
49
50 constexpr uint64_t kDefaultStandByLatency = Seconds(1).toRawNanoseconds();
51
52 // Tracks the ON/OFF state of the gyro.
53 bool gGyroEnabled = false;
54
55 // Defines the indices for the following sensor array definition.
56 enum SensorIndex {
57 SENSOR_INDEX_TEMP = 0,
58 SENSOR_INDEX_ACCEL = 1,
59 SENSOR_INDEX_GYRO = 2,
60 SENSOR_INDEX_MAG = 3,
61 };
62
63 SensorState sensors[] = {
64 [SENSOR_INDEX_TEMP] = {
65 .type = CHRE_SENSOR_TYPE_ACCELEROMETER_TEMPERATURE,
66 .enable = true,
67 .interval = Milliseconds(500).toRawNanoseconds(),
68 .highPerformanceLatency = 0,
69 // TODO(b/63908396): this sensor should be disabled in stand-by mode
70 .standByLatency = Seconds(60).toRawNanoseconds(),
71 },
72 [SENSOR_INDEX_ACCEL] = {
73 .type = CHRE_SENSOR_TYPE_UNCALIBRATED_ACCELEROMETER,
74 .enable = true,
75 .interval = Milliseconds(10).toRawNanoseconds(),
76 .highPerformanceLatency = kDefaultHighPerformanceLatency,
77 .standByLatency = kDefaultStandByLatency,
78 },
79 [SENSOR_INDEX_GYRO] = {
80 .type = CHRE_SENSOR_TYPE_UNCALIBRATED_GYROSCOPE,
81 .enable = true,
82 .interval = Milliseconds(10).toRawNanoseconds(),
83 .highPerformanceLatency = kDefaultHighPerformanceLatency,
84 .standByLatency = kDefaultHighPerformanceLatency,
85 },
86 [SENSOR_INDEX_MAG] = {
87 .type = CHRE_SENSOR_TYPE_UNCALIBRATED_GEOMAGNETIC_FIELD,
88 .enable = true,
89 .interval = Milliseconds(20).toRawNanoseconds(),
90 .highPerformanceLatency = kDefaultHighPerformanceLatency,
91 .standByLatency = kDefaultStandByLatency,
92 },
93 };
94
95 // Container for all runtime calibration algorithms.
96 nano_calibration::NanoSensorCal nanoCal;
97
98 // Configures the Nanoapp's sensors with special adjustment of accel/gyro/mag
99 // sensor latency based on whether high-performance mode is requested.
nanoappDynamicConfigure(bool highPerformance)100 void nanoappDynamicConfigure(bool highPerformance) {
101 LOGD("Dynamic sensor configuration: %s.",
102 (highPerformance) ? "high-performance" : "stand-by");
103
104 // Configures all sensors.
105 for (size_t i = 0; i < ARRAY_SIZE(sensors); i++) {
106 SensorState &sensor = sensors[i];
107 if (!sensor.enable) {
108 // Only configure enabled sensors.
109 continue;
110 }
111
112 // Update the requested latency according to the requested mode.
113 uint64_t latency = (highPerformance)
114 ? sensor.highPerformanceLatency : sensor.standByLatency;
115
116 bool configStatus = chreSensorConfigure(
117 sensor.handle, CHRE_SENSOR_CONFIGURE_MODE_PASSIVE_CONTINUOUS,
118 sensor.interval, latency);
119
120 if (!configStatus) {
121 LOGE("Requested config. failed: handle %" PRIu32 ", interval %" PRIu64
122 " nanos, latency %" PRIu64 " nanos",
123 sensor.handle, sensor.interval, latency);
124 }
125 }
126 }
127 } // namespace
128
nanoappStart()129 bool nanoappStart() {
130 LOGI("App started on platform ID %" PRIx64, chreGetPlatformId());
131
132 // Initialize all sensors to populate their handles.
133 for (size_t i = 0; i < ARRAY_SIZE(sensors); i++) {
134 SensorState &sensor = sensors[i];
135 sensor.isInitialized = chreSensorFindDefault(sensor.type, &sensor.handle);
136
137 // TODO: Handle error condition.
138 if (!sensor.isInitialized) {
139 LOGE("Sensor handle %" PRIu32 " failed to initialize.", sensor.handle);
140 }
141 }
142
143 // Determine initial gyro state
144 struct chreSensorSamplingStatus status;
145 if (chreGetSensorSamplingStatus(sensors[SENSOR_INDEX_GYRO].handle, &status)) {
146 gGyroEnabled = status.enabled;
147 } else {
148 LOGE("Failed to get gyro sampling status.");
149 }
150
151 // Configure the Nanoapp's sensors.
152 nanoappDynamicConfigure(gGyroEnabled);
153
154 // Checks to see if the accelerometer and magnetometer were initialized.
155 bool accelIsInitialized = sensors[SENSOR_INDEX_ACCEL].isInitialized;
156 bool magIsInitialized = sensors[SENSOR_INDEX_MAG].isInitialized;
157
158 // Checks for the minimimal conditions for a nanoCal to have an active
159 // calibration algorithm running.
160 // Sensor Requirements:
161 // - GyroCal: accelerometer, gyroscope, magnetometer [optional]
162 // - OTC-Gyro: GyroCal required sensors + temperature
163 // - AccelCal: accelerometer
164 // - MagCal: magnetometer
165 if (accelIsInitialized || magIsInitialized) {
166 nanoCal.Initialize();
167 } else {
168 LOGE(
169 "None of the required sensors to enable a runtime calibration were "
170 "successfully initialized.");
171 }
172
173 return true;
174 }
175
nanoappHandleEvent(uint32_t senderInstanceId,uint16_t eventType,const void * eventData)176 void nanoappHandleEvent(uint32_t senderInstanceId, uint16_t eventType,
177 const void *eventData) {
178 switch (eventType) {
179 case CHRE_EVENT_SENSOR_UNCALIBRATED_ACCELEROMETER_DATA:
180 case CHRE_EVENT_SENSOR_UNCALIBRATED_GYROSCOPE_DATA:
181 case CHRE_EVENT_SENSOR_UNCALIBRATED_GEOMAGNETIC_FIELD_DATA: {
182 nanoCal.HandleSensorSamples(
183 eventType, static_cast<const chreSensorThreeAxisData *>(eventData));
184 break;
185 }
186
187 case CHRE_EVENT_SENSOR_ACCELEROMETER_TEMPERATURE_DATA: {
188 nanoCal.HandleTemperatureSamples(
189 eventType, static_cast<const chreSensorFloatData *>(eventData));
190 break;
191 }
192
193 case CHRE_EVENT_SENSOR_SAMPLING_CHANGE: {
194 const auto *ev =
195 static_cast<const chreSensorSamplingStatusEvent *>(eventData);
196
197 // Is this the gyro? Check the handle.
198 if (sensors[SENSOR_INDEX_GYRO].isInitialized &&
199 ev->sensorHandle == sensors[SENSOR_INDEX_GYRO].handle &&
200 ev->status.enabled != gGyroEnabled) {
201 // Modify sensor latency based on whether Gyro is enabled.
202 gGyroEnabled = ev->status.enabled;
203 nanoappDynamicConfigure(gGyroEnabled);
204 }
205 break;
206 }
207
208 default:
209 LOGW("Unhandled event %d", eventType);
210 break;
211 }
212 }
213
nanoappEnd()214 void nanoappEnd() {
215 // TODO: Unsubscribe to sensors
216 LOGI("Stopped");
217 }
218
219 #ifdef CHRE_NANOAPP_INTERNAL
220 } // anonymous namespace
221 } // namespace chre
222
223 #include "chre/platform/static_nanoapp_init.h"
224 #include "chre/util/nanoapp/app_id.h"
225
226 CHRE_STATIC_NANOAPP_INIT(ImuCal, chre::kImuCalAppId, 0);
227 #endif // CHRE_NANOAPP_INTERNAL
228