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

ekf2 : move to new vision topic

Browse Source
This commit is contained in:
Kabir Mohammed 2016-12-16 21:05:38 +05:30 committed by Lorenz Meier
parent 8eaddeee0a
commit 7236bafee1
1 changed files with 33 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

69
src/modules/ekf2/ekf2_main.cpp
View File

@ -69,7 +69,6 @@
#include <uORB/topics/airspeed.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vision_position_estimate.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/vehicle_global_position.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 optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));
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 status_sub = orb_subscribe(ORB_ID(vehicle_status));
@ -429,7 +429,8 @@ void Ekf2::task_main()
optical_flow_s optical_flow = {};
distance_sensor_s range_finder = {};
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 = {};
while (!_task_should_exit) {
@ -465,6 +466,7 @@ void Ekf2::task_main()
bool range_finder_updated = false;
bool vehicle_land_detected_updated = false;
bool vision_position_updated = false;
bool vision_attitude_updated = false;
bool vehicle_status_updated = false;
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);
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
@ -653,32 +661,20 @@ void Ekf2::task_main()
// get external vision data
// 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;
ev_data.posNED(0) = ev.x;
ev_data.posNED(1) = ev.y;
ev_data.posNED(2) = ev.z;
Quaternion q(ev.q);
ev_data.posNED(0) = ev_pos.x;
ev_data.posNED(1) = ev_pos.y;
ev_data.posNED(2) = ev_pos.z;
Quaternion q(ev_att.q);
ev_data.quat = q;
// position measurement error
if (ev.pos_err >= 0.001f) {
ev_data.posErr = ev.pos_err;
} 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;
}
// position measurement error from parameters. TODO : use covariances from topic
ev_data.posErr = _default_ev_pos_noise;
ev_data.angErr = _default_ev_ang_noise;
// 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);
@ -1086,17 +1082,18 @@ void Ekf2::task_main()
replay.asp_timestamp = 0;
}
if (vision_position_updated) {
replay.ev_timestamp = ev.timestamp;
replay.pos_ev[0] = ev.x;
replay.pos_ev[1] = ev.y;
replay.pos_ev[2] = ev.z;
replay.quat_ev[0] = ev.q[0];
replay.quat_ev[1] = ev.q[1];
replay.quat_ev[2] = ev.q[2];
replay.quat_ev[3] = ev.q[3];
replay.pos_err = ev.pos_err;
replay.ang_err = ev.ang_err;
if (vision_position_updated || vision_attitude_updated) {
replay.ev_timestamp = vision_position_updated ? ev_pos.timestamp : ev_att.timestamp;
replay.pos_ev[0] = ev_pos.x;
replay.pos_ev[1] = ev_pos.y;
replay.pos_ev[2] = ev_pos.z;
replay.quat_ev[0] = ev_att.q[0];
replay.quat_ev[1] = ev_att.q[1];
replay.quat_ev[2] = ev_att.q[2];
replay.quat_ev[3] = ev_att.q[3];
// TODO : switch to covariances from topic later
replay.pos_err = _default_ev_pos_noise;
replay.ang_err = _default_ev_ang_noise;
} else {
replay.ev_timestamp = 0;