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

[fix]: update the imu assembly

Browse Source
This commit is contained in:
Jacob.lsx 2021-02-07 02:56:32 +08:00
parent c3e58cd5b2
commit 2cb4fe3f05
1 changed files with 11 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
src/mynteye/device/motions.cc
View File

@ -207,21 +207,21 @@ void Motions::ProcImuAssembly(std::shared_ptr<ImuData> data) const {
// use imu_tk to calibration the imu // use imu_tk to calibration the imu
// the param assembly is the Misalignment matrix: T // the param assembly is the Misalignment matrix: T
// the param scale is the scale matrix: K // the param scale is the scale matrix: K
// the param bias is the bias vector: B // the param drift is the drift vector: B
// X' = T*K*(X - B) // X' = T*K*(X - B)
static bool init = false; static bool init = false;
static double TxK_accel[3][3] = {0}; static double TxK_accel[3][3] = {0};
static double TxK_gyro[3][3] = {0}; static double TxK_gyro[3][3] = {0};
static double accel_bias[3] = {0}; static double accel_drift[3] = {0};
static double gyro_bias[3] = {0}; static double gyro_drift[3] = {0};
if (!init) { if (!init) {
matrix_3x3(motion_intrinsics_->accel.assembly, matrix_3x3(motion_intrinsics_->accel.assembly,
motion_intrinsics_->accel.scale, TxK_accel); motion_intrinsics_->accel.scale, TxK_accel);
matrix_3x3(motion_intrinsics_->gyro.assembly, matrix_3x3(motion_intrinsics_->gyro.assembly,
motion_intrinsics_->gyro.scale, TxK_gyro); motion_intrinsics_->gyro.scale, TxK_gyro);
for (int i = 0; i < 3; ++i) { for (int i = 0; i < 3; ++i) {
accel_bias[i] = MPSS2G(motion_intrinsics_->accel.bias[i]); accel_drift[i] = MPSS2G(motion_intrinsics_->accel.drift[i]);
gyro_bias[i] = RAD2DEG(motion_intrinsics_->gyro.bias[i]); gyro_drift[i] = RAD2DEG(motion_intrinsics_->gyro.drift[i]);
} }
// LOG(INFO) << "-------------- accel T*K --------------" << std::endl; // LOG(INFO) << "-------------- accel T*K --------------" << std::endl;
@ -231,9 +231,9 @@ void Motions::ProcImuAssembly(std::shared_ptr<ImuData> data) const {
// for (int i = 0; i < 3; ++i) // for (int i = 0; i < 3; ++i)
// LOG(INFO) << TxK_gyro[i][0] << ", " << TxK_gyro[i][1] << ", " << TxK_gyro[i][2]; // LOG(INFO) << TxK_gyro[i][0] << ", " << TxK_gyro[i][1] << ", " << TxK_gyro[i][2];
// LOG(INFO) << "-------------- accel bias --------------" << std::endl; // LOG(INFO) << "-------------- accel bias --------------" << std::endl;
// LOG(INFO) << accel_bias[0] << ", " << accel_bias[1] << ", " << accel_bias[2]; // LOG(INFO) << accel_drift[0] << ", " << accel_drift[1] << ", " << accel_drift[2];
// LOG(INFO) << "-------------- gyro bias --------------" << std::endl; // LOG(INFO) << "-------------- gyro bias --------------" << std::endl;
// LOG(INFO) << gyro_bias[0] << ", " << gyro_bias[1] << ", " << gyro_bias[2]; // LOG(INFO) << gyro_drift[0] << ", " << gyro_drift[1] << ", " << gyro_drift[2];
init = true; init = true;
} }
@ -242,14 +242,14 @@ void Motions::ProcImuAssembly(std::shared_ptr<ImuData> data) const {
double s[3][1] = {0}; double s[3][1] = {0};
double d[3][1] = {0}; double d[3][1] = {0};
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
s[i][0] = data->accel[i] - accel_bias[i]; s[i][0] = data->accel[i] - accel_drift[i];
} }
matrix_3x1(TxK_accel, s, d); matrix_3x1(TxK_accel, s, d);
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
data->accel[i] = d[i][0]; data->accel[i] = d[i][0];
} }
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
s[i][0] = data->gyro[i] - gyro_bias[i]; s[i][0] = data->gyro[i] - gyro_drift[i];
} }
matrix_3x1(TxK_gyro, s, d); matrix_3x1(TxK_gyro, s, d);
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
@ -259,7 +259,7 @@ void Motions::ProcImuAssembly(std::shared_ptr<ImuData> data) const {
double s[3][1] = {0}; double s[3][1] = {0};
double d[3][1] = {0}; double d[3][1] = {0};
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
s[i][0] = data->accel[i] - accel_bias[i]; s[i][0] = data->accel[i] - accel_drift[i];
} }
matrix_3x1(TxK_accel, s, d); matrix_3x1(TxK_accel, s, d);
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
@ -269,7 +269,7 @@ void Motions::ProcImuAssembly(std::shared_ptr<ImuData> data) const {
double s[3][1] = {0}; double s[3][1] = {0};
double d[3][1] = {0}; double d[3][1] = {0};
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
s[i][0] = data->gyro[i] - gyro_bias[i]; s[i][0] = data->gyro[i] - gyro_drift[i];
} }
matrix_3x1(TxK_gyro, s, d); matrix_3x1(TxK_gyro, s, d);
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {