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_SFE_EDGE)
17 #define ARDUINO_EXCLUDE_CODE
18 #endif // defined(ARDUINO) && !defined(ARDUINO_SFE_EDGE)
19
20 #ifndef ARDUINO_EXCLUDE_CODE
21
22 #include "tensorflow/lite/micro/examples/magic_wand/accelerometer_handler.h"
23
24 // These are headers from Ambiq's Apollo3 SDK.
25 #include <string.h>
26
27 #include "am_bsp.h" // NOLINT
28 #include "am_mcu_apollo.h" // NOLINT
29 #include "am_util.h" // NOLINT
30 #include "lis2dh12_platform_apollo3.h"
31
32 lis2dh12_platform_apollo3_if_t dev_if; // accelerometer device interface
33 lis2dh12_ctx_t dev_ctx; // accelerometer device control
34
35 // A union representing either int16_t[3] or uint8_t[6],
36 // storing the most recent data
37 axis3bit16_t data_raw_acceleration;
38 // A buffer holding the last 200 sets of 3-channel values
39 float save_data[600] = {0.0};
40 // Most recent position in the save_data buffer
41 int begin_index = 0;
42 // True if there is not yet enough data to run inference
43 bool pending_initial_data = true;
44
initAccelerometer(void)45 int initAccelerometer(void) {
46 uint32_t retVal32 = 0;
47 static uint8_t whoamI = 0;
48
49 am_hal_iom_config_t i2cConfig;
50 memset((void*)(&i2cConfig), 0x00, sizeof(am_hal_iom_config_t));
51 i2cConfig.eInterfaceMode = AM_HAL_IOM_I2C_MODE;
52 i2cConfig.ui32ClockFreq = AM_HAL_IOM_100KHZ;
53
54 // Initialize the IOM.
55 retVal32 = am_hal_iom_initialize(
56 AM_BSP_ACCELEROMETER_I2C_IOM,
57 &(dev_if.iomHandle)); // set the iomHandle of the device interface
58 if (retVal32 != AM_HAL_STATUS_SUCCESS) {
59 return (int)retVal32;
60 }
61
62 retVal32 =
63 am_hal_iom_power_ctrl((dev_if.iomHandle), AM_HAL_SYSCTRL_WAKE, false);
64 if (retVal32 != AM_HAL_STATUS_SUCCESS) {
65 return (int)retVal32;
66 }
67
68 retVal32 = am_hal_iom_configure((dev_if.iomHandle), &i2cConfig);
69 if (retVal32 != AM_HAL_STATUS_SUCCESS) {
70 return (int)retVal32;
71 }
72
73 // Configure the IOM pins.
74 am_hal_gpio_pinconfig(AM_BSP_ACCELEROMETER_I2C_SDA_PIN,
75 g_AM_BSP_ACCELEROMETER_I2C_SDA_PIN);
76 am_hal_gpio_pinconfig(AM_BSP_ACCELEROMETER_I2C_SCL_PIN,
77 g_AM_BSP_ACCELEROMETER_I2C_SDA_PIN);
78
79 // Enable the IOM.
80 retVal32 = am_hal_iom_enable((dev_if.iomHandle));
81 if (retVal32 != AM_HAL_STATUS_SUCCESS) {
82 return (int)retVal32;
83 }
84
85 //
86 // Apply accelerometer configuration
87 lis2dh12_device_id_get(&dev_ctx, &whoamI);
88 if (whoamI != LIS2DH12_ID) {
89 return AM_HAL_STATUS_FAIL;
90 }
91
92 lis2dh12_block_data_update_set(&dev_ctx, PROPERTY_ENABLE);
93 lis2dh12_temperature_meas_set(&dev_ctx, LIS2DH12_TEMP_ENABLE);
94 lis2dh12_data_rate_set(&dev_ctx, LIS2DH12_ODR_25Hz);
95 lis2dh12_full_scale_set(&dev_ctx, LIS2DH12_2g);
96 lis2dh12_temperature_meas_set(&dev_ctx, LIS2DH12_TEMP_ENABLE);
97 lis2dh12_operating_mode_set(&dev_ctx, LIS2DH12_HR_12bit);
98
99 return (int)AM_HAL_STATUS_SUCCESS;
100 }
101
SetupAccelerometer(tflite::ErrorReporter * error_reporter)102 TfLiteStatus SetupAccelerometer(tflite::ErrorReporter* error_reporter) {
103 // Set the clock frequency.
104 am_hal_clkgen_control(AM_HAL_CLKGEN_CONTROL_SYSCLK_MAX, 0);
105
106 // Set the default cache configuration
107 am_hal_cachectrl_config(&am_hal_cachectrl_defaults);
108 am_hal_cachectrl_enable();
109
110 // Configure the board for low power operation.
111 am_bsp_low_power_init();
112
113 // Initialize the device interface and control structures
114 dev_if.iomHandle =
115 NULL; // Gets initialized once iomHandle is known (in initAccel())
116 dev_if.addCS = AM_BSP_ACCELEROMETER_I2C_ADDRESS; // Gets the accelerometer
117 // I2C address for the board
118 dev_if.useSPI = false; // Using I2C
119
120 dev_ctx.write_reg = lis2dh12_write_platform_apollo3; // write bytes function
121 dev_ctx.read_reg = lis2dh12_read_platform_apollo3; // read bytes function
122 dev_ctx.handle = (void*)&dev_if; // Apollo3-specific interface information
123
124 // Collecting data at 25Hz.
125 int accInitRes = initAccelerometer();
126 if (accInitRes != (int)AM_HAL_STATUS_SUCCESS) {
127 TF_LITE_REPORT_ERROR(error_reporter,
128 "Failed to initialize the accelerometer. (code %d)",
129 accInitRes);
130 }
131
132 // Enable the accelerometer's FIFO buffer.
133 // Note: LIS2DH12 has a FIFO buffer which holds up to 32 data entries. It
134 // accumulates data while the CPU is busy. Old data will be overwritten if
135 // it's not fetched in time, so we need to make sure that model inference is
136 // faster than 1/25Hz * 32 = 1.28s
137 if (lis2dh12_fifo_set(&dev_ctx, 1)) {
138 TF_LITE_REPORT_ERROR(error_reporter, "Failed to enable FIFO buffer.");
139 }
140
141 if (lis2dh12_fifo_mode_set(&dev_ctx, LIS2DH12_BYPASS_MODE)) {
142 TF_LITE_REPORT_ERROR(error_reporter, "Failed to clear FIFO buffer.");
143 return kTfLiteError;
144 }
145
146 if (lis2dh12_fifo_mode_set(&dev_ctx, LIS2DH12_DYNAMIC_STREAM_MODE)) {
147 TF_LITE_REPORT_ERROR(error_reporter, "Failed to set streaming mode.");
148 return kTfLiteError;
149 }
150
151 TF_LITE_REPORT_ERROR(error_reporter, "Magic starts!");
152
153 return kTfLiteOk;
154 }
155
ReadAccelerometer(tflite::ErrorReporter * error_reporter,float * input,int length)156 bool ReadAccelerometer(tflite::ErrorReporter* error_reporter, float* input,
157 int length) {
158 // Check FIFO buffer for new samples
159 lis2dh12_fifo_src_reg_t status;
160 if (lis2dh12_fifo_status_get(&dev_ctx, &status)) {
161 TF_LITE_REPORT_ERROR(error_reporter, "Failed to get FIFO status.");
162 return false;
163 }
164
165 int samples = status.fss;
166 if (status.ovrn_fifo) {
167 samples++;
168 }
169
170 // Skip this round if data is not ready yet
171 if (samples == 0) {
172 return false;
173 }
174
175 // Load data from FIFO buffer
176 axis3bit16_t data_raw_acceleration_local;
177 for (int i = 0; i < samples; i++) {
178 // Zero out the struct that holds raw accelerometer data
179 memset(data_raw_acceleration_local.u8bit, 0x00, 3 * sizeof(int16_t));
180 // If the return value is non-zero, sensor data was successfully read
181 if (lis2dh12_acceleration_raw_get(&dev_ctx,
182 data_raw_acceleration_local.u8bit)) {
183 TF_LITE_REPORT_ERROR(error_reporter, "Failed to get raw data.");
184 } else {
185 // Convert each raw 16-bit value into floating point values representing
186 // milli-Gs, a unit of acceleration, and store in the current position of
187 // our buffer
188 save_data[begin_index++] =
189 lis2dh12_from_fs2_hr_to_mg(data_raw_acceleration_local.i16bit[0]);
190 save_data[begin_index++] =
191 lis2dh12_from_fs2_hr_to_mg(data_raw_acceleration_local.i16bit[1]);
192 save_data[begin_index++] =
193 lis2dh12_from_fs2_hr_to_mg(data_raw_acceleration_local.i16bit[2]);
194 // Start from beginning, imitating loop array.
195 if (begin_index >= 600) begin_index = 0;
196 }
197 }
198
199 // Check if we are ready for prediction or still pending more initial data
200 if (pending_initial_data && begin_index >= 200) {
201 pending_initial_data = false;
202 }
203
204 // Return if we don't have enough data
205 if (pending_initial_data) {
206 return false;
207 }
208
209 // Copy the requested number of bytes to the provided input tensor
210 for (int i = 0; i < length; ++i) {
211 int ring_array_index = begin_index + i - length;
212 if (ring_array_index < 0) {
213 ring_array_index += 600;
214 }
215 input[i] = save_data[ring_array_index];
216 }
217 return true;
218 }
219
220 #endif // ARDUINO_EXCLUDE_CODE
221