dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

provide ekf2 with landed flag from landing detector

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/lib/ecl

@ -1 +1 @@
Subproject commit ce0ddc02078ff96c5be31f958e515408d158f8aa
Subproject commit ad978c642ff6f2be06fe8dd29693983e6e216779

13
src/modules/ekf2/ekf2_main.cpp
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();