From c5ce8f39ae6a25655fc7f85a0639458bb170b312 Mon Sep 17 00:00:00 2001 From: andrea-nisti Date: Fri, 24 Apr 2015 15:51:52 +0200 Subject: [PATCH 1/3] Order fixed for vision position quaternion changed from [x y z w] to [w x y z]. In this way the notation is consistent and flightplot shows the real values. --- src/modules/sdlog2/sdlog2.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 8193cf1814..e2274342e6 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1648,10 +1648,10 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_VISN.vx = buf.vision_pos.vx; log_msg.body.log_VISN.vy = buf.vision_pos.vy; log_msg.body.log_VISN.vz = buf.vision_pos.vz; - log_msg.body.log_VISN.qx = buf.vision_pos.q[0]; - log_msg.body.log_VISN.qy = buf.vision_pos.q[1]; - log_msg.body.log_VISN.qz = buf.vision_pos.q[2]; - log_msg.body.log_VISN.qw = buf.vision_pos.q[3]; + log_msg.body.log_VISN.qw = buf.vision_pos.q[0]; // vision_position_estimate uses [w,x,y,z] convention + log_msg.body.log_VISN.qx = buf.vision_pos.q[1]; + log_msg.body.log_VISN.qy = buf.vision_pos.q[2]; + log_msg.body.log_VISN.qz = buf.vision_pos.q[3]; LOGBUFFER_WRITE_AND_COUNT(VISN); } From 1f5775615ffb04f22456795ea39cb1402ae51d5f Mon Sep 17 00:00:00 2001 From: Andrea Nistico Date: Fri, 24 Apr 2015 16:45:17 +0200 Subject: [PATCH 2/3] Rvis transposed, in this way we have consisency --- .../attitude_estimator_ekf/attitude_estimator_ekf_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index b2a3572a7d..68780d5dd7 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -465,8 +465,8 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) math::Vector<3> v(1.0f, 0.0f, 0.4f); - math::Vector<3> vn = Rvis * v; - + math::Vector<3> vn = Rvis.transposed() * v; //Rvis is Rwr (robot respect to world) while v is respect to world. Hence Rvis must be transposed having (Rwr)' * Vw + // Rrw * Vw = vn. This way we have consistency z_k[6] = vn(0); z_k[7] = vn(1); z_k[8] = vn(2); From e621b2eb1847aa66c857a16c2046421512fffcc2 Mon Sep 17 00:00:00 2001 From: Andrea Nistico Date: Fri, 24 Apr 2015 17:03:05 +0200 Subject: [PATCH 3/3] code style fix --- .../attitude_estimator_ekf/attitude_estimator_ekf_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 68780d5dd7..3faf63a27f 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -466,7 +466,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) math::Vector<3> v(1.0f, 0.0f, 0.4f); math::Vector<3> vn = Rvis.transposed() * v; //Rvis is Rwr (robot respect to world) while v is respect to world. Hence Rvis must be transposed having (Rwr)' * Vw - // Rrw * Vw = vn. This way we have consistency + // Rrw * Vw = vn. This way we have consistency z_k[6] = vn(0); z_k[7] = vn(1); z_k[8] = vn(2);