diff options
| author | Lorenz Meier <lorenz@px4.io> | 2015-04-24 20:56:00 +0200 |
|---|---|---|
| committer | Lorenz Meier <lorenz@px4.io> | 2015-04-24 20:56:00 +0200 |
| commit | 9c56aa386b76da15f7bdc55f16055f4894405bee (patch) | |
| tree | beafddec08bdfb603c12335769cab411d69eced9 | |
| parent | 3f961bf3c60c3ea380dc1dc62684a026667ec0e9 (diff) | |
| parent | e621b2eb1847aa66c857a16c2046421512fffcc2 (diff) | |
| download | px4-firmware-9c56aa386b76da15f7bdc55f16055f4894405bee.tar.gz px4-firmware-9c56aa386b76da15f7bdc55f16055f4894405bee.tar.bz2 px4-firmware-9c56aa386b76da15f7bdc55f16055f4894405bee.zip | |
Merge pull request #2078 from andrea-nisti/master
Integration with optitrack mocap system. Correction in att_estimator_ekf and sdlog2 modules.
| -rwxr-xr-x | src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp | 4 | ||||
| -rw-r--r-- | src/modules/sdlog2/sdlog2.c | 8 |
2 files changed, 6 insertions, 6 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 b2a3572a7..3faf63a27 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); diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 8193cf181..e2274342e 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); } |