Sanitize vtol_takeoff behavior

src/modules/navigator/mission.cpp

@@ -476,10 +476,17 @@ Mission::set_mission_items()
 					&& _navigator->get_vstatus()->is_rotary_wing
 					&& !_navigator->get_vstatus()->condition_landed
 					&& has_next_position_item) {
+
+				if(do_need_move_to_takeoff()){
+					new_work_item_type = WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF;
+				} else {
+					new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
+				}
+
+
 				_mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
 				_mission_item.params[0] = vehicle_status_s::VEHICLE_VTOL_STATE_FW;
 				_mission_item.yaw = NAN;
-				new_work_item_type = WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF;
 			} else {
 				new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
 				_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
@@ -489,11 +496,13 @@ Mission::set_mission_items()
 
 		}
 
-		if (_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION
+		if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF
 				&& _work_item_type == WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF) {
 
 			new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
 			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
+			_mission_item.autocontinue = true;
+			_mission_item.time_inside = 0.0f;
 		}
 
 			if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND
@@ -586,28 +595,6 @@ Mission::set_mission_items()
 			new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
 		}
 
-		/* 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_CMD_BEFORE_MOVE
-				&& !_navigator->get_vstatus()->is_rotary_wing
-				&& !_navigator->get_vstatus()->condition_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
-				&& 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;
-		}
-
 	}
 
 	/*********************************** set setpoints and check next *********************************************/
@@ -709,6 +696,20 @@ Mission::do_need_move_to_land()
 	return false;
 }
 
+bool
+Mission::do_need_move_to_takeoff()
+{
+	if (_navigator->get_vstatus()->is_rotary_wing && _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF) {
+
+		float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon,
+			_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
+
+		return d_current > _navigator->get_acceptance_radius();
+	}
+
+	return false;
+}
+
 void
 Mission::copy_positon_if_valid(struct mission_item_s *mission_item, struct position_setpoint_s *setpoint)
 {

src/modules/navigator/mission.h

@@ -129,6 +129,11 @@ private:
 	 */
 	bool do_need_move_to_land();
 
+	/**
+	 * Returns true if we need to move to waypoint location after vtol takeoff
+	 */
+	bool do_need_move_to_takeoff();
+
 	/**
 	 * Copies position from setpoint if valid, otherwise copies current position
 	 */