FixedWingLandDetector: force to landed during runway takeoff

msg/FixedWingRunwayControl.msg

@@ -1,8 +1,15 @@
 # Auxiliary control fields for fixed-wing runway takeoff/landing
 
-# Passes information from the FixedWingModeManager to the FixedWingAttitudeController
+# Passes information from the FixedWingModeManager to the FixedWingAttitudeController (wheel control) and FixedWingLandDetector (takeoff state)
 
 uint64 timestamp # [us] time since system start
 
+uint8 STATE_THROTTLE_RAMP = 0		# ramping up throttle
+uint8 STATE_CLAMPED_TO_RUNWAY = 1	# clamped to runway, controlling yaw directly (wheel or rudder)
+uint8 STATE_CLIMBOUT = 2		# climbout to safe height before navigation
+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_nudging_rate	# [norm] [@range -1, 1] [FRD] Manual wheel nudging, added to controller output. NAN is interpreted as 0.

src/modules/fw_mode_manager/FixedWingModeManager.cpp

@@ -1170,6 +1170,7 @@ FixedWingModeManager::control_auto_takeoff(const hrt_abstime &now, const float c
 
 		fixed_wing_runway_control_s fw_runway_control{};
 		fw_runway_control.timestamp = now;
+		fw_runway_control.runway_takeoff_state = _runway_takeoff.getState();
 		fw_runway_control.wheel_steering_enabled = true;
 		fw_runway_control.wheel_steering_nudging_rate = _param_rwto_nudge.get() ? _sticks.getYaw() : 0.f;
 
@@ -1343,6 +1344,7 @@ FixedWingModeManager::control_auto_takeoff_no_nav(const hrt_abstime &now, const
 
 		fixed_wing_runway_control_s fw_runway_control{};
 		fw_runway_control.timestamp = now;
+		fw_runway_control.runway_takeoff_state = _runway_takeoff.getState();
 		fw_runway_control.wheel_steering_enabled = true;
 		fw_runway_control.wheel_steering_nudging_rate = _param_rwto_nudge.get() ? _sticks.getYaw() : 0.f;
 
@@ -1591,6 +1593,7 @@ FixedWingModeManager::control_auto_landing_straight(const hrt_abstime &now, cons
 
 	fixed_wing_runway_control_s fw_runway_control{};
 	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;
 	fw_runway_control.wheel_steering_nudging_rate = _param_fw_lnd_nudge.get() > LandingNudgingOption::kNudgingDisabled ?
 			_sticks.getYaw() : 0.f;
@@ -1761,6 +1764,7 @@ FixedWingModeManager::control_auto_landing_circular(const hrt_abstime &now, cons
 
 	fixed_wing_runway_control_s fw_runway_control{};
 	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;
 	fw_runway_control.wheel_steering_nudging_rate = _param_fw_lnd_nudge.get() > LandingNudgingOption::kNudgingDisabled ?
 			_sticks.getYaw() : 0.f;

src/modules/fw_mode_manager/runway_takeoff/RunwayTakeoff.cpp

@@ -86,7 +86,7 @@ void RunwayTakeoff::update(const hrt_abstime &time_now, const float takeoff_airs
 
 	case RunwayTakeoffState::CLIMBOUT:
 		if (vehicle_altitude > clearance_altitude) {
-			takeoff_state_ = RunwayTakeoffState::FLY;
+			takeoff_state_ = RunwayTakeoffState::FLYING;
 			events::send(events::ID("runway_takeoff_reached_clearance_altitude"), events::Log::Info, "Reached clearance altitude");
 		}
 
@@ -134,7 +134,7 @@ float RunwayTakeoff::getThrottle(const float idle_throttle) const
 
 		break;
 
-	case RunwayTakeoffState::FLY:
+	case RunwayTakeoffState::FLYING:
 		throttle = NAN;
 	}
 
@@ -147,7 +147,7 @@ float RunwayTakeoff::getMinPitch(float min_pitch_in_climbout, float min_pitch) c
 		// constrain to the taxi pitch setpoint
 		return math::radians(param_rwto_psp_.get() - 0.01f);
 
-	} else if (takeoff_state_ < RunwayTakeoffState::FLY) {
+	} else if (takeoff_state_ < RunwayTakeoffState::FLYING) {
 		// ramp in the climbout pitch constraint over the rotation transition time
 		const float taxi_pitch_min = math::radians(param_rwto_psp_.get() - 0.01f);
 		return interpolateValuesOverAbsoluteTime(taxi_pitch_min, min_pitch_in_climbout, takeoff_time_,
@@ -164,7 +164,7 @@ float RunwayTakeoff::getMaxPitch(const float max_pitch) const
 		// constrain to the taxi pitch setpoint
 		return math::radians(param_rwto_psp_.get() + 0.01f);
 
-	} else if (takeoff_state_ < RunwayTakeoffState::FLY) {
+	} else if (takeoff_state_ < RunwayTakeoffState::FLYING) {
 		// ramp in the climbout pitch constraint over the rotation transition time
 		const float taxi_pitch_max = math::radians(param_rwto_psp_.get() + 0.01f);
 		return interpolateValuesOverAbsoluteTime(taxi_pitch_max, max_pitch, takeoff_time_, param_rwto_rot_time_.get());

src/modules/fw_mode_manager/runway_takeoff/RunwayTakeoff.h

@@ -57,7 +57,7 @@ enum RunwayTakeoffState {
 	THROTTLE_RAMP = 0, // ramping up throttle
 	CLAMPED_TO_RUNWAY, // clamped to runway, controlling yaw directly (wheel or rudder)
 	CLIMBOUT, // climbout to safe height before navigation
-	FLY // navigate freely
+	FLYING // navigate freely
 };
 
 class __EXPORT RunwayTakeoff : public ModuleParams
@@ -135,7 +135,7 @@ class __EXPORT RunwayTakeoff : public ModuleParams
 	float getMaxPitch(const float max_pitch) const;
 
 	// NOTE: this is only to be used for mistaken mode transitions to takeoff while already in air
-	void forceSetFlyState() { takeoff_state_ = RunwayTakeoffState::FLY; }
+	void forceSetFlyState() { takeoff_state_ = RunwayTakeoffState::FLYING; }
 
 	/**
 	 * @brief Reset the state machine.

src/modules/land_detector/FixedwingLandDetector.cpp

@@ -57,18 +57,31 @@ bool FixedwingLandDetector::_get_landed_state()
 		return true;
 	}
 
-	bool landDetected = false;
+	// Force the landed state to stay landed if we're currently in an early state of the takeoff state machines.
+	// This prevents premature transitions to in-air during the early takeoff phase.
+	if (_landed_hysteresis.get_state()) {
+		launch_detection_status_s launch_detection_status{};
+		_launch_detection_status_sub.copy(&launch_detection_status);
 
-	launch_detection_status_s launch_detection_status{};
-	_launch_detection_status_sub.copy(&launch_detection_status);
+		fixed_wing_runway_control_s fixed_wing_runway_control{};
+		_fixed_wing_runway_control_sub.copy(&fixed_wing_runway_control);
 
-	// force the landed state to stay landed if we're currently in the catapult/hand-launch launch process. Detect that we are in this state
-	// by checking if the last publication of launch_detection_status is less than 0.5s old, and we're still in the wait for launch state.
-	if (_landed_hysteresis.get_state() &&  hrt_elapsed_time(&launch_detection_status.timestamp) < 500_ms
-	    && launch_detection_status.launch_detection_state == launch_detection_status_s::STATE_WAITING_FOR_LAUNCH) {
-		landDetected = true;
+		// Check if we're in catapult/hand-launch waiting state
+		const bool waiting_for_catapult_launch = hrt_elapsed_time(&launch_detection_status.timestamp) < 500_ms
+				&& launch_detection_status.launch_detection_state == launch_detection_status_s::STATE_WAITING_FOR_LAUNCH;
+
+		// Check if we're in runway takeoff early phase (throttle ramp or clamped to runway)
+		const bool waiting_for_auto_runway_climbout = hrt_elapsed_time(&fixed_wing_runway_control.timestamp) < 500_ms
+				&& fixed_wing_runway_control.runway_takeoff_state < fixed_wing_runway_control_s::STATE_CLIMBOUT;
+
+		if (waiting_for_catapult_launch || waiting_for_auto_runway_climbout) {
+			return true;
+		}
+	}
+
+	bool landDetected = false;
 
-	} else if (hrt_elapsed_time(&_vehicle_local_position.timestamp) < 1_s) {
+	if (hrt_elapsed_time(&_vehicle_local_position.timestamp) < 1_s) {
 
 		float val = 0.0f;
 

src/modules/land_detector/FixedwingLandDetector.h

@@ -44,6 +44,7 @@
 
 #include <matrix/math.hpp>
 #include <uORB/topics/airspeed_validated.h>
+#include <uORB/topics/fixed_wing_runway_control.h>
 #include <uORB/topics/launch_detection_status.h>
 
 #include "LandDetector.h"
@@ -67,6 +68,7 @@ class FixedwingLandDetector final : public LandDetector
 private:
 	uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)};
 	uORB::Subscription _launch_detection_status_sub{ORB_ID(launch_detection_status)};
+	uORB::Subscription _fixed_wing_runway_control_sub{ORB_ID(fixed_wing_runway_control)};
 
 	float _airspeed_filtered{0.0f};
 	float _velocity_xy_filtered{0.0f};