FW land detector: Introduce max rotational speed condition (new param…

…: LNDFW_ROT_MAX).

Checks that the filtered norm of the angular velocity is below LNDFW_ROT_MAX.

src/modules/land_detector/FixedwingLandDetector.cpp

@@ -109,15 +109,26 @@ 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
+		const float rot_vel_hor = _angular_velocity.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() :
-						   _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();
+
+		const float max_rotation_threshold = math::radians(_param_lndfw_rot_max.get()) ;
 
 		// 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 rotational speed
+ *
+ * Maximum allowed norm of the angular velocity in the landed state.
+ *
+ * @unit deg/s
+ * @decimal 1
+ * @group Land Detector
+ */
+PARAM_DEFINE_FLOAT(LNDFW_ROT_MAX, 0.5f);