Add imu tracking

This commit is contained in:
John Zhao 2018-04-10 20:20:42 +08:00
parent f818cbe24c
commit be26072877
4 changed files with 151 additions and 14 deletions

View File

@ -64,26 +64,40 @@ int main(int argc, char *argv[]) {
std::size_t left_count = 0;
device->SetStreamCallback(
Stream::LEFT, [&left_count](const device::StreamData &data) {
CHECK_NOTNULL(data.img);
++left_count;
VLOG(2) << Stream::LEFT << ", count: " << left_count;
if (data.img) {
VLOG(2) << " frame_id: " << data.img->frame_id
<< ", timestamp: " << data.img->timestamp
<< ", exposure_time: " << data.img->exposure_time;
}
VLOG(2) << " frame_id: " << data.img->frame_id
<< ", timestamp: " << data.img->timestamp
<< ", exposure_time: " << data.img->exposure_time;
});
std::size_t right_count = 0;
device->SetStreamCallback(
Stream::RIGHT, [&right_count](const device::StreamData &data) {
CHECK_NOTNULL(data.img);
++right_count;
VLOG(2) << Stream::RIGHT << ", count: " << right_count;
if (data.img) {
VLOG(2) << " frame_id: " << data.img->frame_id
<< ", timestamp: " << data.img->timestamp
<< ", exposure_time: " << data.img->exposure_time;
}
VLOG(2) << " frame_id: " << data.img->frame_id
<< ", timestamp: " << data.img->timestamp
<< ", exposure_time: " << data.img->exposure_time;
});
std::size_t imu_count = 0;
device->SetMotionCallback([&imu_count](const device::MotionData &data) {
CHECK_NOTNULL(data.imu);
++imu_count;
LOG(INFO) << "Imu count: " << imu_count;
LOG(INFO) << " frame_id: " << data.imu->frame_id
<< ", timestamp: " << data.imu->timestamp
<< ", accel_x: " << data.imu->accel[0]
<< ", accel_y: " << data.imu->accel[1]
<< ", accel_z: " << data.imu->accel[2]
<< ", gyro_x: " << data.imu->gyro[0]
<< ", gyro_y: " << data.imu->gyro[1]
<< ", gyro_z: " << data.imu->gyro[2]
<< ", temperature: " << data.imu->temperature;
});
device->Start(Source::ALL);
cv::namedWindow("frame");

View File

@ -324,7 +324,27 @@ void Device::StartMotionTracking() {
LOG(WARNING) << "Cannot start motion tracking without first stopping it";
return;
}
// TODO(JohnZhao)
channels_->StartImuTracking([this](const ImuPacket &packet) {
if (!HasMotionCallback())
return;
for (auto &&seg : packet.segments) {
auto &&imu = std::make_shared<ImuData>();
imu->frame_id = seg.frame_id;
if (seg.offset < 0 &&
static_cast<uint32_t>(-seg.offset) > packet.timestamp) {
LOG(WARNING) << "Imu timestamp offset is incorrect";
}
imu->timestamp = packet.timestamp + seg.offset;
imu->accel[0] = seg.accel[0] * 8.f / 0x10000;
imu->accel[1] = seg.accel[1] * 8.f / 0x10000;
imu->accel[2] = seg.accel[2] * 8.f / 0x10000;
imu->gyro[0] = seg.gyro[0] * 1000.f / 0x10000;
imu->gyro[1] = seg.gyro[1] * 1000.f / 0x10000;
imu->gyro[2] = seg.gyro[2] * 1000.f / 0x10000;
imu->temperature = seg.temperature / 326.8f + 25;
motion_callback_({imu});
}
});
motion_tracking_ = true;
}
@ -333,7 +353,7 @@ void Device::StopMotionTracking() {
LOG(WARNING) << "Cannot stop motion tracking without first starting it";
return;
}
// TODO(JohnZhao)
channels_->StopImuTracking();
motion_tracking_ = false;
}

View File

