diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 4f52bef795..fe1acef979 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -123,7 +123,6 @@ 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() @@ -137,7 +136,6 @@ 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() @@ -194,11 +192,6 @@ 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 @@ -249,11 +242,6 @@ 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 6a84585b02..4efee2d01e 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -126,7 +126,6 @@ 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; @@ -137,7 +136,6 @@ private: 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; diff --git a/src/modules/land_detector/VtolLandDetector.cpp b/src/modules/land_detector/VtolLandDetector.cpp index 54a338777f..1984a96c1b 100644 --- a/src/modules/land_detector/VtolLandDetector.cpp +++ b/src/modules/land_detector/VtolLandDetector.cpp @@ -50,6 +50,7 @@ VtolLandDetector::VtolLandDetector() : _paramHandle(), _params(), _airspeedSub(-1), + _vehicle_status_sub(-1), _airspeed{}, _was_in_air(false), _airspeed_filtered(0) @@ -62,6 +63,7 @@ void VtolLandDetector::_initialize_topics() MulticopterLandDetector::_initialize_topics(); _airspeedSub = orb_subscribe(ORB_ID(airspeed)); + _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); } void VtolLandDetector::_update_topics() @@ -69,10 +71,27 @@ void VtolLandDetector::_update_topics() MulticopterLandDetector::_update_topics(); _orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed); + _orb_update(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); +} + +bool VtolLandDetector::_get_maybe_landed_state() +{ + + // Only trigger in RW mode + if (!_vehicle_status.is_rotary_wing) { + return false; + } + + return MulticopterLandDetector::_get_maybe_landed_state(); } bool VtolLandDetector::_get_landed_state() { + // Only trigger in RW mode + if (!_vehicle_status.is_rotary_wing) { + return false; + } + // this is returned from the mutlicopter land detector bool landed = MulticopterLandDetector::_get_landed_state(); diff --git a/src/modules/land_detector/VtolLandDetector.h b/src/modules/land_detector/VtolLandDetector.h index 8132aadc04..25695553b9 100644 --- a/src/modules/land_detector/VtolLandDetector.h +++ b/src/modules/land_detector/VtolLandDetector.h @@ -58,6 +58,7 @@ protected: void _update_params() override; void _update_topics() override; bool _get_landed_state() override; + bool _get_maybe_landed_state() override; private: @@ -70,8 +71,10 @@ private: } _params; int _airspeedSub; + int _vehicle_status_sub; - struct airspeed_s _airspeed; + struct airspeed_s _airspeed; + struct vehicle_status_s _vehicle_status; bool _was_in_air; /**< indicates whether the vehicle was in the air in the previous iteration */ float _airspeed_filtered; /**< low pass filtered airspeed */