Workitem handling for land

src/modules/navigator/mission.cpp

@@ -432,6 +432,9 @@ Mission::set_mission_items()
 	/*********************************** handle mission item *********************************************/
 
 	/* handle position mission items */
+
+	printf("NAV_CMD %.2f workitem: %2f \n", (double)_mission_item.nav_cmd, (double)_work_item_type);
+
 	if (item_contains_position(&_mission_item)) {
 
 		/* we have a new position item so set previous position setpoint to current */
@@ -464,47 +467,64 @@ Mission::set_mission_items()
 		/* if we just did a takeoff navigate to the actual waypoint now */
 		if (_work_item_type == WORK_ITEM_TYPE_TAKEOFF) {
 
-			if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF) {
+			if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF
+					&& _navigator->get_vstatus()->is_rotary_wing
+					&& !_navigator->get_vstatus()->condition_landed
+					&& has_next_position_item) {
+				_mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
+				_mission_item.params[0] = vehicle_status_s::VEHICLE_VTOL_STATE_FW;
 				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;
+				/* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */
+				_mission_item.yaw = NAN;
 			}
-			
-			_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
-				&& _work_item_type == WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF
-				&& _navigator->get_vstatus()->is_rotary_wing
-				&& !_navigator->get_vstatus()->condition_landed
-				&& has_next_position_item) {
 
-			new_work_item_type = WORK_ITEM_TYPE_ALIGN;
-			set_align_mission_item(&_mission_item, &mission_item_next_position);
 		}
 
-		if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF
+		if (_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION
-				&& _work_item_type == WORK_ITEM_TYPE_ALIGN) {
+				&& _work_item_type == WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF) {
 
-			_mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
-			_mission_item.params[0] = vehicle_status_s::VEHICLE_VTOL_STATE_FW;
 			new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
+			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
 		}
 
-		if(_mission_item.nav_cmd == NAV_CMD_VTOL_LAND ||
+			if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND
-				(_mission_item.nav_cmd == NAV_CMD_LAND && _param_force_vtol.get())){
+					&& _work_item_type == WORK_ITEM_TYPE_DEFAULT
+					&& !_navigator->get_vstatus()->condition_landed) {
+				printf("vt move to land \n");
+				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) {
+					altitude = pos_sp_triplet->current.alt;
+				}
 
-			_mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
+				_mission_item.altitude = altitude;
-			_mission_item.params[0] = vehicle_status_s::VEHICLE_VTOL_STATE_MC;
+				_mission_item.altitude_is_relative = false;
-			new_work_item_type = WORK_ITEM_TYPE_TRANSITON_BEFORE_LAND;
+				_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
-		}
+				_mission_item.autocontinue = true;
+				_mission_item.time_inside = 0;
+			}
+
+			if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND
+					&& _work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND
+					&& !_navigator->get_vstatus()->is_rotary_wing
+					&& !_navigator->get_vstatus()->condition_landed) {
+				printf("vt transition \n");
+				_mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
+				_mission_item.params[0] = vehicle_status_s::VEHICLE_VTOL_STATE_MC;
+				_mission_item.autocontinue = true;
+				new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION;
+			}
 
 		/* move to landing waypoint before descent if necessary */
-		if (do_need_move_to_land() && _work_item_type != WORK_ITEM_TYPE_MOVE_TO_LAND) {
+		if (do_need_move_to_land() && (_work_item_type == WORK_ITEM_TYPE_DEFAULT || _work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION)) {
 			new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND;
+			printf("doneedmovetoland \n");
 
 			/* use current mission item as next position item */
 			memcpy(&mission_item_next_position, &_mission_item, sizeof(struct mission_item_s));
@@ -530,10 +550,14 @@ 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) {
+			printf("move to land complete \n");
 			new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
 		}
 
+
+
 		/* ignore yaw for landing items */
 		/* XXX: if specified heading for landing is desired we could add another step before the descent
 		 * that aligns the vehicle first */
@@ -543,15 +567,16 @@ Mission::set_mission_items()
 
 	/* handle non-position mission items such as commands */
 	} else {
+		printf("not a position item \n");
 
 		/* 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_TRANSITON_AFTER_TAKEOFF
 				&& _navigator->get_vstatus()->is_rotary_wing
 				&& !_navigator->get_vstatus()->condition_landed
 				&& has_next_position_item) {
 
-			new_work_item_type = WORK_ITEM_TYPE_ALIGN;
+			new_work_item_type = WORK_ITEM_TYPE_ALIGN;			
 			set_align_mission_item(&_mission_item, &mission_item_next_position);
 		}
 
@@ -672,7 +697,7 @@ Mission::do_need_takeoff()
 bool
 Mission::do_need_move_to_land()
 {
-	if (_navigator->get_vstatus()->is_rotary_wing && _mission_item.nav_cmd == NAV_CMD_LAND) {
+	if (_navigator->get_vstatus()->is_rotary_wing && (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND)) {
 
 		float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon,
 			_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
@@ -958,14 +983,14 @@ Mission::read_mission_item(bool onboard, int offset, struct mission_item_s *miss
 					(mission_item_tmp.do_jump_current_count)++;
 					/* save repeat count */
 					if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_POWER_ON_RESET,
-					    &mission_item_tmp, len) != len) {
+						&mission_item_tmp, len) != len) {
 						/* not supposed to happen unless the datamanager can't access the
 						 * dataman */
 						mavlink_log_critical(_navigator->get_mavlink_log_pub(), "ERROR DO JUMP waypoint could not be written");
 						return false;
 					}
 					report_do_jump_mission_changed(*mission_index_ptr,
-								       mission_item_tmp.do_jump_repeat_count);
+									   mission_item_tmp.do_jump_repeat_count);
 				}
 				/* set new mission item index and repeat
 				* we don't have to validate here, if it's invalid, we should realize this later .*/

src/modules/navigator/mission.h

@@ -258,7 +258,8 @@ private:
 		WORK_ITEM_TYPE_ALIGN,		/**< align for next waypoint */
 		WORK_ITEM_TYPE_CMD_BEFORE_MOVE,	/**<  */
 		WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF,	/**<  */
-		WORK_ITEM_TYPE_TRANSITON_BEFORE_LAND	/**<  */
+		WORK_ITEM_TYPE_TRANSITON_BEFORE_LAND,	/**<  */
+		WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION	/**<  */
 	} _work_item_type;	/**< current type of work to do (sub mission item) */
 
 };