From 82716012bdf55d8dc99f7d945331063d945679f3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Fri, 24 Mar 2017 09:10:04 +0100 Subject: [PATCH] geofence_update.msg: notify navigator on geofence update --- msg/CMakeLists.txt | 1 + msg/geofence_update.msg | 4 +++ src/modules/mavlink/mavlink_mission.cpp | 7 +++- src/modules/mavlink/mavlink_mission.h | 1 + src/modules/navigator/navigator.h | 3 +- src/modules/navigator/navigator_main.cpp | 44 +++++++++++++++++++++++- 6 files changed, 57 insertions(+), 3 deletions(-) create mode 100644 msg/geofence_update.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index ca35d80078..b23cc6528e 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -59,6 +59,7 @@ set(msg_file_names follow_target.msg fw_pos_ctrl_status.msg geofence_result.msg + geofence_update.msg gps_dump.msg gps_inject_data.msg hil_sensor.msg diff --git a/msg/geofence_update.msg b/msg/geofence_update.msg new file mode 100644 index 0000000000..e05aeaadc0 --- /dev/null +++ b/msg/geofence_update.msg @@ -0,0 +1,4 @@ +# This message is used to notify the system about a geofence update (dataman +# storage) + +uint32 dummy # unused diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 1cb431e600..cc7d0e6f39 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -51,6 +51,7 @@ #include #include +#include #include #include @@ -87,6 +88,7 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : _offboard_mission_sub(-1), _mission_result_sub(-1), _offboard_mission_pub(nullptr), + _geofence_update_pub(nullptr), _slow_rate_limiter(100 * 1000), // Rate limit sending of the current WP sequence to 10 Hz _verbose(mavlink->verbose()), _mavlink(mavlink) @@ -101,6 +103,7 @@ MavlinkMissionManager::~MavlinkMissionManager() { orb_unsubscribe(_mission_result_sub); orb_unadvertise(_offboard_mission_pub); + orb_unadvertise(_geofence_update_pub); } void @@ -210,7 +213,9 @@ MavlinkMissionManager::update_geofence_count(unsigned count) if (res == sizeof(mission_stats_entry_s)) { _count[(uint8_t)MAV_MISSION_TYPE_FENCE] = count; - // TODO: notify via orb + + geofence_update_s geofence_update{}; + orb_publish_auto(ORB_ID(geofence_update), &_geofence_update_pub, &geofence_update, nullptr, ORB_PRIO_DEFAULT); } else { warnx("WPM: ERROR: can't save mission state"); diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h index bb31a66fe7..a3be0c4056 100644 --- a/src/modules/mavlink/mavlink_mission.h +++ b/src/modules/mavlink/mavlink_mission.h @@ -126,6 +126,7 @@ private: int _offboard_mission_sub; int _mission_result_sub; orb_advert_t _offboard_mission_pub; + orb_advert_t _geofence_update_pub; MavlinkRateLimiter _slow_rate_limiter; diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 849f1ee456..825036d445 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -236,6 +236,7 @@ private: int _param_update_sub{-1}; /**< param update subscription */ int _sensor_combined_sub{-1}; /**< sensor combined subscription */ int _vehicle_command_sub{-1}; /**< vehicle commands (onboard and offboard) */ + int _geofence_update_sub{-1}; /**< geofence updates */ int _vstatus_sub{-1}; /**< vehicle status subscription */ orb_advert_t _att_sp_pub{nullptr}; @@ -256,7 +257,7 @@ private: vehicle_global_position_s _global_pos{}; /**< global vehicle position */ vehicle_gps_position_s _gps_pos{}; /**< gps position */ vehicle_land_detected_s _land_detected{}; /**< vehicle land_detected */ - vehicle_local_position_s _local_pos; /**< local vehicle position */ + vehicle_local_position_s _local_pos{}; /**< local vehicle position */ vehicle_status_s _vstatus{}; /**< vehicle status */ int _mission_instance_count{-1}; /**< instance count for the current mission */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 4e9efe705b..5ec9c072b6 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -57,10 +57,15 @@ #include #include #include + #include #include #include -#include +#include +#include + +#include +#include #include #include #include @@ -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;