dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge pull request #2078 from andrea-nisti/master 

Integration with optitrack mocap system. Correction in att_estimator_ekf and sdlog2 modules.
This commit is contained in:
Lorenz Meier 2015-04-24 20:56:00 +02:00
parent 3f961bf3c6 e621b2eb18
commit 9c56aa386b
2 changed files with 6 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
View File

@ -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);

8
src/modules/sdlog2/sdlog2.c
View File

@ -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);
}