Mission:

- weathervane on takeoff
- separate cruising speeds for VTOL in MC and FW
- cruising speed resets
- mission work item logic is more clear
- fixed double execution of certain work item states
- enable cruise speed change on the fly by command
- moved VTOL transition target position generation to mission code and set always

src/modules/navigator/mission.cpp

@@ -73,7 +73,7 @@ Mission::Mission(Navigator *navigator, const char *name) :
 	_param_dist_1wp(this, "MIS_DIST_1WP", false),
 	_param_altmode(this, "MIS_ALTMODE", false),
 	_param_yawmode(this, "MIS_YAWMODE", false),
-	_param_force_vtol(this, "VT_NAV_FORCE_VT", false),
+	_param_force_vtol(this, "NAV_FORCE_VT", false),
 	_param_fw_climbout_diff(this, "FW_CLMBOUT_DIFF", false),
 	_onboard_mission{},
 	_offboard_mission{},
@@ -130,6 +130,7 @@ Mission::on_inactive()
 		if (need_to_reset_mission(false)) {
 			reset_offboard_mission(_offboard_mission);
 			update_offboard_mission();
+			_navigator->reset_cruising_speed();
 		}
 
 	} else {
@@ -159,13 +160,10 @@ Mission::on_inactive()
 	/* require takeoff after non-loiter or landing */
 	if (!_navigator->get_can_loiter_at_sp() || _navigator->get_land_detected()->landed) {
 		_need_takeoff = true;
-
-		/* Reset work item type to default if auto take-off has been paused or aborted,
-		   and we landed in manual mode. */
-		if (_work_item_type == WORK_ITEM_TYPE_TAKEOFF) {
-			_work_item_type = WORK_ITEM_TYPE_DEFAULT;
-		}
 	}
+
+	/* reset so current mission item gets restarted if mission was paused */
+	_work_item_type = WORK_ITEM_TYPE_DEFAULT;
 }
 
 void
@@ -198,6 +196,7 @@ Mission::on_active()
 	if (need_to_reset_mission(true)) {
 		reset_offboard_mission(_offboard_mission);
 		update_offboard_mission();
+		_navigator->reset_cruising_speed();
 		offboard_updated = true;
 	}
 
@@ -330,6 +329,8 @@ Mission::update_onboard_mission()
 		_navigator->get_mission_result()->seq_reached = 0;
 		_navigator->get_mission_result()->seq_total = _onboard_mission.count;
 
+		/* reset work item if new mission has been accepted */
+		_work_item_type = WORK_ITEM_TYPE_DEFAULT;
 		_navigator->increment_mission_instance_count();
 		_navigator->set_mission_result_updated();
 
@@ -378,6 +379,9 @@ Mission::update_offboard_mission()
 			_navigator->get_mission_result()->reached = false;
 			_navigator->get_mission_result()->seq_reached = 0;
 			_navigator->get_mission_result()->seq_total = _offboard_mission.count;
