Skip to content

Commit 61db7c9

Browse files
cparatacparata
committed
Add MLC examples for LSM6DSOX on Nano RP2040
1 parent 9eeba8d commit 61db7c9

File tree

5 files changed

+536
-0
lines changed

5 files changed

+536
-0
lines changed

‎libraries/NanoRP2040Connect_MLC/examples/RP2040_DataLogger_FIFO/RP2040_DataLogger_FIFO.ino

Lines changed: 260 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,260 @@
1+
/*
2+
This example exposes the first MB of Rp2040 flash as a USB disk.
3+
The user can interact with this disk as a bidirectional communication with the board
4+
For example, the board could save data in a file to be retrieved later with a drag and drop.
5+
If the user does a double tap, the firmware goes to datalogger mode (green led on).
6+
Now the user can do another double tap to start a recording of the IMU data
7+
(green led blinking). With another double tap the recording will be stopped (green led on).
8+
Now the user can start/stop other recordings of the IMU data using again the double tap.
9+
The log files are saved in flash with an increasing number suffix data_0.txt, data_1.txt, etc.
10+
If you want to transfer the log files to the PC, you can reset the board and
11+
wait for 10 seconds (blue led blinking).
12+
You can find the video tutorial on LSM6DSOX MLC at: https://docs.arduino.cc/tutorials/nano-rp2040-connect/rp2040-imu-advanced
13+
*/
14+
15+
#include "PluggableUSBMSD.h"
16+
#include "FlashIAPBlockDevice.h"
17+
#include "WiFiNINA.h"
18+
#include "LSM6DSOXSensor.h"
19+
20+
#define INT_1 INT_IMU
21+
#define SENSOR_ODR 104.0f // In Hertz
22+
#define ACC_FS 2 // In g
23+
#define GYR_FS 2000 // In dps
24+
#define MEASUREMENT_TIME_INTERVAL (1000.0f/SENSOR_ODR) // In ms
25+
#define FIFO_SAMPLE_THRESHOLD 199
26+
#define FLASH_BUFF_LEN 8192
27+
28+
typedef enum {
29+
DATA_STORAGE_STATE,
30+
DATA_LOGGER_IDLE_STATE,
31+
DATA_LOGGER_RUNNING_STATE
32+
} demo_state_e;
33+
34+
volatile demo_state_e demo_state = DATA_STORAGE_STATE;
35+
volatile int mems_event = 0;
36+
uint32_t file_count = 0;
37+
unsigned long timestamp_count = 0;
38+
bool acc_available = false;
39+
bool gyr_available = false;
40+
int32_t acc_value[3];
41+
int32_t gyr_value[3];
42+
char buff[FLASH_BUFF_LEN];
43+
uint32_t pos = 0;
44+
45+
static FlashIAPBlockDevice bd(XIP_BASE + 0x100000, 0x100000);
46+
47+
LSM6DSOXSensor AccGyr(&Wire, LSM6DSOX_I2C_ADD_L);
48+
49+
USBMSD MassStorage(&bd);
50+
51+
rtos::Thread acquisition_th;
52+
53+
FILE *f = nullptr;
54+
55+
void INT1Event_cb()
56+
{
57+
mems_event = 1;
58+
}
59+
60+
void USBMSD::begin()
61+
{
62+
int err = getFileSystem().mount(&bd);
63+
if (err) {
64+
err = getFileSystem().reformat(&bd);
65+
}
66+
}
67+
68+
mbed::FATFileSystem &USBMSD::getFileSystem()
69+
{
70+
static mbed::FATFileSystem fs("fs");
71+
return fs;
72+
}
73+
74+
void led_green_thd()
75+
{
76+
while (1) {
77+
if (demo_state == DATA_LOGGER_RUNNING_STATE) {
78+
digitalWrite(LEDG, HIGH);
79+
delay(100);
80+
digitalWrite(LEDG, LOW);
81+
delay(100);
82+
}
83+
}
84+
}
85+
86+
void Read_FIFO_Data(uint16_t samples_to_read)
87+
{
88+
uint16_t i;
89+
90+
for (i = 0; i < samples_to_read; i++) {
91+
uint8_t tag;
92+
// Check the FIFO tag
93+
AccGyr.Get_FIFO_Tag(&tag);
94+
switch (tag) {
95+
// If we have a gyro tag, read the gyro data
96+
case LSM6DSOX_GYRO_NC_TAG: {
97+
AccGyr.Get_FIFO_G_Axes(gyr_value);
98+
gyr_available = true;
99+
break;
100+
}
101+
// If we have an acc tag, read the acc data
102+
case LSM6DSOX_XL_NC_TAG: {
103+
AccGyr.Get_FIFO_X_Axes(acc_value);
104+
acc_available = true;
105+
break;
106+
}
107+
// We can discard other tags
108+
default: {
109+
break;
110+
}
111+
}
112+
// If we have the measurements of both acc and gyro, we can store them with timestamp
113+
if (acc_available && gyr_available) {
114+
int num_bytes;
115+
num_bytes = snprintf(&buff[pos], (FLASH_BUFF_LEN - pos), "%lu %d %d %d %d %d %d\n", (unsigned long)((float)timestamp_count * MEASUREMENT_TIME_INTERVAL), (int)acc_value[0], (int)acc_value[1], (int)acc_value[2], (int)gyr_value[0], (int)gyr_value[1], (int)gyr_value[2]);
116+
pos += num_bytes;
117+
timestamp_count++;
118+
acc_available = false;
119+
gyr_available = false;
120+
}
121+
}
122+
// We can add the termination character to the string, so we are ready to save it in flash
123+
buff[pos] = '\0';
124+
pos = 0;
125+
}
126+
127+
void setup()
128+
{
129+
Serial.begin(115200);
130+
MassStorage.begin();
131+
pinMode(LEDB, OUTPUT);
132+
pinMode(LEDG, OUTPUT);
133+
digitalWrite(LEDB, LOW);
134+
digitalWrite(LEDG, LOW);
135+
136+
// Initialize I2C bus.
137+
Wire.begin();
138+
Wire.setClock(400000);
139+
140+
//Interrupts.
141+
attachInterrupt(INT_1, INT1Event_cb, RISING);
142+
143+
// Initialize IMU.
144+
AccGyr.begin();
145+
AccGyr.Enable_X();
146+
AccGyr.Enable_G();
147+
// Configure ODR and FS of the acc and gyro
148+
AccGyr.Set_X_ODR(SENSOR_ODR);
149+
AccGyr.Set_X_FS(ACC_FS);
150+
AccGyr.Set_G_ODR(SENSOR_ODR);
151+
AccGyr.Set_G_FS(GYR_FS);
152+
// Enable the Double Tap event
153+
AccGyr.Enable_Double_Tap_Detection(LSM6DSOX_INT1_PIN);
154+
// Configure FIFO BDR for acc and gyro
155+
AccGyr.Set_FIFO_X_BDR(SENSOR_ODR);
156+
AccGyr.Set_FIFO_G_BDR(SENSOR_ODR);
157+
// Start Led blinking thread
158+
acquisition_th.start(led_green_thd);
159+
}
160+
161+
void loop()
162+
{
163+
164+
if (mems_event) {
165+
mems_event = 0;
166+
LSM6DSOX_Event_Status_t status;
167+
AccGyr.Get_X_Event_Status(&status);
168+
if (status.DoubleTapStatus) {
169+
switch (demo_state) {
170+
case DATA_STORAGE_STATE: {
171+
// Go to DATA_LOGGER_IDLE_STATE state
172+
demo_state = DATA_LOGGER_IDLE_STATE;
173+
digitalWrite(LEDG, HIGH);
174+
Serial.println("From DATA_STORAGE_STATE To DATA_LOGGER_IDLE_STATE");
175+
break;
176+
}
177+
case DATA_LOGGER_IDLE_STATE: {
178+
char filename[32];
179+
// Go to DATA_LOGGER_RUNNING_STATE state
180+
snprintf(filename, 32, "/fs/data_%lu.txt", file_count);
181+
Serial.print("Start writing file ");
182+
Serial.println(filename);
183+
// open a file to write some data
184+
// w+ means overwrite, so every time the board is rebooted the file will be overwritten
185+
f = fopen(filename, "w+");
186+
if (f != nullptr) {
187+
// write header
188+
fprintf(f, "Timestamp[ms] A_X [mg] A_Y [mg] A_Z [mg] G_X [mdps] G_Y [mdps] G_Z [mdps]\n");
189+
fflush(f);
190+
Serial.println("From DATA_LOGGER_IDLE_STATE To DATA_LOGGER_RUNNING_STATE");
191+
demo_state = DATA_LOGGER_RUNNING_STATE;
192+
digitalWrite(LEDG, LOW);
193+
timestamp_count = 0;
194+
pos = 0;
195+
acc_available = false;
196+
gyr_available = false;
197+
// Set FIFO in Continuous mode
198+
AccGyr.Set_FIFO_Mode(LSM6DSOX_STREAM_MODE);
199+
}
200+
break;
201+
}
202+
case DATA_LOGGER_RUNNING_STATE: {
203+
// Empty the FIFO
204+
uint16_t fifo_samples;
205+
AccGyr.Get_FIFO_Num_Samples(&fifo_samples);
206+
Read_FIFO_Data(fifo_samples);
207+
// Store the string in flash
208+
fprintf(f, "%s", buff);
209+
fflush(f);
210+
211+
// Close the log file and increase the counter
212+
fclose(f);
213+
file_count++;
214+
// Set FIFO in Bypass mode
215+
AccGyr.Set_FIFO_Mode(LSM6DSOX_BYPASS_MODE);
216+
// Go to DATA_LOGGER_IDLE_STATE state
217+
demo_state = DATA_LOGGER_IDLE_STATE;
218+
// Wait for the led thread ends the blinking
219+
delay(250);
220+
digitalWrite(LEDG, HIGH);
221+
Serial.println("From DATA_LOGGER_RUNNING_STATE To DATA_LOGGER_IDLE_STATE");
222+
break;
223+
}
224+
default:
225+
Serial.println("Error! Invalid state");
226+
}
227+
}
228+
}
229+
230+
if (demo_state == DATA_LOGGER_RUNNING_STATE) {
231+
uint16_t fifo_samples;
232+
233+
// Check the number of samples inside FIFO
234+
AccGyr.Get_FIFO_Num_Samples(&fifo_samples);
235+
236+
// Serial.println(fifo_samples);
237+
238+
// If we reach the threshold we can empty the FIFO
239+
if (fifo_samples > FIFO_SAMPLE_THRESHOLD) {
240+
// Empty the FIFO
241+
Read_FIFO_Data(fifo_samples);
242+
// Store the string in flash
243+
fprintf(f, "%s", buff);
244+
fflush(f);
245+
}
246+
}
247+
248+
if (demo_state == DATA_STORAGE_STATE && millis() > 10000) {
249+
// Disable the sensor and go to Mass Storage mode
250+
AccGyr.Disable_Double_Tap_Detection();
251+
AccGyr.Disable_X();
252+
AccGyr.Disable_G();
253+
while (1) {
254+
digitalWrite(LEDB, HIGH);
255+
delay(100);
256+
digitalWrite(LEDB, LOW);
257+
delay(100);
258+
}
259+
}
260+
}

