LandDetector: log rotational_movement

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

msg/VehicleLandDetected.msg

@@ -12,6 +12,7 @@ bool has_low_throttle
 
 bool vertical_movement
 bool horizontal_movement
+bool rotational_movement
 
 bool close_to_ground_or_skipped_check
 

src/modules/land_detector/LandDetector.cpp

@@ -57,6 +57,7 @@ LandDetector::LandDetector() :
 	_land_detected.has_low_throttle = false;
 	_land_detected.vertical_movement = false;
 	_land_detected.horizontal_movement = false;
+	_land_detected.rotational_movement = false;
 	_land_detected.close_to_ground_or_skipped_check = true;
 	_land_detected.at_rest = true;
 }
@@ -174,6 +175,7 @@ void LandDetector::Run()
 		_land_detected.has_low_throttle = _get_has_low_throttle();
 		_land_detected.horizontal_movement = _get_horizontal_movement();
 		_land_detected.vertical_movement = _get_vertical_movement();
+		_land_detected.rotational_movement = _get_rotational_movement();
 		_land_detected.close_to_ground_or_skipped_check = _get_close_to_ground_or_skipped_check();
 		_land_detected.at_rest = at_rest;
 		_land_detected.timestamp = hrt_absolute_time();

src/modules/land_detector/LandDetector.h

@@ -138,6 +138,7 @@ protected:
 	virtual bool _get_has_low_throttle() { return false; }
 	virtual bool _get_horizontal_movement() { return false; }
 	virtual bool _get_vertical_movement() { return false; }
+	virtual bool _get_rotational_movement() { return false; }
 	virtual bool _get_close_to_ground_or_skipped_check() {  return false; }
 	virtual void _set_hysteresis_factor(const int factor) = 0;
 

src/modules/land_detector/MulticopterLandDetector.cpp

@@ -278,21 +278,22 @@ bool MulticopterLandDetector::_get_maybe_landed_state()
 		return false;
 	}
 
+	// Next look if vehicle is not rotating (do not consider yaw)
+	float max_rotation_threshold = math::radians(_param_lndmc_rot_max.get());
 
-	float landThresholdFactor = 1.f;
-
-	// Widen acceptance thresholds for landed state right after landed
+	// Widen max rotation thresholds for landed state right after landed
 	if ((time_now_us - _landed_time) < LAND_DETECTOR_LAND_PHASE_TIME_US) {
-		landThresholdFactor = 2.5f;
+		max_rotation_threshold *= 2.5f;
 	}
 
-	// Next look if all rotation angles are not moving.
-	const float max_rotation_scaled = math::radians(_param_lndmc_rot_max.get()) * landThresholdFactor;
-
 	matrix::Vector2f angular_velocity{_angular_velocity(0), _angular_velocity(1)};
 
-	if (angular_velocity.norm() > max_rotation_scaled) {
+	if (angular_velocity.norm() > max_rotation_threshold) {
+		_rotational_movement = true;
 		return false;
+
+	} else {
+		_rotational_movement = false;
 	}
 
 	// If vertical velocity is available: ground contact, no thrust, no movement -> landed

src/modules/land_detector/MulticopterLandDetector.h

@@ -74,6 +74,7 @@ protected:
 	bool _get_has_low_throttle() override { return _has_low_throttle; }
 	bool _get_horizontal_movement() override { return _horizontal_movement; }
 	bool _get_vertical_movement() override { return _vertical_movement; }
+	bool _get_rotational_movement() override { return _rotational_movement; }
 	bool _get_close_to_ground_or_skipped_check() override { return _close_to_ground_or_skipped_check; }
 
 	void _set_hysteresis_factor(const int factor) override;
@@ -131,6 +132,7 @@ private:
 	bool _in_descend{false};		///< vehicle is commanded to desend
 	bool _horizontal_movement{false};	///< vehicle is moving horizontally
 	bool _vertical_movement{false};
+	bool _rotational_movement{false};
 	bool _has_low_throttle{false};
 	bool _close_to_ground_or_skipped_check{false};
 	bool _below_gnd_effect_hgt{false};	///< vehicle height above ground is below height where ground effect occurs