diff --git a/src/mynteye/device/channel/channels.cc b/src/mynteye/device/channel/channels.cc index 34af9fc..5c0bd5c 100644 --- a/src/mynteye/device/channel/channels.cc +++ b/src/mynteye/device/channel/channels.cc @@ -30,6 +30,30 @@ MYNTEYE_BEGIN_NAMESPACE +mynteye::ImuPacket2 to_pak2(const mynteye::ImuPacket& pak1) { + 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]; + tpr.accel[1] = pak1.segments[i].accel[1]; + tpr.accel[2] = pak1.segments[i].accel[2]; + tpr.gyro[0] = pak1.segments[i].gyro[0]; + tpr.gyro[1] = pak1.segments[i].gyro[1]; + tpr.gyro[2] = pak1.segments[i].gyro[2]; + 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; + tpr.timestamp = pak1.segments[i].timestamp; + res.segments.push_back(tpr); + } + return res; +} + + namespace { const uvc::xu mynteye_xu = {3, 2, @@ -328,7 +352,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 +403,7 @@ void Channels::SetImuCallback(imu_callback_t callback) { } void Channels::DoImuTrack() { - if (IsImuProc2()) { + if (IsImuProtocol2()) { return DoImuTrack2(); } else { return DoImuTrack1(); @@ -424,7 +448,7 @@ void Channels::DoImuTrack1() { if (imu_callback_) { for (auto &&packet : res_packet.packets) { - imu_callback_(packet); + imu_callback_(to_pak2(packet)); } } @@ -432,49 +456,48 @@ 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; + LOG(INFO) << 1; + if (!XuImuWrite(req_packet)) { + return; + } +LOG(INFO) << 2; + if (!XuImuRead(&res_packet)) { + return; + } +LOG(INFO) << 3; + if (res_packet.packets.size() == 0) { + return; + } +LOG(INFO) << 4; + if (res_packet.packets.back().count == 0) { + return; + } +LOG(INFO) << 5; + 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; + }(); +LOG(INFO) << 6; + auto &&sn = res_packet.packets.back().serial_number; + if (imu_sn_ == sn) { + VLOG(2) << "New imu not ready, dropped"; + return; + } + imu_sn_ = sn; +LOG(INFO) << 7; + 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 +522,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 +824,7 @@ bool Channels::XuHalfDuplexSet(Option option, std::uint64_t value) const { static_cast((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 +836,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,6 +847,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); + 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(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; + } + // LOG(INFO) << res->size; + if (enable_imu_correspondence) { + LOG(INFO) << "enable_imu_correspondence"; + } else { + } + 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 diff --git a/src/mynteye/device/channel/channels.h b/src/mynteye/device/channel/channels.h index 20c6e38..7e623fd 100644 --- a/src/mynteye/device/channel/channels.h +++ b/src/mynteye/device/channel/channels.h @@ -54,7 +54,7 @@ class MYNTEYE_API Channels { XU_CMD_LAST } xu_cmd_t; - using imu_callback_t = std::function; + using imu_callback_t = std::function; 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,6 +139,7 @@ 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_; @@ -157,6 +163,8 @@ class ChannelsAdapter { virtual std::vector GetGyroRangeValues() = 0; virtual void GetImuResPacket(const std::uint8_t *data, ImuResPacket *res) = 0; + virtual void GetImuResPacket2(const std::uint8_t *data, + ImuResPacket2 *res) = 0; protected: Model model_; diff --git a/src/mynteye/device/channel/def.h b/src/mynteye/device/channel/def.h index ec8b2b9..ee10bd9 100644 --- a/src/mynteye/device/channel/def.h +++ b/src/mynteye/device/channel/def.h @@ -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 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; + std::int32_t temperature; + std::int32_t accel[3]; + std::int32_t gyro[3]; +}; +#pragma pack(pop) + /** * @ingroup datatypes * Imu packet. @@ -92,6 +137,18 @@ struct ImuPacket { std::vector 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 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 packets; + std::uint8_t checksum; +}; +#pragma pack(pop) + MYNTEYE_END_NAMESPACE #endif // MYNTEYE_DEVICE_CHANNEL_DEF_H_ diff --git a/src/mynteye/device/motions.cc b/src/mynteye/device/motions.cc index 38febaa..e48d6fc 100644 --- a/src/mynteye/device/motions.cc +++ b/src/mynteye/device/motions.cc @@ -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; } diff --git a/src/mynteye/device/standard/channels_adapter_s.cc b/src/mynteye/device/standard/channels_adapter_s.cc index 118ac29..b45fa2e 100644 --- a/src/mynteye/device/standard/channels_adapter_s.cc +++ b/src/mynteye/device/standard/channels_adapter_s.cc @@ -122,4 +122,9 @@ void StandardChannelsAdapter::GetImuResPacket( unpack_imu_res_packet(data, res); } +void StandardChannelsAdapter::GetImuResPacket2( + const std::uint8_t *data, ImuResPacket2 *res) { + LOG(WARNING) << "s1 device can't use ImuResPacket2.0 check the firmware."; +} + MYNTEYE_END_NAMESPACE diff --git a/src/mynteye/device/standard/channels_adapter_s.h b/src/mynteye/device/standard/channels_adapter_s.h index bc3fcbd..c0c9fd1 100644 --- a/src/mynteye/device/standard/channels_adapter_s.h +++ b/src/mynteye/device/standard/channels_adapter_s.h @@ -35,6 +35,7 @@ class StandardChannelsAdapter : public ChannelsAdapter { std::vector GetGyroRangeValues() override; void GetImuResPacket(const std::uint8_t *data, ImuResPacket *res) override; + void GetImuResPacket2(const std::uint8_t *data, ImuResPacket2 *res) override; }; MYNTEYE_END_NAMESPACE diff --git a/src/mynteye/device/standard2/channels_adapter_s2.cc b/src/mynteye/device/standard2/channels_adapter_s2.cc index 948e17e..edf1829 100644 --- a/src/mynteye/device/standard2/channels_adapter_s2.cc +++ b/src/mynteye/device/standard2/channels_adapter_s2.cc @@ -52,6 +52,50 @@ struct ImuData { }; #pragma pack(pop) +#pragma pack(push, 1) + +#define BYTE_8(data, begin) (static_cast( \ + ((*(data + begin) << 24) | (*(data + begin+1) << 16) | \ + (*(data + begin+2) << 8) | *(data + begin+3)) << 32) | \ + ((*(data + begin+4) << 24) | (*(data + begin+5) << 16) | \ + (*(data + begin+6) << 8) | *(data + begin+7))) +#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]; + + 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 = (*(data) << 24) | (*(data + 1) << 16) | (*(data + 2) << 8) | + *(data + 3); + // 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 = BYTE_8(data, 4); + flag = *(data + 12); + temperature = BYTE_4(data, 13); + // accel_or_gyro[0] = BYTE_4(data, 17); + // accel_or_gyro[1] = BYTE_4(data, 21); + // accel_or_gyro[2] = BYTE_4(data, 25); + } +}; + +#pragma pack(pop) + void unpack_imu_segment(const ImuData &imu, ImuSegment *seg) { seg->frame_id = imu.frame_id; seg->timestamp = imu.timestamp; @@ -66,6 +110,21 @@ 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) { + LOG(INFO) << "unpack_imu_segment2" << imu.timestamp; + 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; + seg->accel[0] = (seg->flag == 1) ? imu.accel_or_gyro[0] : 0; + seg->accel[1] = (seg->flag == 1) ? imu.accel_or_gyro[1] : 0; + seg->accel[2] = (seg->flag == 1) ? imu.accel_or_gyro[2] : 0; + seg->gyro[0] = (seg->flag == 2) ? imu.accel_or_gyro[0] : 0; + seg->gyro[1] = (seg->flag == 2) ? imu.accel_or_gyro[1] : 0; + seg->gyro[2] = (seg->flag == 2) ? imu.accel_or_gyro[2] : 0; +} + 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 +140,21 @@ void unpack_imu_packet(const std::uint8_t *data, ImuPacket *pkg) { } } +void unpack_imu_packet2(const std::uint8_t *data, ImuPacket2 *pkg) { + std::size_t data_n = sizeof(ImuData2); // 29 + 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 +168,21 @@ 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) { + 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 = sizeof(ImuData2); // 21 + // LOG(INFO) << "size:" << data_n; + ImuPacket2 packet; + packet.count = res->size / data_n; + LOG(INFO) << "count:" << (int)(packet.count); + unpack_imu_packet2(data + 4, &packet); + res->packets.push_back(packet); + res->checksum = *(data + 4 + res->size); +} + } // namespace Standard2ChannelsAdapter::Standard2ChannelsAdapter(const Model &model) @@ -124,4 +213,9 @@ void Standard2ChannelsAdapter::GetImuResPacket( unpack_imu_res_packet(data, res); } +void Standard2ChannelsAdapter::GetImuResPacket2( + const std::uint8_t *data, ImuResPacket2 *res) { + unpack_imu_res_packet2(data, res); +} + MYNTEYE_END_NAMESPACE diff --git a/src/mynteye/device/standard2/channels_adapter_s2.h b/src/mynteye/device/standard2/channels_adapter_s2.h index 7301be0..64510b1 100644 --- a/src/mynteye/device/standard2/channels_adapter_s2.h +++ b/src/mynteye/device/standard2/channels_adapter_s2.h @@ -35,6 +35,7 @@ class Standard2ChannelsAdapter : public ChannelsAdapter { std::vector GetGyroRangeValues() override; void GetImuResPacket(const std::uint8_t *data, ImuResPacket *res) override; + void GetImuResPacket2(const std::uint8_t *data, ImuResPacket2 *res) override; }; MYNTEYE_END_NAMESPACE