landdetector: hit ground logic

src/modules/land_detector/MulticopterLandDetector.cpp

@@ -75,6 +75,7 @@ MulticopterLandDetector::MulticopterLandDetector() :
 	_paramHandle(),
 	_params(),
 	_vehicleLocalPositionSub(-1),
+	_vehicleLocalPositionSetpointSub(-1),
 	_actuatorsSub(-1),
 	_armingSub(-1),
 	_attitudeSub(-1),
@@ -83,6 +84,7 @@ MulticopterLandDetector::MulticopterLandDetector() :
 	_vehicle_control_mode_sub(-1),
 	_battery_sub(-1),
 	_vehicleLocalPosition{},
+	_vehicleLocalPositionSetpoint{},
 	_actuators{},
 	_arming{},
 	_vehicleAttitude{},
@@ -105,12 +107,14 @@ MulticopterLandDetector::MulticopterLandDetector() :
 	_paramHandle.manual_stick_down_threshold = param_find("LNDMC_MAN_DWNTHR");
 	_paramHandle.altitude_max = param_find("LNDMC_ALT_MAX");
 	_paramHandle.manual_stick_up_position_takeoff_threshold = param_find("LNDMC_POS_UPTHR");
+	_paramHandle.landSpeed = param_find("MPC_LAND_SPEED");
 }
 
 void MulticopterLandDetector::_initialize_topics()
 {
 	// subscribe to position, attitude, arming and velocity changes
 	_vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position));
+	_vehicleLocalPositionSetpointSub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
 	_attitudeSub = orb_subscribe(ORB_ID(vehicle_attitude));
 	_actuatorsSub = orb_subscribe(ORB_ID(actuator_controls_0));
 	_armingSub = orb_subscribe(ORB_ID(actuator_armed));
@@ -124,6 +128,7 @@ void MulticopterLandDetector::_initialize_topics()
 void MulticopterLandDetector::_update_topics()
 {
 	_orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition);
+	_orb_update(ORB_ID(vehicle_local_position_setpoint), _vehicleLocalPositionSetpointSub, &_vehicleLocalPositionSetpoint);
 	_orb_update(ORB_ID(vehicle_attitude), _attitudeSub, &_vehicleAttitude);
 	_orb_update(ORB_ID(actuator_controls_0), _actuatorsSub, &_actuators);
 	_orb_update(ORB_ID(actuator_armed), _armingSub, &_arming);
@@ -149,6 +154,7 @@ void MulticopterLandDetector::_update_params()
 	param_get(_paramHandle.manual_stick_down_threshold, &_params.manual_stick_down_threshold);
 	param_get(_paramHandle.altitude_max, &_params.altitude_max);
 	param_get(_paramHandle.manual_stick_up_position_takeoff_threshold, &_params.manual_stick_up_position_takeoff_threshold);
+	param_get(_paramHandle.landSpeed, &_params.landSpeed);
 }
 
 
@@ -206,9 +212,14 @@ bool MulticopterLandDetector::_get_ground_contact_state()
 	// an accurate in-air indication.
 	bool verticalMovement = fabsf(_vehicleLocalPosition.vz) > _params.maxClimbRate * armThresholdFactor;
 
+	// 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
+	bool in_descend = _is_velocity_control_active() && (_vehicleLocalPositionSetpoint.vz >= 0.9f * _params.landSpeed);
+	bool hit_ground = in_descend && !verticalMovement;
+
 	// If pilots commands down or in auto mode and we are already below minimal thrust and we do not move down we assume ground contact
 	// TODO: we need an accelerometer based check for vertical movement for flying without GPS
-	if (manual_control_idle_or_auto && _has_low_thrust() &&
+	if (manual_control_idle_or_auto && (_has_low_thrust() || hit_ground) &&
 	    (!verticalMovement || !_has_altitude_lock())) {
 		return true;
 	}
@@ -349,6 +360,15 @@ bool MulticopterLandDetector::_has_manual_control_present()
 	return _control_mode.flag_control_manual_enabled && _manual.timestamp > 0;
 }
 
+bool MulticopterLandDetector::_is_velocity_control_active()
+{
+	bool is_finite = PX4_ISFINITE(_vehicleLocalPositionSetpoint.vx) && PX4_ISFINITE(_vehicleLocalPositionSetpoint.vy)
+			 && PX4_ISFINITE(_vehicleLocalPositionSetpoint.vz);
+
+	return (_vehicleLocalPositionSetpoint.timestamp != 0) &&
+	       (hrt_elapsed_time(&_vehicleLocalPositionSetpoint.timestamp) < 500000) && is_finite;
+}
+
 bool MulticopterLandDetector::_has_low_thrust()
 {
 	// 30% of throttle range between min and hover

src/modules/land_detector/MulticopterLandDetector.h

@@ -44,6 +44,7 @@
 
 #include <systemlib/param/param.h>
 #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/actuator_armed.h>
@@ -97,6 +98,7 @@ private:
 		param_t manual_stick_down_threshold;
 		param_t altitude_max;
 		param_t manual_stick_up_position_takeoff_threshold;
+		param_t landSpeed;
 	} _paramHandle;
 
 	struct {
@@ -112,9 +114,11 @@ private:
 		float manual_stick_down_threshold;
 		float altitude_max;
 		float manual_stick_up_position_takeoff_threshold;
+		float landSpeed;
 	} _params;
 
 	int _vehicleLocalPositionSub;
+	int _vehicleLocalPositionSetpointSub;
 	int _actuatorsSub;
 	int _armingSub;
 	int _attitudeSub;
@@ -124,6 +128,7 @@ private:
 	int _battery_sub;
 
 	struct vehicle_local_position_s		_vehicleLocalPosition;
+	struct vehicle_local_position_setpoint_s _vehicleLocalPositionSetpoint;
 	struct actuator_controls_s		_actuators;
 	struct actuator_armed_s			_arming;
 	struct vehicle_attitude_s		_vehicleAttitude;
@@ -142,6 +147,7 @@ private:
 	bool _has_manual_control_present();
 	bool _has_minimal_thrust();
 	bool _has_low_thrust();
+	bool _is_velocity_control_active();
 };