fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-20 17:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

mocap_support: added support for mocap data on firmware

Browse Source
This commit is contained in:
TSC21
2015-06-13 17:03:31 +01:00
parent c92655b850
commit dccd4df7bc
13 changed files with 225 additions and 90 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+25 -1
View File
@@ -63,6 +63,7 @@
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vision_position_estimate.h>
#include <uORB/topics/att_pos_mocap.h>
#include <drivers/drv_hrt.h>
#include <lib/mathlib/mathlib.h>
@@ -261,6 +262,9 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
/* subscribe to vision estimate */
int vision_sub = orb_subscribe(ORB_ID(vision_position_estimate));
/* subscribe to mocap data */
int mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap));
/* advertise attitude */
orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
@@ -291,6 +295,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
R_decl.identity();
struct vision_position_estimate_s vision {};
struct att_pos_mocap_s mocap {};
/* register the perf counter */
perf_counter_t ekf_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_ekf");
@@ -445,11 +450,30 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
bool vision_updated = false;
orb_check(vision_sub, &vision_updated);
bool mocap_updated = false;
orb_check(mocap_sub, &mocap_updated);
if (vision_updated) {
orb_copy(ORB_ID(vision_position_estimate), vision_sub, &vision);
}
if (vision.timestamp_boot > 0 && (hrt_elapsed_time(&vision.timestamp_boot) < 500000)) {
if (mocap_updated) {
orb_copy(ORB_ID(att_pos_mocap), mocap_sub, &mocap);
}
if (mocap.timestamp_boot > 0 && (hrt_elapsed_time(&mocap.timestamp_boot) < 500000)) {
math::Quaternion q(mocap.q);
math::Matrix<3, 3> Rmoc = q.to_dcm();
math::Vector<3> v(1.0f, 0.0f, 0.4f);
math::Vector<3> vn = Rmoc.transposed() * v; //Rmoc is Rwr (robot respect to world) while v is respect to world. Hence Rmoc 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);
}else if (vision.timestamp_boot > 0 && (hrt_elapsed_time(&vision.timestamp_boot) < 500000)) {
math::Quaternion q(vision.q);
math::Matrix<3, 3> Rvis = q.to_dcm();