mavlink: set current WP working as expected, report current WP with 0.5 Hz

1 files changed, 11 insertions, 8 deletions

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 15f2fd2ca..1ce467a7b 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -987,8 +987,6 @@ void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq)
 		mavlink_msg_mission_current_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpc);
 		mavlink_missionlib_send_message(&msg);
 
-		if (_verbose) warnx("Broadcasted new current waypoint %u", wpc.seq);
-
 	} else if (seq == 0 && wpm->size == 0) {
 
 		/* don't broadcast if no WPs */
@@ -1145,7 +1143,8 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 						mission.current_index = wpc.seq;
 						publish_mission();
 						
-						mavlink_wpm_send_waypoint_current(wpc.seq);
+						/* don't answer yet, wait for the navigator to respond, then publish the mission_result */
+//						mavlink_wpm_send_waypoint_current(wpc.seq);
 
 					} else {
 						mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list");
@@ -1708,8 +1707,7 @@ Mavlink::task_main(int argc, char *argv[])
 
 	thread_running = true;
 
-	/* arm counter to go off immediately */
-	unsigned lowspeed_counter = 10;
+	unsigned lowspeed_counter = 0;
 
 	/* wakeup source(s) */
 	struct pollfd fds[1];
@@ -1739,7 +1737,7 @@ Mavlink::task_main(int argc, char *argv[])
 		}
 
 		/* 1 Hz */
-		if (lowspeed_counter == 10) {
+		if (lowspeed_counter % 10 == 0) {
 			mavlink_update_system();
 
 			/* translate the current system state to mavlink state and mode */
@@ -1772,7 +1770,12 @@ Mavlink::task_main(int argc, char *argv[])
 						    v_status.errors_count2,
 						    v_status.errors_count3,
 						    v_status.errors_count4);
-			lowspeed_counter = 0;
+		}
+
+		/* 0.5 Hz */
+		if (lowspeed_counter % 20 == 0) {
+
+			mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission);
 		}
 
 		lowspeed_counter++;
@@ -1783,7 +1786,7 @@ Mavlink::task_main(int argc, char *argv[])
 		if (updated) {
 			orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result);
 
-			if (_verbose) warnx("Got mission result");
+			if (_verbose) warnx("Got mission result: new current: %d", mission_result.index_current_mission);
 
 			if (mission_result.mission_reached) {
 				mavlink_wpm_send_waypoint_reached((uint16_t)mission_result.mission_index_reached);