Merge pull request #60 from manchoz/main

giulcioffi · web-flow · commit 77e0f0327fa4 · 2022-07-12T16:19:20.000+02:00
Add the DataHarvester sketch and companion py app
diff --git a/examples/DataHarvester/DataHarvester.ino b/examples/DataHarvester/DataHarvester.ino
@@ -0,0 +1,117 @@
+/*
+    This sketch shows how the use the Nicla Sense ME to collect
+    acceleration data at maximum speed, and how to send them to
+    a companion app via UART.
+
+    It uses a COBS-based, packed-oriented serial protocol to establish
+    a reliable communication to the companion app, to receive operation
+    commands and to send collected data.
+
+    Requirements:
+    - Nicla Sense ME
+    - PacketSerial library https://github.com/bakercp/PacketSerial
+    - Commander.py companion app
+*/
+
+#include <Nicla_System.h>
+#include <Arduino_BHY2.h>
+
+// Manage the communication with the companion app
+#include <PacketSerial.h>
+PacketSerial myPacketSerial;
+
+auto capture { false };
+
+// Send data as packed struct, ie. as the raw bytes
+struct __attribute__((__packed__)) Data {
+    float ts;
+    float x;
+    float y;
+    float z;
+};
+
+SensorXYZ accel(SENSOR_ID_ACC_PASS);
+
+void setup()
+{
+    nicla::begin();
+    nicla::leds.begin();
+    BHY2.begin();
+    accel.begin();
+
+    // Init the PacketSerial communication
+    myPacketSerial.begin(115200);
+    // Set the function for handling commands from the companion app
+    myPacketSerial.setPacketHandler(&onPacketReceived);
+
+    pinMode(LED_BUILTIN, OUTPUT);
+    for (auto i = 0u; i < 10; i++) {
+        nicla::leds.setColor(green);
+        delay(25);
+        nicla::leds.setColor(off);
+        delay(25);
+    }
+}
+
+void loop()
+{
+    // Update communication-channel and sensors
+    myPacketSerial.update();
+    BHY2.update();
+
+    // Check for a receive buffer overflow (optional).
+    if (myPacketSerial.overflow()) {
+        for (auto i = 0u; i < 5; i++) {
+            nicla::leds.setColor(green);
+            delay(25);
+            nicla::leds.setColor(off);
+            delay(25);
+        }
+    }
+
+    // Capture and send data as soon as we read it
+    if (capture == true) {
+        auto now = micros() / 1000.0;
+
+        // Collect data from accel sensor
+        Data data { now, accel.x(), accel.y(), accel.z() };
+        constexpr size_t dataBufLen { sizeof(Data) };
+        uint8_t dataBuf[dataBufLen] {};
+
+        // Convert the Data struct to an array of bytes
+        memcpy(dataBuf, reinterpret_cast<void*>(&data), dataBufLen);
+        
+        // Send data
+        myPacketSerial.send(dataBuf, dataBufLen);
+    }
+}
+
+
+// Parse commands from the companion app
+void onPacketReceived(const uint8_t* buffer, size_t size)
+{
+    uint8_t tempBuffer[size];
+
+    for (auto i = 0u; i < 2; i++) {
+        nicla::leds.setColor(green);
+        delay(25);
+        nicla::leds.setColor(off);
+        delay(25);
+    }
+
+    memcpy(tempBuffer, buffer, size);
+
+    switch (tempBuffer[0]) {
+    case 'R':
+        nicla::leds.setColor(green);
+        capture = true;
+        break;
+    case 'S':
+        nicla::leds.setColor(off);
+        capture = false;
+        break;
+
+    default:
+        break;
+    }
+}
diff --git a/examples/DataHarvester/extras/Commander/Commander.py b/examples/DataHarvester/extras/Commander/Commander.py
@@ -0,0 +1,150 @@
+#!/usr/bin/env python3
+#
+# This scripts is the companion application for the DataHarvester sketch.
+# It is a very basic application and can be improved and extended.
+#
+# It waits for user input to control the data acquisition operations and the
+# connection to the Arduino board and to save the received data to a CSV file.
+#
+# It establish a serial communication to the board and uses COBS as enconding and
+# reliable transport protocol.
+#
+# The main thread will always wait for user input while a second one is created
+# to exchange both control and data with the Arduino board.
+# The two threads coordinate via a simple Queue.
+
+# Thread and Queue
+from threading import Thread
+import queue
+
+# Helpers
+from datetime import date, datetime
+import struct
+
+# User input
+import readline
+import argparse
+
+# Communication with the Arduino board
+import serial
+from cobs import cobs
+
+
+# The main serial object
+# Initialise later
+ser = serial.Serial()
+
+# The function to be run in the Arduino-facing thread
+def receiver(cmd_q, uart, name, tag):
+    f = None
+    running = False
+    cmd = 'N'
+
+    while True:
+        # Check for commands from the main thread
+        try:
+            cmd = cmd_q.get(block=False)
+        except queue.Empty:
+            pass
+
+        # Start acquisition, prepare output file, etc.
+        if cmd == 'R':
+            running = True
+            today = datetime.today()
+            iso = datetime.isoformat(today)
+            filename = f'{name}_{tag}_{iso}.csv'
+            f = open(filename, 'wt')
+            print(f'timestamp,accX,accY,accZ', file=f)
+            cmd = 'N'
+        # Stop acquisition
+        elif cmd == 'S':
+            running = False
+            if f != None:
+                f.close()
+            cmd = 'N'
+        # Close connection
+        elif cmd == 'C':
+            running = False
+        # Quit program
+        elif cmd == 'Q':
+            running = False
+            if f != None:
+                if not f.closed:
+                    f.close()
+            break
+
+        # Receive data packet-by-packet and save to file
+        if running:
+            if uart.is_open:
+                # Read full COBS packet
+                data = uart.read_until(b'\x00')
+                n = len(data)
+                if n > 0:
+                    # Skip last byte
+                    decoded = cobs.decode(data[0:(n - 1)])
+                    # Unpack the binary data
+                    ts, x, y, z = struct.unpack('ffff', decoded)
+                    # Create CSV line and print
+                    print(f'{ts},{x},{y},{z}', file=f)
+                    f.flush()
+
+
+# The main thread
+if __name__ == '__main__':
+
+    # Parse arugments
+    # - port == Arduino Serial Port
+    # - name == Filename
+    # - tag == Custom tag to append to filename
+    parser = argparse.ArgumentParser()
+    parser.add_argument('port')
+    parser.add_argument('name')
+    parser.add_argument('tag')
+    args = parser.parse_args()
+
+    ser.port = args.port
+    ser.baudrate = 115200
+
+    ser.close()
+    ser.open()
+
+    # Inter-thread communication queue
+    q = queue.Queue()
+
+    # Arduino-facing thread
+    recv_thread = Thread(target=receiver, args=(q, ser, args.name, args.tag))
+    recv_thread.start()
+
+    # Wait for user input
+    while True:
+        s = input("> ").upper()
+
+        # Send commands to receiver thread
+        if s == 'S':
+            print('Stopping...')
+            q.put('S')
+        elif s == 'R':
+            print('Running...')
+            q.put('R')
+        elif s == 'C':
+            print('Closing...')
+            q.put('C')
+            if ser.is_open:
+                ser.close()
+        elif s == 'O':
+            print('Opening...')
+            if not ser.is_open:
+                ser.open()
+        elif s == 'Q':
+            print('Quitting...')
+            q.put('Q')
+            recv_thread.join()
+            if ser.is_open:
+                ser.close()
+            break
+
+        # Send commands to the Arduino board
+        if ser.is_open:
+            enc = cobs.encode(s.encode())
+            ser.write(enc)
+            ser.write(b'\x00')
diff --git a/examples/DataHarvester/extras/Commander/requirements.txt b/examples/DataHarvester/extras/Commander/requirements.txt
@@ -0,0 +1,2 @@
+pyserial
+cobs
