diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 34c0887780..859d380fe5 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -309,6 +309,12 @@ MavlinkMissionManager::send(const hrt_abstime now) send_mission_current(_current_seq); + if (mission_result.item_do_jump_changed) { + /* send a mission item again if the remaining DO_JUMPs has changed */ + send_mission_item(_transfer_partner_sysid, _transfer_partner_compid, + (uint16_t)mission_result.item_changed_index); + } + } else { if (_slow_rate_limiter.check(now)) { send_mission_current(_current_seq); @@ -808,7 +814,7 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s * case NAV_CMD_DO_JUMP: mavlink_mission_item->param1 = mission_item->do_jump_mission_index; - mavlink_mission_item->param2 = mission_item->do_jump_repeat_count; + mavlink_mission_item->param2 = mission_item->do_jump_repeat_count - mission_item->do_jump_current_count; break; default: diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 3f0a6bb51c..9b0a092dad 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -630,6 +630,8 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s "ERROR DO JUMP waypoint could not be written"); return false; } + report_do_jump_mission_changed(*mission_index_ptr, + mission_item_tmp.do_jump_repeat_count); } /* set new mission item index and repeat * we don't have to validate here, if it's invalid, we should realize this later .*/ @@ -700,6 +702,16 @@ Mission::save_offboard_mission_state() dm_unlock(DM_KEY_MISSION_STATE); } +void +Mission::report_do_jump_mission_changed(int index, int do_jumps_remaining) +{ + /* inform about the change */ + _navigator->get_mission_result()->item_do_jump_changed = true; + _navigator->get_mission_result()->item_changed_index = index; + _navigator->get_mission_result()->item_do_jump_remaining = do_jumps_remaining; + _navigator->set_mission_result_updated(); +} + void Mission::set_mission_item_reached() { diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index ea7cc0927c..a8a644b0f5 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -38,6 +38,7 @@ * @author Julian Oes * @author Thomas Gubler * @author Anton Babushkin + * @author Ban Siesta */ #ifndef NAVIGATOR_MISSION_H @@ -130,6 +131,11 @@ private: */ void save_offboard_mission_state(); + /** + * Inform about a changed mission item after a DO_JUMP + */ + void report_do_jump_mission_changed(int index, int do_jumps_remaining); + /** * Set a mission item as reached */ diff --git a/src/modules/uORB/topics/mission_result.h b/src/modules/uORB/topics/mission_result.h index 17ddaa01d3..2ddc529a3f 100644 --- a/src/modules/uORB/topics/mission_result.h +++ b/src/modules/uORB/topics/mission_result.h @@ -61,6 +61,9 @@ struct mission_result_s bool finished; /**< true if mission has been completed */ bool stay_in_failsafe; /**< true if the commander should not switch out of the failsafe mode*/ bool flight_termination; /**< true if the navigator demands a flight termination from the commander app */ + bool item_do_jump_changed; /**< true if the number of do jumps remaining has changed */ + unsigned item_changed_index; /**< indicate which item has changed */ + unsigned item_do_jump_remaining;/**< set to the number of do jumps remaining for that item */ }; /**