feat(FWModeManager): runway takeoff set yaw setpoint to bearing to takeoff waypoint

msg/FixedWingRunwayControl.msg

@@ -12,4 +12,5 @@ uint8 STATE_FLYING = 3			# navigate freely
 uint8 runway_takeoff_state		# Current state of runway takeoff state machine
 
 bool wheel_steering_enabled		# Flag that enables the wheel steering.
+float32 wheel_steering_yaw_setpoint	# [rad] [@range -pi, pi] For the wheel controller to track. NAN means hold the heading captured when steering engaged.
 float32 wheel_steering_nudging_rate	# [norm] [@range -1, 1] [FRD] Manual wheel nudging, added to controller output. NAN is interpreted as 0.

src/modules/fw_att_control/FixedwingAttitudeControl.cpp

@@ -359,8 +359,12 @@ void FixedwingAttitudeControl::Run()
 
 			}
 
-			// set now yaw setpoint once we're entering the first time
-			if (!PX4_ISFINITE(_steering_wheel_yaw_setpoint)) {
+			// track the heading commanded by the mode manager (e.g. the runway bearing towards the
+			// takeoff waypoint); if none is provided, hold the heading captured when steering engaged
+			if (PX4_ISFINITE(runway_control.wheel_steering_yaw_setpoint)) {
+				_steering_wheel_yaw_setpoint = runway_control.wheel_steering_yaw_setpoint;
+
+			} else if (!PX4_ISFINITE(_steering_wheel_yaw_setpoint)) {
 				_steering_wheel_yaw_setpoint = euler_angles.psi();
 			}
 

src/modules/fw_mode_manager/FixedWingModeManager.cpp

@@ -1220,6 +1220,8 @@ FixedWingModeManager::control_auto_takeoff(const hrt_abstime &now, const float c
 		fw_runway_control.timestamp = now;
 		fw_runway_control.runway_takeoff_state = _runway_takeoff.getState();
 		fw_runway_control.wheel_steering_enabled = true;
+		// keep the nose aligned with the runway bearing towards the takeoff waypoint during the ground roll
+		fw_runway_control.wheel_steering_yaw_setpoint = takeoff_bearing;
 		fw_runway_control.wheel_steering_nudging_rate = _param_rwto_nudge.get() ? _sticks.getYaw() : 0.f;
 
 		_fixed_wing_runway_control_pub.publish(fw_runway_control);
@@ -1398,6 +1400,8 @@ FixedWingModeManager::control_auto_takeoff_no_nav(const hrt_abstime &now, const
 		fw_runway_control.timestamp = now;
 		fw_runway_control.runway_takeoff_state = _runway_takeoff.getState();
 		fw_runway_control.wheel_steering_enabled = true;
+		// no navigation reference available: hold the heading captured when steering engaged
+		fw_runway_control.wheel_steering_yaw_setpoint = NAN;
 		fw_runway_control.wheel_steering_nudging_rate = _param_rwto_nudge.get() ? _sticks.getYaw() : 0.f;
 
 		_fixed_wing_runway_control_pub.publish(fw_runway_control);
@@ -1647,6 +1651,8 @@ FixedWingModeManager::control_auto_landing_straight(const hrt_abstime &now, cons
 	fw_runway_control.timestamp = now;
 	fw_runway_control.runway_takeoff_state = fixed_wing_runway_control_s::STATE_FLYING; // not in takeoff, use FLYING as default
 	fw_runway_control.wheel_steering_enabled = true;
+	// during landing rollout hold the heading captured at touchdown
+	fw_runway_control.wheel_steering_yaw_setpoint = NAN;
 	fw_runway_control.wheel_steering_nudging_rate = _param_fw_lnd_nudge.get() > LandingNudgingOption::kNudgingDisabled ?
 			_sticks.getYaw() : 0.f;
 
@@ -1820,6 +1826,8 @@ FixedWingModeManager::control_auto_landing_circular(const hrt_abstime &now, cons
 	fw_runway_control.timestamp = now;
 	fw_runway_control.runway_takeoff_state = fixed_wing_runway_control_s::STATE_FLYING; // not in takeoff, use FLYING as default
 	fw_runway_control.wheel_steering_enabled = true;
+	// during landing rollout hold the heading captured at touchdown
+	fw_runway_control.wheel_steering_yaw_setpoint = NAN;
 	fw_runway_control.wheel_steering_nudging_rate = _param_fw_lnd_nudge.get() > LandingNudgingOption::kNudgingDisabled ?
 			_sticks.getYaw() : 0.f;