Correct parametrization packets and adding sleep for QHB processing time (it works now!)

src/recorder.cpp

@@ -15,6 +15,8 @@
 #include <algorithm>
 #include <fstream>
 #include <cstring>
+#include <thread>
+#include <chrono>
 
 JasonRecorder::JasonRecorder(bool verbose) : verbose(verbose) {
     // create libusb context
@@ -139,64 +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) (ACCEL),
-        //     (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::cout << "Accel set\n";
-        // sleep(0.01);
+        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) (GYRO),
-        //     (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::cout << "Gyro set\n";
-        // sleep(0.1);
+        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) (MAG),
-        //     (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::cout << "Mag set\n";
+        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,