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

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

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(