Skip to content

Commit dc59867

Browse files
cparatacparata
authoredMar 16, 2023
Merge pull request #15 from elle-bi/main
Add Flight2 application, which uses newest BlueST protocol and STM32d…
·
1.8.01.6.0
2 parents d857ed2 + 959fdee commit dc59867

File tree

3 files changed

+927
-1
lines changed

3 files changed

+927
-1
lines changed
 

‎README.md

+6
Original file line numberDiff line numberDiff line change
@@ -25,6 +25,11 @@ to SensiBLE and see sensors data. The data are also printed to the serial port.
2525
X-NUCLEO-IKS01A2 or X-NUCLEO-IKS01A3, X-NUCLEO-IDB05A1 and X-NUCLEO-53L1A1 Expansion Boards. The application gather data from the sensor mounted and then communicates
2626
them via bluetooth to a suitable Android app (such as ST BLE Sensor). The data gathered include enviromental data (temperature, pression, humidity),
2727
distance, gesture recognition (tap and directional swipe), acceleration (with accelerometer hardware events) and a gyroscope.
28+
* Flight2: This application provides an example of usage of a NUCLEO board (it was tested with NUCLEO-F401RE) with
29+
X-NUCLEO-IKS01A3, X-NUCLEO-IDB05A2 and X-NUCLEO-53L1A1 Expansion Boards. The application gathers data from the sensors mounted and then communicates
30+
them via bluetooth to a suitable Android or iOS app (such as ST BLE Sensor). The data gathered include enviromental data (temperature, pression, humidity),
31+
distance, gesture recognition (tap and directional swipe), acceleration (with accelerometer hardware events), a gyroscope and a magnetometer. Note that
32+
unlike the Flight1 application, this one makes use of the STM32duinoBLE API instead of SPBTLE-RF, and does not support X-NUCLEO-IKS01A2 boards. In addition, an up-to-date version of the STM32duinoBLE library is required, as this application uses some of the newest features.
2833
* IKS01A3_S2LP_P2P_Demo: This application provides a simple example of usage of two NUCLEO boards (it was tested with NUCLEO-F401RE and NUCLEO-L053R8)
2934
with a X-NUCLEO-IKS01A3 and one among X-NUCLEO-S2868A1, X-NUCLEO-S2868A2 and X-NUCLEO-S2915A1. It shows how to send and receive all the sensor data
3035
provided by the X-NUCLEO-IKS01A3 between two NUCLEO boards connected each other through S2-LP based Expansion Boards; in order to send the sensor data
@@ -60,6 +65,7 @@ The FP_Examples library requires the following STM32duino libraries (link to the
6065
* STM32duino S2-LP: https://github.com/stm32duino/S2-LP
6166
* STM32duino M95640-R: https://github.com/stm32duino/M95640-R
6267
* STM32duino X-NUCLEO-IKS02A1 Audio: https://github.com/stm32duino/X-NUCLEO-IKS02A1-Audio
68+
* STM32duinoBLE: https://github.com/stm32duino/STM32duinoBLE
6369

6470
## Documentation
6571

‎examples/Flight2/Flight2.ino

+920
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,920 @@
1+
/**
2+
******************************************************************************
3+
@file Flight2.ino
4+
@author STMicroelectronics
5+
@version V1.0.0
6+
@date 15 March 2023
7+
@brief Arduino demo application for the STMicrolectronics
8+
X-NUCLEO-IKS01A3, X-NUCLEO-53L1A1
9+
and X-NUCLEO-IDB05A1
10+
******************************************************************************
11+
@attention
12+
13+
<h2><center>&copy; COPYRIGHT(c) 2023 STMicroelectronics</center></h2>
14+
15+
Redistribution and use in source and binary forms, with or without modification,
16+
are permitted provided that the following conditions are met:
17+
1. Redistributions of source code must retain the above copyright notice,
18+
this list of conditions and the following disclaimer.
19+
2. Redistributions in binary form must reproduce the above copyright notice,
20+
this list of conditions and the following disclaimer in the documentation
21+
and/or other materials provided with the distribution.
22+
3. Neither the name of STMicroelectronics nor the names of its contributors
23+
may be used to endorse or promote products derived from this software
24+
without specific prior written permission.
25+
26+
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
27+
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
28+
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
29+
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
30+
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
31+
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
32+
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
33+
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
34+
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
35+
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
36+
37+
******************************************************************************
38+
*/
39+
40+
// Note: make sure your STM32duinoBLE is updated to the latest version!
41+
42+
// Includes ------------------------------------------------------------------
43+
44+
#include <string.h>
45+
#include <Wire.h>
46+
#include <SPI.h>
47+
#include <vl53l1x_x_nucleo_53l1a1_class.h>
48+
#include <stmpe1600_class.h>
49+
#include <tof_gestures.h>
50+
#include <tof_gestures_DIRSWIPE_1.h>
51+
#include <tof_gestures_TAP_1.h>
52+
53+
#include <HTS221Sensor.h>
54+
#include <LPS22HHSensor.h>
55+
#include <LIS2DW12Sensor.h>
56+
#include <LIS2MDLSensor.h>
57+
#include <LSM6DSOSensor.h>
58+
#include <STTS751Sensor.h>
59+
60+
#include <STM32duinoBLE.h>
61+
62+
#define DEV_I2C Wire
63+
#define SerialPort Serial
64+
65+
//#define DEBUG_MODE
66+
67+
#define INT_1 4
68+
#define INT_2 5
69+
70+
// BLE boards
71+
#if defined(ARDUINO_STEVAL_MKBOXPRO)
72+
/* STEVAL-MKBOXPRO */
73+
SPIClass SpiHCI(PA7, PA6, PA5);
74+
HCISpiTransportClass HCISpiTransport(SpiHCI, BLUENRG_LP, PA2, PB11, PD4, 1000000, SPI_MODE3);
75+
#if !defined(FAKE_BLELOCALDEVICE)
76+
BLELocalDevice BLEObj(&HCISpiTransport);
77+
BLELocalDevice &BLE = BLEObj;
78+
#endif
79+
#elif defined(ARDUINO_STEVAL_MKSBOX1V1)
80+
/* STEVAL-MKSBOX1V1 */
81+
SPIClass SpiHCI(PC3, PD3, PD1);
82+
HCISpiTransportClass HCISpiTransport(SpiHCI, SPBTLE_1S, PD0, PD4, PA8, 1000000, SPI_MODE1);
83+
#if !defined(FAKE_BLELOCALDEVICE)
84+
BLELocalDevice BLEObj(&HCISpiTransport);
85+
BLELocalDevice &BLE = BLEObj;
86+
#endif
87+
#elif defined(ARDUINO_B_L475E_IOT01A) || defined(ARDUINO_B_L4S5I_IOT01A)
88+
/* B-L475E-IOT01A1 or B_L4S5I_IOT01A */
89+
SPIClass SpiHCI(PC12, PC11, PC10);
90+
HCISpiTransportClass HCISpiTransport(SpiHCI, SPBTLE_RF, PD13, PE6, PA8, 8000000, SPI_MODE0);
91+
#if !defined(FAKE_BLELOCALDEVICE)
92+
BLELocalDevice BLEObj(&HCISpiTransport);
93+
BLELocalDevice &BLE = BLEObj;
94+
#endif
95+
#elif defined(ARDUINO_NUCLEO_WB15CC) || defined(ARDUINO_P_NUCLEO_WB55RG) || defined(ARDUINO_STM32WB5MM_DK)
96+
HCISharedMemTransportClass HCISharedMemTransport;
97+
#if !defined(FAKE_BLELOCALDEVICE)
98+
BLELocalDevice BLEObj(&HCISharedMemTransport);
99+
BLELocalDevice &BLE = BLEObj;
100+
#endif
101+
#else
102+
/* Shield IDB05A2 with SPI clock on D3 */
103+
SPIClass SpiHCI(D11, D12, D3);
104+
HCISpiTransportClass HCISpiTransport(SpiHCI, BLUENRG_M0, A1, A0, D7, 8000000, SPI_MODE0);
105+
#if !defined(FAKE_BLELOCALDEVICE)
106+
BLELocalDevice BLEObj(&HCISpiTransport);
107+
BLELocalDevice &BLE = BLEObj;
108+
#endif
109+
/* Shield IDB05A2 with SPI clock on D13 */
110+
// #define SpiHCI SPI
111+
// HCISpiTransportClass HCISpiTransport(SpiHCI, BLUENRG_M0, A1, A0, D7, 8000000, SPI_MODE0);
112+
// #if !defined(FAKE_BLELOCALDEVICE)
113+
// BLELocalDevice BLEObj(&HCISpiTransport);
114+
// BLELocalDevice& BLE = BLEObj;
115+
// #endif
116+
/* Shield IDB05A1 with SPI clock on D3 */
117+
// SPIClass SpiHCI(D11, D12, D3);
118+
// HCISpiTransportClass HCISpiTransport(SpiHCI, SPBTLE_RF, A1, A0, D7, 8000000, SPI_MODE0);
119+
// #if !defined(FAKE_BLELOCALDEVICE)
120+
// BLELocalDevice BLEObj(&HCISpiTransport);
121+
// BLELocalDevice& BLE = BLEObj;
122+
// #endif
123+
/* Shield IDB05A1 with SPI clock on D13 */
124+
// #define SpiHCI SPI
125+
// HCISpiTransportClass HCISpiTransport(SpiHCI, SPBTLE_RF, A1, A0, D7, 8000000, SPI_MODE0);
126+
// #if !defined(FAKE_BLELOCALDEVICE)
127+
// BLELocalDevice BLEObj(&HCISpiTransport);
128+
// BLELocalDevice& BLE = BLEObj;
129+
// #endif
130+
/* Shield BNRG2A1 with SPI clock on D3 */
131+
// SPIClass SpiHCI(D11, D12, D3);
132+
// HCISpiTransportClass HCISpiTransport(SpiHCI, BLUENRG_M2SP, A1, A0, D7, 1000000, SPI_MODE1);
133+
// #if !defined(FAKE_BLELOCALDEVICE)
134+
// BLELocalDevice BLEObj(&HCISpiTransport);
135+
// BLELocalDevice& BLE = BLEObj;
136+
// #endif
137+
/* Shield BNRG2A1 with SPI clock on D13 */
138+
// #define SpiHCI SPI
139+
// HCISpiTransportClass HCISpiTransport(SpiHCI, BLUENRG_M2SP, A1, A0, D7, 1000000, SPI_MODE1);
140+
// #if !defined(FAKE_BLELOCALDEVICE)
141+
// BLELocalDevice BLEObj(&HCISpiTransport);
142+
// BLELocalDevice& BLE = BLEObj;
143+
// #endif
144+
#endif
145+
146+
// Interrupts
147+
volatile int mems_event = 0;
148+
149+
// Package Version only numbers 0->9
150+
#define FLIGHT1_VERSION_MAJOR '5'
151+
#define FLIGHT1_VERSION_MINOR '0'
152+
#define FLIGHT1_VERSION_PATCH '0'
153+
154+
#ifdef DEBUG_MODE
155+
#define FLIGHT1_PRINTF(...) \
156+
do {\
157+
char report[130];\
158+
snprintf(report, sizeof(report), __VA_ARGS__);\
159+
SerialPort.print(report);\
160+
} while(0);
161+
#else
162+
#define FLIGHT1_PRINTF(...) while(0){;}
163+
#endif
164+
// Define the FLIGHT1 Name MUST be 7 char long
165+
#define NAME_FLIGHT1 'F','L','2','V',FLIGHT1_VERSION_MAJOR,FLIGHT1_VERSION_MINOR,FLIGHT1_VERSION_PATCH
166+
167+
// SPI Configuration
168+
#define IDB0XA1_PIN_SPI_MOSI (11)
169+
#define IDB0XA1_PIN_SPI_MISO (12)
170+
#define IDB0XA1_PIN_SPI_SCK (3)
171+
172+
#define IDB0XA1_PIN_SPI_nCS (A1)
173+
#define IDB0XA1_PIN_SPI_RESET (7)
174+
#define IDB0XA1_PIN_SPI_IRQ (A0)
175+
176+
#define STORE_LE_16(buf, val) ( ((buf)[0] = (uint8_t) (val) ) , \
177+
((buf)[1] = (uint8_t) (val>>8) ) )
178+
179+
#define STORE_LE_32(buf, val) ( ((buf)[0] = (uint8_t) (val) ) , \
180+
((buf)[1] = (uint8_t) (val>>8) ) , \
181+
((buf)[2] = (uint8_t) (val>>16) ) , \
182+
((buf)[3] = (uint8_t) (val>>24) ) )
183+
184+
#define STORE_BE_32(buf, val) ( ((buf)[3] = (uint8_t) (val) ) , \
185+
((buf)[2] = (uint8_t) (val>>8) ) , \
186+
((buf)[1] = (uint8_t) (val>>16) ) , \
187+
((buf)[0] = (uint8_t) (val>>24) ) )
188+
189+
// Macros for conversion from float to int
190+
#define MCR_BLUEMS_F2I_1D(in, out_int, out_dec) {out_int = (int32_t)in; out_dec= (int32_t)((in-out_int)*10);};
191+
#define MCR_BLUEMS_F2I_2D(in, out_int, out_dec) {out_int = (int32_t)in; out_dec= (int32_t)((in-out_int)*100);};
192+
193+
#define FEATURE_MASK_ACC_EVENTS 0x00000400u
194+
195+
// Distance components
196+
STMPE1600DigiOut *xshutdown_top;
197+
STMPE1600DigiOut *xshutdown_left;
198+
STMPE1600DigiOut *xshutdown_right;
199+
VL53L1X_X_NUCLEO_53L1A1 *sensor_vl53l1_top;
200+
VL53L1X_X_NUCLEO_53L1A1 *sensor_vl53l1_left;
201+
VL53L1X_X_NUCLEO_53L1A1 *sensor_vl53l1_right;
202+
203+
204+
// Gesture structure
205+
Gesture_DIRSWIPE_1_Data_t gestureDirSwipeData;
206+
Gesture_TAP_1_Data_t gestureTapData;
207+
// Range values
208+
uint16_t distance_top, distance_left, distance_right;
209+
210+
// MEMS sensors
211+
HTS221Sensor *HumTemp;
212+
LPS22HHSensor *PressTemp;
213+
LSM6DSOSensor *AccGyr;
214+
LIS2DW12Sensor *Acc2;
215+
LIS2MDLSensor *Mag;
216+
STTS751Sensor *Temp;
217+
218+
// STRING UUIDs
219+
char *uuidSensorService = "00000000-0001-11e1-9ab4-0002a5d5c51b";
220+
char *uuidConfigService = "00000000-000f-11e1-9ab4-0002a5d5c51b";
221+
char *uuidPressChar = "00100000-0001-11e1-ac36-0002a5d5c51b";
222+
char *uuidHumChar = "00080000-0001-11e1-ac36-0002a5d5c51b";
223+
char *uuidTempChar = "00040000-0001-11e1-ac36-0002a5d5c51b";
224+
char *uuidAccChar = "00800000-0001-11e1-ac36-0002a5d5c51b";
225+
char *uuidGyroChar = "00400000-0001-11e1-ac36-0002a5d5c51b";
226+
char *uuidMagChar = "00200000-0001-11e1-ac36-0002a5d5c51b";
227+
char *uuidProxChar = "02000000-0001-11e1-ac36-0002a5d5c51b";
228+
char *uuidGestureChar = "00000004-0001-11e1-ac36-0002a5d5c51b";
229+
char *uuidAccEventChar = "00000400-0001-11e1-ac36-0002a5d5c51b";
230+
char *uuidConfigChar = "00000002-000f-11e1-ac36-0002a5d5c51b";
231+
232+
#define LEN_DISTANCE 4
233+
#define LEN_GESTURE 3
234+
#define LEN_MAG 8
235+
#define LEN_GYRO 8
236+
#define LEN_ACC 8
237+
#define LEN_ACCEVENT 5 // This might become 3 or 4, make sure to check!
238+
#define LEN_PRESS 6
239+
#define LEN_HUM 4
240+
#define LEN_TEMP 4
241+
#define LEN_CFG 20
242+
243+
// ABLE services and characteristics
244+
BLEService sensorService(uuidSensorService);
245+
BLEService configService(uuidConfigService);
246+
247+
BLECharacteristic distanceC(uuidProxChar, BLERead | BLENotify, LEN_DISTANCE);
248+
BLECharacteristic gestureC(uuidGestureChar, BLERead | BLENotify, LEN_GESTURE);
249+
250+
BLECharacteristic pressC(uuidPressChar, BLERead | BLENotify, LEN_PRESS);
251+
BLECharacteristic humC(uuidHumChar, BLERead | BLENotify, LEN_HUM);
252+
BLECharacteristic tempC(uuidTempChar, BLERead | BLENotify, LEN_TEMP);
253+
254+
BLECharacteristic accC(uuidAccChar, BLERead | BLENotify, LEN_ACC);
255+
BLECharacteristic accEventC(uuidAccEventChar, BLERead | BLENotify, LEN_ACCEVENT);
256+
BLECharacteristic gyroC(uuidGyroChar, BLERead | BLENotify, LEN_GYRO);
257+
BLECharacteristic magC(uuidMagChar, BLERead | BLENotify, LEN_MAG);
258+
259+
BLECharacteristic configC(uuidConfigChar, BLENotify | BLEWrite, LEN_CFG);
260+
261+
// Class for bluetooth communication and services
262+
class Flight1Service {
263+
public:
264+
265+
uint16_t globalSteps = 0;
266+
boolean shortMode = false;
267+
268+
Flight1Service(void)
269+
{
270+
}
271+
272+
// Initializes the sensor with the default params
273+
int begin()
274+
{
275+
const char BoardName[8] = {NAME_FLIGHT1, 0};
276+
int ret;
277+
ret = BLE.begin();
278+
279+
BLE.setLocalName(BoardName);
280+
BLE.setAdvertisedService(sensorService);
281+
282+
uint8_t addr[6] = {0xff};
283+
BLE.getRandomAddress(addr);
284+
285+
#define FEATURE_MASK 0x02,0xfc,0x04,0x15
286+
287+
uint8_t data [14] = {0x0d, 0xff, 0x01, 0x80, FEATURE_MASK};
288+
289+
data[8] = addr[5];
290+
data[9] = addr[4];
291+
data[10] = addr[3];
292+
data[11] = addr[2];
293+
data[12] = addr[1];
294+
data[13] = addr[0];
295+
296+
BLEAdvertisingData adv;
297+
adv.setRawData(data, 14);
298+
BLE.setAdvertisingData(adv);
299+
300+
BLE.advertise();
301+
302+
Add_HWServW2ST_Service();
303+
304+
return ret;
305+
}
306+
307+
// Update enviromental data
308+
int Environmental_Update(int32_t Press, uint16_t Hum, int16_t Temp)
309+
{
310+
uint8_t pBuff[LEN_PRESS];
311+
uint8_t hBuff[LEN_HUM];
312+
uint8_t tBuff[LEN_TEMP];
313+
314+
STORE_LE_16(pBuff, millis());
315+
STORE_LE_32(pBuff + 2, (Press));
316+
317+
STORE_LE_16(hBuff, millis());
318+
STORE_LE_16(hBuff + 2, (Hum));
319+
320+
STORE_LE_16(tBuff, millis());
321+
STORE_LE_16(tBuff + 2, (Temp));
322+
323+
int ret = 0;
324+
ret += pressC.writeValue(pBuff, LEN_PRESS);
325+
ret += humC.writeValue(hBuff, LEN_HUM);
326+
ret += tempC.writeValue(tBuff, LEN_TEMP);
327+
328+
return ret; // Should be 3 (true) on return
329+
}
330+
331+
//Update the proximity sensor distance
332+
int FlightSense_Distance_Update(uint16_t Distance)
333+
{
334+
uint8_t buff[LEN_DISTANCE];
335+
336+
/* To discriminate the long proximity range from 53L1A1*/
337+
//Distance= Distance | (1 << 15);
338+
339+
STORE_LE_16(buff, millis());
340+
STORE_LE_16(buff + 2, Distance);
341+
342+
int err = 0;
343+
err += distanceC.writeValue(buff, LEN_DISTANCE);
344+
345+
return err;
346+
}
347+
348+
// Update the accelerometer and gyroscope
349+
// Order of axes: X, Y, Z
350+
int AccGyroMag_Update(int32_t *Accel, int32_t *Gyros, int32_t *Mag)
351+
{
352+
int32_t AXIS_X;
353+
int32_t AXIS_Y;
354+
int32_t AXIS_Z;
355+
356+
uint8_t buff[2 + 3 * 2 * 2];
357+
358+
uint8_t aBuff[LEN_ACC];
359+
uint8_t gBuff[LEN_GYRO];
360+
uint8_t mBuff[LEN_MAG];
361+
362+
STORE_LE_16(aBuff, millis());
363+
STORE_LE_16(aBuff + 2, Accel[0]);
364+
STORE_LE_16(aBuff + 4, Accel[1]);
365+
STORE_LE_16(aBuff + 6, Accel[2]);
366+
367+
STORE_LE_16(mBuff, millis());
368+
STORE_LE_16(mBuff + 2, Mag[0]);
369+
STORE_LE_16(mBuff + 4, Mag[1]);
370+
STORE_LE_16(mBuff + 6, Mag[2]);
371+
372+
AXIS_X = (Gyros[0]) * 10;
373+
AXIS_Y = (Gyros[1]) * 10;
374+
AXIS_Z = (Gyros[2]) * 10;
375+
376+
STORE_LE_16(gBuff, millis());
377+
STORE_LE_16(gBuff + 2, AXIS_X);
378+
STORE_LE_16(gBuff + 4, AXIS_Y);
379+
STORE_LE_16(gBuff + 6, AXIS_Z);
380+
381+
int ret = 0;
382+
ret += gyroC.writeValue(gBuff, LEN_GYRO);
383+
ret += accC.writeValue(aBuff, LEN_ACC);
384+
ret += magC.writeValue(mBuff, LEN_MAG);
385+
386+
return ret;
387+
}
388+
389+
// Update the gesture recognition
390+
int Gestures_Update(uint8_t gest_code)
391+
{
392+
uint8_t buff[LEN_GESTURE];
393+
394+
STORE_LE_16(buff, millis());
395+
buff[2] = gest_code;
396+
397+
int ret = 0;
398+
ret += gestureC.writeValue(buff, LEN_GESTURE);
399+
400+
return ret;
401+
}
402+
403+
int AccEvent_Notify(uint16_t steps, uint8_t event)
404+
{
405+
uint8_t buff_2[2 + 1]; // Only event
406+
uint8_t buff_3[2 + 2]; // Only pedometer
407+
uint8_t buff_4[5]; // Event and pedometer
408+
409+
int ret = 0;
410+
411+
if (shortMode) {
412+
STORE_LE_16(buff_4, millis());
413+
buff_2[2] = event;
414+
ret += accEventC.writeValue(buff_2, 2 + 1);
415+
} else {
416+
STORE_LE_16(buff_4, millis());
417+
buff_4[2] = event;
418+
STORE_LE_16(buff_4 + 3, steps);
419+
ret += accEventC.writeValue(buff_4, LEN_ACCEVENT);
420+
}
421+
return ret;
422+
}
423+
424+
int Config_Notify(uint8_t Feature [4], uint8_t Command, uint8_t data)
425+
{
426+
uint8_t buff[2 + 4 + 1 + 1];
427+
int ret = 0;
428+
429+
STORE_LE_16(buff, millis());
430+
buff[2] = Feature[0];
431+
buff[3] = Feature[1];
432+
buff[4] = Feature[2];
433+
buff[5] = Feature[3];
434+
buff[6] = Command;
435+
buff[7] = data;
436+
437+
ret = configC.writeValue(buff, 2 + 4 + 1 + 1);
438+
439+
return ret;
440+
}
441+
442+
private:
443+
444+
// Add the services
445+
void Add_HWServW2ST_Service(void)
446+
{
447+
sensorService.addCharacteristic(gestureC);
448+
sensorService.addCharacteristic(distanceC);
449+
sensorService.addCharacteristic(pressC);
450+
sensorService.addCharacteristic(humC);
451+
sensorService.addCharacteristic(tempC);
452+
sensorService.addCharacteristic(accC);
453+
sensorService.addCharacteristic(accEventC);
454+
sensorService.addCharacteristic(gyroC);
455+
sensorService.addCharacteristic(magC);
456+
457+
configService.addCharacteristic(configC);
458+
459+
BLE.addService(sensorService);
460+
BLE.addService(configService);
461+
462+
return;
463+
}
464+
};
465+
466+
Flight1Service Flight1;
467+
468+
void enableAllFunc()
469+
{
470+
AccGyr->Enable_Pedometer();
471+
AccGyr->Enable_Tilt_Detection(LSM6DSO_INT1_PIN);
472+
AccGyr->Enable_Free_Fall_Detection(LSM6DSO_INT1_PIN);
473+
AccGyr->Enable_Single_Tap_Detection(LSM6DSO_INT1_PIN);
474+
AccGyr->Enable_Double_Tap_Detection(LSM6DSO_INT1_PIN);
475+
AccGyr->Enable_6D_Orientation(LSM6DSO_INT1_PIN);
476+
AccGyr->Step_Counter_Reset();
477+
}
478+
479+
void disableAllFunc()
480+
{
481+
AccGyr->Disable_Pedometer();
482+
AccGyr->Disable_Tilt_Detection();
483+
AccGyr->Disable_Free_Fall_Detection();
484+
AccGyr->Disable_Single_Tap_Detection();
485+
AccGyr->Disable_Double_Tap_Detection();
486+
AccGyr->Disable_6D_Orientation();
487+
}
488+
489+
// Setup distance sensors for gesture detection
490+
void SetupSingleShot(VL53L1X_X_NUCLEO_53L1A1 *sensor)
491+
{
492+
int status;
493+
494+
//Change distance mode to short range
495+
status = sensor->VL53L1X_SetDistanceMode(1);
496+
if (status) {
497+
SerialPort.println("SetDistanceMode failed");
498+
}
499+
500+
//Change timing budget to 20 ms
501+
status = sensor->VL53L1X_SetTimingBudgetInMs(20);
502+
if (status) {
503+
SerialPort.println("SetMeasurementTimingBudgetMicroSeconds failed");
504+
}
505+
status = sensor->VL53L1X_SetInterMeasurementInMs(20);
506+
if (status) {
507+
SerialPort.println("SetInterMeasurementPeriodMilliSeconds failed");
508+
}
509+
510+
}
511+
512+
void INT1Event_cb()
513+
{
514+
mems_event = 1;
515+
}
516+
517+
void INT2Event_cb()
518+
{
519+
mems_event = 1;
520+
}
521+
522+
523+
524+
void configCB(BLEDevice unused1, BLECharacteristic unused2)
525+
{
526+
uint8_t buf[LEN_CFG];
527+
528+
configC.readValue(buf, LEN_CFG);
529+
char command = buf[4];
530+
uint8_t data = buf[5];
531+
uint8_t signalEvents [4] = {0x00, 0x00, 0x04, 0x00};
532+
533+
if (!memcmp(buf, signalEvents, 4) && data) {
534+
enableAllFunc();
535+
switch (command) {
536+
case 'o':
537+
Flight1.shortMode = true;
538+
break;
539+
default:
540+
Flight1.shortMode = false;
541+
break;
542+
}
543+
} else {
544+
disableAllFunc();
545+
}
546+
547+
// Either way, respond by repeating the command
548+
uint8_t feature [4] = {0x00};
549+
memcpy(feature, buf, 4);
550+
Flight1.Config_Notify(feature, buf[4], buf[5]);
551+
}
552+
553+
int gestureGuard = 0;
554+
555+
bool envEnable = false;
556+
bool proxEnable = false;
557+
bool eventEnable = false;
558+
bool accEnable = false;
559+
bool gyroEnable = false;
560+
bool magEnable = false;
561+
562+
void setup()
563+
{
564+
SerialPort.begin(115200);
565+
DEV_I2C.begin();
566+
567+
pinMode(LED_BUILTIN, OUTPUT); //D13 LED
568+
569+
//Interrupts.
570+
attachInterrupt(INT_1, INT1Event_cb, RISING);
571+
attachInterrupt(INT_2, INT2Event_cb, RISING);
572+
573+
if (!Flight1.begin()) {
574+
SerialPort.println("Bluetooth services configuration error!");
575+
while (1);
576+
}
577+
SerialPort.println("Bluetooth configuration done!");
578+
579+
// Create VL53L1X top component.
580+
xshutdown_top = new STMPE1600DigiOut(&DEV_I2C, GPIO_15, (0x42 * 2));
581+
sensor_vl53l1_top = new VL53L1X_X_NUCLEO_53L1A1(&DEV_I2C, xshutdown_top);
582+
sensor_vl53l1_top->begin();
583+
584+
// Switch off VL53L1X top component.
585+
sensor_vl53l1_top->VL53L1X_Off();
586+
587+
// Create (if present) VL53L1X left component.
588+
xshutdown_left = new STMPE1600DigiOut(&DEV_I2C, GPIO_14, (0x43 * 2));
589+
sensor_vl53l1_left = new VL53L1X_X_NUCLEO_53L1A1(&DEV_I2C, xshutdown_left);
590+
sensor_vl53l1_left->begin();
591+
592+
//Switch off (if present) VL53L1X left component.
593+
sensor_vl53l1_left->VL53L1X_Off();
594+
595+
// Create (if present) VL53L1X right component.
596+
xshutdown_right = new STMPE1600DigiOut(&DEV_I2C, GPIO_15, (0x43 * 2));
597+
sensor_vl53l1_right = new VL53L1X_X_NUCLEO_53L1A1(&DEV_I2C, xshutdown_right);
598+
sensor_vl53l1_right->begin();
599+
600+
// Switch off (if present) VL53L1X right component.
601+
sensor_vl53l1_right->VL53L1X_Off();
602+
603+
//Initialize the sensor
604+
sensor_vl53l1_top->InitSensor(0x10);
605+
sensor_vl53l1_left->InitSensor(0x12);
606+
sensor_vl53l1_right->InitSensor(0x14);
607+
608+
//Change Distance mode and timings
609+
SetupSingleShot(sensor_vl53l1_top);
610+
SetupSingleShot(sensor_vl53l1_left);
611+
SetupSingleShot(sensor_vl53l1_right);
612+
613+
614+
//Top sensor should be in long distance mode
615+
sensor_vl53l1_top->VL53L1X_SetDistanceMode(2);
616+
617+
// Initialize VL53L1X gesture library.
618+
tof_gestures_initDIRSWIPE_1(400, 0, 500, &gestureDirSwipeData);
619+
tof_gestures_initTAP_1(&gestureTapData);
620+
621+
//Start measurement
622+
sensor_vl53l1_top->VL53L1X_StartRanging();
623+
sensor_vl53l1_left->VL53L1X_StartRanging();
624+
sensor_vl53l1_right->VL53L1X_StartRanging();
625+
626+
//Setup MEMS sensors
627+
HumTemp = new HTS221Sensor(&DEV_I2C);
628+
PressTemp = new LPS22HHSensor(&DEV_I2C);
629+
AccGyr = new LSM6DSOSensor(&DEV_I2C);
630+
Acc2 = new LIS2DW12Sensor(&DEV_I2C);
631+
Mag = new LIS2MDLSensor(&DEV_I2C);
632+
Temp = new STTS751Sensor(&DEV_I2C);
633+
Temp->begin();
634+
Temp->Enable();
635+
Acc2->begin();
636+
Acc2->Enable_X();
637+
HumTemp->begin();
638+
HumTemp->Enable();
639+
PressTemp->begin();
640+
PressTemp->Enable();
641+
AccGyr->begin();
642+
AccGyr->Enable_X();
643+
AccGyr->Set_X_ODR(4.0f);
644+
AccGyr->Enable_G();
645+
Mag->begin();
646+
Mag->Enable();
647+
648+
configC.setEventHandler(BLEWritten, configCB);
649+
}
650+
651+
void loop()
652+
{
653+
654+
int status;
655+
uint8_t ready = 0;
656+
uint16_t distance;
657+
int32_t decPart, intPart;
658+
int32_t PressToSend = 0;
659+
uint16_t HumToSend = 0;
660+
int16_t TempToSend = 0;
661+
int gesture_code;
662+
int left_done = 0;
663+
int right_done = 0;
664+
uint8_t NewDataReady = 0;
665+
uint8_t RangeStatus;
666+
667+
BLE.poll();
668+
669+
envEnable = pressC.subscribed() || tempC.subscribed() || humC.subscribed();
670+
accEnable = accC.subscribed();
671+
gyroEnable = gyroC.subscribed();
672+
magEnable = magC.subscribed();
673+
eventEnable = accEventC.subscribed();
674+
proxEnable = distanceC.subscribed() || gestureC.subscribed();
675+
676+
if (envEnable) {
677+
//Get enviroment data
678+
float humidity, temperature, pressure;
679+
HumTemp->GetHumidity(&humidity);
680+
Temp->GetTemperature(&temperature);
681+
PressTemp->GetPressure(&pressure);
682+
MCR_BLUEMS_F2I_2D(pressure, intPart, decPart);
683+
PressToSend = intPart * 100 + decPart;
684+
MCR_BLUEMS_F2I_1D(humidity, intPart, decPart);
685+
HumToSend = intPart * 10 + decPart;
686+
MCR_BLUEMS_F2I_1D(temperature, intPart, decPart);
687+
TempToSend = intPart * 10 + decPart;
688+
}
689+
690+
int32_t accelerometer[3];
691+
int32_t gyroscope[3];
692+
int32_t magnetometer[3];
693+
694+
if (accEnable) {
695+
// Read accelerometer
696+
AccGyr->Get_X_Axes(accelerometer);
697+
}
698+
if (gyroEnable) {
699+
// Read gyroscope
700+
AccGyr->Get_G_Axes(gyroscope);
701+
}
702+
if (magEnable) {
703+
// Read magnetometer
704+
Mag->GetAxes(magnetometer);
705+
}
706+
707+
if (mems_event) {
708+
mems_event = 0;
709+
LSM6DSO_Event_Status_t Astatus;
710+
AccGyr->Get_X_Event_Status(&Astatus);
711+
uint8_t stat = 0;
712+
713+
if (Astatus.StepStatus) {
714+
uint16_t step_count = 0;
715+
AccGyr->Get_Step_Count(&step_count);
716+
FLIGHT1_PRINTF("Step %d\n", step_count);
717+
718+
Flight1.globalSteps = step_count;
719+
}
720+
if (Astatus.FreeFallStatus) {
721+
FLIGHT1_PRINTF("Free fall\n");
722+
stat = stat | 0x10u;
723+
}
724+
725+
if (Astatus.TapStatus) {
726+
FLIGHT1_PRINTF("Single tap\n");
727+
stat = stat | 0x20u;
728+
}
729+
730+
if (Astatus.DoubleTapStatus) {
731+
FLIGHT1_PRINTF("Double tap\n");
732+
stat = stat | 0x40u;
733+
}
734+
735+
if (Astatus.TiltStatus) {
736+
FLIGHT1_PRINTF("Tilt\n");
737+
stat = stat | 0x08u;
738+
}
739+
740+
if (Astatus.D6DOrientationStatus) {
741+
FLIGHT1_PRINTF("6D Interrupt\n");
742+
uint8_t xl = 0;
743+
uint8_t xh = 0;
744+
uint8_t yl = 0;
745+
uint8_t yh = 0;
746+
uint8_t zl = 0;
747+
uint8_t zh = 0;
748+
uint8_t OrientationResult = 0;
749+
AccGyr->Get_6D_Orientation_XL(&xl);
750+
AccGyr->Get_6D_Orientation_XH(&xh);
751+
AccGyr->Get_6D_Orientation_YL(&yl);
752+
AccGyr->Get_6D_Orientation_YH(&yh);
753+
AccGyr->Get_6D_Orientation_ZL(&zl);
754+
AccGyr->Get_6D_Orientation_ZH(&zh);
755+
if (xl == 0 && yl == 0 && zl == 0 && xh == 0 && yh == 1 && zh == 0) {
756+
OrientationResult = 0x04u;
757+
} else if (xl == 1 && yl == 0 && zl == 0 && xh == 0 && yh == 0 && zh == 0) {
758+
OrientationResult = 0x01u;
759+
} else if (xl == 0 && yl == 0 && zl == 0 && xh == 1 && yh == 0 && zh == 0) {
760+
OrientationResult = 0x03u;
761+
} else if (xl == 0 && yl == 1 && zl == 0 && xh == 0 && yh == 0 && zh == 0) {
762+
OrientationResult = 0x02u;
763+
} else if (xl == 0 && yl == 0 && zl == 0 && xh == 0 && yh == 0 && zh == 1) {
764+
OrientationResult = 0x05u;
765+
} else if (xl == 0 && yl == 0 && zl == 1 && xh == 0 && yh == 0 && zh == 0) {
766+
OrientationResult = 0x06u;
767+
}
768+
stat = stat | OrientationResult;
769+
}
770+
771+
if (Astatus.WakeUpStatus) {
772+
FLIGHT1_PRINTF("Wake Up\n");
773+
stat = stat | 0x80u;
774+
}
775+
Flight1.AccEvent_Notify(Flight1.globalSteps, stat);
776+
}
777+
778+
if (proxEnable) {
779+
//Get top sensor distance and transmit
780+
do {
781+
sensor_vl53l1_top->VL53L1X_CheckForDataReady(&ready);
782+
} while (!ready);
783+
784+
status = sensor_vl53l1_top->VL53L1X_GetRangeStatus(&RangeStatus);
785+
status = sensor_vl53l1_top->VL53L1X_GetDistance(&distance);
786+
787+
if (status == VL53L1X_ERROR_NONE) {
788+
Flight1.FlightSense_Distance_Update(distance);
789+
}
790+
791+
//Clear interrupt
792+
status = sensor_vl53l1_top->VL53L1X_ClearInterrupt();
793+
794+
distance = (RangeStatus == 0 && distance < 1400) ? distance : 1400;
795+
796+
// Launch gesture detection algorithm.
797+
gesture_code = tof_gestures_detectTAP_1(distance, &gestureTapData);
798+
799+
// Check the result of the gesture detection algorithm.
800+
switch (gesture_code) {
801+
case GESTURES_SINGLE_TAP:
802+
Flight1.Gestures_Update(1);
803+
gestureGuard = 0;
804+
break;
805+
default:
806+
// Do nothing
807+
break;
808+
}
809+
810+
811+
// Wait for data ready
812+
do {
813+
//if left not done
814+
if (left_done == 0) {
815+
NewDataReady = 0;
816+
//check measurement data ready
817+
int status = sensor_vl53l1_left->VL53L1X_CheckForDataReady(&NewDataReady);
818+
819+
if (status) {
820+
SerialPort.println("GetMeasurementDataReady left sensor failed");
821+
}
822+
//if ready
823+
if (NewDataReady) {
824+
//get status
825+
status = sensor_vl53l1_left->VL53L1X_GetRangeStatus(&RangeStatus);
826+
if (status) {
827+
SerialPort.println("GetRangeStatus left sensor failed");
828+
}
829+
830+
//if distance < 1.3 m
831+
if (RangeStatus == 0) {
832+
// we have a valid range.
833+
status = sensor_vl53l1_left->VL53L1X_GetDistance(&distance_left);
834+
if (status) {
835+
SerialPort.println("GetDistance left sensor failed");
836+
}
837+
} else {
838+
distance_left = 1400; //default distance
839+
}
840+
841+
//restart measurement
842+
status = sensor_vl53l1_left->VL53L1X_ClearInterrupt();
843+
if (status) {
844+
SerialPort.println("Restart left sensor failed");
845+
}
846+
847+
left_done = 1 ;
848+
}
849+
}
850+
851+
//if right not done
852+
if (right_done == 0) {
853+
NewDataReady = 0;
854+
//check measurement data ready
855+
int status = sensor_vl53l1_right->VL53L1X_CheckForDataReady(&NewDataReady);
856+
857+
if (status) {
858+
SerialPort.println("GetMeasurementDataReady right sensor failed");
859+
}
860+
//if ready
861+
if (NewDataReady) {
862+
//get status
863+
status = sensor_vl53l1_right->VL53L1X_GetRangeStatus(&RangeStatus);
864+
if (status) {
865+
SerialPort.println("GetRangeStatus right sensor failed");
866+
}
867+
//if distance < 1.3 m
868+
if (RangeStatus == 0) {
869+
// we have a valid range.
870+
status = sensor_vl53l1_right->VL53L1X_GetDistance(&distance_right);
871+
if (status) {
872+
SerialPort.println("GetDistance right sensor failed");
873+
}
874+
} else {
875+
distance_right = 1400; //default distance
876+
}
877+
878+
//restart measurement
879+
status = sensor_vl53l1_right->VL53L1X_ClearInterrupt();
880+
if (status) {
881+
SerialPort.println("Restart right sensor failed");
882+
}
883+
884+
right_done = 1 ;
885+
}
886+
}
887+
} while (left_done == 0 || right_done == 0);
888+
889+
890+
// Launch gesture detection algorithm.
891+
gesture_code = tof_gestures_detectDIRSWIPE_1(distance_left, distance_right, &gestureDirSwipeData);
892+
893+
// Check the result of the gesture detection algorithm.
894+
switch (gesture_code) {
895+
case GESTURES_SWIPE_LEFT_RIGHT:
896+
Flight1.Gestures_Update(3);
897+
gestureGuard = 0;
898+
break;
899+
case GESTURES_SWIPE_RIGHT_LEFT:
900+
Flight1.Gestures_Update(2);
901+
gestureGuard = 0;
902+
break;
903+
default:
904+
if (!gestureGuard) {
905+
Flight1.Gestures_Update(0);
906+
gestureGuard = 1;
907+
}
908+
break;
909+
}
910+
}
911+
912+
//Send all mems sensors data
913+
if (envEnable) {
914+
Flight1.Environmental_Update(PressToSend, HumToSend, TempToSend);
915+
}
916+
917+
if (gyroEnable || accEnable || magEnable) {
918+
Flight1.AccGyroMag_Update(accelerometer, gyroscope, magnetometer);
919+
}
920+
}

