fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-17 09:57:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander: internalize system status bools

Browse Source 
Most condition bools in the commander are not used anywhere but in the
commander. It therefore makes sense to move them to a different internal
struct and remove them from the vehicle_status message.

Also, the land_detected should be used by all the modules instead of
getting it through the commander and system_status.
This commit is contained in:
Julian Oes
2016-02-25 17:00:39 +00:00
parent 705979e3c7
commit 1f44fb1efd
18 changed files with 450 additions and 332 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/ekf2_main.cpp
+22 -10
View File
@@ -75,11 +75,11 @@
#include <uORB/topics/wind_estimate.h>
#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 <uORB/topics/actuator_armed.h>
#include <uORB/topics/ekf2_replay.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <ecl/EKF/ekf.h>
@@ -139,6 +139,8 @@ private:
int _vehicle_status_sub = -1;
int _optical_flow_sub = -1;
int _range_finder_sub = -1;
int _actuator_armed_sub = -1;
int _vehicle_land_detected_sub = -1;
bool _prev_motors_armed = false; // motors armed status from the previous frame
@@ -319,6 +321,8 @@ void Ekf2::task_main()
_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));
_actuator_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
_vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
px4_pollfd_struct_t fds[2] = {};
fds[0].fd = _sensors_sub;
@@ -338,6 +342,7 @@ void Ekf2::task_main()
vehicle_control_mode_s vehicle_control_mode = {};
optical_flow_s optical_flow = {};
distance_sensor_s range_finder = {};
actuator_armed_s actuator_armed = {};
while (!_task_should_exit) {
int ret = px4_poll(fds, sizeof(fds) / sizeof(fds[0]), 1000);
@@ -371,6 +376,8 @@ void Ekf2::task_main()
bool vehicle_status_updated = false;
bool optical_flow_updated = false;
bool range_finder_updated = false;
bool actuator_armed_updated = false;
bool vehicle_land_detected_updated = false;
orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors);
// update all other topics if they have new data
@@ -465,14 +472,19 @@ void Ekf2::task_main()
_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);
orb_check(_actuator_armed_sub, &actuator_armed_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);
_ekf->set_arm_status(status.arming_state & vehicle_status_s::ARMING_STATE_ARMED);
if (actuator_armed_updated) {
orb_copy(ORB_ID(actuator_armed), _actuator_armed_sub, &actuator_armed);
_ekf->set_arm_status(&actuator_armed.armed);
}
orb_check(_vehicle_land_detected_sub, &vehicle_land_detected_updated);
if (vehicle_land_detected_updated) {
struct vehicle_land_detected_s vehicle_land_detected = {};
orb_copy(ORB_ID(vehicle_land_detected), _vehicle_land_detected_sub, &vehicle_land_detected);
_ekf->set_in_air_status(!vehicle_land_detected.landed);
}
// run the EKF update and output
@@ -703,7 +715,7 @@ void Ekf2::task_main()
}
// save the declination to the EKF2_MAG_DECL parameter when a dis-arm event is detected
if ((_params->mag_declination_source & (1 << 1)) && _prev_motors_armed && !vehicle_control_mode.flag_armed) {
if ((_params->mag_declination_source & (1 << 1)) && _prev_motors_armed && !actuator_armed.armed) {
float decl_deg;
_ekf->copy_mag_decl_deg(&decl_deg);
_mag_declination_deg->set(decl_deg);