Navigator: VTOL: disable weather vane during yaw aligning before front transition

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

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 @@ protected:
 					(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() &&
@@ -754,6 +757,9 @@ Mission::set_mission_items()
 				    _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 */
+				    !_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;
@@ -934,6 +940,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);