land_detector: Add and max_altitude and timestamp validity checks to …

…MulticopterLandDetector class. (#12681)

 * Rename local camelCase vars to snake_case and control_mode -> vehicle_control_mode to match typdef with established class convention.

src/modules/land_detector/LandDetector.h

@@ -42,16 +42,16 @@
 
 #pragma once
 
-#include <px4_module.h>
-#include <px4_module_params.h>
 #include <lib/hysteresis/hysteresis.h>
 #include <parameters/param.h>
 #include <perf/perf_counter.h>
+#include <px4_module.h>
+#include <px4_module_params.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/vehicle_land_detected.h>
-#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
 
 using namespace time_literals;
 

src/modules/land_detector/MulticopterLandDetector.cpp

@@ -91,7 +91,7 @@ void MulticopterLandDetector::_update_topics()
 	_vehicle_acceleration_sub.update(&_vehicle_acceleration);
 	_vehicle_angular_velocity_sub.update(&_vehicle_angular_velocity);
 	_vehicle_attitude_sub.update(&_vehicle_attitude);
-	_vehicle_control_mode_sub.update(&_control_mode);
+	_vehicle_control_mode_sub.update(&_vehicle_control_mode);
 	_vehicle_local_position_sub.update(&_vehicle_local_position);
 	_vehicle_local_position_setpoint_sub.update(&_vehicle_local_position_setpoint);
 }
@@ -137,20 +137,20 @@ bool MulticopterLandDetector::_get_ground_contact_state()
 	// 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
 	// an accurate in-air indication.
-	bool verticalMovement;
+	bool vertical_movement = false;
 
 	if (hrt_elapsed_time(&_landed_time) < LAND_DETECTOR_LAND_PHASE_TIME_US) {
 
 		// Widen acceptance thresholds for landed state right after arming
 		// so that motor spool-up and other effects do not trigger false negatives.
-		verticalMovement = fabsf(_vehicle_local_position.vz) > _param_lndmc_z_vel_max.get()  * 2.5f;
+		vertical_movement = fabsf(_vehicle_local_position.vz) > _param_lndmc_z_vel_max.get()  * 2.5f;
 
 	} else {
 
-		// Adjust maxClimbRate if land_speed is lower than 2x maxClimbrate
-		float maxClimbRate = ((land_speed_threshold * 0.5f) < _param_lndmc_z_vel_max.get()) ? (0.5f * land_speed_threshold) :
-				     _param_lndmc_z_vel_max.get();
-		verticalMovement = fabsf(_vehicle_local_position.vz) > maxClimbRate;
+		// Adjust max_climb_rate if land_speed is lower than 2x max_climb_rate
+		float max_climb_rate = ((land_speed_threshold * 0.5f) < _param_lndmc_z_vel_max.get()) ? (0.5f * land_speed_threshold) :
+				       _param_lndmc_z_vel_max.get();
+		vertical_movement = fabsf(_vehicle_local_position.vz) > max_climb_rate;
 	}
 
 	// Check if we are moving horizontally.
@@ -161,11 +161,11 @@ bool MulticopterLandDetector::_get_ground_contact_state()
 	// we then can assume that the vehicle hit ground
 	_in_descend = _is_climb_rate_enabled()
 		      && (_vehicle_local_position_setpoint.vz >= land_speed_threshold);
-	bool hit_ground = _in_descend && !verticalMovement;
+	bool hit_ground = _in_descend && !vertical_movement;
 
 	// TODO: we need an accelerometer based check for vertical movement for flying without GPS
 	if ((_has_low_thrust() || hit_ground) && (!_horizontal_movement || !_has_position_lock())
-	    && (!verticalMovement || !_has_altitude_lock())) {
+	    && (!vertical_movement || !_has_altitude_lock())) {
 		return true;
 	}
 
@@ -178,7 +178,7 @@ bool MulticopterLandDetector::_get_maybe_landed_state()
 	const hrt_abstime now = hrt_absolute_time();
 
 	// When not armed, consider to be maybe-landed
-	if (!_actuator_armed.armed) {
+	if (!_actuator_armed.armed || (_vehicle_attitude.timestamp == 0)) {
 		return true;
 	}
 
@@ -199,11 +199,11 @@ bool MulticopterLandDetector::_get_maybe_landed_state()
 	}
 
 	// Next look if all rotation angles are not moving.
-	float maxRotationScaled = math::radians(_param_lndmc_rot_max.get()) * landThresholdFactor;
+	float max_rotation_scaled = math::radians(_param_lndmc_rot_max.get()) * landThresholdFactor;
 
