diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json new file mode 100644 index 0000000000000000000000000000000000000000..4ace5d5b3405c4d1c56366b4220b66bb813637d7 --- /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 new file mode 100644 index 0000000000000000000000000000000000000000..c3caa63b339bd7fab827ce946b9c00ad22051e16 --- /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": "${workspaceFolder}/build/src/jasonrec", + "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": [], + "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 0000000000000000000000000000000000000000..4b96cbb09c2f975589d7b66b200f70ef3ce9e93c --- /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 0000000000000000000000000000000000000000..4bd97533ef7fc8c647f5631b71a90241d5d224b6 --- /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=Debug" + ], + "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", + "options": { + "cwd": "${workspaceFolder}/build" + }, + "dependsOn": [ + "Make" + ], + "problemMatcher": [ + "$gcc" + ] + }, + { + "label": "Configure and Build HighBlueParsers", + "dependsOn": ["Build with CMake", "Make"], + "dependsOrder": "sequence", + "problemMatcher": [], + "group": { + "kind": "build", + "isDefault": true + } + } + ] +} \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index ccd70e9a9f1cc3ea46e4b4223602374bf6c0bb44..d819d19f9a49afc462b5ea14efc95df714bea716 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/README.md b/README.md index 93dfcc2864ccde39e62f6dad09956e55549f2890..635835db309af65c3e327958dc5b13484b836688 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 +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 directories have to be created in advance. diff --git a/src/cleanexit.h b/src/cleanexit.h index 24a88dd473897e30a69d9f3010128315095de43b..f74687c8f8a0b27f7246107a9eb9ba2a10dca0de 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 01009d01be8de7a453518546a3110b63ff266229..fb4c766054f227bf54a8f012bf1c9b87cacbf592 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> @@ -122,16 +123,19 @@ 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) : +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), + timestampByteReceived(0), rcvState(StateReception::Waiting), msgDecodedFunction(0), msgDecodedPayloadLength(0), msgDecodedPayload(nullptr), msgDecodedPayloadIndex(0), + softwareMajorRev(0), + softwareMinorRev(0), msgDecoded(0) { outfile << header; } @@ -150,11 +154,28 @@ 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; + } break; case StateReception::FunctionMSB: msgDecodedFunction = static_cast<int16_t>(c << 8); @@ -182,9 +203,10 @@ void IMUFileWriter::DecodeMessage(unsigned char c) { rcvState = StateReception::Waiting; } } else - rcvState = StateReception::Decode; + rcvState = StateReception::CheckSum; break; case StateReception::Payload: + { if (msgDecodedPayloadIndex > msgDecodedPayloadLength) { //Erreur @@ -194,17 +216,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++; - 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; @@ -212,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 = (uint32_t)raw_data.timeStamp; + + 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)) { @@ -276,8 +318,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); @@ -292,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 { @@ -307,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 { @@ -322,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+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 = dataXYZ; //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 { @@ -448,17 +525,16 @@ 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++) + outfile << "PACKET TIMESTAMP: " << global_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; @@ -557,16 +633,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); } } - 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); // 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); @@ -589,14 +665,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); } } - 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); 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 8cbf897a80334dd503d4be767a8081ecab3c1950..2d1fac7fe582d159537dba56808ec81043083774 100644 --- a/src/filewriter.h +++ b/src/filewriter.h @@ -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; @@ -204,7 +207,7 @@ private: PayloadLengthMSB, PayloadLengthLSB, Payload, - Decode + CheckSum, }; StateReception rcvState; @@ -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, @@ -233,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); void write(uint8_t *samples, size_t num_samples, uint8_t *imu_data) override; size_t get_last_timestamp(); }; @@ -276,5 +283,14 @@ public: void write(uint8_t *samples, size_t num_samples, uint8_t *imu_data) override; }; +typedef struct SensorXYZData_s +{ + uint32_t timeStamp; + double X; + double Y; + double Z; +}SensorXYZData; + +SensorXYZData NormalizeSensorsData(SensorXYZData data, float range, unsigned char resolutionBits); #endif // FILEWRITER_H diff --git a/src/main.cpp b/src/main.cpp index e8a6f59da449241e2ced2a5d35c275fd4d2bd3c9..984eef1cecc1398580b12a4c69aa1a05538baf85 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -8,10 +8,13 @@ #include <string> #include <memory> #include <cstring> +#include <atomic> #include "recorder.h" #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 @@ -44,6 +47,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 +76,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)); } - 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{ + 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)); } - // 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,13 +106,15 @@ 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; } + // Get audio and imu samples recorder.get_samples(samples, imu_data, false, 500); if (!samples.empty()) { total_samples_read += samples.size() / sample_size; @@ -95,7 +122,7 @@ int record(size_t qhb_version, size_t channels, size_t rate, size_t depth, size_ 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 +134,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 +151,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 +196,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 +257,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 efb1d7291977fc2490ab7d6296a4b358c30791b8..b2ab254dd0124730c77fa103d5020d0b51fcf7de 100644 --- a/src/recorder.cpp +++ b/src/recorder.cpp @@ -6,11 +6,17 @@ */ #include "recorder.h" +#include <unistd.h> #include <stdexcept> #include <iostream> +#include <iomanip> #include <vector> #include <array> #include <algorithm> +#include <fstream> +#include <cstring> +#include <thread> +#include <chrono> JasonRecorder::JasonRecorder(bool verbose) : verbose(verbose) { // create libusb context @@ -90,7 +96,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,59 +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) (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::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) (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::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) (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<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, @@ -195,12 +195,14 @@ void JasonRecorder::start_recording(int qhb_version, std::uint8_t num_channels, (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"; } + this->qhb_version = qhb_version; this->num_channels = num_channels; this->sample_rate = sample_rate; this->depth = depth; @@ -238,6 +240,22 @@ size_t JasonRecorder::receive_message(uint8_t *buffer, size_t max_wait) { return received; } +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, bool planar, size_t max_wait) { if (!num_channels || !sample_rate) { throw std::logic_error("must call set_format() first"); @@ -251,23 +269,39 @@ void JasonRecorder::get_samples(std::vector<std::uint8_t> &samples, std::vector< 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; + 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 + //hex_dump(buffer.data(), 16); + for (int i = 0; i < 8; ++i){ + global_packet_timestamp <<= 8; + global_packet_timestamp |= static_cast<uint64_t>(buffer[7 + i]); + } + } + 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 4bcf71f3c3786e87b517fae464fcd910987787e2..9a9dc2d2aad4478856e4cc87cf3930b52bfbc2af 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 { @@ -33,9 +35,15 @@ 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: + void hex_dump(const uint8_t* data, size_t size); // libusb handles struct libusb_context *ctx; std::vector<struct libusb_device*> devices; @@ -73,7 +81,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;