Skip to content
Snippets Groups Projects
Commit 946c424f authored by Philemon Prevot's avatar Philemon Prevot
Browse files

Add Accel Gyro and Mag aliases and test start message

parent 95637f0f
Branches
No related tags found
1 merge request!3Add packet timestamp file logging to QHBv3 parser and correct wrong mag data bytes reading
Show whitespace changes
Inline Side-by-side
src/recorder.cpp
+ 70
− 57
View file @ 946c424f
......@@ -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);
......
src/recorder.h
+ 7
− 2
View file @ 946c424f
......@@ -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;
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment