Mission looping implemented with DO_JUMP waypoint type.follow_track

Currently repeat counter not taken into account

4 files changed, 36 insertions, 1 deletions

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index e020605ca..0e34f633f 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -894,6 +894,12 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item
 		mission_item->path_mode = (enum NAV_PATH_MODE)(mavlink_mission_item->param4);
 		break;
 
+	case MAV_CMD_DO_JUMP:
+			mission_item->jump_to_wp_index = (int)(mavlink_mission_item->param1);
+			mission_item->jump_repeat_count = (int)(mavlink_mission_item->param2);
+			break;
+
+
 	default:
 		mission_item->acceptance_radius = mavlink_mission_item->param2;
 		break;
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 357575e36..60892c29d 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -1313,10 +1313,25 @@ Navigator::set_mission_item()
 
 	int ret = ERROR;
 
+	switch (_mission._current_mission_type) {
+		case _mission.MISSION_TYPE_ONBOARD:
+			mavlink_log_info(_mavlink_fd, "ONBOARD");
+			break;
+
+		case _mission.MISSION_TYPE_OFFBOARD:
+			mavlink_log_info(_mavlink_fd, "OFFBOARD");
+			break;
+
+		default:
+			break;
+		}
+
 	while (_mission.current_mission_available()) {
 		bool onboard = false;
 		unsigned index = 0;
+		mavlink_log_critical(_mavlink_fd, "#audio: Getting next WP");
 		ret = _mission.get_current_mission_item(&_mission_item, &onboard, &index);
+		mavlink_log_critical(_mavlink_fd, "#audio: Getting next WP done: %d", ret);
 
 		if (ret == OK) {
 			_mission_item_valid = true;
@@ -1332,6 +1347,17 @@ Navigator::set_mission_item()
 				continue;
 			}
 
+			if (_mission_item.nav_cmd == NAV_CMD_DO_JUMP){
+                /* jump to  item */
+                _mission.set_current_offboard_mission_index(_mission_item.jump_to_wp_index);
+                mavlink_log_info(_mavlink_fd, "[navigator] do jump");
+                mavlink_log_info(_mavlink_fd, "#audio: Jump detected. Jumping to index: %d", _mission_item.jump_to_wp_index);
+                mavlink_log_info(_mavlink_fd, "#audio: Total mission items: %d", _mission._offboard_mission_item_count);
+                mavlink_log_info(_mavlink_fd, "#audio: Current mission item: %d", _mission._current_offboard_mission_index);
+                continue;
+            }
+
+
 			_mission.report_current_offboard_mission_item();
 
 			if (_roi_item_valid && _roi_item.roi_mode == ROI_MODE_FOLLOW && _mission_item.nav_cmd == NAV_CMD_WAYPOINT) {
@@ -1450,6 +1476,7 @@ Navigator::set_mission_item()
 		_pos_sp_triplet_updated = true;
 		break;
 	}
+	mavlink_log_info(_mavlink_fd, "set mission item end...");
 }
 
 void
diff --git a/src/modules/navigator/navigator_mission.h b/src/modules/navigator/navigator_mission.h
index b0f88e016..4cf8681a1 100644
--- a/src/modules/navigator/navigator_mission.h
+++ b/src/modules/navigator/navigator_mission.h
@@ -77,7 +77,7 @@ public:
 	void		report_current_offboard_mission_item();
 	void		publish_mission_result();
 
-private:
+public:
 	bool		current_onboard_mission_available();
 	bool		current_offboard_mission_available();
 	bool		next_onboard_mission_available();
diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h
index 46b2a425e..a607778ab 100644
--- a/src/modules/uORB/topics/mission.h
+++ b/src/modules/uORB/topics/mission.h
@@ -110,6 +110,8 @@ struct mission_item_s {
 	enum NAV_PATH_MODE path_mode;	/**< path building mode */
 	bool autocontinue;		/**< true if next waypoint should follow after this one */
 	enum ORIGIN origin;		/**< where the waypoint has been generated		*/
+	int jump_to_wp_index; /** jump to waypoint index	*/
+	int jump_repeat_count; /** jump to waypoint index repeat count	*/
 };
 
 struct mission_s