Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-25 16:56:25 +08:00
Code Issues Actions 4 Packages Projects Releases Wiki Activity

provide ekf2 with landed flag from landing detector

Browse Source
This commit is contained in:
Roman
2016-02-13 08:56:25 +01:00
parent 2177c0e18a
commit 11df8168ee
2 changed files with 14 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/ecl
+1 -1
Submodule src/lib/ecl updated: ce0ddc0207...ad978c642f
src/modules/ekf2/ekf2_main.cpp
+13
View File
@@ -75,6 +75,7 @@
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/ekf2_innovations.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_status.h>
#include <ecl/EKF/ekf.h>
@@ -128,6 +129,7 @@ private:
int _airspeed_sub = -1;
int _params_sub = -1;
int _control_mode_sub = -1;
int _vehicle_status_sub = -1;
orb_advert_t _att_pub;
orb_advert_t _lpos_pub;
@@ -253,6 +255,7 @@ void Ekf2::task_main()
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
px4_pollfd_struct_t fds[2] = {};
fds[0].fd = _sensors_sub;
@@ -295,6 +298,7 @@ void Ekf2::task_main()
bool gps_updated = false;
bool airspeed_updated = false;
bool control_mode_updated = false;
bool vehicle_status_updated = false;
sensor_combined_s sensors = {};
airspeed_s airspeed = {};
@@ -364,6 +368,15 @@ void Ekf2::task_main()
_ekf->setAirspeedData(airspeed.timestamp, &airspeed.indicated_airspeed_m_s);
}
// read vehicle status if available for 'landed' information
orb_check(_vehicle_status_sub, &vehicle_status_updated);
if (vehicle_status_updated) {
struct vehicle_status_s status = {};
orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &status);
_ekf->set_in_air_status(!status.condition_landed);
}
// run the EKF update
_ekf->update();