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

geofence_update.msg: notify navigator on geofence update

Browse Source
This commit is contained in:
Beat Küng
2017-08-04 18:08:51 +02:00
committed by Lorenz Meier
parent 328e84117e
commit 82716012bd
6 changed files with 57 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/navigator_main.cpp
+43 -1
View File
@@ -57,10 +57,15 @@
#include <px4_tasks.h>
#include <sys/ioctl.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <systemlib/mavlink_log.h>
#include <systemlib/systemlib.h>
#include <uORB/topics/fence.h>
#include <drivers/device/device.h>
#include <arch/board/board.h>
#include <uORB/uORB.h>
#include <uORB/topics/geofence_update.h>
#include <uORB/topics/fw_pos_ctrl_status.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/mission.h>
@@ -247,6 +252,7 @@ Navigator::task_main()
_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
_param_update_sub = orb_subscribe(ORB_ID(parameter_update));
_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));
_geofence_update_sub = orb_subscribe(ORB_ID(geofence_update));
/* copy all topics first time */
vehicle_status_update();
@@ -340,6 +346,15 @@ Navigator::task_main()
params_update();
}
/* geofence updated */
orb_check(_geofence_update_sub, &updated);
if (updated) {
geofence_update_s geofence_update;
orb_copy(ORB_ID(geofence_update), _geofence_update_sub, &geofence_update);
_geofence.updateFence();
}
/* vehicle status updated */
orb_check(_vstatus_sub, &updated);
@@ -667,6 +682,33 @@ Navigator::task_main()
perf_end(_loop_perf);
}
orb_unsubscribe(_global_pos_sub);
_global_pos_sub = -1;
orb_unsubscribe(_local_pos_sub);
_local_pos_sub = -1;
orb_unsubscribe(_gps_pos_sub);
_gps_pos_sub = -1;
orb_unsubscribe(_sensor_combined_sub);
_sensor_combined_sub = -1;
orb_unsubscribe(_fw_pos_ctrl_status_sub);
_fw_pos_ctrl_status_sub = -1;
orb_unsubscribe(_vstatus_sub);
_vstatus_sub = -1;
orb_unsubscribe(_land_detected_sub);
_land_detected_sub = -1;
orb_unsubscribe(_home_pos_sub);
_home_pos_sub = -1;
orb_unsubscribe(_onboard_mission_sub);
_onboard_mission_sub = -1;
orb_unsubscribe(_offboard_mission_sub);
_offboard_mission_sub = -1;
orb_unsubscribe(_param_update_sub);
_param_update_sub = -1;
orb_unsubscribe(_vehicle_command_sub);
_vehicle_command_sub = -1;
orb_unsubscribe(_geofence_update_sub);
_geofence_update_sub = -1;
PX4_INFO("exiting");
_navigator_task = -1;