navigator move parameters out of MissionBlock

- MissionBlock is an interface with > 10 implementations

msg/position_setpoint.msg

@@ -14,33 +14,40 @@ uint8 VELOCITY_FRAME_BODY_NED = 8 # MAV_FRAME_BODY_NED
 
 bool valid			# true if setpoint is valid
 uint8 type			# setpoint type to adjust behavior of position controller
+
 float32 x			# local position setpoint in m in NED
 float32 y			# local position setpoint in m in NED
 float32 z			# local position setpoint in m in NED
 bool position_valid		# true if local position setpoint valid
+
 float32 vx			# local velocity setpoint in m/s in NED
 float32 vy			# local velocity setpoint in m/s in NED
 float32 vz			# local velocity setpoint in m/s in NED
 bool velocity_valid		# true if local velocity setpoint valid
 uint8 velocity_frame		# to set velocity setpoints in NED or body
-
 bool alt_valid		# do not set for 3D position control. Set to true if you want z-position control while doing vx,vy velocity control.
+
 float64 lat			# latitude, in deg
 float64 lon			# longitude, in deg
 float32 alt			# altitude AMSL, in m
+
 float32 yaw			# yaw (only for multirotors), in rad [-PI..PI), NaN = hold current yaw
 bool yaw_valid			# true if yaw setpoint valid
-bool disable_mc_yaw_control		# control yaw for mc (used for vtol weather-vane mode)
+
 float32 yawspeed		# yawspeed (only for multirotors, in rad/s)
 bool yawspeed_valid		# true if yawspeed setpoint valid
+
 float32 loiter_radius		# loiter radius (only for fixed wing), in m
 int8 loiter_direction		# loiter direction: 1 = CW, -1 = CCW
 float32 pitch_min		# minimal pitch angle for fixed wing takeoff waypoints
+
 float32 a_x			# acceleration x setpoint
 float32 a_y			# acceleration y setpoint
 float32 a_z			# acceleration z setpoint
 bool acceleration_valid		# true if acceleration setpoint is valid/should be used
 bool acceleration_is_force	# interprete acceleration as force
+
 float32 acceptance_radius   # navigation acceptance_radius if we're doing waypoint navigation
+
 float32 cruising_speed		# the generally desired cruising speed (not a hard constraint)
 float32 cruising_throttle	# the generally desired cruising throttle (not a hard constraint)

src/modules/fw_att_control/fw_att_control_main.cpp

@@ -460,8 +460,6 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 
 	_parameter_handles.rattitude_thres = param_find("FW_RATT_TH");
 
-	_parameter_handles.vtol_type = param_find("VT_TYPE");
-
 	_parameter_handles.bat_scale_en = param_find("FW_BAT_SCALE_EN");
 
 	/* fetch initial parameter values */
@@ -567,7 +565,9 @@ FixedwingAttitudeControl::parameters_update()
 
 	param_get(_parameter_handles.rattitude_thres, &_parameters.rattitude_thres);
 
-	param_get(_parameter_handles.vtol_type, &_parameters.vtol_type);
+	if (_vehicle_status.is_vtol) {
+		param_get(_parameter_handles.vtol_type, &_parameters.vtol_type);
+	}
 
 	param_get(_parameter_handles.bat_scale_en, &_parameters.bat_scale_en);
 
@@ -676,6 +676,10 @@ FixedwingAttitudeControl::vehicle_status_poll()
 				_actuators_id = ORB_ID(actuator_controls_virtual_fw);
 				_attitude_setpoint_id = ORB_ID(fw_virtual_attitude_setpoint);
 
+				_parameter_handles.vtol_type = param_find("VT_TYPE");
+
+				parameters_update();
+
 			} else {
 				_rates_sp_id = ORB_ID(vehicle_rates_setpoint);
 				_actuators_id = ORB_ID(actuator_controls_0);

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -49,7 +49,6 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN");
 	_parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM");
 	_parameter_handles.airspeed_max = param_find("FW_AIRSPD_MAX");
-	_parameter_handles.airspeed_trans = param_find("VT_ARSP_TRANS");
 	_parameter_handles.airspeed_disabled = param_find("FW_ARSP_MODE");
 
 	_parameter_handles.pitch_limit_min = param_find("FW_P_LIM_MIN");
@@ -94,8 +93,6 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_parameter_handles.heightrate_ff =			param_find("FW_T_HRATE_FF");
 	_parameter_handles.speedrate_p =			param_find("FW_T_SRATE_P");
 
-	_parameter_handles.vtol_type = 				param_find("VT_TYPE");
-
 	/* fetch initial parameter values */
 	parameters_update();
 }
@@ -135,7 +132,6 @@ FixedwingPositionControl::parameters_update()
 	param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min));
 	param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim));
 	param_get(_parameter_handles.airspeed_max, &(_parameters.airspeed_max));
-	param_get(_parameter_handles.airspeed_trans, &(_parameters.airspeed_trans));
 	param_get(_parameter_handles.airspeed_disabled, &(_parameters.airspeed_disabled));
 
 	param_get(_parameter_handles.pitch_limit_min, &(_parameters.pitch_limit_min));
@@ -194,7 +190,6 @@ FixedwingPositionControl::parameters_update()
 	param_get(_parameter_handles.land_flare_pitch_max_deg, &(_parameters.land_flare_pitch_max_deg));
 	param_get(_parameter_handles.land_use_terrain_estimate, &(_parameters.land_use_terrain_estimate));
 	param_get(_parameter_handles.land_airspeed_scale, &(_parameters.land_airspeed_scale));
-	param_get(_parameter_handles.vtol_type, &(_parameters.vtol_type));
 
 	_l1_control.set_l1_damping(_parameters.l1_damping);
 	_l1_control.set_l1_period(_parameters.l1_period);
@@ -288,6 +283,11 @@ FixedwingPositionControl::vehicle_status_poll()
 			if (_vehicle_status.is_vtol) {
 				_attitude_setpoint_id = ORB_ID(fw_virtual_attitude_setpoint);
 
+				_parameter_handles.airspeed_trans = param_find("VT_ARSP_TRANS");
+				_parameter_handles.vtol_type = param_find("VT_TYPE");
+
+				parameters_update();
+
 			} else {
 				_attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint);
 			}
@@ -1059,7 +1059,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons
 
 			if (_runway_takeoff.runwayTakeoffEnabled()) {
 				if (!_runway_takeoff.isInitialized()) {
-					_runway_takeoff.init(_yaw, _global_pos.lat, _global_pos.lon);
+					Eulerf euler(Quatf(_att.q));
+					_runway_takeoff.init(euler.psi(), _global_pos.lat, _global_pos.lon);
 
 					/* need this already before takeoff is detected
 					 * doesn't matter if it gets reset when takeoff is detected eventually */

src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c

@@ -382,6 +382,20 @@ PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 10.0f);
  */
 PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 20.0f);
 
+/**
+ * Cruise Airspeed
+ *
+ * The fixed wing controller tries to fly at this airspeed.
+ *
+ * @unit m/s
+ * @min 0.0
+ * @max 40
+ * @decimal 1
+ * @increment 0.5
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 15.0f);
+
 /**
  * Maximum climb rate
  *

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -68,7 +68,6 @@
 #include <systemlib/param/param.h>
 #include <systemlib/perf_counter.h>
 #include <systemlib/systemlib.h>
-#include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/manual_control_setpoint.h>
@@ -134,7 +133,6 @@ private:
 	int		_v_control_mode_sub;	/**< vehicle control mode subscription */
 	int		_params_sub;			/**< parameter updates subscription */
 	int		_manual_control_sp_sub;	/**< manual control setpoint subscription */
-	int		_armed_sub;				/**< arming status subscription */
 	int		_vehicle_status_sub;	/**< vehicle status subscription */
 	int		_motor_limits_sub;		/**< motor limits subscription */
 	int		_battery_status_sub;	/**< battery status subscription */
@@ -155,18 +153,17 @@ private:
 	bool		_actuators_0_circuit_breaker_enabled;	/**< circuit breaker to suppress output */
 
 	struct vehicle_attitude_s		_v_att;			/**< vehicle attitude */
