land detector: log more states in order to facilitate debugging ground contact state

Signed-off-by: RomanBapst <bapstroman@gmail.com>

msg/vehicle_land_detected.msg

@@ -5,3 +5,8 @@ bool ground_contact	# true if vehicle has ground contact but is not landed (1. s
 bool maybe_landed	# true if the vehicle might have landed (2. stage)
 bool landed		# true if vehicle is currently landed on the ground (3. stage)
 bool in_ground_effect # indicates if from the perspective of the landing detector the vehicle might be in ground effect (baro). This flag will become true if the vehicle is not moving horizontally and is descending (crude assumption that user is landing).
+bool in_descend
+bool has_low_throttle
+bool vertical_movement
+bool horizontal_movement
+bool close_to_ground_or_skipped_check

src/modules/land_detector/LandDetector.cpp

@@ -147,6 +147,11 @@ void LandDetector::Run()
 		_land_detected.ground_contact = ground_contactDetected;
 		_land_detected.alt_max = alt_max;
 		_land_detected.in_ground_effect = in_ground_effect;
+		_land_detected.in_descend = _get_in_descend();
+		_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.close_to_ground_or_skipped_check = _get_close_to_ground_or_skipped_check();
 		_land_detected.timestamp = hrt_absolute_time();
 		_vehicle_land_detected_pub.publish(_land_detected);
 	}

src/modules/land_detector/LandDetector.h

@@ -136,6 +136,11 @@ protected:
 	 */
 	virtual bool _get_ground_effect_state() { return false; }
 
+	virtual bool _get_in_descend() { return false; }
+	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_close_to_ground_or_skipped_check() {  return false; }
 	virtual void _set_hysteresis_factor(const int factor) = 0;
 
 	systemlib::Hysteresis _freefall_hysteresis{false};

src/modules/land_detector/MulticopterLandDetector.cpp

@@ -162,8 +162,6 @@ bool MulticopterLandDetector::_get_ground_contact_state()
 	// land speed threshold, 90% of MPC_LAND_SPEED
 	const float land_speed_threshold = 0.9f * math::max(_params.landSpeed, 0.1f);
 
-	bool vertical_movement = true;
-
 	if (lpos_available && _vehicle_local_position.v_z_valid) {
 		// Check if we are moving vertically - this might see a spike after arming due to
 		// throttle-up vibration. If accelerating fast the throttle thresholds will still give
@@ -176,7 +174,10 @@ bool MulticopterLandDetector::_get_ground_contact_state()
 			max_climb_rate = _param_lndmc_z_vel_max.get() * 2.5f;
 		}
 
-		vertical_movement = (fabsf(_vehicle_local_position.vz) > max_climb_rate);
+		_vertical_movement = (fabsf(_vehicle_local_position.vz) > max_climb_rate);
+
+	} else {
+		_vertical_movement = true;
 	}
 
 
@@ -206,7 +207,8 @@ bool MulticopterLandDetector::_get_ground_contact_state()
 	// low thrust: 30% of throttle range between min and hover, relaxed to 60% if hover thrust estimate available
 	const float thr_pct_hover = _hover_thrust_estimate_valid ? 0.6f : 0.3f;
 	const float sys_low_throttle = _params.minThrottle + (_params.hoverThrottle - _params.minThrottle) * thr_pct_hover;
-	bool ground_contact = (_actuator_controls_throttle <= sys_low_throttle);
+	_has_low_throttle = (_actuator_controls_throttle <= sys_low_throttle);
+	bool ground_contact = _has_low_throttle;
 
 	// if we have a valid velocity setpoint and the vehicle is demanded to go down but no vertical movement present,
 	// we then can assume that the vehicle hit ground
@@ -233,20 +235,20 @@ bool MulticopterLandDetector::_get_ground_contact_state()
 		_in_descend = false;
 	}
 
+	// if there is no distance to ground estimate available then don't enforce using it.
+	// if a distance to the ground estimate is generally available (_dist_bottom_is_observable=true), then
+	// we already increased the hysteresis for the land detection states in order to reduce the chance of false positives.
+	const bool skip_close_to_ground_check = !_dist_bottom_is_observable || !_vehicle_local_position.dist_bottom_valid;
+	_close_to_ground_or_skipped_check = _is_close_to_ground() || skip_close_to_ground_check;
 
 	// When not armed, consider to have ground-contact
 	if (!_armed) {
 		return true;
 	}
 
-	// if there is no distance to ground estimate available then don't enforce using it.
-	// if a distance to the ground estimate is generally available (_dist_bottom_is_observable=true), then
-	// we already increased the hysteresis for the land detection states in order to reduce the chance of false positives.
-	const bool skip_close_to_ground_check = !_dist_bottom_is_observable || !_vehicle_local_position.dist_bottom_valid;
-
 	// TODO: we need an accelerometer based check for vertical movement for flying without GPS
-	return (_is_close_to_ground() || skip_close_to_ground_check) && ground_contact && !_horizontal_movement
-	       && !vertical_movement;
+	return _close_to_ground_or_skipped_check && ground_contact && !_horizontal_movement
+	       && !_vertical_movement;
 }
 
 bool MulticopterLandDetector::_get_maybe_landed_state()

src/modules/land_detector/MulticopterLandDetector.h

@@ -71,7 +71,11 @@ protected:
 	bool _get_maybe_landed_state() override;
 	bool _get_freefall_state() override;
 	bool _get_ground_effect_state() override;
-
+	bool _get_in_descend() override { return _in_descend; }
+	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_close_to_ground_or_skipped_check() override { return _close_to_ground_or_skipped_check; }
 	float _get_max_altitude() override;
 
 	void _set_hysteresis_factor(const int factor) override;
@@ -137,6 +141,9 @@ private:
 
 	bool _in_descend{false};		///< vehicle is desending
 	bool _horizontal_movement{false};	///< vehicle is moving horizontally
+	bool _vertical_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
 
 	DEFINE_PARAMETERS_CUSTOM_PARENT(