fix tool bug

This commit is contained in:
KalmanSLightech 2018-07-21 17:09:47 +08:00
parent 259dc968aa
commit 01c3c71516
3 changed files with 19 additions and 9 deletions

View File

@ -298,6 +298,10 @@ void Channels::DoImuTrack() {
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) {

View File

@ -51,15 +51,22 @@ void Motions::SetMotionCallback(motion_callback_t callback) {
imu->flag = seg.flag;
imu->temperature = seg.temperature / 326.8f + 25;
if((imu->flag) & 0x01 != 0) {
if(imu->flag == 1) {
imu->accel[0] = seg.aceel_or_gyro[0] * 8.f / 0x10000;
imu->accel[1] = seg.aceel_or_gyro[1] * 8.f / 0x10000;
imu->accel[2] = seg.aceel_or_gyro[2] * 8.f / 0x10000;
}
if((imu->flag) & 0x02 != 0) {
imu->gyro[0] = 0;
imu->gyro[1] = 0;
imu->gyro[2] = 0;
} else if(imu->flag == 2) {
imu->accel[0] = 0;
imu->accel[1] = 0;
imu->accel[2] = 0;
imu->gyro[0] = seg.aceel_or_gyro[0] * 1000.f / 0x10000;
imu->gyro[1] = seg.aceel_or_gyro[1] * 1000.f / 0x10000;
imu->gyro[2] = seg.aceel_or_gyro[2] * 1000.f / 0x10000;
} else {
imu->Reset();
}
std::lock_guard<std::mutex> _(mtx_datas_);

View File

@ -79,12 +79,11 @@ void Dataset::SaveStreamData(
void Dataset::SaveMotionData(const device::MotionData &data) {
auto &&writer = GetMotionWriter();
auto seq = motion_count_;
writer->ofs << seq << ", " << data.imu->frame_id << ", "
<< data.imu->timestamp << ", " << data.imu->accel[0] << ", "
<< data.imu->accel[1] << ", " << data.imu->accel[2] << ", "
<< data.imu->gyro[0] << ", " << data.imu->gyro[1] << ", "
<< data.imu->gyro[2] << ", " << data.imu->temperature
<< std::endl;
writer->ofs << seq << ", " << data.imu->timestamp << ", "
<< data.imu->accel[0] << ", " << data.imu->accel[1] << ", "
<< data.imu->accel[2] << ", " << data.imu->gyro[0] << ", "
<< data.imu->gyro[1] << ", " << data.imu->gyro[2] << ", "
<< data.imu->temperature << std::endl;
++motion_count_;
}