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

Obey vision estimates in attitude estimator if available

Browse Source
This commit is contained in:
Lorenz Meier
2014-08-22 09:24:37 +02:00
parent d6219ac1e0
commit fcebafba77
1 changed files with 36 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+36 -16
View File
@@ -62,6 +62,7 @@
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vision_position_estimate.h>
#include <drivers/drv_hrt.h>
#include <lib/mathlib/mathlib.h>
@@ -206,10 +207,6 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
0, 0, 1.f
}; /**< init: identity matrix */
// print text
printf("Extended Kalman Filter Attitude Estimator initialized..\n\n");
fflush(stdout);
int overloadcounter = 19;
/* Initialize filter */
@@ -222,6 +219,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
memset(&raw, 0, sizeof(raw));
struct vehicle_gps_position_s gps;
memset(&gps, 0, sizeof(gps));
gps.eph = 100000;
gps.epv = 100000;
struct vehicle_global_position_s global_pos;
memset(&global_pos, 0, sizeof(global_pos));
struct vehicle_attitude_s att;
@@ -260,9 +259,12 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
/* subscribe to param changes */
int sub_params = orb_subscribe(ORB_ID(parameter_update));
/* subscribe to control mode*/
/* subscribe to control mode */
int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode));
/* subscribe to vision estimate */
int vision_sub = orb_subscribe(ORB_ID(vision_position_estimate));
/* advertise attitude */
orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
@@ -270,16 +272,12 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
thread_running = true;
/* advertise debug value */
// struct debug_key_value_s dbg = { .key = "", .value = 0.0f };
// orb_advert_t pub_dbg = -1;
/* keep track of sensor updates */
uint64_t sensor_last_timestamp[3] = {0, 0, 0};
struct attitude_estimator_ekf_params ekf_params;
struct attitude_estimator_ekf_params ekf_params {};
struct attitude_estimator_ekf_param_handles ekf_param_handles;
struct attitude_estimator_ekf_param_handles ekf_param_handles {};
/* initialize parameter handles */
parameters_init(&ekf_param_handles);
@@ -295,6 +293,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
math::Matrix<3, 3> R_decl;
R_decl.identity();
struct vision_position_estimate vision {};
/* register the perf counter */
perf_counter_t ekf_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_ekf");
@@ -315,8 +315,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode);
if (!control_mode.flag_system_hil_enabled) {
fprintf(stderr,
"[att ekf] WARNING: Not getting sensors - sensor app running?\n");
warnx("WARNING: Not getting sensors - sensor app running?");
}
} else {
@@ -451,9 +450,30 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
sensor_last_timestamp[2] = raw.magnetometer_timestamp;
}
z_k[6] = raw.magnetometer_ga[0];
z_k[7] = raw.magnetometer_ga[1];
z_k[8] = raw.magnetometer_ga[2];
bool vision_updated = false;
orb_check(vision_sub, &vision_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) && gps.eph > 3.0f) {
math::Quaternion q(vision.q);
math::Matrix<3, 3> R = q.to_dcm();
math::Vector<3> v(1.0f, 0.0f, 0.4f);
math::Vector<3> vn = R * v;
z_k[6] = vn(0);
z_k[7] = vn(1);
z_k[8] = vn(2);
} else {
z_k[6] = raw.magnetometer_ga[0];
z_k[7] = raw.magnetometer_ga[1];
z_k[8] = raw.magnetometer_ga[2];
}
uint64_t now = hrt_absolute_time();
unsigned int time_elapsed = now - last_run;