Navigator: Do not interpret DO_SET_SPEED as 3D location, reset speed on negative value.

src/modules/navigator/mission_block.cpp

@@ -125,6 +125,8 @@ MissionBlock::is_mission_item_reached()
 			// 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();
 			}
 			return true;
 
@@ -319,7 +321,8 @@ MissionBlock::item_contains_position(const struct mission_item_s *item)
 	if (item->nav_cmd == NAV_CMD_DO_DIGICAM_CONTROL ||
 			item->nav_cmd == NAV_CMD_DO_SET_CAM_TRIGG_DIST ||
 			item->nav_cmd == NAV_CMD_DO_VTOL_TRANSITION ||
-			item->nav_cmd == NAV_CMD_DO_SET_SERVO) {
+			item->nav_cmd == NAV_CMD_DO_SET_SERVO ||
+			item->nav_cmd == NAV_CMD_DO_CHANGE_SPEED) {
 		return false;
 	}
 

src/modules/navigator/mission_feasibility_checker.cpp

@@ -242,12 +242,12 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s
 			missionitem.nav_cmd != NAV_CMD_PATHPLANNING &&
 			missionitem.nav_cmd != NAV_CMD_DO_JUMP &&
 			missionitem.nav_cmd != NAV_CMD_DO_SET_SERVO &&
-			missionitem.nav_cmd != vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED &&
+			missionitem.nav_cmd != NAV_CMD_DO_CHANGE_SPEED &&
 			missionitem.nav_cmd != NAV_CMD_DO_DIGICAM_CONTROL &&
 			missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_DIST &&
 			missionitem.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION) {
 
-			mavlink_log_critical(_mavlink_fd, "Rejecting mission item %i: unsupported action.", (int)(i+1));
+			mavlink_log_critical(_mavlink_fd, "Rejecting mission item %i: unsupported cmd: %d", (int)(i+1), (int)missionitem.nav_cmd);
 			return false;
 		}
 

src/modules/navigator/navigation.h

@@ -62,6 +62,7 @@ enum NAV_CMD {
 	NAV_CMD_ROI = 80,
 	NAV_CMD_PATHPLANNING = 81,
 	NAV_CMD_DO_JUMP = 177,
+	NAV_CMD_DO_CHANGE_SPEED = 178,
 	NAV_CMD_DO_SET_SERVO=183,
 	NAV_CMD_DO_REPEAT_SERVO=184,
 	NAV_CMD_DO_DIGICAM_CONTROL=203,

src/modules/navigator/navigator.h

@@ -166,7 +166,7 @@ public:
 	/**
 	 * Set the cruising speed
 	 */
-	void		set_cruising_speed(float speed) { _mission_cruising_speed = speed; }
+	void		set_cruising_speed(float speed=-1.0f) { _mission_cruising_speed = speed; }
 
 	/**
 	 * Get the acceptance radius given the mission item preset radius

src/modules/navigator/navigator_main.cpp

@@ -154,7 +154,7 @@ Navigator::Navigator() :
 	_param_rcloss_obc(this, "RCL_OBC"),
 	_param_cruising_speed_hover(this, "MPC_XY_VEL_MAX", false),
 	_param_cruising_speed_plane(this, "FW_AIRSPD_TRIM", false),
-	_mission_cruising_speed(0.0f)
+	_mission_cruising_speed(-1.0f)
 {
 	/* Create a list of our possible navigation types */
 	_navigation_mode_array[0] = &_mission;
@@ -622,7 +622,7 @@ 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.001f) {
+	if (_mission_cruising_speed > 0.0f) {
 		return _mission_cruising_speed;
 	} else if (_vstatus.is_rotary_wing) {
 		return _param_cruising_speed_hover.get();