-	struct vehicle_attitude_setpoint_s	_v_att_sp;			/**< vehicle attitude setpoint */
+	struct vehicle_attitude_setpoint_s	_v_att_sp;		/**< vehicle attitude setpoint */
 	struct vehicle_rates_setpoint_s		_v_rates_sp;		/**< vehicle rates setpoint */
 	struct manual_control_setpoint_s	_manual_control_sp;	/**< manual control setpoint */
 	struct vehicle_control_mode_s		_v_control_mode;	/**< vehicle control mode */
-	struct actuator_controls_s			_actuators;			/**< actuator controls */
-	struct actuator_armed_s				_armed;				/**< actuator arming status */
-	struct vehicle_status_s				_vehicle_status;	/**< vehicle status */
-	struct mc_att_ctrl_status_s 		_controller_status; /**< controller status */
-	struct battery_status_s				_battery_status;	/**< battery status */
+	struct actuator_controls_s		_actuators;		/**< actuator controls */
+	struct vehicle_status_s			_vehicle_status;	/**< vehicle status */
+	struct mc_att_ctrl_status_s 		_controller_status;	/**< controller status */
+	struct battery_status_s			_battery_status;	/**< battery status */
 	struct sensor_gyro_s			_sensor_gyro;		/**< gyro data before thermal correctons and ekf bias estimates are applied */
-	struct sensor_correction_s		_sensor_correction;		/**< sensor thermal corrections */
-	struct sensor_bias_s		_sensor_bias;		/**< sensor in-run bias corrections */
+	struct sensor_correction_s		_sensor_correction;	/**< sensor thermal corrections */
+	struct sensor_bias_s			_sensor_bias;		/**< sensor in-run bias corrections */
 
 	MultirotorMixer::saturation_status _saturation_status{};
 
@@ -222,9 +219,10 @@ private:
 		param_t acro_superexpo;
 		param_t rattitude_thres;
 
-		param_t vtol_type;
 		param_t roll_tc;
 		param_t pitch_tc;
+
+		param_t vtol_type;
 		param_t vtol_opt_recovery_enabled;
 		param_t vtol_wv_yaw_rate_scale;
 
@@ -262,7 +260,8 @@ private:
 		float acro_expo;					/**< function parameter for expo stick curve shape */
 		float acro_superexpo;				/**< function parameter for superexpo stick curve shape */
 		float rattitude_thres;
-		int vtol_type;						/**< 0 = Tailsitter, 1 = Tiltrotor, 2 = Standard airframe */
+
+		int32_t vtol_type;						/**< 0 = Tailsitter, 1 = Tiltrotor, 2 = Standard airframe */
 		bool vtol_opt_recovery_enabled;
 		float vtol_wv_yaw_rate_scale;			/**< Scale value [0, 1] for yaw rate setpoint  */
 
@@ -274,42 +273,27 @@ private:
 
 	}		_params;
 
-	TailsitterRecovery *_ts_opt_recovery;	/**< Computes optimal rates for tailsitter recovery */
+	TailsitterRecovery *_ts_opt_recovery{nullptr};	/**< Computes optimal rates for tailsitter recovery */
 
 	/**
 	 * Update our local parameter cache.
 	 */
-	int			parameters_update();
+	void			parameters_update();
 
 	/**
 	 * Check for parameter update and handle it.
 	 */
+	void		battery_status_poll();
 	void		parameter_update_poll();
-
-	/**
-	 * Check for changes in vehicle control mode.
-	 */
+	void		sensor_bias_poll();
+	void		sensor_correction_poll();
+	void		vehicle_attitude_poll();
+	void		vehicle_attitude_setpoint_poll();
 	void		vehicle_control_mode_poll();
-
-	/**
-	 * Check for changes in manual inputs.
-	 */
 	void		vehicle_manual_poll();
-
-	/**
-	 * Check for attitude setpoint updates.
-	 */
-	void		vehicle_attitude_setpoint_poll();
-
-	/**
-	 * Check for rates setpoint updates.
-	 */
+	void		vehicle_motor_limits_poll();
 	void		vehicle_rates_setpoint_poll();
-
-	/**
-	 * Check for arming status updates.
-	 */
-	void		arming_status_poll();
+	void		vehicle_status_poll();
 
 	/**
 	 * Attitude controller.
@@ -326,36 +310,6 @@ private:
 	 */
 	math::Vector<3> pid_attenuations(float tpa_breakpoint, float tpa_rate);
 
-	/**
-	 * Check for vehicle status updates.
-	 */
-	void		vehicle_status_poll();
-
-	/**
-	 * Check for vehicle motor limits status.
-	 */
-	void		vehicle_motor_limits_poll();
-
-	/**
-	 * Check for battery status updates.
-	 */
-	void		battery_status_poll();
-
-	/**
-	 * Check for control state updates.
-	 */
-	void		vehicle_attitude_poll();
-
-	/**
-	 * Check for sensor thermal correction updates.
-	 */
-	void		sensor_correction_poll();
-
-	/**
-	 * Check for sensor in-run bias correction updates.
-	 */
-	void		sensor_bias_poll();
-
 	/**
 	 * Shim for calling task_main from task_create.
 	 */
@@ -384,7 +338,6 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_v_control_mode_sub(-1),
 	_params_sub(-1),
 	_manual_control_sp_sub(-1),
-	_armed_sub(-1),
 	_vehicle_status_sub(-1),
 	_motor_limits_sub(-1),
 	_battery_status_sub(-1),
@@ -410,7 +363,6 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_manual_control_sp{},
 	_v_control_mode{},
 	_actuators{},
-	_armed{},
 	_vehicle_status{},
 	_controller_status{},
 	_battery_status{},
