From 3f252987988738d9246eec9716b780d23cb8b0f7 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 27 Nov 2013 09:27:08 +0100 Subject: Mavlink and navigator: Disable some functions in mavlink that are taken over by navigator, introduce topic to report mission status from commander back to mavlink --- src/modules/mavlink/mavlink.c | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) (limited to 'src/modules/mavlink/mavlink.c') diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 7c10e297b..2ec00a9bc 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -74,6 +74,8 @@ #include "waypoints.h" #include "mavlink_parameters.h" +#include + /* define MAVLink specific parameters */ PARAM_DEFINE_INT32(MAV_SYS_ID, 1); PARAM_DEFINE_INT32(MAV_COMP_ID, 50); @@ -644,6 +646,10 @@ int mavlink_thread_main(int argc, char *argv[]) set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 10000); } + int mission_result_sub = orb_subscribe(ORB_ID(mission_result)); + struct mission_result_s mission_result; + memset(&mission_result, 0, sizeof(mission_result)); + thread_running = true; /* arm counter to go off immediately */ @@ -690,6 +696,17 @@ int mavlink_thread_main(int argc, char *argv[]) lowspeed_counter++; + bool updated; + orb_check(mission_result_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result); + + if (mission_result.mission_reached) { + mavlink_wpm_send_waypoint_reached((uint16_t)mission_result.mission_index); + } + } + mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap); /* sleep quarter the time */ -- cgit v1.2.3