diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 2c6a7bdafc2..650ae5724d3 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -53,6 +53,7 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(), _armingSub(-1), _parameterSub(-1), _attitudeSub(-1), + _manualSub(-1), _vehicleLocalPosition{}, _actuators{}, _arming{}, @@ -73,6 +74,7 @@ void MulticopterLandDetector::initialize() _actuatorsSub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); _armingSub = orb_subscribe(ORB_ID(actuator_armed)); _parameterSub = orb_subscribe(ORB_ID(parameter_update)); + _manualSub = orb_subscribe(ORB_ID(manual_control_setpoint)); // download parameters updateParameterCache(true); @@ -84,6 +86,7 @@ void MulticopterLandDetector::updateSubscriptions() orb_update(ORB_ID(vehicle_attitude), _attitudeSub, &_vehicleAttitude); orb_update(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _actuatorsSub, &_actuators); orb_update(ORB_ID(actuator_armed), _armingSub, &_arming); + orb_update(ORB_ID(manual_control_setpoint), _manualSub, &_manual); } bool MulticopterLandDetector::update() @@ -107,6 +110,11 @@ bool MulticopterLandDetector::get_landed_state() _arming_time = hrt_absolute_time(); } + // Check if user commands throttle and if so, report not landed + if (_manual.z > 0.3f) { + return false; + } + // Check if thrust output is less than max throttle param. bool minimalThrust = _actuators.control[3] <= _params.maxThrottle; diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index b5e308cc421..4e520fea947 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -49,6 +49,7 @@ #include #include #include +#include #include class MulticopterLandDetector : public LandDetector @@ -107,11 +108,13 @@ private: int _armingSub; int _parameterSub; int _attitudeSub; + int _manualSub; struct vehicle_local_position_s _vehicleLocalPosition; struct actuator_controls_s _actuators; struct actuator_armed_s _arming; struct vehicle_attitude_s _vehicleAttitude; + struct manual_control_setpoint_s _manual; /* timestamp in microseconds since a possible land was detected */ uint64_t _landTimer;