tami/MYNT-EYE-S-SDK
6
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fix(src): fixed imu size not matched with old version

Browse Source
This commit is contained in:
Osenberg 2019-03-25 18:18:54 +08:00
parent f25685153b
commit ed42efc5cb
3 changed files with 41 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/mynteye/device/channel/channels.cc
View File

@ -492,13 +492,9 @@ bool Channels::GetFiles(
if (imu_params->ok) {
auto &&n = file_channel_.GetImuParamsFromData(
data + i, file_size, imu_params);
// TODO(Kalman): Use CHECK_EQ
if (n != file_size) {
LOG(WARNING) << "Wrong sizes of imu params";
}
// CHECK_EQ(n, file_size)
// << "The firmware not support getting device info, you could "
// "upgrade to latest";
CHECK_EQ(n, file_size)
<< "The firmware not support getting device info, you could "
"upgrade to latest";
}
} break;
default:

37
src/mynteye/device/channel/file_channel.cc
View File

@ -443,9 +443,9 @@ std::size_t ImuParamsParser::GetFromData(
std::size_t ImuParamsParser::SetToData(
const imu_params_t *imu_params, std::uint8_t *data) const {
if (spec_version_ >= Version(1, 2)) {
return SetToData_new(imu_params, data);
return SetToData_bool_new(imu_params, data, true);
} else {
return SetToData_old(imu_params, data);
return SetToData_bool_new(imu_params, data, false);
}
}
@ -524,4 +524,37 @@ std::size_t ImuParamsParser::SetToData_new(
return size + 3;
}
std::size_t ImuParamsParser::SetToData_bool_new(
const imu_params_t *imu_params, std::uint8_t *data, bool is_set) const {
std::size_t i = 3; // skip id, size
Version version_new(1, 2); // new version
Version version_raw(imu_params->version);
// version, 2
data[i] = version_new.major();
data[i + 1] = version_new.minor();
i += 2;
// set imu params with new version format
if (version_raw <= version_new) {
if (is_set) {
i += bytes::to_data(&imu_params->in_accel, data + i, true);
i += bytes::to_data(&imu_params->in_gyro, data + i, true);
i += bytes::to_data(&imu_params->ex_left_to_imu, data + i);
} else {
i += bytes::to_data(&imu_params->in_accel, data + i, false);
i += bytes::to_data(&imu_params->in_gyro, data + i, false);
i += bytes::to_data(&imu_params->ex_left_to_imu, data + i);
}
} else {
LOG(FATAL) << "Could not set imu params of version "
<< version_raw.to_string() << ", please use latest SDK.";
}
// others
std::size_t size = i - 3;
data[0] = FID_IMU_PARAMS;
data[1] = static_cast<std::uint8_t>((size >> 8) & 0xFF);
data[2] = static_cast<std::uint8_t>(size & 0xFF);
return size + 3;
}
MYNTEYE_END_NAMESPACE

3
src/mynteye/device/channel/file_channel.h
View File

@ -144,6 +144,9 @@ class ImuParamsParser {
std::size_t SetToData_new(
const imu_params_t *imu_params, std::uint8_t *data) const;
std::size_t SetToData_bool_new(
const imu_params_t *imu_params, std::uint8_t *data, bool is_set) const;
private:
Version spec_version_;
};