‎library.properties

+1-1
Original file line numberDiff line numberDiff line change
@@ -7,4 +7,4 @@ paragraph=This library provides several Function Packs that combine the usage of
77
category=Other
88
url=https://github.com/stm32duino/FP_Examples
99
architectures=stm32
10-
depends=STM32duino LSM6DSL, STM32duino LSM303AGR, STM32duino LPS22HB, STM32duino VL6180X, STM32duino VL53L0X, STM32duino VL53L1X, STM32duino Proximity Gesture, STM32duino X-NUCLEO-6180XA1, STM32duino X-NUCLEO-53L0A1, STM32duino X-NUCLEO-53L1A1, STM32duino SPBTLE-RF, STM32duino X-NUCLEO-IHM02A1, STM32duino X-NUCLEO-LED61A1, STM32duino LPS25HB, STM32duino LSM6DS3, STM32duino LSM6DSO, STM32duino LIS2DW12, STM32duino LIS2MDL, STM32duino HTS221, STM32duino LPS22HH, STM32duino STTS751, STM32duino S2-LP, STM32duino M95640-R, STM32duino X-NUCLEO-IKS02A1 Audio
10+
depends=STM32duino LSM6DSL, STM32duino LSM303AGR, STM32duino LPS22HB, STM32duino VL6180X, STM32duino VL53L0X, STM32duino VL53L1X, STM32duino Proximity Gesture, STM32duino X-NUCLEO-6180XA1, STM32duino X-NUCLEO-53L0A1, STM32duino X-NUCLEO-53L1A1, STM32duino SPBTLE-RF, STM32duino X-NUCLEO-IHM02A1, STM32duino X-NUCLEO-LED61A1, STM32duino LPS25HB, STM32duino LSM6DS3, STM32duino LSM6DSO, STM32duino LIS2DW12, STM32duino LIS2MDL, STM32duino HTS221, STM32duino LPS22HH, STM32duino STTS751, STM32duino S2-LP, STM32duino M95640-R, STM32duino X-NUCLEO-IKS02A1 Audio, STM32duinoBLE

0 commit comments

Comments
 (0)
Please sign in to comment.