diff --git a/Arduino_BHY2/examples/SyncAndCollectIMUData/.reuse/dep5 b/Arduino_BHY2/examples/SyncAndCollectIMUData/.reuse/dep5 new file mode 100644 index 00000000..ac0b60f2 --- /dev/null +++ b/Arduino_BHY2/examples/SyncAndCollectIMUData/.reuse/dep5 @@ -0,0 +1,9 @@ +Format: https://www.debian.org/doc/packaging-manuals/copyright-format/1.0/ + +Upstream-Name: SyncAndCollectIMUData.ino +Upstream-Contact: Lucas Guo +Source: + +Files: base64.hpp +Copyright: Copyright (c) 2016 Densauge +License: MIT diff --git a/Arduino_BHY2/examples/SyncAndCollectIMUData/LICENSES/MIT.txt b/Arduino_BHY2/examples/SyncAndCollectIMUData/LICENSES/MIT.txt new file mode 100644 index 00000000..1cef10b5 --- /dev/null +++ b/Arduino_BHY2/examples/SyncAndCollectIMUData/LICENSES/MIT.txt @@ -0,0 +1,21 @@ +The MIT License (MIT) + +Copyright (c) 2016 Densaugeo + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/Arduino_BHY2/examples/SyncAndCollectIMUData/NiclaSenseME-nobsx-1600hz-IMU-passthrough-flash.fw b/Arduino_BHY2/examples/SyncAndCollectIMUData/NiclaSenseME-nobsx-1600hz-IMU-passthrough-flash.fw new file mode 100644 index 00000000..6f9db77e Binary files /dev/null and b/Arduino_BHY2/examples/SyncAndCollectIMUData/NiclaSenseME-nobsx-1600hz-IMU-passthrough-flash.fw differ diff --git a/Arduino_BHY2/examples/SyncAndCollectIMUData/README.md b/Arduino_BHY2/examples/SyncAndCollectIMUData/README.md new file mode 100644 index 00000000..59320d1f --- /dev/null +++ b/Arduino_BHY2/examples/SyncAndCollectIMUData/README.md @@ -0,0 +1,19 @@ +# Overview +This example shows how to stream sensor data with very high data rate (up to 1600hz for both accel and gyro) through the USB Serial interface to the host (PC) + +#prerequisite +## Host Side +To stream both accel and gyro data at high frequency (e.g.: 800hz or above), +the host PC needs to be able to support USB serial baud rate of 1Mbps or at least 921600bps which are widely available for regular PCs, +however, it is observed that on Mac OS based devices, the baud rate seems to be capped at 230400bps. + +If only one sensor needs to be streamed, the clock rate on the host side could be relaxed by about a half. + +## Device Side +The default device FW for the BHI260AP device supports sensor data output rate of up to 400hz, to support higher rates, the firmware needs to be updated, +and for the convenience of the users, a stock firmware ("NiclaSenseME-nobsx-1600hz-IMU-passthrough-flash.fw") for this purpose has been provided at the same +folder with this README, please refer to the example sketch BHYFirmwareUpdate within the Arduino_BHY2 library for the steps of updating the BHI260AP firmware. + +# The "tools" Folder +For reliable data transfer at high data rates, the data is encoded in base64 format, and to help convert the data (captured at the host side) back to its original format, some helper tools have been provided for the users' convenience. +Please refer to the README.md under the "tools" folder for more details. diff --git a/Arduino_BHY2/examples/SyncAndCollectIMUData/SyncAndCollectIMUData.ino b/Arduino_BHY2/examples/SyncAndCollectIMUData/SyncAndCollectIMUData.ino new file mode 100644 index 00000000..59ad6329 --- /dev/null +++ b/Arduino_BHY2/examples/SyncAndCollectIMUData/SyncAndCollectIMUData.ino @@ -0,0 +1,259 @@ +/* + * This sketch shows how nicla can be used in standalone mode. + * Without the need for an host, nicla can run sketches that + * are able to configure the bhi sensors and are able to read all + * the bhi sensors data. + */ + +#include "Arduino.h" +#include "Arduino_BHY2.h" + +#include "base64.hpp" //remember to include the library "base64 by Densaugeo" from Library manager + +#define PDEBUG 0 + +#if PDEBUG +#include "MbedQDebug.h" +#else +void *nicla_dbg_info_ptr; +#endif + + +class IMUDataSyncer; +class SensorMotion; + +enum { + IMU_SENSOR_ID_ACC = 0, + IMU_SENSOR_ID_GYR, +}; + +class IMUDataSyncer { + public: + IMUDataSyncer() { + uint8_t i; + for (i = 0; i < sizeof(_seq)/sizeof(_seq[0]); i++) { + _seq[i] = -1; + } + } + + //@param outputMetaInfo enable this if you want to send the sequence number in each line of output, + //when outputInBase64 is false, for highest ODR like 1600hz, ignore this to save bandwidth + virtual bool begin(int accelSampleRate, int gyroSampleRate, bool outputInBase64 = false, bool outputMetaInfo = true) { + //int sampleRate = accelSampleRate > gyroSampleRate ? accelSampleRate : gyroSampleRate; + _outputInBase64 = outputInBase64; + _outputMetaInfo = outputMetaInfo; + if ((accelSampleRate > 0) && (gyroSampleRate > 0)) { + if (accelSampleRate != gyroSampleRate) { + //'u' means unaliged data rate + _logId = 'u'; //for simplicity, we do not sync acc and gyro data when the rates are not the same + return false; + } else { + _logId = 'i'; + } + } else if (accelSampleRate > 0) { + _logId = 'a'; + } else if (gyroSampleRate > 0){ + _logId = 'g'; + } else { + _logId = 'n'; //null + } + + return true; + } + + virtual void end() { + } + + void onSensorDataUpdate(SensorDataPacket &data, uint8_t id) { + const uint8_t IDS_ALL = (1<= 0) { + _seq[id] = (_seq[id]+1) % 10; + } else { + _seq[id] = 0; //this is to wait for another sensor to generate the 1st sample + } + + if (_seq[id] == _seq[1 - id]) { + //data synced well and ready for sending out + outputData(IDS_ALL); + } + } else { + //we are only collecting data for a single sensor + _seq[id] = (_seq[id]+1) % 10; + outputData(1<onSensorDataUpdate(data, _id); + } + + void setData(SensorLongDataPacket &data) {} + + String toString() { + //we are delegating the data parsing to the _dataSyncer + return (""); + } + + protected: + //DataXYZ _data; + uint8_t _id; + IMUDataSyncer *_dataSyncer; +}; + + +SensorMotion accel(1); //sensor id 1: accel raw data passthrough +SensorMotion gyro(10); //sensor id 10: gyro raw data passthrough +IMUDataSyncer imuDataSyncer; + + +#define DATA_RATE_ACC 1600 +#define DATA_RATE_GYR 1600 + + +void testSerial() +{ +#if PDEBUG + while (0) { + mbq_dbg_1(1); + Serial.println("0123456789"); + mbq_dbg_1(0); + delay(10); + } +#endif +} + +void setup() +{ + //Serial.begin(115200); + Serial.begin(1000000); //max br: 1Mbps for nRF52 + while(!Serial); + + BHY2.begin(NICLA_STANDALONE); + + testSerial(); + + SensorConfig cfg = accel.getConfiguration(); + Serial.println(String("range of accel: +/-") + cfg.range + String("g")); + accel.setRange(8); //this sets the range of accel to +/-8g, + cfg = accel.getConfiguration(); + Serial.println(String("range of accel: +/-") + cfg.range + String("g")); + + + //imuDataSyncer.begin(DATA_RATE_ACC, DATA_RATE_GYR, false); + imuDataSyncer.begin(DATA_RATE_ACC, DATA_RATE_GYR, true); + accel.setDataSyncer(imuDataSyncer); + gyro.setDataSyncer(imuDataSyncer); + accel.begin(DATA_RATE_ACC); + gyro.begin(DATA_RATE_GYR); +} + +void loop() +{ +#if PDEBUG + mbq_dbg_0(1); +#endif + + BHY2.update(); + +#if PDEBUG + mbq_dbg_0(0); +#endif +} diff --git a/Arduino_BHY2/examples/SyncAndCollectIMUData/base64.hpp b/Arduino_BHY2/examples/SyncAndCollectIMUData/base64.hpp new file mode 100644 index 00000000..60b2f9ff --- /dev/null +++ b/Arduino_BHY2/examples/SyncAndCollectIMUData/base64.hpp @@ -0,0 +1,221 @@ +/** + * Base64 encoding and decoding of strings. Uses '+' for 62, '/' for 63, '=' for padding + */ + +#ifndef BASE64_H_INCLUDED +#define BASE64_H_INCLUDED + +/* binary_to_base64: + * Description: + * Converts a single byte from a binary value to the corresponding base64 character + * Parameters: + * v - Byte to convert + * Returns: + * ascii code of base64 character. If byte is >= 64, then there is not corresponding base64 character + * and 255 is returned + */ +unsigned char binary_to_base64(unsigned char v); + +/* base64_to_binary: + * Description: + * Converts a single byte from a base64 character to the corresponding binary value + * Parameters: + * c - Base64 character (as ascii code) + * Returns: + * 6-bit binary value + */ +unsigned char base64_to_binary(unsigned char c); + +/* encode_base64_length: + * Description: + * Calculates length of base64 string needed for a given number of binary bytes + * Parameters: + * input_length - Amount of binary data in bytes + * Returns: + * Number of base64 characters needed to encode input_length bytes of binary data + */ +unsigned int encode_base64_length(unsigned int input_length); + +/* decode_base64_length: + * Description: + * Calculates number of bytes of binary data in a base64 string + * Variant that does not use input_length no longer used within library, retained for API compatibility + * Parameters: + * input - Base64-encoded null-terminated string + * input_length (optional) - Number of bytes to read from input pointer + * Returns: + * Number of bytes of binary data in input + */ +unsigned int decode_base64_length(unsigned char input[]); +unsigned int decode_base64_length(unsigned char input[], unsigned int input_length); + +/* encode_base64: + * Description: + * Converts an array of bytes to a base64 null-terminated string + * Parameters: + * input - Pointer to input data + * input_length - Number of bytes to read from input pointer + * output - Pointer to output string. Null terminator will be added automatically + * Returns: + * Length of encoded string in bytes (not including null terminator) + */ +unsigned int encode_base64(unsigned char input[], unsigned int input_length, unsigned char output[]); + +/* decode_base64: + * Description: + * Converts a base64 null-terminated string to an array of bytes + * Parameters: + * input - Pointer to input string + * input_length (optional) - Number of bytes to read from input pointer + * output - Pointer to output array + * Returns: + * Number of bytes in the decoded binary + */ +unsigned int decode_base64(unsigned char input[], unsigned char output[]); +unsigned int decode_base64(unsigned char input[], unsigned int input_length, unsigned char output[]); + +unsigned char binary_to_base64(unsigned char v) { + // Capital letters - 'A' is ascii 65 and base64 0 + if(v < 26) return v + 'A'; + + // Lowercase letters - 'a' is ascii 97 and base64 26 + if(v < 52) return v + 71; + + // Digits - '0' is ascii 48 and base64 52 + if(v < 62) return v - 4; + + #ifdef BASE64_URL + // '-' is ascii 45 and base64 62 + if(v == 62) return '-'; + #else + // '+' is ascii 43 and base64 62 + if(v == 62) return '+'; + #endif + + #ifdef BASE64_URL + // '_' is ascii 95 and base64 62 + if(v == 63) return '_'; + #else + // '/' is ascii 47 and base64 63 + if(v == 63) return '/'; + #endif + + return 64; +} + +unsigned char base64_to_binary(unsigned char c) { + // Capital letters - 'A' is ascii 65 and base64 0 + if('A' <= c && c <= 'Z') return c - 'A'; + + // Lowercase letters - 'a' is ascii 97 and base64 26 + if('a' <= c && c <= 'z') return c - 71; + + // Digits - '0' is ascii 48 and base64 52 + if('0' <= c && c <= '9') return c + 4; + + #ifdef BASE64_URL + // '-' is ascii 45 and base64 62 + if(c == '-') return 62; + #else + // '+' is ascii 43 and base64 62 + if(c == '+') return 62; + #endif + + #ifdef BASE64_URL + // '_' is ascii 95 and base64 62 + if(c == '_') return 63; + #else + // '/' is ascii 47 and base64 63 + if(c == '/') return 63; + #endif + + return 255; +} + +unsigned int encode_base64_length(unsigned int input_length) { + return (input_length + 2)/3*4; +} + +unsigned int decode_base64_length(unsigned char input[]) { + return decode_base64_length(input, -1); +} + +unsigned int decode_base64_length(unsigned char input[], unsigned int input_length) { + unsigned char *start = input; + + while(base64_to_binary(input[0]) < 64 && (unsigned int) (input - start) < input_length) { + ++input; + } + + input_length = (unsigned int) (input - start); + return input_length/4*3 + (input_length % 4 ? input_length % 4 - 1 : 0); +} + +unsigned int encode_base64(unsigned char input[], unsigned int input_length, unsigned char output[]) { + unsigned int full_sets = input_length/3; + + // While there are still full sets of 24 bits... + for(unsigned int i = 0; i < full_sets; ++i) { + output[0] = binary_to_base64( input[0] >> 2); + output[1] = binary_to_base64((input[0] & 0x03) << 4 | input[1] >> 4); + output[2] = binary_to_base64((input[1] & 0x0F) << 2 | input[2] >> 6); + output[3] = binary_to_base64( input[2] & 0x3F); + + input += 3; + output += 4; + } + + switch(input_length % 3) { + case 0: + output[0] = '\0'; + break; + case 1: + output[0] = binary_to_base64( input[0] >> 2); + output[1] = binary_to_base64((input[0] & 0x03) << 4); + output[2] = '='; + output[3] = '='; + output[4] = '\0'; + break; + case 2: + output[0] = binary_to_base64( input[0] >> 2); + output[1] = binary_to_base64((input[0] & 0x03) << 4 | input[1] >> 4); + output[2] = binary_to_base64((input[1] & 0x0F) << 2); + output[3] = '='; + output[4] = '\0'; + break; + } + + return encode_base64_length(input_length); +} + +unsigned int decode_base64(unsigned char input[], unsigned char output[]) { + return decode_base64(input, -1, output); +} + +unsigned int decode_base64(unsigned char input[], unsigned int input_length, unsigned char output[]) { + unsigned int output_length = decode_base64_length(input, input_length); + + // While there are still full sets of 24 bits... + for(unsigned int i = 2; i < output_length; i += 3) { + output[0] = base64_to_binary(input[0]) << 2 | base64_to_binary(input[1]) >> 4; + output[1] = base64_to_binary(input[1]) << 4 | base64_to_binary(input[2]) >> 2; + output[2] = base64_to_binary(input[2]) << 6 | base64_to_binary(input[3]); + + input += 4; + output += 3; + } + + switch(output_length % 3) { + case 1: + output[0] = base64_to_binary(input[0]) << 2 | base64_to_binary(input[1]) >> 4; + break; + case 2: + output[0] = base64_to_binary(input[0]) << 2 | base64_to_binary(input[1]) >> 4; + output[1] = base64_to_binary(input[1]) << 4 | base64_to_binary(input[2]) >> 2; + break; + } + + return output_length; +} + +#endif // ifndef diff --git a/Arduino_BHY2/examples/SyncAndCollectIMUData/tools/README.md b/Arduino_BHY2/examples/SyncAndCollectIMUData/tools/README.md new file mode 100644 index 00000000..d2cd8245 --- /dev/null +++ b/Arduino_BHY2/examples/SyncAndCollectIMUData/tools/README.md @@ -0,0 +1,41 @@ +# Usage of the scripts +## process_nicla_bhy2_log_base64.py +This script is used to convert the log encoded in base64 to ascii +### Usage +``` +./process_nicla_bhy2_log_base64.py [log_file_name] [options] +``` +The [log_file_name] is the log captured on the host side from the USB serial port. +[options] tells what kind of data is embedded in the base64-encoded data. +possible options are: + - "accel" + - "gyro" + - "meta" + - "accel+meta" + - "gyro+meta" + - "accel+gyro+meta" +### Example +./process_nicla_bhy2_log_base64.py log_nicla_bhy2.txt "accel+gyro+meta" + + +## check_for_data_loss.sh +This script is used to check for any potential data loss during the transfer, +and it reports some errors if it does find any data loss + +### Usage +``` +./check_for_data_loss.sh [OPTION] [log_file_name] +``` + +### Example +- Example 1 + ``` + ./check_for_data_loss.sh -b ./minicom.log + ``` + The above command check for the data loss using the log "./minicom.log" which is base on base64 encoding +- Example 2 + ``` + ./check_for_data_loss.sh -a ./minicom.log + ``` + The above command check for the data loss using the log "./minicom.log" which is base on ascii encoding + diff --git a/Arduino_BHY2/examples/SyncAndCollectIMUData/tools/check_for_data_loss.py b/Arduino_BHY2/examples/SyncAndCollectIMUData/tools/check_for_data_loss.py new file mode 100755 index 00000000..982d2dd9 --- /dev/null +++ b/Arduino_BHY2/examples/SyncAndCollectIMUData/tools/check_for_data_loss.py @@ -0,0 +1,73 @@ +#!/usr/bin/env python +# this script resamples the data from the log to 1/60hz + +import sys +import time +import datetime +import pandas as pd +import math + + +#DEBUG = True +DEBUG = False +SEP_COLUMN = ',' + +LINE_TO_SKIP = 500 + +DOWNSAMPLE_FACTOR=1 + +def PDBG(*args): + if DEBUG: + print("DEBUG:", *args, file=sys.stderr) + + +PDBG('Number of arguments:', len(sys.argv)) + +if (len(sys.argv) < 2): + raise BaseException("missing argument") + +filename_in = sys.argv[1] +file_out = None + +if (len(sys.argv) > 2): + filename_out = sys.argv[2] + file_out = open(filename_out, 'w') + sys.stdout = file_out + + +line_cnt = 1 #first row is the header +df_in = pd.read_csv(filename_in, sep=SEP_COLUMN) + +seq_last = None + + +for index, row in df_in.iterrows(): + line_cnt += 1 + if (line_cnt <= LINE_TO_SKIP): + continue + + seq = row['seq'] + if (seq_last is not None): + try: + delta = seq - seq_last + if (seq < seq_last): + delta += 10 + if (delta != 1): + if (line_cnt < len(df_in.index)): + print("error: line: ", line_cnt, " has data missing, delta:", delta, "seq:", seq) + else: + PDBG("last line: ignored") + seq_last = seq + except: + if (line_cnt < len(df_in.index)): + print("error: line: ", line_cnt, " has data missing") + else: + PDBG("last line: ignored") + pass + else: + PDBG("first row") + seq_last = seq + + +if file_out is not None: + file_out.close() diff --git a/Arduino_BHY2/examples/SyncAndCollectIMUData/tools/check_for_data_loss.sh b/Arduino_BHY2/examples/SyncAndCollectIMUData/tools/check_for_data_loss.sh new file mode 100755 index 00000000..c313492a --- /dev/null +++ b/Arduino_BHY2/examples/SyncAndCollectIMUData/tools/check_for_data_loss.sh @@ -0,0 +1,63 @@ +#!/bin/sh + + +log_encoding_is_base64=false + +if [ ":""$1" = ":-b" ] ; then + log_encoding_is_base64=true + echo "log is using base64 encoding" +elif [ ":""$1" = ":-a" ] ; then + log_encoding_is_base64=false + echo "log is using ascii encoding" +else + echo "usage: ./check_for_data_loss.sh [OPTION] [log_filename] [info_included_in_data]" + echo "\t[OPTION]:" + echo "\t\t" "-a" + echo "\t\t" "\t:log use ascii encoding" + echo "\t\t" "-b" + echo "\t\t" "\t:log use base64 encoding" + echo "\t[info_included_in_data]:" + echo "\t\t" '"accel" or "accel+meta"' + echo "\t\t" '"gyro" or "gyro+meta"' + echo "\t\t" '"accel+gyro" or "accel+gyro+meta"' + + + echo "\texample: ./check_for_data_loss.sh -b minicom.cap accel+gyro+meta" + return +fi + +if [ ":""$2" = ":" ] ; then + log_file="./minicom.cap" +else + log_file="$2" +fi + +if [ ":""$3" = ":" ] ; then + info_included_in_data="accel+gyro+meta" +else + info_included_in_data="$3" +fi + + +tmp_file="./tmp.csv" +log_file_cp="${log_file}.cp" +log_file_in="${log_file_cp}.tmp" + +echo "log_file:$log_file" + + +if [ $log_encoding_is_base64 = true ] ; then + cp $log_file $log_file_cp + ./process_nicla_bhy2_log_base64.py $log_file_cp $info_included_in_data > $log_file_in +else + cp $log_file $log_file_in +fi + +echo "log_id,seq,ax,ay,az,gx,gy,gz" > $tmp_file + +sed 's/.*\([a-zA-Z]\)\([0-9]\)/\1,\2/g' $log_file_in >> $tmp_file +./check_for_data_loss.py $tmp_file + +rm -f $tmp_file +rm -f $log_file_cp +rm -f $log_file_in diff --git a/Arduino_BHY2/examples/SyncAndCollectIMUData/tools/process_nicla_bhy2_log_base64.py b/Arduino_BHY2/examples/SyncAndCollectIMUData/tools/process_nicla_bhy2_log_base64.py new file mode 100755 index 00000000..26c1bf1f --- /dev/null +++ b/Arduino_BHY2/examples/SyncAndCollectIMUData/tools/process_nicla_bhy2_log_base64.py @@ -0,0 +1,102 @@ +#!/usr/bin/env python +import math +import base64 +import struct +import sys + +#configurations +#DEBUG = True +DEBUG = False +lines_to_ignore = 500 + +# the following should be set according to the fimrware settings on the Nicla Senes ME firmware/sketch +data_include_acc = False +data_include_gyr = False +data_include_meta_info = False + + +if (len(sys.argv) > 1): + log_file_name = sys.argv[1] +else: + log_file_name = 'log_nicla_bhy2.txt' + +if (len(sys.argv) > 2): + info_included_in_data = sys.argv[2] + if "accel" in info_included_in_data: + data_include_acc = True + if DEBUG: + print("accel data included") + if "gyro" in info_included_in_data: + data_include_gyr = True + if DEBUG: + print("gyro data included") + if "meta" in info_included_in_data: + data_include_meta_info= True + if DEBUG: + print("meta data included") + +#working variables +file_log = open(log_file_name, 'r') +lines = file_log.readlines() + +lineCnt = 0 +record_len = 0 +record_len_before_encoding = 0 + +if data_include_meta_info: + record_len_before_encoding += 2 + +if data_include_acc: + record_len_before_encoding += 6 + +if data_include_gyr: + record_len_before_encoding += 6 + + +record_len = int(math.ceil(record_len_before_encoding / 3)) * 4 +if DEBUG: + print("record_len:", record_len) + + +DEBUG = False +# Strips the newline character +for line in lines: + lineCnt += 1 + + if (lineCnt <= lines_to_ignore): + continue + + line = line.strip() + len_line = len(line) + + if (len_line < record_len) or (line[len_line - 1] != '='): + if (lineCnt < len(lines)): + print("line:", lineCnt, " skipped") + else: + if DEBUG: + print("last line:", lineCnt, " skipped") + continue + + line = line[-record_len:] + try: + data_imu = base64.b64decode(line) + if DEBUG: + print(data_imu) + print(len(data_imu)) + + try: + if (data_include_acc and data_include_gyr): + (log_id, seq, ax,ay,az,gx,gy,gz) = struct.unpack("