‎libraries/NanoRP2040Connect_MLC/examples/RP2040_MLC_Motion_Intensity/RP2040_MLC_Motion_Intensity.ino

Lines changed: 137 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,137 @@
1+
/*
2+
This example shows how to load the MLC bytecode for Motion Intensity on LSM6DSOX
3+
of the Arduino Nano RP2040 Connect.
4+
You can find the video tutorial on LSM6DSOX MLC at: https://docs.arduino.cc/tutorials/nano-rp2040-connect/rp2040-imu-advanced
5+
*/
6+
7+
// Includes
8+
#include "WiFiNINA.h"
9+
#include "LSM6DSOXSensor.h"
10+
#include "lsm6dsox_motion_intensity.h"
11+
12+
#define INT_1 INT_IMU
13+
14+
//Interrupts.
15+
volatile int mems_event = 0;
16+
17+
// Components
18+
LSM6DSOXSensor AccGyr(&Wire, LSM6DSOX_I2C_ADD_L);
19+
20+
// MLC
21+
ucf_line_t *ProgramPointer;
22+
int32_t LineCounter;
23+
int32_t TotalNumberOfLine;
24+
25+
void INT1Event_cb();
26+
void printMLCStatus(uint8_t status);
27+
28+
void setup()
29+
{
30+
uint8_t mlc_out[8];
31+
// Led.
32+
pinMode(LEDB, OUTPUT);
33+
pinMode(LEDG, OUTPUT);
34+
pinMode(LEDR, OUTPUT);
35+
digitalWrite(LEDB, LOW);
36+
digitalWrite(LEDG, LOW);
37+
digitalWrite(LEDR, LOW);
38+
39+
// Initialize serial for output.
40+
Serial.begin(115200);
41+
42+
// Initialize I2C bus.
43+
Wire.begin();
44+
45+
AccGyr.begin();
46+
47+
/* Feed the program to Machine Learning Core */
48+
/* Motion Intensity Default program */
49+
ProgramPointer = (ucf_line_t *)lsm6dsox_motion_intensity;
50+
TotalNumberOfLine = sizeof(lsm6dsox_motion_intensity) / sizeof(ucf_line_t);
51+
Serial.println("Motion Intensity for LSM6DSOX MLC");
52+
Serial.print("UCF Number Line=");
53+
Serial.println(TotalNumberOfLine);
54+
55+
for (LineCounter = 0; LineCounter < TotalNumberOfLine; LineCounter++) {
56+
if (AccGyr.Write_Reg(ProgramPointer[LineCounter].address, ProgramPointer[LineCounter].data)) {
57+
Serial.print("Error loading the Program to LSM6DSOX at line: ");
58+
Serial.println(LineCounter);
59+
while (1) {
60+
// Led blinking.
61+
digitalWrite(LED_BUILTIN, HIGH);
62+
delay(250);
63+
digitalWrite(LED_BUILTIN, LOW);
64+
delay(250);
65+
}
66+
}
67+
}
68+
69+
Serial.println("Program loaded inside the LSM6DSOX MLC");
70+
71+
AccGyr.Enable_X();
72+
AccGyr.Set_X_ODR(104.0f);
73+
AccGyr.Set_X_FS(2);
74+
75+
//Interrupts.
76+
pinMode(INT_1, INPUT);
77+
attachInterrupt(INT_1, INT1Event_cb, RISING);
78+
}
79+
80+
void loop()
81+
{
82+
if (mems_event) {
83+
mems_event = 0;
84+
LSM6DSOX_MLC_Status_t status;
85+
AccGyr.Get_MLC_Status(&status);
86+
if (status.is_mlc1) {
87+
uint8_t mlc_out[8];
88+
AccGyr.Get_MLC_Output(mlc_out);
89+
printMLCStatus(mlc_out[0]);
90+
}
91+
}
92+
}
93+
94+
void INT1Event_cb()
95+
{
96+
mems_event = 1;
97+
}
98+
99+
void printMLCStatus(uint8_t status)
100+
{
101+
switch (status) {
102+
case 1:
103+
// Reset leds status
104+
digitalWrite(LEDB, LOW);
105+
digitalWrite(LEDG, LOW);
106+
digitalWrite(LEDR, LOW);
107+
// LEDB On
108+
digitalWrite(LEDB, HIGH);
109+
Serial.println("Stationary");
110+
break;
111+
case 4:
112+
// Reset leds status
113+
digitalWrite(LEDB, LOW);
114+
digitalWrite(LEDG, LOW);
115+
digitalWrite(LEDR, LOW);
116+
// LEDG On
117+
digitalWrite(LEDG, HIGH);
118+
Serial.println("Medium Intensity");
119+
break;
120+
case 8:
121+
// Reset leds status
122+
digitalWrite(LEDB, LOW);
123+
digitalWrite(LEDG, LOW);
124+
digitalWrite(LEDR, LOW);
125+
// LEDR On
126+
digitalWrite(LEDR, HIGH);
127+
Serial.println("High Intensity");
128+
break;
129+
default:
130+
// Reset leds status
131+
digitalWrite(LEDB, LOW);
132+
digitalWrite(LEDG, LOW);
133+
digitalWrite(LEDR, LOW);
134+
Serial.println("Unknown");
135+
break;
136+
}
137+
}

0 commit comments

Comments
 (0)