tami/MYNT-EYE-S-SDK
6
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Add imu write & read

Browse Source
This commit is contained in:
John Zhao 2018-04-10 17:48:27 +08:00
parent 46b5ed077a
commit b81a905e78
4 changed files with 161 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
doc/zh-Hans/spec_imu_data.md
View File

@ -32,7 +32,7 @@ IMU 包/小包，是一组 IMU 数据。
| Name | Offset | FrameID | Accelerometer | Temperature | Gyroscope |
| :--- | :----- | :------ | :------------ | :---------- | :-------- |
| 字节数 | 2 | 2 | 6 | 2 | 6 |
| 字节数 | 2 | 2 | 3 | 1 | 3 |
| 类型 | int16_t | uint16_t | int8_t * 3 | int8_t | int8_t * 3 |
| Description | 相对基准时间戳的偏移量 | 图像帧 ID | 加速度计 x y z 三轴的值 | IMU 的温度 | 陀螺仪 x y z 三轴的值 |

49
src/internal/channels.cc
View File

@ -108,11 +108,11 @@ std::int32_t Channels::GetControlValue(const Option &option) const {
case Option::BRIGHTNESS:
case Option::CONTRAST:
std::int32_t value;
if (!PuControlQuery(option, uvc::PU_QUERY_GET, &value)) {
if (PuControlQuery(option, uvc::PU_QUERY_GET, &value)) {
return value;
} else {
LOG(WARNING) << option << " get value failed";
return -1;
} else {
return value;
}
case Option::FRAME_RATE:
case Option::IMU_FREQUENCY:
@ -251,12 +251,12 @@ void Channels::XuCamCtrlSet(Option option, std::int32_t value) const {
std::uint8_t data[3] = {static_cast<std::uint8_t>(id & 0xFF),
static_cast<std::uint8_t>((value >> 8) & 0xFF),
static_cast<std::uint8_t>(value & 0xFF)};
if (!XuCamCtrlQuery(uvc::XU_QUERY_SET, 3, data)) {
LOG(WARNING) << "XuCamCtrlSet value (" << value << ") of " << option
<< " failed";
} else {
if (XuCamCtrlQuery(uvc::XU_QUERY_SET, 3, data)) {
VLOG(2) << "XuCamCtrlSet value (" << value << ") of " << option
<< " success";
} else {
LOG(WARNING) << "XuCamCtrlSet value (" << value << ") of " << option
<< " failed";
}
}
@ -264,14 +264,39 @@ bool Channels::XuHalfDuplexSet(Option option, xu_cmd_t cmd) const {
int id = XuHalfDuplexId(option);
std::uint8_t data[3] = {// must be 3 now
static_cast<std::uint8_t>(id & 0xFF), cmd};
if (!XuControlQuery(CHANNEL_HALF_DUPLEX, uvc::XU_QUERY_SET, 3, data)) {
LOG(WARNING) << "XuHalfDuplexSet value (0x" << std::hex << std::uppercase
<< cmd << ") of " << option << " failed";
return false;
} else {
if (XuControlQuery(CHANNEL_HALF_DUPLEX, uvc::XU_QUERY_SET, 3, data)) {
VLOG(2) << "XuHalfDuplexSet value (0x" << std::hex << std::uppercase << cmd
<< ") of " << option << " success";
return true;
} else {
LOG(WARNING) << "XuHalfDuplexSet value (0x" << std::hex << std::uppercase
<< cmd << ") of " << option << " failed";
return false;
}
}
bool Channels::XuImuWrite(const ImuReqPacket &req) const {
auto &&data = req.to_data();
if (XuControlQuery(
CHANNEL_IMU_WRITE, uvc::XU_QUERY_SET, data.size(), data.data())) {
VLOG(2) << "XuImuWrite request success";
return true;
} else {
LOG(WARNING) << "XuImuWrite request failed";
return false;
}
}
bool Channels::XuImuRead(ImuResPacket *res) const {
static std::uint8_t data[2000]{};
// std::fill(data, data + 2000, 0); // reset
if (XuControlQuery(CHANNEL_IMU_READ, uvc::XU_QUERY_GET, 2000, data)) {
VLOG(2) << "XuImuRead response success";
res->from_data(data);
return true;
} else {
LOG(WARNING) << "XuImuRead response failed";
return false;
}
}

5
src/internal/channels.h
View File

@ -6,6 +6,8 @@
#include <memory>
#include "mynteye/mynteye.h"
#include "internal/types.h"
#include "uvc/uvc.h"
MYNTEYE_BEGIN_NAMESPACE
@ -70,6 +72,9 @@ class Channels {
bool XuHalfDuplexSet(Option option, xu_cmd_t cmd) const;
bool XuImuWrite(const ImuReqPacket &req) const;
bool XuImuRead(ImuResPacket *res) const;
control_info_t PuControlInfo(Option option) const;
control_info_t XuControlInfo(Option option) const;

118
src/internal/types.h
View File

@ -4,6 +4,7 @@
#include <cstdint>
#include <array>
#include <bitset>
#include <string>
#include <vector>
@ -140,6 +141,10 @@ struct ImagePacket {
ImagePacket() = default;
explicit ImagePacket(std::uint8_t *data) {
from_data(data);
}
void from_data(std::uint8_t *data) {
header = *data;
size = *(data + 1);
frame_id = (*(data + 2) << 8) + *(data + 3);
@ -151,6 +156,119 @@ struct ImagePacket {
};
#pragma pack(pop)
/**
* @ingroup datatypes
* Imu request packet.
*/
#pragma pack(push, 1)
struct ImuReqPacket {
std::uint8_t header;
std::uint32_t serial_number;
ImuReqPacket() = default;
explicit ImuReqPacket(std::uint32_t serial_number)
: ImuReqPacket(0x5A, serial_number) {}
ImuReqPacket(std::uint8_t header, std::uint32_t serial_number)
: header(header), serial_number(serial_number) {}
std::array<std::uint8_t, 5> to_data() const {
return {header, static_cast<std::uint8_t>((serial_number >> 24) & 0xFF),
static_cast<std::uint8_t>((serial_number >> 16) & 0xFF),
static_cast<std::uint8_t>((serial_number >> 8) & 0xFF),
static_cast<std::uint8_t>(serial_number & 0xFF)};
}
};
#pragma pack(pop)
/**
* @ingroup datatypes
* Imu segment.
*/
#pragma pack(push, 1)
struct ImuSegment {
std::int16_t offset;
std::uint16_t frame_id;
std::int8_t accel[3];
std::int8_t temperature;
std::int8_t gyro[3];
ImuSegment() = default;
explicit ImuSegment(std::uint8_t *data) {
from_data(data);
}
void from_data(std::uint8_t *data) {
offset = (*(data) << 8) + *(data + 1);
frame_id = (*(data + 2) << 8) + *(data + 3);
accel[0] = *(data + 4);
accel[1] = *(data + 5);
accel[2] = *(data + 6);
temperature = *(data + 7);
gyro[0] = *(data + 8);
gyro[1] = *(data + 9);
gyro[2] = *(data + 10);
}
};
#pragma pack(pop)
/**
* @ingroup datatypes
* Imu packet.
*/
#pragma pack(push, 1)
struct ImuPacket {
std::uint32_t serial_number;
std::uint32_t timestamp;
std::uint8_t count;
std::vector<ImuSegment> segments;
ImuPacket() = default;
explicit ImuPacket(std::uint8_t *data) {
from_data(data);
}
void from_data(std::uint8_t *data) {
serial_number = (*(data) << 24) + (*(data + 1) << 16) + (*(data + 2) << 8) +
*(data + 3);
timestamp = (*(data + 4) << 24) + (*(data + 5) << 16) + (*(data + 6) << 8) +
*(data + 7);
count = *(data + 8);
std::size_t seg_n = sizeof(ImuSegment);
for (std::size_t i = 0; i < count; i++) {
segments.push_back(ImuSegment(data + 9 + (seg_n * i)));
}
}
};
#pragma pack(pop)
/**
* @ingroup datatypes
* Imu response packet.
*/
#pragma pack(push, 1)
struct ImuResPacket {
std::uint8_t header;
std::uint8_t state;
std::uint16_t size;
ImuPacket packet;
std::uint8_t checksum;
ImuResPacket() = default;
explicit ImuResPacket(std::uint8_t *data) {
from_data(data);
}
void from_data(std::uint8_t *data) {
header = *data;
state = *(data + 1);
size = (*(data + 2) << 8) + *(data + 3);
packet.from_data(data + 4);
checksum = *(data + 4 + size);
}
};
#pragma pack(pop)
#undef MYNTEYE_PROPERTY
MYNTEYE_END_NAMESPACE