diff options
author | Ban Siesta <bansiesta@gmail.com> | 2014-12-19 23:38:55 +0000 |
---|---|---|
committer | Ban Siesta <bansiesta@gmail.com> | 2014-12-19 23:38:55 +0000 |
commit | 180e17de335f337b17c0c411d0eb430cec760619 (patch) | |
tree | d6f38067e10c525059acf66e1ed1b19af17b91d8 | |
parent | 7cb4786e7934af50b5ceb370c593e27111d9dd17 (diff) | |
download | px4-firmware-180e17de335f337b17c0c411d0eb430cec760619.tar.gz px4-firmware-180e17de335f337b17c0c411d0eb430cec760619.tar.bz2 px4-firmware-180e17de335f337b17c0c411d0eb430cec760619.zip |
navigator: report using mission result if a DO_JUMP waypoint has been changed
-rw-r--r-- | src/modules/mavlink/mavlink_mission.cpp | 8 | ||||
-rw-r--r-- | src/modules/navigator/mission.cpp | 12 | ||||
-rw-r--r-- | src/modules/navigator/mission.h | 6 | ||||
-rw-r--r-- | src/modules/uORB/topics/mission_result.h | 3 |
4 files changed, 28 insertions, 1 deletions
diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 34c088778..859d380fe 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 3f0a6bb51..9b0a092da 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 .*/ @@ -701,6 +703,16 @@ Mission::save_offboard_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() { _navigator->get_mission_result()->reached = true; diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index ea7cc0927..a8a644b0f 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -38,6 +38,7 @@ * @author Julian Oes <julian@oes.ch> * @author Thomas Gubler <thomasgubler@gmail.com> * @author Anton Babushkin <anton.babushkin@me.com> + * @author Ban Siesta <bansiesta@gmail.com> */ #ifndef NAVIGATOR_MISSION_H @@ -131,6 +132,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 */ void set_mission_item_reached(); diff --git a/src/modules/uORB/topics/mission_result.h b/src/modules/uORB/topics/mission_result.h index 17ddaa01d..2ddc529a3 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 */ }; /** |