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 "[ImuCal]"
26
27 #ifdef CHRE_NANOAPP_INTERNAL
28 namespace chre {
29 namespace {
30 #endif // CHRE_NANOAPP_INTERNAL
31
32 namespace {
33
34 struct SensorState {
35 const uint8_t type;
36 uint32_t handle;
37 bool isInitialized;
38 bool enable;
39 uint64_t interval; // nsec
40 uint64_t latency; // nsec
41 chreSensorInfo info;
42 };
43
44 SensorState sensors[] = {
45 { .type = CHRE_SENSOR_TYPE_ACCELEROMETER_TEMPERATURE,
46 .enable = true,
47 .interval = Seconds(2).toRawNanoseconds(),
48 .latency = 0,
49 },
50 { .type = CHRE_SENSOR_TYPE_GYROSCOPE_TEMPERATURE,
51 .enable = true,
52 .interval = Seconds(2).toRawNanoseconds(),
53 .latency = 0,
54 },
55 { .type = CHRE_SENSOR_TYPE_UNCALIBRATED_ACCELEROMETER,
56 .enable = true,
57 .interval = Milliseconds(80).toRawNanoseconds(),
58 .latency = Seconds(4).toRawNanoseconds(),
59 },
60 { .type = CHRE_SENSOR_TYPE_UNCALIBRATED_GYROSCOPE,
61 .enable = true,
62 .interval = Milliseconds(80).toRawNanoseconds(),
63 .latency = Seconds(4).toRawNanoseconds(),
64 },
65 { .type = CHRE_SENSOR_TYPE_UNCALIBRATED_GEOMAGNETIC_FIELD,
66 .enable = true,
67 .interval = Milliseconds(80).toRawNanoseconds(),
68 .latency = Seconds(4).toRawNanoseconds(),
69 },
70 };
71
72 } // namespace
73
nanoappStart()74 bool nanoappStart() {
75 LOGI("App started on platform ID %" PRIx64, chreGetPlatformId());
76
77 for (size_t i = 0; i < ARRAY_SIZE(sensors); i++) {
78 SensorState& sensor = sensors[i];
79 sensor.isInitialized = chreSensorFindDefault(sensor.type, &sensor.handle);
80 LOGI("sensor %d initialized: %s with handle %" PRIu32,
81 i, sensor.isInitialized ? "true" : "false", sensor.handle);
82
83 if (sensor.isInitialized) {
84 // Get sensor info
85 chreSensorInfo& info = sensor.info;
86 bool infoStatus = chreGetSensorInfo(sensor.handle, &info);
87 if (infoStatus) {
88 LOGI("SensorInfo: %s, Type=%" PRIu8 " OnChange=%d"
89 " OneShot=%d minInterval=%" PRIu64 "nsec",
90 info.sensorName, info.sensorType, info.isOnChange,
91 info.isOneShot, info.minInterval);
92 } else {
93 LOGE("chreGetSensorInfo failed");
94 }
95
96
97 // Subscribe to sensors
98 if (sensor.enable) {
99 float odrHz = 1e9 / sensor.interval;
100 float latencySec = sensor.latency / 1e9;
101 bool status = chreSensorConfigure(sensor.handle,
102 CHRE_SENSOR_CONFIGURE_MODE_CONTINUOUS, sensor.interval,
103 sensor.latency);
104 LOGI("Requested data: odr %f Hz, latency %f sec, %s",
105 odrHz, latencySec, status ? "success" : "failure");
106 }
107 }
108 }
109
110 return true;
111 }
112
nanoappHandleEvent(uint32_t senderInstanceId,uint16_t eventType,const void * eventData)113 void nanoappHandleEvent(uint32_t senderInstanceId,
114 uint16_t eventType,
115 const void *eventData) {
116 switch (eventType) {
117 case CHRE_EVENT_SENSOR_UNCALIBRATED_ACCELEROMETER_DATA:
118 case CHRE_EVENT_SENSOR_UNCALIBRATED_GYROSCOPE_DATA:
119 case CHRE_EVENT_SENSOR_UNCALIBRATED_GEOMAGNETIC_FIELD_DATA: {
120 const auto *ev = static_cast<const chreSensorThreeAxisData *>(eventData);
121 const auto header = ev->header;
122 const auto *data = ev->readings;
123
124 // TODO: replace by cal algo
125 float x = 0, y = 0, z = 0;
126 for (size_t i = 0; i < header.readingCount; i++) {
127 x += data[i].v[0];
128 y += data[i].v[1];
129 z += data[i].v[2];
130 }
131 x /= header.readingCount;
132 y /= header.readingCount;
133 z /= header.readingCount;
134
135 LOGI("%s, %d samples: %f %f %f",
136 getSensorNameForEventType(eventType), header.readingCount, x, y, z);
137 break;
138 }
139
140 case CHRE_EVENT_SENSOR_ACCELEROMETER_TEMPERATURE_DATA:
141 case CHRE_EVENT_SENSOR_GYROSCOPE_TEMPERATURE_DATA: {
142 const auto *ev = static_cast<const chreSensorFloatData *>(eventData);
143 const auto header = ev->header;
144
145 // TODO: replace by cal algo
146 float v = 0;
147 for (size_t i = 0; i < header.readingCount; i++) {
148 v += ev->readings[i].value;
149 }
150 v /= header.readingCount;
151
152 LOGI("%s, %d samples: %f",
153 getSensorNameForEventType(eventType), header.readingCount, v);
154 break;
155 }
156
157 default:
158 LOGW("Unhandled event %d", eventType);
159 break;
160 }
161 }
162
nanoappEnd()163 void nanoappEnd() {
164 // TODO: Unscribe to sensors
165 LOGI("Stopped");
166 }
167
168 #ifdef CHRE_NANOAPP_INTERNAL
169 } // anonymous namespace
170 } // namespace chre
171
172 #include "chre/util/nanoapp/app_id.h"
173 #include "chre/platform/static_nanoapp_init.h"
174
175 CHRE_STATIC_NANOAPP_INIT(ImuCal, chre::kImuCalAppId, 0);
176 #endif // CHRE_NANOAPP_INTERNAL
177