From f1587da4c46e92474687f37ad49fbd003b7c91db Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Wed, 14 Jan 2015 17:47:17 +0100 Subject: MulticopterLandDetector: Detect land even if autopilot is not landing --- .../land_detector/FixedwingLandDetector.cpp | 2 +- .../land_detector/MulticopterLandDetector.cpp | 44 +++++++++++----------- src/modules/land_detector/land_detector_main.cpp | 2 +- 3 files changed, 23 insertions(+), 25 deletions(-) (limited to 'src/modules/land_detector') diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index 7df89257f..38f6c00a9 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -43,7 +43,7 @@ #include #include -FixedwingLandDetector::FixedwingLandDetector() : +FixedwingLandDetector::FixedwingLandDetector() : LandDetector(), _vehicleLocalPositionSub(-1), _vehicleLocalPosition({}), _airspeedSub(-1), diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 36d92fd79..81af029fb 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -44,7 +44,7 @@ #include #include -MulticopterLandDetector::MulticopterLandDetector() : +MulticopterLandDetector::MulticopterLandDetector() : LandDetector(), _vehicleGlobalPositionSub(-1), _sensorsCombinedSub(-1), _waypointSub(-1), @@ -86,34 +86,32 @@ bool MulticopterLandDetector::update() //First poll for new data from our subscriptions updateSubscriptions(); - const uint64_t now = hrt_absolute_time(); - - //only detect landing if the autopilot is actively trying to land - if (!_waypoint.current.valid || _waypoint.current.type != SETPOINT_TYPE_LAND) { - _landTimer = now; - - } else { + //Only trigger flight conditions if we are armed + if(!_arming.armed) { + return true; + } - //Check if we are moving vertically - bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > MC_LAND_DETECTOR_CLIMBRATE_MAX; + const uint64_t now = hrt_absolute_time(); - //Check if we are moving horizontally - bool horizontalMovement = sqrtf(_vehicleGlobalPosition.vel_n * _vehicleGlobalPosition.vel_n - + _vehicleGlobalPosition.vel_e * _vehicleGlobalPosition.vel_e) > MC_LAND_DETECTOR_VELOCITY_MAX; + //Check if we are moving vertically + bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > MC_LAND_DETECTOR_CLIMBRATE_MAX; - //Next look if all rotation angles are not moving - bool rotating = sqrtf(_sensors.gyro_rad_s[0] * _sensors.gyro_rad_s[0] + - _sensors.gyro_rad_s[1] * _sensors.gyro_rad_s[1] + - _sensors.gyro_rad_s[2] * _sensors.gyro_rad_s[2]) > MC_LAND_DETECTOR_ROTATION_MAX; + //Check if we are moving horizontally + bool horizontalMovement = sqrtf(_vehicleGlobalPosition.vel_n * _vehicleGlobalPosition.vel_n + + _vehicleGlobalPosition.vel_e * _vehicleGlobalPosition.vel_e) > MC_LAND_DETECTOR_VELOCITY_MAX; - //Check if thrust output is minimal (about half of default) - bool minimalThrust = _arming.armed && _actuators.control[3] <= MC_LAND_DETECTOR_THRUST_MAX; + //Next look if all rotation angles are not moving + bool rotating = sqrtf(_sensors.gyro_rad_s[0] * _sensors.gyro_rad_s[0] + + _sensors.gyro_rad_s[1] * _sensors.gyro_rad_s[1] + + _sensors.gyro_rad_s[2] * _sensors.gyro_rad_s[2]) > MC_LAND_DETECTOR_ROTATION_MAX; - if (verticalMovement || rotating || !minimalThrust || horizontalMovement) { - //Sensed movement, so reset the land detector - _landTimer = now; - } + //Check if thrust output is minimal (about half of default) + bool minimalThrust = _actuators.control[3] <= MC_LAND_DETECTOR_THRUST_MAX; + if (verticalMovement || rotating || !minimalThrust || horizontalMovement) { + //Sensed movement, so reset the land detector + _landTimer = now; + return false; } return now - _landTimer > MC_LAND_DETECTOR_TRIGGER_TIME; diff --git a/src/modules/land_detector/land_detector_main.cpp b/src/modules/land_detector/land_detector_main.cpp index 7c0ffb82a..9e135b5f1 100644 --- a/src/modules/land_detector/land_detector_main.cpp +++ b/src/modules/land_detector/land_detector_main.cpp @@ -104,7 +104,7 @@ static void land_detector_stop() delete land_detector_task; land_detector_task = nullptr; _landDetectorTaskID = -1; - warn("land_detector has been stopped"); + errx(0, "land_detector has been stopped"); } /** -- cgit v1.2.3