diff options
| -rw-r--r-- | src/modules/mavlink/mavlink_main.cpp | 19 | ||||
| -rw-r--r-- | src/modules/navigator/navigator_main.cpp | 2 | ||||
| -rw-r--r-- | src/modules/navigator/navigator_mission.cpp | 8 | ||||
| -rw-r--r-- | src/modules/navigator/navigator_mission.h | 2 |
4 files changed, 16 insertions, 15 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); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 5559d7b56..4d135a0ef 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1105,7 +1105,7 @@ Navigator::set_mission_item() if (ret == OK) { - _mission.report_current_mission_item(); + _mission.report_current_offboard_mission_item(); /* reset time counter for new item */ _time_first_inside_orbit = 0; diff --git a/src/modules/navigator/navigator_mission.cpp b/src/modules/navigator/navigator_mission.cpp index fdc4ffff6..72dddebfe 100644 --- a/src/modules/navigator/navigator_mission.cpp +++ b/src/modules/navigator/navigator_mission.cpp @@ -89,7 +89,7 @@ Mission::set_current_offboard_mission_index(int new_index) _current_offboard_mission_index = 0; } } - report_current_mission_item(); + report_current_offboard_mission_item(); } void @@ -296,11 +296,9 @@ Mission::report_mission_item_reached() } void -Mission::report_current_mission_item() +Mission::report_current_offboard_mission_item() { - if (_current_mission_type == MISSION_TYPE_OFFBOARD) { - _mission_result.index_current_mission = _current_offboard_mission_index; - } + _mission_result.index_current_mission = _current_offboard_mission_index; } void diff --git a/src/modules/navigator/navigator_mission.h b/src/modules/navigator/navigator_mission.h index 845c16583..2bd4da82e 100644 --- a/src/modules/navigator/navigator_mission.h +++ b/src/modules/navigator/navigator_mission.h @@ -73,7 +73,7 @@ public: void move_to_next(); void report_mission_item_reached(); - void report_current_mission_item(); + void report_current_offboard_mission_item(); void publish_mission_result(); private: |