122 lines
3.8 KiB
C++
122 lines
3.8 KiB
C++
// Copyright 2018 Slightech Co., Ltd. All rights reserved.
|
|
//
|
|
// Licensed under the Apache License, Version 2.0 (the "License");
|
|
// you may not use this file except in compliance with the License.
|
|
// You may obtain a copy of the License at
|
|
//
|
|
// http://www.apache.org/licenses/LICENSE-2.0
|
|
//
|
|
// Unless required by applicable law or agreed to in writing, software
|
|
// distributed under the License is distributed on an "AS IS" BASIS,
|
|
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
// See the License for the specific language governing permissions and
|
|
// limitations under the License.
|
|
#include "mynteye/device/standard2/channels_adapter_s2.h"
|
|
|
|
#include "mynteye/logger.h"
|
|
|
|
MYNTEYE_BEGIN_NAMESPACE
|
|
|
|
namespace {
|
|
|
|
#pragma pack(push, 1)
|
|
struct ImuData {
|
|
std::uint32_t frame_id;
|
|
std::uint64_t timestamp;
|
|
std::uint8_t flag;
|
|
std::int16_t temperature;
|
|
std::int16_t accel_or_gyro[3];
|
|
|
|
ImuData() = default;
|
|
explicit ImuData(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 = (static_cast<std::uint64_t>(timestamp_h) << 32) | timestamp_l;
|
|
flag = *(data + 12);
|
|
temperature = (*(data + 13) << 8) | *(data + 14);
|
|
accel_or_gyro[0] = (*(data + 15) << 8) | *(data + 16);
|
|
accel_or_gyro[1] = (*(data + 17) << 8) | *(data + 18);
|
|
accel_or_gyro[2] = (*(data + 19) << 8) | *(data + 20);
|
|
}
|
|
};
|
|
#pragma pack(pop)
|
|
|
|
void unpack_imu_segment(const ImuData &imu, ImuSegment *seg) {
|
|
seg->frame_id = imu.frame_id;
|
|
seg->timestamp = imu.timestamp;
|
|
seg->flag = imu.flag;
|
|
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++) {
|
|
ImuSegment seg;
|
|
unpack_imu_segment(ImuData(data + data_n * i), &seg);
|
|
pkg->segments.push_back(seg);
|
|
}
|
|
pkg->serial_number = pkg->segments.back().frame_id;
|
|
}
|
|
|
|
void unpack_imu_res_packet(const std::uint8_t *data, ImuResPacket *res) {
|
|
res->header = *data;
|
|
res->state = *(data + 1);
|
|
res->size = (*(data + 2) << 8) | *(data + 3);
|
|
|
|
std::size_t data_n = sizeof(ImuData); // 21
|
|
ImuPacket packet;
|
|
packet.count = res->size / data_n;
|
|
unpack_imu_packet(data + 4, &packet);
|
|
res->packets.push_back(packet);
|
|
res->checksum = *(data + 4 + res->size);
|
|
}
|
|
|
|
} // namespace
|
|
|
|
Standard2ChannelsAdapter::Standard2ChannelsAdapter()
|
|
: ChannelsAdapter(Model::STANDARD2) {
|
|
}
|
|
|
|
Standard2ChannelsAdapter::~Standard2ChannelsAdapter() {
|
|
}
|
|
|
|
std::int32_t Standard2ChannelsAdapter::GetAccelRangeDefault() {
|
|
return 12;
|
|
}
|
|
|
|
std::vector<std::int32_t> Standard2ChannelsAdapter::GetAccelRangeValues() {
|
|
return {6, 12, 24, 48};
|
|
}
|
|
|
|
std::int32_t Standard2ChannelsAdapter::GetGyroRangeDefault() {
|
|
return 1000;
|
|
}
|
|
|
|
std::vector<std::int32_t> Standard2ChannelsAdapter::GetGyroRangeValues() {
|
|
return {250, 500, 1000, 2000, 4000};
|
|
}
|
|
|
|
void Standard2ChannelsAdapter::GetImuResPacket(
|
|
const std::uint8_t *data, ImuResPacket *res) {
|
|
unpack_imu_res_packet(data, res);
|
|
}
|
|
|
|
MYNTEYE_END_NAMESPACE
|