diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 40de5fc614f1..ea50f0b85c64 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -71,25 +71,7 @@ namespace land_detector { -MulticopterLandDetector::MulticopterLandDetector() : - _paramHandle(), - _params(), - _vehicleLocalPositionSub(-1), - _vehicleLocalPositionSetpointSub(-1), - _actuatorsSub(-1), - _attitudeSub(-1), - _sensor_bias_sub(-1), - _vehicle_control_mode_sub(-1), - _battery_sub(-1), - _vehicleLocalPosition{}, - _vehicleLocalPositionSetpoint{}, - _actuators{}, - _vehicleAttitude{}, - _sensors{}, - _control_mode{}, - _battery{}, - _min_trust_start(0), - _landed_time(0) +MulticopterLandDetector::MulticopterLandDetector() { _paramHandle.maxRotation = param_find("LNDMC_ROT_MAX"); _paramHandle.maxVelocity = param_find("LNDMC_XY_VEL_MAX"); @@ -334,7 +316,7 @@ bool MulticopterLandDetector::_has_low_thrust() float sys_min_throttle = _params.minThrottle + (_params.hoverThrottle - _params.minThrottle) * 0.3f; // Check if thrust output is less than the minimum auto throttle param. - return _actuators.control[3] <= sys_min_throttle; + return _actuators.control[actuator_controls_s::INDEX_THROTTLE] <= sys_min_throttle; } bool MulticopterLandDetector::_has_minimal_thrust() @@ -348,7 +330,7 @@ bool MulticopterLandDetector::_has_minimal_thrust() } // Check if thrust output is less than the minimum auto throttle param. - return _actuators.control[3] <= sys_min_throttle; + return _actuators.control[actuator_controls_s::INDEX_THROTTLE] <= sys_min_throttle; } } // namespace land_detector diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index bf1e5e43261e..8599a2535711 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -66,21 +66,16 @@ class MulticopterLandDetector : public LandDetector MulticopterLandDetector(); protected: - virtual void _initialize_topics() override; + void _initialize_topics() override; + void _update_params() override; + void _update_topics() override; - virtual void _update_params() override; + bool _get_landed_state() override; + bool _get_ground_contact_state() override; + bool _get_maybe_landed_state() override; + bool _get_freefall_state() override; - virtual void _update_topics() override; - - virtual bool _get_landed_state() override; - - virtual bool _get_ground_contact_state() override; - - virtual bool _get_maybe_landed_state() override; - - virtual bool _get_freefall_state() override; - - virtual float _get_max_altitude() override; + float _get_max_altitude() override; private: /** Time in us that landing conditions have to hold before triggering a land. */ @@ -110,7 +105,7 @@ class MulticopterLandDetector : public LandDetector param_t freefall_trigger_time; param_t altitude_max; param_t landSpeed; - } _paramHandle; + } _paramHandle{}; struct { float maxClimbRate; @@ -124,26 +119,26 @@ class MulticopterLandDetector : public LandDetector float freefall_trigger_time; float altitude_max; float landSpeed; - } _params; - - int _vehicleLocalPositionSub; - int _vehicleLocalPositionSetpointSub; - int _actuatorsSub; - int _attitudeSub; - int _sensor_bias_sub; - int _vehicle_control_mode_sub; - int _battery_sub; - - struct vehicle_local_position_s _vehicleLocalPosition; - struct vehicle_local_position_setpoint_s _vehicleLocalPositionSetpoint; - struct actuator_controls_s _actuators; - struct vehicle_attitude_s _vehicleAttitude; - struct sensor_bias_s _sensors; - struct vehicle_control_mode_s _control_mode; - struct battery_status_s _battery; - - uint64_t _min_trust_start; ///< timestamp when minimum trust was applied first - uint64_t _landed_time; + } _params{}; + + int _vehicleLocalPositionSub{-1}; + int _vehicleLocalPositionSetpointSub{-1}; + int _actuatorsSub{-1}; + int _attitudeSub{-1}; + int _sensor_bias_sub{-1}; + int _vehicle_control_mode_sub{-1}; + int _battery_sub{-1}; + + vehicle_local_position_s _vehicleLocalPosition {}; + vehicle_local_position_setpoint_s _vehicleLocalPositionSetpoint {}; + actuator_controls_s _actuators {}; + vehicle_attitude_s _vehicleAttitude {}; + sensor_bias_s _sensors {}; + vehicle_control_mode_s _control_mode {}; + battery_status_s _battery {}; + + uint64_t _min_trust_start{0}; ///< timestamp when minimum trust was applied first + uint64_t _landed_time{0}; /* get control mode dependent pilot throttle threshold with which we should quit landed state and take off */ float _get_takeoff_throttle(); diff --git a/src/modules/land_detector/VtolLandDetector.cpp b/src/modules/land_detector/VtolLandDetector.cpp index 0be1d2540b55..dca943345563 100644 --- a/src/modules/land_detector/VtolLandDetector.cpp +++ b/src/modules/land_detector/VtolLandDetector.cpp @@ -70,7 +70,7 @@ void VtolLandDetector::_update_topics() bool VtolLandDetector::_get_maybe_landed_state() { // Only trigger in RW mode - if (!_vehicle_status.is_rotary_wing) { + if ((_vehicle_status.timestamp != 0) && !_vehicle_status.is_rotary_wing) { return false; } @@ -80,7 +80,7 @@ bool VtolLandDetector::_get_maybe_landed_state() bool VtolLandDetector::_get_landed_state() { // Only trigger in RW mode - if (!_vehicle_status.is_rotary_wing) { + if ((_vehicle_status.timestamp != 0) && !_vehicle_status.is_rotary_wing) { return false; }