@ -2,6 +2,8 @@
#include <glog/logging.h>
#include <chrono>
#include <iomanip>
#include <stdexcept>
MYNTEYE_BEGIN_NAMESPACE
@ -54,13 +56,19 @@ int XuHalfDuplexId(Option option) {
} // namespace
Channels::Channels(std::shared_ptr<uvc::device> device) : device_(device) {
Channels::Channels(std::shared_ptr<uvc::device> device)
: device_(device),
is_imu_tracking_(false),
imu_track_stop_(false),
imu_sn_(0),
imu_callback_(nullptr) {
VLOG(2) << __func__;
UpdateControlInfos();
}
Channels::~Channels() {
VLOG(2) << __func__;
StopImuTracking();
}
void Channels::LogControlInfos() const {
@ -198,6 +206,59 @@ bool Channels::RunControlAction(const Option &option) const {
}
}
void Channels::SetImuCallback(imu_callback_t callback) {
imu_callback_ = callback;
}
void Channels::StartImuTracking(imu_callback_t callback) {
if (is_imu_tracking_) {
LOG(WARNING) << "start imu tracking failed, is tracking already";
return;
}
if (callback) {
imu_callback_ = callback;
}
is_imu_tracking_ = true;
imu_track_thread_ = std::thread([this]() {
imu_sn_ = 0;
ImuReqPacket req_packet{imu_sn_};
ImuResPacket res_packet;
auto sleep_milli = [](std::intmax_t n) {
std::this_thread::sleep_for(std::chrono::milliseconds(n));
};
while (!imu_track_stop_) {
req_packet.serial_number = imu_sn_;
if (!XuImuWrite(req_packet)) {
sleep_milli(5);
continue;
}
if (!XuImuRead(&res_packet)) {
sleep_milli(5);
continue;
}
imu_sn_ = res_packet.packet.serial_number;
if (imu_callback_) {
imu_callback_(res_packet.packet);
}
}
});
}
void Channels::StopImuTracking() {
if (!is_imu_tracking_) {
return;
}
if (imu_track_thread_.joinable()) {
imu_track_stop_ = true;
imu_track_thread_.join();
imu_track_stop_ = false;
is_imu_tracking_ = false;
}
}
bool Channels::PuControlRange(
Option option, int32_t *min, int32_t *max, int32_t *def) const {
CHECK_NOTNULL(device_);
@ -291,8 +352,36 @@ 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);
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
<< " now";
return false;
}
std::uint8_t checksum = 0;
for (std::size_t i = 4, n = 4 + res->size; i < n; i++) {
checksum = (checksum ^ data[i]);
}
if (checksum != res->checksum) {
LOG(WARNING) << "Imu response packet checksum should be 0x" << std::hex
<< std::uppercase << std::setw(2) << std::setfill('0')
<< static_cast<int>(res->checksum) << ", but 0x"
<< std::setw(2) << std::setfill('0')
<< static_cast<int>(res->checksum) << " now";
return false;
}
VLOG(2) << "XuImuRead response success";
return true;
} else {
LOG(WARNING) << "XuImuRead response failed";

View File

@ -4,6 +4,7 @@
#include <map>
#include <memory>
#include <thread>
#include "mynteye/mynteye.h"
@ -42,6 +43,8 @@ class Channels {
XU_CMD_LAST
} xu_cmd_t;
using imu_callback_t = std::function<void(const ImuPacket &packet)>;
explicit Channels(std::shared_ptr<uvc::device> device);
~Channels();
@ -54,6 +57,10 @@ class Channels {
bool RunControlAction(const Option &option) const;
void SetImuCallback(imu_callback_t callback);
void StartImuTracking(imu_callback_t callback = nullptr);
void StopImuTracking();
private:
bool PuControlRange(
Option option, int32_t *min, int32_t *max, int32_t *def) const;
@ -81,6 +88,13 @@ class Channels {
std::shared_ptr<uvc::device> device_;
std::map<Option, control_info_t> control_infos_;
bool is_imu_tracking_;
std::thread imu_track_thread_;
volatile bool imu_track_stop_;
std::uint32_t imu_sn_;
imu_callback_t imu_callback_;
};
MYNTEYE_END_NAMESPACE