@@ -420,9 +372,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_saturation_status{},
 	/* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")),
-	_controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")),
-	_ts_opt_recovery(nullptr)
-
+	_controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency"))
 {
 	for (uint8_t i = 0; i < MAX_GYRO_COUNT; i++) {
 		_sensor_gyro_sub[i] = -1;
@@ -444,8 +394,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_params.auto_rate_max.zero();
 	_params.acro_rate_max.zero();
 	_params.rattitude_thres = 1.0f;
-	_params.vtol_opt_recovery_enabled = false;
-	_params.vtol_wv_yaw_rate_scale = 1.0f;
+
 	_params.bat_scale_en = 0;
 
 	_params.board_rotation = 0;
@@ -469,19 +418,22 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_params_handles.roll_rate_i		= 	param_find("MC_ROLLRATE_I");
 	_params_handles.roll_rate_integ_lim	= 	param_find("MC_RR_INT_LIM");
 	_params_handles.roll_rate_d		= 	param_find("MC_ROLLRATE_D");
-	_params_handles.roll_rate_ff	= 	param_find("MC_ROLLRATE_FF");
+	_params_handles.roll_rate_ff		= 	param_find("MC_ROLLRATE_FF");
+
 	_params_handles.pitch_p			= 	param_find("MC_PITCH_P");
-	_params_handles.pitch_rate_p	= 	param_find("MC_PITCHRATE_P");
-	_params_handles.pitch_rate_i	= 	param_find("MC_PITCHRATE_I");
+	_params_handles.pitch_rate_p		= 	param_find("MC_PITCHRATE_P");
+	_params_handles.pitch_rate_i		= 	param_find("MC_PITCHRATE_I");
 	_params_handles.pitch_rate_integ_lim	= 	param_find("MC_PR_INT_LIM");
-	_params_handles.pitch_rate_d	= 	param_find("MC_PITCHRATE_D");
-	_params_handles.pitch_rate_ff 	= 	param_find("MC_PITCHRATE_FF");
+	_params_handles.pitch_rate_d		= 	param_find("MC_PITCHRATE_D");
+	_params_handles.pitch_rate_ff 		= 	param_find("MC_PITCHRATE_FF");
+
 	_params_handles.tpa_breakpoint_p 	= 	param_find("MC_TPA_BREAK_P");
 	_params_handles.tpa_breakpoint_i 	= 	param_find("MC_TPA_BREAK_I");
 	_params_handles.tpa_breakpoint_d 	= 	param_find("MC_TPA_BREAK_D");
 	_params_handles.tpa_rate_p	 	= 	param_find("MC_TPA_RATE_P");
 	_params_handles.tpa_rate_i	 	= 	param_find("MC_TPA_RATE_I");
 	_params_handles.tpa_rate_d	 	= 	param_find("MC_TPA_RATE_D");
+
 	_params_handles.yaw_p			=	param_find("MC_YAW_P");
 	_params_handles.yaw_rate_p		= 	param_find("MC_YAWRATE_P");
 	_params_handles.yaw_rate_i		= 	param_find("MC_YAWRATE_I");
@@ -489,41 +441,34 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_params_handles.yaw_rate_d		= 	param_find("MC_YAWRATE_D");
 	_params_handles.yaw_rate_ff	 	= 	param_find("MC_YAWRATE_FF");
 	_params_handles.yaw_ff			= 	param_find("MC_YAW_FF");
-	_params_handles.roll_rate_max	= 	param_find("MC_ROLLRATE_MAX");
-	_params_handles.pitch_rate_max	= 	param_find("MC_PITCHRATE_MAX");
-	_params_handles.yaw_rate_max	= 	param_find("MC_YAWRATE_MAX");
-	_params_handles.yaw_auto_max	= 	param_find("MC_YAWRAUTO_MAX");
-	_params_handles.acro_roll_max	= 	param_find("MC_ACRO_R_MAX");
-	_params_handles.acro_pitch_max	= 	param_find("MC_ACRO_P_MAX");
-	_params_handles.acro_yaw_max	= 	param_find("MC_ACRO_Y_MAX");
-	_params_handles.acro_expo	= 	param_find("MC_ACRO_EXPO");
-	_params_handles.acro_superexpo	= 	param_find("MC_ACRO_SUPEXPO");
-	_params_handles.rattitude_thres = 	param_find("MC_RATT_TH");
-	_params_handles.vtol_type 		= 	param_find("VT_TYPE");
+
+	_params_handles.roll_rate_max		= 	param_find("MC_ROLLRATE_MAX");
+	_params_handles.pitch_rate_max		= 	param_find("MC_PITCHRATE_MAX");
+	_params_handles.yaw_rate_max		= 	param_find("MC_YAWRATE_MAX");
+	_params_handles.yaw_auto_max		= 	param_find("MC_YAWRAUTO_MAX");
+
+	_params_handles.acro_roll_max		= 	param_find("MC_ACRO_R_MAX");
+	_params_handles.acro_pitch_max		= 	param_find("MC_ACRO_P_MAX");
+	_params_handles.acro_yaw_max		= 	param_find("MC_ACRO_Y_MAX");
+
+	_params_handles.rattitude_thres 	= 	param_find("MC_RATT_TH");
+
 	_params_handles.roll_tc			= 	param_find("MC_ROLL_TC");
 	_params_handles.pitch_tc		= 	param_find("MC_PITCH_TC");
-	_params_handles.vtol_opt_recovery_enabled	= param_find("VT_OPT_RECOV_EN");
-	_params_handles.vtol_wv_yaw_rate_scale		= param_find("VT_WV_YAWR_SCL");
-	_params_handles.bat_scale_en		= param_find("MC_BAT_SCALE_EN");
+
+	_params_handles.bat_scale_en		=	param_find("MC_BAT_SCALE_EN");
 
 	/* rotations */
-	_params_handles.board_rotation = param_find("SENS_BOARD_ROT");
+	_params_handles.board_rotation		=	param_find("SENS_BOARD_ROT");
 
 	/* rotation offsets */
-	_params_handles.board_offset[0] = param_find("SENS_BOARD_X_OFF");
-	_params_handles.board_offset[1] = param_find("SENS_BOARD_Y_OFF");
-	_params_handles.board_offset[2] = param_find("SENS_BOARD_Z_OFF");
-
-
+	_params_handles.board_offset[0]		=	param_find("SENS_BOARD_X_OFF");
+	_params_handles.board_offset[1]		=	param_find("SENS_BOARD_Y_OFF");
+	_params_handles.board_offset[2]		=	param_find("SENS_BOARD_Z_OFF");
 
 	/* fetch initial parameter values */
 	parameters_update();
 
-	if (_params.vtol_type == 0 && _params.vtol_opt_recovery_enabled) {
-		// the vehicle is a tailsitter, use optimal recovery control strategy
-		_ts_opt_recovery = new TailsitterRecovery();
-	}
-
 	/* initialize thermal corrections as we might not immediately get a topic update (only non-zero values) */
 	for (unsigned i = 0; i < 3; i++) {
 		// used scale factors to unity
@@ -531,7 +476,6 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 		_sensor_correction.gyro_scale_1[i] = 1.0f;
 		_sensor_correction.gyro_scale_2[i] = 1.0f;
 	}
-
 }
 
 MulticopterAttitudeControl::~MulticopterAttitudeControl()
@@ -555,14 +499,12 @@ MulticopterAttitudeControl::~MulticopterAttitudeControl()
 		} while (_control_task != -1);
 	}
 
-	if (_ts_opt_recovery != nullptr) {
-		delete _ts_opt_recovery;
-	}
+	delete _ts_opt_recovery;
 
 	mc_att_control::g_control = nullptr;
 }
 
-int
+void
 MulticopterAttitudeControl::parameters_update()
 {
 	float v;
@@ -652,13 +594,15 @@ MulticopterAttitudeControl::parameters_update()
 	/* stick deflection needed in rattitude mode to control rates not angles */
 	param_get(_params_handles.rattitude_thres, &_params.rattitude_thres);
 
-	param_get(_params_handles.vtol_type, &_params.vtol_type);
+	if (_vehicle_status.is_vtol) {
+		param_get(_params_handles.vtol_type, &_params.vtol_type);
 
-	int tmp;
-	param_get(_params_handles.vtol_opt_recovery_enabled, &tmp);
-	_params.vtol_opt_recovery_enabled = (bool)tmp;
+		int32_t tmp;
+		param_get(_params_handles.vtol_opt_recovery_enabled, &tmp);
+		_params.vtol_opt_recovery_enabled = (tmp == 1);
 
-	param_get(_params_handles.vtol_wv_yaw_rate_scale, &_params.vtol_wv_yaw_rate_scale);
+		param_get(_params_handles.vtol_wv_yaw_rate_scale, &_params.vtol_wv_yaw_rate_scale);
+	}
 
 	param_get(_params_handles.bat_scale_en, &_params.bat_scale_en);
 
@@ -672,7 +616,6 @@ MulticopterAttitudeControl::parameters_update()
 	param_get(_params_handles.board_offset[1], &(_params.board_offset[1]));
 	param_get(_params_handles.board_offset[2], &(_params.board_offset[2]));
 
-
 	/* get transformation matrix from sensor/board to body frame */
 	get_rot_matrix((enum Rotation)_params.board_rotation, &_board_rotation);
 
@@ -682,8 +625,6 @@ MulticopterAttitudeControl::parameters_update()
 					 M_DEG_TO_RAD_F * _params.board_offset[1],
 					 M_DEG_TO_RAD_F * _params.board_offset[2]);
 	_board_rotation = board_rotation_offset * _board_rotation;
-
-	return OK;
 }
 
 void
@@ -751,18 +692,6 @@ MulticopterAttitudeControl::vehicle_rates_setpoint_poll()
 	}
 }
 
