Merge branch 'imu_2.0_protocl' into develop
This commit is contained in:
commit
a499136449
|
@ -347,7 +347,12 @@ class MYNTEYE_API API {
|
|||
std::vector<api::MotionData> GetMotionDatas();
|
||||
|
||||
/**
|
||||
* Enable motion datas with timestamp correspondence of some stream.
|
||||
* enable motion datas timestamp correspondence in device.
|
||||
*/
|
||||
void EnableImuTimestampCorrespondence(bool is_enable);
|
||||
|
||||
/**
|
||||
* Enable motion datas with timestamp correspondence of some stream in api.
|
||||
*/
|
||||
void EnableTimestampCorrespondence(const Stream &stream,
|
||||
bool keep_accel_then_gyro = true);
|
||||
|
@ -385,6 +390,9 @@ class MYNTEYE_API API {
|
|||
|
||||
motion_callback_t callback_;
|
||||
|
||||
bool api_correspondence_enable_;
|
||||
bool dev_correspondence_enable_;
|
||||
|
||||
void CheckImageParams();
|
||||
};
|
||||
|
||||
|
|
|
@ -290,6 +290,10 @@ class MYNTEYE_API Device {
|
|||
* Enable cache motion datas.
|
||||
*/
|
||||
void EnableMotionDatas();
|
||||
/**
|
||||
* Enable motion datas timestamp correspondence.
|
||||
*/
|
||||
void EnableImuCorrespondence(bool is_enable);
|
||||
/**
|
||||
* Enable cache motion datas.
|
||||
*/
|
||||
|
|
|
@ -31,6 +31,8 @@ int main(int argc, char *argv[]) {
|
|||
|
||||
// Enable this will cache the motion datas until you get them.
|
||||
api->EnableMotionDatas();
|
||||
// Enable imu timestamp correspondence int device;
|
||||
api->EnableImuTimestampCorrespondence(true);
|
||||
|
||||
api->Start(Source::ALL);
|
||||
|
||||
|
|
|
@ -167,23 +167,8 @@ cv::Rect CVPainter::DrawImuData(
|
|||
img, ss.str(), gravity, 5, 0,
|
||||
sign * (10 + rect_i.height + rect_a.height));
|
||||
|
||||
static double gyro1_s = 0.0;
|
||||
static double gyro2_s = 0.0;
|
||||
static double gyro3_s = 0.0;
|
||||
|
||||
if (data.gyro[0] > 0.01 ||
|
||||
data.gyro[1] > 0.01 ||
|
||||
data.gyro[2] > 0.01 ||
|
||||
data.gyro[0] < -0.01 ||
|
||||
data.gyro[1] < -0.01 ||
|
||||
data.gyro[2] < -0.01 ) {
|
||||
gyro1_s = data.gyro[0];
|
||||
gyro2_s = data.gyro[1];
|
||||
gyro3_s = data.gyro[2];
|
||||
}
|
||||
|
||||
Clear(ss) << "gyro(x,y,z): " << fmt_imu << gyro1_s << "," << fmt_imu
|
||||
<< gyro2_s << "," << fmt_imu << gyro3_s;
|
||||
Clear(ss) << "gyro(x,y,z): " << fmt_imu << data.gyro[0] << "," << fmt_imu
|
||||
<< data.gyro[1] << "," << fmt_imu << data.gyro[2];
|
||||
cv::Rect rect_g = DrawText(
|
||||
img, ss.str(), gravity, 5, 0,
|
||||
sign * (15 + rect_i.height + rect_a.height + rect_p.height));
|
||||
|
|
|
@ -213,7 +213,9 @@ std::vector<std::string> get_plugin_paths() {
|
|||
} // namespace
|
||||
|
||||
API::API(std::shared_ptr<Device> device, CalibrationModel calib_model)
|
||||
: device_(device), correspondence_(nullptr) {
|
||||
: device_(device), correspondence_(nullptr),
|
||||
api_correspondence_enable_(false),
|
||||
dev_correspondence_enable_(false) {
|
||||
VLOG(2) << __func__;
|
||||
// std::dynamic_pointer_cast<StandardDevice>(device_);
|
||||
synthetic_.reset(new Synthetic(this, calib_model));
|
||||
|
@ -505,6 +507,15 @@ std::vector<api::MotionData> API::GetMotionDatas() {
|
|||
|
||||
void API::EnableTimestampCorrespondence(const Stream &stream,
|
||||
bool keep_accel_then_gyro) {
|
||||
if (!dev_correspondence_enable_) {
|
||||
api_correspondence_enable_ = keep_accel_then_gyro;
|
||||
} else {
|
||||
LOG(WARNING) << "dev_correspondence_enable_ "
|
||||
"has been set to true, "
|
||||
"you should close it first when you want to use "
|
||||
"api_correspondence_enable_.";
|
||||
return;
|
||||
}
|
||||
if (correspondence_ == nullptr) {
|
||||
correspondence_.reset(new Correspondence(device_, stream));
|
||||
correspondence_->KeepAccelThenGyro(keep_accel_then_gyro);
|
||||
|
@ -526,6 +537,19 @@ void API::EnableTimestampCorrespondence(const Stream &stream,
|
|||
}
|
||||
}
|
||||
|
||||
void API::EnableImuTimestampCorrespondence(bool is_enable) {
|
||||
if (!api_correspondence_enable_) {
|
||||
dev_correspondence_enable_= is_enable;
|
||||
} else {
|
||||
LOG(WARNING) << "api_correspondence_enable_ "
|
||||
"has been set to true, "
|
||||
"you should close it first when you want to use "
|
||||
"dev_correspondence_enable_.";
|
||||
return;
|
||||
}
|
||||
device_->EnableImuCorrespondence(is_enable);
|
||||
}
|
||||
|
||||
void API::EnablePlugin(const std::string &path) {
|
||||
static DL dl;
|
||||
CHECK(dl.Open(path.c_str())) << "Open plugin failed: " << path;
|
||||
|
|
|
@ -30,6 +30,32 @@
|
|||
|
||||
MYNTEYE_BEGIN_NAMESPACE
|
||||
|
||||
mynteye::ImuPacket2 to_pak2(const mynteye::ImuPacket& pak1,
|
||||
const int &accel_range,
|
||||
const int &gyro_range) {
|
||||
mynteye::ImuPacket2 res;
|
||||
res.version = pak1.version;
|
||||
res.count = pak1.count;
|
||||
res.serial_number = pak1.serial_number;
|
||||
for (size_t i = 0; i < pak1.segments.size(); i++) {
|
||||
mynteye::ImuSegment2 tpr;
|
||||
tpr.accel[0] = pak1.segments[i].accel[0] * 1.f * accel_range / 0x10000;
|
||||
tpr.accel[1] = pak1.segments[i].accel[1] * 1.f * accel_range / 0x10000;
|
||||
tpr.accel[2] = pak1.segments[i].accel[2] * 1.f * accel_range / 0x10000;
|
||||
tpr.gyro[0] = pak1.segments[i].gyro[0] * gyro_range / 0x10000;
|
||||
tpr.gyro[1] = pak1.segments[i].gyro[1] * gyro_range / 0x10000;
|
||||
tpr.gyro[2] = pak1.segments[i].gyro[2] * gyro_range / 0x10000;
|
||||
tpr.flag = pak1.segments[i].flag;
|
||||
tpr.frame_id = pak1.segments[i].frame_id;
|
||||
tpr.is_ets = pak1.segments[i].is_ets;
|
||||
tpr.temperature = pak1.segments[i].temperature / 326.8f + 25;
|
||||
tpr.timestamp = pak1.segments[i].timestamp;
|
||||
res.segments.push_back(tpr);
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
|
||||
namespace {
|
||||
|
||||
const uvc::xu mynteye_xu = {3, 2,
|
||||
|
@ -112,12 +138,20 @@ Channels::Channels(const std::shared_ptr<uvc::device> &device,
|
|||
adapter_(adapter),
|
||||
is_imu_tracking_(false),
|
||||
is_imu_proto2_(false),
|
||||
enable_imu_correspondence(false),
|
||||
imu_track_stop_(false),
|
||||
imu_sn_(0),
|
||||
imu_callback_(nullptr),
|
||||
dev_info_(nullptr) {
|
||||
VLOG(2) << __func__;
|
||||
UpdateControlInfos();
|
||||
accel_range = GetControlValue(Option::ACCELEROMETER_RANGE);
|
||||
if (accel_range == -1)
|
||||
accel_range = GetAccelRangeDefault();
|
||||
|
||||
gyro_range = GetControlValue(Option::GYROSCOPE_RANGE);
|
||||
if (gyro_range == -1)
|
||||
gyro_range = GetGyroRangeDefault();
|
||||
}
|
||||
|
||||
Channels::~Channels() {
|
||||
|
@ -328,7 +362,7 @@ bool Channels::SetControlValue(const Option &option, std::uint64_t value) {
|
|||
case Option::MIN_EXPOSURE_TIME:
|
||||
case Option::IIC_ADDRESS_SETTING:
|
||||
case Option::ZERO_DRIFT_CALIBRATION:
|
||||
LOG(WARNING) << option << " refer to function SetControlValue(const Option &option, std::int32_t value)";
|
||||
LOG(WARNING) << option << " refer to function SetControlValue(const Option &option, std::int32_t value)"; // NOLINT
|
||||
break;
|
||||
case Option::ERASE_CHIP:
|
||||
LOG(WARNING) << option << " set value useless";
|
||||
|
@ -379,7 +413,7 @@ void Channels::SetImuCallback(imu_callback_t callback) {
|
|||
}
|
||||
|
||||
void Channels::DoImuTrack() {
|
||||
if (IsImuProc2()) {
|
||||
if (IsImuProtocol2()) {
|
||||
return DoImuTrack2();
|
||||
} else {
|
||||
return DoImuTrack1();
|
||||
|
@ -424,7 +458,7 @@ void Channels::DoImuTrack1() {
|
|||
|
||||
if (imu_callback_) {
|
||||
for (auto &&packet : res_packet.packets) {
|
||||
imu_callback_(packet);
|
||||
imu_callback_(to_pak2(packet, accel_range, gyro_range));
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -432,49 +466,41 @@ void Channels::DoImuTrack1() {
|
|||
}
|
||||
|
||||
void Channels::DoImuTrack2() {
|
||||
// static ImuReqPacket req_packet{0};
|
||||
// static ImuResPacket res_packet;
|
||||
// LOG(INFO) << "wait to adapter!";
|
||||
static ImuReqPacket2 req_packet{0x5A, imu_sn_, enable_imu_correspondence};
|
||||
static ImuResPacket2 res_packet;
|
||||
if (!XuImuWrite(req_packet)) {
|
||||
return;
|
||||
}
|
||||
if (!XuImuRead(&res_packet)) {
|
||||
return;
|
||||
}
|
||||
if (res_packet.packets.size() == 0) {
|
||||
return;
|
||||
}
|
||||
if (res_packet.packets.back().count == 0) {
|
||||
return;
|
||||
}
|
||||
VLOG(2) << "Imu req sn: " << imu_sn_ << ", res count: " << []() {
|
||||
std::size_t n = 0;
|
||||
for (auto &&packet : res_packet.packets) {
|
||||
n += packet.count;
|
||||
}
|
||||
return n;
|
||||
}();
|
||||
auto &&sn = res_packet.packets.back().serial_number;
|
||||
if (imu_sn_ == sn) {
|
||||
VLOG(2) << "New imu not ready, dropped";
|
||||
return;
|
||||
}
|
||||
imu_sn_ = sn;
|
||||
if (imu_callback_) {
|
||||
for (auto &&packet : res_packet.packets) {
|
||||
imu_callback_(packet);
|
||||
}
|
||||
}
|
||||
|
||||
// req_packet.serial_number = imu_sn_;
|
||||
// if (!XuImuWrite(req_packet)) {
|
||||
// return;
|
||||
// }
|
||||
|
||||
// if (!XuImuRead(&res_packet)) {
|
||||
// return;
|
||||
// }
|
||||
|
||||
// if (res_packet.packets.size() == 0) {
|
||||
// return;
|
||||
// }
|
||||
|
||||
// if (res_packet.packets.back().count == 0) {
|
||||
// return;
|
||||
// }
|
||||
|
||||
// VLOG(2) << "Imu req sn: " << imu_sn_ << ", res count: " << []() {
|
||||
// std::size_t n = 0;
|
||||
// for (auto &&packet : res_packet.packets) {
|
||||
// n += packet.count;
|
||||
// }
|
||||
// return n;
|
||||
// }();
|
||||
|
||||
// auto &&sn = res_packet.packets.back().serial_number;
|
||||
// if (imu_sn_ == sn) {
|
||||
// VLOG(2) << "New imu not ready, dropped";
|
||||
// return;
|
||||
// }
|
||||
// imu_sn_ = sn;
|
||||
|
||||
// if (imu_callback_) {
|
||||
// for (auto &&packet : res_packet.packets) {
|
||||
// imu_callback_(packet);
|
||||
// }
|
||||
// }
|
||||
|
||||
// res_packet.packets.clear();
|
||||
LOG(INFO) << "wait to adapter!";
|
||||
res_packet.packets.clear();
|
||||
}
|
||||
|
||||
|
||||
|
@ -499,7 +525,7 @@ void Channels::StartImuTracking(imu_callback_t callback) {
|
|||
<< ", sleep " << (IMU_TRACK_PERIOD - time_elapsed_ms) << " ms";
|
||||
}
|
||||
};
|
||||
if (IsImuProc2()) {
|
||||
if (IsImuProtocol2()) {
|
||||
while (!imu_track_stop_) {
|
||||
auto &&time_beg = times::now();
|
||||
DoImuTrack2();
|
||||
|
@ -801,7 +827,7 @@ bool Channels::XuHalfDuplexSet(Option option, std::uint64_t value) const {
|
|||
static_cast<std::uint8_t>((value >> 56) & 0xFF)};
|
||||
|
||||
if (XuControlQuery(CHANNEL_HALF_DUPLEX, uvc::XU_QUERY_SET, 20, data)) {
|
||||
VLOG(2) << "XuHalfDuplexSet value (0x" << std::hex << std::uppercase << value
|
||||
VLOG(2) << "XuHalfDuplexSet value (0x" << std::hex << std::uppercase << value // NOLINT
|
||||
<< ") of " << option << " success";
|
||||
return true;
|
||||
} else {
|
||||
|
@ -813,7 +839,7 @@ bool Channels::XuHalfDuplexSet(Option option, std::uint64_t value) const {
|
|||
|
||||
bool Channels::XuImuWrite(const ImuReqPacket &req) const {
|
||||
auto &&data = req.to_data();
|
||||
// LOG(INFO) << data.size() << "||" << (int)data[0] << " " << (int)data[1] << " " << (int)data[2] << " " << (int)data[3] << " " << (int)data[4];
|
||||
// LOG(INFO) << data.size() << "||" << (int)data[0] << " " << (int)data[1] << " " << (int)data[2] << " " << (int)data[3] << " " << (int)data[4]; // NOLINT
|
||||
if (XuControlQuery(
|
||||
CHANNEL_IMU_WRITE, uvc::XU_QUERY_SET, data.size(), data.data())) {
|
||||
VLOG(2) << "XuImuWrite request success";
|
||||
|
@ -824,12 +850,48 @@ bool Channels::XuImuWrite(const ImuReqPacket &req) const {
|
|||
}
|
||||
}
|
||||
|
||||
bool Channels::XuImuWrite(const ImuReqPacket2 &req) const {
|
||||
auto &&data = req.to_data();
|
||||
// LOG(INFO) << data.size() << "||" << (int)data[0] << " " << (int)data[1] << " " << (int)data[2] << " " << (int)data[3] << " " << (int)data[4]; // NOLINT
|
||||
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(ImuResPacket2 *res) const {
|
||||
static std::uint8_t data[2000]{};
|
||||
if (XuControlQuery(CHANNEL_IMU_READ, uvc::XU_QUERY_GET, 2000, data)) {
|
||||
adapter_->GetImuResPacket2(data, res, enable_imu_correspondence);
|
||||
if (res->header != 0x5B) {
|
||||
LOG(WARNING) << "Imu response packet header must be 0x5B, but 0x"
|
||||
<< std::hex << std::uppercase << std::setw(2)
|
||||
<< std::setfill('0') << static_cast<int>(res->header)
|
||||
<< " now";
|
||||
return false;
|
||||
}
|
||||
if (res->state != 0) {
|
||||
LOG(WARNING) << "Imu response packet state must be 0, but " << res->state // NOLINT
|
||||
<< " now";
|
||||
return false;
|
||||
}
|
||||
|
||||
VLOG(2) << "XuImuRead response success";
|
||||
return true;
|
||||
} else {
|
||||
LOG(WARNING) << "XuImuRead response 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)) {
|
||||
adapter_->GetImuResPacket(data, res);
|
||||
|
||||
if (res->header != 0x5B) {
|
||||
LOG(WARNING) << "Imu response packet header must be 0x5B, but 0x"
|
||||
<< std::hex << std::uppercase << std::setw(2)
|
||||
|
|
|
@ -54,7 +54,7 @@ class MYNTEYE_API Channels {
|
|||
XU_CMD_LAST
|
||||
} xu_cmd_t;
|
||||
|
||||
using imu_callback_t = std::function<void(const ImuPacket &packet)>;
|
||||
using imu_callback_t = std::function<void(const ImuPacket2 &packet)>;
|
||||
|
||||
using device_info_t = FileChannel::device_info_t;
|
||||
using img_params_t = FileChannel::img_params_t;
|
||||
|
@ -89,7 +89,10 @@ class MYNTEYE_API Channels {
|
|||
device_info_t *info, img_params_t *img_params, imu_params_t *imu_params);
|
||||
bool SetFiles(
|
||||
device_info_t *info, img_params_t *img_params, imu_params_t *imu_params);
|
||||
inline bool IsImuProc2() const { return is_imu_proto2_; }
|
||||
inline bool IsImuProtocol2() const { return is_imu_proto2_; }
|
||||
inline void EnableImuCorrespondence(bool is_enable) {
|
||||
enable_imu_correspondence = is_enable;
|
||||
}
|
||||
|
||||
private:
|
||||
bool PuControlRange(
|
||||
|
@ -118,7 +121,9 @@ class MYNTEYE_API Channels {
|
|||
bool XuHalfDuplexSet(Option option, std::uint64_t value) const;
|
||||
|
||||
bool XuImuWrite(const ImuReqPacket &req) const;
|
||||
bool XuImuWrite(const ImuReqPacket2 &req) const;
|
||||
bool XuImuRead(ImuResPacket *res) const;
|
||||
bool XuImuRead(ImuResPacket2 *res) const;
|
||||
|
||||
bool XuFileQuery(uvc::xu_query query, uint16_t size, uint8_t *data) const;
|
||||
|
||||
|
@ -134,8 +139,11 @@ class MYNTEYE_API Channels {
|
|||
|
||||
bool is_imu_tracking_;
|
||||
bool is_imu_proto2_;
|
||||
bool enable_imu_correspondence;
|
||||
std::thread imu_track_thread_;
|
||||
volatile bool imu_track_stop_;
|
||||
int accel_range;
|
||||
int gyro_range;
|
||||
|
||||
std::uint32_t imu_sn_;
|
||||
imu_callback_t imu_callback_;
|
||||
|
@ -157,6 +165,8 @@ class ChannelsAdapter {
|
|||
virtual std::vector<std::int32_t> GetGyroRangeValues() = 0;
|
||||
|
||||
virtual void GetImuResPacket(const std::uint8_t *data, ImuResPacket *res) = 0;
|
||||
virtual void GetImuResPacket2(const std::uint8_t *data,
|
||||
ImuResPacket2 *res, bool is_correspondence_on) = 0;
|
||||
|
||||
protected:
|
||||
Model model_;
|
||||
|
|
|
@ -63,6 +63,34 @@ struct ImuReqPacket {
|
|||
};
|
||||
#pragma pack(pop)
|
||||
|
||||
/**
|
||||
* @ingroup datatypes
|
||||
* Imu request packet 2.0.
|
||||
*/
|
||||
#pragma pack(push, 1)
|
||||
struct ImuReqPacket2 {
|
||||
std::uint8_t header;
|
||||
std::uint32_t serial_number;
|
||||
std::uint8_t correspondence_switch;
|
||||
|
||||
ImuReqPacket2() = default;
|
||||
explicit ImuReqPacket2(std::uint32_t serial_number)
|
||||
: ImuReqPacket2(0x5A, serial_number) {}
|
||||
ImuReqPacket2(std::uint8_t header, std::uint32_t serial_number)
|
||||
: ImuReqPacket2(header, serial_number, false) {}
|
||||
ImuReqPacket2(std::uint8_t header,
|
||||
std::uint32_t serial_number,
|
||||
bool correspondence_switch_in)
|
||||
: header(header),
|
||||
serial_number(serial_number),
|
||||
correspondence_switch(correspondence_switch_in ? 1:0) {}
|
||||
|
||||
std::array<std::uint8_t, 5> to_data() const {
|
||||
return {header, correspondence_switch, 0, 0, 0};
|
||||
}
|
||||
};
|
||||
#pragma pack(pop)
|
||||
|
||||
/**
|
||||
* @ingroup datatypes
|
||||
* Imu segment.
|
||||
|
@ -80,6 +108,23 @@ struct ImuSegment {
|
|||
};
|
||||
#pragma pack(pop)
|
||||
|
||||
/**
|
||||
* @ingroup datatypes
|
||||
* Imu segment.
|
||||
*/
|
||||
#pragma pack(push, 1)
|
||||
struct ImuSegment2 {
|
||||
std::uint32_t frame_id;
|
||||
std::uint64_t timestamp;
|
||||
std::uint8_t flag;
|
||||
// Is external time source
|
||||
bool is_ets;
|
||||
float temperature;
|
||||
float accel[3];
|
||||
float gyro[3];
|
||||
};
|
||||
#pragma pack(pop)
|
||||
|
||||
/**
|
||||
* @ingroup datatypes
|
||||
* Imu packet.
|
||||
|
@ -92,6 +137,18 @@ struct ImuPacket {
|
|||
std::vector<ImuSegment> segments;
|
||||
};
|
||||
#pragma pack(pop)
|
||||
/**
|
||||
* @ingroup datatypes
|
||||
* Imu packet.
|
||||
*/
|
||||
#pragma pack(push, 1)
|
||||
struct ImuPacket2 {
|
||||
std::uint8_t version;
|
||||
std::uint8_t count;
|
||||
std::uint32_t serial_number;
|
||||
std::vector<ImuSegment2> segments;
|
||||
};
|
||||
#pragma pack(pop)
|
||||
|
||||
/**
|
||||
* @ingroup datatypes
|
||||
|
@ -108,6 +165,21 @@ struct ImuResPacket {
|
|||
};
|
||||
#pragma pack(pop)
|
||||
|
||||
/**
|
||||
* @ingroup datatypes
|
||||
* Imu response packet.
|
||||
*/
|
||||
#pragma pack(push, 1)
|
||||
struct ImuResPacket2 {
|
||||
std::uint8_t version;
|
||||
std::uint8_t header;
|
||||
std::uint8_t state;
|
||||
std::uint16_t size;
|
||||
std::vector<ImuPacket2> packets;
|
||||
std::uint8_t checksum;
|
||||
};
|
||||
#pragma pack(pop)
|
||||
|
||||
MYNTEYE_END_NAMESPACE
|
||||
|
||||
#endif // MYNTEYE_DEVICE_CHANNEL_DEF_H_
|
||||
|
|
|
@ -602,6 +602,10 @@ void Device::StopMotionTracking() {
|
|||
|
||||
void Device::OnStereoStreamUpdate() {}
|
||||
|
||||
void Device::EnableImuCorrespondence(bool is_enable) {
|
||||
channels_->EnableImuCorrespondence(is_enable);
|
||||
}
|
||||
|
||||
void Device::ReadAllInfos() {
|
||||
device_info_ = std::make_shared<DeviceInfo>();
|
||||
|
||||
|
|
|
@ -70,7 +70,7 @@ void Motions::SetMotionCallback(motion_callback_t callback) {
|
|||
if (gyro_range == -1)
|
||||
gyro_range = channels_->GetGyroRangeDefault();
|
||||
|
||||
channels_->SetImuCallback([this](const ImuPacket &packet) {
|
||||
channels_->SetImuCallback([this](const ImuPacket2 &packet) {
|
||||
if (!motion_callback_ && !motion_datas_enabled_) {
|
||||
return;
|
||||
}
|
||||
|
@ -85,13 +85,34 @@ void Motions::SetMotionCallback(motion_callback_t callback) {
|
|||
imu->timestamp = seg.timestamp;
|
||||
imu->flag = seg.flag;
|
||||
imu->is_ets = seg.is_ets;
|
||||
imu->temperature = seg.temperature / 326.8f + 25;
|
||||
imu->accel[0] = seg.accel[0] * 1.f * accel_range / 0x10000;
|
||||
imu->accel[1] = seg.accel[1] * 1.f * accel_range / 0x10000;
|
||||
imu->accel[2] = seg.accel[2] * 1.f * accel_range / 0x10000;
|
||||
imu->gyro[0] = seg.gyro[0] * 1.f * gyro_range / 0x10000;
|
||||
imu->gyro[1] = seg.gyro[1] * 1.f * gyro_range / 0x10000;
|
||||
imu->gyro[2] = seg.gyro[2] * 1.f * gyro_range / 0x10000;
|
||||
// imu->temperature = seg.temperature / 326.8f + 25;
|
||||
imu->temperature = seg.temperature;
|
||||
// LOG(INFO) << "beforea" << seg.accel[0];
|
||||
// LOG(INFO) << "before" << seg.accel[1];
|
||||
// LOG(INFO) << "before" << seg.accel[2];
|
||||
// LOG(INFO) << "beforeg" << seg.gyro[0];
|
||||
// LOG(INFO) << "before" << seg.gyro[1];
|
||||
// LOG(INFO) << "before" << seg.gyro[2];
|
||||
// imu->accel[0] = seg.accel[0] * 1.f * accel_range / 0x10000;
|
||||
// imu->accel[1] = seg.accel[1] * 1.f * accel_range / 0x10000;
|
||||
// imu->accel[2] = seg.accel[2] * 1.f * accel_range / 0x10000;
|
||||
// imu->gyro[0] = seg.gyro[0] * 1.f * gyro_range / 0x10000;
|
||||
// imu->gyro[1] = seg.gyro[1] * 1.f * gyro_range / 0x10000;
|
||||
// imu->gyro[2] = seg.gyro[2] * 1.f * gyro_range / 0x10000;
|
||||
|
||||
imu->accel[0] = seg.accel[0];
|
||||
imu->accel[1] = seg.accel[1];
|
||||
imu->accel[2] = seg.accel[2];
|
||||
imu->gyro[0] = seg.gyro[0];
|
||||
imu->gyro[1] = seg.gyro[1];
|
||||
imu->gyro[2] = seg.gyro[2];
|
||||
|
||||
// LOG(INFO)<< "aftera" << imu->accel[0];
|
||||
// LOG(INFO)<< "after" << imu->accel[1];
|
||||
// LOG(INFO)<< "after" << imu->accel[2];
|
||||
// LOG(INFO)<< "afterg" << imu->gyro[0];
|
||||
// LOG(INFO)<< "afterg" << imu->gyro[1];
|
||||
// LOG(INFO)<< "afterg" << imu->gyro[2];
|
||||
|
||||
bool proc_assembly = ((proc_mode_ & ProcessMode::PROC_IMU_ASSEMBLY) > 0);
|
||||
bool proc_temp_drift = ((proc_mode_ & ProcessMode::PROC_IMU_TEMP_DRIFT) > 0);
|
||||
|
|
|
@ -122,4 +122,9 @@ void StandardChannelsAdapter::GetImuResPacket(
|
|||
unpack_imu_res_packet(data, res);
|
||||
}
|
||||
|
||||
void StandardChannelsAdapter::GetImuResPacket2(
|
||||
const std::uint8_t *data, ImuResPacket2 *res, bool is_correspondence_on) {
|
||||
LOG(WARNING) << "s1 device can't use ImuResPacket2.0 check the firmware.";
|
||||
}
|
||||
|
||||
MYNTEYE_END_NAMESPACE
|
||||
|
|
|
@ -35,6 +35,10 @@ class StandardChannelsAdapter : public ChannelsAdapter {
|
|||
std::vector<std::int32_t> GetGyroRangeValues() override;
|
||||
|
||||
void GetImuResPacket(const std::uint8_t *data, ImuResPacket *res) override;
|
||||
void GetImuResPacket2(
|
||||
const std::uint8_t *data,
|
||||
ImuResPacket2 *res,
|
||||
bool is_correspondence_on) override;
|
||||
};
|
||||
|
||||
MYNTEYE_END_NAMESPACE
|
||||
|
|
|
@ -52,6 +52,53 @@ struct ImuData {
|
|||
};
|
||||
#pragma pack(pop)
|
||||
|
||||
#pragma pack(push, 1)
|
||||
|
||||
#define BYTE_4(data, begin) (*(data + begin) << 24) | \
|
||||
(*(data + begin + 1) << 16) | \
|
||||
(*(data + begin + 2) << 8) | \
|
||||
*(data + begin + 3)
|
||||
struct ImuData2 {
|
||||
std::uint32_t frame_id;
|
||||
std::uint64_t timestamp;
|
||||
std::uint8_t flag;
|
||||
float temperature;
|
||||
float accel_or_gyro[3];
|
||||
float gyro_add[3];
|
||||
|
||||
ImuData2() = default;
|
||||
explicit ImuData2(const std::uint8_t *data) {
|
||||
from_data(data);
|
||||
}
|
||||
|
||||
void from_data(const std::uint8_t *data) {
|
||||
std::uint32_t timestamp_l;
|
||||
std::uint32_t timestamp_h;
|
||||
frame_id = BYTE_4(data, 0);
|
||||
timestamp_h = (*(data + 4) << 24) | (*(data + 5) << 16) |
|
||||
(*(data + 6) << 8) | *(data + 7);
|
||||
timestamp_l = (*(data + 8) << 24) | (*(data + 9) << 16) |
|
||||
(*(data + 10) << 8) | *(data + 11);
|
||||
timestamp = (static_cast<std::uint64_t>(timestamp_h) << 32) | timestamp_l;
|
||||
flag = *(data + 12);
|
||||
temperature = *((float*)(data+ 13)); // NOLINT
|
||||
// LOG(INFO) << "temperature:" << temperature;
|
||||
accel_or_gyro[0] = *((float*)(data + 17)); // NOLINT
|
||||
// LOG(INFO) << "accel_or_gyro[0]:" << accel_or_gyro[0];
|
||||
accel_or_gyro[1] = *((float*)(data + 21)); // NOLINT
|
||||
// LOG(INFO) << "accel_or_gyro[1]:" << accel_or_gyro[1];
|
||||
accel_or_gyro[2] = *((float*)(data + 25)); // NOLINT
|
||||
// LOG(INFO) << "accel_or_gyro[2]:" << accel_or_gyro[2];
|
||||
if (flag == 3) {
|
||||
gyro_add[0] = *((float*)(data + 29)); // NOLINT
|
||||
gyro_add[1] = *((float*)(data + 33)); // NOLINT
|
||||
gyro_add[2] = *((float*)(data + 37)); // NOLINT
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
#pragma pack(pop)
|
||||
|
||||
void unpack_imu_segment(const ImuData &imu, ImuSegment *seg) {
|
||||
seg->frame_id = imu.frame_id;
|
||||
seg->timestamp = imu.timestamp;
|
||||
|
@ -66,6 +113,36 @@ void unpack_imu_segment(const ImuData &imu, ImuSegment *seg) {
|
|||
seg->gyro[2] = (seg->flag == 2) ? imu.accel_or_gyro[2] : 0;
|
||||
}
|
||||
|
||||
void unpack_imu_segment2(const ImuData2 &imu, ImuSegment2 *seg) {
|
||||
seg->frame_id = imu.frame_id;
|
||||
seg->timestamp = imu.timestamp;
|
||||
seg->flag = imu.flag & 0b0011;
|
||||
seg->is_ets = ((imu.flag & 0b0100) == 0b0100);
|
||||
seg->temperature = imu.temperature;
|
||||
if (seg->flag == 1) {
|
||||
seg->accel[0] = imu.accel_or_gyro[0];
|
||||
seg->accel[1] = imu.accel_or_gyro[1];
|
||||
seg->accel[2] = imu.accel_or_gyro[2];
|
||||
seg->gyro[0] = 0.;
|
||||
seg->gyro[1] = 0.;
|
||||
seg->gyro[2] = 0.;
|
||||
} else if (seg->flag == 2) {
|
||||
seg->gyro[0] = imu.accel_or_gyro[0];
|
||||
seg->gyro[1] = imu.accel_or_gyro[1];
|
||||
seg->gyro[2] = imu.accel_or_gyro[2];
|
||||
seg->accel[0] = 0.;
|
||||
seg->accel[1] = 0.;
|
||||
seg->accel[2] = 0.;
|
||||
} else if (seg->flag == 3) {
|
||||
seg->gyro[0] = imu.accel_or_gyro[0];
|
||||
seg->gyro[1] = imu.accel_or_gyro[1];
|
||||
seg->gyro[2] = imu.accel_or_gyro[2];
|
||||
seg->accel[0] = imu.gyro_add[0];
|
||||
seg->accel[1] = imu.gyro_add[1];
|
||||
seg->accel[2] = imu.gyro_add[2];
|
||||
}
|
||||
}
|
||||
|
||||
void unpack_imu_packet(const std::uint8_t *data, ImuPacket *pkg) {
|
||||
std::size_t data_n = sizeof(ImuData); // 21
|
||||
for (std::size_t i = 0; i < pkg->count; i++) {
|
||||
|
@ -81,6 +158,25 @@ void unpack_imu_packet(const std::uint8_t *data, ImuPacket *pkg) {
|
|||
}
|
||||
}
|
||||
|
||||
void unpack_imu_packet2(
|
||||
const std::uint8_t *data, ImuPacket2 *pkg, bool is_correspondence_on) {
|
||||
std::size_t data_n = 29;
|
||||
if (is_correspondence_on) {
|
||||
data_n = 41;
|
||||
}
|
||||
for (std::size_t i = 0; i < pkg->count; i++) {
|
||||
ImuSegment2 seg;
|
||||
unpack_imu_segment2(ImuData2(data + data_n * i), &seg);
|
||||
pkg->segments.push_back(seg);
|
||||
}
|
||||
if (pkg->count) {
|
||||
pkg->serial_number = pkg->segments.back().frame_id;
|
||||
} else {
|
||||
LOG(ERROR) << "The imu data pipeline lost more than 5 samples continuously, "
|
||||
<< "please check the device and firmware";
|
||||
}
|
||||
}
|
||||
|
||||
void unpack_imu_res_packet(const std::uint8_t *data, ImuResPacket *res) {
|
||||
res->header = *data;
|
||||
res->state = *(data + 1);
|
||||
|
@ -94,6 +190,24 @@ void unpack_imu_res_packet(const std::uint8_t *data, ImuResPacket *res) {
|
|||
res->checksum = *(data + 4 + res->size);
|
||||
}
|
||||
|
||||
void unpack_imu_res_packet2(
|
||||
const std::uint8_t *data, ImuResPacket2 *res, bool is_correspondence_on) {
|
||||
res->header = *data;
|
||||
// u_int64_t* jj = (u_int64_t*) data;
|
||||
res->state = *(data + 1);
|
||||
res->size = (*(data + 2) << 8) | *(data + 3);
|
||||
std::size_t data_n = 29;
|
||||
if (is_correspondence_on) {
|
||||
data_n = 41;
|
||||
}
|
||||
// LOG(INFO) << "size:" << data_n;
|
||||
ImuPacket2 packet;
|
||||
packet.count = res->size / data_n;
|
||||
unpack_imu_packet2(data + 4, &packet, is_correspondence_on);
|
||||
res->packets.push_back(packet);
|
||||
res->checksum = *(data + 4 + res->size);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
Standard2ChannelsAdapter::Standard2ChannelsAdapter(const Model &model)
|
||||
|
@ -124,4 +238,9 @@ void Standard2ChannelsAdapter::GetImuResPacket(
|
|||
unpack_imu_res_packet(data, res);
|
||||
}
|
||||
|
||||
void Standard2ChannelsAdapter::GetImuResPacket2(
|
||||
const std::uint8_t *data, ImuResPacket2 *res, bool is_correspondence_on) {
|
||||
unpack_imu_res_packet2(data, res, is_correspondence_on);
|
||||
}
|
||||
|
||||
MYNTEYE_END_NAMESPACE
|
||||
|
|
|
@ -35,6 +35,10 @@ class Standard2ChannelsAdapter : public ChannelsAdapter {
|
|||
std::vector<std::int32_t> GetGyroRangeValues() override;
|
||||
|
||||
void GetImuResPacket(const std::uint8_t *data, ImuResPacket *res) override;
|
||||
void GetImuResPacket2(
|
||||
const std::uint8_t *data,
|
||||
ImuResPacket2 *res,
|
||||
bool is_correspondence_on) override;
|
||||
};
|
||||
|
||||
MYNTEYE_END_NAMESPACE
|
||||
|
|
Loading…
Reference in New Issue
Block a user