From 0839392a114d184d9aaa976c40827a1efa377352 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Phil=C3=A9mon=20Pr=C3=A9vot?= <philemon.prevot@gmail.com> Date: Wed, 11 Dec 2024 13:27:16 +0100 Subject: [PATCH 01/15] Create timestamp_dev branch --- src/filewriter.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/filewriter.cpp b/src/filewriter.cpp index 01009d0..8fba290 100644 --- a/src/filewriter.cpp +++ b/src/filewriter.cpp @@ -153,22 +153,27 @@ float IMUFileWriter::GetFloatSafe(const unsigned char *p, int index) { void IMUFileWriter::DecodeMessage(unsigned char c) { switch (rcvState) { case StateReception::Waiting: + std::cout << "Waiting : " << c; if (c == 0xFE) rcvState = StateReception::FunctionMSB; break; case StateReception::FunctionMSB: + std::cout << "CMD_MSB : " << c; msgDecodedFunction = static_cast<int16_t>(c << 8); rcvState = StateReception::FunctionLSB; break; case StateReception::FunctionLSB: + std::cout << "CMD_LSB : " << c; msgDecodedFunction += static_cast<int16_t>(c << 0); rcvState = StateReception::PayloadLengthMSB; break; case StateReception::PayloadLengthMSB: + std::cout << "PLD_MSB : " << c; msgDecodedPayloadLength = static_cast<uint16_t>(c << 8); rcvState = StateReception::PayloadLengthLSB; break; case StateReception::PayloadLengthLSB: + std::cout << "PLD_LSB : " << c; msgDecodedPayloadLength += static_cast<uint16_t>(c << 0); if (msgDecodedPayloadLength > 0) { if (msgDecodedPayloadLength < 1024) { @@ -185,6 +190,7 @@ void IMUFileWriter::DecodeMessage(unsigned char c) { rcvState = StateReception::Decode; break; case StateReception::Payload: + std::cout << "PLD : " << c; if (msgDecodedPayloadIndex > msgDecodedPayloadLength) { //Erreur @@ -200,6 +206,7 @@ void IMUFileWriter::DecodeMessage(unsigned char c) { break; case StateReception::Decode: { + std::cout << "DCD : " << c; //Lance l'event de fin de decodage ProcessDecodedMessage(msgDecodedFunction, msgDecodedPayloadLength, msgDecodedPayload); msgDecoded++; @@ -448,8 +455,6 @@ void IMUFileWriter::ProcessDecodedMessage(int msgFunction, int msgPayloadLength, } void IMUFileWriter::write(uint8_t *sample, size_t size, uint8_t *imu_data) { - uint8_t *imu_data_cur(imu_data); - if(this->qhb_version == 3) { for(int i=1; i<size-1; i++) { -- GitLab From 07405b994f0e587915171a4fed145dd7923c7fcd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Phil=C3=A9mon=20Pr=C3=A9vot?= <philemon.prevot@gmail.com> Date: Wed, 18 Dec 2024 14:39:11 +0100 Subject: [PATCH 02/15] Adding packet timestamp decoding and printing --- src/filewriter.cpp | 35 +++++++++++++++++++++++++++-------- src/filewriter.h | 6 ++++++ 2 files changed, 33 insertions(+), 8 deletions(-) diff --git a/src/filewriter.cpp b/src/filewriter.cpp index 8fba290..68d8c22 100644 --- a/src/filewriter.cpp +++ b/src/filewriter.cpp @@ -127,11 +127,14 @@ IMUFileWriter::IMUFileWriter(std::string &filename_template, size_t qhb_version, outfile(generate_filename(), std::ios::out | std::ios::trunc), last_timestamp(0), + timestampByteReceived(0), rcvState(StateReception::Waiting), msgDecodedFunction(0), msgDecodedPayloadLength(0), msgDecodedPayload(nullptr), msgDecodedPayloadIndex(0), + softwareMajorRev(0), + softwareMinorRev(0), msgDecoded(0) { outfile << header; } @@ -153,28 +156,24 @@ float IMUFileWriter::GetFloatSafe(const unsigned char *p, int index) { void IMUFileWriter::DecodeMessage(unsigned char c) { switch (rcvState) { case StateReception::Waiting: - std::cout << "Waiting : " << c; if (c == 0xFE) rcvState = StateReception::FunctionMSB; break; case StateReception::FunctionMSB: - std::cout << "CMD_MSB : " << c; msgDecodedFunction = static_cast<int16_t>(c << 8); rcvState = StateReception::FunctionLSB; break; case StateReception::FunctionLSB: - std::cout << "CMD_LSB : " << c; msgDecodedFunction += static_cast<int16_t>(c << 0); rcvState = StateReception::PayloadLengthMSB; break; case StateReception::PayloadLengthMSB: - std::cout << "PLD_MSB : " << c; msgDecodedPayloadLength = static_cast<uint16_t>(c << 8); rcvState = StateReception::PayloadLengthLSB; break; case StateReception::PayloadLengthLSB: - std::cout << "PLD_LSB : " << c; msgDecodedPayloadLength += static_cast<uint16_t>(c << 0); + std::cout << "PayloadLength : " << msgDecodedPayloadLength << "\n"; if (msgDecodedPayloadLength > 0) { if (msgDecodedPayloadLength < 1024) { msgDecodedPayloadIndex = 0; @@ -182,15 +181,34 @@ void IMUFileWriter::DecodeMessage(unsigned char c) { if (msgDecodedPayload == nullptr) { throw std::bad_alloc(); // Handle memory allocation failure } - rcvState = StateReception::Payload; + rcvState = StateReception::SoftwareMajorRev; + std::cout << "StateReception : SoftwareMajorRev\n"; } else { rcvState = StateReception::Waiting; + std::cout << "StateReception : Waiting\n"; } } else rcvState = StateReception::Decode; + std::cout << "StateReception : Decode\n"; + break; + case StateReception::SoftwareMajorRev: + softwareMajorRev = static_cast<u_int8_t>(c); + rcvState = StateReception::SoftwareMinorRev; + break; + case StateReception::SoftwareMinorRev: + softwareMinorRev = static_cast<u_int8_t>(c); + rcvState = StateReception::TimestampB; + break; + case StateReception::TimestampB: + last_timestamp += static_cast<uint64_t>(c << 8 * (8 - timestampByteReceived)); + timestampByteReceived += 1; + if (timestampByteReceived == 8) + { + timestampByteReceived = 0; + rcvState = StateReception::Payload; + } break; case StateReception::Payload: - std::cout << "PLD : " << c; if (msgDecodedPayloadIndex > msgDecodedPayloadLength) { //Erreur @@ -206,10 +224,10 @@ void IMUFileWriter::DecodeMessage(unsigned char c) { break; case StateReception::Decode: { - std::cout << "DCD : " << c; //Lance l'event de fin de decodage ProcessDecodedMessage(msgDecodedFunction, msgDecodedPayloadLength, msgDecodedPayload); msgDecoded++; + last_timestamp = 0; rcvState = StateReception::Waiting; } break; @@ -279,6 +297,7 @@ void IMUFileWriter::ProcessDecodedMessage(int msgFunction, int msgPayloadLength, } break; case static_cast<short>(HS_DATA_PACKET_FULL_TIMESTAMP_V2): { + outfile << "PACKET TIMESTAMP: " << last_timestamp; IMUFileWriter::SensorType sensorType = static_cast<SensorType>(msgPayload[0]); unsigned char id = msgPayload[1]; unsigned char nbChannels = msgPayload[2]; diff --git a/src/filewriter.h b/src/filewriter.h index 8cbf897..e584a30 100644 --- a/src/filewriter.h +++ b/src/filewriter.h @@ -203,6 +203,9 @@ private: FunctionLSB, PayloadLengthMSB, PayloadLengthLSB, + SoftwareMajorRev, + SoftwareMinorRev, + TimestampB, Payload, Decode }; @@ -212,6 +215,9 @@ private: int msgDecodedPayloadLength; unsigned char *msgDecodedPayload; int msgDecodedPayloadIndex; + int softwareMajorRev; + int softwareMinorRev; + int timestampByteReceived; unsigned int msgDecoded; void ProcessDecodedMessage(int msgFunction, int msgPayloadLength, -- GitLab From 95637f0f57a0e8e2013c35717a3959f2f99d6f51 Mon Sep 17 00:00:00 2001 From: Philemon Prevot <philemon.prevot@lis-lab.fr> Date: Fri, 18 Apr 2025 15:25:50 +0200 Subject: [PATCH 03/15] Update README to take qhb_version in account --- .gitignore | 3 ++- README.md | 22 ++++++++++++++++++++-- 2 files changed, 22 insertions(+), 3 deletions(-) diff --git a/.gitignore b/.gitignore index f3d6549..be2b66a 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,2 @@ -/build/ \ No newline at end of file +/build/ +/.vscode \ No newline at end of file diff --git a/README.md b/README.md index 93dfcc2..82d0d88 100644 --- a/README.md +++ b/README.md @@ -6,6 +6,7 @@ samples from the JASON Qualilife sound card developed by SMIoT. Prerequisites ------------- + To build and run the application, you will need: * a C++ compiler supporting C++11 (e.g., g++ 4.8.1 and above) @@ -13,16 +14,20 @@ To build and run the application, you will need: * CMake 3.1 or above (optional) On a debian-based Linux system, these can be installed with: + ``` sudo apt install g++ libusb-1.0-0-dev cmake ``` Compilation / Installation -------------------------- + Clone the repository somewhere or download and extract it. ### Using CMake + If cmake is available, create a build directory, compile, and install: + ``` mkdir build cd build @@ -30,27 +35,32 @@ cmake .. -DCMAKE_BUILD_TYPE=Release make sudo make install/strip ``` + This will install globally; pass `-DCMAKE_INSTALL_PREFIX=/some/directory` to the `cmake` call to install to `/some/directory` instead. It is also possible to run the compiled application from the `src` subdirectory of the `build` directory, skipping installation altogether. ### Without CMake + If cmake is not available, you can still try to compile it manually. Just make sure to link against `libusb`. For `g++`, an example `Makefile` is included in the `src` directory, so the following may work: + ``` cd src make ``` ### Under Windows + As for any other platform, there are multiple options on Windows. The following has been tested successfully: Install CMake using the MSI installer from https://cmake.org, install the Microsoft Visual Studio Build Tools from https://aka.ms/buildtools (specifically, the C++ compiler), download and extract the precompiled Windows binaries from https://libusb.info. Open the x64 Native Tools Command Prompt and navigate to the source directory. Run the following: + ``` mkdir build mkdir install @@ -59,21 +69,24 @@ cmake .. -DCMAKE_BUILD_TYPE=Release -DLIBUSB_INCLUDE_DIR=<libusb_dir>/include -D nmake nmake install ``` + Replace `<libusb_dir>` with the path you extracted libusb to. If compilation and installation succeeded, you will find a `jasonrec.exe` in `install/bin`. Copy the `MS64/dll/libusb-1.0.dll` file from the libusb directory into `install/bin`. You can now run `jasonrec.exe` from a command prompt, or by creating a shortcut to it that includes suitable command line options. - Usage ----- + Running `jasonrec` without any arguments (or with any unsupported number of arguments, in fact) will display information on its usage: + ``` SMIoT JASON Qualilife sound recorder v1.3 -Usage:jasonrec channels rate filename [--help, -h] [--chunk_len, -c CHUNK_LEN] [--total_len, -t TOTAL_LEN] [--device, -d DEVICE] [--bit_depth, -b BIT_DEPTH] [--imu, -i IMU] [--filter, -f FILTER] [--verbose, -v] +Usage:jasonrec qhb_version channels rate filename [--help, -h] [--chunk_len, -c CHUNK_LEN] [--total_len, -t TOTAL_LEN] [--device, -d DEVICE] [--bit_depth, -b BIT_DEPTH] [--imu, -i IMU] [--filter, -f FILTER] [--verbose, -v] Positional arguments: + QHB_VERSION: version of the qhb card used (currently supports version 2 to 3 included) CHANNELS: number of channels to record (1 to 5) RATE: sample rate in Hz to record at (integral number) FILENAME: output file name. should include strftime() format specifiers @@ -93,19 +106,24 @@ Optional arguments: ``` As an example, to record a single 30-minute file of 2 channels at 16 kHz, run: + ``` jasonrec 2 16000 recording.wav -t 1800 ``` To record 4 channels at 128 kHz sample rate in 5-minute chunks with filenames based on time stamps, without stopping, run: + ``` jasonrec 4 128000 %Y-%m-%d_%H-%M-%S.wav -c 300 ``` + To record the same 4 channels at 128 kHz sample rate in 5-minute chunks with filenames based on time stamps, without stopping, but with the saving of the imu data run: + ``` jasonrec 4 128000 %Y-%m-%d_%H-%M-%S.wav -c 300 -i %Y-%m-%d_%H-%M-%S.csv ``` + File names may also include directory names based on time stamps, but the directories have to be created in advance. -- GitLab From 946c424f08926d4bc060494242c3919ff952fa26 Mon Sep 17 00:00:00 2001 From: Philemon Prevot <philemon.prevot@lis-lab.fr> Date: Fri, 18 Apr 2025 15:26:44 +0200 Subject: [PATCH 04/15] Add Accel Gyro and Mag aliases and test start message --- src/recorder.cpp | 127 ++++++++++++++++++++++++++--------------------- src/recorder.h | 9 +++- 2 files changed, 77 insertions(+), 59 deletions(-) diff --git a/src/recorder.cpp b/src/recorder.cpp index efb1d72..2f02a81 100644 --- a/src/recorder.cpp +++ b/src/recorder.cpp @@ -6,11 +6,13 @@ */ #include "recorder.h" +#include <unistd.h> #include <stdexcept> #include <iostream> #include <vector> #include <array> #include <algorithm> +#include <fstream> JasonRecorder::JasonRecorder(bool verbose) : verbose(verbose) { // create libusb context @@ -90,7 +92,7 @@ void JasonRecorder::send_message(std::uint16_t cmd, std::uint8_t *payload, size_ if (!handle) { throw std::logic_error("must call set_device() first"); } - // message format: 0xfe + payload size (2 byte) + command (1 byte) + payload + // message format: 0xfe + command (2 byte) + payload size (2 byte) + payload std::vector<std::uint8_t> data; data.reserve(6 + length); data.push_back(FRAME_START); @@ -135,70 +137,79 @@ void JasonRecorder::start_recording(int qhb_version, std::uint8_t num_channels, send_message(START_ID, payload1); } else if (qhb_version == 3) { - std::array<unsigned char, 4> accelSamplingRateBytes = {0}; - std::array<unsigned char, 4> accelRangeScaleBytes = {0}; - getBytesFromFloat(accelSamplingRateBytes, accelSamplingRate); - getBytesFromFloat(accelRangeScaleBytes, accelRangeScale); - std::vector<std::uint8_t> payload2 = { - (std::uint8_t) (0x01), - (std::uint8_t) (0x00), - (std::uint8_t) (accelRangeScaleBytes[0]), - (std::uint8_t) (accelRangeScaleBytes[1]), - (std::uint8_t) (accelRangeScaleBytes[2]), - (std::uint8_t) (accelRangeScaleBytes[3]), - (std::uint8_t) (accelSamplingRateBytes[0]), - (std::uint8_t) (accelSamplingRateBytes[1]), - (std::uint8_t) (accelSamplingRateBytes[2]), - (std::uint8_t) (accelSamplingRateBytes[3]) - }; - send_message(SET_SENSOR, payload2); - - std::array<unsigned char, 4> gyroSamplingRateBytes = {0}; - std::array<unsigned char, 4> gyroRangeScaleBytes = {0}; - getBytesFromFloat(gyroSamplingRateBytes, gyroSamplingRate); - getBytesFromFloat(gyroRangeScaleBytes, gyroRangeScale); - std::vector<std::uint8_t> payload3 = { - (std::uint8_t) (0x02), - (std::uint8_t) (0x00), - (std::uint8_t) (gyroRangeScaleBytes[0]), - (std::uint8_t) (gyroRangeScaleBytes[1]), - (std::uint8_t) (gyroRangeScaleBytes[2]), - (std::uint8_t) (gyroRangeScaleBytes[3]), - (std::uint8_t) (gyroSamplingRateBytes[0]), - (std::uint8_t) (gyroSamplingRateBytes[1]), - (std::uint8_t) (gyroSamplingRateBytes[2]), - (std::uint8_t) (gyroSamplingRateBytes[3]) - }; - send_message(SET_SENSOR, payload3); - - std::array<unsigned char, 4> magSamplingRateBytes = {0}; - std::array<unsigned char, 4> magRangeScaleBytes = {0}; - getBytesFromFloat(magSamplingRateBytes, magSamplingRate); - getBytesFromFloat(magRangeScaleBytes, magRangeScale); - std::vector<uint8_t> payload4 { - (std::uint8_t) (0x03), - (std::uint8_t) (0x00), - (std::uint8_t) (magRangeScaleBytes[0]), - (std::uint8_t) (magRangeScaleBytes[1]), - (std::uint8_t) (magRangeScaleBytes[2]), - (std::uint8_t) (magRangeScaleBytes[3]), - (std::uint8_t) (magSamplingRateBytes[0]), - (std::uint8_t) (magSamplingRateBytes[1]), - (std::uint8_t) (magSamplingRateBytes[2]), - (std::uint8_t) (magSamplingRateBytes[3]) - }; - send_message(SET_SENSOR, payload4); - std::vector<std::uint8_t> payload1 = { START, (std::uint8_t) ((sample_rate >> 24) & 0xFF), (std::uint8_t) ((sample_rate >> 16) & 0xFF), (std::uint8_t) ((sample_rate >> 8) & 0xFF), (std::uint8_t) (sample_rate & 0xFF), - num_channels, + (std::uint8_t) (num_channels), (std::uint8_t) (8 * depth), - num_filter}; + (std::uint8_t) (1 * num_filter)}; send_message(START_ID, payload1); + std::cout << "Start message sent\n"; + + // std::array<unsigned char, 4> accelSamplingRateBytes = {0}; + // std::array<unsigned char, 4> accelRangeScaleBytes = {0}; + // getBytesFromFloat(accelSamplingRateBytes, accelSamplingRate); + // getBytesFromFloat(accelRangeScaleBytes, accelRangeScale); + // std::vector<std::uint8_t> payload2 = { + // (std::uint8_t) (ACCEL), + // (std::uint8_t) (0x00), + // (std::uint8_t) (accelRangeScaleBytes[0]), + // (std::uint8_t) (accelRangeScaleBytes[1]), + // (std::uint8_t) (accelRangeScaleBytes[2]), + // (std::uint8_t) (accelRangeScaleBytes[3]), + // (std::uint8_t) (accelSamplingRateBytes[0]), + // (std::uint8_t) (accelSamplingRateBytes[1]), + // (std::uint8_t) (accelSamplingRateBytes[2]), + // (std::uint8_t) (accelSamplingRateBytes[3]) + // }; + // send_message(SET_SENSOR, payload2); + // std::cout << "Accel set\n"; + // sleep(0.01); + + // std::array<unsigned char, 4> gyroSamplingRateBytes = {0}; + // std::array<unsigned char, 4> gyroRangeScaleBytes = {0}; + // getBytesFromFloat(gyroSamplingRateBytes, gyroSamplingRate); + // getBytesFromFloat(gyroRangeScaleBytes, gyroRangeScale); + // std::vector<std::uint8_t> payload3 = { + // (std::uint8_t) (GYRO), + // (std::uint8_t) (0x00), + // (std::uint8_t) (gyroRangeScaleBytes[0]), + // (std::uint8_t) (gyroRangeScaleBytes[1]), + // (std::uint8_t) (gyroRangeScaleBytes[2]), + // (std::uint8_t) (gyroRangeScaleBytes[3]), + // (std::uint8_t) (gyroSamplingRateBytes[0]), + // (std::uint8_t) (gyroSamplingRateBytes[1]), + // (std::uint8_t) (gyroSamplingRateBytes[2]), + // (std::uint8_t) (gyroSamplingRateBytes[3]) + // }; + // send_message(SET_SENSOR, payload3); + // std::cout << "Gyro set\n"; + // sleep(0.01); + + // std::array<unsigned char, 4> magSamplingRateBytes = {0}; + // std::array<unsigned char, 4> magRangeScaleBytes = {0}; + // getBytesFromFloat(magSamplingRateBytes, magSamplingRate); + // getBytesFromFloat(magRangeScaleBytes, magRangeScale); + // std::vector<uint8_t> payload4 { + // (std::uint8_t) (MAG), + // (std::uint8_t) (0x00), + // (std::uint8_t) (magRangeScaleBytes[0]), + // (std::uint8_t) (magRangeScaleBytes[1]), + // (std::uint8_t) (magRangeScaleBytes[2]), + // (std::uint8_t) (magRangeScaleBytes[3]), + // (std::uint8_t) (magSamplingRateBytes[0]), + // (std::uint8_t) (magSamplingRateBytes[1]), + // (std::uint8_t) (magSamplingRateBytes[2]), + // (std::uint8_t) (magSamplingRateBytes[3]) + // }; + // send_message(SET_SENSOR, payload4); + // std::cout << "Mag set\n"; + // sleep(0.01); + + // libusb_clear_halt(handle, ENDPOINT_SEND); } this->num_channels = num_channels; @@ -250,6 +261,8 @@ void JasonRecorder::get_samples(std::vector<std::uint8_t> &samples, std::vector< //size_t length = buffer[1] << 8 + buffer[2]; if (buffer[0] != FRAME_START); // invalid message else if ((((std::uint16_t) buffer[1] << 8 )|(buffer[2])) == DATA_ID) { + std::cout << "IMU Data buffer\n"; + // find the beginning and length of the samples in the buffer size_t start = this->additional_data_size + 6; imu_data.resize(0); diff --git a/src/recorder.h b/src/recorder.h index 4bcf71f..98a7a48 100644 --- a/src/recorder.h +++ b/src/recorder.h @@ -33,8 +33,13 @@ class JasonRecorder { const std::uint16_t DATA_ID = 0x0B01; const std::uint16_t STATUS_ID = 0x0B02; - const std::uint8_t START = 1; - const std::uint8_t STOP = 0; + const std::uint8_t START = 0x01; + const std::uint8_t STOP = 0x00; + + // sensor types for sensor parameters setting + const std::uint16_t ACCEL = 0x01; + const std::uint16_t GYRO = 0x02; + const std::uint16_t MAG = 0x03; private: // libusb handles struct libusb_context *ctx; -- GitLab From 84a8cdc1c2e93d0bb385ccb8604993d3fabec367 Mon Sep 17 00:00:00 2001 From: Philemon Prevot <philemon.prevot@lis-lab.fr> Date: Fri, 18 Apr 2025 15:27:35 +0200 Subject: [PATCH 05/15] Add debug lines --- CMakeLists.txt | 3 +++ src/filewriter.cpp | 12 +++++++++++- 2 files changed, 14 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ccd70e9..d819d19 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,3 +15,6 @@ find_library(LIBUSB_LIBRARY PATH_SUFFIXES "lib" "lib32" "lib64") add_subdirectory(src) + +message(STATUS "LIBUSB_INCLUDE_DIR: ${LIBUSB_INCLUDE_DIR}") +message(STATUS "LIBUSB_LIBRARY: ${LIBUSB_LIBRARY}") \ No newline at end of file diff --git a/src/filewriter.cpp b/src/filewriter.cpp index 68d8c22..7c2538d 100644 --- a/src/filewriter.cpp +++ b/src/filewriter.cpp @@ -158,21 +158,27 @@ void IMUFileWriter::DecodeMessage(unsigned char c) { case StateReception::Waiting: if (c == 0xFE) rcvState = StateReception::FunctionMSB; + std::cout << "Batch start : "; + std::cout << std::hex << std::setfill('0') << std::setw(2) << c << " "; break; case StateReception::FunctionMSB: msgDecodedFunction = static_cast<int16_t>(c << 8); rcvState = StateReception::FunctionLSB; + std::cout << std::hex << std::setfill('0') << std::setw(2) << c << " "; break; case StateReception::FunctionLSB: msgDecodedFunction += static_cast<int16_t>(c << 0); rcvState = StateReception::PayloadLengthMSB; + std::cout << std::hex << std::setfill('0') << std::setw(2) << c << " "; break; case StateReception::PayloadLengthMSB: msgDecodedPayloadLength = static_cast<uint16_t>(c << 8); rcvState = StateReception::PayloadLengthLSB; + std::cout << std::hex << std::setfill('0') << std::setw(2) << c << " "; break; case StateReception::PayloadLengthLSB: msgDecodedPayloadLength += static_cast<uint16_t>(c << 0); + std::cout << std::hex << std::setfill('0') << std::setw(2) << c << " "; std::cout << "PayloadLength : " << msgDecodedPayloadLength << "\n"; if (msgDecodedPayloadLength > 0) { if (msgDecodedPayloadLength < 1024) { @@ -194,13 +200,16 @@ void IMUFileWriter::DecodeMessage(unsigned char c) { case StateReception::SoftwareMajorRev: softwareMajorRev = static_cast<u_int8_t>(c); rcvState = StateReception::SoftwareMinorRev; + std::cout << std::hex << std::setfill('0') << std::setw(2) << c << " "; break; case StateReception::SoftwareMinorRev: softwareMinorRev = static_cast<u_int8_t>(c); rcvState = StateReception::TimestampB; + std::cout << std::hex << std::setfill('0') << std::setw(2) << c << " "; break; case StateReception::TimestampB: last_timestamp += static_cast<uint64_t>(c << 8 * (8 - timestampByteReceived)); + std::cout << std::hex << std::setfill('0') << std::setw(2) << c << " "; timestampByteReceived += 1; if (timestampByteReceived == 8) { @@ -209,6 +218,7 @@ void IMUFileWriter::DecodeMessage(unsigned char c) { } break; case StateReception::Payload: + std::cout << std::hex << std::setfill('0') << std::setw(2) << c << " "; if (msgDecodedPayloadIndex > msgDecodedPayloadLength) { //Erreur @@ -475,7 +485,7 @@ void IMUFileWriter::ProcessDecodedMessage(int msgFunction, int msgPayloadLength, void IMUFileWriter::write(uint8_t *sample, size_t size, uint8_t *imu_data) { if(this->qhb_version == 3) { - for(int i=1; i<size-1; i++) + for(int i=0; i<size-1; i++) { DecodeMessage(imu_data[i]); } -- GitLab From e8bd296564fb6ecad83dba9f12486b049bf77535 Mon Sep 17 00:00:00 2001 From: Philemon Prevot <philemon.prevot@lis-lab.fr> Date: Tue, 22 Apr 2025 18:18:06 +0200 Subject: [PATCH 06/15] Add debug files to .vscode --- .vscode/launch.json | 26 +++++++++++++++++ .vscode/settings.json | 65 +++++++++++++++++++++++++++++++++++++++++++ .vscode/tasks.json | 53 +++++++++++++++++++++++++++++++++++ 3 files changed, 144 insertions(+) create mode 100644 .vscode/launch.json create mode 100644 .vscode/settings.json create mode 100644 .vscode/tasks.json diff --git a/.vscode/launch.json b/.vscode/launch.json new file mode 100644 index 0000000..c6d3c59 --- /dev/null +++ b/.vscode/launch.json @@ -0,0 +1,26 @@ +{ + "version": "0.2.0", + "configurations": [ + { + "name": "Debug jasonrec", + "type": "cppdbg", + "preLaunchTask": "Configure and Build HighBlueParsers", + "request": "launch", + "program": "/usr/local/bin/jasonrec", // adjust if different + "args": ["3", "1", "256000", "debug_audio_file.wav", "--imu", "debug_imu_file.csv"], // replace with real args + "stopAtEntry": false, + "cwd": "${workspaceFolder}", + "environment": [], + "externalConsole": false, + "MIMode": "gdb", + "setupCommands": [ + { + "description": "Enable pretty-printing for gdb", + "text": "-enable-pretty-printing", + "ignoreFailures": true + } + ] + } + ] + } + \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..4b96cbb --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,65 @@ +{ + "files.associations": { + "array": "cpp", + "atomic": "cpp", + "bit": "cpp", + "*.tcc": "cpp", + "cctype": "cpp", + "charconv": "cpp", + "chrono": "cpp", + "clocale": "cpp", + "cmath": "cpp", + "compare": "cpp", + "complex": "cpp", + "concepts": "cpp", + "csignal": "cpp", + "cstdarg": "cpp", + "cstddef": "cpp", + "cstdint": "cpp", + "cstdio": "cpp", + "cstdlib": "cpp", + "cstring": "cpp", + "ctime": "cpp", + "cwchar": "cpp", + "cwctype": "cpp", + "deque": "cpp", + "string": "cpp", + "unordered_map": "cpp", + "vector": "cpp", + "exception": "cpp", + "algorithm": "cpp", + "functional": "cpp", + "iterator": "cpp", + "memory": "cpp", + "memory_resource": "cpp", + "numeric": "cpp", + "optional": "cpp", + "random": "cpp", + "ratio": "cpp", + "string_view": "cpp", + "system_error": "cpp", + "tuple": "cpp", + "type_traits": "cpp", + "utility": "cpp", + "format": "cpp", + "fstream": "cpp", + "initializer_list": "cpp", + "iomanip": "cpp", + "iosfwd": "cpp", + "iostream": "cpp", + "istream": "cpp", + "limits": "cpp", + "new": "cpp", + "numbers": "cpp", + "ostream": "cpp", + "span": "cpp", + "sstream": "cpp", + "stdexcept": "cpp", + "streambuf": "cpp", + "cinttypes": "cpp", + "typeinfo": "cpp", + "variant": "cpp" + }, + "cmake.sourceDirectory": "/home/prevot/Bureau/highblueparsers/src", + "cmake.buildDirectory": "${workspaceFolder}/build" +} \ No newline at end of file diff --git a/.vscode/tasks.json b/.vscode/tasks.json new file mode 100644 index 0000000..52b16c1 --- /dev/null +++ b/.vscode/tasks.json @@ -0,0 +1,53 @@ +{ + "version": "2.0.0", + "tasks": [ + { + "label": "Build with CMake", + "type": "shell", + "command": "cmake", + "args": [ + "..", + "-DCMAKE_BUILD_TYPE=Release" + ], + "options": { + "cwd": "${workspaceFolder}/build" + }, + "problemMatcher": [] + }, + { + "label": "Make", + "type": "shell", + "command": "make", + "options": { + "cwd": "${workspaceFolder}/build" + }, + "dependsOn": [ + "Build with CMake" + ] + }, + { + "label": "Install", + "type": "shell", + "command": "sudo make install/strip", + "options": { + "cwd": "${workspaceFolder}/build" + }, + "dependsOn": [ + "Make" + ], + "problemMatcher": [ + "$gcc" + ] + }, + { + "label": "Configure and Build HighBlueParsers", + "dependsOn": ["Build with CMake", "Make", "Install"], + "dependsOrder": "sequence", + "problemMatcher": [], + "group": { + "kind": "build", + "isDefault": true + } + } + ] +} \ No newline at end of file -- GitLab From 91a9acafc694d576880504394ce3b0820f3b2a61 Mon Sep 17 00:00:00 2001 From: Philemon Prevot <philemon.prevot@lis-lab.fr> Date: Thu, 24 Apr 2025 15:19:37 +0200 Subject: [PATCH 07/15] Update debug files --- .gitignore | 3 +-- .vscode/c_cpp_properties.json | 17 +++++++++++++++++ .vscode/launch.json | 4 ++-- .vscode/tasks.json | 6 +++--- 4 files changed, 23 insertions(+), 7 deletions(-) create mode 100644 .vscode/c_cpp_properties.json diff --git a/.gitignore b/.gitignore index be2b66a..f3d6549 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1 @@ -/build/ -/.vscode \ No newline at end of file +/build/ \ No newline at end of file diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..4ace5d5 --- /dev/null +++ b/.vscode/c_cpp_properties.json @@ -0,0 +1,17 @@ +{ + "configurations": [ + { + "name": "Linux", + "includePath": [ + "${workspaceFolder}/**", + "/usr/include/libusb-1.0" + ], + "defines": [], + "compilerPath": "/usr/bin/gcc", + "cStandard": "c17", + "cppStandard": "gnu++17", + "intelliSenseMode": "linux-gcc-x64" + } + ], + "version": 4 +} \ No newline at end of file diff --git a/.vscode/launch.json b/.vscode/launch.json index c6d3c59..be5fa7e 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -6,8 +6,8 @@ "type": "cppdbg", "preLaunchTask": "Configure and Build HighBlueParsers", "request": "launch", - "program": "/usr/local/bin/jasonrec", // adjust if different - "args": ["3", "1", "256000", "debug_audio_file.wav", "--imu", "debug_imu_file.csv"], // replace with real args + "program": "${workspaceFolder}/build/src/jasonrec", + "args": ["3", "1", "256000", "debug_audio_file_%Y%m%d_%H%M%S.wav", "--imu", "debug_imu_file_%Y%m%d_%H%M%S.csv"], // replace with real args "stopAtEntry": false, "cwd": "${workspaceFolder}", "environment": [], diff --git a/.vscode/tasks.json b/.vscode/tasks.json index 52b16c1..4bd9753 100644 --- a/.vscode/tasks.json +++ b/.vscode/tasks.json @@ -7,7 +7,7 @@ "command": "cmake", "args": [ "..", - "-DCMAKE_BUILD_TYPE=Release" + "-DCMAKE_BUILD_TYPE=Debug" ], "options": { "cwd": "${workspaceFolder}/build" @@ -28,7 +28,7 @@ { "label": "Install", "type": "shell", - "command": "sudo make install/strip", + "command": "sudo make install", "options": { "cwd": "${workspaceFolder}/build" }, @@ -41,7 +41,7 @@ }, { "label": "Configure and Build HighBlueParsers", - "dependsOn": ["Build with CMake", "Make", "Install"], + "dependsOn": ["Build with CMake", "Make"], "dependsOrder": "sequence", "problemMatcher": [], "group": { -- GitLab From 84f78f5d714eb61d838c6c2f6b1f38ccc54fe64e Mon Sep 17 00:00:00 2001 From: Philemon Prevot <philemon.prevot@lis-lab.fr> Date: Fri, 25 Apr 2025 10:08:49 +0200 Subject: [PATCH 08/15] Correct continuous recording and add debug hex_dump method, plus change header sizes selection and correct timestamp logging --- src/cleanexit.h | 5 ++ src/filewriter.cpp | 111 +++++++++++++++++++++------------------------ src/filewriter.h | 19 ++++---- src/main.cpp | 66 +++++++++++++++++++-------- src/recorder.cpp | 82 +++++++++++++++++++++++---------- src/recorder.h | 8 +++- 6 files changed, 178 insertions(+), 113 deletions(-) diff --git a/src/cleanexit.h b/src/cleanexit.h index 24a88dd..f74687c 100644 --- a/src/cleanexit.h +++ b/src/cleanexit.h @@ -12,6 +12,11 @@ */ void allow_clean_exit(); +/** + * Sets exit signal to extern for access from the main loop + */ +extern std::atomic<bool> _exit_requested; // Exit flag + /** * Check if the application was requested to terminate. * \returns whether the application was requested to terminate. diff --git a/src/filewriter.cpp b/src/filewriter.cpp index 7c2538d..6a943d3 100644 --- a/src/filewriter.cpp +++ b/src/filewriter.cpp @@ -16,8 +16,8 @@ #include <cstring> #include <cmath> -FileWriter::FileWriter(std::string &filename_template, size_t qhb_version, size_t num_channels, size_t sample_rate, size_t depth) : - filename_template(filename_template), qhb_version(qhb_version), num_channels(num_channels), sample_rate(sample_rate), depth(depth) { +FileWriter::FileWriter(std::string &filename_template, size_t qhb_version, size_t num_channels, size_t sample_rate, size_t depth, uint64_t packet_timestamp) : + filename_template(filename_template), qhb_version(qhb_version), num_channels(num_channels), sample_rate(sample_rate), depth(depth), packet_timestamp(packet_timestamp) { // nothing } @@ -82,7 +82,7 @@ WavFileWriter::WavFileWriter(std::string &filename_template, size_t qhb_version, WavFileWriter::WavFileWriter(std::string &filename_template, size_t qhb_version, size_t num_channels, size_t sample_rate, size_t depth, size_t expected_num_samples) : - FileWriter(filename_template, qhb_version, num_channels, sample_rate, depth), + FileWriter(filename_template, qhb_version, num_channels, sample_rate, depth, 0), outfile(generate_filename(), std::ios::out | std::ios::binary | std::ios::trunc), samples_written(0) { // check if we could open the file @@ -122,8 +122,8 @@ void WavFileWriter::write(uint8_t *samples, size_t num_samples, uint8_t *imu_dat samples_written += num_samples /(num_channels * depth); } -IMUFileWriter::IMUFileWriter(std::string &filename_template, size_t qhb_version, size_t num_channels, size_t sample_rate,size_t depth, size_t timestamp) : - FileWriter(filename_template, qhb_version, num_channels, sample_rate, depth), +IMUFileWriter::IMUFileWriter(std::string &filename_template, size_t qhb_version, size_t num_channels, size_t sample_rate, size_t depth, uint64_t packet_timestamp) : + FileWriter(filename_template, qhb_version, num_channels, sample_rate, depth, packet_timestamp), outfile(generate_filename(), std::ios::out | std::ios::trunc), last_timestamp(0), @@ -153,33 +153,43 @@ float IMUFileWriter::GetFloatSafe(const unsigned char *p, int index) { return result; } +unsigned char IMUFileWriter::CalculateChecksum(int msgFunction, + int msgPayloadLength, const unsigned char msgPayload[]) +{ + unsigned char checksum = 0; + checksum ^= static_cast<unsigned char>(0xFE); + checksum ^= static_cast<unsigned char>(msgFunction >> 8); + checksum ^= static_cast<unsigned char>(msgFunction >> 0); + checksum ^= static_cast<unsigned char>(msgPayloadLength >> 8); + checksum ^= static_cast<unsigned char>(msgPayloadLength >> 0); + for (int i = 0; i < msgPayloadLength; i++) { + checksum ^= msgPayload[i]; + } + return checksum; +} + + void IMUFileWriter::DecodeMessage(unsigned char c) { switch (rcvState) { case StateReception::Waiting: - if (c == 0xFE) + if (c == 0xFE) { rcvState = StateReception::FunctionMSB; - std::cout << "Batch start : "; - std::cout << std::hex << std::setfill('0') << std::setw(2) << c << " "; + } break; case StateReception::FunctionMSB: msgDecodedFunction = static_cast<int16_t>(c << 8); rcvState = StateReception::FunctionLSB; - std::cout << std::hex << std::setfill('0') << std::setw(2) << c << " "; break; case StateReception::FunctionLSB: msgDecodedFunction += static_cast<int16_t>(c << 0); rcvState = StateReception::PayloadLengthMSB; - std::cout << std::hex << std::setfill('0') << std::setw(2) << c << " "; break; case StateReception::PayloadLengthMSB: msgDecodedPayloadLength = static_cast<uint16_t>(c << 8); rcvState = StateReception::PayloadLengthLSB; - std::cout << std::hex << std::setfill('0') << std::setw(2) << c << " "; break; case StateReception::PayloadLengthLSB: msgDecodedPayloadLength += static_cast<uint16_t>(c << 0); - std::cout << std::hex << std::setfill('0') << std::setw(2) << c << " "; - std::cout << "PayloadLength : " << msgDecodedPayloadLength << "\n"; if (msgDecodedPayloadLength > 0) { if (msgDecodedPayloadLength < 1024) { msgDecodedPayloadIndex = 0; @@ -187,38 +197,15 @@ void IMUFileWriter::DecodeMessage(unsigned char c) { if (msgDecodedPayload == nullptr) { throw std::bad_alloc(); // Handle memory allocation failure } - rcvState = StateReception::SoftwareMajorRev; - std::cout << "StateReception : SoftwareMajorRev\n"; + rcvState = StateReception::Payload; } else { rcvState = StateReception::Waiting; - std::cout << "StateReception : Waiting\n"; } } else - rcvState = StateReception::Decode; - std::cout << "StateReception : Decode\n"; - break; - case StateReception::SoftwareMajorRev: - softwareMajorRev = static_cast<u_int8_t>(c); - rcvState = StateReception::SoftwareMinorRev; - std::cout << std::hex << std::setfill('0') << std::setw(2) << c << " "; - break; - case StateReception::SoftwareMinorRev: - softwareMinorRev = static_cast<u_int8_t>(c); - rcvState = StateReception::TimestampB; - std::cout << std::hex << std::setfill('0') << std::setw(2) << c << " "; - break; - case StateReception::TimestampB: - last_timestamp += static_cast<uint64_t>(c << 8 * (8 - timestampByteReceived)); - std::cout << std::hex << std::setfill('0') << std::setw(2) << c << " "; - timestampByteReceived += 1; - if (timestampByteReceived == 8) - { - timestampByteReceived = 0; - rcvState = StateReception::Payload; - } + rcvState = StateReception::CheckSum; break; case StateReception::Payload: - std::cout << std::hex << std::setfill('0') << std::setw(2) << c << " "; + { if (msgDecodedPayloadIndex > msgDecodedPayloadLength) { //Erreur @@ -228,18 +215,24 @@ void IMUFileWriter::DecodeMessage(unsigned char c) { msgDecodedPayload[msgDecodedPayloadIndex++] = c; if (msgDecodedPayloadIndex >= msgDecodedPayloadLength) { - rcvState = StateReception::Decode; + rcvState = StateReception::CheckSum; msgDecodedPayloadIndex = 0; } break; - case StateReception::Decode: - { - //Lance l'event de fin de decodage - ProcessDecodedMessage(msgDecodedFunction, msgDecodedPayloadLength, msgDecodedPayload); - msgDecoded++; - last_timestamp = 0; - rcvState = StateReception::Waiting; } + case StateReception::CheckSum: + { + unsigned char calculatedChecksum = CalculateChecksum(msgDecodedFunction, msgDecodedPayloadLength, msgDecodedPayload); + unsigned char receivedChecksum = c; + if (calculatedChecksum == receivedChecksum) { + //Lance l'event de fin de decodage + ProcessDecodedMessage(msgDecodedFunction, msgDecodedPayloadLength, msgDecodedPayload); + msgDecoded++; + } else { + std::cerr << "Checksum error" << std::endl; + } + rcvState = StateReception::Waiting; + } break; default: rcvState = StateReception::Waiting; @@ -307,7 +300,6 @@ void IMUFileWriter::ProcessDecodedMessage(int msgFunction, int msgPayloadLength, } break; case static_cast<short>(HS_DATA_PACKET_FULL_TIMESTAMP_V2): { - outfile << "PACKET TIMESTAMP: " << last_timestamp; IMUFileWriter::SensorType sensorType = static_cast<SensorType>(msgPayload[0]); unsigned char id = msgPayload[1]; unsigned char nbChannels = msgPayload[2]; @@ -485,14 +477,16 @@ void IMUFileWriter::ProcessDecodedMessage(int msgFunction, int msgPayloadLength, void IMUFileWriter::write(uint8_t *sample, size_t size, uint8_t *imu_data) { if(this->qhb_version == 3) { - for(int i=0; i<size-1; i++) + std::cout << static_cast<int>(this->packet_timestamp) << std::endl; + outfile << "PACKET TIMESTAMP: " << this->packet_timestamp << "\n"; + for(int i=0; i<this->additionnal_data_size; i++) { DecodeMessage(imu_data[i]); } } else { uint8_t *imu_data_cur(imu_data); - while(imu_data_cur + frame_size + 5 < imu_data + additional_data_size){ + while(imu_data_cur + frame_size + 5 < imu_data + additionnal_data_size){ if(!(imu_data_cur[0]==0xFE && imu_data_cur[1]==0x0A && imu_data_cur[2]==0x0A && imu_data_cur[5]==0x08)) { // skip trame if header is incorrect imu_data_cur += frame_size + 5; @@ -526,7 +520,7 @@ size_t IMUFileWriter::get_last_timestamp(){ SplitWavFileWriter::SplitWavFileWriter(std::string &filename_template, size_t qhb_version, size_t num_channels, size_t sample_rate, size_t depth, size_t samples_per_file) : - FileWriter(filename_template, qhb_version, num_channels, sample_rate, depth), + FileWriter(filename_template, qhb_version, num_channels, sample_rate, depth, 0), samples_per_file(samples_per_file), current_file(nullptr), current_file_samples_written(0) { @@ -568,8 +562,8 @@ void SplitWavFileWriter::write(uint8_t *samples, size_t num_samples, uint8_t *im SplitIMUWavFileWriter::SplitIMUWavFileWriter(std::string &filename_template, std::string &imu_name_template, size_t qhb_version, - size_t num_channels, size_t sample_rate, size_t depth, size_t samples_per_file): - FileWriter(filename_template, qhb_version, num_channels, sample_rate, depth), + size_t num_channels, size_t sample_rate, size_t depth, size_t samples_per_file, uint64_t packet_timestamp): + FileWriter(filename_template, qhb_version, num_channels, sample_rate, depth, packet_timestamp), imu_name_template(imu_name_template), samples_per_file(samples_per_file), current_file(nullptr), @@ -591,16 +585,16 @@ SplitIMUWavFileWriter::~SplitIMUWavFileWriter() { void SplitIMUWavFileWriter::write(uint8_t *samples, size_t num_samples, uint8_t* imu_data) { size_t available = num_samples /(num_channels * depth); // start as many new files as required to write out the samples - while (current_file_samples_written + available >= samples_per_file) { + while ((current_file_samples_written + available >= samples_per_file) && (samples_per_file != 0)) { if (!current_file) { current_file = new WavFileWriter(filename_template, qhb_version, num_channels, sample_rate, depth, samples_per_file); if (imu_file) { max_timestamp = imu_file->get_last_timestamp(); delete imu_file; - imu_file = new IMUFileWriter(imu_name_template, qhb_version, num_channels, sample_rate, depth, max_timestamp); + imu_file = new IMUFileWriter(imu_name_template, qhb_version, num_channels, sample_rate, depth, this->packet_timestamp); } } - if (!imu_file) imu_file = new IMUFileWriter(imu_name_template, qhb_version, num_channels, sample_rate, depth, max_timestamp); + if (!imu_file) imu_file = new IMUFileWriter(imu_name_template, qhb_version, num_channels, sample_rate, depth, this->packet_timestamp); // write out as much as fits into the current file and move the pointer size_t missing = samples_per_file - current_file_samples_written; current_file->write(samples, missing * num_channels * depth, imu_data); @@ -623,14 +617,13 @@ void SplitIMUWavFileWriter::write(uint8_t *samples, size_t num_samples, uint8_t* if (imu_file) { max_timestamp = imu_file->get_last_timestamp(); delete imu_file; - imu_file = new IMUFileWriter(imu_name_template, qhb_version, num_channels, sample_rate, depth, max_timestamp); + imu_file = new IMUFileWriter(imu_name_template, qhb_version, num_channels, sample_rate, depth, this->packet_timestamp); } } - if (!imu_file) imu_file = new IMUFileWriter(imu_name_template, qhb_version, num_channels, sample_rate, depth, max_timestamp); + if (!imu_file) imu_file = new IMUFileWriter(imu_name_template, qhb_version, num_channels, sample_rate, depth, this->packet_timestamp); current_file->write(samples, available * num_channels * depth, imu_data); if (imu_data) imu_file->write(samples, available * num_channels * depth, imu_data); imu_data = nullptr; current_file_samples_written += available; } - } diff --git a/src/filewriter.h b/src/filewriter.h index e584a30..cae305b 100644 --- a/src/filewriter.h +++ b/src/filewriter.h @@ -36,7 +36,7 @@ protected: public: /** Abstract constructor to be used by subclasses. */ - FileWriter(std::string &filename_template, size_t qhb_version, size_t num_channels, size_t sample_rate, size_t depth); + FileWriter(std::string &filename_template, size_t qhb_version, size_t num_channels, size_t sample_rate, size_t depth, uint64_t packet_timestamp); virtual ~FileWriter(); /** Writes out the given vector of 8-bit samples. @@ -44,6 +44,10 @@ public: */ void write(std::vector<uint8_t> &samples, std::vector<uint8_t> &imu_data); virtual void write(uint8_t *samples, size_t num_samples, uint8_t *imu_data) = 0; + + /** Save the last packet timestamp + */ + uint64_t packet_timestamp = 0; }; @@ -148,7 +152,7 @@ class IMUFileWriter: public FileWriter { // const std::string header = "Timestamp,ax,ay,az,gx,gy,gz,mx,my,mz\n"; const std::string header = "Sensor Type,TimeStamp(ms) or Time, val0,val1,val2,val3,val4,val5,val6,val7\n"; const size_t frame_size = 32; - const size_t additional_data_size = 736; + const size_t additionnal_data_size = 736; private: enum class SensorType { Unknown = 0, @@ -186,7 +190,6 @@ private: }; std::ofstream outfile; - size_t last_timestamp = 0; unsigned int lastAccelTimeStamp = 0; unsigned int lastGyroTimeStamp = 0; unsigned int lastMagTimeStamp = 0; @@ -203,11 +206,8 @@ private: FunctionLSB, PayloadLengthMSB, PayloadLengthLSB, - SoftwareMajorRev, - SoftwareMinorRev, - TimestampB, Payload, - Decode + CheckSum, }; StateReception rcvState; @@ -239,11 +239,12 @@ public: * \param[in] samples_per_file The target number of samples (per channel) * that will be written to a file before starting the next one. */ + size_t last_timestamp = 0; unsigned char CalculateChecksum(int msgFunction, int msgPayloadLength, const unsigned char msgPayload[]); void DecodeMessage(unsigned char c); - IMUFileWriter(std::string &filename_template, size_t qhb_version, size_t num_channels, size_t sample_rate, size_t depth, size_t timestamp); + IMUFileWriter(std::string &filename_template, size_t qhb_version, size_t num_channels, size_t sample_rate, size_t depth, uint64_t packet_timestamp); void write(uint8_t *samples, size_t num_samples, uint8_t *imu_data) override; size_t get_last_timestamp(); }; @@ -276,7 +277,7 @@ public: * that will be written to a file before starting the next one. */ SplitIMUWavFileWriter(std::string &filename_template, std::string &imu_name_template, size_t qhb_version, size_t num_channels, - size_t sample_rate, size_t depth, size_t samples_per_file); + size_t sample_rate, size_t depth, size_t samples_per_file, uint64_t packet_timestamp); ~SplitIMUWavFileWriter() override; void write(uint8_t *samples, size_t num_samples, uint8_t *imu_data) override; diff --git a/src/main.cpp b/src/main.cpp index e8a6f59..b905fbc 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -8,6 +8,7 @@ #include <string> #include <memory> #include <cstring> +#include <atomic> #include "recorder.h" #include "filewriter.h" #include "cleanexit.h" @@ -44,6 +45,28 @@ void print_usage(char *name) { int record(size_t qhb_version, size_t channels, size_t rate, size_t depth, size_t filter, std::string &filename, std::string &imu_name, float chunklen, float totallen, size_t device, bool verbose, size_t accelSamplingFrequency, size_t gyroSamplingFrequency, size_t magSamplingFrequency, size_t accelRangeScale, size_t gyroRangeScale, size_t magRangeScale) { + /** + * @brief Set device, initialize writers and loop the message reception and file writing methods + * + * @param qhb_version : version of the QHB card used for recording + * @param channels : number of channels recording + * @param rate : audio sampling rate + * @param depth : audio data bit precision (either 16 or 24) + * @param filter : number of filter used + * @param filename : address of the audio filename to use + * @param imu_name : address of the imu filename to use + * @param chunklen : duration of individual files if given, else defaults to 0 + * @param totallen : total recording duration if given, else defaults to 0 + * @param device : index of the device to use if several QHB Cards are plugged in + * @param verbose : boolean enabling the printing of status messages + * @param accelSamplingFrequency : accelerometer sampling frequency + * @param gyroSamplingFrequency : gyroscope sampling frequency + * @param magSamplingFrequency : magnetometer sampling frequency + * @param accelRangeScale : range of the accelerometer values + * @param gyroRangeScale : range of the gyroscope values + * @param magRangeScale : range of the magnetometer values + */ + // Initialize the recorder object and select the device to use JasonRecorder recorder = JasonRecorder(verbose); std::cout << "Found " << recorder.get_device_count() << " JASON card(s)." << std::endl; if (recorder.get_device_count() == 0) { @@ -51,28 +74,28 @@ int record(size_t qhb_version, size_t channels, size_t rate, size_t depth, size_ return 2; } try { - // prepare the device + // Prepare the device std::cout << "Selecting device number " << device << "..." << std::endl; recorder.set_device(device); - // prepare the file writer + // Prepare the file writer std::unique_ptr<FileWriter> filewriter; - if (chunklen > 0 && imu_name.empty()) { + if (chunklen > 0 && imu_name.empty()) { // When we only record audio, in several files of chunklen length // implementation note: in C++14 we would use std::make_unique<SplitWavFileWriter>(...) filewriter.reset(new SplitWavFileWriter(filename, qhb_version, channels, rate, depth, chunklen * rate)); } - else if (chunklen > 0) { + else if (chunklen > 0) { // When we record audio and imu, in several files of chunklen length // implementation note: in C++14 we would use std::make_unique<SplitWavFileWriter>(...) - filewriter.reset(new SplitIMUWavFileWriter(filename, imu_name, qhb_version, channels, rate, depth, chunklen * rate)); + filewriter.reset(new SplitIMUWavFileWriter(filename, imu_name, qhb_version, channels, rate, depth, chunklen * rate, 0)); } - else if (imu_name.empty()){ + else if (imu_name.empty()){ // When we only record audio data, in one file only filewriter.reset(new WavFileWriter(filename, qhb_version, channels, rate, depth, totallen * rate)); } - else{ - filewriter.reset(new SplitIMUWavFileWriter(filename, imu_name, qhb_version, channels, rate, depth, totallen * rate)); + else{ // When we record both audio and imu in one file only + filewriter.reset(new SplitIMUWavFileWriter(filename, imu_name, qhb_version, channels, rate, depth, totallen * rate, 0)); } - // start the recording loop + // Start the recording and writing main loop std::cout << "Starting to record..." << std::endl; - allow_clean_exit(); + allow_clean_exit(); // Handles for killing signals and clean program termination size_t total_samples_wanted = totallen * rate; size_t total_samples_read = 0; size_t failed_attempts = 0; @@ -81,21 +104,23 @@ int record(size_t qhb_version, size_t channels, size_t rate, size_t depth, size_ std::vector<std::uint8_t> imu_data; try { std::cout << "Setting recording format to " << channels << " channels at " << rate << " Hz " << (8 * depth) << " bits" << std::endl; + // Send start and parameters messages to QHB recorder.start_recording(qhb_version, channels, rate, depth, filter, accelSamplingFrequency, gyroSamplingFrequency, magSamplingFrequency, accelRangeScale, gyroRangeScale, magRangeScale); - // we will record until we have enough (or forever, if totallen == 0) - while ((total_samples_wanted == 0) || (total_samples_read < total_samples_wanted)) { - if (exit_requested()) { + // Loop until we have enough samples (or forever, if totallen == 0) + while (((total_samples_wanted == 0) || (total_samples_read < total_samples_wanted))) { + if (exit_requested()) { // In case of killing signal received std::cout << "Termination requested." << std::endl; break; } - recorder.get_samples(samples, imu_data, false, 500); + // Get audio and imu samples + recorder.get_samples(samples, imu_data, &filewriter->packet_timestamp, false, 500); if (!samples.empty()) { total_samples_read += samples.size() / sample_size; // if we have too much now, crop the last packet if ((total_samples_wanted > 0) && (total_samples_read > total_samples_wanted)) { samples.resize(samples.size() - (total_samples_read - total_samples_wanted) * sample_size); } - // pass it on to the file writer + // Pass it on to the file writer filewriter->write(samples, imu_data); failed_attempts = 0; } @@ -107,6 +132,7 @@ int record(size_t qhb_version, size_t channels, size_t rate, size_t depth, size_ } } } + // Send stop message to QHB recorder.stop_recording(); std::cout << "Stopped recording." << std::endl; } @@ -123,12 +149,15 @@ int record(size_t qhb_version, size_t channels, size_t rate, size_t depth, size_ } int main(int argc, char *argv[]) { + /** + * @brief Main function. Retrieve program arguments and hand them over to the record function. + */ if (argc < 5) { print_usage(argv[0]); return 2; } - // parse command line options + // Parse mandatory command line options int qhb_version; try { qhb_version = std::stoi(argv[1]); @@ -165,14 +194,15 @@ int main(int argc, char *argv[]) { } std::string filename = argv[4]; + // Program args initialization int i=5; - int device(0), bit_depth(16), filter(0); float chunklen(0), totallen(0); float accelSamplingFrequency(25), gyroSamplingFrequency(25), magSamplingFrequency(20); float accelRangeScale(8), gyroRangeScale(250), magRangeScale(12); std::string imu_name; bool verbose(false); + // Retrieve program args while (i < argc){ try{ if (strcmp(argv[i], "--chunk_len") == 0 || strcmp(argv[i], "-c") == 0) { @@ -225,6 +255,6 @@ int main(int argc, char *argv[]) { } i++; } - // hand over to the recording function + // Hand over to the recording function return record(qhb_version, num_channels, rate, bit_depth/8, filter, filename, imu_name, chunklen, totallen, device, verbose, accelSamplingFrequency, gyroSamplingFrequency, magSamplingFrequency, accelRangeScale, gyroRangeScale, magRangeScale); } diff --git a/src/recorder.cpp b/src/recorder.cpp index 2f02a81..086fc9e 100644 --- a/src/recorder.cpp +++ b/src/recorder.cpp @@ -9,10 +9,12 @@ #include <unistd.h> #include <stdexcept> #include <iostream> +#include <iomanip> #include <vector> #include <array> #include <algorithm> #include <fstream> +#include <cstring> JasonRecorder::JasonRecorder(bool verbose) : verbose(verbose) { // create libusb context @@ -137,18 +139,6 @@ void JasonRecorder::start_recording(int qhb_version, std::uint8_t num_channels, send_message(START_ID, payload1); } else if (qhb_version == 3) { - std::vector<std::uint8_t> payload1 = { - START, - (std::uint8_t) ((sample_rate >> 24) & 0xFF), - (std::uint8_t) ((sample_rate >> 16) & 0xFF), - (std::uint8_t) ((sample_rate >> 8) & 0xFF), - (std::uint8_t) (sample_rate & 0xFF), - (std::uint8_t) (num_channels), - (std::uint8_t) (8 * depth), - (std::uint8_t) (1 * num_filter)}; - send_message(START_ID, payload1); - std::cout << "Start message sent\n"; - // std::array<unsigned char, 4> accelSamplingRateBytes = {0}; // std::array<unsigned char, 4> accelRangeScaleBytes = {0}; // getBytesFromFloat(accelSamplingRateBytes, accelSamplingRate); @@ -187,7 +177,7 @@ void JasonRecorder::start_recording(int qhb_version, std::uint8_t num_channels, // }; // send_message(SET_SENSOR, payload3); // std::cout << "Gyro set\n"; - // sleep(0.01); + // sleep(0.1); // std::array<unsigned char, 4> magSamplingRateBytes = {0}; // std::array<unsigned char, 4> magRangeScaleBytes = {0}; @@ -207,11 +197,21 @@ void JasonRecorder::start_recording(int qhb_version, std::uint8_t num_channels, // }; // send_message(SET_SENSOR, payload4); // std::cout << "Mag set\n"; - // sleep(0.01); - // libusb_clear_halt(handle, ENDPOINT_SEND); + std::vector<std::uint8_t> payload1 = { + START, + (std::uint8_t) ((sample_rate >> 24) & 0xFF), + (std::uint8_t) ((sample_rate >> 16) & 0xFF), + (std::uint8_t) ((sample_rate >> 8) & 0xFF), + (std::uint8_t) (sample_rate & 0xFF), + (std::uint8_t) (num_channels), + (std::uint8_t) (8 * depth), + (std::uint8_t) (1 * num_filter)}; + send_message(START_ID, payload1); + std::cout << "Start message sent\n"; } + this->qhb_version = qhb_version; this->num_channels = num_channels; this->sample_rate = sample_rate; this->depth = depth; @@ -249,7 +249,23 @@ size_t JasonRecorder::receive_message(uint8_t *buffer, size_t max_wait) { return received; } -void JasonRecorder::get_samples(std::vector<std::uint8_t> &samples, std::vector<std::uint8_t> &imu_data, bool planar, size_t max_wait) { +void JasonRecorder::hex_dump(const uint8_t* data, size_t size) { + /** + * @brief Debugging method that prints out the data in a hex format, in the same way as the Hex Editor + * + * @param data : pointer to the start of a data buffer + * @param size : quantity of bytes that you want to print out + */ + for (size_t i = 0; i < size; ++i) { + if (i % 16 == 0) std::cout << std::setw(4) << std::setfill('0') << std::hex << i << ": "; + std::cout << std::setw(2) << std::setfill('0') << std::hex + << static_cast<int>(data[i]) << " "; + if ((i + 1) % 16 == 0) std::cout << std::endl; + } + if (size % 16 != 0) std::cout << std::endl; +} + +void JasonRecorder::get_samples(std::vector<std::uint8_t> &samples, std::vector<std::uint8_t> &imu_data, uint64_t *packet_timestamp, bool planar, size_t max_wait) { if (!num_channels || !sample_rate) { throw std::logic_error("must call set_format() first"); } @@ -261,26 +277,42 @@ void JasonRecorder::get_samples(std::vector<std::uint8_t> &samples, std::vector< //size_t length = buffer[1] << 8 + buffer[2]; if (buffer[0] != FRAME_START); // invalid message else if ((((std::uint16_t) buffer[1] << 8 )|(buffer[2])) == DATA_ID) { - std::cout << "IMU Data buffer\n"; - // find the beginning and length of the samples in the buffer - size_t start = this->additional_data_size + 6; + size_t audio_start = this->additionnal_data_buffer_size; + size_t imu_start = 0; + if (this->qhb_version == 3) + { + imu_start = this->additionnal_buffer_header_size_v3; + // Retrieve the packet timestamp for later writing + uint64_t packet_timestamp_value = 0; + // hex_dump(buffer.data(), 16); + for (int i = 0; i < 8; ++i){ + packet_timestamp_value |= static_cast<uint64_t>(buffer[7 + i]) << (8 * (8 - i)); + } + memcpy(packet_timestamp, &packet_timestamp_value, 8); + // std::cout << static_cast<int>(*packet_timestamp) << std::endl; + } + else if (this->qhb_version == 2) + { + imu_start = this->additionnal_buffer_header_size_v2; + } + imu_data.resize(0); - imu_data.reserve(this->additional_data_size); - imu_data.insert(imu_data.begin(), &buffer[6], &buffer[start]); - size_t num_samples = (received - start); - num_samples = (num_samples / (num_channels * this->depth)) * num_channels * this->depth; + imu_data.reserve(this->additionnal_data_buffer_size); + imu_data.insert(imu_data.begin(), &buffer[0], &buffer[audio_start]); + size_t num_samples = (received - audio_start); + num_samples = (num_samples / (num_channels * this->depth)) * num_channels * this->depth; // Mais d'où ça sort ça encore TODO: Voir si ça marche toujours quand on l'enlève (y'a pas de raison) // copy data to provided vector if (planar || (num_channels == 1)) { // copy out directly samples.resize(0); samples.reserve(num_samples); - samples.insert(samples.end(), &buffer[start], &buffer[start] + num_samples); + samples.insert(samples.end(), &buffer[audio_start], &buffer[audio_start] + num_samples); } else { // convert from blocked channels to interleaved channels samples.resize(num_samples); - JasonRecorder::interleave_channels(&buffer[start], + JasonRecorder::interleave_channels(&buffer[audio_start], samples.data(), num_samples, this->num_channels, this->depth); } diff --git a/src/recorder.h b/src/recorder.h index 98a7a48..4891b43 100644 --- a/src/recorder.h +++ b/src/recorder.h @@ -41,6 +41,7 @@ class JasonRecorder { const std::uint16_t GYRO = 0x02; const std::uint16_t MAG = 0x03; private: + void hex_dump(const uint8_t* data, size_t size); // libusb handles struct libusb_context *ctx; std::vector<struct libusb_device*> devices; @@ -78,7 +79,10 @@ private: size_t receive_message(uint8_t *buffer, size_t max_wait = 0); // device state, as far as known - size_t additional_data_size = 730; + size_t additionnal_data_buffer_size = 736; // Size of the AdditionnalBuffer data + size_t additionnal_buffer_header_size_v3 = 16; // Size of the AdditionnalBuffer header for QHBv3 + size_t additionnal_buffer_header_size_v2 = 6; // Size of the AdditionnalBuffer header for QHBv2 + std::uint8_t qhb_version = 0; std::uint8_t num_channels = 0; std::uint8_t depth = 0; std::uint8_t num_filter = 0; @@ -119,7 +123,7 @@ public: * or if a different type of packet was received, returns with an empty * samples vector. Set to zero to wait indefinitely for a sample packet. */ - void get_samples(std::vector<std::uint8_t> &samples, std::vector<std::uint8_t> &imu_data, bool planar=false, size_t max_wait=0); + void get_samples(std::vector<std::uint8_t> &samples, std::vector<std::uint8_t> &imu_data, uint64_t *packet_timestamp, bool planar=false, size_t max_wait=0); /** Converts samples from planar to interleaved form. * \param[in] input A pointer to the planar samples to read. * \param[out] output A pointer to write the interleaved samples to. -- GitLab From 3d5123c59cdf0a86609e44f2053f2f73fe49f0f1 Mon Sep 17 00:00:00 2001 From: Philemon Prevot <philemon.prevot@lis-lab.fr> Date: Fri, 25 Apr 2025 14:37:40 +0200 Subject: [PATCH 09/15] Correct parametrization packets and adding sleep for QHB processing time (it works now!) --- src/recorder.cpp | 103 +++++++++++++++++++++-------------------------- 1 file changed, 47 insertions(+), 56 deletions(-) diff --git a/src/recorder.cpp b/src/recorder.cpp index 086fc9e..e24fad9 100644 --- a/src/recorder.cpp +++ b/src/recorder.cpp @@ -15,6 +15,8 @@ #include <algorithm> #include <fstream> #include <cstring> +#include <thread> +#include <chrono> JasonRecorder::JasonRecorder(bool verbose) : verbose(verbose) { // create libusb context @@ -139,64 +141,53 @@ void JasonRecorder::start_recording(int qhb_version, std::uint8_t num_channels, send_message(START_ID, payload1); } else if (qhb_version == 3) { - // std::array<unsigned char, 4> accelSamplingRateBytes = {0}; - // std::array<unsigned char, 4> accelRangeScaleBytes = {0}; - // getBytesFromFloat(accelSamplingRateBytes, accelSamplingRate); - // getBytesFromFloat(accelRangeScaleBytes, accelRangeScale); - // std::vector<std::uint8_t> payload2 = { - // (std::uint8_t) (ACCEL), - // (std::uint8_t) (0x00), - // (std::uint8_t) (accelRangeScaleBytes[0]), - // (std::uint8_t) (accelRangeScaleBytes[1]), - // (std::uint8_t) (accelRangeScaleBytes[2]), - // (std::uint8_t) (accelRangeScaleBytes[3]), - // (std::uint8_t) (accelSamplingRateBytes[0]), - // (std::uint8_t) (accelSamplingRateBytes[1]), - // (std::uint8_t) (accelSamplingRateBytes[2]), - // (std::uint8_t) (accelSamplingRateBytes[3]) - // }; - // send_message(SET_SENSOR, payload2); - // std::cout << "Accel set\n"; - // sleep(0.01); + std::vector<std::uint8_t> payload2 = { + (std::uint8_t) (ACCEL), + (std::uint8_t) (0x00), + (std::uint8_t) ((accelRangeScale >> 24) & 0xFF), + (std::uint8_t) ((accelRangeScale >> 16) & 0xFF), + (std::uint8_t) ((accelRangeScale >> 8) & 0xFF), + (std::uint8_t) (accelRangeScale & 0xFF), + (std::uint8_t) ((accelSamplingRate >> 24) & 0xFF), + (std::uint8_t) ((accelSamplingRate >> 16) & 0xFF), + (std::uint8_t) ((accelSamplingRate >> 8) & 0xFF), + (std::uint8_t) (accelSamplingRate & 0xFF) + }; + send_message(SET_SENSOR, payload2); + std::cout << "Accel set\n"; + std::this_thread::sleep_for(std::chrono::milliseconds(10)); - // std::array<unsigned char, 4> gyroSamplingRateBytes = {0}; - // std::array<unsigned char, 4> gyroRangeScaleBytes = {0}; - // getBytesFromFloat(gyroSamplingRateBytes, gyroSamplingRate); - // getBytesFromFloat(gyroRangeScaleBytes, gyroRangeScale); - // std::vector<std::uint8_t> payload3 = { - // (std::uint8_t) (GYRO), - // (std::uint8_t) (0x00), - // (std::uint8_t) (gyroRangeScaleBytes[0]), - // (std::uint8_t) (gyroRangeScaleBytes[1]), - // (std::uint8_t) (gyroRangeScaleBytes[2]), - // (std::uint8_t) (gyroRangeScaleBytes[3]), - // (std::uint8_t) (gyroSamplingRateBytes[0]), - // (std::uint8_t) (gyroSamplingRateBytes[1]), - // (std::uint8_t) (gyroSamplingRateBytes[2]), - // (std::uint8_t) (gyroSamplingRateBytes[3]) - // }; - // send_message(SET_SENSOR, payload3); - // std::cout << "Gyro set\n"; - // sleep(0.1); + std::vector<std::uint8_t> payload3 = { + (std::uint8_t) (GYRO), + (std::uint8_t) (0x00), + (std::uint8_t) ((gyroRangeScale >> 24) & 0xFF), + (std::uint8_t) ((gyroRangeScale >> 16) & 0xFF), + (std::uint8_t) ((gyroRangeScale >> 8) & 0xFF), + (std::uint8_t) (gyroRangeScale & 0xFF), + (std::uint8_t) ((gyroSamplingRate >> 24) & 0xFF), + (std::uint8_t) ((gyroSamplingRate >> 16) & 0xFF), + (std::uint8_t) ((gyroSamplingRate >> 8) & 0xFF), + (std::uint8_t) (gyroSamplingRate & 0xFF) + }; + send_message(SET_SENSOR, payload3); + std::cout << "Gyro set\n"; + std::this_thread::sleep_for(std::chrono::milliseconds(10)); - // std::array<unsigned char, 4> magSamplingRateBytes = {0}; - // std::array<unsigned char, 4> magRangeScaleBytes = {0}; - // getBytesFromFloat(magSamplingRateBytes, magSamplingRate); - // getBytesFromFloat(magRangeScaleBytes, magRangeScale); - // std::vector<uint8_t> payload4 { - // (std::uint8_t) (MAG), - // (std::uint8_t) (0x00), - // (std::uint8_t) (magRangeScaleBytes[0]), - // (std::uint8_t) (magRangeScaleBytes[1]), - // (std::uint8_t) (magRangeScaleBytes[2]), - // (std::uint8_t) (magRangeScaleBytes[3]), - // (std::uint8_t) (magSamplingRateBytes[0]), - // (std::uint8_t) (magSamplingRateBytes[1]), - // (std::uint8_t) (magSamplingRateBytes[2]), - // (std::uint8_t) (magSamplingRateBytes[3]) - // }; - // send_message(SET_SENSOR, payload4); - // std::cout << "Mag set\n"; + std::vector<uint8_t> payload4 { + (std::uint8_t) (MAG), + (std::uint8_t) (0x00), + (std::uint8_t) ((magRangeScale >> 24) & 0xFF), + (std::uint8_t) ((magRangeScale >> 16) & 0xFF), + (std::uint8_t) ((magRangeScale >> 8) & 0xFF), + (std::uint8_t) (magRangeScale & 0xFF), + (std::uint8_t) ((magSamplingRate >> 24) & 0xFF), + (std::uint8_t) ((magSamplingRate >> 16) & 0xFF), + (std::uint8_t) ((magSamplingRate >> 8) & 0xFF), + (std::uint8_t) (magSamplingRate & 0xFF), + }; + send_message(SET_SENSOR, payload4); + std::cout << "Mag set\n"; + std::this_thread::sleep_for(std::chrono::milliseconds(10)); std::vector<std::uint8_t> payload1 = { START, -- GitLab From 65d201ed19bf5985f2ff4b0a22838056b654473a Mon Sep 17 00:00:00 2001 From: Philemon Prevot <philemon.prevot@lis-lab.fr> Date: Mon, 12 May 2025 15:01:41 +0200 Subject: [PATCH 10/15] Update file README.md --- README.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index 82d0d88..635835d 100644 --- a/README.md +++ b/README.md @@ -108,21 +108,21 @@ Optional arguments: As an example, to record a single 30-minute file of 2 channels at 16 kHz, run: ``` -jasonrec 2 16000 recording.wav -t 1800 +jasonrec 3 2 16000 recording.wav -t 1800 ``` To record 4 channels at 128 kHz sample rate in 5-minute chunks with filenames based on time stamps, without stopping, run: ``` -jasonrec 4 128000 %Y-%m-%d_%H-%M-%S.wav -c 300 +jasonrec 3 4 128000 %Y-%m-%d_%H-%M-%S.wav -c 300 ``` To record the same 4 channels at 128 kHz sample rate in 5-minute chunks with filenames based on time stamps, without stopping, but with the saving of the imu data run: ``` -jasonrec 4 128000 %Y-%m-%d_%H-%M-%S.wav -c 300 -i %Y-%m-%d_%H-%M-%S.csv +jasonrec 3 4 128000 %Y-%m-%d_%H-%M-%S.wav -c 300 -i %Y-%m-%d_%H-%M-%S.csv ``` File names may also include directory names based on time stamps, but the -- GitLab From 5bafdb01f40c64f2b91d7c2a6448110d8198c81a Mon Sep 17 00:00:00 2001 From: Philemon Prevot <philemon.prevot@lis-lab.fr> Date: Tue, 27 May 2025 17:45:50 +0200 Subject: [PATCH 11/15] Remove packet timestamp shenaningans from filewriter --- src/filewriter.cpp | 28 ++++++++++++++-------------- src/filewriter.h | 6 +++--- src/main.cpp | 4 ++-- 3 files changed, 19 insertions(+), 19 deletions(-) diff --git a/src/filewriter.cpp b/src/filewriter.cpp index 6a943d3..0285cb1 100644 --- a/src/filewriter.cpp +++ b/src/filewriter.cpp @@ -16,8 +16,8 @@ #include <cstring> #include <cmath> -FileWriter::FileWriter(std::string &filename_template, size_t qhb_version, size_t num_channels, size_t sample_rate, size_t depth, uint64_t packet_timestamp) : - filename_template(filename_template), qhb_version(qhb_version), num_channels(num_channels), sample_rate(sample_rate), depth(depth), packet_timestamp(packet_timestamp) { +FileWriter::FileWriter(std::string &filename_template, size_t qhb_version, size_t num_channels, size_t sample_rate, size_t depth) : + filename_template(filename_template), qhb_version(qhb_version), num_channels(num_channels), sample_rate(sample_rate), depth(depth) { // nothing } @@ -82,7 +82,7 @@ WavFileWriter::WavFileWriter(std::string &filename_template, size_t qhb_version, WavFileWriter::WavFileWriter(std::string &filename_template, size_t qhb_version, size_t num_channels, size_t sample_rate, size_t depth, size_t expected_num_samples) : - FileWriter(filename_template, qhb_version, num_channels, sample_rate, depth, 0), + FileWriter(filename_template, qhb_version, num_channels, sample_rate, depth), outfile(generate_filename(), std::ios::out | std::ios::binary | std::ios::trunc), samples_written(0) { // check if we could open the file @@ -122,8 +122,8 @@ void WavFileWriter::write(uint8_t *samples, size_t num_samples, uint8_t *imu_dat samples_written += num_samples /(num_channels * depth); } -IMUFileWriter::IMUFileWriter(std::string &filename_template, size_t qhb_version, size_t num_channels, size_t sample_rate, size_t depth, uint64_t packet_timestamp) : - FileWriter(filename_template, qhb_version, num_channels, sample_rate, depth, packet_timestamp), +IMUFileWriter::IMUFileWriter(std::string &filename_template, size_t qhb_version, size_t num_channels, size_t sample_rate, size_t depth) : + FileWriter(filename_template, qhb_version, num_channels, sample_rate, depth), outfile(generate_filename(), std::ios::out | std::ios::trunc), last_timestamp(0), @@ -477,8 +477,8 @@ void IMUFileWriter::ProcessDecodedMessage(int msgFunction, int msgPayloadLength, void IMUFileWriter::write(uint8_t *sample, size_t size, uint8_t *imu_data) { if(this->qhb_version == 3) { - std::cout << static_cast<int>(this->packet_timestamp) << std::endl; - outfile << "PACKET TIMESTAMP: " << this->packet_timestamp << "\n"; + // std::cout << static_cast<int>(this->packet_timestamp) << std::endl; + // outfile << "PACKET TIMESTAMP: " << this->packet_timestamp << "\n"; for(int i=0; i<this->additionnal_data_size; i++) { DecodeMessage(imu_data[i]); @@ -520,7 +520,7 @@ size_t IMUFileWriter::get_last_timestamp(){ SplitWavFileWriter::SplitWavFileWriter(std::string &filename_template, size_t qhb_version, size_t num_channels, size_t sample_rate, size_t depth, size_t samples_per_file) : - FileWriter(filename_template, qhb_version, num_channels, sample_rate, depth, 0), + FileWriter(filename_template, qhb_version, num_channels, sample_rate, depth), samples_per_file(samples_per_file), current_file(nullptr), current_file_samples_written(0) { @@ -562,8 +562,8 @@ void SplitWavFileWriter::write(uint8_t *samples, size_t num_samples, uint8_t *im SplitIMUWavFileWriter::SplitIMUWavFileWriter(std::string &filename_template, std::string &imu_name_template, size_t qhb_version, - size_t num_channels, size_t sample_rate, size_t depth, size_t samples_per_file, uint64_t packet_timestamp): - FileWriter(filename_template, qhb_version, num_channels, sample_rate, depth, packet_timestamp), + size_t num_channels, size_t sample_rate, size_t depth, size_t samples_per_file): + FileWriter(filename_template, qhb_version, num_channels, sample_rate, depth), imu_name_template(imu_name_template), samples_per_file(samples_per_file), current_file(nullptr), @@ -591,10 +591,10 @@ void SplitIMUWavFileWriter::write(uint8_t *samples, size_t num_samples, uint8_t* if (imu_file) { max_timestamp = imu_file->get_last_timestamp(); delete imu_file; - imu_file = new IMUFileWriter(imu_name_template, qhb_version, num_channels, sample_rate, depth, this->packet_timestamp); + imu_file = new IMUFileWriter(imu_name_template, qhb_version, num_channels, sample_rate, depth); } } - if (!imu_file) imu_file = new IMUFileWriter(imu_name_template, qhb_version, num_channels, sample_rate, depth, this->packet_timestamp); + if (!imu_file) imu_file = new IMUFileWriter(imu_name_template, qhb_version, num_channels, sample_rate, depth); // write out as much as fits into the current file and move the pointer size_t missing = samples_per_file - current_file_samples_written; current_file->write(samples, missing * num_channels * depth, imu_data); @@ -617,10 +617,10 @@ void SplitIMUWavFileWriter::write(uint8_t *samples, size_t num_samples, uint8_t* if (imu_file) { max_timestamp = imu_file->get_last_timestamp(); delete imu_file; - imu_file = new IMUFileWriter(imu_name_template, qhb_version, num_channels, sample_rate, depth, this->packet_timestamp); + imu_file = new IMUFileWriter(imu_name_template, qhb_version, num_channels, sample_rate, depth); } } - if (!imu_file) imu_file = new IMUFileWriter(imu_name_template, qhb_version, num_channels, sample_rate, depth, this->packet_timestamp); + if (!imu_file) imu_file = new IMUFileWriter(imu_name_template, qhb_version, num_channels, sample_rate, depth); current_file->write(samples, available * num_channels * depth, imu_data); if (imu_data) imu_file->write(samples, available * num_channels * depth, imu_data); imu_data = nullptr; diff --git a/src/filewriter.h b/src/filewriter.h index cae305b..739c9c8 100644 --- a/src/filewriter.h +++ b/src/filewriter.h @@ -36,7 +36,7 @@ protected: public: /** Abstract constructor to be used by subclasses. */ - FileWriter(std::string &filename_template, size_t qhb_version, size_t num_channels, size_t sample_rate, size_t depth, uint64_t packet_timestamp); + FileWriter(std::string &filename_template, size_t qhb_version, size_t num_channels, size_t sample_rate, size_t depth); virtual ~FileWriter(); /** Writes out the given vector of 8-bit samples. @@ -244,7 +244,7 @@ public: int msgPayloadLength, const unsigned char msgPayload[]); void DecodeMessage(unsigned char c); - IMUFileWriter(std::string &filename_template, size_t qhb_version, size_t num_channels, size_t sample_rate, size_t depth, uint64_t packet_timestamp); + IMUFileWriter(std::string &filename_template, size_t qhb_version, size_t num_channels, size_t sample_rate, size_t depth); void write(uint8_t *samples, size_t num_samples, uint8_t *imu_data) override; size_t get_last_timestamp(); }; @@ -277,7 +277,7 @@ public: * that will be written to a file before starting the next one. */ SplitIMUWavFileWriter(std::string &filename_template, std::string &imu_name_template, size_t qhb_version, size_t num_channels, - size_t sample_rate, size_t depth, size_t samples_per_file, uint64_t packet_timestamp); + size_t sample_rate, size_t depth, size_t samples_per_file); ~SplitIMUWavFileWriter() override; void write(uint8_t *samples, size_t num_samples, uint8_t *imu_data) override; diff --git a/src/main.cpp b/src/main.cpp index b905fbc..3d2d835 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -85,13 +85,13 @@ int record(size_t qhb_version, size_t channels, size_t rate, size_t depth, size_ } else if (chunklen > 0) { // When we record audio and imu, in several files of chunklen length // implementation note: in C++14 we would use std::make_unique<SplitWavFileWriter>(...) - filewriter.reset(new SplitIMUWavFileWriter(filename, imu_name, qhb_version, channels, rate, depth, chunklen * rate, 0)); + filewriter.reset(new SplitIMUWavFileWriter(filename, imu_name, qhb_version, channels, rate, depth, chunklen * rate)); } else if (imu_name.empty()){ // When we only record audio data, in one file only filewriter.reset(new WavFileWriter(filename, qhb_version, channels, rate, depth, totallen * rate)); } else{ // When we record both audio and imu in one file only - filewriter.reset(new SplitIMUWavFileWriter(filename, imu_name, qhb_version, channels, rate, depth, totallen * rate, 0)); + filewriter.reset(new SplitIMUWavFileWriter(filename, imu_name, qhb_version, channels, rate, depth, totallen * rate)); } // Start the recording and writing main loop std::cout << "Starting to record..." << std::endl; -- GitLab From 8862df1dcb0e95e8d17c82e55004be9cd4c35075 Mon Sep 17 00:00:00 2001 From: Philemon Prevot <philemon.prevot@lis-lab.fr> Date: Tue, 27 May 2025 17:49:16 +0200 Subject: [PATCH 12/15] Remove filewriter timestamp from recorder and replace by global_packet_timestamp --- src/main.cpp | 4 +++- src/recorder.cpp | 7 ++----- src/recorder.h | 4 +++- 3 files changed, 8 insertions(+), 7 deletions(-) diff --git a/src/main.cpp b/src/main.cpp index 3d2d835..984eef1 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -13,6 +13,8 @@ #include "filewriter.h" #include "cleanexit.h" +uint64_t global_packet_timestamp = 0; + void print_usage(char *name) { std::cout << "SMIoT JASON Qualilife sound recorder"; #ifdef JASONREC_VERSION @@ -113,7 +115,7 @@ int record(size_t qhb_version, size_t channels, size_t rate, size_t depth, size_ break; } // Get audio and imu samples - recorder.get_samples(samples, imu_data, &filewriter->packet_timestamp, false, 500); + recorder.get_samples(samples, imu_data, false, 500); if (!samples.empty()) { total_samples_read += samples.size() / sample_size; // if we have too much now, crop the last packet diff --git a/src/recorder.cpp b/src/recorder.cpp index e24fad9..5c5313b 100644 --- a/src/recorder.cpp +++ b/src/recorder.cpp @@ -256,7 +256,7 @@ void JasonRecorder::hex_dump(const uint8_t* data, size_t size) { if (size % 16 != 0) std::cout << std::endl; } -void JasonRecorder::get_samples(std::vector<std::uint8_t> &samples, std::vector<std::uint8_t> &imu_data, uint64_t *packet_timestamp, bool planar, size_t max_wait) { +void JasonRecorder::get_samples(std::vector<std::uint8_t> &samples, std::vector<std::uint8_t> &imu_data, bool planar, size_t max_wait) { if (!num_channels || !sample_rate) { throw std::logic_error("must call set_format() first"); } @@ -275,13 +275,10 @@ void JasonRecorder::get_samples(std::vector<std::uint8_t> &samples, std::vector< { imu_start = this->additionnal_buffer_header_size_v3; // Retrieve the packet timestamp for later writing - uint64_t packet_timestamp_value = 0; // hex_dump(buffer.data(), 16); for (int i = 0; i < 8; ++i){ - packet_timestamp_value |= static_cast<uint64_t>(buffer[7 + i]) << (8 * (8 - i)); + global_packet_timestamp |= static_cast<uint64_t>(buffer[7 + i]) << (8 * (8 - i)); } - memcpy(packet_timestamp, &packet_timestamp_value, 8); - // std::cout << static_cast<int>(*packet_timestamp) << std::endl; } else if (this->qhb_version == 2) { diff --git a/src/recorder.h b/src/recorder.h index 4891b43..9a9dc2d 100644 --- a/src/recorder.h +++ b/src/recorder.h @@ -17,6 +17,8 @@ #define MAX_MSG_LENGTH 65536 #define MAX_CHANNELS 6 +extern uint64_t global_packet_timestamp; + /** Class for retrieving sample data from a JASON Qualilife sound card. */ class JasonRecorder { @@ -123,7 +125,7 @@ public: * or if a different type of packet was received, returns with an empty * samples vector. Set to zero to wait indefinitely for a sample packet. */ - void get_samples(std::vector<std::uint8_t> &samples, std::vector<std::uint8_t> &imu_data, uint64_t *packet_timestamp, bool planar=false, size_t max_wait=0); + void get_samples(std::vector<std::uint8_t> &samples, std::vector<std::uint8_t> &imu_data, bool planar=false, size_t max_wait=0); /** Converts samples from planar to interleaved form. * \param[in] input A pointer to the planar samples to read. * \param[out] output A pointer to write the interleaved samples to. -- GitLab From 20a732606a59f60b56fdb86dd685a269ac30ba54 Mon Sep 17 00:00:00 2001 From: Philemon Prevot <philemon.prevot@lis-lab.fr> Date: Tue, 27 May 2025 20:30:41 +0200 Subject: [PATCH 13/15] Removing sensor parameters (not working for now), BUT RECORDER WORKING --- .vscode/launch.json | 2 +- src/filewriter.cpp | 12 +++++- src/recorder.cpp | 95 +++++++++++++++++++++++---------------------- 3 files changed, 59 insertions(+), 50 deletions(-) diff --git a/.vscode/launch.json b/.vscode/launch.json index be5fa7e..c3caa63 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -7,7 +7,7 @@ "preLaunchTask": "Configure and Build HighBlueParsers", "request": "launch", "program": "${workspaceFolder}/build/src/jasonrec", - "args": ["3", "1", "256000", "debug_audio_file_%Y%m%d_%H%M%S.wav", "--imu", "debug_imu_file_%Y%m%d_%H%M%S.csv"], // replace with real args + "args": ["3", "2", "256000", "debug_audio_file_%Y%m%d_%H%M%S.wav", "--imu", "debug_imu_file_%Y%m%d_%H%M%S.csv"], // replace with real args "stopAtEntry": false, "cwd": "${workspaceFolder}", "environment": [], diff --git a/src/filewriter.cpp b/src/filewriter.cpp index 0285cb1..3b25eca 100644 --- a/src/filewriter.cpp +++ b/src/filewriter.cpp @@ -6,6 +6,7 @@ */ #include "filewriter.h" +#include "recorder.h" #include "macros.h" #include <stdexcept> #include <vector> @@ -304,8 +305,16 @@ void IMUFileWriter::ProcessDecodedMessage(int msgFunction, int msgPayloadLength, unsigned char id = msgPayload[1]; unsigned char nbChannels = msgPayload[2]; float rangeScale = GetFloatSafe(msgPayload, 3); + u_char tab[4]; + for (int i=0; i<4; i++){ + tab[i] = msgPayload[3+3-i]; + } unsigned char resolutionBits = msgPayload[7]; float samplingFrequency = GetFloatSafe(msgPayload, 8); + u_char tab2[4]; + for (int i=0; i<4; i++){ + tab2[i] = msgPayload[8+3-i]; + } unsigned short nbSamples = msgPayload[12]; unsigned char dataSize = (resolutionBits / 8); @@ -477,8 +486,7 @@ void IMUFileWriter::ProcessDecodedMessage(int msgFunction, int msgPayloadLength, void IMUFileWriter::write(uint8_t *sample, size_t size, uint8_t *imu_data) { if(this->qhb_version == 3) { - // std::cout << static_cast<int>(this->packet_timestamp) << std::endl; - // outfile << "PACKET TIMESTAMP: " << this->packet_timestamp << "\n"; + outfile << "PACKET TIMESTAMP: " << global_packet_timestamp << "\n"; for(int i=0; i<this->additionnal_data_size; i++) { DecodeMessage(imu_data[i]); diff --git a/src/recorder.cpp b/src/recorder.cpp index 5c5313b..b2ab254 100644 --- a/src/recorder.cpp +++ b/src/recorder.cpp @@ -141,53 +141,53 @@ void JasonRecorder::start_recording(int qhb_version, std::uint8_t num_channels, send_message(START_ID, payload1); } else if (qhb_version == 3) { - std::vector<std::uint8_t> payload2 = { - (std::uint8_t) (ACCEL), - (std::uint8_t) (0x00), - (std::uint8_t) ((accelRangeScale >> 24) & 0xFF), - (std::uint8_t) ((accelRangeScale >> 16) & 0xFF), - (std::uint8_t) ((accelRangeScale >> 8) & 0xFF), - (std::uint8_t) (accelRangeScale & 0xFF), - (std::uint8_t) ((accelSamplingRate >> 24) & 0xFF), - (std::uint8_t) ((accelSamplingRate >> 16) & 0xFF), - (std::uint8_t) ((accelSamplingRate >> 8) & 0xFF), - (std::uint8_t) (accelSamplingRate & 0xFF) - }; - send_message(SET_SENSOR, payload2); - std::cout << "Accel set\n"; - std::this_thread::sleep_for(std::chrono::milliseconds(10)); + // std::vector<std::uint8_t> payload2 = { + // (std::uint8_t) (ACCEL), + // (std::uint8_t) (0x00), + // (std::uint8_t) ((accelRangeScale >> 24) & 0xFF), + // (std::uint8_t) ((accelRangeScale >> 16) & 0xFF), + // (std::uint8_t) ((accelRangeScale >> 8) & 0xFF), + // (std::uint8_t) (accelRangeScale & 0xFF), + // (std::uint8_t) ((accelSamplingRate >> 24) & 0xFF), + // (std::uint8_t) ((accelSamplingRate >> 16) & 0xFF), + // (std::uint8_t) ((accelSamplingRate >> 8) & 0xFF), + // (std::uint8_t) (accelSamplingRate & 0xFF) + // }; + // send_message(SET_SENSOR, payload2); + // std::cout << "Accel set\n"; + // std::this_thread::sleep_for(std::chrono::milliseconds(10)); - std::vector<std::uint8_t> payload3 = { - (std::uint8_t) (GYRO), - (std::uint8_t) (0x00), - (std::uint8_t) ((gyroRangeScale >> 24) & 0xFF), - (std::uint8_t) ((gyroRangeScale >> 16) & 0xFF), - (std::uint8_t) ((gyroRangeScale >> 8) & 0xFF), - (std::uint8_t) (gyroRangeScale & 0xFF), - (std::uint8_t) ((gyroSamplingRate >> 24) & 0xFF), - (std::uint8_t) ((gyroSamplingRate >> 16) & 0xFF), - (std::uint8_t) ((gyroSamplingRate >> 8) & 0xFF), - (std::uint8_t) (gyroSamplingRate & 0xFF) - }; - send_message(SET_SENSOR, payload3); - std::cout << "Gyro set\n"; - std::this_thread::sleep_for(std::chrono::milliseconds(10)); + // std::vector<std::uint8_t> payload3 = { + // (std::uint8_t) (GYRO), + // (std::uint8_t) (0x00), + // (std::uint8_t) ((gyroRangeScale >> 24) & 0xFF), + // (std::uint8_t) ((gyroRangeScale >> 16) & 0xFF), + // (std::uint8_t) ((gyroRangeScale >> 8) & 0xFF), + // (std::uint8_t) (gyroRangeScale & 0xFF), + // (std::uint8_t) ((gyroSamplingRate >> 24) & 0xFF), + // (std::uint8_t) ((gyroSamplingRate >> 16) & 0xFF), + // (std::uint8_t) ((gyroSamplingRate >> 8) & 0xFF), + // (std::uint8_t) (gyroSamplingRate & 0xFF) + // }; + // send_message(SET_SENSOR, payload3); + // std::cout << "Gyro set\n"; + // std::this_thread::sleep_for(std::chrono::milliseconds(10)); - std::vector<uint8_t> payload4 { - (std::uint8_t) (MAG), - (std::uint8_t) (0x00), - (std::uint8_t) ((magRangeScale >> 24) & 0xFF), - (std::uint8_t) ((magRangeScale >> 16) & 0xFF), - (std::uint8_t) ((magRangeScale >> 8) & 0xFF), - (std::uint8_t) (magRangeScale & 0xFF), - (std::uint8_t) ((magSamplingRate >> 24) & 0xFF), - (std::uint8_t) ((magSamplingRate >> 16) & 0xFF), - (std::uint8_t) ((magSamplingRate >> 8) & 0xFF), - (std::uint8_t) (magSamplingRate & 0xFF), - }; - send_message(SET_SENSOR, payload4); - std::cout << "Mag set\n"; - std::this_thread::sleep_for(std::chrono::milliseconds(10)); + // std::vector<uint8_t> payload4 { + // (std::uint8_t) (MAG), + // (std::uint8_t) (0x00), + // (std::uint8_t) ((magRangeScale >> 24) & 0xFF), + // (std::uint8_t) ((magRangeScale >> 16) & 0xFF), + // (std::uint8_t) ((magRangeScale >> 8) & 0xFF), + // (std::uint8_t) (magRangeScale & 0xFF), + // (std::uint8_t) ((magSamplingRate >> 24) & 0xFF), + // (std::uint8_t) ((magSamplingRate >> 16) & 0xFF), + // (std::uint8_t) ((magSamplingRate >> 8) & 0xFF), + // (std::uint8_t) (magSamplingRate & 0xFF), + // }; + // send_message(SET_SENSOR, payload4); + // std::cout << "Mag set\n"; + // std::this_thread::sleep_for(std::chrono::milliseconds(10)); std::vector<std::uint8_t> payload1 = { START, @@ -275,9 +275,10 @@ void JasonRecorder::get_samples(std::vector<std::uint8_t> &samples, std::vector< { imu_start = this->additionnal_buffer_header_size_v3; // Retrieve the packet timestamp for later writing - // hex_dump(buffer.data(), 16); + //hex_dump(buffer.data(), 16); for (int i = 0; i < 8; ++i){ - global_packet_timestamp |= static_cast<uint64_t>(buffer[7 + i]) << (8 * (8 - i)); + global_packet_timestamp <<= 8; + global_packet_timestamp |= static_cast<uint64_t>(buffer[7 + i]); } } else if (this->qhb_version == 2) -- GitLab From 9d1050d572d8f37e9893ec4d0487dead2b7acfd2 Mon Sep 17 00:00:00 2001 From: Philemon Prevot <philemon.prevot@lis-lab.fr> Date: Wed, 28 May 2025 16:45:21 +0200 Subject: [PATCH 14/15] Correcting sensors normalization --- src/filewriter.cpp | 64 +++++++++++++++++++++++++++++++++++++--------- src/filewriter.h | 9 +++++++ 2 files changed, 61 insertions(+), 12 deletions(-) diff --git a/src/filewriter.cpp b/src/filewriter.cpp index 3b25eca..cced33d 100644 --- a/src/filewriter.cpp +++ b/src/filewriter.cpp @@ -241,6 +241,19 @@ void IMUFileWriter::DecodeMessage(unsigned char c) { } } +SensorXYZData NormalizeSensorsData(SensorXYZData raw_data, float range, unsigned char resolutionBits) +{ + SensorXYZData normalized_data; + double dataMaxValue = pow(2, resolutionBits) / 2; + + normalized_data.timeStamp = (double)raw_data.timeStamp / 1000; + + normalized_data.X = range / dataMaxValue * raw_data.X; + normalized_data.Y = range / dataMaxValue * raw_data.Y; + normalized_data.Z = range / dataMaxValue * raw_data.Z; + return normalized_data; +} + void IMUFileWriter::ProcessDecodedMessage(int msgFunction, int msgPayloadLength, const unsigned char* msgPayload) { unsigned int timeStamp = 0; switch(static_cast<short>(msgFunction)) { @@ -329,10 +342,19 @@ void IMUFileWriter::ProcessDecodedMessage(int msgFunction, int msgPayloadLength, lastAccelTimeStamp = 0; if (timeStamp > lastAccelTimeStamp) { lastAccelTimeStamp = timeStamp; - outfile << "ACCEL, " << timeStamp; - outfile << ", " << BUILD_INT16(msgPayload[17 + i * lengthPerSample],msgPayload[17 + i * lengthPerSample+1]) * ( rangeScale / dataMaxValue ); - outfile << ", " << BUILD_INT16(msgPayload[17 + dataSize + i * lengthPerSample],msgPayload[17 +dataSize + i * lengthPerSample+1]) * ( rangeScale / dataMaxValue ); - outfile << ", " << BUILD_INT16(msgPayload[17 + 2*dataSize + i * lengthPerSample],msgPayload[17 +2*dataSize + i * lengthPerSample+1]) * ( rangeScale / dataMaxValue ); + SensorXYZData dataXYZ; + dataXYZ.timeStamp = timeStamp; + if (dataSize == 2) + { + dataXYZ.X = BUILD_INT16(msgPayload[17 + i * lengthPerSample],msgPayload[17 + i * lengthPerSample+1]); + dataXYZ.Y = BUILD_INT16(msgPayload[17 + dataSize + i * lengthPerSample],msgPayload[17 +dataSize + i * lengthPerSample+1]); + dataXYZ.Z = BUILD_INT16(msgPayload[17 + 2*dataSize + i * lengthPerSample],msgPayload[17 +2*dataSize + i * lengthPerSample+1]); + } + SensorXYZData normalized_data = NormalizeSensorsData(dataXYZ, rangeScale, resolutionBits); + outfile << "ACCEL, " << normalized_data.timeStamp; + outfile << ", " << normalized_data.X; + outfile << ", " << normalized_data.Y; + outfile << ", " << normalized_data.Z; outfile << std::endl; } else { @@ -344,10 +366,19 @@ void IMUFileWriter::ProcessDecodedMessage(int msgFunction, int msgPayloadLength, lastGyroTimeStamp = 0; if (timeStamp > lastGyroTimeStamp) { lastGyroTimeStamp = timeStamp; - outfile << "GYRO, " << timeStamp; - outfile << ", " << BUILD_INT16(msgPayload[17 + i * lengthPerSample],msgPayload[17 + i * lengthPerSample+1]) * ( rangeScale / dataMaxValue); - outfile << ", " << BUILD_INT16(msgPayload[17 + dataSize + i * lengthPerSample],msgPayload[17 +dataSize + i * lengthPerSample+1]) * ( rangeScale / dataMaxValue); - outfile << ", " << BUILD_INT16(msgPayload[17 + 2*dataSize + i * lengthPerSample],msgPayload[17 +2*dataSize + i * lengthPerSample+1]) * ( rangeScale / dataMaxValue); + SensorXYZData dataXYZ; + dataXYZ.timeStamp = timeStamp; + if (dataSize == 2) + { + dataXYZ.X = BUILD_INT16(msgPayload[17 + i * lengthPerSample],msgPayload[17 + i * lengthPerSample+1]); + dataXYZ.Y = BUILD_INT16(msgPayload[17 + dataSize + i * lengthPerSample],msgPayload[17 +dataSize + i * lengthPerSample+1]); + dataXYZ.Z = BUILD_INT16(msgPayload[17 + 2*dataSize + i * lengthPerSample],msgPayload[17 +2*dataSize + i * lengthPerSample+1]); + } + SensorXYZData normalized_data = NormalizeSensorsData(dataXYZ, rangeScale, resolutionBits); + outfile << "GYRO, " << normalized_data.timeStamp; + outfile << ", " << normalized_data.X; + outfile << ", " << normalized_data.Y; + outfile << ", " << normalized_data.Z; outfile << std::endl; } else { @@ -359,10 +390,19 @@ void IMUFileWriter::ProcessDecodedMessage(int msgFunction, int msgPayloadLength, lastMagTimeStamp = 0; if (timeStamp > lastMagTimeStamp) { lastMagTimeStamp = timeStamp; - outfile << "MAG, " << timeStamp; - outfile << ", " << BUILD_INT16(msgPayload[17 + i * lengthPerSample+1],msgPayload[17 + i * lengthPerSample]) * ( rangeScale / dataMaxValue); - outfile << ", " << BUILD_INT16(msgPayload[17 + dataSize + i * lengthPerSample+1],msgPayload[17 +dataSize + i * lengthPerSample]) * ( rangeScale / dataMaxValue); - outfile << ", " << BUILD_INT16(msgPayload[17 + 2*dataSize + i * lengthPerSample+1],msgPayload[17 +2*dataSize + i * lengthPerSample]) * ( rangeScale / dataMaxValue); + SensorXYZData dataXYZ; + dataXYZ.timeStamp = timeStamp; + if (dataSize == 2) + { + dataXYZ.X = BUILD_INT16(msgPayload[17 + i * lengthPerSample],msgPayload[17 + i * lengthPerSample+1]); + dataXYZ.Y = BUILD_INT16(msgPayload[17 + dataSize + i * lengthPerSample],msgPayload[17 +dataSize + i * lengthPerSample+1]); + dataXYZ.Z = BUILD_INT16(msgPayload[17 + 2*dataSize + i * lengthPerSample],msgPayload[17 +2*dataSize + i * lengthPerSample+1]); + } + SensorXYZData normalized_data = NormalizeSensorsData(dataXYZ, rangeScale, resolutionBits); + outfile << "MAG, " << normalized_data.timeStamp; + outfile << ", " << normalized_data.X; + outfile << ", " << normalized_data.Y; + outfile << ", " << normalized_data.Z; outfile << std::endl; } else { diff --git a/src/filewriter.h b/src/filewriter.h index 739c9c8..65ceeac 100644 --- a/src/filewriter.h +++ b/src/filewriter.h @@ -283,5 +283,14 @@ public: void write(uint8_t *samples, size_t num_samples, uint8_t *imu_data) override; }; +typedef struct SensorXYZData_s +{ + double timeStamp; + double X; + double Y; + double Z; +}SensorXYZData; + +SensorXYZData NormalizeSensorsData(SensorXYZData data, float range, unsigned char resolutionBits); #endif // FILEWRITER_H -- GitLab From 1f8322f69b77ee63f61a14bcea8b8012c4dd652d Mon Sep 17 00:00:00 2001 From: Philemon Prevot <philemon.prevot@lis-lab.fr> Date: Tue, 3 Jun 2025 16:12:50 +0200 Subject: [PATCH 15/15] Correcting mag bytes reading --- src/filewriter.cpp | 10 +++++----- src/filewriter.h | 2 +- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/filewriter.cpp b/src/filewriter.cpp index cced33d..fb4c766 100644 --- a/src/filewriter.cpp +++ b/src/filewriter.cpp @@ -246,7 +246,7 @@ SensorXYZData NormalizeSensorsData(SensorXYZData raw_data, float range, unsigned SensorXYZData normalized_data; double dataMaxValue = pow(2, resolutionBits) / 2; - normalized_data.timeStamp = (double)raw_data.timeStamp / 1000; + normalized_data.timeStamp = (uint32_t)raw_data.timeStamp; normalized_data.X = range / dataMaxValue * raw_data.X; normalized_data.Y = range / dataMaxValue * raw_data.Y; @@ -394,11 +394,11 @@ void IMUFileWriter::ProcessDecodedMessage(int msgFunction, int msgPayloadLength, dataXYZ.timeStamp = timeStamp; if (dataSize == 2) { - dataXYZ.X = BUILD_INT16(msgPayload[17 + i * lengthPerSample],msgPayload[17 + i * lengthPerSample+1]); - dataXYZ.Y = BUILD_INT16(msgPayload[17 + dataSize + i * lengthPerSample],msgPayload[17 +dataSize + i * lengthPerSample+1]); - dataXYZ.Z = BUILD_INT16(msgPayload[17 + 2*dataSize + i * lengthPerSample],msgPayload[17 +2*dataSize + i * lengthPerSample+1]); + dataXYZ.X = BUILD_INT16(msgPayload[17 + i * lengthPerSample+1],msgPayload[17 + i * lengthPerSample]); + dataXYZ.Y = BUILD_INT16(msgPayload[17 + dataSize + i * lengthPerSample+1],msgPayload[17 +dataSize + i * lengthPerSample]); + dataXYZ.Z = BUILD_INT16(msgPayload[17 + 2*dataSize + i * lengthPerSample+1],msgPayload[17 +2*dataSize + i * lengthPerSample]); } - SensorXYZData normalized_data = NormalizeSensorsData(dataXYZ, rangeScale, resolutionBits); + SensorXYZData normalized_data = dataXYZ; //NormalizeSensorsData(dataXYZ, rangeScale, resolutionBits); outfile << "MAG, " << normalized_data.timeStamp; outfile << ", " << normalized_data.X; outfile << ", " << normalized_data.Y; diff --git a/src/filewriter.h b/src/filewriter.h index 65ceeac..2d1fac7 100644 --- a/src/filewriter.h +++ b/src/filewriter.h @@ -285,7 +285,7 @@ public: typedef struct SensorXYZData_s { - double timeStamp; + uint32_t timeStamp; double X; double Y; double Z; -- GitLab