-void
-MulticopterAttitudeControl::arming_status_poll()
-{
-	/* check if there is a new setpoint */
-	bool updated;
-	orb_check(_armed_sub, &updated);
-
-	if (updated) {
-		orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
-	}
-}
-
 void
 MulticopterAttitudeControl::vehicle_status_poll()
 {
@@ -774,11 +703,22 @@ MulticopterAttitudeControl::vehicle_status_poll()
 		orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
 
 		/* set correct uORB ID, depending on if vehicle is VTOL or not */
-		if (!_rates_sp_id) {
+		if (_rates_sp_id == nullptr) {
 			if (_vehicle_status.is_vtol) {
 				_rates_sp_id = ORB_ID(mc_virtual_rates_setpoint);
 				_actuators_id = ORB_ID(actuator_controls_virtual_mc);
 
+				_params_handles.vtol_type = param_find("VT_TYPE");
+				_params_handles.vtol_opt_recovery_enabled = param_find("VT_OPT_RECOV_EN");
+				_params_handles.vtol_wv_yaw_rate_scale = param_find("VT_WV_YAWR_SCL");
+
+				parameters_update();
+
+				if (_params.vtol_type == 0 && _params.vtol_opt_recovery_enabled) {
+					// the vehicle is a tailsitter, use optimal recovery control strategy
+					_ts_opt_recovery = new TailsitterRecovery();
+				}
+
 			} else {
 				_rates_sp_id = ORB_ID(vehicle_rates_setpoint);
 				_actuators_id = ORB_ID(actuator_controls_0);
@@ -970,13 +910,16 @@ MulticopterAttitudeControl::control_attitude(float dt)
 		}
 	}
 
-	/* weather-vane mode, dampen yaw rate */
-	if ((_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_auto_enabled) &&
-	    _v_att_sp.disable_mc_yaw_control == true && !_v_control_mode.flag_control_manual_enabled) {
-		float wv_yaw_rate_max = _params.auto_rate_max(2) * _params.vtol_wv_yaw_rate_scale;
-		_rates_sp(2) = math::constrain(_rates_sp(2), -wv_yaw_rate_max, wv_yaw_rate_max);
-		// prevent integrator winding up in weathervane mode
-		_rates_int(2) = 0.0f;
+	/* VTOL weather-vane mode, dampen yaw rate */
+	if (_vehicle_status.is_vtol && _v_att_sp.disable_mc_yaw_control) {
+		if (_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_auto_enabled) {
+
+			const float wv_yaw_rate_max = _params.auto_rate_max(2) * _params.vtol_wv_yaw_rate_scale;
+			_rates_sp(2) = math::constrain(_rates_sp(2), -wv_yaw_rate_max, wv_yaw_rate_max);
+
+			// prevent integrator winding up in weathervane mode
+			_rates_int(2) = 0.0f;
+		}
 	}
 }
 
@@ -1010,7 +953,7 @@ void
 MulticopterAttitudeControl::control_attitude_rates(float dt)
 {
 	/* reset integral if disarmed */
-	if (!_armed.armed || !_vehicle_status.is_rotary_wing) {
+	if (!_v_control_mode.flag_armed || !_vehicle_status.is_rotary_wing) {
 		_rates_int.zero();
 	}
 
@@ -1124,7 +1067,6 @@ MulticopterAttitudeControl::task_main()
 	_v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 	_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
-	_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
 	_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
 	_motor_limits_sub = orb_subscribe(ORB_ID(multirotor_motor_limits));
 	_battery_status_sub = orb_subscribe(ORB_ID(battery_status));
@@ -1191,7 +1133,6 @@ MulticopterAttitudeControl::task_main()
 			/* check for updates in other topics */
 			parameter_update_poll();
 			vehicle_control_mode_poll();
-			arming_status_poll();
 			vehicle_manual_poll();
 			vehicle_status_poll();
 			vehicle_motor_limits_poll();
@@ -1334,7 +1275,6 @@ MulticopterAttitudeControl::task_main()
 					_thrust_sp = 0.0f;
 					_att_control.zero();
 
-
 					/* publish actuator controls */
 					_actuators.control[0] = 0.0f;
 					_actuators.control[1] = 0.0f;

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -254,7 +254,9 @@ private:
 		float slow_land_alt1;
 		float slow_land_alt2;
 		int32_t alt_mode;
-		int32_t opt_recover;
+
+		bool opt_recover;
+
 		float rc_flt_smp_rate;
 		float rc_flt_cutoff;
 
@@ -528,7 +530,6 @@ MulticopterPositionControl::MulticopterPositionControl() :
 	_params_handles.hold_max_xy = param_find("MPC_HOLD_MAX_XY");
 	_params_handles.hold_max_z = param_find("MPC_HOLD_MAX_Z");
 	_params_handles.alt_mode = param_find("MPC_ALT_MODE");
-	_params_handles.opt_recover = param_find("VT_OPT_RECOV_EN");
 	_params_handles.rc_flt_cutoff = param_find("RC_FLT_CUTOFF");
 	_params_handles.rc_flt_smp_rate = param_find("RC_FLT_SMP_RATE");
 
@@ -655,7 +656,11 @@ MulticopterPositionControl::parameters_update(bool force)
 		param_get(_params_handles.alt_mode, &v_i);
 		_params.alt_mode = v_i;
 
-		param_get(_params_handles.opt_recover, &_params.opt_recover);
+		if (_vehicle_status.is_vtol) {
+			int32_t i = 0;
+			param_get(_params_handles.opt_recover, &i);
+			_params.opt_recover = (i == 1);
+		}
 
 		/* mc attitude control parameters*/
 		/* manual control scale */
@@ -714,6 +719,9 @@ MulticopterPositionControl::poll_subscriptions()
 			if (_vehicle_status.is_vtol) {
 				_attitude_setpoint_id = ORB_ID(mc_virtual_attitude_setpoint);
 
+				_params_handles.opt_recover = param_find("VT_OPT_RECOV_EN");
+				parameters_update(true);
+
 			} else {
 				_attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint);
 			}
@@ -1464,14 +1472,6 @@ MulticopterPositionControl::control_non_manual(float dt)
 		control_auto(dt);
 	}
 
-	/* weather-vane mode for vtol: disable yaw control */
-	if (_vehicle_status.is_vtol) {
-		_att_sp.disable_mc_yaw_control = _pos_sp_triplet.current.disable_mc_yaw_control;
-
-	} else {
-		_att_sp.disable_mc_yaw_control = false;
-	}
-
 	// guard against any bad velocity values
 	bool velocity_valid = PX4_ISFINITE(_pos_sp_triplet.current.vx) &&
 			      PX4_ISFINITE(_pos_sp_triplet.current.vy) &&
@@ -2880,7 +2880,7 @@ MulticopterPositionControl::generate_attitude_setpoint(float dt)
 		_att_sp.yaw_body += euler_sp(2);
 
 		/* only if optimal recovery is not used, modify roll/pitch */
-		if (_params.opt_recover <= 0) {
+		if (!(_vehicle_status.is_vtol && _params.opt_recover)) {
 			// construct attitude setpoint rotation matrix. modify the setpoints for roll
 			// and pitch such that they reflect the user's intention even if a yaw error
 			// (yaw_sp - yaw) is present. In the presence of a yaw error constructing a rotation matrix
@@ -3014,7 +3014,6 @@ MulticopterPositionControl::task_main()
 		/* set default max velocity in xy to vel_max */
 		_vel_max_xy = _params.vel_max_xy;
 
-
 		/* reset flags when landed */
 		if (_vehicle_land_detected.landed) {
 			_reset_pos_sp = true;

src/modules/navigator/follow_target.cpp

@@ -400,3 +400,37 @@ bool FollowTarget::target_position_valid()
 	// need at least 1 continuous data points for position estimate
 	return (_target_updates >= 1);
 }
+
+void
+FollowTarget::set_follow_target_item(struct mission_item_s *item, float min_clearance, follow_target_s &target,
+				     float yaw)
+{
+	if (_navigator->get_land_detected()->landed) {
+		/* landed, don't takeoff, but switch to IDLE mode */
+		item->nav_cmd = NAV_CMD_IDLE;
+
+	} else {
+
+		item->nav_cmd = NAV_CMD_DO_FOLLOW_REPOSITION;
+
+		/* use current target position */
+		item->lat = target.lat;
+		item->lon = target.lon;
+		item->altitude = _navigator->get_home_position()->alt;
+
+		if (min_clearance > 8.0f) {
+			item->altitude += min_clearance;
+
+		} else {
+			item->altitude += 8.0f; // if min clearance is bad set it to 8.0 meters (well above the average height of a person)
+		}
+	}
+
+	item->altitude_is_relative = false;
+	item->yaw = yaw;
+	item->loiter_radius = _navigator->get_loiter_radius();
+	item->acceptance_radius = _navigator->get_acceptance_radius();
+	item->time_inside = 0.0f;
+	item->autocontinue = false;
+	item->origin = ORIGIN_ONBOARD;
+}

src/modules/navigator/follow_target.h

@@ -46,6 +46,7 @@
 #include <lib/mathlib/math/Matrix.hpp>
 #include "navigator_mode.h"
 #include "mission_block.h"
+#include <uORB/topics/follow_target.h>
 
 class FollowTarget : public MissionBlock
 {
@@ -162,4 +163,9 @@ private:
 	void update_position_sp(bool velocity_valid, bool position_valid, float yaw_rate);
 	void update_target_motion();
 	void update_target_velocity();
+
+	/**
+	 * Set follow_target item
+	 */
+	void set_follow_target_item(struct mission_item_s *item, float min_clearance, follow_target_s &target, float yaw);
 };

src/modules/navigator/loiter.cpp

@@ -58,7 +58,6 @@
 
 Loiter::Loiter(Navigator *navigator, const char *name) :
 	MissionBlock(navigator, name),
-	_param_min_alt(this, "MIS_LTRMIN_ALT", false),
 	_param_yawmode(this, "MIS_YAWMODE", false),
 	_loiter_pos_set(false)
 {
@@ -123,7 +122,7 @@ Loiter::set_loiter_position()
 	_loiter_pos_set = true;
 
 	// set current mission item to loiter
-	set_loiter_item(&_mission_item, _param_min_alt.get());
+	set_loiter_item(&_mission_item, _navigator->get_loiter_min_alt());
 
 	// convert mission item to current setpoint
 	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();

src/modules/navigator/loiter.h

@@ -80,7 +80,6 @@ private:
 	 */
 	void set_loiter_position();
 
-	control::BlockParamFloat _param_min_alt;
 	control::BlockParamInt _param_yawmode;
 	bool _loiter_pos_set;
 };

src/modules/navigator/mission.cpp

@@ -66,7 +66,6 @@ Mission::Mission(Navigator *navigator, const char *name) :
 	_param_dist_between_wps(this, "MIS_DIST_WPS", false),
 	_param_altmode(this, "MIS_ALTMODE", false),
 	_param_yawmode(this, "MIS_YAWMODE", false),
-	_param_force_vtol(this, "NAV_FORCE_VT", false),
 	_param_fw_climbout_diff(this, "FW_CLMBOUT_DIFF", false),
 	_missionFeasibilityChecker(navigator)
 {
@@ -545,9 +544,7 @@ Mission::set_mission_items()
 	if (item_contains_position(_mission_item)) {
 
 		/* force vtol land */
-		if (_mission_item.nav_cmd == NAV_CMD_LAND && _param_force_vtol.get() == 1
-		    && !_navigator->get_vstatus()->is_rotary_wing
-		    && _navigator->get_vstatus()->is_vtol) {
+		if (_navigator->force_vtol() && _mission_item.nav_cmd == NAV_CMD_LAND) {
 
 			_mission_item.nav_cmd = NAV_CMD_VTOL_LAND;
 		}
@@ -732,7 +729,6 @@ Mission::set_mission_items()
 			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
 			_mission_item.autocontinue = true;
 			_mission_item.time_inside = 0.0f;
-			_mission_item.disable_mc_yaw = true;
 		}
 
 		/* we just moved to the landing waypoint, now descend */
@@ -1176,7 +1172,7 @@ Mission::do_abort_landing()
 	//  or 2 * FW_CLMBOUT_DIFF above the current altitude
 	float alt_landing = get_absolute_altitude_for_item(_mission_item);
 	float min_climbout = _navigator->get_global_position()->alt + (2 * _param_fw_climbout_diff.get());
-	float alt_sp = math::max(alt_landing + _param_loiter_min_alt.get(), min_climbout);
+	float alt_sp = math::max(alt_landing + _navigator->get_loiter_min_alt(), min_climbout);
 
 	// turn current landing waypoint into an indefinite loiter
 	_mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED;

src/modules/navigator/mission.h

@@ -242,7 +242,6 @@ private:
 	control::BlockParamFloat _param_dist_between_wps;
 	control::BlockParamInt _param_altmode;
 	control::BlockParamInt _param_yawmode;
-	control::BlockParamInt _param_force_vtol;
 	control::BlockParamFloat _param_fw_climbout_diff;
 
 	struct mission_s _onboard_mission {};

src/modules/navigator/mission_block.cpp

@@ -56,13 +56,8 @@
 
 MissionBlock::MissionBlock(Navigator *navigator, const char *name) :
 	NavigatorMode(navigator, name),
-	_param_loiter_min_alt(this, "MIS_LTRMIN_ALT", false),
 	_param_yaw_timeout(this, "MIS_YAW_TMT", false),
 	_param_yaw_err(this, "MIS_YAW_ERR", false),
-	_param_vtol_wv_land(this, "VT_WV_LND_EN", false),
-	_param_vtol_wv_takeoff(this, "VT_WV_TKO_EN", false),
-	_param_vtol_wv_loiter(this, "VT_WV_LTR_EN", false),
-	_param_force_vtol(this, "NAV_FORCE_VT", false),
 	_param_back_trans_dec_mss(this, "VT_B_DEC_MSS", false),
 	_param_reverse_delay(this, "VT_B_REV_DEL", false)
 {
@@ -422,19 +417,21 @@ MissionBlock::issue_command(const mission_item_s &item)
 	}
 
 	if (item.nav_cmd == NAV_CMD_DO_SET_SERVO) {
-		PX4_INFO("do_set_servo command");
+		PX4_INFO("DO_SET_SERVO command");
+
 		// XXX: we should issue a vehicle command and handle this somewhere else
-		_actuators = {};
+		actuator_controls_s actuators = {};
+		actuators.timestamp = hrt_absolute_time();
+
 		// params[0] actuator number to be set 0..5 (corresponds to AUX outputs 1..6)
 		// params[1] new value for selected actuator in ms 900...2000
-		_actuators.control[(int)item.params[0]] = 1.0f / 2000 * -item.params[1];
-		_actuators.timestamp = hrt_absolute_time();
+		actuators.control[(int)item.params[0]] = 1.0f / 2000 * -item.params[1];
 
 		if (_actuator_pub != nullptr) {
-			orb_publish(ORB_ID(actuator_controls_2), _actuator_pub, &_actuators);
+			orb_publish(ORB_ID(actuator_controls_2), _actuator_pub, &actuators);
 
 		} else {
-			_actuator_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators);
+			_actuator_pub = orb_advertise(ORB_ID(actuator_controls_2), &actuators);
 		}
 
 	} else {
@@ -498,7 +495,6 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi
 			    _navigator->get_loiter_radius();
 	sp->loiter_direction = (item.loiter_radius > 0) ? 1 : -1;
 	sp->acceptance_radius = item.acceptance_radius;
-	sp->disable_mc_yaw_control = item.disable_mc_yaw;
 
 	sp->cruising_speed = _navigator->get_cruising_speed();
 	sp->cruising_throttle = _navigator->get_cruising_throttle();
@@ -527,29 +523,19 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi
 
 	case NAV_CMD_VTOL_TAKEOFF:
 		sp->type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF;
-
-		if (_navigator->get_vstatus()->is_vtol && _param_vtol_wv_takeoff.get()) {
-			sp->disable_mc_yaw_control = true;
-		}
-
 		break;
 
 	case NAV_CMD_LAND:
 	case NAV_CMD_VTOL_LAND:
 		sp->type = position_setpoint_s::SETPOINT_TYPE_LAND;
-
-		if (_navigator->get_vstatus()->is_vtol && _param_vtol_wv_land.get()) {
-			sp->disable_mc_yaw_control = true;
-		}
-
 		break;
 
 	case NAV_CMD_LOITER_TO_ALT:
 
 		// initially use current altitude, and switch to mission item altitude once in loiter position
-		if (_param_loiter_min_alt.get() > 0.0f) { // ignore _param_loiter_min_alt if smaller then 0 (-1)
+		if (_navigator->get_loiter_min_alt() > 0.0f) { // ignore _param_loiter_min_alt if smaller then 0 (-1)
 			sp->alt = math::max(_navigator->get_global_position()->alt,
-					    _navigator->get_home_position()->alt + _param_loiter_min_alt.get());
+					    _navigator->get_home_position()->alt + _navigator->get_loiter_min_alt());
 
 		} else {
 			sp->alt = _navigator->get_global_position()->alt;
@@ -559,11 +545,6 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi
 	case NAV_CMD_LOITER_TIME_LIMIT:
 	case NAV_CMD_LOITER_UNLIMITED:
 		sp->type = position_setpoint_s::SETPOINT_TYPE_LOITER;
-
-		if (_navigator->get_vstatus()->is_vtol && _param_vtol_wv_loiter.get()) {
-			sp->disable_mc_yaw_control = true;
-		}
-
 		break;
 
 	default:
@@ -625,40 +606,6 @@ MissionBlock::set_loiter_item(struct mission_item_s *item, float min_clearance)
 	}
 }
 
-void
-MissionBlock::set_follow_target_item(struct mission_item_s *item, float min_clearance, follow_target_s &target,
-				     float yaw)
-{
-	if (_navigator->get_land_detected()->landed) {
-		/* landed, don't takeoff, but switch to IDLE mode */
-		item->nav_cmd = NAV_CMD_IDLE;
-
-	} else {
-
-		item->nav_cmd = NAV_CMD_DO_FOLLOW_REPOSITION;
-
-		/* use current target position */
-		item->lat = target.lat;
-		item->lon = target.lon;
-		item->altitude = _navigator->get_home_position()->alt;
-
-		if (min_clearance > 8.0f) {
-			item->altitude += min_clearance;
-
-		} else {
-			item->altitude += 8.0f; // if min clearance is bad set it to 8.0 meters (well above the average height of a person)
-		}
-	}
-
-	item->altitude_is_relative = false;
-	item->yaw = yaw;
-	item->loiter_radius = _navigator->get_loiter_radius();
-	item->acceptance_radius = _navigator->get_acceptance_radius();
-	item->time_inside = 0.0f;
-	item->autocontinue = false;
-	item->origin = ORIGIN_ONBOARD;
-}
-
 void
 MissionBlock::set_takeoff_item(struct mission_item_s *item, float abs_altitude, float min_pitch)
 {
@@ -681,16 +628,13 @@ MissionBlock::set_takeoff_item(struct mission_item_s *item, float abs_altitude,
 void
 MissionBlock::set_land_item(struct mission_item_s *item, bool at_current_location)
 {
-
 	/* VTOL transition to RW before landing */
-	if (_navigator->get_vstatus()->is_vtol &&
-	    !_navigator->get_vstatus()->is_rotary_wing &&
-	    _param_force_vtol.get() == 1) {
-
-		vehicle_command_s cmd = {};
-		cmd.command = NAV_CMD_DO_VTOL_TRANSITION;
-		cmd.param1 = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
-		_navigator->publish_vehicle_cmd(&cmd);
+	if (_navigator->force_vtol()) {
+
+		vehicle_command_s vcmd = {};
+		vcmd.command = NAV_CMD_DO_VTOL_TRANSITION;
+		vcmd.param1 = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
+		_navigator->publish_vehicle_cmd(&vcmd);
 	}
 
 	/* set the land item */

src/modules/navigator/mission_block.h

@@ -44,8 +44,6 @@
 #include "navigator_mode.h"
 
 #include <navigator/navigation.h>
-#include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/follow_target.h>
 #include <uORB/topics/mission.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_command.h>
@@ -115,11 +113,6 @@ protected:
 	 */
 	void set_idle_item(struct mission_item_s *item);
 
-	/**
-	 * Set follow_target item
-	 */
-	void set_follow_target_item(struct mission_item_s *item, float min_clearance, follow_target_s &target, float yaw);
-
 	/**
 	 * General function used to adjust the mission item based on vehicle specific limitations
 	 */
@@ -138,16 +131,12 @@ protected:
 	hrt_abstime _action_start{0};
 	hrt_abstime _time_wp_reached{0};
 
-	actuator_controls_s _actuators{};
 	orb_advert_t    _actuator_pub{nullptr};
 
-	control::BlockParamFloat _param_loiter_min_alt;
 	control::BlockParamFloat _param_yaw_timeout;
 	control::BlockParamFloat _param_yaw_err;
-	control::BlockParamInt _param_vtol_wv_land;
-	control::BlockParamInt _param_vtol_wv_takeoff;
-	control::BlockParamInt _param_vtol_wv_loiter;
-	control::BlockParamInt _param_force_vtol;
+
+	// VTOL parameters
 	control::BlockParamFloat _param_back_trans_dec_mss;
 	control::BlockParamFloat _param_reverse_delay;
 };

src/modules/navigator/mission_params.c

@@ -174,41 +174,3 @@ PARAM_DEFINE_FLOAT(MIS_YAW_TMT, -1.0f);
  * @group Mission
  */
 PARAM_DEFINE_FLOAT(MIS_YAW_ERR, 12.0f);
-
-/**
- * Weather-vane mode landings for missions
- *
- * @boolean
- * @group Mission
- */
-PARAM_DEFINE_INT32(VT_WV_LND_EN, 0);
-
-/**
- * Enable weather-vane mode takeoff for missions
- *
- * @boolean
- * @group Mission
- */
-PARAM_DEFINE_INT32(VT_WV_TKO_EN, 0);
-
-/**
- * Weather-vane mode for loiter
- *
- * @boolean
- * @group Mission
- */
-PARAM_DEFINE_INT32(VT_WV_LTR_EN, 0);
-
-/**
- * Cruise Airspeed
- *
- * The fixed wing controller tries to fly at this airspeed.
- *
- * @unit m/s
- * @min 0.0
- * @max 40
- * @decimal 1
- * @increment 0.5
- * @group FW TECS
- */
-PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 15.0f);

src/modules/navigator/navigation.h

@@ -141,7 +141,6 @@ struct mission_item_s {
 			 force_heading : 1,				/**< heading needs to be reached */
 			 altitude_is_relative : 1,		/**< true if altitude is relative from start point	*/
 			 autocontinue : 1,				/**< true if next waypoint should follow after this one */
-			 disable_mc_yaw : 1,				/**< weathervane mode */
 			 vtol_back_transition : 1;		/**< part of the vtol back transition sequence */
 	};
 };

src/modules/navigator/navigator.h

@@ -222,6 +222,10 @@ public:
 
 	bool		abort_landing();
 
+	float		get_loiter_min_alt() const { return _param_loiter_min_alt.get(); }
+
+	bool		force_vtol() const { return _vstatus.is_vtol && !_vstatus.is_rotary_wing && _param_force_vtol.get(); }
+
 private:
 
 	bool		_task_should_exit{false};	/**< if true, sensor task should exit */
@@ -292,11 +296,15 @@ private:
 
 	NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE];	/**< array of navigation modes */
 
+	// navigator parameters
 	control::BlockParamFloat _param_loiter_radius;	/**< loiter radius for fixedwing */
-
 	control::BlockParamFloat _param_acceptance_radius;	/**< acceptance for takeoff */
 	control::BlockParamFloat _param_fw_alt_acceptance_radius;	/**< acceptance radius for fixedwing altitude */
 	control::BlockParamFloat _param_mc_alt_acceptance_radius;	/**< acceptance radius for multicopter altitude */
+	control::BlockParamInt _param_force_vtol;	/**< acceptance radius for multicopter altitude */
+
+	// non-navigator parameters
+	control::BlockParamFloat _param_loiter_min_alt;
 
 	float _mission_cruising_speed_mc{-1.0f};
 	float _mission_cruising_speed_fw{-1.0f};

src/modules/navigator/navigator_main.cpp

@@ -46,6 +46,9 @@
 #include "navigator.h"
 
 #include <cfloat>
+#include <sys/ioctl.h>
+#include <sys/stat.h>
+#include <sys/types.h>
 
 #include <dataman/dataman.h>
 #include <drivers/drv_hrt.h>
@@ -55,9 +58,6 @@
 #include <px4_defines.h>
 #include <px4_posix.h>
 #include <px4_tasks.h>
-#include <sys/ioctl.h>
-#include <sys/stat.h>
-#include <sys/types.h>
 #include <systemlib/mavlink_log.h>
 #include <systemlib/systemlib.h>
 #include <uORB/topics/fw_pos_ctrl_status.h>
@@ -96,10 +96,14 @@ Navigator::Navigator() :
 	_engineFailure(this, "EF"),
 	_gpsFailure(this, "GPSF"),
 	_follow_target(this, "TAR"),
+	// navigator params
 	_param_loiter_radius(this, "LOITER_RAD"),
 	_param_acceptance_radius(this, "ACC_RAD"),
 	_param_fw_alt_acceptance_radius(this, "FW_ALT_RAD"),
-	_param_mc_alt_acceptance_radius(this, "MC_ALT_RAD")
+	_param_mc_alt_acceptance_radius(this, "MC_ALT_RAD"),
+	_param_force_vtol(this, "FORCE_VT"),
+	// non-navigator params
+	_param_loiter_min_alt(this, "MIS_LTRMIN_ALT", false)
 {
 	/* Create a list of our possible navigation types */
 	_navigation_mode_array[0] = &_mission;

src/modules/navigator/rtl.cpp

@@ -54,7 +54,6 @@ RTL::RTL(Navigator *navigator, const char *name) :
 	MissionBlock(navigator, name),
 	_rtl_state(RTL_STATE_NONE),
 	_param_return_alt(this, "RTL_RETURN_ALT", false),
-	_param_min_loiter_alt(this, "MIS_LTRMIN_ALT", false),
 	_param_descend_alt(this, "RTL_DESCEND_ALT", false),
 	_param_land_delay(this, "RTL_LAND_DELAY", false),
 	_param_rtl_min_dist(this, "RTL_MIN_DIST", false)

src/modules/navigator/rtl.h

@@ -92,7 +92,6 @@ private:
 	} _rtl_state{RTL_STATE_NONE};
 
 	control::BlockParamFloat _param_return_alt;
-	control::BlockParamFloat _param_min_loiter_alt;
 	control::BlockParamFloat _param_descend_alt;
 	control::BlockParamFloat _param_land_delay;
 	control::BlockParamFloat _param_rtl_min_dist;

src/modules/vtol_att_control/standard.cpp

@@ -426,7 +426,6 @@ void Standard::update_mc_state()
 		return;
 	}
 
-
 	matrix::Dcmf R(matrix::Quatf(_v_att->q));
 	matrix::Dcmf R_sp(matrix::Quatf(_v_att_sp->q_d));
 	matrix::Eulerf euler(R);
@@ -543,7 +542,7 @@ void Standard::fill_actuator_outputs()
 	}
 
 	// set the fixed wing throttle control
-	if (_vtol_schedule.flight_mode == FW_MODE && _armed->armed) {
+	if (_vtol_schedule.flight_mode == FW_MODE) {
 
 		// take the throttle value commanded by the fw controller
 		_actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] =

src/modules/vtol_att_control/vtol_att_control_main.cpp

@@ -74,7 +74,6 @@ VtolAttitudeControl::VtolAttitudeControl() :
 	_v_control_mode_sub(-1),
 	_params_sub(-1),
 	_manual_control_sp_sub(-1),
-	_armed_sub(-1),
 	_local_pos_sub(-1),
 	_pos_sp_triplet_sub(-1),
 	_airspeed_sub(-1),
@@ -108,7 +107,6 @@ VtolAttitudeControl::VtolAttitudeControl() :
 	memset(&_actuators_out_1, 0, sizeof(_actuators_out_1));
 	memset(&_actuators_mc_in, 0, sizeof(_actuators_mc_in));
 	memset(&_actuators_fw_in, 0, sizeof(_actuators_fw_in));
-	memset(&_armed, 0, sizeof(_armed));
 	memset(&_local_pos, 0, sizeof(_local_pos));
 	memset(&_pos_sp_triplet, 0, sizeof(_pos_sp_triplet));
 	memset(&_airspeed, 0, sizeof(_airspeed));
@@ -139,6 +137,10 @@ VtolAttitudeControl::VtolAttitudeControl() :
 	_params_handles.front_trans_time_openloop = param_find("VT_F_TR_OL_TM");
 	_params_handles.front_trans_time_min = param_find("VT_TRANS_MIN_TM");
 
+	_params_handles.wv_takeoff = param_find("VT_WV_TKO_EN");
+	_params_handles.wv_land = param_find("VT_WV_LND_EN");
+	_params_handles.wv_loiter = param_find("VT_WV_LTR_EN");
+
 	/* fetch initial parameter values */
 	parameters_update();
 
@@ -217,19 +219,6 @@ void VtolAttitudeControl::vehicle_manual_poll()
 		orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp);
 	}
 }
-/**
-* Check for arming status updates.
-*/
-void VtolAttitudeControl::arming_status_poll()
-{
-	/* check if there is a new setpoint */
-	bool updated;
-	orb_check(_armed_sub, &updated);
-
-	if (updated) {
-		orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
-	}
-}
 
 /**
 * Check for inputs from mc attitude controller.
@@ -601,6 +590,16 @@ VtolAttitudeControl::parameters_update()
 	_params.front_trans_time_min = math::min(_params.front_trans_time_openloop * 0.9f,
 				       _params.front_trans_time_min);
 
+	/* weathervane */
+	param_get(_params_handles.wv_takeoff, &l);
+	_params.wv_takeoff = (l == 1);
+
+	param_get(_params_handles.wv_loiter, &l);
+	_params.wv_loiter = (l == 1);
+
+	param_get(_params_handles.wv_land, &l);
+	_params.wv_land = (l == 1);
+
 	// update the parameters of the instances of base VtolType
 	if (_vtol_type != nullptr) {
 		_vtol_type->parameters_update();
@@ -666,7 +665,6 @@ void VtolAttitudeControl::task_main()
 	_v_control_mode_sub    = orb_subscribe(ORB_ID(vehicle_control_mode));
 	_params_sub            = orb_subscribe(ORB_ID(parameter_update));
 	_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
-	_armed_sub             = orb_subscribe(ORB_ID(actuator_armed));
 	_local_pos_sub         = orb_subscribe(ORB_ID(vehicle_local_position));
 	_pos_sp_triplet_sub    = orb_subscribe(ORB_ID(position_setpoint_triplet));
 	_airspeed_sub          = orb_subscribe(ORB_ID(airspeed));
@@ -748,7 +746,6 @@ void VtolAttitudeControl::task_main()
 		fw_virtual_att_sp_poll();
 		vehicle_control_mode_poll();	//Check for changes in vehicle control mode.
 		vehicle_manual_poll();			//Check for changes in manual inputs.
-		arming_status_poll();			//Check for arming status updates.
 		vehicle_attitude_setpoint_poll();//Check for changes in attitude set points
 		vehicle_attitude_poll();		//Check for changes in attitude
 		actuator_controls_mc_poll();	//Check for changes in mc_attitude_control output

src/modules/vtol_att_control/vtol_att_control_main.h

@@ -63,7 +63,6 @@
 #include <systemlib/param/param.h>
 #include <systemlib/systemlib.h>
 
-#include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/battery_status.h>
@@ -114,7 +113,6 @@ public:
 	struct actuator_controls_s 			*get_actuators_out1() {return &_actuators_out_1;}
 	struct actuator_controls_s 			*get_actuators_mc_in() {return &_actuators_mc_in;}
 	struct actuator_controls_s 			*get_actuators_fw_in() {return &_actuators_fw_in;}
-	struct actuator_armed_s 			*get_armed() {return &_armed;}
 	struct vehicle_local_position_s 		*get_local_pos() {return &_local_pos;}
 	struct position_setpoint_triplet_s		*get_pos_sp_triplet() {return &_pos_sp_triplet;}
 	struct airspeed_s 				*get_airspeed() {return &_airspeed;}
@@ -141,7 +139,6 @@ private:
 	int	_v_control_mode_sub;	//vehicle control mode subscription
 	int	_params_sub;			//parameter updates subscription
 	int	_manual_control_sp_sub;	//manual control setpoint subscription
-	int	_armed_sub;				//arming status subscription
 	int	_local_pos_sub;			// sensor subscription
 	int	_pos_sp_triplet_sub;			// local position setpoint subscription
 	int	_airspeed_sub;			// airspeed subscription
@@ -175,7 +172,6 @@ private:
 	struct actuator_controls_s			_actuators_out_1;	//actuator controls going to the fw mixer (used for elevons)
 	struct actuator_controls_s			_actuators_mc_in;	//actuator controls from mc_att_control
 	struct actuator_controls_s			_actuators_fw_in;	//actuator controls from fw_att_control
-	struct actuator_armed_s				_armed;				//actuator arming status
 	struct vehicle_local_position_s			_local_pos;
 	struct position_setpoint_triplet_s		_pos_sp_triplet;
 	struct airspeed_s 				_airspeed;			// airspeed
@@ -204,6 +200,9 @@ private:
 		param_t fw_qc_max_roll;
 		param_t front_trans_time_openloop;
 		param_t front_trans_time_min;
+		param_t wv_takeoff;
+		param_t wv_loiter;
+		param_t wv_land;
 	} _params_handles;
 
 	/* for multicopters it is usual to have a non-zero idle speed of the engines
@@ -221,7 +220,6 @@ private:
 
 	void		vehicle_control_mode_poll();	//Check for changes in vehicle control mode.
 	void		vehicle_manual_poll();			//Check for changes in manual inputs.
-	void		arming_status_poll();			//Check for arming status updates.
 	void 		mc_virtual_att_sp_poll();
 	void 		fw_virtual_att_sp_poll();
 	void 		actuator_controls_mc_poll();	//Check for changes in mc_attitude_control output

src/modules/vtol_att_control/vtol_att_control_params.c

@@ -271,20 +271,6 @@ PARAM_DEFINE_FLOAT(VT_ARSP_TRANS, 10.0f);
  */
 PARAM_DEFINE_INT32(VT_OPT_RECOV_EN, 0);
 
-/**
- * Weather-vane yaw rate scale.
- *
- * The desired yawrate from the controller will be scaled in order to avoid
- * yaw fighting against the wind.
- *
- * @min 0.0
- * @max 1.0
- * @increment 0.01
- * @decimal 3
- * @group VTOL Attitude Control
- */
-PARAM_DEFINE_FLOAT(VT_WV_YAWR_SCL, 0.15f);
-
 /**
  * Front transition timeout
  *
@@ -356,3 +342,41 @@ PARAM_DEFINE_INT32(VT_FW_QC_R, 0);
  * @group VTOL Attitude Control
  */
 PARAM_DEFINE_FLOAT(VT_F_TR_OL_TM, 6.0f);
+
+/**
+ * Weather-vane yaw rate scale.
+ *
+ * The desired yawrate from the controller will be scaled in order to avoid
+ * yaw fighting against the wind.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @increment 0.01
+ * @decimal 3
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_WV_YAWR_SCL, 0.15f);
+
+/**
+ * Enable weather-vane mode takeoff for missions
+ *
+ * @boolean
+ * @group Mission
+ */
+PARAM_DEFINE_INT32(VT_WV_TKO_EN, 0);
+
+/**
+ * Weather-vane mode for loiter
+ *
+ * @boolean
+ * @group Mission
+ */
+PARAM_DEFINE_INT32(VT_WV_LTR_EN, 0);
+
+/**
+ * Weather-vane mode landings for missions
+ *
+ * @boolean
+ * @group Mission
+ */
+PARAM_DEFINE_INT32(VT_WV_LND_EN, 0);

src/modules/vtol_att_control/vtol_type.cpp

@@ -64,7 +64,6 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) :
 	_actuators_out_1 = _attc->get_actuators_out1();
 	_actuators_mc_in = _attc->get_actuators_mc_in();
 	_actuators_fw_in = _attc->get_actuators_fw_in();
