From 0348180fd72eaecb1e539e989214195f33f5c7d9 Mon Sep 17 00:00:00 2001
From: Philemon Prevot <philemon.prevot@lis-lab.fr>
Date: Fri, 6 Sep 2024 17:29:06 +0200
Subject: [PATCH] Add sensors set-up trames encoding
---
src/recorder.cpp | 82 ++++++++++++++++++++++++++++++++++++++++++++++--
src/recorder.h | 10 +++++-
2 files changed, 88 insertions(+), 4 deletions(-)
diff --git a/src/recorder.cpp b/src/recorder.cpp
index 5e149c1..efb1d72 100644
--- a/src/recorder.cpp
+++ b/src/recorder.cpp
@@ -114,8 +114,16 @@ void JasonRecorder::send_message(std::uint16_t cmd, std::uint8_t *payload, size_
};
}
-void JasonRecorder::start_recording(std::uint8_t num_channels,size_t sample_rate, std::uint8_t depth, std::uint8_t num_filter) {
- std::vector<std::uint8_t> payload1 = {
+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),
@@ -124,7 +132,75 @@ void JasonRecorder::start_recording(std::uint8_t num_channels,size_t sample_rat
num_channels,
(std::uint8_t) (8 * depth),
num_filter};
- send_message(START_ID, payload1);
+ 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;
diff --git a/src/recorder.h b/src/recorder.h
index 7c323cf..4bcf71f 100644
--- a/src/recorder.h
+++ b/src/recorder.h
@@ -11,6 +11,7 @@
#include <libusb.h>
#include <cstdint>
#include <vector>
+#include <array>
#define MAX_MSG_LENGTH 65536
@@ -27,6 +28,7 @@ class JasonRecorder {
// 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;
@@ -53,6 +55,12 @@ private:
* \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.
@@ -90,7 +98,7 @@ public:
* \param[in] depth The number of bytes of each sample.
* \param[in] num_filter The filter number (between 0 and 2).
*/
- void start_recording(std::uint8_t num_channels, size_t sample_rate, std::uint8_t depth, std::uint8_t num_filter);
+ 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().
--
GitLab