Add sensors set-up trames encoding

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;

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().