-	_armed = _attc->get_armed();
 	_local_pos = _attc->get_local_pos();
 	_airspeed = _attc->get_airspeed();
 	_batt_status = _attc->get_batt_status();
@@ -153,6 +152,24 @@ void VtolType::update_mc_state()
 	_mc_roll_weight = 1.0f;
 	_mc_pitch_weight = 1.0f;
 	_mc_yaw_weight = 1.0f;
+
+	// VTOL weathervane
+	_v_att_sp->disable_mc_yaw_control = false;
+
+	if (_attc->get_pos_sp_triplet()->current.valid &&
+	    !_v_control_mode->flag_control_manual_enabled) {
+
+		if (_params->wv_takeoff && _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
+			_v_att_sp->disable_mc_yaw_control = true;
+
+		} else if (_params->wv_loiter
+			   && _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
+			_v_att_sp->disable_mc_yaw_control = true;
+
+		} else if (_params->wv_land && _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
+			_v_att_sp->disable_mc_yaw_control = true;
+		}
+	}
 }
 
 void VtolType::update_fw_state()
@@ -190,13 +207,13 @@ void VtolType::update_transition_state()
 
 bool VtolType::can_transition_on_ground()
 {
-	return !_armed->armed || _land_detected->landed;
+	return !_v_control_mode->flag_armed || _land_detected->landed;
 }
 
 void VtolType::check_quadchute_condition()
 {
 
-	if (_armed->armed && !_land_detected->landed) {
+	if (_v_control_mode->flag_armed && !_land_detected->landed) {
 		matrix::Eulerf euler = matrix::Quatf(_v_att->q);
 
 		// fixed-wing minimum altitude

src/modules/vtol_att_control/vtol_type.h

@@ -47,9 +47,9 @@
 #include <drivers/drv_hrt.h>
 
 struct Params {
-	int idle_pwm_mc;			// pwm value for idle in mc mode
-	int vtol_motor_count;		// number of motors
-	int vtol_fw_permanent_stab;	// in fw mode stabilize attitude also in manual mode
+	int32_t idle_pwm_mc;			// pwm value for idle in mc mode
+	int32_t vtol_motor_count;		// number of motors
+	int32_t vtol_fw_permanent_stab;	// in fw mode stabilize attitude also in manual mode
 	float mc_airspeed_min;		// min airspeed in multicoper mode (including prop-wash)
 	float mc_airspeed_trim;		// trim airspeed in multicopter mode
 	float mc_airspeed_max;		// max airpseed in multicopter mode
@@ -57,13 +57,16 @@ struct Params {
 	float power_max;			// maximum power of one engine
 	float prop_eff;				// factor to calculate prop efficiency
 	float arsp_lp_gain;			// total airspeed estimate low pass gain
-	int vtol_type;
-	int elevons_mc_lock;		// lock elevons in multicopter mode
+	int32_t vtol_type;
+	int32_t elevons_mc_lock;		// lock elevons in multicopter mode
 	float fw_min_alt;			// minimum relative altitude for FW mode (QuadChute)
 	float fw_qc_max_pitch;		// maximum pitch angle FW mode (QuadChute)
 	float fw_qc_max_roll;		// maximum roll angle FW mode (QuadChute)
 	float front_trans_time_openloop;
 	float front_trans_time_min;
+	bool wv_takeoff;
+	bool wv_loiter;
+	bool wv_land;
 };
 
 // Has to match 1:1 msg/vtol_vehicle_status.msg
@@ -164,7 +167,6 @@ protected:
 	struct actuator_controls_s			*_actuators_out_1;			//actuator controls going to the fw mixer (used for elevons)
 	struct actuator_controls_s			*_actuators_mc_in;			//actuator controls from mc_att_control
 	struct actuator_controls_s			*_actuators_fw_in;			//actuator controls from fw_att_control
-	struct actuator_armed_s				*_armed;					//actuator arming status
 	struct vehicle_local_position_s			*_local_pos;
 	struct airspeed_s 				*_airspeed;					// airspeed
 	struct battery_status_s 			*_batt_status; 				// battery status