yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 09:28:58 +00:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: add listeners for range and optical flow data

Browse Source
This commit is contained in:
bugobliterator
2016-02-17 11:45:45 -08:00
committed by Lorenz Meier
parent 31a975e501
commit 6c5812c528
1 changed files with 47 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -77,6 +77,8 @@
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/ekf2_replay.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/distance_sensor.h>
#include <ecl/EKF/ekf.h>
@@ -134,6 +136,8 @@ private:
int _airspeed_sub = -1;
int _params_sub = -1;
int _vehicle_status_sub = -1;
int _optical_flow_sub = -1;
int _range_finder_sub = -1;
bool _prev_motors_armed = false; // motors armed status from the previous frame
@@ -273,6 +277,8 @@ void Ekf2::task_main()
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
_optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));
_range_finder_sub = orb_subscribe(ORB_ID(distance_sensor));
px4_pollfd_struct_t fds[2] = {};
fds[0].fd = _sensors_sub;
@@ -290,6 +296,8 @@ void Ekf2::task_main()
vehicle_gps_position_s gps = {};
airspeed_s airspeed = {};
vehicle_control_mode_s vehicle_control_mode = {};
optical_flow_s optical_flow = {};
distance_sensor_s range_finder = {};
while (!_task_should_exit) {
int ret = px4_poll(fds, sizeof(fds) / sizeof(fds[0]), 1000);
@@ -321,6 +329,8 @@ void Ekf2::task_main()
bool gps_updated = false;
bool airspeed_updated = false;
bool vehicle_status_updated = false;
bool optical_flow_updated = false;
bool range_finder_updated = false;
orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors);
// update all other topics if they have new data
@@ -336,6 +346,26 @@ void Ekf2::task_main()
orb_copy(ORB_ID(airspeed), _airspeed_sub, &airspeed);
}
orb_check(_optical_flow_sub, &optical_flow_updated);
if(optical_flow_updated) {
orb_copy(ORB_ID(optical_flow), _optical_flow_sub, &optical_flow);
}
orb_check(_range_finder_sub, &range_finder_updated);
if(range_finder_updated) {
orb_copy(ORB_ID(distance_sensor), _range_finder_sub, &range_finder);
}
// Use the control model data to determine if the motors are armed as a surrogate for an on-ground vs in-air status
// TODO implement a global vehicle on-ground/in-air check
orb_check(_control_mode_sub, &control_mode_updated);
if (control_mode_updated) {
orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &vehicle_control_mode);
_ekf->set_arm_status(vehicle_control_mode.flag_armed);
}
// in replay mode we are getting the actual timestamp from the sensor topic
hrt_abstime now = 0;
if (_replay_mode) {
@@ -383,6 +413,23 @@ void Ekf2::task_main()
_ekf->setAirspeedData(airspeed.timestamp, &airspeed.indicated_airspeed_m_s);
}
if(optical_flow_updated) {
flow_message flow;
flow.flowdata(0) = optical_flow.pixel_flow_x_integral;
flow.flowdata(1) = optical_flow.pixel_flow_y_integral;
flow.quality = optical_flow.quality;
flow.gyrodata(0) = optical_flow.gyro_x_rate_integral;
flow.gyrodata(1) = optical_flow.gyro_y_rate_integral;
flow.dt = optical_flow.integration_timespan;
if(!isnan(optical_flow.pixel_flow_y_integral) && !isnan(optical_flow.pixel_flow_x_integral)) {
_ekf->setOpticalFlowData(optical_flow.timestamp, &flow);
}
}
if(range_finder_updated) {
_ekf->setRangeData(range_finder.timestamp, &range_finder.current_distance);
}
// read vehicle status if available for 'landed' information
orb_check(_vehicle_status_sub, &vehicle_status_updated);