read current and next mission item at the same time, issue item commands while processing items, guarding setpoint from non-position items

src/modules/navigator/mission.cpp

@@ -40,6 +40,7 @@
  * @author Anton Babushkin <anton.babushkin@me.com>
  * @author Ban Siesta <bansiesta@gmail.com>
  * @author Simon Wilks <simon@uaventure.com>
+ * @author Andreas Antener <andreas@uaventure.com>
  */
 
 #include <sys/types.h>
@@ -84,7 +85,8 @@ Mission::Mission(Navigator *navigator, const char *name) :
 	_min_current_sp_distance_xy(FLT_MAX),
 	_mission_item_previous_alt(NAN),
   	_on_arrival_yaw(NAN),
-	_distance_current_previous(0.0f)
+	_distance_current_previous(0.0f),
+	_work_item_type(WORK_ITEM_TYPE_DEFAULT)
 {
 	/* load initial params */
 	updateParams();
@@ -338,8 +340,12 @@ Mission::set_mission_items()
 	/* the home dist check provides user feedback, so we initialize it to this */
 	bool user_feedback_done = false;
 
+	/* mission item that comes after current if available */
+	struct mission_item_s mission_item_next;
+	bool has_next_item = false;
+
 	/* try setting onboard mission item */
-	if (_param_onboard_enabled.get() && read_mission_item(true, true, &_mission_item)) {
+	if (_param_onboard_enabled.get() && prepare_mission_items(true, &_mission_item, &mission_item_next, &has_next_item)) {
 		/* if mission type changed, notify */
 		if (_mission_type != MISSION_TYPE_ONBOARD) {
 			mavlink_log_critical(_navigator->get_mavlink_fd(), "onboard mission now running");
@@ -348,7 +354,7 @@ Mission::set_mission_items()
 		_mission_type = MISSION_TYPE_ONBOARD;
 
 	/* try setting offboard mission item */
-	} else if (read_mission_item(false, true, &_mission_item)) {
+	} else if (prepare_mission_items(false, &_mission_item, &mission_item_next, &has_next_item)) {
 		/* if mission type changed, notify */
 		if (_mission_type != MISSION_TYPE_OFFBOARD) {
 			mavlink_log_info(_navigator->get_mavlink_fd(), "offboard mission now running");
@@ -404,6 +410,12 @@ Mission::set_mission_items()
 		return;
 	}
 
+	if (!item_contains_position(&_mission_item)) {
+		// XXX: before issuing command, check if we need to align for transition first
+		issue_command(&_mission_item);
+		return;
+	}
+
 	if (pos_sp_triplet->current.valid) {
 		_on_arrival_yaw = _mission_item.yaw;
 	}
@@ -494,10 +506,9 @@ Mission::set_mission_items()
 	// TODO: report onboard mission item somehow
 
 	if (_mission_item.autocontinue && _mission_item.time_inside <= 0.001f) {
-		/* try to read next mission item */
-		struct mission_item_s mission_item_next;
+		/* try to process next mission item */
 
-		if (read_mission_item(_mission_type == MISSION_TYPE_ONBOARD, false, &mission_item_next)) {
+		if (has_next_item) {
 			/* got next mission item, update setpoint triplet */
 			mission_item_to_position_setpoint(&mission_item_next, &pos_sp_triplet->next);
 		} else {
@@ -650,14 +661,40 @@ Mission::altitude_sp_foh_reset()
 }
 
 bool
-Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s *mission_item)
+Mission::prepare_mission_items(bool onboard, struct mission_item_s *mission_item,
+	struct mission_item_s *next_mission_item, bool *has_next_item)
+{
+	bool first_res = false;
+	int offset = 1;
+
+	if (read_mission_item(onboard, 0, mission_item)) {
+		
+		first_res = true;
+
+		/* trying to find next position mission item */
+		while(read_mission_item(onboard, offset, next_mission_item)) {
+
+			if (item_contains_position(next_mission_item)) {
+				*has_next_item = true;
+				break;
+			}
+
+			offset++;
+		}
+	}
+
+	return first_res;
+}
+
+bool
+Mission::read_mission_item(bool onboard, int offset, struct mission_item_s *mission_item)
 {
 	/* select onboard/offboard mission */
 	int *mission_index_ptr;
 	dm_item_t dm_item;
 
 	struct mission_s *mission = (onboard) ? &_onboard_mission : &_offboard_mission;
-	int mission_index_next = (onboard) ? _current_onboard_mission_index : _current_offboard_mission_index;
+	int index_to_read = (onboard) ? _current_onboard_mission_index : _current_offboard_mission_index;
 
 	/* do not work on empty missions */
 	if (mission->count == 0) {
@@ -665,18 +702,18 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
 	}
 
 	/* move to next item if there is one */
-	if (mission_index_next < ((int)mission->count - 1)) {
-		mission_index_next++;
+	if (index_to_read + offset < ((int)mission->count - 1)) {
+		index_to_read += offset;
 	}
 
+	mission_index_ptr = &index_to_read;
+
 	if (onboard) {
 		/* onboard mission */
-		mission_index_ptr = is_current ? &_current_onboard_mission_index : &mission_index_next;
 		dm_item = DM_KEY_WAYPOINTS_ONBOARD;
 
 	} else {
 		/* offboard mission */
-		mission_index_ptr = is_current ? &_current_offboard_mission_index : &mission_index_next;
 		dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
 	}
 
@@ -713,7 +750,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
 
 				/* only raise the repeat count if this is for the current mission item
 				* but not for the next mission item */
-				if (is_current) {
+				if (offset == 0) {
 					(mission_item_tmp.do_jump_current_count)++;
 					/* save repeat count */
 					if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_POWER_ON_RESET,
@@ -732,7 +769,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
 				*mission_index_ptr = mission_item_tmp.do_jump_mission_index;
 
 			} else {
-				if (is_current) {
+				if (offset == 0) {
 					mavlink_log_critical(_navigator->get_mavlink_fd(),
 							     "DO JUMP repetitions completed");
 				}

src/modules/navigator/mission.h

@@ -136,11 +136,14 @@ private:
 
 	float get_absolute_altitude_for_item(struct mission_item_s &mission_item);
 
+	bool prepare_mission_items(bool onboard, struct mission_item_s *mission_item,
+		struct mission_item_s *next_mission_item, bool *has_next_item);
+
 	/**
 	 * Read current or next mission item from the dataman and watch out for DO_JUMPS
 	 * @return true if successful
 	 */
-	bool read_mission_item(bool onboard, bool is_current, struct mission_item_s *mission_item);
+	bool read_mission_item(bool onboard, int offset, struct mission_item_s *mission_item);
 
 	/**
 	 * Save current offboard mission state to dataman
@@ -204,6 +207,15 @@ private:
 	float _on_arrival_yaw; /**< holds the yaw value that should be applied when the current waypoint is reached */
 	float _distance_current_previous; /**< distance from previous to current sp in pos_sp_triplet,
 					    only use if current and previous are valid */
+
+	enum {
+		WORK_ITEM_TYPE_DEFAULT,
+		WORK_ITEM_TYPE_TAKEOFF,
+		WORK_ITEM_TYPE_LAND,
+		WORK_ITEM_TYPE_TRANSITION,
+		WORK_ITEM_TYPE_YAW
+	} _work_item_type;		/**< current type of work to do, intermediate to complete mission item */
+
 };
 
 #endif

src/modules/navigator/mission_block.cpp

@@ -67,6 +67,7 @@ MissionBlock::MissionBlock(Navigator *navigator, const char *name) :
 	_waypoint_position_reached(false),
 	_waypoint_yaw_reached(false),
 	_time_first_inside_orbit(0),
+	_action_start(0),
 	_actuators{},
 	_actuator_pub(nullptr),
 	_cmd_pub(nullptr)
@@ -82,6 +83,7 @@ MissionBlock::is_mission_item_reached()
 {
 	/* handle non-navigation or indefinite waypoints */
 	switch (_mission_item.nav_cmd) {
+		// XXX: move handling to mission as well
 		case NAV_CMD_DO_SET_SERVO: {
 			memset(&actuators, 0, sizeof(actuators));
 			actuators.control[_mission_item.actuator_num] = 1.0f / 2000 * -_mission_item.actuator_value;
@@ -105,18 +107,13 @@ MissionBlock::is_mission_item_reached()
 
 		case vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL: /* fallthrough */
 		case vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION:
-			{
-			/* forward the command to other processes */
-			warnx("got instantaneous command, forwarding.\n");
-			struct vehicle_command_s cmd = {};
-			cmd.command = _mission_item.nav_cmd;
-			mission_item_to_vehicle_command(&_mission_item, &cmd);
-			if (_cmd_pub != nullptr) {
-				orb_publish(ORB_ID(vehicle_command), _cmd_pub, &cmd);
+			// XXX: we should wait on command ACK or status change instead
+			// currently we just wait so the command be processed
+			if (hrt_absolute_time() - _action_start > 1000) {
+				return true;
+
 			} else {
-				_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &cmd);
-			}
-			return true;
+				return false;
 			}
 
 		default:
@@ -238,6 +235,7 @@ MissionBlock::mission_item_to_vehicle_command(const struct mission_item_s *item,
 	cmd->param5 = item->params[4];
 	cmd->param6 = item->params[5];
 	cmd->param7 = item->params[6];
+	cmd->command = item->nav_cmd;
 
 	cmd->target_system = _navigator->get_vstatus()->system_id;
 	cmd->target_component = _navigator->get_vstatus()->component_id;
@@ -246,9 +244,46 @@ MissionBlock::mission_item_to_vehicle_command(const struct mission_item_s *item,
 	cmd->confirmation = false;
 }
 
+void
+MissionBlock::issue_command(const struct mission_item_s *item)
+{
+	if (item_contains_position(item)) {
+		return;
+	}
+
+	warnx("forwarding command.\n");
+	struct vehicle_command_s cmd = {};
+	mission_item_to_vehicle_command(item, &cmd);
+	_action_start = hrt_absolute_time();
+
+	if (_cmd_pub != nullptr) {
+		orb_publish(ORB_ID(vehicle_command), _cmd_pub, &cmd);
+
+	} else {
+		_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &cmd);
+	}
+}
+
+bool
+MissionBlock::item_contains_position(const struct mission_item_s *item)
+{
+	// XXX: maybe extend that check onto item properties
+	if (item->nav_cmd == vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL ||
+			item->nav_cmd == vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION) {
+		return false;
+	}
+
+	return true;
+}
+
 void
 MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *item, struct position_setpoint_s *sp)
 {
+	/* don't change the setpoint for non-position items */
+	if (!item_contains_position(item)) {
+		return;
+	}
+
 	sp->valid = true;
 	sp->lat = item->lat;
 	sp->lon = item->lon;
@@ -261,7 +296,7 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
 	sp->acceptance_radius = item->acceptance_radius;
 
 	switch (item->nav_cmd) {
-	case NAV_CMD_DO_SET_SERVO:
+	case NAV_CMD_DO_SET_SERVO: // XXX: should be also return in the beginning for this?
 			/* Set current position for loitering set point*/
 			sp->lat = _navigator->get_global_position()->lat;
 			sp->lon = _navigator->get_global_position()->lon;

src/modules/navigator/mission_block.h

@@ -78,6 +78,8 @@ protected:
 	 */
 	void reset_mission_item_reached();
 
+	bool item_contains_position(const struct mission_item_s *item);
+
 	/**
 	 * Convert a mission item to a position setpoint
 	 *
@@ -116,10 +118,13 @@ protected:
 	 */
 	void mission_item_to_vehicle_command(const struct mission_item_s *item, struct vehicle_command_s *cmd);
 
+	void issue_command(const struct mission_item_s *item);
+
 	mission_item_s _mission_item;
 	bool _waypoint_position_reached;
 	bool _waypoint_yaw_reached;
 	hrt_abstime _time_first_inside_orbit;
+	hrt_abstime _action_start;
 
 	actuator_controls_s _actuators;
 	orb_advert_t    _actuator_pub;