1 /*
2  * Copyright (C) 2016 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.h>
18 #include <cinttypes>
19 
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 
25 #define LOG_TAG "[SensorWorld]"
26 
27 #ifdef CHRE_NANOAPP_INTERNAL
28 namespace chre {
29 namespace {
30 #endif  // CHRE_NANOAPP_INTERNAL
31 
32 using chre::Milliseconds;
33 using chre::Seconds;
34 using chre::getSensorNameForEventType;
35 using chre::kOneMillisecondInNanoseconds;
36 
37 namespace {
38 
39 //! Enable BreakIt test mode.
40 // In BreakIt test mode, a timer will be set periodically to randomly
41 // enable/disable each sensor.
42 constexpr bool kBreakIt = false;
43 constexpr Milliseconds kBreakItPeriod = Milliseconds(2000);
44 
45 //! Whether to enable sensor event logging or not.
46 constexpr bool kEnableSensorEventLogging = true;
47 
48 //! Enable/disable all sensors by default.
49 // This allows disabling all sensens by default and enabling only targeted
50 // sensors for testing by locally overriding 'enable' field in SensorState.
51 // Note that enabling BreakIt test disables all sensors at init by default.
52 constexpr bool kEnableDefault = !kBreakIt;
53 
54 struct SensorState {
55   const uint8_t type;
56   uint32_t handle;
57   bool isInitialized;
58   bool enable;
59   uint64_t interval;  // nsec
60   uint64_t latency;  // nsec
61   chreSensorInfo info;
62 };
63 
64 SensorState sensors[] = {
65   { .type = CHRE_SENSOR_TYPE_ACCELEROMETER,
66     .handle = 0,
67     .isInitialized = false,
68     .enable = kEnableDefault,
69     .interval = Milliseconds(80).toRawNanoseconds(),
70     .latency = Seconds(4).toRawNanoseconds(),
71     .info = {},
72   },
73   { .type = CHRE_SENSOR_TYPE_INSTANT_MOTION_DETECT,
74     .handle = 0,
75     .isInitialized = false,
76     .enable = false,  // InstantMotion is triggered by Prox
77     .interval = CHRE_SENSOR_INTERVAL_DEFAULT,
78     .latency = CHRE_SENSOR_LATENCY_DEFAULT,
79     .info = {},
80   },
81   { .type = CHRE_SENSOR_TYPE_STATIONARY_DETECT,
82     .handle = 0,
83     .isInitialized = false,
84     .enable = false,  // StationaryDetect is triggered by Prox
85     .interval = CHRE_SENSOR_INTERVAL_DEFAULT,
86     .latency = CHRE_SENSOR_LATENCY_DEFAULT,
87     .info = {},
88   },
89   { .type = CHRE_SENSOR_TYPE_GYROSCOPE,
90     .handle = 0,
91     .isInitialized = false,
92     .enable = kEnableDefault,
93     .interval = Milliseconds(80).toRawNanoseconds(),
94     .latency = Seconds(4).toRawNanoseconds(),
95     .info = {},
96   },
97   { .type = CHRE_SENSOR_TYPE_GEOMAGNETIC_FIELD,
98     .handle = 0,
99     .isInitialized = false,
100     .enable = kEnableDefault,
101     .interval = Milliseconds(80).toRawNanoseconds(),
102     .latency = Seconds(4).toRawNanoseconds(),
103     .info = {},
104   },
105   { .type = CHRE_SENSOR_TYPE_PRESSURE,
106     .handle = 0,
107     .isInitialized = false,
108     .enable = kEnableDefault,
109     .interval = Milliseconds(200).toRawNanoseconds(),
110     .latency = Seconds(4).toRawNanoseconds(),
111     .info = {},
112   },
113   { .type = CHRE_SENSOR_TYPE_LIGHT,
114     .handle = 0,
115     .isInitialized = false,
116     .enable = kEnableDefault,
117     .interval = Milliseconds(200).toRawNanoseconds(),
118     .latency = 0,
119     .info = {},
120   },
121   { .type = CHRE_SENSOR_TYPE_PROXIMITY,
122     .handle = 0,
123     .isInitialized = false,
124     .enable = kEnableDefault,
125     .interval = Milliseconds(200).toRawNanoseconds(),
126     .latency = 0,
127     .info = {},
128   },
129   { .type = CHRE_SENSOR_TYPE_ACCELEROMETER_TEMPERATURE,
130     .handle = 0,
131     .isInitialized = false,
132     .enable = kEnableDefault,
133     .interval = Seconds(2).toRawNanoseconds(),
134     .latency = 0,
135     .info = {},
136   },
137   { .type = CHRE_SENSOR_TYPE_GYROSCOPE_TEMPERATURE,
138     .handle = 0,
139     .isInitialized = false,
140     .enable = kEnableDefault,
141     .interval = Seconds(2).toRawNanoseconds(),
142     .latency = 0,
143     .info = {},
144   },
145   { .type = CHRE_SENSOR_TYPE_GEOMAGNETIC_FIELD_TEMPERATURE,
146     .handle = 0,
147     .isInitialized = false,
148     .enable = kEnableDefault,
149     .interval = Seconds(2).toRawNanoseconds(),
150     .latency = 0,
151     .info = {},
152   },
153   { .type = CHRE_SENSOR_TYPE_UNCALIBRATED_ACCELEROMETER,
154     .handle = 0,
155     .isInitialized = false,
156     .enable = kEnableDefault,
157     .interval = Milliseconds(80).toRawNanoseconds(),
158     .latency = Seconds(4).toRawNanoseconds(),
159     .info = {},
160   },
161   { .type = CHRE_SENSOR_TYPE_UNCALIBRATED_GYROSCOPE,
162     .handle = 0,
163     .isInitialized = false,
164     .enable = kEnableDefault,
165     .interval = Milliseconds(80).toRawNanoseconds(),
166     .latency = Seconds(4).toRawNanoseconds(),
167     .info = {},
168   },
169   { .type = CHRE_SENSOR_TYPE_UNCALIBRATED_GEOMAGNETIC_FIELD,
170     .handle = 0,
171     .isInitialized = false,
172     .enable = kEnableDefault,
173     .interval = Milliseconds(80).toRawNanoseconds(),
174     .latency = Seconds(4).toRawNanoseconds(),
175     .info = {},
176   },
177 };
178 
179 uint32_t gBreakItTimerHandle;
180 
181 // Conditional logging macro
182 #define CLOGI(fmt, ...) do {               \
183     if (kEnableSensorEventLogging) {       \
184       LOGI(fmt, ##__VA_ARGS__);            \
185     }                                      \
186   } while (0);
187 
188 // Helpers for testing InstantMotion and StationaryDetect
189 enum class MotionMode {
190   Instant,
191   Stationary,
192 };
193 
194 // Storage to help access InstantMotion and StationaryDetect sensor handle and
195 // info
196 size_t motionSensorIndices[2];
197 MotionMode motionMode = MotionMode::Instant;
198 
getMotionSensorIndex()199 size_t getMotionSensorIndex() {
200   motionMode = (motionMode == MotionMode::Instant) ?
201       MotionMode::Stationary : MotionMode::Instant;
202   return motionSensorIndices[static_cast<size_t>(motionMode)];
203 }
204 
205 //! Used to loop through all sensors to query sensor sampling status.
206 size_t statusIndex = 0;
207 
208 // Obtains 16-bit psuedo-random numbers.
getNextLfsrState()209 uint16_t getNextLfsrState() {
210   // 15-bit LFSR with feedback polynomial x^15 + x^14 + 1 gives us a
211   // pseudo-random sequence over all 32767 possible values
212   static uint16_t lfsr = 0x1337;
213   uint16_t nextBit = ((lfsr << 14) ^ (lfsr << 13)) & 0x4000;
214   lfsr = nextBit | (lfsr >> 1);
215 
216   return lfsr;
217 }
218 
handleTimerEvent(const void * eventData)219 void handleTimerEvent(const void *eventData) {
220   for (size_t i = 0; i < ARRAY_SIZE(sensors); i++) {
221     SensorState& sensor = sensors[i];
222 
223     bool enable = getNextLfsrState() & 0x1;
224     if (sensor.isInitialized && sensor.enable != enable) {
225       sensor.enable = enable;
226 
227       bool status;
228       if (!enable) {
229         status = chreSensorConfigureModeOnly(
230             sensor.handle, CHRE_SENSOR_CONFIGURE_MODE_DONE);
231       } else {
232         enum chreSensorConfigureMode mode = sensor.info.isOneShot
233             ? CHRE_SENSOR_CONFIGURE_MODE_ONE_SHOT
234             : CHRE_SENSOR_CONFIGURE_MODE_CONTINUOUS;
235         status = chreSensorConfigure(
236             sensor.handle, mode, sensor.interval, sensor.latency);
237       }
238 
239       LOGI("Configure [enable %d, status %d]: %s",
240            enable, status, getSensorTypeName(sensor.type));
241     }
242   }
243 
244   gBreakItTimerHandle = chreTimerSet(kBreakItPeriod.toRawNanoseconds(),
245         nullptr /* data */, true /* oneShot */);
246 }
247 
248 } // namespace
249 
nanoappStart()250 bool nanoappStart() {
251   LOGI("App started on platform ID %" PRIx64, chreGetPlatformId());
252 
253   for (size_t i = 0; i < ARRAY_SIZE(sensors); i++) {
254     SensorState& sensor = sensors[i];
255     sensor.isInitialized = chreSensorFindDefault(sensor.type, &sensor.handle);
256     LOGI("Sensor %zu initialized: %s with handle %" PRIu32,
257          i, sensor.isInitialized ? "true" : "false", sensor.handle);
258 
259     if (sensor.type == CHRE_SENSOR_TYPE_INSTANT_MOTION_DETECT) {
260       motionSensorIndices[static_cast<size_t>(MotionMode::Instant)] = i;
261     } else if (sensor.type == CHRE_SENSOR_TYPE_STATIONARY_DETECT) {
262       motionSensorIndices[static_cast<size_t>(MotionMode::Stationary)] = i;
263     }
264 
265     if (sensor.isInitialized) {
266       // Get sensor info
267       chreSensorInfo& info = sensor.info;
268       bool infoStatus = chreGetSensorInfo(sensor.handle, &info);
269       if (infoStatus) {
270         LOGI("SensorInfo: %s, Type=%" PRIu8 " OnChange=%d"
271              " OneShot=%d minInterval=%" PRIu64 "nsec",
272              info.sensorName, info.sensorType, info.isOnChange,
273              info.isOneShot, info.minInterval);
274       } else {
275         LOGE("chreGetSensorInfo failed");
276       }
277 
278       // Subscribe to sensors
279       if (sensor.enable) {
280         float odrHz = 1e9f / sensor.interval;
281         float latencySec = sensor.latency / 1e9f;
282         bool status = chreSensorConfigure(sensor.handle,
283             CHRE_SENSOR_CONFIGURE_MODE_CONTINUOUS, sensor.interval,
284             sensor.latency);
285         LOGI("Requested data: odr %f Hz, latency %f sec, %s",
286              odrHz, latencySec, status ? "success" : "failure");
287       }
288     }
289   }
290 
291   // Set timer for BreakIt test.
292   if (kBreakIt) {
293     gBreakItTimerHandle = chreTimerSet(kBreakItPeriod.toRawNanoseconds(),
294         nullptr /* data */, true /* oneShot */);
295   }
296 
297   return true;
298 }
299 
nanoappHandleEvent(uint32_t senderInstanceId,uint16_t eventType,const void * eventData)300 void nanoappHandleEvent(uint32_t senderInstanceId,
301                         uint16_t eventType,
302                         const void *eventData) {
303   uint64_t chreTime = chreGetTime();
304   uint64_t sampleTime;
305   switch (eventType) {
306     case CHRE_EVENT_SENSOR_ACCELEROMETER_DATA:
307     case CHRE_EVENT_SENSOR_UNCALIBRATED_ACCELEROMETER_DATA:
308     case CHRE_EVENT_SENSOR_GYROSCOPE_DATA:
309     case CHRE_EVENT_SENSOR_UNCALIBRATED_GYROSCOPE_DATA:
310     case CHRE_EVENT_SENSOR_GEOMAGNETIC_FIELD_DATA:
311     case CHRE_EVENT_SENSOR_UNCALIBRATED_GEOMAGNETIC_FIELD_DATA: {
312       const auto *ev = static_cast<const chreSensorThreeAxisData *>(eventData);
313       const auto header = ev->header;
314       const auto *data = ev->readings;
315       sampleTime = header.baseTimestamp;
316 
317       float x = 0, y = 0, z = 0;
318       for (size_t i = 0; i < header.readingCount; i++) {
319         x += data[i].v[0];
320         y += data[i].v[1];
321         z += data[i].v[2];
322         sampleTime += data[i].timestampDelta;
323       }
324       x /= header.readingCount;
325       y /= header.readingCount;
326       z /= header.readingCount;
327 
328       CLOGI("%s, %d samples: %f %f %f, t=%" PRIu64 " ms",
329             getSensorNameForEventType(eventType), header.readingCount, x, y, z,
330             header.baseTimestamp / kOneMillisecondInNanoseconds);
331 
332       if (eventType == CHRE_EVENT_SENSOR_UNCALIBRATED_GYROSCOPE_DATA) {
333         CLOGI("UncalGyro time: first %" PRIu64 " last %" PRIu64 " chre %" PRIu64
334               " delta [%" PRId64 ", %" PRId64 "]ms",
335               header.baseTimestamp, sampleTime, chreTime,
336               static_cast<int64_t>(header.baseTimestamp - chreTime)
337               / static_cast<int64_t>(kOneMillisecondInNanoseconds),
338               static_cast<int64_t>(sampleTime - chreTime)
339               / static_cast<int64_t>(kOneMillisecondInNanoseconds));
340       }
341       break;
342     }
343 
344     case CHRE_EVENT_SENSOR_PRESSURE_DATA:
345     case CHRE_EVENT_SENSOR_LIGHT_DATA:
346     case CHRE_EVENT_SENSOR_ACCELEROMETER_TEMPERATURE_DATA:
347     case CHRE_EVENT_SENSOR_GYROSCOPE_TEMPERATURE_DATA:
348     case CHRE_EVENT_SENSOR_GEOMAGNETIC_FIELD_TEMPERATURE_DATA: {
349       const auto *ev = static_cast<const chreSensorFloatData *>(eventData);
350       const auto header = ev->header;
351 
352       float v = 0;
353       for (size_t i = 0; i < header.readingCount; i++) {
354         v += ev->readings[i].value;
355       }
356       v /= header.readingCount;
357 
358       CLOGI("%s, %d samples: %f, t=%" PRIu64 " ms",
359             getSensorNameForEventType(eventType), header.readingCount, v,
360             header.baseTimestamp / kOneMillisecondInNanoseconds);
361       break;
362     }
363 
364     case CHRE_EVENT_SENSOR_PROXIMITY_DATA: {
365       const auto *ev = static_cast<const chreSensorByteData *>(eventData);
366       const auto header = ev->header;
367       const auto reading = ev->readings[0];
368       sampleTime = header.baseTimestamp;
369 
370       CLOGI("%s, %d samples: isNear %d, invalid %d",
371             getSensorNameForEventType(eventType), header.readingCount,
372             reading.isNear, reading.invalid);
373 
374       CLOGI("Prox time: sample %" PRIu64 " chre %" PRIu64 " delta %" PRId64
375             "ms", header.baseTimestamp, chreTime,
376             static_cast<int64_t>(sampleTime - chreTime) / 1000000);
377 
378       // Enable InstantMotion and StationaryDetect alternatively on near->far.
379       if (reading.isNear == 0 && !kBreakIt) {
380         size_t motionSensorIndex = getMotionSensorIndex();
381         bool status = chreSensorConfigure(sensors[motionSensorIndex].handle,
382             CHRE_SENSOR_CONFIGURE_MODE_ONE_SHOT,
383             CHRE_SENSOR_INTERVAL_DEFAULT,
384             CHRE_SENSOR_LATENCY_DEFAULT);
385         LOGI("Requested %s: %s", sensors[motionSensorIndex].info.sensorName,
386               status ? "success" : "failure");
387       }
388 
389       // Exercise chreGetSensorSamplingStatus on one sensor on near->far.
390       if (sensors[statusIndex].isInitialized && reading.isNear == 0) {
391         struct chreSensorSamplingStatus status;
392         bool success = chreGetSensorSamplingStatus(sensors[statusIndex].handle,
393                                                    &status);
394         LOGI("%s success %d: enabled %d interval %" PRIu64 " latency %" PRIu64,
395              sensors[statusIndex].info.sensorName, success, status.enabled,
396              status.interval, status.latency);
397       }
398       statusIndex = (statusIndex + 1) % ARRAY_SIZE(sensors);
399       break;
400     }
401 
402     case CHRE_EVENT_SENSOR_INSTANT_MOTION_DETECT_DATA:
403     case CHRE_EVENT_SENSOR_STATIONARY_DETECT_DATA: {
404       const auto *ev = static_cast<const chreSensorOccurrenceData *>(eventData);
405       const auto header = ev->header;
406 
407       CLOGI("%s, %d samples",
408             getSensorNameForEventType(eventType), header.readingCount);
409       break;
410     }
411 
412     case CHRE_EVENT_SENSOR_SAMPLING_CHANGE: {
413       const auto *ev = static_cast<const chreSensorSamplingStatusEvent *>(
414           eventData);
415 
416       CLOGI("Sampling Change: handle %" PRIu32 ", status: interval %" PRIu64
417             " latency %" PRIu64 " enabled %d",
418             ev->sensorHandle, ev->status.interval, ev->status.latency,
419             ev->status.enabled);
420       break;
421     }
422 
423 
424     case CHRE_EVENT_TIMER:
425       if (!kBreakIt) {
426         LOGE("Timer event received with gBreakIt is disabled");
427       } else {
428         handleTimerEvent(eventData);
429       }
430       break;
431 
432     default:
433       LOGW("Unhandled event %d", eventType);
434       break;
435   }
436 }
437 
nanoappEnd()438 void nanoappEnd() {
439   LOGI("Stopped");
440 }
441 
442 #ifdef CHRE_NANOAPP_INTERNAL
443 }  // anonymous namespace
444 }  // namespace chre
445 
446 #include "chre/util/nanoapp/app_id.h"
447 #include "chre/platform/static_nanoapp_init.h"
448 
449 CHRE_STATIC_NANOAPP_INIT(SensorWorld, chre::kSensorWorldAppId, 0);
450 #endif  // CHRE_NANOAPP_INTERNAL
451