Land-detector: Better granularity for manual and auto flight modes

src/modules/land_detector/LandDetector.cpp

@@ -51,10 +51,11 @@ namespace landdetection
 LandDetector::LandDetector() :
 	_landDetectedPub(0),
 	_landDetected( {0, false}),
-	       _arming_time(0),
+	_arming_time(0),
-	       _taskShouldExit(false),
+	_taskShouldExit(false),
-	       _taskIsRunning(false),
+	_taskIsRunning(false),
-_work{} {
+	_work{}
+{
 	// ctor
 }
 

src/modules/land_detector/MulticopterLandDetector.cpp

@@ -57,11 +57,14 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(),
 	_parameterSub(-1),
 	_attitudeSub(-1),
 	_manualSub(-1),
+	_ctrl_state_sub(-1),
+	_vehicle_control_mode_sub(-1),
 	_vehicleLocalPosition{},
 	_actuators{},
 	_arming{},
 	_vehicleAttitude{},
 	_ctrl_state{},
+	_ctrl_mode{},
 	_landTimer(0),
 	_freefallTimer(0)
 {
@@ -79,11 +82,12 @@ void MulticopterLandDetector::initialize()
 	// subscribe to position, attitude, arming and velocity changes
 	_vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position));
 	_attitudeSub = orb_subscribe(ORB_ID(vehicle_attitude));
-	_actuatorsSub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
+	_actuatorsSub = orb_subscribe(ORB_ID(actuator_controls_0));
 	_armingSub = orb_subscribe(ORB_ID(actuator_armed));
 	_parameterSub = orb_subscribe(ORB_ID(parameter_update));
 	_manualSub = orb_subscribe(ORB_ID(manual_control_setpoint));
 	_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
+	_vehicle_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 
 	// download parameters
 	updateParameterCache(true);
@@ -93,10 +97,11 @@ void MulticopterLandDetector::updateSubscriptions()
 {
 	orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition);
 	orb_update(ORB_ID(vehicle_attitude), _attitudeSub, &_vehicleAttitude);
-	orb_update(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _actuatorsSub, &_actuators);
+	orb_update(ORB_ID(actuator_controls_0), _actuatorsSub, &_actuators);
 	orb_update(ORB_ID(actuator_armed), _armingSub, &_arming);
 	orb_update(ORB_ID(manual_control_setpoint), _manualSub, &_manual);
 	orb_update(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
+	orb_update(ORB_ID(vehicle_control_mode), _vehicle_control_mode_sub, &_ctrl_mode);
 }
 
 LandDetectionResult MulticopterLandDetector::update()
@@ -148,8 +153,15 @@ bool MulticopterLandDetector::get_landed_state()
 	// Time base for this function
 	const uint64_t now = hrt_absolute_time();
 
-	// Check if thrust output is less than max throttle param.
+	float sys_min_throttle = (_params.maxThrottle + 0.01f);
-	bool minimalThrust = (_actuators.control[3] <= (_params.maxThrottle + 0.05f));
+
+	// Determine the system min throttle based on flight mode
+	if (!_ctrl_mode.flag_control_altitude_enabled) {
+		sys_min_throttle = (_params.minManThrottle + 0.01f);
+	}
+
+	// Check if thrust output is less than the minimum auto throttle param.
+	bool minimalThrust = (_actuators.control[3] <= sys_min_throttle);
 
 	if (minimalThrust && _min_trust_start == 0) {
 		_min_trust_start = now;
@@ -166,19 +178,26 @@ bool MulticopterLandDetector::get_landed_state()
 		_arming_time = now;
 	}
 
+	// If in manual flight mode never report landed if the user has more than idle throttle
+	// Check if user commands throttle and if so, report not landed based on
+	// the user intent to take off (even if the system might physically still have
+	// ground contact at this point).
+	if (_manual.timestamp > 0 && _manual.z > 0.15f && _ctrl_mode.flag_control_manual_enabled) {
+		return false;
+	}
+
 	// Return status based on armed state and throttle if no position lock is available.
 	if (_vehicleLocalPosition.timestamp == 0 ||
 	    hrt_elapsed_time(&_vehicleLocalPosition.timestamp) > 500000 ||
 	    !_vehicleLocalPosition.xy_valid ||
 	    !_vehicleLocalPosition.z_valid) {
 
-	    	// Check if user commands throttle and if so, report not landed
+		// The system has minimum trust set (manual or in failsafe)
-		if (_manual.z > _params.minManThrottle + 0.01f) {
+		// if this persists for 8 seconds AND the drone is not
-			return false;
+		// falling consider it to be landed. This should even sustain
-		}
+		// quite acrobatic flight.
-
 		if ((_min_trust_start > 0) &&
-			(hrt_elapsed_time(&_min_trust_start) > 5 * 1000 * 1000)) {
+			(hrt_elapsed_time(&_min_trust_start) > 8 * 1000 * 1000)) {
 			return !get_freefall_state();
 		} else {
 			return false;

src/modules/land_detector/MulticopterLandDetector.h

@@ -51,6 +51,7 @@
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/control_state.h>
+#include <uORB/topics/vehicle_control_mode.h>
 #include <systemlib/param/param.h>
 
 namespace landdetection
@@ -125,6 +126,7 @@ private:
 	int _attitudeSub;
 	int _manualSub;
 	int _ctrl_state_sub;
+	int _vehicle_control_mode_sub;
 
 	struct vehicle_local_position_s		_vehicleLocalPosition;
 	struct actuator_controls_s		_actuators;
@@ -132,6 +134,7 @@ private:
 	struct vehicle_attitude_s		_vehicleAttitude;
 	struct manual_control_setpoint_s	_manual;
 	struct control_state_s			_ctrl_state;
+	struct vehicle_control_mode_s		_ctrl_mode;
 
 	uint64_t _landTimer;			///< timestamp in microseconds since a possible land was detected
 	uint64_t _freefallTimer;		///< timestamp in microseconds since a possible freefall was detected