1 /* Copyright 2019 The TensorFlow Authors. All Rights Reserved.
2
3 Licensed under the Apache License, Version 2.0 (the "License");
4 you may not use this file except in compliance with the License.
5 You may obtain a copy of the License at
6
7 http://www.apache.org/licenses/LICENSE-2.0
8
9 Unless required by applicable law or agreed to in writing, software
10 distributed under the License is distributed on an "AS IS" BASIS,
11 WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 See the License for the specific language governing permissions and
13 limitations under the License.
14 ==============================================================================*/
15
16 #if defined(ARDUINO) && !defined(ARDUINO_ARDUINO_NANO33BLE)
17 #define ARDUINO_EXCLUDE_CODE
18 #endif // defined(ARDUINO) && !defined(ARDUINO_ARDUINO_NANO33BLE)
19
20 #ifndef ARDUINO_EXCLUDE_CODE
21
22 #include "tensorflow/lite/micro/examples/magic_wand/accelerometer_handler.h"
23
24 #include <Arduino.h>
25 #include <Arduino_LSM9DS1.h>
26
27 #include "tensorflow/lite/micro/examples/magic_wand/constants.h"
28
29 // A buffer holding the last 200 sets of 3-channel values
30 float save_data[600] = {0.0};
31 // Most recent position in the save_data buffer
32 int begin_index = 0;
33 // True if there is not yet enough data to run inference
34 bool pending_initial_data = true;
35 // How often we should save a measurement during downsampling
36 int sample_every_n;
37 // The number of measurements since we last saved one
38 int sample_skip_counter = 1;
39
SetupAccelerometer(tflite::ErrorReporter * error_reporter)40 TfLiteStatus SetupAccelerometer(tflite::ErrorReporter* error_reporter) {
41 // Switch on the IMU
42 if (!IMU.begin()) {
43 TF_LITE_REPORT_ERROR(error_reporter, "Failed to initialize IMU");
44 return kTfLiteError;
45 }
46
47 // Make sure we are pulling measurements into a FIFO.
48 // If you see an error on this line, make sure you have at least v1.1.0 of the
49 // Arduino_LSM9DS1 library installed.
50 IMU.setContinuousMode();
51
52 // Determine how many measurements to keep in order to
53 // meet kTargetHz
54 float sample_rate = IMU.accelerationSampleRate();
55 sample_every_n = static_cast<int>(roundf(sample_rate / kTargetHz));
56
57 TF_LITE_REPORT_ERROR(error_reporter, "Magic starts!");
58
59 return kTfLiteOk;
60 }
61
ReadAccelerometer(tflite::ErrorReporter * error_reporter,float * input,int length)62 bool ReadAccelerometer(tflite::ErrorReporter* error_reporter, float* input,
63 int length) {
64 // Keep track of whether we stored any new data
65 bool new_data = false;
66 // Loop through new samples and add to buffer
67 while (IMU.accelerationAvailable()) {
68 float x, y, z;
69 // Read each sample, removing it from the device's FIFO buffer
70 if (!IMU.readAcceleration(x, y, z)) {
71 TF_LITE_REPORT_ERROR(error_reporter, "Failed to read data");
72 break;
73 }
74 // Throw away this sample unless it's the nth
75 if (sample_skip_counter != sample_every_n) {
76 sample_skip_counter += 1;
77 continue;
78 }
79 // Write samples to our buffer, converting to milli-Gs and rotating the axis
80 // order for compatibility with model (sensor orientation is different on
81 // Arduino Nano BLE Sense compared with SparkFun Edge).
82 // The expected orientation of the Arduino on the wand is with the USB port
83 // facing down the shaft towards the user's hand, with the reset button
84 // pointing at the user's face:
85 //
86 // ____
87 // | |<- Arduino board
88 // | |
89 // | () | <- Reset button
90 // | |
91 // -TT- <- USB port
92 // ||
93 // ||<- Wand
94 // ....
95 // ||
96 // ||
97 // ()
98 //
99 const float norm_x = -z;
100 const float norm_y = y;
101 const float norm_z = x;
102 save_data[begin_index++] = norm_x * 1000;
103 save_data[begin_index++] = norm_y * 1000;
104 save_data[begin_index++] = norm_z * 1000;
105 // Since we took a sample, reset the skip counter
106 sample_skip_counter = 1;
107 // If we reached the end of the circle buffer, reset
108 if (begin_index >= 600) {
109 begin_index = 0;
110 }
111 new_data = true;
112 }
113
114 // Skip this round if data is not ready yet
115 if (!new_data) {
116 return false;
117 }
118
119 // Check if we are ready for prediction or still pending more initial data
120 if (pending_initial_data && begin_index >= 200) {
121 pending_initial_data = false;
122 }
123
124 // Return if we don't have enough data
125 if (pending_initial_data) {
126 return false;
127 }
128
129 // Copy the requested number of bytes to the provided input tensor
130 for (int i = 0; i < length; ++i) {
131 int ring_array_index = begin_index + i - length;
132 if (ring_array_index < 0) {
133 ring_array_index += 600;
134 }
135 input[i] = save_data[ring_array_index];
136 }
137
138 return true;
139 }
140
141 #endif // ARDUINO_EXCLUDE_CODE
142
