From 84f78f5d714eb61d838c6c2f6b1f38ccc54fe64e Mon Sep 17 00:00:00 2001
From: Philemon Prevot <philemon.prevot@lis-lab.fr>
Date: Fri, 25 Apr 2025 10:08:49 +0200
Subject: [PATCH] Correct continuous recording and add debug hex_dump method,
plus change header sizes selection and correct timestamp logging
---
src/cleanexit.h | 5 ++
src/filewriter.cpp | 111 +++++++++++++++++++++------------------------
src/filewriter.h | 19 ++++----
src/main.cpp | 66 +++++++++++++++++++--------
src/recorder.cpp | 82 +++++++++++++++++++++++----------
src/recorder.h | 8 +++-
6 files changed, 178 insertions(+), 113 deletions(-)
diff --git a/src/cleanexit.h b/src/cleanexit.h
index 24a88dd..f74687c 100644
--- a/src/cleanexit.h
+++ b/src/cleanexit.h
@@ -12,6 +12,11 @@
*/
void allow_clean_exit();
+/**
+ * Sets exit signal to extern for access from the main loop
+ */
+extern std::atomic<bool> _exit_requested; // Exit flag
+
/**
* Check if the application was requested to terminate.
* \returns whether the application was requested to terminate.
diff --git a/src/filewriter.cpp b/src/filewriter.cpp
index 7c2538d..6a943d3 100644
--- a/src/filewriter.cpp
+++ b/src/filewriter.cpp
@@ -16,8 +16,8 @@
#include <cstring>
#include <cmath>
-FileWriter::FileWriter(std::string &filename_template, size_t qhb_version, size_t num_channels, size_t sample_rate, size_t depth) :
- filename_template(filename_template), qhb_version(qhb_version), num_channels(num_channels), sample_rate(sample_rate), depth(depth) {
+FileWriter::FileWriter(std::string &filename_template, size_t qhb_version, size_t num_channels, size_t sample_rate, size_t depth, uint64_t packet_timestamp) :
+ filename_template(filename_template), qhb_version(qhb_version), num_channels(num_channels), sample_rate(sample_rate), depth(depth), packet_timestamp(packet_timestamp) {
// nothing
}
@@ -82,7 +82,7 @@ WavFileWriter::WavFileWriter(std::string &filename_template, size_t qhb_version,
WavFileWriter::WavFileWriter(std::string &filename_template, size_t qhb_version, size_t num_channels, size_t sample_rate, size_t depth,
size_t expected_num_samples) :
- FileWriter(filename_template, qhb_version, num_channels, sample_rate, depth),
+ FileWriter(filename_template, qhb_version, num_channels, sample_rate, depth, 0),
outfile(generate_filename(), std::ios::out | std::ios::binary | std::ios::trunc),
samples_written(0) {
// check if we could open the file
@@ -122,8 +122,8 @@ void WavFileWriter::write(uint8_t *samples, size_t num_samples, uint8_t *imu_dat
samples_written += num_samples /(num_channels * depth);
}
-IMUFileWriter::IMUFileWriter(std::string &filename_template, size_t qhb_version, size_t num_channels, size_t sample_rate,size_t depth, size_t timestamp) :
- FileWriter(filename_template, qhb_version, num_channels, sample_rate, depth),
+IMUFileWriter::IMUFileWriter(std::string &filename_template, size_t qhb_version, size_t num_channels, size_t sample_rate, size_t depth, uint64_t packet_timestamp) :
+ FileWriter(filename_template, qhb_version, num_channels, sample_rate, depth, packet_timestamp),
outfile(generate_filename(),
std::ios::out | std::ios::trunc),
last_timestamp(0),
@@ -153,33 +153,43 @@ float IMUFileWriter::GetFloatSafe(const unsigned char *p, int index) {
return result;
}
+unsigned char IMUFileWriter::CalculateChecksum(int msgFunction,
+ int msgPayloadLength, const unsigned char msgPayload[])
+{
+ unsigned char checksum = 0;
+ checksum ^= static_cast<unsigned char>(0xFE);
+ checksum ^= static_cast<unsigned char>(msgFunction >> 8);
+ checksum ^= static_cast<unsigned char>(msgFunction >> 0);
+ checksum ^= static_cast<unsigned char>(msgPayloadLength >> 8);
+ checksum ^= static_cast<unsigned char>(msgPayloadLength >> 0);
+ for (int i = 0; i < msgPayloadLength; i++) {
+ checksum ^= msgPayload[i];
+ }
+ return checksum;
+}
+
+
void IMUFileWriter::DecodeMessage(unsigned char c) {
switch (rcvState) {
case StateReception::Waiting:
- if (c == 0xFE)
+ if (c == 0xFE) {
rcvState = StateReception::FunctionMSB;
- std::cout << "Batch start : ";
- std::cout << std::hex << std::setfill('0') << std::setw(2) << c << " ";
+ }
break;
case StateReception::FunctionMSB:
msgDecodedFunction = static_cast<int16_t>(c << 8);
rcvState = StateReception::FunctionLSB;
- std::cout << std::hex << std::setfill('0') << std::setw(2) << c << " ";
break;
case StateReception::FunctionLSB:
msgDecodedFunction += static_cast<int16_t>(c << 0);
rcvState = StateReception::PayloadLengthMSB;
- std::cout << std::hex << std::setfill('0') << std::setw(2) << c << " ";
break;
case StateReception::PayloadLengthMSB:
msgDecodedPayloadLength = static_cast<uint16_t>(c << 8);
rcvState = StateReception::PayloadLengthLSB;
- std::cout << std::hex << std::setfill('0') << std::setw(2) << c << " ";
break;
case StateReception::PayloadLengthLSB:
msgDecodedPayloadLength += static_cast<uint16_t>(c << 0);
- std::cout << std::hex << std::setfill('0') << std::setw(2) << c << " ";
- std::cout << "PayloadLength : " << msgDecodedPayloadLength << "\n";
if (msgDecodedPayloadLength > 0) {
if (msgDecodedPayloadLength < 1024) {
msgDecodedPayloadIndex = 0;
@@ -187,38 +197,15 @@ void IMUFileWriter::DecodeMessage(unsigned char c) {
if (msgDecodedPayload == nullptr) {
throw std::bad_alloc(); // Handle memory allocation failure
}
- rcvState = StateReception::SoftwareMajorRev;
- std::cout << "StateReception : SoftwareMajorRev\n";
+ rcvState = StateReception::Payload;
} else {
rcvState = StateReception::Waiting;
- std::cout << "StateReception : Waiting\n";
}
} else
- rcvState = StateReception::Decode;
- std::cout << "StateReception : Decode\n";
- break;
- case StateReception::SoftwareMajorRev:
- softwareMajorRev = static_cast<u_int8_t>(c);
- rcvState = StateReception::SoftwareMinorRev;
- std::cout << std::hex << std::setfill('0') << std::setw(2) << c << " ";
- break;
- case StateReception::SoftwareMinorRev:
- softwareMinorRev = static_cast<u_int8_t>(c);
- rcvState = StateReception::TimestampB;
- std::cout << std::hex << std::setfill('0') << std::setw(2) << c << " ";
- break;
- case StateReception::TimestampB:
- last_timestamp += static_cast<uint64_t>(c << 8 * (8 - timestampByteReceived));
- std::cout << std::hex << std::setfill('0') << std::setw(2) << c << " ";
- timestampByteReceived += 1;
- if (timestampByteReceived == 8)
- {
- timestampByteReceived = 0;
- rcvState = StateReception::Payload;
- }
+ rcvState = StateReception::CheckSum;
break;
case StateReception::Payload:
- std::cout << std::hex << std::setfill('0') << std::setw(2) << c << " ";
+ {
if (msgDecodedPayloadIndex > msgDecodedPayloadLength)
{
//Erreur
@@ -228,18 +215,24 @@ void IMUFileWriter::DecodeMessage(unsigned char c) {
msgDecodedPayload[msgDecodedPayloadIndex++] = c;
if (msgDecodedPayloadIndex >= msgDecodedPayloadLength)
{
- rcvState = StateReception::Decode;
+ rcvState = StateReception::CheckSum;
msgDecodedPayloadIndex = 0;
}
break;
- case StateReception::Decode:
- {
- //Lance l'event de fin de decodage
- ProcessDecodedMessage(msgDecodedFunction, msgDecodedPayloadLength, msgDecodedPayload);
- msgDecoded++;
- last_timestamp = 0;
- rcvState = StateReception::Waiting;
}
+ case StateReception::CheckSum:
+ {
+ unsigned char calculatedChecksum = CalculateChecksum(msgDecodedFunction, msgDecodedPayloadLength, msgDecodedPayload);
+ unsigned char receivedChecksum = c;
+ if (calculatedChecksum == receivedChecksum) {
+ //Lance l'event de fin de decodage
+ ProcessDecodedMessage(msgDecodedFunction, msgDecodedPayloadLength, msgDecodedPayload);
+ msgDecoded++;
+ } else {
+ std::cerr << "Checksum error" << std::endl;
+ }
+ rcvState = StateReception::Waiting;
+ }
break;
default:
rcvState = StateReception::Waiting;
@@ -307,7 +300,6 @@ void IMUFileWriter::ProcessDecodedMessage(int msgFunction, int msgPayloadLength,
}
break;
case static_cast<short>(HS_DATA_PACKET_FULL_TIMESTAMP_V2): {
- outfile << "PACKET TIMESTAMP: " << last_timestamp;
IMUFileWriter::SensorType sensorType = static_cast<SensorType>(msgPayload[0]);
unsigned char id = msgPayload[1];
unsigned char nbChannels = msgPayload[2];
@@ -485,14 +477,16 @@ void IMUFileWriter::ProcessDecodedMessage(int msgFunction, int msgPayloadLength,
void IMUFileWriter::write(uint8_t *sample, size_t size, uint8_t *imu_data) {
if(this->qhb_version == 3) {
- for(int i=0; i<size-1; i++)
+ std::cout << static_cast<int>(this->packet_timestamp) << std::endl;
+ outfile << "PACKET TIMESTAMP: " << this->packet_timestamp << "\n";
+ for(int i=0; i<this->additionnal_data_size; i++)
{
DecodeMessage(imu_data[i]);
}
}
else {
uint8_t *imu_data_cur(imu_data);
- while(imu_data_cur + frame_size + 5 < imu_data + additional_data_size){
+ while(imu_data_cur + frame_size + 5 < imu_data + additionnal_data_size){
if(!(imu_data_cur[0]==0xFE && imu_data_cur[1]==0x0A && imu_data_cur[2]==0x0A && imu_data_cur[5]==0x08)) {
// skip trame if header is incorrect
imu_data_cur += frame_size + 5;
@@ -526,7 +520,7 @@ size_t IMUFileWriter::get_last_timestamp(){
SplitWavFileWriter::SplitWavFileWriter(std::string &filename_template, size_t qhb_version, size_t num_channels, size_t sample_rate,
size_t depth, size_t samples_per_file) :
- FileWriter(filename_template, qhb_version, num_channels, sample_rate, depth),
+ FileWriter(filename_template, qhb_version, num_channels, sample_rate, depth, 0),
samples_per_file(samples_per_file),
current_file(nullptr),
current_file_samples_written(0) {
@@ -568,8 +562,8 @@ void SplitWavFileWriter::write(uint8_t *samples, size_t num_samples, uint8_t *im
SplitIMUWavFileWriter::SplitIMUWavFileWriter(std::string &filename_template, std::string &imu_name_template, size_t qhb_version,
- size_t num_channels, size_t sample_rate, size_t depth, size_t samples_per_file):
- FileWriter(filename_template, qhb_version, num_channels, sample_rate, depth),
+ size_t num_channels, size_t sample_rate, size_t depth, size_t samples_per_file, uint64_t packet_timestamp):
+ FileWriter(filename_template, qhb_version, num_channels, sample_rate, depth, packet_timestamp),
imu_name_template(imu_name_template),
samples_per_file(samples_per_file),
current_file(nullptr),
@@ -591,16 +585,16 @@ SplitIMUWavFileWriter::~SplitIMUWavFileWriter() {
void SplitIMUWavFileWriter::write(uint8_t *samples, size_t num_samples, uint8_t* imu_data) {
size_t available = num_samples /(num_channels * depth);
// start as many new files as required to write out the samples
- while (current_file_samples_written + available >= samples_per_file) {
+ while ((current_file_samples_written + available >= samples_per_file) && (samples_per_file != 0)) {
if (!current_file) {
current_file = new WavFileWriter(filename_template, qhb_version, num_channels, sample_rate, depth, samples_per_file);
if (imu_file) {
max_timestamp = imu_file->get_last_timestamp();
delete imu_file;
- imu_file = new IMUFileWriter(imu_name_template, qhb_version, num_channels, sample_rate, depth, max_timestamp);
+ imu_file = new IMUFileWriter(imu_name_template, qhb_version, num_channels, sample_rate, depth, this->packet_timestamp);
}
}
- if (!imu_file) imu_file = new IMUFileWriter(imu_name_template, qhb_version, num_channels, sample_rate, depth, max_timestamp);
+ if (!imu_file) imu_file = new IMUFileWriter(imu_name_template, qhb_version, num_channels, sample_rate, depth, this->packet_timestamp);
// write out as much as fits into the current file and move the pointer
size_t missing = samples_per_file - current_file_samples_written;
current_file->write(samples, missing * num_channels * depth, imu_data);
@@ -623,14 +617,13 @@ void SplitIMUWavFileWriter::write(uint8_t *samples, size_t num_samples, uint8_t*
if (imu_file) {
max_timestamp = imu_file->get_last_timestamp();
delete imu_file;
- imu_file = new IMUFileWriter(imu_name_template, qhb_version, num_channels, sample_rate, depth, max_timestamp);
+ imu_file = new IMUFileWriter(imu_name_template, qhb_version, num_channels, sample_rate, depth, this->packet_timestamp);
}
}
- if (!imu_file) imu_file = new IMUFileWriter(imu_name_template, qhb_version, num_channels, sample_rate, depth, max_timestamp);
+ if (!imu_file) imu_file = new IMUFileWriter(imu_name_template, qhb_version, num_channels, sample_rate, depth, this->packet_timestamp);
current_file->write(samples, available * num_channels * depth, imu_data);
if (imu_data) imu_file->write(samples, available * num_channels * depth, imu_data);
imu_data = nullptr;
current_file_samples_written += available;
}
-
}
diff --git a/src/filewriter.h b/src/filewriter.h
index e584a30..cae305b 100644
--- a/src/filewriter.h
+++ b/src/filewriter.h
@@ -36,7 +36,7 @@ protected:
public:
/** Abstract constructor to be used by subclasses.
*/
- FileWriter(std::string &filename_template, size_t qhb_version, size_t num_channels, size_t sample_rate, size_t depth);
+ FileWriter(std::string &filename_template, size_t qhb_version, size_t num_channels, size_t sample_rate, size_t depth, uint64_t packet_timestamp);
virtual ~FileWriter();
/** Writes out the given vector of 8-bit samples.
@@ -44,6 +44,10 @@ public:
*/
void write(std::vector<uint8_t> &samples, std::vector<uint8_t> &imu_data);
virtual void write(uint8_t *samples, size_t num_samples, uint8_t *imu_data) = 0;
+
+ /** Save the last packet timestamp
+ */
+ uint64_t packet_timestamp = 0;
};
@@ -148,7 +152,7 @@ class IMUFileWriter: public FileWriter {
// const std::string header = "Timestamp,ax,ay,az,gx,gy,gz,mx,my,mz\n";
const std::string header = "Sensor Type,TimeStamp(ms) or Time, val0,val1,val2,val3,val4,val5,val6,val7\n";
const size_t frame_size = 32;
- const size_t additional_data_size = 736;
+ const size_t additionnal_data_size = 736;
private:
enum class SensorType {
Unknown = 0,
@@ -186,7 +190,6 @@ private:
};
std::ofstream outfile;
- size_t last_timestamp = 0;
unsigned int lastAccelTimeStamp = 0;
unsigned int lastGyroTimeStamp = 0;
unsigned int lastMagTimeStamp = 0;
@@ -203,11 +206,8 @@ private:
FunctionLSB,
PayloadLengthMSB,
PayloadLengthLSB,
- SoftwareMajorRev,
- SoftwareMinorRev,
- TimestampB,
Payload,
- Decode
+ CheckSum,
};
StateReception rcvState;
@@ -239,11 +239,12 @@ public:
* \param[in] samples_per_file The target number of samples (per channel)
* that will be written to a file before starting the next one.
*/
+ size_t last_timestamp = 0;
unsigned char CalculateChecksum(int msgFunction,
int msgPayloadLength, const unsigned char msgPayload[]);
void DecodeMessage(unsigned char c);
- IMUFileWriter(std::string &filename_template, size_t qhb_version, size_t num_channels, size_t sample_rate, size_t depth, size_t timestamp);
+ IMUFileWriter(std::string &filename_template, size_t qhb_version, size_t num_channels, size_t sample_rate, size_t depth, uint64_t packet_timestamp);
void write(uint8_t *samples, size_t num_samples, uint8_t *imu_data) override;
size_t get_last_timestamp();
};
@@ -276,7 +277,7 @@ public:
* that will be written to a file before starting the next one.
*/
SplitIMUWavFileWriter(std::string &filename_template, std::string &imu_name_template, size_t qhb_version, size_t num_channels,
- size_t sample_rate, size_t depth, size_t samples_per_file);
+ size_t sample_rate, size_t depth, size_t samples_per_file, uint64_t packet_timestamp);
~SplitIMUWavFileWriter() override;
void write(uint8_t *samples, size_t num_samples, uint8_t *imu_data) override;
diff --git a/src/main.cpp b/src/main.cpp
index e8a6f59..b905fbc 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -8,6 +8,7 @@
#include <string>
#include <memory>
#include <cstring>
+#include <atomic>
#include "recorder.h"
#include "filewriter.h"
#include "cleanexit.h"
@@ -44,6 +45,28 @@ void print_usage(char *name) {
int record(size_t qhb_version, size_t channels, size_t rate, size_t depth, size_t filter, std::string &filename, std::string &imu_name,
float chunklen, float totallen, size_t device, bool verbose, size_t accelSamplingFrequency, size_t gyroSamplingFrequency, size_t magSamplingFrequency, size_t accelRangeScale, size_t gyroRangeScale, size_t magRangeScale) {
+ /**
+ * @brief Set device, initialize writers and loop the message reception and file writing methods
+ *
+ * @param qhb_version : version of the QHB card used for recording
+ * @param channels : number of channels recording
+ * @param rate : audio sampling rate
+ * @param depth : audio data bit precision (either 16 or 24)
+ * @param filter : number of filter used
+ * @param filename : address of the audio filename to use
+ * @param imu_name : address of the imu filename to use
+ * @param chunklen : duration of individual files if given, else defaults to 0
+ * @param totallen : total recording duration if given, else defaults to 0
+ * @param device : index of the device to use if several QHB Cards are plugged in
+ * @param verbose : boolean enabling the printing of status messages
+ * @param accelSamplingFrequency : accelerometer sampling frequency
+ * @param gyroSamplingFrequency : gyroscope sampling frequency
+ * @param magSamplingFrequency : magnetometer sampling frequency
+ * @param accelRangeScale : range of the accelerometer values
+ * @param gyroRangeScale : range of the gyroscope values
+ * @param magRangeScale : range of the magnetometer values
+ */
+ // Initialize the recorder object and select the device to use
JasonRecorder recorder = JasonRecorder(verbose);
std::cout << "Found " << recorder.get_device_count() << " JASON card(s)." << std::endl;
if (recorder.get_device_count() == 0) {
@@ -51,28 +74,28 @@ int record(size_t qhb_version, size_t channels, size_t rate, size_t depth, size_
return 2;
}
try {
- // prepare the device
+ // Prepare the device
std::cout << "Selecting device number " << device << "..." << std::endl;
recorder.set_device(device);
- // prepare the file writer
+ // Prepare the file writer
std::unique_ptr<FileWriter> filewriter;
- if (chunklen > 0 && imu_name.empty()) {
+ if (chunklen > 0 && imu_name.empty()) { // When we only record audio, in several files of chunklen length
// implementation note: in C++14 we would use std::make_unique<SplitWavFileWriter>(...)
filewriter.reset(new SplitWavFileWriter(filename, qhb_version, channels, rate, depth, chunklen * rate));
}
- else if (chunklen > 0) {
+ else if (chunklen > 0) { // When we record audio and imu, in several files of chunklen length
// implementation note: in C++14 we would use std::make_unique<SplitWavFileWriter>(...)
- filewriter.reset(new SplitIMUWavFileWriter(filename, imu_name, qhb_version, channels, rate, depth, chunklen * rate));
+ filewriter.reset(new SplitIMUWavFileWriter(filename, imu_name, qhb_version, channels, rate, depth, chunklen * rate, 0));
}
- else if (imu_name.empty()){
+ else if (imu_name.empty()){ // When we only record audio data, in one file only
filewriter.reset(new WavFileWriter(filename, qhb_version, channels, rate, depth, totallen * rate));
}
- else{
- filewriter.reset(new SplitIMUWavFileWriter(filename, imu_name, qhb_version, channels, rate, depth, totallen * rate));
+ else{ // When we record both audio and imu in one file only
+ filewriter.reset(new SplitIMUWavFileWriter(filename, imu_name, qhb_version, channels, rate, depth, totallen * rate, 0));
}
- // start the recording loop
+ // Start the recording and writing main loop
std::cout << "Starting to record..." << std::endl;
- allow_clean_exit();
+ allow_clean_exit(); // Handles for killing signals and clean program termination
size_t total_samples_wanted = totallen * rate;
size_t total_samples_read = 0;
size_t failed_attempts = 0;
@@ -81,21 +104,23 @@ int record(size_t qhb_version, size_t channels, size_t rate, size_t depth, size_
std::vector<std::uint8_t> imu_data;
try {
std::cout << "Setting recording format to " << channels << " channels at " << rate << " Hz " << (8 * depth) << " bits" << std::endl;
+ // Send start and parameters messages to QHB
recorder.start_recording(qhb_version, channels, rate, depth, filter, accelSamplingFrequency, gyroSamplingFrequency, magSamplingFrequency, accelRangeScale, gyroRangeScale, magRangeScale);
- // we will record until we have enough (or forever, if totallen == 0)
- while ((total_samples_wanted == 0) || (total_samples_read < total_samples_wanted)) {
- if (exit_requested()) {
+ // Loop until we have enough samples (or forever, if totallen == 0)
+ while (((total_samples_wanted == 0) || (total_samples_read < total_samples_wanted))) {
+ if (exit_requested()) { // In case of killing signal received
std::cout << "Termination requested." << std::endl;
break;
}
- recorder.get_samples(samples, imu_data, false, 500);
+ // Get audio and imu samples
+ recorder.get_samples(samples, imu_data, &filewriter->packet_timestamp, false, 500);
if (!samples.empty()) {
total_samples_read += samples.size() / sample_size;
// if we have too much now, crop the last packet
if ((total_samples_wanted > 0) && (total_samples_read > total_samples_wanted)) {
samples.resize(samples.size() - (total_samples_read - total_samples_wanted) * sample_size);
}
- // pass it on to the file writer
+ // Pass it on to the file writer
filewriter->write(samples, imu_data);
failed_attempts = 0;
}
@@ -107,6 +132,7 @@ int record(size_t qhb_version, size_t channels, size_t rate, size_t depth, size_
}
}
}
+ // Send stop message to QHB
recorder.stop_recording();
std::cout << "Stopped recording." << std::endl;
}
@@ -123,12 +149,15 @@ int record(size_t qhb_version, size_t channels, size_t rate, size_t depth, size_
}
int main(int argc, char *argv[]) {
+ /**
+ * @brief Main function. Retrieve program arguments and hand them over to the record function.
+ */
if (argc < 5) {
print_usage(argv[0]);
return 2;
}
- // parse command line options
+ // Parse mandatory command line options
int qhb_version;
try {
qhb_version = std::stoi(argv[1]);
@@ -165,14 +194,15 @@ int main(int argc, char *argv[]) {
}
std::string filename = argv[4];
+ // Program args initialization
int i=5;
-
int device(0), bit_depth(16), filter(0);
float chunklen(0), totallen(0);
float accelSamplingFrequency(25), gyroSamplingFrequency(25), magSamplingFrequency(20);
float accelRangeScale(8), gyroRangeScale(250), magRangeScale(12);
std::string imu_name;
bool verbose(false);
+ // Retrieve program args
while (i < argc){
try{
if (strcmp(argv[i], "--chunk_len") == 0 || strcmp(argv[i], "-c") == 0) {
@@ -225,6 +255,6 @@ int main(int argc, char *argv[]) {
}
i++;
}
- // hand over to the recording function
+ // Hand over to the recording function
return record(qhb_version, num_channels, rate, bit_depth/8, filter, filename, imu_name, chunklen, totallen, device, verbose, accelSamplingFrequency, gyroSamplingFrequency, magSamplingFrequency, accelRangeScale, gyroRangeScale, magRangeScale);
}
diff --git a/src/recorder.cpp b/src/recorder.cpp
index 2f02a81..086fc9e 100644
--- a/src/recorder.cpp
+++ b/src/recorder.cpp
@@ -9,10 +9,12 @@
#include <unistd.h>
#include <stdexcept>
#include <iostream>
+#include <iomanip>
#include <vector>
#include <array>
#include <algorithm>
#include <fstream>
+#include <cstring>
JasonRecorder::JasonRecorder(bool verbose) : verbose(verbose) {
// create libusb context
@@ -137,18 +139,6 @@ void JasonRecorder::start_recording(int qhb_version, std::uint8_t num_channels,
send_message(START_ID, payload1);
} else if (qhb_version == 3)
{
- std::vector<std::uint8_t> payload1 = {
- START,
- (std::uint8_t) ((sample_rate >> 24) & 0xFF),
- (std::uint8_t) ((sample_rate >> 16) & 0xFF),
- (std::uint8_t) ((sample_rate >> 8) & 0xFF),
- (std::uint8_t) (sample_rate & 0xFF),
- (std::uint8_t) (num_channels),
- (std::uint8_t) (8 * depth),
- (std::uint8_t) (1 * num_filter)};
- send_message(START_ID, payload1);
- std::cout << "Start message sent\n";
-
// std::array<unsigned char, 4> accelSamplingRateBytes = {0};
// std::array<unsigned char, 4> accelRangeScaleBytes = {0};
// getBytesFromFloat(accelSamplingRateBytes, accelSamplingRate);
@@ -187,7 +177,7 @@ void JasonRecorder::start_recording(int qhb_version, std::uint8_t num_channels,
// };
// send_message(SET_SENSOR, payload3);
// std::cout << "Gyro set\n";
- // sleep(0.01);
+ // sleep(0.1);
// std::array<unsigned char, 4> magSamplingRateBytes = {0};
// std::array<unsigned char, 4> magRangeScaleBytes = {0};
@@ -207,11 +197,21 @@ void JasonRecorder::start_recording(int qhb_version, std::uint8_t num_channels,
// };
// send_message(SET_SENSOR, payload4);
// std::cout << "Mag set\n";
- // sleep(0.01);
- // libusb_clear_halt(handle, ENDPOINT_SEND);
+ std::vector<std::uint8_t> payload1 = {
+ START,
+ (std::uint8_t) ((sample_rate >> 24) & 0xFF),
+ (std::uint8_t) ((sample_rate >> 16) & 0xFF),
+ (std::uint8_t) ((sample_rate >> 8) & 0xFF),
+ (std::uint8_t) (sample_rate & 0xFF),
+ (std::uint8_t) (num_channels),
+ (std::uint8_t) (8 * depth),
+ (std::uint8_t) (1 * num_filter)};
+ send_message(START_ID, payload1);
+ std::cout << "Start message sent\n";
}
+ this->qhb_version = qhb_version;
this->num_channels = num_channels;
this->sample_rate = sample_rate;
this->depth = depth;
@@ -249,7 +249,23 @@ size_t JasonRecorder::receive_message(uint8_t *buffer, size_t max_wait) {
return received;
}
-void JasonRecorder::get_samples(std::vector<std::uint8_t> &samples, std::vector<std::uint8_t> &imu_data, bool planar, size_t max_wait) {
+void JasonRecorder::hex_dump(const uint8_t* data, size_t size) {
+ /**
+ * @brief Debugging method that prints out the data in a hex format, in the same way as the Hex Editor
+ *
+ * @param data : pointer to the start of a data buffer
+ * @param size : quantity of bytes that you want to print out
+ */
+ for (size_t i = 0; i < size; ++i) {
+ if (i % 16 == 0) std::cout << std::setw(4) << std::setfill('0') << std::hex << i << ": ";
+ std::cout << std::setw(2) << std::setfill('0') << std::hex
+ << static_cast<int>(data[i]) << " ";
+ if ((i + 1) % 16 == 0) std::cout << std::endl;
+ }
+ if (size % 16 != 0) std::cout << std::endl;
+}
+
+void JasonRecorder::get_samples(std::vector<std::uint8_t> &samples, std::vector<std::uint8_t> &imu_data, uint64_t *packet_timestamp, bool planar, size_t max_wait) {
if (!num_channels || !sample_rate) {
throw std::logic_error("must call set_format() first");
}
@@ -261,26 +277,42 @@ void JasonRecorder::get_samples(std::vector<std::uint8_t> &samples, std::vector<
//size_t length = buffer[1] << 8 + buffer[2];
if (buffer[0] != FRAME_START); // invalid message
else if ((((std::uint16_t) buffer[1] << 8 )|(buffer[2])) == DATA_ID) {
- std::cout << "IMU Data buffer\n";
-
// find the beginning and length of the samples in the buffer
- size_t start = this->additional_data_size + 6;
+ size_t audio_start = this->additionnal_data_buffer_size;
+ size_t imu_start = 0;
+ if (this->qhb_version == 3)
+ {
+ imu_start = this->additionnal_buffer_header_size_v3;
+ // Retrieve the packet timestamp for later writing
+ uint64_t packet_timestamp_value = 0;
+ // hex_dump(buffer.data(), 16);
+ for (int i = 0; i < 8; ++i){
+ packet_timestamp_value |= static_cast<uint64_t>(buffer[7 + i]) << (8 * (8 - i));
+ }
+ memcpy(packet_timestamp, &packet_timestamp_value, 8);
+ // std::cout << static_cast<int>(*packet_timestamp) << std::endl;
+ }
+ else if (this->qhb_version == 2)
+ {
+ imu_start = this->additionnal_buffer_header_size_v2;
+ }
+
imu_data.resize(0);
- imu_data.reserve(this->additional_data_size);
- imu_data.insert(imu_data.begin(), &buffer[6], &buffer[start]);
- size_t num_samples = (received - start);
- num_samples = (num_samples / (num_channels * this->depth)) * num_channels * this->depth;
+ imu_data.reserve(this->additionnal_data_buffer_size);
+ imu_data.insert(imu_data.begin(), &buffer[0], &buffer[audio_start]);
+ size_t num_samples = (received - audio_start);
+ num_samples = (num_samples / (num_channels * this->depth)) * num_channels * this->depth; // Mais d'où ça sort ça encore TODO: Voir si ça marche toujours quand on l'enlève (y'a pas de raison)
// copy data to provided vector
if (planar || (num_channels == 1)) {
// copy out directly
samples.resize(0);
samples.reserve(num_samples);
- samples.insert(samples.end(), &buffer[start], &buffer[start] + num_samples);
+ samples.insert(samples.end(), &buffer[audio_start], &buffer[audio_start] + num_samples);
}
else {
// convert from blocked channels to interleaved channels
samples.resize(num_samples);
- JasonRecorder::interleave_channels(&buffer[start],
+ JasonRecorder::interleave_channels(&buffer[audio_start],
samples.data(), num_samples,
this->num_channels, this->depth);
}
diff --git a/src/recorder.h b/src/recorder.h
index 98a7a48..4891b43 100644
--- a/src/recorder.h
+++ b/src/recorder.h
@@ -41,6 +41,7 @@ class JasonRecorder {
const std::uint16_t GYRO = 0x02;
const std::uint16_t MAG = 0x03;
private:
+ void hex_dump(const uint8_t* data, size_t size);
// libusb handles
struct libusb_context *ctx;
std::vector<struct libusb_device*> devices;
@@ -78,7 +79,10 @@ private:
size_t receive_message(uint8_t *buffer, size_t max_wait = 0);
// device state, as far as known
- size_t additional_data_size = 730;
+ size_t additionnal_data_buffer_size = 736; // Size of the AdditionnalBuffer data
+ size_t additionnal_buffer_header_size_v3 = 16; // Size of the AdditionnalBuffer header for QHBv3
+ size_t additionnal_buffer_header_size_v2 = 6; // Size of the AdditionnalBuffer header for QHBv2
+ std::uint8_t qhb_version = 0;
std::uint8_t num_channels = 0;
std::uint8_t depth = 0;
std::uint8_t num_filter = 0;
@@ -119,7 +123,7 @@ public:
* or if a different type of packet was received, returns with an empty
* samples vector. Set to zero to wait indefinitely for a sample packet.
*/
- void get_samples(std::vector<std::uint8_t> &samples, std::vector<std::uint8_t> &imu_data, bool planar=false, size_t max_wait=0);
+ void get_samples(std::vector<std::uint8_t> &samples, std::vector<std::uint8_t> &imu_data, uint64_t *packet_timestamp, bool planar=false, size_t max_wait=0);
/** Converts samples from planar to interleaved form.
* \param[in] input A pointer to the planar samples to read.
* \param[out] output A pointer to write the interleaved samples to.
--
GitLab