FW LandDetector: disregard horizontal velocity if local_position.v_xy_valid is false

src/modules/land_detector/FixedwingLandDetector.cpp

@@ -71,9 +71,18 @@ bool FixedwingLandDetector::_get_landed_state()
 
 	} else if (hrt_elapsed_time(&_vehicle_local_position.timestamp) < 1_s) {
 
-		// Horizontal velocity complimentary filter.
-		float val = 0.97f * _velocity_xy_filtered + 0.03f * sqrtf(_vehicle_local_position.vx * _vehicle_local_position.vx +
-				_vehicle_local_position.vy * _vehicle_local_position.vy);
+		float val;
+
+		if (!_vehicle_local_position.v_xy_valid) {
+			// set _velocity_xy_filtered to 0 if data is invalid
+			val = 0.0f;
+
+		} else {
+			// Horizontal velocity complimentary filter.
+			val = 0.97f * _velocity_xy_filtered + 0.03f * sqrtf(_vehicle_local_position.vx * _vehicle_local_position.vx +
+					_vehicle_local_position.vy * _vehicle_local_position.vy);
+		}
+
 
 		if (PX4_ISFINITE(val)) {
 			_velocity_xy_filtered = val;
@@ -105,15 +114,28 @@ bool FixedwingLandDetector::_get_landed_state()
 		const float acc_hor = matrix::Vector2f(_acceleration).norm();
 		_xy_accel_filtered = _xy_accel_filtered * 0.8f + acc_hor * 0.18f;
 
+		// Check for angular velocity (disregard yaw)
+		const float rot_vel_hor = _angular_velocity.xy().norm();
+		val = _velocity_rot_filtered * 0.95f + rot_vel_hor * 0.05f;
+
+		if (PX4_ISFINITE(val)) {
+			_velocity_rot_filtered = val;
+		}
+
 		// make groundspeed threshold tighter if airspeed is invalid
-		const float vel_xy_max_threshold = airspeed_invalid ? 0.7f * _param_lndfw_vel_xy_max.get() :
+		const float vel_xy_max_threshold   = airspeed_invalid ? 0.7f * _param_lndfw_vel_xy_max.get() :
 						   _param_lndfw_vel_xy_max.get();
 
+		// make rotation threshold tighter if groundspeed is invalid
+		const float max_rotation_threshold = _vehicle_local_position.v_xy_valid ? math::radians(_param_lndfw_rot_max.get()) :
+						    math::radians(_param_lndfw_rot_max.get()) * 0.5f;
+
 		// Crude land detector for fixedwing.
-		landDetected = _airspeed_filtered       < _param_lndfw_airspd.get()
-			       && _velocity_xy_filtered < vel_xy_max_threshold
-			       && _velocity_z_filtered  < _param_lndfw_vel_z_max.get()
-			       && _xy_accel_filtered    < _param_lndfw_xyaccel_max.get();
+		landDetected = _airspeed_filtered         < _param_lndfw_airspd.get()
+			       && _velocity_xy_filtered   < vel_xy_max_threshold
+			       && _velocity_z_filtered    < _param_lndfw_vel_z_max.get()
+			       && _xy_accel_filtered      < _param_lndfw_xyaccel_max.get()
+			       && _velocity_rot_filtered  < max_rotation_threshold;
 
 	} else {
 		// Control state topic has timed out and we need to assume we're landed.

src/modules/land_detector/FixedwingLandDetector.h

@@ -75,13 +75,15 @@ class FixedwingLandDetector final : public LandDetector
 	float _velocity_xy_filtered{0.0f};
 	float _velocity_z_filtered{0.0f};
 	float _xy_accel_filtered{0.0f};
+	float _velocity_rot_filtered{0.0f};
 
 	DEFINE_PARAMETERS_CUSTOM_PARENT(
 		LandDetector,
 		(ParamFloat<px4::params::LNDFW_XYACC_MAX>)  _param_lndfw_xyaccel_max,
 		(ParamFloat<px4::params::LNDFW_AIRSPD_MAX>) _param_lndfw_airspd,
 		(ParamFloat<px4::params::LNDFW_VEL_XY_MAX>) _param_lndfw_vel_xy_max,
 		(ParamFloat<px4::params::LNDFW_VEL_Z_MAX>)  _param_lndfw_vel_z_max,
+		(ParamFloat<px4::params::LNDFW_ROT_MAX>)    _param_lndfw_rot_max,
 		(ParamFloat<px4::params::LNDFW_TRIG_TIME>)  _param_lndfw_trig_time
 	);
 };

src/modules/land_detector/land_detector_params_fw.c

@@ -102,3 +102,14 @@ PARAM_DEFINE_FLOAT(LNDFW_AIRSPD_MAX, 6.00f);
  * @group Land Detector
  */
 PARAM_DEFINE_FLOAT(LNDFW_TRIG_TIME, 2.f);
+
+/**
+ * Fixed-wing land detector: max rotation
+ *
+ * Maximum allowed angular velocity around each axis allowed in the landed state.
+ *
+ * @unit deg/s
+ * @decimal 1
+ * @group Land Detector
+ */
+PARAM_DEFINE_FLOAT(LNDFW_ROT_MAX, 0.06f);