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

ekf2 : move to new vision topic

Browse Source
This commit is contained in:
Kabir Mohammed
2017-02-15 21:48:33 +01:00
committed by Lorenz Meier
parent 8eaddeee0a
commit 7236bafee1
1 changed files with 33 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/ekf2_main.cpp
+33 -36
View File
@@ -69,7 +69,6 @@
#include <uORB/topics/airspeed.h> #include <uORB/topics/airspeed.h>
#include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_local_position.h> #include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vision_position_estimate.h>
#include <uORB/topics/control_state.h> #include <uORB/topics/control_state.h>
#include <uORB/topics/vehicle_global_position.h> #include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/wind_estimate.h> #include <uORB/topics/wind_estimate.h>
@@ -407,7 +406,8 @@ void Ekf2::task_main()
int params_sub = orb_subscribe(ORB_ID(parameter_update)); int params_sub = orb_subscribe(ORB_ID(parameter_update));
int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow)); int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));
int range_finder_sub = orb_subscribe(ORB_ID(distance_sensor)); int range_finder_sub = orb_subscribe(ORB_ID(distance_sensor));
int ev_pos_sub = orb_subscribe(ORB_ID(vision_position_estimate)); int ev_pos_sub = orb_subscribe(ORB_ID(vehicle_vision_position));
int ev_att_sub = orb_subscribe(ORB_ID(vehicle_vision_attitude));
int vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); int vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
int status_sub = orb_subscribe(ORB_ID(vehicle_status)); int status_sub = orb_subscribe(ORB_ID(vehicle_status));
@@ -429,7 +429,8 @@ void Ekf2::task_main()
optical_flow_s optical_flow = {}; optical_flow_s optical_flow = {};
distance_sensor_s range_finder = {}; distance_sensor_s range_finder = {};
vehicle_land_detected_s vehicle_land_detected = {}; vehicle_land_detected_s vehicle_land_detected = {};
vision_position_estimate_s ev = {}; vehicle_local_position_s ev_pos = {};
vehicle_attitude_s ev_att = {};
vehicle_status_s vehicle_status = {}; vehicle_status_s vehicle_status = {};
while (!_task_should_exit) { while (!_task_should_exit) {
@@ -465,6 +466,7 @@ void Ekf2::task_main()
bool range_finder_updated = false; bool range_finder_updated = false;
bool vehicle_land_detected_updated = false; bool vehicle_land_detected_updated = false;
bool vision_position_updated = false; bool vision_position_updated = false;
bool vision_attitude_updated = false;
bool vehicle_status_updated = false; bool vehicle_status_updated = false;
orb_copy(ORB_ID(sensor_combined), sensors_sub, &sensors); orb_copy(ORB_ID(sensor_combined), sensors_sub, &sensors);
@@ -508,7 +510,13 @@ void Ekf2::task_main()
orb_check(ev_pos_sub, &vision_position_updated); orb_check(ev_pos_sub, &vision_position_updated);
if (vision_position_updated) { if (vision_position_updated) {
orb_copy(ORB_ID(vision_position_estimate), ev_pos_sub, &ev); orb_copy(ORB_ID(vehicle_vision_position), ev_pos_sub, &ev_pos);
}
orb_check(ev_att_sub, &vision_attitude_updated);
if (vision_attitude_updated) {
orb_copy(ORB_ID(vehicle_vision_attitude), ev_att_sub, &ev_att);
} }
// in replay mode we are getting the actual timestamp from the sensor topic // in replay mode we are getting the actual timestamp from the sensor topic
@@ -653,32 +661,20 @@ void Ekf2::task_main()
// get external vision data // get external vision data
// if error estimates are unavailable, use parameter defined defaults // if error estimates are unavailable, use parameter defined defaults
if (vision_position_updated) { if (vision_position_updated || vision_attitude_updated) {
ext_vision_message ev_data; ext_vision_message ev_data;
ev_data.posNED(0) = ev.x; ev_data.posNED(0) = ev_pos.x;
ev_data.posNED(1) = ev.y; ev_data.posNED(1) = ev_pos.y;
ev_data.posNED(2) = ev.z; ev_data.posNED(2) = ev_pos.z;
Quaternion q(ev.q); Quaternion q(ev_att.q);
ev_data.quat = q; ev_data.quat = q;
// position measurement error // position measurement error from parameters. TODO : use covariances from topic
if (ev.pos_err >= 0.001f) { ev_data.posErr = _default_ev_pos_noise;
ev_data.posErr = ev.pos_err; ev_data.angErr = _default_ev_ang_noise;
} else {
ev_data.posErr = _default_ev_pos_noise;
}
// angle measurement error
if (ev.ang_err >= 0.001f) {
ev_data.angErr = ev.ang_err;
} else {
ev_data.angErr = _default_ev_ang_noise;
}
// use timestamp from external computer, clocks are synchronized when using MAVROS // use timestamp from external computer, clocks are synchronized when using MAVROS
_ekf.setExtVisionData(ev.timestamp, &ev_data); _ekf.setExtVisionData(vision_position_updated ? ev_pos.timestamp : ev_att.timestamp, &ev_data);
} }
orb_check(vehicle_land_detected_sub, &vehicle_land_detected_updated); orb_check(vehicle_land_detected_sub, &vehicle_land_detected_updated);
@@ -1086,17 +1082,18 @@ void Ekf2::task_main()
replay.asp_timestamp = 0; replay.asp_timestamp = 0;
} }
if (vision_position_updated) { if (vision_position_updated || vision_attitude_updated) {
replay.ev_timestamp = ev.timestamp; replay.ev_timestamp = vision_position_updated ? ev_pos.timestamp : ev_att.timestamp;
replay.pos_ev[0] = ev.x; replay.pos_ev[0] = ev_pos.x;
replay.pos_ev[1] = ev.y; replay.pos_ev[1] = ev_pos.y;
replay.pos_ev[2] = ev.z; replay.pos_ev[2] = ev_pos.z;
replay.quat_ev[0] = ev.q[0]; replay.quat_ev[0] = ev_att.q[0];
replay.quat_ev[1] = ev.q[1]; replay.quat_ev[1] = ev_att.q[1];
replay.quat_ev[2] = ev.q[2]; replay.quat_ev[2] = ev_att.q[2];
replay.quat_ev[3] = ev.q[3]; replay.quat_ev[3] = ev_att.q[3];
replay.pos_err = ev.pos_err; // TODO : switch to covariances from topic later
replay.ang_err = ev.ang_err; replay.pos_err = _default_ev_pos_noise;
replay.ang_err = _default_ev_ang_noise;
} else { } else {
replay.ev_timestamp = 0; replay.ev_timestamp = 0;