VTOL front transition fixes/enhancements in mission mode

msg/position_setpoint.msg

@@ -54,3 +54,5 @@ float32 acceptance_radius   # navigation acceptance_radius if we're doing waypoi
 
 float32 cruising_speed		# the generally desired cruising speed (not a hard constraint)
 float32 cruising_throttle	# the generally desired cruising throttle (not a hard constraint)
+
+bool allow_weather_vane   # VTOL: allow (in mission mode) the weather vane feature that turns the nose into the wind

src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp

@@ -259,6 +259,12 @@ bool FlightTaskAuto::_evaluateTriplets()
 		}
 	}
 
+	if (_ext_yaw_handler != nullptr) {
+		// activation/deactivation of weather vane is based on parameter WV_EN and setting of navigator (allow_weather_vane)
+		(_param_wv_en.get() && _sub_triplet_setpoint->get().current.allow_weather_vane) ?	_ext_yaw_handler->activate() :
+		_ext_yaw_handler->deactivate();
+	}
+
 	// set heading
 	if (_ext_yaw_handler != nullptr && _ext_yaw_handler->is_active()) {
 		_yaw_setpoint = _yaw;

src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp

@@ -120,7 +120,8 @@ class FlightTaskAuto : public FlightTask
 					(ParamInt<px4::params::MPC_YAW_MODE>) _param_mpc_yaw_mode, // defines how heading is executed,
 					(ParamInt<px4::params::COM_OBS_AVOID>) _param_com_obs_avoid, // obstacle avoidance active
 					(ParamFloat<px4::params::MPC_YAWRAUTO_MAX>) _param_mpc_yawrauto_max,
-					(ParamFloat<px4::params::MIS_YAW_ERR>) _param_mis_yaw_err // yaw-error threshold
+					(ParamFloat<px4::params::MIS_YAW_ERR>) _param_mis_yaw_err, // yaw-error threshold
+					(ParamBool<px4::params::WV_EN>) _param_wv_en // enable/disable weather vane (VTOL)
 				       );
 
 private:

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -535,12 +535,14 @@ MulticopterPositionControl::run()
 		if (_wv_controller != nullptr) {
 
 			// in manual mode we just want to use weathervane if position is controlled as well
-			if (_wv_controller->weathervane_enabled() && (!_control_mode.flag_control_manual_enabled
-					|| _control_mode.flag_control_position_enabled)) {
-				_wv_controller->activate();
+			// in mission, enabling wv is done in flight task
+			if (_control_mode.flag_control_manual_enabled) {
+				if (_control_mode.flag_control_position_enabled && _wv_controller->weathervane_enabled()) {
+					_wv_controller->activate();
 
-			} else {
-				_wv_controller->deactivate();
+				} else {
+					_wv_controller->deactivate();
+				}
 			}
 
 			_wv_controller->update(matrix::Quatf(_att_sp.q_d), _states.yaw);

src/modules/navigator/mission.cpp

@@ -673,6 +673,9 @@ Mission::set_mission_items()
 
 				position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 
+				// allow weather vane in mission
+				pos_sp_triplet->current.allow_weather_vane = true;
+
 				/* do takeoff before going to setpoint if needed and not already in takeoff */
 				/* in fixed-wing this whole block will be ignored and a takeoff item is always propagated */
 				if (do_need_vertical_takeoff() &&
@@ -752,6 +755,29 @@ Mission::set_mission_items()
 				    _work_item_type == WORK_ITEM_TYPE_TAKEOFF &&
 				    new_work_item_type == WORK_ITEM_TYPE_DEFAULT &&
 				    _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING &&
+				    !_navigator->get_land_detected()->landed) {
+
+					/* disable weathervane before front transition for allowing yaw to align */
+					pos_sp_triplet->current.allow_weather_vane = false;
+
+					/* set yaw setpoint to heading of VTOL_TAKEOFF wp against current position */
+					_mission_item.yaw = get_bearing_to_next_waypoint(
+								    _navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
+								    _mission_item.lat, _mission_item.lon);
+
+					_mission_item.force_heading = true;
+
+					new_work_item_type = WORK_ITEM_TYPE_ALIGN;
+
+					/* set position setpoint to current while aligning */
+					_mission_item.lat = _navigator->get_global_position()->lat;
+					_mission_item.lon = _navigator->get_global_position()->lon;
+				}
+
+				/* heading is aligned now, prepare transition */
+				if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF &&
+				    _work_item_type == WORK_ITEM_TYPE_ALIGN &&
+				    _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING &&
 				    !_navigator->get_land_detected()->landed) {
 
 					/* check if the vtol_takeoff waypoint is on top of us */
@@ -934,6 +960,9 @@ Mission::set_mission_items()
 				    && !_navigator->get_land_detected()->landed
 				    && has_next_position_item) {
 
+					/* disable weathervane before front transition for allowing yaw to align */
+					pos_sp_triplet->current.allow_weather_vane = false;
+
 					new_work_item_type = WORK_ITEM_TYPE_ALIGN;
 
 					set_align_mission_item(&_mission_item, &mission_item_next_position);

src/modules/navigator/mission_block.cpp

@@ -319,7 +319,7 @@ MissionBlock::is_mission_item_reached()
 			}
 		}
 
-		if (_waypoint_position_reached) {
+		if (_waypoint_position_reached && !_waypoint_position_reached_previously) {
 			// reached just now
 			_time_wp_reached = now;
 		}
@@ -408,6 +408,7 @@ MissionBlock::is_mission_item_reached()
 	}
 
 	// all acceptance criteria must be met in the same iteration
+	_waypoint_position_reached_previously = _waypoint_position_reached;
 	_waypoint_position_reached = false;
 	_waypoint_yaw_reached = false;
 	return false;

src/modules/navigator/mission_block.h

@@ -129,6 +129,7 @@ class MissionBlock : public NavigatorMode
 
 	bool _waypoint_position_reached{false};
 	bool _waypoint_yaw_reached{false};
+	bool _waypoint_position_reached_previously{false};
 
 	hrt_abstime _time_first_inside_orbit{0};
 	hrt_abstime _action_start{0};