diff --git a/src/recorder.cpp b/src/recorder.cpp
index efb1d7291977fc2490ab7d6296a4b358c30791b8..2f02a81c07c9af72d2045c6f762fecfc36a2bcd9 100644
--- a/src/recorder.cpp
+++ b/src/recorder.cpp
@@ -6,11 +6,13 @@
*/
#include "recorder.h"
+#include <unistd.h>
#include <stdexcept>
#include <iostream>
#include <vector>
#include <array>
#include <algorithm>
+#include <fstream>
JasonRecorder::JasonRecorder(bool verbose) : verbose(verbose) {
// create libusb context
@@ -90,7 +92,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,70 +137,79 @@ 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::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) (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";
+
+ // 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::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.01);
+
+ // 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";
+ // sleep(0.01);
+
+ // libusb_clear_halt(handle, ENDPOINT_SEND);
}
this->num_channels = num_channels;
@@ -250,6 +261,8 @@ 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;
imu_data.resize(0);
diff --git a/src/recorder.h b/src/recorder.h
index 4bcf71f3c3786e87b517fae464fcd910987787e2..98a7a48f73c56206b3937db0ae5179f908cd989b 100644
--- a/src/recorder.h
+++ b/src/recorder.h
@@ -33,8 +33,13 @@ 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:
// libusb handles
struct libusb_context *ctx;