From 49d2e8a3ff15d79a6de60f82122cf4dac82d44eb Mon Sep 17 00:00:00 2001 From: sanderux Date: Tue, 8 Aug 2017 11:39:51 +0200 Subject: [PATCH] Land detector fix for VTOL in FW mode --- .../land_detector/MulticopterLandDetector.cpp | 12 +++++++++++ .../land_detector/MulticopterLandDetector.h | 20 ++++++++++--------- 2 files changed, 23 insertions(+), 9 deletions(-) diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index fe1acef979..4f52bef795 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -123,6 +123,7 @@ void MulticopterLandDetector::_initialize_topics() _ctrl_state_sub = orb_subscribe(ORB_ID(control_state)); _vehicle_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _battery_sub = orb_subscribe(ORB_ID(battery_status)); + _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); } void MulticopterLandDetector::_update_topics() @@ -136,6 +137,7 @@ void MulticopterLandDetector::_update_topics() _orb_update(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state); _orb_update(ORB_ID(vehicle_control_mode), _vehicle_control_mode_sub, &_control_mode); _orb_update(ORB_ID(battery_status), _battery_sub, &_battery); + _orb_update(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); } void MulticopterLandDetector::_update_params() @@ -192,6 +194,11 @@ bool MulticopterLandDetector::_get_ground_contact_state() _arming_time = now; } + // Only trigger in RW mode + if (!_vehicle_status.is_rotary_wing) { + return false; + } + // 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 no ground contact based on // the user intent to take off (even if the system might physically still have @@ -242,6 +249,11 @@ bool MulticopterLandDetector::_get_maybe_landed_state() return true; } + // Only trigger in RW mode + if (!_vehicle_status.is_rotary_wing) { + return false; + } + // If we control manually and are still landed, we want to stay idle until the pilot rises the throttle for takeoff if (_state == LandDetectionState::LANDED && _has_manual_control_present()) { if (_manual.z < _get_takeoff_throttle()) { diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index 99c5f486b7..6a84585b02 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -126,16 +126,18 @@ private: int _ctrl_state_sub; int _vehicle_control_mode_sub; int _battery_sub; + int _vehicle_status_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; - struct manual_control_setpoint_s _manual; - struct control_state_s _ctrl_state; - struct vehicle_control_mode_s _control_mode; - struct battery_status_s _battery; + 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; + struct manual_control_setpoint_s _manual; + struct control_state_s _ctrl_state; + struct vehicle_control_mode_s _control_mode; + struct battery_status_s _battery; + struct vehicle_status_s _vehicle_status; uint64_t _min_trust_start; ///< timestamp when minimum trust was applied first uint64_t _arming_time;