1 /*
2 * Copyright 2019 The TensorFlow Authors. All Rights Reserved.
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 "accelerometer_handler.hpp"
18
19 #include <zephyr/device.h>
20 #include <zephyr/drivers/sensor.h>
21 #include <stdio.h>
22 #include <string.h>
23 #include <zephyr/kernel.h>
24
25 #define BUFLEN 300
26 int begin_index = 0;
27 const struct device *const sensor = DEVICE_DT_GET_ONE(adi_adxl345);
28 int current_index = 0;
29
30 float bufx[BUFLEN] = { 0.0f };
31 float bufy[BUFLEN] = { 0.0f };
32 float bufz[BUFLEN] = { 0.0f };
33
34 bool initial = true;
35
SetupAccelerometer()36 TfLiteStatus SetupAccelerometer()
37 {
38 if (!device_is_ready(sensor)) {
39 printk("%s: device not ready.\n", sensor->name);
40 return kTfLiteApplicationError;
41 }
42
43 MicroPrintf("Got accelerometer, name: %s\n", sensor->name);
44
45 return kTfLiteOk;
46 }
47
ReadAccelerometer(float * input,int length)48 bool ReadAccelerometer(float *input, int length)
49 {
50 int rc;
51 struct sensor_value accel[3];
52 int samples_count;
53
54 rc = sensor_sample_fetch(sensor);
55 if (rc < 0) {
56 MicroPrintf("Fetch failed\n");
57 return false;
58 }
59 /* Skip if there is no data */
60 if (!rc) {
61 return false;
62 }
63
64 samples_count = rc;
65 for (int i = 0; i < samples_count; i++) {
66 rc = sensor_channel_get(sensor, SENSOR_CHAN_ACCEL_XYZ, accel);
67 if (rc < 0) {
68 MicroPrintf("ERROR: Update failed: %d\n", rc);
69 return false;
70 }
71 bufx[begin_index] = (float)sensor_value_to_double(&accel[0]);
72 bufy[begin_index] = (float)sensor_value_to_double(&accel[1]);
73 bufz[begin_index] = (float)sensor_value_to_double(&accel[2]);
74 begin_index++;
75 if (begin_index >= BUFLEN) {
76 begin_index = 0;
77 }
78 }
79
80 if (initial && begin_index >= 100) {
81 initial = false;
82 }
83
84 if (initial) {
85 return false;
86 }
87
88 int sample = 0;
89 for (int i = 0; i < (length - 3); i += 3) {
90 int ring_index = begin_index + sample - length / 3;
91 if (ring_index < 0) {
92 ring_index += BUFLEN;
93 }
94 input[i] = bufx[ring_index];
95 input[i + 1] = bufy[ring_index];
96 input[i + 2] = bufz[ring_index];
97 sample++;
98 }
99 return true;
100 }
101
