diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..f3d6549d836622181a9d050a803b132fb753c32b
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1 @@
+/build/
\ No newline at end of file
diff --git a/CMakeLists.txt b/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..ccd70e9a9f1cc3ea46e4b4223602374bf6c0bb44
--- /dev/null
+++ b/CMakeLists.txt
@@ -0,0 +1,17 @@
+cmake_minimum_required(VERSION 3.1)
+if(POLICY CMP0063)
+ cmake_policy(SET CMP0063 NEW)
+endif()
+
+project(jasonrec)
+set(JASONREC_VERSION "2.0")
+add_definitions(-DJASONREC_VERSION="${JASONREC_VERSION}")
+
+find_path(LIBUSB_INCLUDE_DIR
+ NAMES libusb.h
+ PATH_SUFFIXES "include" "libusb" "libusb-1.0")
+find_library(LIBUSB_LIBRARY
+ NAMES usb usb-1.0
+ PATH_SUFFIXES "lib" "lib32" "lib64")
+
+add_subdirectory(src)
diff --git a/LICENSE b/LICENSE
new file mode 100644
index 0000000000000000000000000000000000000000..82994a1745c76a1fa435a0d2dbbc3b3b1fa8e7c4
--- /dev/null
+++ b/LICENSE
@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2018 Université de Toulon
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
diff --git a/README.md b/README.md
new file mode 100644
index 0000000000000000000000000000000000000000..93dfcc2864ccde39e62f6dad09956e55549f2890
--- /dev/null
+++ b/README.md
@@ -0,0 +1,111 @@
+SMIoT JASON Qualilife sound recorder
+====================================
+
+This repository provides `jasonrec`, a command line application to record audio
+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)
+* the `libusb` library and header
+* 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
+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
+cd build
+cmake .. -DCMAKE_BUILD_TYPE=Release -DLIBUSB_INCLUDE_DIR=<libusb_dir>/include -DLIBUSB_LIBRARY=<libusb_dir>/MS64/dll/libusb-1.0.lib -DCMAKE_INSTALL_PREFIX=../install
+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]
+Positional arguments:
+ 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
+ if CHUNK_LEN is specified. For miliseconds, use %z. Example: location/recording_%Y%m%d_%H%M%S_%z.wav
+Optional arguments:
+-h, --help show this help message and exit
+ --bit_depth, -b BIT_DEPTH: Size of each samples in bits. Must be a multiple of 8. (Default: 16)
+ --imu, -i IMU: IMU file name. Similar to FILENAME. Disable by default.
+ --filter, -f FILTER: Number of the filter to use. Must be between 0 and 2. (Default: 0)
+ --chunk_len, -c CHUNK_LEN: length per output file in seconds; will start a new file whenever
+ this length is reached. If not given or zero, will record a single file.
+ --total_len, -t TOTAL_LEN: Total recording length; will stop when this length is reached.
+ If not given or zero, will record continuously until killed.
+ --device, -d DEVICE: Which device to use in case multiple JASON cards are connected,
+ where 0 is the first, 1 is the second card found (and so on).
+ --verbose, -v Enable the printing of status message
+```
+
+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.
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..3a9ed88ff9ef8b948129bd0d5613341499e14236
--- /dev/null
+++ b/src/CMakeLists.txt
@@ -0,0 +1,20 @@
+add_executable(jasonrec
+ recorder.cpp
+ filewriter.cpp
+ cleanexit.cpp
+ main.cpp)
+# compile as C++11
+set_property(TARGET jasonrec
+ PROPERTY CXX_STANDARD 11)
+# do not export any symbols by default
+set_property(TARGET jasonrec
+ PROPERTY CXX_VISIBILITY_PRESET hidden)
+set_property(TARGET jasonrec
+ PROPERTY VISIBILITY_INLINES_HIDDEN true)
+
+target_include_directories(jasonrec
+ PRIVATE ${LIBUSB_INCLUDE_DIR})
+target_link_libraries(jasonrec
+ ${LIBUSB_LIBRARY})
+
+install(TARGETS jasonrec DESTINATION bin)
diff --git a/src/Makefile b/src/Makefile
new file mode 100644
index 0000000000000000000000000000000000000000..4ddfea9fa111a0fb4f45ee01323cefacf6c14076
--- /dev/null
+++ b/src/Makefile
@@ -0,0 +1,3 @@
+# If you cannot build with cmake, this file should work for debian-based Linux.
+all:
+ g++ -std=c++11 main.cpp recorder.cpp filewriter.cpp -o jasonrec -lusb-1.0 -I/usr/include/libusb-1.0/
diff --git a/src/cleanexit.cpp b/src/cleanexit.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..bd977c8327f21ed2dfce4625bb2d89edffabcce8
--- /dev/null
+++ b/src/cleanexit.cpp
@@ -0,0 +1,35 @@
+/**
+ * Simple signal handler for clean termination of an application.
+ *
+ * Author: Jan Schlüter <jan.schluter@lis-lab.fr>
+ */
+
+#include <atomic>
+#include <csignal>
+#ifdef __unix__
+#include <unistd.h>
+#endif
+
+std::atomic<bool> _exit_requested(false);
+
+void caught_signal(int)
+{
+ _exit_requested.store(true);
+}
+
+void allow_clean_exit() {
+#ifdef __unix__
+ struct sigaction sa = {0};
+ sa.sa_handler = caught_signal;
+ sigfillset(&sa.sa_mask);
+ sigaction(SIGINT, &sa, NULL);
+ sigaction(SIGTERM, &sa, NULL);
+#else
+ signal(SIGINT, caught_signal);
+ signal(SIGTERM, caught_signal);
+#endif
+}
+
+bool exit_requested() {
+ return _exit_requested.load();
+}
diff --git a/src/cleanexit.h b/src/cleanexit.h
new file mode 100644
index 0000000000000000000000000000000000000000..24a88dd473897e30a69d9f3010128315095de43b
--- /dev/null
+++ b/src/cleanexit.h
@@ -0,0 +1,21 @@
+/**
+ * Simple signal handler for clean termination of an application.
+ *
+ * Author: Jan Schlüter <jan.schluter@lis-lab.fr>
+ */
+
+#ifndef CLEANEXIT_H
+#define CLEANEXIT_H
+
+/**
+ * Set up signal handlers to make exit_requested() work.
+ */
+void allow_clean_exit();
+
+/**
+ * Check if the application was requested to terminate.
+ * \returns whether the application was requested to terminate.
+ */
+bool exit_requested();
+
+#endif
diff --git a/src/filewriter.cpp b/src/filewriter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..01009d01be8de7a453518546a3110b63ff266229
--- /dev/null
+++ b/src/filewriter.cpp
@@ -0,0 +1,602 @@
+/**
+ * Wave file writing, with or without splitting by length.
+ *
+ * Author: Jan Schlüter <jan.schluter@lis-lab.fr>
+ * Author: Maxence Ferrari <maxence.ferrari@lis-lab.fr>
+ */
+
+#include "filewriter.h"
+#include "macros.h"
+#include <stdexcept>
+#include <vector>
+#include <iostream>
+#include <chrono>
+#include <sstream>
+#include <iomanip>
+#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) {
+ // nothing
+}
+
+FileWriter::~FileWriter() {
+ // nothing
+}
+
+std::string FileWriter::generate_filename() {
+ // this has of course nothing to do with file writing and could be pulled
+ // out, but I doubt anybody will ever care
+ using namespace std::chrono;
+ auto miliseconds = duration_cast<milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
+ long seconds = miliseconds/1000;
+ struct tm *timeinfo = localtime(&seconds);
+
+ size_t found = filename_template.find("%z");
+ while(found != std::string::npos){
+ std::stringstream ss;
+ ss << std::setw(3) << std::setfill('0') << miliseconds%1000;
+ std::string s = ss.str();
+ filename_template.replace(found, 2, s);
+ found = filename_template.find("%z", found+3);
+ }
+
+ size_t length = 0;
+ size_t space = 0;
+ std::vector<char> buffer;
+ while (!length) {
+ space += 100;
+ buffer.resize(filename_template.size() + space);
+ length = strftime(buffer.data(), buffer.size(),
+ filename_template.c_str(), timeinfo);
+ }
+ return std::string(buffer.begin(), buffer.begin() + length);
+}
+
+void FileWriter::write(std::vector<uint8_t> &samples, std::vector<uint8_t> &imu_data) {
+ write(samples.data(), samples.size(), imu_data.data());
+}
+
+
+void store_little_endian(uint8_t (&target)[2], uint16_t value) {
+ target[0] = value & 0xFF;
+ target[1] = (value >> 8) & 0xFF;
+}
+
+void store_little_endian(uint8_t (&target)[4], uint32_t value) {
+ target[0] = value & 0xFF;
+ target[1] = (value >> 8) & 0xFF;
+ target[2] = (value >> 16) & 0xFF;
+ target[3] = (value >> 24) & 0xFF;
+}
+
+uint32_t read_little_endian(uint8_t (&source)[4]) {
+ return (source[0] + source[1] << 8 + source[2] << 16 + source[3] << 24);
+}
+
+WavFileWriter::WavFileWriter(std::string &filename_template, size_t qhb_version, size_t num_channels, size_t sample_rate, size_t depth) :
+ WavFileWriter(filename_template, qhb_version, num_channels, sample_rate, depth, 0) {
+ // nothing
+}
+
+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),
+ outfile(generate_filename(), std::ios::out | std::ios::binary | std::ios::trunc),
+ samples_written(0) {
+ // check if we could open the file
+ if (!outfile.is_open()) {
+ throw std::runtime_error("could not create output file");
+ }
+ // write header
+ store_little_endian(header.fmt_channels, num_channels);
+ store_little_endian(header.fmt_sample_rate, sample_rate);
+ store_little_endian(header.fmt_bits_per_sample, 8 * depth);
+ store_little_endian(header.fmt_byte_rate, num_channels * sample_rate * depth);
+ store_little_endian(header.fmt_frame_size, num_channels * depth);
+ if (expected_num_samples) {
+ size_t expected_data_size = expected_num_samples * num_channels * depth;
+ store_little_endian(header.data_size, expected_data_size);
+ store_little_endian(header.chunk_size, expected_data_size + 36);
+ // TODO: on linux, we could use fallocate to reserve the final size
+ }
+ // TODO: on posix, we could use posix_fadvice to indicate sequential access
+ outfile.seekp(0);
+ outfile.write((char*) &header, sizeof(header));
+}
+
+WavFileWriter::~WavFileWriter() {
+ // finalize header, if needed
+ size_t data_size = samples_written * num_channels * depth;
+ if (data_size != read_little_endian(header.data_size)) {
+ store_little_endian(header.data_size, data_size);
+ store_little_endian(header.chunk_size, data_size + 36);
+ outfile.seekp(0);
+ outfile.write((char*) &header, sizeof(header));
+ }
+}
+
+void WavFileWriter::write(uint8_t *samples, size_t num_samples, uint8_t *imu_data) {
+ outfile.write((char*) samples, num_samples * sizeof(*samples));
+ 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),
+ outfile(generate_filename(),
+ std::ios::out | std::ios::trunc),
+ last_timestamp(0),
+ rcvState(StateReception::Waiting),
+ msgDecodedFunction(0),
+ msgDecodedPayloadLength(0),
+ msgDecodedPayload(nullptr),
+ msgDecodedPayloadIndex(0),
+ msgDecoded(0) {
+ outfile << header;
+}
+
+inline float le16tof(uint8_t *array){
+ return static_cast<float>(static_cast<int16_t>(__builtin_bswap16(*reinterpret_cast<uint16_t*>(array))));
+}
+
+float IMUFileWriter::GetFloatSafe(const unsigned char *p, int index) {
+ unsigned char tmp[4];
+ std::memcpy(tmp, p + index, 4); // Copy 4 bytes from p + index into tmp
+
+ float result;
+ std::memcpy(&result, tmp, sizeof(result)); // Copy bytes from tmp into result
+
+ return result;
+}
+
+void IMUFileWriter::DecodeMessage(unsigned char c) {
+ switch (rcvState) {
+ case StateReception::Waiting:
+ if (c == 0xFE)
+ rcvState = StateReception::FunctionMSB;
+ break;
+ case StateReception::FunctionMSB:
+ msgDecodedFunction = static_cast<int16_t>(c << 8);
+ rcvState = StateReception::FunctionLSB;
+ break;
+ case StateReception::FunctionLSB:
+ msgDecodedFunction += static_cast<int16_t>(c << 0);
+ rcvState = StateReception::PayloadLengthMSB;
+ break;
+ case StateReception::PayloadLengthMSB:
+ msgDecodedPayloadLength = static_cast<uint16_t>(c << 8);
+ rcvState = StateReception::PayloadLengthLSB;
+ break;
+ case StateReception::PayloadLengthLSB:
+ msgDecodedPayloadLength += static_cast<uint16_t>(c << 0);
+ if (msgDecodedPayloadLength > 0) {
+ if (msgDecodedPayloadLength < 1024) {
+ msgDecodedPayloadIndex = 0;
+ msgDecodedPayload = static_cast<unsigned char*>(malloc(msgDecodedPayloadLength));
+ if (msgDecodedPayload == nullptr) {
+ throw std::bad_alloc(); // Handle memory allocation failure
+ }
+ rcvState = StateReception::Payload;
+ } else {
+ rcvState = StateReception::Waiting;
+ }
+ } else
+ rcvState = StateReception::Decode;
+ break;
+ case StateReception::Payload:
+ if (msgDecodedPayloadIndex > msgDecodedPayloadLength)
+ {
+ //Erreur
+ msgDecodedPayloadIndex = 0;
+ rcvState = StateReception::Waiting;
+ }
+ msgDecodedPayload[msgDecodedPayloadIndex++] = c;
+ if (msgDecodedPayloadIndex >= msgDecodedPayloadLength)
+ {
+ rcvState = StateReception::Decode;
+ msgDecodedPayloadIndex = 0;
+ }
+ break;
+ case StateReception::Decode:
+ {
+ //Lance l'event de fin de decodage
+ ProcessDecodedMessage(msgDecodedFunction, msgDecodedPayloadLength, msgDecodedPayload);
+ msgDecoded++;
+ rcvState = StateReception::Waiting;
+ }
+ break;
+ default:
+ rcvState = StateReception::Waiting;
+ break;
+ }
+}
+
+void IMUFileWriter::ProcessDecodedMessage(int msgFunction, int msgPayloadLength, const unsigned char* msgPayload) {
+ unsigned int timeStamp = 0;
+ switch(static_cast<short>(msgFunction)) {
+ case static_cast<short>(HS_DATA_PACKET_FULL_TIMESTAMP): {
+ IMUFileWriter::SensorType sensorType = static_cast<SensorType>(msgPayload[0]);
+ unsigned char id = msgPayload[1];
+ unsigned char nbChannels = msgPayload[2];
+ unsigned char range = msgPayload[3];
+ unsigned char resolutionBits = msgPayload[4];
+ unsigned short samplingFrequency = BUILD_UINT16(msgPayload[6], msgPayload[5]);
+ unsigned short nbSamples = BUILD_UINT16(msgPayload[8], msgPayload[7]);
+
+ int lengthPerSample = nbChannels * resolutionBits / 8 + 4;
+ double accelMaxValue = pow(2, resolutionBits)/2;
+ double gyroMaxValue = pow(2, resolutionBits) / 2;
+ double gyroRange = 250.0; //Hardcode pour le moment
+ double magRange = 4900.0; //Fixe
+
+ for(int i=0; i < nbSamples && msgPayloadLength >= lengthPerSample * i + 9; i++) {
+ timeStamp = BUILD_UINT32(msgPayload[9 + i * lengthPerSample+3], msgPayload[9 + i * lengthPerSample+2], msgPayload[9 + i * lengthPerSample+1], msgPayload[9 + i * lengthPerSample]);
+
+ if(timeStamp > lastTimeStamp) {
+ lastTimeStamp = timeStamp;
+ switch(sensorType) {
+ case IMUFileWriter::SensorType::IMU: {
+ outfile << timeStamp;
+ outfile << ", " << BUILD_UINT16(msgPayload[13 + i * lengthPerSample],msgPayload[13 + i * lengthPerSample+1]); // AccelX
+ outfile << ", " << BUILD_UINT16(msgPayload[15 + i * lengthPerSample],msgPayload[15 + i * lengthPerSample+1]); // AccelY
+ outfile << ", " << BUILD_UINT16(msgPayload[17 + i * lengthPerSample],msgPayload[17 + i * lengthPerSample+1]); // AccelZ
+ outfile << ", " << BUILD_UINT16(msgPayload[19 + i * lengthPerSample],msgPayload[19 + i * lengthPerSample+1]); // GyroX
+ outfile << ", " << BUILD_UINT16(msgPayload[21 + i * lengthPerSample],msgPayload[21 + i * lengthPerSample+1]); // GyroY
+ outfile << ", " << BUILD_UINT16(msgPayload[23 + i * lengthPerSample],msgPayload[23 + i * lengthPerSample+1]); // GyroZ
+ outfile << ", " << BUILD_UINT16(msgPayload[25 + i * lengthPerSample],msgPayload[25 + i * lengthPerSample+1]); // MagX
+ outfile << ", " << BUILD_UINT16(msgPayload[27 + i * lengthPerSample],msgPayload[27 + i * lengthPerSample+1]); // MagY
+ outfile << ", " << BUILD_UINT16(msgPayload[29 + i * lengthPerSample],msgPayload[29 + i * lengthPerSample+1]); // MagZ
+ outfile << std::endl;
+ }
+ break;
+ case IMUFileWriter::SensorType::Accel:
+ break;
+ case IMUFileWriter::SensorType::Gyro:
+ break;
+ case IMUFileWriter::SensorType::Mag:
+ break;
+ case IMUFileWriter::SensorType::Temperature:
+ break;
+ case IMUFileWriter::SensorType::Pressure:
+ break;
+ case IMUFileWriter::SensorType::Light:
+ break;
+ default:
+ break;
+ }
+ } else {
+ outfile << "TS IMU Error" << std::endl;
+ }
+ }
+ }
+ break;
+ case static_cast<short>(HS_DATA_PACKET_FULL_TIMESTAMP_V2): {
+ IMUFileWriter::SensorType sensorType = static_cast<SensorType>(msgPayload[0]);
+ unsigned char id = msgPayload[1];
+ unsigned char nbChannels = msgPayload[2];
+ float rangeScale = GetFloatSafe(msgPayload, 3);
+ unsigned char resolutionBits = msgPayload[7];
+ float samplingFrequency = GetFloatSafe(msgPayload, 8);
+ unsigned short nbSamples = msgPayload[12];
+ unsigned char dataSize = (resolutionBits / 8);
+
+ int lengthPerSample = nbChannels * resolutionBits / 8 + 4;
+ double dataMaxValue = std::pow(2, resolutionBits) / 2.0;
+
+ for(int i = 0; i < nbSamples && msgPayloadLength >= static_cast<size_t>(lengthPerSample * i + 13); i++) {
+ uint32_t timeStamp = BUILD_UINT32(msgPayload[13 + i * lengthPerSample+3],msgPayload[13 + i * lengthPerSample+2],msgPayload[13 + i * lengthPerSample+1],msgPayload[13 + i * lengthPerSample]);
+ switch(sensorType) {
+ case IMUFileWriter::SensorType::Accel:
+ if (lastAccelTimeStamp >= 500000000)
+ 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 );
+ outfile << std::endl;
+ }
+ else {
+ //printf("TS Accel Error\n");
+ }
+ break;
+ case IMUFileWriter::SensorType::Gyro:
+ if (lastGyroTimeStamp >= 500000000)
+ 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);
+ outfile << std::endl;
+ }
+ else {
+ //printf("TS Gyro Error\n");
+ }
+ break;
+ case IMUFileWriter::SensorType::Mag:
+ if (lastMagTimeStamp >= 500000000)
+ 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);
+ outfile << std::endl;
+ }
+ else {
+ //printf("TS Mag Error\n");
+ }
+ break;
+ case IMUFileWriter::SensorType::Temperature:
+ if (lastTemperatureTimeStamp >= 500000000)
+ lastTemperatureTimeStamp = 0;
+ if (timeStamp > lastTemperatureTimeStamp) {
+ lastTemperatureTimeStamp = timeStamp;
+ outfile << "TEMP, " << timeStamp;
+ outfile << ", " << GetFloatSafe(msgPayload,17 + i * lengthPerSample);
+ outfile << std::endl;
+ }
+ else {
+ //printf("TS Temperature Error\n");
+ }
+ break;
+ case IMUFileWriter::SensorType::Pressure:
+ if (lastPressureTimeStamp >= 500000000)
+ lastPressureTimeStamp = 0;
+ if (timeStamp > lastPressureTimeStamp) {
+ lastPressureTimeStamp = timeStamp;
+ outfile << "PRESSURE, " << timeStamp;
+ outfile << ", " << IMUFileWriter::GetFloatSafe(msgPayload,17 + i * lengthPerSample);
+ outfile << std::endl;
+ }
+ else {
+ //printf("TS Pressure Error\n");
+ }
+ break;
+ case IMUFileWriter::SensorType::Light:
+ if (lastLightTimeStamp >= 500000000)
+ lastLightTimeStamp = 0;
+ if (timeStamp > lastLightTimeStamp) {
+ lastLightTimeStamp = timeStamp;
+ outfile << "LIGHT, " << timeStamp;
+ outfile << ", " << BUILD_UINT16(msgPayload[17 + i * lengthPerSample],msgPayload[17 + i * lengthPerSample+1]);
+ outfile << ", " << BUILD_UINT16(msgPayload[17 + dataSize+i * lengthPerSample],msgPayload[17 +dataSize+ i * lengthPerSample+1]);
+ outfile << std::endl;
+ }
+ else {
+ //printf("TS Light Error\n");
+ }
+ break;
+ case IMUFileWriter::SensorType::Unknown:
+ outfile << "UNKNOWN, " << timeStamp;
+ outfile << std::endl;
+ default:
+ break;
+ }
+ }
+ }
+ break;
+ case static_cast<short>(GPS_DATA_PACKET): {
+ IMUFileWriter::GPSDatas gpsDatas;
+ unsigned short ms = BUILD_UINT16(msgPayload[3], msgPayload[4]);
+ if(ms > 999) {
+ ms = 0;
+ }
+
+ gpsDatas.dateOfFix.year = msgPayload[7];
+ gpsDatas.dateOfFix.month = msgPayload[6];
+ gpsDatas.dateOfFix.day = msgPayload[5];
+ gpsDatas.dateOfFix.hour = msgPayload[0];
+ gpsDatas.dateOfFix.minute = msgPayload[1];
+ gpsDatas.dateOfFix.second = msgPayload[2];
+
+ gpsDatas.fix = msgPayload[8] != 0;
+ gpsDatas.fixQuality = msgPayload[9];
+ gpsDatas.latitude = GetFloatSafe(msgPayload, 10);
+ gpsDatas.latitudeDirection = static_cast<char>(msgPayload[14]);
+ gpsDatas.longitude = GetFloatSafe(msgPayload, 15);
+ gpsDatas.longitudeDirection = static_cast<char>(msgPayload[19]);
+ gpsDatas.speed = GetFloatSafe(msgPayload, 20);
+ gpsDatas.angle = GetFloatSafe(msgPayload, 24);
+ gpsDatas.altitude = GetFloatSafe(msgPayload, 28);
+ gpsDatas.satellites = msgPayload[32];
+ gpsDatas.antenna = msgPayload[33];
+
+ if (lastGPSDate.year != gpsDatas.dateOfFix.year || lastGPSDate.month != gpsDatas.dateOfFix.month ||lastGPSDate.day != gpsDatas.dateOfFix.day ||
+ lastGPSDate.hour!=gpsDatas.dateOfFix.hour || lastGPSDate.minute!=gpsDatas.dateOfFix.minute || lastGPSDate.second!=gpsDatas.dateOfFix.second) {
+ lastGPSDate = gpsDatas.dateOfFix;
+ outfile << "GPS, " << static_cast<int>(gpsDatas.dateOfFix.year);
+ outfile << "/" << static_cast<int>(gpsDatas.dateOfFix.month);
+ outfile << "/" << static_cast<int>(gpsDatas.dateOfFix.day);
+ outfile << " " << static_cast<int>(gpsDatas.dateOfFix.hour);
+ outfile << ":" << static_cast<int>(gpsDatas.dateOfFix.minute);
+ outfile << ":" << static_cast<int>(gpsDatas.dateOfFix.second);
+ outfile << " fix:" << gpsDatas.fix;
+ outfile << ", fixQual:" << static_cast<int>(gpsDatas.fixQuality);
+ outfile << ", Lat:" << gpsDatas.latitude;
+ outfile << " " << gpsDatas.latitudeDirection;
+ outfile << ", Lon:" << gpsDatas.longitude;
+ outfile << " " << gpsDatas.longitudeDirection;
+ outfile << ", speed:" << gpsDatas.speed;
+ outfile << ", ang:" << gpsDatas.angle;
+ outfile << ", alt:" << gpsDatas.altitude;
+ outfile << ", sat:" << static_cast<int>(gpsDatas.satellites);
+ outfile << std::endl;
+ }
+ }
+ break;
+ case static_cast<short>(GPS_PPS_PACKET): {
+ uint64_t PPSTimeStamp = BUILD_UINT64(msgPayload[7], msgPayload[6], msgPayload[5], msgPayload[4],
+ msgPayload[3], msgPayload[2], msgPayload[1], msgPayload[0]);
+
+ PPSTimeStamp *= 10; // Convert to nanoseconds (assuming internal clock frequency is 100MHz)
+
+ if(PPSTimeStamp>lastPPSTimeStampNS) {
+ lastPPSTimeStampNS = PPSTimeStamp;
+ outfile << "PPS: " << PPSTimeStamp;
+ outfile << std::endl;
+ }
+ }
+ break;
+ default: break;
+ }
+}
+
+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++)
+ {
+ DecodeMessage(imu_data[i]);
+ }
+ }
+ else {
+ uint8_t *imu_data_cur(imu_data);
+ while(imu_data_cur + frame_size + 5 < imu_data + additional_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;
+ continue;
+ }
+ imu_data_cur += 5; // skip frame header
+ auto timestamp = static_cast<int32_t>(__builtin_bswap32(*reinterpret_cast<uint32_t*>(imu_data_cur + 9)));
+ if (timestamp > last_timestamp) {
+ last_timestamp = timestamp;
+ outfile << timestamp;
+ outfile << "," << le16tof(imu_data_cur + 13) / 32756 * 19.62; // ax resolution +- 2g
+ outfile << "," << le16tof(imu_data_cur + 15) / 32756 * 19.62; // ay resolution +- 2g
+ outfile << "," << le16tof(imu_data_cur + 17) / 32756 * 19.62; // az resolution +- 2g
+ outfile << "," << le16tof(imu_data_cur + 19) / 32756 * 250; // gx resolution +- 255deg/sec
+ outfile << "," << le16tof(imu_data_cur + 21) / 32756 * 250; // gy resolution +- 255deg/sec
+ outfile << "," << le16tof(imu_data_cur + 23) / 32756 * 250; // gz resolution +- 255deg/sec
+ outfile << "," << le16tof(imu_data_cur + 25) / 32756 * 4900.; // mx +- 4900µTesla
+ outfile << "," << le16tof(imu_data_cur + 27) / 32756 * (-4900.); // my +- 4900µTesla
+ outfile << "," << le16tof(imu_data_cur + 29) / 32756 * (-4900.); // mz +- 4900µTesla
+ outfile << std::endl;
+ }
+ imu_data_cur += frame_size;
+ }
+ }
+}
+
+size_t IMUFileWriter::get_last_timestamp(){
+ return 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),
+ samples_per_file(samples_per_file),
+ current_file(nullptr),
+ current_file_samples_written(0) {
+ // nothing
+}
+
+SplitWavFileWriter::~SplitWavFileWriter() {
+ if (current_file) {
+ delete current_file;
+ }
+}
+
+void SplitWavFileWriter::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) {
+ if (!current_file) {
+ current_file = new WavFileWriter(filename_template, qhb_version, num_channels, sample_rate, depth, samples_per_file);
+ }
+ // 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);
+ samples += missing * num_channels * depth;
+ available -= missing;
+ // start a new file
+ delete current_file;
+ current_file = nullptr;
+ current_file_samples_written = 0;
+ }
+ // if there are samples left, write them to the current file
+ if (available) {
+ if (!current_file) {
+ current_file = new WavFileWriter(filename_template, qhb_version, num_channels, sample_rate, depth, samples_per_file);
+ }
+ current_file->write(samples, available * num_channels * depth, imu_data);
+ current_file_samples_written += available;
+ }
+}
+
+
+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),
+ imu_name_template(imu_name_template),
+ samples_per_file(samples_per_file),
+ current_file(nullptr),
+ imu_file(nullptr),
+ current_file_samples_written(0),
+ max_timestamp(0) {
+ // nothing
+}
+
+SplitIMUWavFileWriter::~SplitIMUWavFileWriter() {
+ if (current_file) {
+ delete current_file;
+ }
+ if (imu_file) {
+ delete imu_file;
+ }
+}
+
+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) {
+ 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);
+ }
+ }
+ if (!imu_file) imu_file = new IMUFileWriter(imu_name_template, qhb_version, num_channels, sample_rate, depth, max_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);
+ if (imu_data) imu_file->write(samples, missing * num_channels * depth, imu_data);
+ imu_data = nullptr; // prevent multiple writing
+ samples += missing * num_channels * depth;
+ available -= missing;
+ // start a new file
+ delete current_file;
+ max_timestamp = imu_file->get_last_timestamp();
+ delete imu_file;
+ current_file = nullptr;
+ imu_file = nullptr;
+ current_file_samples_written = 0;
+ }
+ // if there are samples left, write them to the current file
+ if (available) {
+ 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);
+ }
+ }
+ if (!imu_file) imu_file = new IMUFileWriter(imu_name_template, qhb_version, num_channels, sample_rate, depth, max_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
new file mode 100644
index 0000000000000000000000000000000000000000..8cbf897a80334dd503d4be767a8081ecab3c1950
--- /dev/null
+++ b/src/filewriter.h
@@ -0,0 +1,280 @@
+/**
+ * Wave file writing, with or without splitting by length.
+ *
+ * Author: Jan Schlüter <jan.schluter@lis-lab.fr>
+ * Author: Maxence Ferrari <maxence.ferrari@lis-lab.fr>
+ */
+
+#ifndef FILEWRITER_H
+#define FILEWRITER_H
+
+#include <cstdint>
+#include <string>
+#include <vector>
+#include <fstream>
+#include <iostream>
+#include <cstdio>
+#define HS_DATA_PACKET_FULL_TIMESTAMP 0x0A0A
+#define HS_DATA_PACKET_FULL_TIMESTAMP_V2 0x0A0C
+#define GPS_DATA_PACKET 0x0A0D
+#define GPS_PPS_PACKET 0x0A0E
+
+
+/** Abstract base class for writing sample data to files.
+ */
+class FileWriter {
+protected:
+ // output file name template
+ std::string filename_template;
+ std::string generate_filename();
+
+ // sample format options
+ size_t qhb_version;
+ size_t num_channels;
+ size_t sample_rate;
+ size_t depth;
+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);
+ virtual ~FileWriter();
+
+ /** Writes out the given vector of 8-bit samples.
+ * \param[in] samples The samples to write, with interleaved channels.
+ */
+ 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;
+};
+
+
+/** The header of a PCM WAVE file as stored on disk.
+ */
+typedef struct {
+ char chunk_id[4] = {'R', 'I', 'F', 'F'};
+ std::uint8_t chunk_size[4] = {0, 0, 0, 0}; // sample data size + 36 = file size - 8, little endian
+ char riff_type[4] = {'W', 'A', 'V', 'E'};
+ char fmt_id[4] = {'f', 'm', 't', ' '};
+ std::uint8_t fmt_length[4] = {16, 0, 0, 0}; // 16, little endian
+ std::uint8_t fmt_tag[2] = {1, 0}; // 0x0001 (= PCM), little endian
+ std::uint8_t fmt_channels[2] = {0, 0}; // number of channels, little endian
+ std::uint8_t fmt_sample_rate[4] = {0, 0, 0, 0}; // samples per second, little endian
+ std::uint8_t fmt_byte_rate[4] = {0, 0, 0, 0}; // bytes per second (per channel), little endian
+ std::uint8_t fmt_frame_size[2] = {0, 0}; // channels * 16 bit, little endian
+ std::uint8_t fmt_bits_per_sample[2] = {16, 0}; // 16, little endian
+ char data_id[4] = {'d', 'a', 't', 'a'};
+ std::uint8_t data_size[4] = {0, 0, 0, 0}; // sample data size = file size - 44, little endian
+} WavFileHeader;
+// TODO: possibly use WAVEFORMATEXTENSIBLE for more than two channels:
+// https://docs.microsoft.com/en-us/windows-hardware/drivers/audio/extensible-wave-format-descriptors
+
+
+/** Class for writing sample data to a single PCM WAVE file.
+ */
+class WavFileWriter: public FileWriter {
+private:
+ std::ofstream outfile;
+ WavFileHeader header;
+ size_t samples_written;
+public:
+ /** Instantiates a wave file writer.
+ * \param[in] filename_template The name of the file to write to. Will be
+ * created or opened immediately, truncating any existing content. May
+ * contain format specifiers as understood by the strftime() function,
+ * filled in based on the current system date and local time.
+ * \param[in] num_channels The number of channels the sample data to be
+ * written will contain.
+ * \param[in] sample_rate The number of samples per second (per channel) the
+ * sample data to be written will contain.
+ * \param[in] depth The number of bytes per samples the
+ * sample data to be written will contain.
+ */
+ WavFileWriter(std::string &filename_template, size_t qhb_version, size_t num_channels, size_t sample_rate, size_t depth);
+ /** Instantiates a wave file writer.
+ * \param[in] filename_template The name of the file to write to. Will be
+ * created or opened immediately, truncating any existing content. May
+ * contain format specifiers as understood by the strftime() function,
+ * filled in based on the current system date and local time.
+ * \param[in] num_channels The number of channels the sample data to be
+ * written will contain.
+ * \param[in] sample_rate The number of samples per second (per channel) the
+ * sample data to be written will contain.
+ * \param[in] depth The number of bytes per samples the
+ * sample data to be written will contain.
+ * \param[in] expected_num_samples The expected total number of samples (per
+ * channel) that will be written. The file header will be written
+ * accordingly and not rewritten on closing the file if the number matches.
+ */
+ 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);
+ ~WavFileWriter() override;
+
+ void write(uint8_t *samples, size_t num_samples, uint8_t *imu_data) override;
+};
+
+
+/** Class for writing sample data to a sequence of PCM WAVE files, split up to
+ * reach a given target length per file.
+ */
+class SplitWavFileWriter: public FileWriter {
+private:
+ size_t samples_per_file;
+ WavFileWriter *current_file;
+ size_t current_file_samples_written;
+public:
+ /** Instantiates a splitted wave file writer.
+ * \param[in] filename_template The name of the file to write to. Will be
+ * created or opened when required, truncating any existing content. Should
+ * contain format specifiers as understood by the strftime() function,
+ * filled in based on the current system date and local time, otherwise it
+ * will be repeatedly overwritten.
+ * \param[in] num_channels The number of channels the sample data to be
+ * written will contain.
+ * \param[in] sample_rate The number of samples per second (per channel) the
+ * sample data to be written will contain.
+ * \param[in] depth The number of bytes per samples the
+ * sample data to be written will contain.
+ * \param[in] samples_per_file The target number of samples (per channel)
+ * that will be written to a file before starting the next one.
+ */
+ 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);
+ ~SplitWavFileWriter() override;
+
+ void write(uint8_t *samples, size_t num_samples, uint8_t *imu_data) override;
+};
+
+
+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;
+private:
+ enum class SensorType {
+ Unknown = 0,
+ Accel = 1,
+ Gyro = 2,
+ Mag = 3,
+ Temperature = 4,
+ Pressure = 5,
+ Light = 6,
+ Piezo = 7,
+ IMU = 8
+ };
+ struct DateTime {
+ unsigned short year;
+ unsigned char month;
+ unsigned char day;
+ unsigned char weekDay;
+ unsigned char hour;
+ unsigned char minute;
+ unsigned char second;
+ };
+ struct GPSDatas {
+ DateTime dateOfFix;
+ bool fix;
+ unsigned char fixQuality;
+ double latitude;
+ char latitudeDirection;
+ double longitude;
+ char longitudeDirection;
+ double speed;
+ double angle;
+ double altitude;
+ unsigned char satellites;
+ unsigned char antenna;
+ };
+
+ std::ofstream outfile;
+ size_t last_timestamp = 0;
+ unsigned int lastAccelTimeStamp = 0;
+ unsigned int lastGyroTimeStamp = 0;
+ unsigned int lastMagTimeStamp = 0;
+ unsigned int lastLightTimeStamp = 0;
+ unsigned int lastPressureTimeStamp = 0;
+ unsigned int lastTemperatureTimeStamp = 0;
+ unsigned int lastTimeStamp = 0;
+ DateTime lastGPSDate;
+ double lastPPSTimeStampNS;
+
+ enum class StateReception {
+ Waiting,
+ FunctionMSB,
+ FunctionLSB,
+ PayloadLengthMSB,
+ PayloadLengthLSB,
+ Payload,
+ Decode
+ };
+
+ StateReception rcvState;
+ int msgDecodedFunction;
+ int msgDecodedPayloadLength;
+ unsigned char *msgDecodedPayload;
+ int msgDecodedPayloadIndex;
+ unsigned int msgDecoded;
+
+ void ProcessDecodedMessage(int msgFunction, int msgPayloadLength,
+ const unsigned char* msgPayload);
+ float GetFloatSafe(const unsigned char *p, int index);
+public:
+ /** Instantiates a splitted wave file writer.
+ * \param[in] filename_template The name of the file to write to. Will be
+ * created or opened when required, truncating any existing content. Should
+ * contain format specifiers as understood by the strftime() function,
+ * filled in based on the current system date and local time, otherwise it
+ * will be repeatedly overwritten.
+ * \param[in] num_channels The number of channels the sample data to be
+ * written will contain.
+ * \param[in] sample_rate The number of samples per second (per channel) the
+ * sample data to be written will contain.
+ * \param[in] depth The number of bytes per samples the
+ * sample data to be written will contain.
+ * \param[in] samples_per_file The target number of samples (per channel)
+ * that will be written to a file before starting the next one.
+ */
+ 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);
+ void write(uint8_t *samples, size_t num_samples, uint8_t *imu_data) override;
+ size_t get_last_timestamp();
+};
+
+/** Class for writing sample data to a sequence of PCM WAVE files, split up to
+ * reach a given target length per file.
+ */
+class SplitIMUWavFileWriter: public FileWriter {
+private:
+ size_t samples_per_file;
+ WavFileWriter *current_file;
+ size_t current_file_samples_written;
+ std::string &imu_name_template;
+ IMUFileWriter *imu_file;
+ size_t max_timestamp;
+public:
+ /** Instantiates a splitted wave file writer.
+ * \param[in] filename_template The name of the file to write to. Will be
+ * created or opened when required, truncating any existing content. Should
+ * contain format specifiers as understood by the strftime() function,
+ * filled in based on the current system date and local time, otherwise it
+ * will be repeatedly overwritten.
+ * \param[in] num_channels The number of channels the sample data to be
+ * written will contain.
+ * \param[in] sample_rate The number of samples per second (per channel) the
+ * sample data to be written will contain.
+ * \param[in] depth The number of bytes per samples the
+ * sample data to be written will contain.
+ * \param[in] samples_per_file The target number of samples (per channel)
+ * 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);
+ ~SplitIMUWavFileWriter() override;
+
+ void write(uint8_t *samples, size_t num_samples, uint8_t *imu_data) override;
+};
+
+
+#endif // FILEWRITER_H
diff --git a/src/macros.h b/src/macros.h
new file mode 100644
index 0000000000000000000000000000000000000000..eaeff1277efd9b8b6a10d04b576b82e055a198b3
--- /dev/null
+++ b/src/macros.h
@@ -0,0 +1,28 @@
+#include <cstdint> // For standard integer types like uint8_t, uint16_t, etc.
+
+inline uint16_t BUILD_UINT16(uint8_t loByte, uint8_t hiByte) {
+ return static_cast<uint16_t>((loByte & 0x00FF) + ((hiByte & 0x00FF) << 8));
+}
+
+inline int16_t BUILD_INT16(uint8_t hiByte, uint8_t loByte) {
+ return static_cast<int16_t>((loByte & 0x00FF) + ((hiByte & 0x00FF) << 8));
+}
+
+inline uint32_t BUILD_UINT32(uint8_t Byte0, uint8_t Byte1, uint8_t Byte2, uint8_t Byte3) {
+ return static_cast<uint32_t>(static_cast<uint32_t>(Byte0 & 0x00FF)
+ + (static_cast<uint32_t>(Byte1 & 0x00FF) << 8)
+ + (static_cast<uint32_t>(Byte2 & 0x00FF) << 16)
+ + (static_cast<uint32_t>(Byte3 & 0x00FF) << 24));
+}
+
+inline uint64_t BUILD_UINT64(uint8_t Byte0, uint8_t Byte1, uint8_t Byte2, uint8_t Byte3,
+ uint8_t Byte4, uint8_t Byte5, uint8_t Byte6, uint8_t Byte7) {
+ return static_cast<uint64_t>(static_cast<uint64_t>(Byte0 & 0x00FF)
+ + (static_cast<uint64_t>(Byte1 & 0x00FF) << 8)
+ + (static_cast<uint64_t>(Byte2 & 0x00FF) << 16)
+ + (static_cast<uint64_t>(Byte3 & 0x00FF) << 24)
+ + (static_cast<uint64_t>(Byte4 & 0x00FF) << 32)
+ + (static_cast<uint64_t>(Byte5 & 0x00FF) << 40)
+ + (static_cast<uint64_t>(Byte6 & 0x00FF) << 48)
+ + (static_cast<uint64_t>(Byte7 & 0x00FF) << 56));
+}
\ No newline at end of file
diff --git a/src/main.cpp b/src/main.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e8a6f59da449241e2ced2a5d35c275fd4d2bd3c9
--- /dev/null
+++ b/src/main.cpp
@@ -0,0 +1,230 @@
+/**
+ * SMIoT JASON Qualilife sound recorder command line program.
+ *
+ * Author: Jan Schlüter <jan.schluter@lis-lab.fr>
+ * Author: Maxence Ferrari <maxence.ferrari@lis-lab.fr>
+ */
+#include <iostream>
+#include <string>
+#include <memory>
+#include <cstring>
+#include "recorder.h"
+#include "filewriter.h"
+#include "cleanexit.h"
+
+void print_usage(char *name) {
+ std::cout << "SMIoT JASON Qualilife sound recorder";
+#ifdef JASONREC_VERSION
+ std::cout << " v" << JASONREC_VERSION;
+#endif
+ std::cout << std::endl;
+ std::cout << "Usage: " << name << " qhbversion 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]" << std::endl;
+ std::cout << "Positional arguments:" << std::endl;
+ std::cout << " QHB VERSION:\tversion of the QHB audio card (2 or 3)" << std::endl;
+ std::cout << " CHANNELS:\tnumber of channels to record (1 to 5)" << std::endl;
+ std::cout << " RATE:\tsample rate in Hz to record at (integral number)" << std::endl;
+ std::cout << " FILENAME:\toutput file name. should include strftime() format specifiers" << std::endl;
+ std::cout << " if CHUNK_LEN is specified. For miliseconds, use %z. Example: location/recording_%Y%m%d_%H%M%S_%z.wav" << std::endl;
+ std::cout << "Optional arguments:" << std::endl;
+ std::cout << "-h, --help\t\tshow this help message and exit" << std::endl;
+
+ std::cout << " --bit_depth, -b\tBIT_DEPTH:\tSize of each samples in bits. Must be a multiple of 8. (Default: 16)" << std::endl;
+ std::cout << " --imu, -i\tIMU:\tIMU file name. Similar to FILENAME. Disable by default." << std::endl;
+ std::cout << " --filter, -f\tFILTER:\tNumber of the filter to use. Must be between 0 and 2. (Default: 0)" << std::endl;
+ std::cout << " --chunk_len, -c\tCHUNK_LEN:\tlength per output file in seconds; will start a new file whenever" << std::endl;
+ std::cout << " this length is reached. If not given or zero, will record a single file." << std::endl;
+ std::cout << " --total_len, -t\tTOTAL_LEN:\tTotal recording length; will stop when this length is reached." << std::endl;
+ std::cout << " If not given or zero, will record continuously until killed." << std::endl;
+ std::cout << " --device, -d\tDEVICE:\tWhich device to use in case multiple JASON cards are connected," << std::endl;
+ std::cout << " where 0 is the first, 1 is the second card found (and so on)." << std::endl;
+ std::cout << " --verbose, -v\t\tEnable the printing of status message " << std::endl;
+}
+
+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) {
+ JasonRecorder recorder = JasonRecorder(verbose);
+ std::cout << "Found " << recorder.get_device_count() << " JASON card(s)." << std::endl;
+ if (recorder.get_device_count() == 0) {
+ std::cout << "Aborting." << std::endl;
+ return 2;
+ }
+ try {
+ // prepare the device
+ std::cout << "Selecting device number " << device << "..." << std::endl;
+ recorder.set_device(device);
+ // prepare the file writer
+ std::unique_ptr<FileWriter> filewriter;
+ if (chunklen > 0 && imu_name.empty()) {
+ // 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) {
+ // 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));
+ }
+ else if (imu_name.empty()){
+ 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));
+ }
+ // start the recording loop
+ std::cout << "Starting to record..." << std::endl;
+ allow_clean_exit();
+ size_t total_samples_wanted = totallen * rate;
+ size_t total_samples_read = 0;
+ size_t failed_attempts = 0;
+ size_t sample_size = channels * depth;
+ std::vector<std::uint8_t> samples;
+ std::vector<std::uint8_t> imu_data;
+ try {
+ std::cout << "Setting recording format to " << channels << " channels at " << rate << " Hz " << (8 * depth) << " bits" << std::endl;
+ 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()) {
+ std::cout << "Termination requested." << std::endl;
+ break;
+ }
+ 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
+ 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
+ filewriter->write(samples, imu_data);
+ failed_attempts = 0;
+ }
+ else {
+ // if we received no message or no audio data 20x in a row, abort
+ failed_attempts += 1;
+ if (failed_attempts >= 20) {
+ throw std::runtime_error("Device does not send audio data.");
+ }
+ }
+ }
+ recorder.stop_recording();
+ std::cout << "Stopped recording." << std::endl;
+ }
+ catch (const std::exception& e) {
+ recorder.stop_recording();
+ throw;
+ }
+ }
+ catch (const std::exception& e) {
+ std::cout << "Error: " << e.what() << std::endl;
+ return 2;
+ }
+ return 0;
+}
+
+int main(int argc, char *argv[]) {
+ if (argc < 5) {
+ print_usage(argv[0]);
+ return 2;
+ }
+
+ // parse command line options
+ int qhb_version;
+ try {
+ qhb_version = std::stoi(argv[1]);
+ if ((qhb_version < 2) || (qhb_version > 3)) {
+ std::cout << "Error: QHBVERSION must be 2..3, got " << qhb_version << " instead. Version unsupported." << std::endl;
+ return 2;}
+ }
+ catch (const std::exception& e) {
+ std::cout << "Error: Could not interpret " << argv[1] << " as an integer." << std::endl;
+ return 2;
+ }
+ int num_channels;
+ try {
+ num_channels = std::stoi(argv[2]);
+ if ((num_channels < 1) || (num_channels > MAX_CHANNELS)) {
+ std::cout << "Error: CHANNELS must be in 1.." << MAX_CHANNELS << ", got " << num_channels << " instead" << std::endl;
+ return 2;}
+ }
+ catch (const std::exception& e) {
+ std::cout << "Error: Could not interpret " << argv[2] << " as an integer." << std::endl;
+ return 2;
+ }
+ int rate;
+ try {
+ rate = std::stoi(argv[3]);
+ if (rate!= 32000 && rate!=64000 && rate!=128000 && rate!=256000 && rate!=512000) {
+ std::cout << "Error: RATE must be a power 2 times 32kHz, got " << rate << " instead" << std::endl;
+ return 2;
+ }
+ }
+ catch (const std::exception& e) {
+ std::cout << "Error: Could not interpret " << argv[3] << " as an integer." << std::endl;
+ return 2;
+ }
+ std::string filename = argv[4];
+
+ 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);
+ while (i < argc){
+ try{
+ if (strcmp(argv[i], "--chunk_len") == 0 || strcmp(argv[i], "-c") == 0) {
+ chunklen = atof(argv[++i]);
+ if ((chunklen < 0)) {
+ std::cout << "Error: CHUNKLEN must be positive or zero, got " << chunklen << " instead" << std::endl;
+ return 2;
+ }}
+ else if (strcmp(argv[i], "--imu") == 0 || strcmp(argv[i], "-i") == 0) {
+ imu_name = argv[++i];
+ }
+ else if (strcmp(argv[i], "--device") == 0 || strcmp(argv[i], "-d") == 0) {
+ device = atoi(argv[++i]);
+ if ((device < 0)) {
+ std::cout << "Error: DEVICE must be nonnegative, got " << device << " instead" << std::endl;
+ return 2;
+ }}
+ else if (strcmp(argv[i], "--total_len") == 0 || strcmp(argv[i], "-t") == 0) {
+ totallen = atof(argv[++i]);
+ if ((totallen < 0)) {
+ std::cout << "Error: TOTALLEN must be positive or zero, got " << totallen << " instead" << std::endl;
+ return 2;
+ }}
+ else if (strcmp(argv[i], "--bit_depth") == 0 || strcmp(argv[i], "-b") == 0) {
+ bit_depth = atoi(argv[++i]);
+ if (bit_depth % 8) {
+ std::cout << "Error: DEPTH must be a multiple of 8, got " << bit_depth << " instead" << std::endl;
+ return 2;
+ }}
+ else if (strcmp(argv[i], "--filter") == 0 || strcmp(argv[i], "-f") == 0) {
+ filter = atoi(argv[++i]);
+ if (filter < 0 || filter > 2 ) {
+ std::cout << "Error: filter must be between 0 and 2, got " << filter << " instead" << std::endl;
+ return 2;
+ }}
+ else if (strcmp(argv[i], "--help") == 0 || strcmp(argv[i], "-h") == 0){
+ print_usage(argv[0]);
+ return 1;
+ }
+ else if (strcmp(argv[i], "--verbose") == 0 || strcmp(argv[i], "-v") == 0){
+ verbose = true;
+ }
+ else {
+ std::cout << "Unrecognized argument " << argv[i] << std::endl;
+ return 2;
+ }}
+ catch (const std::exception& e) {
+ std::cout << "Error: Could not interpret " << argv[i] << " ( " << argv[i-1] << " ) " " as a number." << std::endl;
+ return 2;
+ }
+ i++;
+ }
+ // 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
new file mode 100644
index 0000000000000000000000000000000000000000..efb1d7291977fc2490ab7d6296a4b358c30791b8
--- /dev/null
+++ b/src/recorder.cpp
@@ -0,0 +1,321 @@
+/**
+ * SMIoT JASON Qualilife sound recording class.
+ *
+ * Author: Jan Schlüter <jan.schluter@lis-lab.fr>
+ * Author: Maxence Ferrari <maxence.ferrari@lis-lab.fr>
+ */
+
+#include "recorder.h"
+#include <stdexcept>
+#include <iostream>
+#include <vector>
+#include <array>
+#include <algorithm>
+
+JasonRecorder::JasonRecorder(bool verbose) : verbose(verbose) {
+ // create libusb context
+ if (libusb_init(&ctx) < 0) {
+ throw std::runtime_error("libusb initialization failed");
+ }
+
+ // set debug level
+ libusb_set_debug(ctx, 3);
+
+ // discover JASON sound cards
+ libusb_device** all_devices;
+ if (libusb_get_device_list(ctx, &all_devices) < 0) {
+ throw std::runtime_error("libusb device enumeration failed");
+ }
+ libusb_device** device = all_devices;
+ while (*device != NULL) {
+ struct libusb_device_descriptor desc;
+ if (libusb_get_device_descriptor(*device, &desc) < 0) {
+ continue;
+ }
+ if ((desc.idVendor == VENDOR_ID) && (desc.idProduct == PRODUCT_ID)) {
+ devices.push_back(*device);
+ }
+ else {
+ libusb_unref_device(*device);
+ }
+ device++;
+ }
+ libusb_free_device_list(all_devices, 0);
+}
+
+JasonRecorder::~JasonRecorder() {
+ // free handle
+ if (handle) {
+ libusb_release_interface(handle, 0);
+ libusb_close(handle);
+ }
+ // free devices
+ for (auto& device : devices) {
+ libusb_unref_device(device);
+ }
+ // free libusb libusb context
+ libusb_exit(ctx);
+}
+
+size_t JasonRecorder::get_device_count() {
+ return devices.size();
+}
+
+void JasonRecorder::set_device(size_t number) {
+ if (handle) {
+ libusb_release_interface(handle, 0);
+ libusb_close(handle);
+ handle = NULL;
+ }
+ if (number >= devices.size()) {
+ throw std::out_of_range("device number too large");
+ }
+ if (libusb_open(devices[number], &handle) < 0) {
+ throw std::runtime_error("could not open USB device (try again as root)");
+ }
+ if (libusb_claim_interface(handle, 0) < 0) {
+ throw std::runtime_error("could not claim USB interface");
+ }
+}
+
+void JasonRecorder::send_message(std::uint16_t cmd) {
+ send_message(cmd, NULL, 0);
+}
+
+void JasonRecorder::send_message(std::uint16_t cmd, std::vector<std::uint8_t> &payload) {
+ send_message(cmd, payload.data(), payload.size());
+}
+
+void JasonRecorder::send_message(std::uint16_t cmd, std::uint8_t *payload, size_t length) {
+ if (!handle) {
+ throw std::logic_error("must call set_device() first");
+ }
+ // message format: 0xfe + payload size (2 byte) + command (1 byte) + payload
+ std::vector<std::uint8_t> data;
+ data.reserve(6 + length);
+ data.push_back(FRAME_START);
+ data.push_back((std::uint8_t) ((cmd >> 8) & 0xFF));
+ data.push_back((std::uint8_t) (cmd & 0xFF));
+ data.push_back((std::uint8_t) ((length >> 8) & 0xFF));
+ data.push_back((std::uint8_t) (length & 0xFF));
+ if (length) {
+ data.insert(data.end(), payload, payload + length);
+ }
+ // compute the checksum
+ data.push_back(FRAME_START);
+ for (int i=1; i < 5+length; data[5 + length] ^= data[i++]);
+ // send message, allow a maximum of 10 seconds for it to go through
+ int sent;
+ if (libusb_bulk_transfer(handle, ENDPOINT_SEND, data.data(), data.size(), &sent, 10000) < 0) {
+ throw std::runtime_error("could not send message to device");
+ }
+ else if (sent != data.size()) {
+ throw std::runtime_error("could not send complete message to device");
+ };
+}
+
+void JasonRecorder::getBytesFromFloat(std::array<unsigned char, 4> &p, float f) {
+ unsigned char *f_ptr = reinterpret_cast<unsigned char*>(&f);
+ for (int i = 0; i < 4; i++) {
+ p[i] = f_ptr[i];
+ }
+}
+
+void JasonRecorder::start_recording(int qhb_version, std::uint8_t num_channels, size_t sample_rate, std::uint8_t depth, std::uint8_t num_filter, size_t accelSamplingRate, size_t gyroSamplingRate, size_t magSamplingRate, size_t accelRangeScale, size_t gyroRangeScale, size_t magRangeScale) {
+ if (qhb_version == 2) {
+ 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) (8 * depth),
+ num_filter};
+ 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) (8 * depth),
+ num_filter};
+ send_message(START_ID, payload1);
+ }
+
+ this->num_channels = num_channels;
+ this->sample_rate = sample_rate;
+ this->depth = depth;
+ this->num_filter = num_filter;
+ recording = true;
+}
+
+
+void JasonRecorder::stop_recording() {
+ std::vector<std::uint8_t> payload1 = {
+ STOP,
+ (std::uint8_t) ((this->sample_rate >> 24) & 0xFF),
+ (std::uint8_t) ((this->sample_rate >> 16) & 0xFF),
+ (std::uint8_t) ((this->sample_rate >> 8) & 0xFF),
+ (std::uint8_t) (this->sample_rate & 0xFF),
+ this->num_channels,
+ (std::uint8_t) (8 * this->depth),
+ this->num_filter};
+ send_message(START_ID, payload1);
+ recording = false;
+}
+
+size_t JasonRecorder::receive_message(uint8_t *buffer, size_t max_wait) {
+ if (!handle) {
+ throw std::logic_error("must call set_device() first");
+ }
+ int received;
+ int status = libusb_bulk_transfer(handle, ENDPOINT_RECEIVE, buffer, MAX_MSG_LENGTH, &received, max_wait);
+ if (status == LIBUSB_ERROR_OVERFLOW) {
+ throw std::runtime_error("buffer too small to receive message from device");
+ }
+ else if ((status < 0) && (status != LIBUSB_ERROR_TIMEOUT)) {
+ throw std::runtime_error("could not receive message from device");
+ }
+ 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) {
+ if (!num_channels || !sample_rate) {
+ throw std::logic_error("must call set_format() first");
+ }
+ std::array<std::uint8_t, MAX_MSG_LENGTH> buffer{};
+ while (true) {
+ size_t received = receive_message(buffer.data(), max_wait);
+ if (received) {
+ // we could read the payload length, but it is wrong for sample data
+ //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) {
+ // find the beginning and length of the samples in the buffer
+ size_t start = this->additional_data_size + 6;
+ 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;
+ // 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);
+ }
+ else {
+ // convert from blocked channels to interleaved channels
+ samples.resize(num_samples);
+ JasonRecorder::interleave_channels(&buffer[start],
+ samples.data(), num_samples,
+ this->num_channels, this->depth);
+ }
+ break;
+ }
+ else if (this->verbose && (((std::uint16_t) buffer[1] << 8 )|(buffer[2])) == STATUS_ID) {
+ samples.resize(0);
+ std::uint8_t cks=FRAME_START; //buffer[0] == FRAME_START already check
+ for (int i=1; i < 31; cks ^= buffer[i++]);
+ std::cout << " Sr: " << ( ((size_t) buffer[5] << 24) | ((size_t) buffer[6] << 16)
+ | ((size_t) buffer[7] << 8) | ((size_t) buffer[8]))
+ << " #Ch: " << (size_t) buffer[9] << " D: " << (size_t) buffer[10] << " Time: "
+ << 2000 + buffer[11] <<'-'<< (size_t) buffer[12] <<'-'<< (size_t) buffer[13] <<' '
+ << (size_t) buffer[14] <<':'<< (size_t) buffer[15] <<':'<< (size_t) buffer[16]
+ << " UUID: " << std::hex << (size_t) buffer[17] << (size_t) buffer[18] << (size_t) buffer[19] << (size_t) buffer[20]
+ << (size_t) buffer[21] << (size_t) buffer[22] << (size_t) buffer[23] << (size_t) buffer[24] << std::dec
+ << " Rec: " << (buffer[25] !=0)
+ << " SPI: " << (size_t) buffer[26] << (size_t) buffer[27] << (size_t) buffer[28] << (size_t) buffer[29]
+ << " CKS: " << (cks == 0?"True":"False") << std::endl;
+ break;
+ }
+ }
+ else if (max_wait > 0) {
+ // we timed out, we do not want to wait again
+ samples.resize(0);
+ break;
+ }
+ }
+}
+
+void JasonRecorder::interleave_channels(std::uint8_t *input, std::uint8_t *output, size_t num_bytes,
+ size_t num_channels, size_t depth) {
+ // the input comes in num_channels blocks of num_samples_per_channel little-endian 16-bit samples each
+ // we write these to the output in a round-robin manner, interleaving the channels
+ // we use a pattern that accesses the output strictly sequentially, so it can be used to write to a mem-mapped file
+ if ((num_channels < 1) || (num_channels > MAX_CHANNELS)) {
+ throw std::out_of_range("num_channels must be in [1, 6]");
+ }
+ // prepare one input pointer per channel
+ std::uint8_t *inputs[num_channels];
+ for (size_t c = 0; c < num_channels; c++) {
+ inputs[c] = input + c * (num_bytes/num_channels);
+ }
+ // iterate over the samples, copying in interleaved fashion
+ size_t c = 0;
+ for (size_t b=0; b < num_bytes;) {
+ *(output++) = *(inputs[c]++);
+ if (++b % depth == 0)
+ c = (c + 1) % num_channels;
+ }
+}
diff --git a/src/recorder.h b/src/recorder.h
new file mode 100644
index 0000000000000000000000000000000000000000..4bcf71f3c3786e87b517fae464fcd910987787e2
--- /dev/null
+++ b/src/recorder.h
@@ -0,0 +1,130 @@
+/**
+ * SMIoT JASON Qualilife sound recording class.
+ *
+ * Author: Jan Schlüter <jan.schluter@lis-lab.fr>
+ * Author: Maxence Ferrari <maxence.ferrari@lis-lab.fr>
+ */
+
+#ifndef RECORDER_H
+#define RECORDER_H
+
+#include <libusb.h>
+#include <cstdint>
+#include <vector>
+#include <array>
+
+
+#define MAX_MSG_LENGTH 65536
+#define MAX_CHANNELS 6
+
+/** Class for retrieving sample data from a JASON Qualilife sound card.
+ */
+class JasonRecorder {
+ const std::int16_t VENDOR_ID = 0x04D8;
+ const std::int16_t PRODUCT_ID = 0x0053;
+ const std::uint8_t ENDPOINT_SEND = 0x01;
+ const std::uint8_t ENDPOINT_RECEIVE = 0x81;
+
+ // device control messages
+ const std::uint8_t FRAME_START = 0xFE;
+ const std::uint16_t START_ID = 0x0C01;
+ const std::uint16_t SET_SENSOR = 0x0C09;
+ const std::uint16_t SET_CLOCK_ID = 0x0C06;
+ 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;
+private:
+ // libusb handles
+ struct libusb_context *ctx;
+ std::vector<struct libusb_device*> devices;
+ struct libusb_device_handle *handle = NULL;
+ /** Sends a message to the device, without payload.
+ * \param[in] cmd The command identifier.
+ */
+ void send_message(std::uint16_t cmd);
+ /** Sends a message with payload to the device.
+ * \param[in] cmd The command identifier.
+ * \param[in] payload The payload data to include.
+ */
+ void send_message(std::uint16_t cmd, std::vector<std::uint8_t> &payload);
+ /** Sends a message with payload to the device.
+ * \param[in] cmd The command identifier.
+ * \param[in] payload Pointer to the payload data to include.
+ * \param[in] length The size of the payload data in bytes.
+ */
+ void send_message(std::uint16_t cmd, std::uint8_t *payload, size_t length);
+ /** Transform a float32 into an array of bytes.
+ * \param[in] p A pointer to the array where the bytes of the float will be stored.
+ * \param[in] index The starting position in the array `p` where the first byte will be stored.
+ * \param[in] f The float value from which the bytes will be extracted.
+ */
+ void getBytesFromFloat(std::array<unsigned char, 4> &p, float f);
+
+ // device messages sent back
+ /** Reads a message from the device. Waits for a message if necessary.
+ * \param[out] buffer Pointer to memory to write the message to.
+ * \param[out] length Length of the buffer.
+ * \param[in] max_wait Waiting time in milliseconds. Set to zero to wait
+ * indefinitely. If no message could be read within this time, returns zero.
+ * \returns the number of bytes received and written to the buffer.
+ */
+ size_t receive_message(uint8_t *buffer, size_t max_wait = 0);
+
+ // device state, as far as known
+ size_t additional_data_size = 730;
+ std::uint8_t num_channels = 0;
+ std::uint8_t depth = 0;
+ std::uint8_t num_filter = 0;
+ size_t sample_rate = 0;
+ bool recording = false;
+ bool verbose = false;
+public:
+ JasonRecorder(bool verbose);
+ ~JasonRecorder();
+
+ /** Searches for JASON Qualilife sound cards attached via USB.
+ * \returns the number of devices found. May be zero.
+ */
+ size_t get_device_count();
+ /** Selects the given device and opens it.
+ * \param[in] number The device, must be smaller than get_device_count().
+ */
+ void set_device(size_t number);
+ /** Starts recording from the device chosen with set_device(). Requires set_device() to be called before.
+ * \param[in] num_channels The number of channels, between 1 and 5.
+ * \param[in] sample_rate The number of samples per second (per channel).
+ * \param[in] depth The number of bytes of each sample.
+ * \param[in] num_filter The filter number (between 0 and 2).
+ */
+ void start_recording(int qhb_version, std::uint8_t num_channels, size_t sample_rate, std::uint8_t depth, std::uint8_t num_filter, size_t accelSamplingRate, size_t gyroSamplingRate, size_t magSamplingRate, size_t accelRangeScale, size_t gyroRangeScale, size_t magRangeScale);
+ /** Stops recording from the device chosen with set_device(). */
+ void stop_recording();
+ /** Fetches a messages from the device chosen with set_device().
+ * Requires the format to have been set before using set_format().
+ * If the messsages contains samples, they will be put in samples.
+ * \param[out] samples A vector the samples will be written to, replacing
+ * existing content if any.
+ * \param[in] planar If true, return the samples in planar form, i.e., one
+ * channel after the other, the format sent by the device. If false (the
+ * default), interleave the channels as done in PCM WAVE files.
+ * \param[in] max_wait Maximum time in milliseconds to block waiting for a
+ * packet from the device. If the time elapses before receiving any packets,
+ * 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);
+ /** 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.
+ * \param[in] num_bytes The total number of bytes
+ * to convert.
+ * \param[in] num_channels The number of channels.
+ * \param[in] depth The number of bytes per sample.
+ */
+ static void interleave_channels(std::uint8_t *input, std::uint8_t *output, size_t num_bytes,
+ size_t num_channels, size_t depth);
+};
+
+#endif // RECORDER_H