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

ekf2: Add external vision to replay

Browse Source
This commit is contained in:
Paul Riseborough
2016-05-28 14:54:14 +02:00
committed by Lorenz Meier
parent 37b4955f07
commit 26d81418fa
1 changed files with 33 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2_replay/ekf2_replay_main.cpp
+33 -5
View File
@@ -70,6 +70,7 @@
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/vision_position_estimate.h>
#include <sdlog2/sdlog2_messages.h>
@@ -135,6 +136,7 @@ private:
orb_advert_t _flow_pub;
orb_advert_t _range_pub;
orb_advert_t _airspeed_pub;
orb_advert_t _ev_pub;
int _att_sub;
int _estimator_status_sub;
@@ -151,6 +153,7 @@ private:
struct optical_flow_s _flow;
struct distance_sensor_s _range;
struct airspeed_s _airspeed;
struct vision_position_estimate_s _ev;
unsigned _message_counter; // counter which will increase with every message read from the log
unsigned _part1_counter_ref; // this is the value of _message_counter when the part1 of the replay message is read (imu data)
@@ -158,6 +161,7 @@ private:
bool _read_part3;
bool _read_part4;
bool _read_part6;
bool _read_part5;
int _write_fd = -1;
px4_pollfd_struct_t _fds[1];
@@ -206,6 +210,7 @@ Ekf2Replay::Ekf2Replay(char *logfile) :
_flow_pub(nullptr),
_range_pub(nullptr),
_airspeed_pub(nullptr),
_ev_pub(nullptr),
_att_sub(-1),
_estimator_status_sub(-1),
_innov_sub(-1),
@@ -223,6 +228,7 @@ Ekf2Replay::Ekf2Replay(char *logfile) :
_read_part3(false),
_read_part4(false),
_read_part6(false),
_read_part5(false),
_write_fd(-1)
{
// build the path to the log
@@ -270,6 +276,15 @@ void Ekf2Replay::publishEstimatorInput()
_read_part4 = false;
if (_ev_pub == nullptr && _read_part5) {
_ev_pub = orb_advertise(ORB_ID(vision_position_estimate), &_ev);
} else if (_ev_pub != nullptr && _read_part5) {
orb_publish(ORB_ID(vision_position_estimate), _ev_pub, &_ev);
}
_read_part5 = false;
if (_sensors_pub == nullptr) {
_sensors_pub = orb_advertise(ORB_ID(sensor_combined), &_sensors);
@@ -347,6 +362,7 @@ void Ekf2Replay::setEstimatorInput(uint8_t *data, uint8_t type)
struct log_RPL3_s replay_part3 = {};
struct log_RPL4_s replay_part4 = {};
struct log_RPL6_s replay_part6 = {};
struct log_RPL5_s replay_part5 = {};
struct log_LAND_s vehicle_landed = {};
if (type == LOG_RPL1_MSG) {
@@ -409,9 +425,7 @@ void Ekf2Replay::setEstimatorInput(uint8_t *data, uint8_t type)
_range.current_distance = replay_part4.range_to_ground;
_read_part4 = true;
}
else if (type == LOG_RPL6_MSG){
} else if (type == LOG_RPL6_MSG){
uint8_t *dest_ptr = (uint8_t *)&replay_part6.time_airs_usec;
parseMessage(data, dest_ptr, type);
_airspeed.timestamp = replay_part6.time_airs_usec;
@@ -422,9 +436,23 @@ void Ekf2Replay::setEstimatorInput(uint8_t *data, uint8_t type)
_airspeed.confidence = replay_part6.confidence;
_read_part6 = true;
}
} else if (type == LOG_RPL5_MSG) {
uint8_t *dest_ptr = (uint8_t *)&replay_part5.time_ev_usec;
parseMessage(data, dest_ptr, type);
_ev.timestamp = replay_part5.time_ev_usec;
_ev.timestamp_computer = replay_part5.time_ev_usec; // fake this timestamp
_ev.x = replay_part5.x;
_ev.y = replay_part5.y;
_ev.z = replay_part5.z;
_ev.q[0] = replay_part5.q0;
_ev.q[1] = replay_part5.q1;
_ev.q[2] = replay_part5.q2;
_ev.q[3] = replay_part5.q3;
_ev.pos_err = replay_part5.pos_err;
_ev.ang_err = replay_part5.pos_err;
_read_part5 = true;
else if (type == LOG_LAND_MSG) {
} else if (type == LOG_LAND_MSG) {
uint8_t *dest_ptr = (uint8_t *)&vehicle_landed.landed;
parseMessage(data, dest_ptr, type);
_land_detected.landed = vehicle_landed.landed;