-	bool rotating = (fabsf(_vehicle_angular_velocity.xyz[0]) > maxRotationScaled) ||
-			(fabsf(_vehicle_angular_velocity.xyz[1]) > maxRotationScaled) ||
-			(fabsf(_vehicle_angular_velocity.xyz[2]) > maxRotationScaled);
+	bool rotating = (fabsf(_vehicle_angular_velocity.xyz[0]) > max_rotation_scaled) ||
+			(fabsf(_vehicle_angular_velocity.xyz[1]) > max_rotation_scaled) ||
+			(fabsf(_vehicle_angular_velocity.xyz[2]) > max_rotation_scaled);
 
 	// Return status based on armed state and throttle if no position lock is available.
 	if (!_has_altitude_lock() && !rotating) {
@@ -251,6 +251,10 @@ float MulticopterLandDetector::_get_max_altitude()
 	/* TODO: add a meaningful altitude */
 	float valid_altitude_max = _param_lndmc_alt_max.get();
 
+	if (valid_altitude_max < 0.0f) {
+		return INFINITY;
+	}
+
 	if (_battery_status.warning == battery_status_s::BATTERY_WARNING_LOW) {
 		valid_altitude_max = _param_lndmc_alt_max.get() * 0.75f;
 	}
@@ -283,7 +287,7 @@ bool MulticopterLandDetector::_is_climb_rate_enabled()
 	bool has_updated = (_vehicle_local_position_setpoint.timestamp != 0)
 			   && (hrt_elapsed_time(&_vehicle_local_position_setpoint.timestamp) < 500_ms);
 
-	return (_control_mode.flag_control_climb_rate_enabled && has_updated
+	return (_vehicle_control_mode.flag_control_climb_rate_enabled && has_updated
 		&& PX4_ISFINITE(_vehicle_local_position_setpoint.vz));
 }
 
@@ -303,7 +307,7 @@ bool MulticopterLandDetector::_has_minimal_thrust()
 	float sys_min_throttle = _params.minThrottle + (_params.hoverThrottle - _params.minThrottle) * 0.1f;
 
 	// Determine the system min throttle based on flight mode
-	if (!_control_mode.flag_control_climb_rate_enabled) {
+	if (!_vehicle_control_mode.flag_control_climb_rate_enabled) {
 		sys_min_throttle = (_params.minManThrottle + 0.01f);
 	}
 

src/modules/land_detector/MulticopterLandDetector.h

@@ -42,22 +42,19 @@
 
 #pragma once
 
-#include "LandDetector.h"
+#include <math.h>
 
-#include <parameters/param.h>
-#include <uORB/Subscription.hpp>
-#include <uORB/topics/vehicle_local_position.h>
-#include <uORB/topics/vehicle_local_position_setpoint.h>
-#include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/parameter_update.h>
-#include <uORB/topics/sensor_bias.h>
 #include <uORB/topics/vehicle_acceleration.h>
 #include <uORB/topics/vehicle_angular_velocity.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_local_position_setpoint.h>
+
+#include "LandDetector.h"
 
 using namespace time_literals;
 
@@ -122,7 +119,6 @@ class MulticopterLandDetector : public LandDetector
 
 	uORB::Subscription _actuator_controls_sub{ORB_ID(actuator_controls_0)};
 	uORB::Subscription _battery_sub{ORB_ID(battery_status)};
-	uORB::Subscription _sensor_bias_sub{ORB_ID(sensor_bias)};
 	uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
 	uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
 	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
@@ -132,17 +128,17 @@ class MulticopterLandDetector : public LandDetector
 
 	actuator_controls_s               _actuator_controls {};
 	battery_status_s                  _battery_status {};
-	vehicle_control_mode_s            _control_mode {};
 	vehicle_acceleration_s            _vehicle_acceleration{};
-	vehicle_attitude_s                _vehicle_attitude {};
 	vehicle_angular_velocity_s        _vehicle_angular_velocity{};
+	vehicle_attitude_s                _vehicle_attitude {};
+	vehicle_control_mode_s            _vehicle_control_mode {};
 	vehicle_local_position_s          _vehicle_local_position {};
 	vehicle_local_position_setpoint_s _vehicle_local_position_setpoint {};
 
-	hrt_abstime _min_trust_start{0};		///< timestamp when minimum trust was applied first
+	hrt_abstime _min_trust_start{0};	///< timestamp when minimum trust was applied first
 	hrt_abstime _landed_time{0};
 
-	bool _in_descend{false};	///< vehicle is desending
+	bool _in_descend{false};		///< vehicle is desending
 	bool _horizontal_movement{false};	///< vehicle is moving horizontally
 
 	DEFINE_PARAMETERS_CUSTOM_PARENT(