From 8b96a9fea4893ff99a29bd9965849ac540cbc1f2 Mon Sep 17 00:00:00 2001 From: zg guo Date: Sat, 22 Oct 2022 22:51:27 -0700 Subject: [PATCH] add support for output in base64 encoding for more reliability --- .../SyncAndCollectIMUData/.reuse/dep5 | 9 + .../SyncAndCollectIMUData/LICENSES/MIT.txt | 21 + .../SyncAndCollectIMUData.ino | 361 +++++++++++------- .../examples/SyncAndCollectIMUData/base64.hpp | 221 +++++++++++ 4 files changed, 467 insertions(+), 145 deletions(-) create mode 100644 Arduino_BHY2/examples/SyncAndCollectIMUData/.reuse/dep5 create mode 100644 Arduino_BHY2/examples/SyncAndCollectIMUData/LICENSES/MIT.txt create mode 100644 Arduino_BHY2/examples/SyncAndCollectIMUData/base64.hpp 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/SyncAndCollectIMUData.ino b/Arduino_BHY2/examples/SyncAndCollectIMUData/SyncAndCollectIMUData.ino index 61a2e789..59ad6329 100644 --- a/Arduino_BHY2/examples/SyncAndCollectIMUData/SyncAndCollectIMUData.ino +++ b/Arduino_BHY2/examples/SyncAndCollectIMUData/SyncAndCollectIMUData.ino @@ -3,133 +3,199 @@ * 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" -class IMUDataSyncer; -class SensorMotion; +#include "base64.hpp" //remember to include the library "base64 by Densaugeo" from Library manager -#define DEBUG 0 +#define PDEBUG 0 -#if DEBUG +#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() { - int i; - for (i = 0; i < sizeof(_seq)/sizeof(_seq[0]); i++) { - _seq[i] = -1; - _data[i] = NULL; - } - } - - virtual bool begin(int accelSampleRate, int gyroSampleRate) { - int sampleRate = accelSampleRate > gyroSampleRate ? accelSampleRate : gyroSampleRate; - - if ((accelSampleRate > 0) && (gyroSampleRate > 0)) { - - if (accelSampleRate != gyroSampleRate) { - _logId = 'u'; //data cannot be synced - return false; - } else { - _logId = 'i'; + public: + IMUDataSyncer() { + uint8_t i; + for (i = 0; i < sizeof(_seq)/sizeof(_seq[0]); i++) { + _seq[i] = -1; + } } - } else if (accelSampleRate > 0) { - _logId = 'a'; - } else if (gyroSampleRate > 0){ - _logId = 'g'; - } else { - _logId = 'n'; - } - return true; - } - - void onSensorDataUpdate(DataXYZ &data, int id) { - _data[id] = &data; - - if ('i' == _logId) { - //we assume id is always 0 (for accel) or 1 (for gyro) - //when _seq[1 - id] is less than 0, it means the other sensor has not received any value yet - if (_seq[1 - id] >= 0) { - _seq[id] = (_seq[id]+1) % 10; - } else { - _seq[id] = 0; //wait for another sensor to generate the 1st sample - } - - if (_seq[id] == _seq[1 - id]) { - //data synced well and ready for sending out - #if DEBUG - mbq_dbg_1(1); - #endif - - #if 0 - //enable this if you want to send the sequence number in each line, - //for highest ODR, we ignore this to save bandwidth - Serial.print(_logId); - Serial.print((char)(_seq[id]+'0')); Serial.print(','); - #endif - //accel data fields - Serial.print(_data[0]->x); Serial.print(','); - Serial.print(_data[0]->y); Serial.print(','); - Serial.print(_data[0]->z); Serial.print(','); - //gyro data fields - Serial.print(_data[1]->x); Serial.print(','); - Serial.print(_data[1]->y); Serial.print(','); - Serial.print(_data[1]->z); Serial.println(); - - #if DEBUG - mbq_dbg_1(0); - #endif - } - } else { - _seq[id] = (_seq[id]+1) % 10; - Serial.print(_logId); - Serial.print(_seq[id]); Serial.print(','); - Serial.print(_data[id]->x); Serial.print(','); - Serial.print(_data[id]->y); Serial.print(','); - Serial.print(_data[id]->z); Serial.println(); - } - } - - protected: - char _logId; - int8_t _seq[2]; - DataXYZ *_data[2]; + //@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() { - return _data.toString(); - } - - protected: - DataXYZ _data; - uint8_t _id; - IMUDataSyncer *_dataSyncer; + public: + SensorMotion(uint8_t id) : SensorClass(id) { + _id = (id / 10); //accel: 0, gyro: 1, mag: 2 + _dataSyncer = NULL; + } + + void setDataSyncer(IMUDataSyncer &dataSyncer) { + _dataSyncer = &dataSyncer; + } + + void setData(SensorDataPacket &data) { + //DataParser::parse3DVector(data, _data); + _dataSyncer->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; }; @@ -137,52 +203,57 @@ SensorMotion accel(1); //sensor id 1: accel raw data passthrough SensorMotion gyro(10); //sensor id 10: gyro raw data passthrough IMUDataSyncer imuDataSyncer; -#define IMU_DATA_RATE 1600 +#define DATA_RATE_ACC 1600 +#define DATA_RATE_GYR 1600 void testSerial() { - while (0) { - Serial.print('*'); - //Serial.println(); - - delay(1000); - } +#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(IMU_DATA_RATE, IMU_DATA_RATE); - accel.setDataSyncer(imuDataSyncer); - gyro.setDataSyncer(imuDataSyncer); - accel.begin(IMU_DATA_RATE); - gyro.begin(IMU_DATA_RATE); + //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 DEBUG - mbq_dbg_0(1); - #endif - - BHY2.update(); - - #if DEBUG - mbq_dbg_0(0); - #endif +#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