+
+			/* reset work item if new mission has been accepted */
+			_work_item_type = WORK_ITEM_TYPE_DEFAULT;
 		}
 
 	} else {
@@ -533,9 +537,9 @@ Mission::set_mission_items()
 	if (item_contains_position(&_mission_item)) {
 
 		/* force vtol land */
-		if (_mission_item.nav_cmd == NAV_CMD_LAND && _navigator->get_vstatus()->is_vtol
+		if (_mission_item.nav_cmd == NAV_CMD_LAND && _param_force_vtol.get() == 1
-		    && _param_force_vtol.get() && !_navigator->get_vstatus()->is_rotary_wing) {
+		    && !_navigator->get_vstatus()->is_rotary_wing
-
+		    && _navigator->get_vstatus()->is_vtol) {
 			_mission_item.nav_cmd = NAV_CMD_VTOL_LAND;
 		}
 
@@ -543,7 +547,9 @@ Mission::set_mission_items()
 		set_previous_pos_setpoint();
 
 		/* do takeoff before going to setpoint if needed and not already in takeoff */
-		if (do_need_takeoff() && _work_item_type != WORK_ITEM_TYPE_TAKEOFF) {
+		if (do_need_takeoff() &&
+		    _work_item_type == WORK_ITEM_TYPE_DEFAULT &&
+		    new_work_item_type == WORK_ITEM_TYPE_DEFAULT) {
 
 			new_work_item_type = WORK_ITEM_TYPE_TAKEOFF;
 
@@ -566,40 +572,63 @@ Mission::set_mission_items()
 			_mission_item.altitude_is_relative = false;
 			_mission_item.autocontinue = true;
 			_mission_item.time_inside = 0.0f;
-		}
 
-		/* if we just did a takeoff navigate to the actual waypoint now */
+		} else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
-		if (_work_item_type == WORK_ITEM_TYPE_TAKEOFF) {
+			/* if there is no need to do a takeoff but we have a takeoff item, treat is as waypoint */
+			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
+			/* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */
+			_mission_item.yaw = NAN;
 
-			if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF
+		} else if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF) {
-			    && _navigator->get_vstatus()->is_rotary_wing
-			    && !_navigator->get_land_detected()->landed
-			    && has_next_position_item) {
 
-				/* check if the vtol_takeoff command is on top of us */
+			if (_navigator->get_vstatus()->is_rotary_wing) {
-				if (do_need_move_to_takeoff()) {
+				/* haven't transitioned yet, trigger vtol takeoff logic below */
-					new_work_item_type = WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF;
+				_work_item_type = WORK_ITEM_TYPE_TAKEOFF;
-
-				} else {
-					new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
-				}
-
-				_mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
-				_mission_item.params[0] = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW;
-				_mission_item.yaw = _navigator->get_global_position()->yaw;
 
 			} else {
-				new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
+				/* already in fixed-wing, go to waypoint */
 				_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
-				/* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */
-				_mission_item.yaw = NAN;
-				_mission_item.time_inside = 0.0f;
 			}
+
+			/* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */
+			_mission_item.yaw = NAN;
+		}
+
+		/* if we just did a normal takeoff navigate to the actual waypoint now */
+		if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF &&
+		    _work_item_type == WORK_ITEM_TYPE_TAKEOFF &&
+		    new_work_item_type == WORK_ITEM_TYPE_DEFAULT) {
+
+			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
+			/* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */
+			_mission_item.yaw = NAN;
+		}
+
+		/* if we just did a VTOL takeoff, prepare transition */
+		if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF &&
+		    _work_item_type == WORK_ITEM_TYPE_TAKEOFF &&
+		    new_work_item_type == WORK_ITEM_TYPE_DEFAULT &&
+		    _navigator->get_vstatus()->is_rotary_wing &&
+		    !_navigator->get_land_detected()->landed) {
+
+			/* check if the vtol_takeoff waypoint is on top of us */
+			if (do_need_move_to_takeoff()) {
+				new_work_item_type = WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF;
+			}
+
+			_mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
+			_mission_item.params[0] = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW;
+			_mission_item.yaw = _navigator->get_global_position()->yaw;
+
+			/* set position setpoint to target during the transition */
+			// TODO: if has_next_position_item and use_next set next, or if use_heading set generated
+			generate_waypoint_from_heading(&pos_sp_triplet->current, _mission_item.yaw);
 		}
 
 		/* takeoff completed and transitioned, move to takeoff wp as fixed wing */
 		if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF
