FixedWingPositionControl: Use reference model for all auto modes except when in vtol transition

src/modules/fw_pos_control/FixedwingPositionControl.cpp

@@ -52,6 +52,8 @@ FixedwingPositionControl::FixedwingPositionControl(bool vtol) :
 	ModuleParams(nullptr),
 	WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
 	_attitude_sp_pub(vtol ? ORB_ID(fw_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)),
+	_attitude_ref_sp_pub(vtol ? ORB_ID(fw_virtual_vehicle_attitude_reference_setpoint) : ORB_ID(
+				     vehicle_attitude_reference_setpoint)),
 	_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")),
 	_launchDetector(this),
 	_runway_takeoff(this)
@@ -2435,7 +2437,14 @@ FixedwingPositionControl::Run()
 				q.copyTo(_att_sp.q_d);
 
 				_att_sp.timestamp = hrt_absolute_time();
+
+				// Enable reference model use for auto modes except when transitioning.
+				if (_control_mode_current == FW_POSCTRL_MODE_AUTO && !_vehicle_status.in_transition_mode) {
+					_attitude_ref_sp_pub.publish(_att_sp);
+
+				} else {
 					_attitude_sp_pub.publish(_att_sp);
+				}
 
 				// only publish status in full FW mode
 				if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING

src/modules/fw_pos_control/FixedwingPositionControl.hpp

@@ -204,6 +204,7 @@ private:
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
 
 	uORB::Publication<vehicle_attitude_setpoint_s> _attitude_sp_pub;
+	uORB::Publication<vehicle_attitude_setpoint_s> _attitude_ref_sp_pub;
 	uORB::Publication<vehicle_local_position_setpoint_s> _local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)};
 	uORB::Publication<npfg_status_s> _npfg_status_pub{ORB_ID(npfg_status)};
 	uORB::Publication<position_controller_status_s>	_pos_ctrl_status_pub{ORB_ID(position_controller_status)};

src/modules/vtol_att_control/vtol_att_control_main.cpp

@@ -378,6 +378,18 @@ VtolAttitudeControl::Run()
 		// check if mc and fw sp were updated
 		const bool mc_att_sp_updated = _mc_virtual_att_sp_sub.update(&_mc_virtual_att_sp);
 		const bool fw_att_sp_updated = _fw_virtual_att_sp_sub.update(&_fw_virtual_att_sp);
+		const bool fw_att_ref_sp_updated = _fw_virtual_att_ref_sp_sub.update(&_fw_virtual_att_ref_sp);
+
+		if (fw_att_sp_updated) {
+			_is_in_att_ref_mode = false;
+
+		} else if (fw_att_ref_sp_updated) {
+			_is_in_att_ref_mode = true;
+		}
+
+		if (_is_in_att_ref_mode) {
+			_fw_virtual_att_sp = _fw_virtual_att_ref_sp;
+		}
 
 		// update the vtol state machine which decides which mode we are in
 		_vtol_type->update_vtol_state();
@@ -424,6 +436,10 @@ VtolAttitudeControl::Run()
 			if (fw_att_sp_updated) {
 				_vtol_type->update_fw_state();
 				_vehicle_attitude_sp_pub.publish(_vehicle_attitude_sp);
+
+			} else if (fw_att_ref_sp_updated) {
+				_vtol_type->update_fw_state();
+				_vehicle_attitude_ref_sp_pub.publish(_vehicle_attitude_sp);
 			}
 
 			break;

src/modules/vtol_att_control/vtol_att_control_main.h

@@ -159,6 +159,7 @@ private:
 	uORB::Subscription _action_request_sub{ORB_ID(action_request)};
 	uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)};
 	uORB::Subscription _fw_virtual_att_sp_sub{ORB_ID(fw_virtual_attitude_setpoint)};
+	uORB::Subscription _fw_virtual_att_ref_sp_sub{ORB_ID(fw_virtual_vehicle_attitude_reference_setpoint)};
 	uORB::Subscription _home_position_sub{ORB_ID(home_position)};
 	uORB::Subscription _land_detected_sub{ORB_ID(vehicle_land_detected)};
 	uORB::Subscription _local_pos_sp_sub{ORB_ID(vehicle_local_position_setpoint)};
@@ -175,6 +176,7 @@ private:
 	uORB::Publication<normalized_unsigned_setpoint_s>	_flaps_setpoint_pub{ORB_ID(flaps_setpoint)};
 	uORB::Publication<normalized_unsigned_setpoint_s>	_spoilers_setpoint_pub{ORB_ID(spoilers_setpoint)};
 	uORB::Publication<vehicle_attitude_setpoint_s>		_vehicle_attitude_sp_pub{ORB_ID(vehicle_attitude_setpoint)};
+	uORB::Publication<vehicle_attitude_setpoint_s>		_vehicle_attitude_ref_sp_pub{ORB_ID(vehicle_attitude_reference_setpoint)};
 	uORB::PublicationMulti<vehicle_thrust_setpoint_s>	_vehicle_thrust_setpoint0_pub{ORB_ID(vehicle_thrust_setpoint)};
 	uORB::PublicationMulti<vehicle_thrust_setpoint_s>	_vehicle_thrust_setpoint1_pub{ORB_ID(vehicle_thrust_setpoint)};
 	uORB::PublicationMulti<vehicle_torque_setpoint_s>	_vehicle_torque_setpoint0_pub{ORB_ID(vehicle_torque_setpoint)};
@@ -185,6 +187,7 @@ private:
 
 	vehicle_attitude_setpoint_s		_vehicle_attitude_sp{};	// vehicle attitude setpoint
 	vehicle_attitude_setpoint_s 		_fw_virtual_att_sp{};	// virtual fw attitude setpoint
+	vehicle_attitude_setpoint_s 		_fw_virtual_att_ref_sp{};	// virtual fw attitude setpoint
 	vehicle_attitude_setpoint_s 		_mc_virtual_att_sp{};	// virtual mc attitude setpoint
 
 	vehicle_torque_setpoint_s		_vehicle_torque_setpoint_virtual_mc{};
@@ -211,6 +214,8 @@ private:
 
 	float _air_density{atmosphere::kAirDensitySeaLevelStandardAtmos};	// [kg/m^3]
 
+	bool _is_in_att_ref_mode{false};
+
 	hrt_abstime _last_run_timestamp{0};
 
 	/* For multicopters it is usual to have a non-zero idle speed of the engines