-		    && _work_item_type == WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF) {
+		    && _work_item_type == WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF
+		    && new_work_item_type == WORK_ITEM_TYPE_DEFAULT) {
 
 			new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
 			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
@@ -610,15 +639,19 @@ Mission::set_mission_items()
 		/* move to land wp as fixed wing */
 		if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND
 		    && _work_item_type == WORK_ITEM_TYPE_DEFAULT
+		    && new_work_item_type == WORK_ITEM_TYPE_DEFAULT
 		    && !_navigator->get_land_detected()->landed) {
 
 			new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND;
+
 			/* use current mission item as next position item */
 			memcpy(&mission_item_next_position, &_mission_item, sizeof(struct mission_item_s));
 			has_next_position_item = true;
+
 			float altitude = _navigator->get_global_position()->alt;
 
-			if (pos_sp_triplet->current.valid) {
+			if (pos_sp_triplet->current.valid
+			    && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
 				altitude = pos_sp_triplet->current.alt;
 			}
 
@@ -632,6 +665,7 @@ Mission::set_mission_items()
 		/* transition to MC */
 		if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND
 		    && _work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND
+		    && new_work_item_type == WORK_ITEM_TYPE_DEFAULT
 		    && !_navigator->get_vstatus()->is_rotary_wing
 		    && !_navigator->get_land_detected()->landed) {
 
@@ -644,7 +678,8 @@ Mission::set_mission_items()
 		/* move to landing waypoint before descent if necessary */
 		if (do_need_move_to_land() &&
 		    (_work_item_type == WORK_ITEM_TYPE_DEFAULT ||
-		     _work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION)) {
+		     _work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION) &&
+		    new_work_item_type == WORK_ITEM_TYPE_DEFAULT) {
 
 			new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND;
 
@@ -661,6 +696,11 @@ Mission::set_mission_items()
 			 */
 			float altitude = _navigator->get_global_position()->alt;
 
+			if (pos_sp_triplet->current.valid
+			    && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
+				altitude = pos_sp_triplet->current.alt;
+			}
+
 			_mission_item.altitude = altitude;
 			_mission_item.altitude_is_relative = false;
 			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
@@ -670,10 +710,11 @@ Mission::set_mission_items()
 		}
 
 		/* we just moved to the landing waypoint, now descend */
-		if (_work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND
+		if (_work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND &&
-		    && _navigator->get_vstatus()->is_rotary_wing) {
+		    new_work_item_type == WORK_ITEM_TYPE_DEFAULT) {
 
 			new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
+			/* XXX: noop */
 		}
 
 		/* ignore yaw for landing items */
@@ -688,19 +729,53 @@ Mission::set_mission_items()
 
 		/* turn towards next waypoint before MC to FW transition */
 		if (_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION
-		    && _work_item_type != WORK_ITEM_TYPE_ALIGN
+		    && _work_item_type == WORK_ITEM_TYPE_DEFAULT
+		    && new_work_item_type == WORK_ITEM_TYPE_DEFAULT
 		    && _navigator->get_vstatus()->is_rotary_wing
 		    && !_navigator->get_land_detected()->landed
 		    && has_next_position_item) {
 
 			new_work_item_type = WORK_ITEM_TYPE_ALIGN;
+
 			set_align_mission_item(&_mission_item, &mission_item_next_position);
+
+			/* set position setpoint to target during the transition */
+			mission_item_to_position_setpoint(&mission_item_next_position, &pos_sp_triplet->current);
 		}
 
 		/* yaw is aligned now */
-		if (_work_item_type == WORK_ITEM_TYPE_ALIGN) {
+		if (_work_item_type == WORK_ITEM_TYPE_ALIGN &&
+		    new_work_item_type == WORK_ITEM_TYPE_DEFAULT) {
+
 			new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
+
+			/* set position setpoint to target during the transition */
 			set_previous_pos_setpoint();
+			generate_waypoint_from_heading(&pos_sp_triplet->current, pos_sp_triplet->current.yaw);
+		}
+
+		/* don't advance mission after FW to MC command */
+		if (_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION
+		    && _work_item_type == WORK_ITEM_TYPE_DEFAULT
+		    && new_work_item_type == WORK_ITEM_TYPE_DEFAULT
+		    && !_navigator->get_vstatus()->is_rotary_wing
+		    && !_navigator->get_land_detected()->landed
+		    && pos_sp_triplet->current.valid) {
+
+			new_work_item_type = WORK_ITEM_TYPE_CMD_BEFORE_MOVE;
+		}
+
+		/* after FW to MC transition finish moving to the waypoint */
+		if (_work_item_type == WORK_ITEM_TYPE_CMD_BEFORE_MOVE &&
+		    new_work_item_type == WORK_ITEM_TYPE_DEFAULT
+		    && pos_sp_triplet->current.valid) {
+
+			new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
+
+			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
+			copy_positon_if_valid(&_mission_item, &pos_sp_triplet->current);
+			_mission_item.autocontinue = true;
+			_mission_item.time_inside = 0;
 		}
 	}
 
@@ -766,24 +841,29 @@ Mission::do_need_takeoff()
 
 		float takeoff_alt = calculate_takeoff_altitude(&_mission_item);
 
-		/* force takeoff if landed (additional protection) */
 		if (_navigator->get_land_detected()->landed) {
+			/* force takeoff if landed (additional protection) */
 			_need_takeoff = true;
 
+		} else if (_navigator->get_global_position()->alt > takeoff_alt - _navigator->get_acceptance_radius()) {
 			/* if in-air and already above takeoff height, don't do takeoff */
-
-		} else if (_navigator->get_global_position()->alt > takeoff_alt) {
 			_need_takeoff = false;
+
+		} else if (_navigator->get_global_position()->alt <= takeoff_alt - _navigator->get_acceptance_radius()
+			&& (_mission_item.nav_cmd == NAV_CMD_TAKEOFF
+			    || _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF)) {
+			/* if in-air but below takeoff height and we have a takeoff item */
+			_need_takeoff = true;
 		}
 
 		/* check if current mission item is one that requires takeoff before */
 		if (_need_takeoff && (
-			    _mission_item.nav_cmd == NAV_CMD_WAYPOINT ||
-			    _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED ||
-			    _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
 			    _mission_item.nav_cmd == NAV_CMD_TAKEOFF ||
-			    _mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT ||
+			    _mission_item.nav_cmd == NAV_CMD_WAYPOINT ||
-			    _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF)) {
+			    _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF ||
+			    _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
+			    _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED ||
+			    _mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH)) {
 
 			_need_takeoff = false;
 			return true;
@@ -825,7 +905,7 @@ Mission::do_need_move_to_takeoff()
 void
 Mission::copy_positon_if_valid(struct mission_item_s *mission_item, struct position_setpoint_s *setpoint)
 {
-	if (setpoint->valid) {
+	if (setpoint->valid && setpoint->type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
 		mission_item->lat = setpoint->lat;
 		mission_item->lon = setpoint->lon;
 		mission_item->altitude = setpoint->alt;
@@ -1261,6 +1341,7 @@ Mission::set_mission_item_reached()
 void
 Mission::set_current_offboard_mission_item()
 {
+	_navigator->get_mission_result()->reached = false;
 	_navigator->get_mission_result()->finished = false;
 	_navigator->get_mission_result()->seq_current = _current_offboard_mission_index;
 	_navigator->set_mission_result_updated();
@@ -1278,7 +1359,6 @@ Mission::set_mission_finished()
 void
 Mission::check_mission_valid(bool force)
 {
-
 	if ((!_home_inited && _navigator->home_position_valid()) || force) {
 
 		dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
@@ -1368,3 +1448,18 @@ Mission::need_to_reset_mission(bool active)
 
 	return false;
 }
+
+void
+Mission::generate_waypoint_from_heading(struct position_setpoint_s *setpoint, float yaw)
+{
+	waypoint_from_heading_and_distance(
+		_navigator->get_global_position()->lat,
+		_navigator->get_global_position()->lon,
+		yaw,
+		1000000.0f,
+		&(setpoint->lat),
+		&(setpoint->lon));
+	setpoint->alt = _navigator->get_global_position()->alt;
+	setpoint->type = position_setpoint_s::SETPOINT_TYPE_POSITION;
+	setpoint->yaw = yaw;
+}

src/modules/navigator/mission.h

@@ -227,6 +227,11 @@ private:
 	 */
 	bool need_to_reset_mission(bool active);
 
+	/**
+	 * Project current location with heading to far away location and fill setpoint.
+	 */
+	void generate_waypoint_from_heading(struct position_setpoint_s *setpoint, float yaw);
+
 	control::BlockParamInt _param_onboard_enabled;
 	control::BlockParamFloat _param_takeoff_alt;
 	control::BlockParamFloat _param_dist_1wp;

src/modules/navigator/mission_block.cpp

@@ -79,7 +79,9 @@ MissionBlock::MissionBlock(Navigator *navigator, const char *name) :
 	_param_yaw_timeout(this, "MIS_YAW_TMT", false),
 	_param_yaw_err(this, "MIS_YAW_ERR", false),
 	_param_vtol_wv_land(this, "VT_WV_LND_EN", false),
-	_param_vtol_wv_loiter(this, "VT_WV_LTR_EN", false)
+	_param_vtol_wv_takeoff(this, "VT_WV_TKO_EN", false),
+	_param_vtol_wv_loiter(this, "VT_WV_LTR_EN", false),
+	_param_force_vtol(this, "NAV_FORCE_VT", false)
 {
 }
 
@@ -131,19 +133,6 @@ MissionBlock::is_mission_item_reached()
 			}
 
 		case NAV_CMD_DO_CHANGE_SPEED:
-			// XXX not differentiating ground and airspeed yet
-			if (_mission_item.params[1] > 0.0f) {
-				_navigator->set_cruising_speed(_mission_item.params[1]);
-			} else {
-				_navigator->set_cruising_speed();
-				/* if no speed target was given try to set throttle */
-				if (_mission_item.params[2] > 0.0f) {
-					_navigator->set_cruising_throttle(_mission_item.params[2] / 100);
-				} else {
-					_navigator->set_cruising_throttle();
-				}
-			}
-
 			return true;
 
 		default:
@@ -498,22 +487,6 @@ MissionBlock::item_contains_position(const struct mission_item_s *item)
 void
 MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *item, struct position_setpoint_s *sp)
 {
-	/* set the correct setpoint for vtol transition */
-
-	if (item->nav_cmd == NAV_CMD_DO_VTOL_TRANSITION && PX4_ISFINITE(item->yaw)
-			&& item->params[0] >= vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW - 0.5f) {
-
-		sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION;
-		waypoint_from_heading_and_distance(_navigator->get_global_position()->lat,
-										   _navigator->get_global_position()->lon,
-										   item->yaw,
-										   1000000.0f,
-										   &sp->lat,
-										   &sp->lon);
-		sp->alt = _navigator->get_global_position()->alt;
-	}
-
-
 	/* don't change the setpoint for non-position items */
 	if (!item_contains_position(item)) {
 		return;
@@ -542,6 +515,9 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
 		sp->pitch_min = item->pitch_min;
 	case NAV_CMD_VTOL_TAKEOFF:
 		sp->type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF;
+		if(_navigator->get_vstatus()->is_vtol && _param_vtol_wv_takeoff.get()){
+			sp->disable_mc_yaw_control = true;
+		}
 		break;
 
 	case NAV_CMD_LAND:
@@ -679,7 +655,8 @@ MissionBlock::set_land_item(struct mission_item_s *item, bool at_current_locatio
 {
 
 	/* VTOL transition to RW before landing */
-	if (_navigator->get_vstatus()->is_vtol && !_navigator->get_vstatus()->is_rotary_wing) {
+	if (_navigator->get_vstatus()->is_vtol && !_navigator->get_vstatus()->is_rotary_wing &&
+			_param_force_vtol.get() == 1) {
 		struct vehicle_command_s cmd = {};
 		cmd.command = NAV_CMD_DO_VTOL_TRANSITION;
 		cmd.param1 = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;

src/modules/navigator/mission_block.h

@@ -148,7 +148,9 @@ protected:
 	control::BlockParamFloat _param_yaw_timeout;
 	control::BlockParamFloat _param_yaw_err;
 	control::BlockParamInt _param_vtol_wv_land;
+	control::BlockParamInt _param_vtol_wv_takeoff;
 	control::BlockParamInt _param_vtol_wv_loiter;
+	control::BlockParamInt _param_force_vtol;
 };
 
 #endif

src/modules/navigator/mission_params.c

@@ -160,15 +160,33 @@ PARAM_DEFINE_FLOAT(MIS_YAW_ERR, 12.0f);
 /**
  * Weather-vane mode landings for missions
  *
- * @boolean
+ * @unit bool
+ * @min 0
+ * @max 1
+ * @increment 1
+ * @decimal 0
  * @group Mission
  */
 PARAM_DEFINE_INT32(VT_WV_LND_EN, 0);
 
 /**
- * Weather-vane mode for loiter mode
+ * Enable weather-vane mode takeoff for missions
  *
- * @boolean
+ * @min 0
+ * @max 1
+ * @decimal 0
+ * @group Mission
+ */
+PARAM_DEFINE_INT32(VT_WV_TKO_EN, 0);
+
+/**
+ * Weather-vane mode for loiter
+ *
+ * @unit bool
+ * @min 0
+ * @max 1
+ * @increment 1
+ * @decimal 0
  * @group Mission
  */
 PARAM_DEFINE_INT32(VT_WV_LTR_EN, 0);

src/modules/navigator/navigation.h

@@ -59,6 +59,7 @@ enum NAV_CMD {
 	NAV_CMD_WAYPOINT = 16,
 	NAV_CMD_LOITER_UNLIMITED = 17,
 	NAV_CMD_LOITER_TIME_LIMIT = 19,
+	NAV_CMD_RETURN_TO_LAUNCH = 20,
 	NAV_CMD_LAND = 21,
 	NAV_CMD_TAKEOFF = 22,
 	NAV_CMD_LOITER_TO_ALT = 31,

src/modules/navigator/navigator.h

@@ -95,7 +95,7 @@ public:
 	 *
 	 * @return		OK on success.
 	 */
-	int		start();
+	int			start();
 
 	/**
 	 * Display the navigator status.
@@ -182,8 +182,21 @@ public:
 
 	/**
 	 * Set the cruising speed
+	 *
+	 * Passing a negative value or leaving the parameter away will reset the cruising speed
+	 * to its default value.
+	 *
+	 * For VTOL: sets cuising speed for current mode only (multirotor or fixed-wing).
+	 *
 	 */
-	void		set_cruising_speed(float speed=-1.0f) { _mission_cruising_speed = speed; }
+	void		set_cruising_speed(float speed=-1.0f);
+
+	/**
+	 * Reset cruising speed to default values
+	 *
+	 * For VTOL: resets both cruising speeds.
+	 */
+	void		reset_cruising_speed();
 
 	/**
 	 * Get the target throttle
@@ -302,7 +315,8 @@ private:
 	control::BlockParamFloat _param_cruising_speed_plane;
 	control::BlockParamFloat _param_cruising_throttle_plane;
 
-	float _mission_cruising_speed;
+	float _mission_cruising_speed_mc;
+	float _mission_cruising_speed_fw;
 	float _mission_throttle;
 
 	/**

src/modules/navigator/navigator_main.cpp

@@ -161,7 +161,8 @@ Navigator::Navigator() :
 	_param_cruising_speed_hover(this, "MPC_XY_CRUISE", false),
 	_param_cruising_speed_plane(this, "FW_AIRSPD_TRIM", false),
 	_param_cruising_throttle_plane(this, "FW_THR_CRUISE", false),
-	_mission_cruising_speed(-1.0f),
+	_mission_cruising_speed_mc(-1.0f),
+	_mission_cruising_speed_fw(-1.0f),
 	_mission_throttle(-1.0f)
 {
 	/* Create a list of our possible navigation types */
@@ -517,7 +518,24 @@ Navigator::task_main()
 				}
 
 			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_PAUSE_CONTINUE) {
-				PX4_INFO("got pause/continue command");
+				warnx("navigator: got pause/continue command");
+
+			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED) {
+				if (cmd.param2 > FLT_EPSILON) {
+					// XXX not differentiating ground and airspeed yet
+					set_cruising_speed(cmd.param2);
+
+				} else {
+					set_cruising_speed();
+
+					/* if no speed target was given try to set throttle */
+					if (cmd.param3 > FLT_EPSILON) {
+						set_cruising_throttle(cmd.param3 / 100);
+
+					} else {
+						set_cruising_throttle();
+					}
+				}
 			}
 		}
 
@@ -738,20 +756,48 @@ float
 Navigator::get_cruising_speed()
 {
 	/* there are three options: The mission-requested cruise speed, or the current hover / plane speed */
-	if (_mission_cruising_speed > 0.0f) {
+	if (_vstatus.is_rotary_wing) {
-		return _mission_cruising_speed;
+		if (_mission_cruising_speed_mc > FLT_EPSILON
-	} else if (_vstatus.is_rotary_wing) {
+		    && _vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) {
-		return _param_cruising_speed_hover.get();
+			return _mission_cruising_speed_mc;
+
+		} else {
+			return _param_cruising_speed_hover.get();
+		}
+
 	} else {
-		return _param_cruising_speed_plane.get();
+		if (_mission_cruising_speed_fw > FLT_EPSILON
+		    && _vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) {
+			return _mission_cruising_speed_fw;
+
+		} else {
+			return _param_cruising_speed_plane.get();
+		}
 	}
 }
 
+void
+Navigator::set_cruising_speed(float speed) {
+	if (_vstatus.is_rotary_wing) {
+		_mission_cruising_speed_mc = speed;
+
+	} else {
+		_mission_cruising_speed_fw = speed;
+	}
+}
+
+void
+Navigator::reset_cruising_speed()
+{
+	_mission_cruising_speed_mc = -1.0f;
+	_mission_cruising_speed_fw = -1.0f;
+}
+
 float
 Navigator::get_cruising_throttle()
 {
 	/* Return the mission-requested cruise speed, or default FW_THR_CRUISE value */
-	if (_mission_throttle > 0.0f) {
+	if (_mission_throttle > FLT_EPSILON) {
 		return _mission_throttle;
 	} else {
 		return _param_cruising_throttle_plane.get();
@@ -882,6 +928,7 @@ Navigator::publish_mission_result()
 	}
 
 	/* reset some of the flags */
+	_mission_result.seq_reached = false;
 	_mission_result.seq_current = 0;
 	_mission_result.item_do_jump_changed = false;
 	_mission_result.item_changed_index = 0;

src/modules/navigator/navigator_params.c

@@ -175,3 +175,12 @@ PARAM_DEFINE_INT32(NAV_AH_LON, 1518423250);
  * @group Data Link Loss
  */
 PARAM_DEFINE_FLOAT(NAV_AH_ALT, 600.0f);
+
+/**
+ * Force VTOL mode takeoff and land
+ *
+ * @min 0
+ * @max 1
+ * @group Mission
+ */
+PARAM_DEFINE_INT32(NAV_FORCE_VT, 1);

src/modules/vtol_att_control/vtol_att_control_params.c

@@ -296,15 +296,6 @@ PARAM_DEFINE_FLOAT(VT_TRANS_TIMEOUT, 15.0f);
  */
 PARAM_DEFINE_FLOAT(VT_TRANS_MIN_TM, 2.0f);
 
-/**
- * Force VTOL mode takeoff and land
- *
- * @min 0
- * @max 1
- * @group VTOL Attitude Control
- */
-PARAM_DEFINE_INT32(VT_NAV_FORCE_VT, 1);
-
 /**
  * QuadChute
  *