diff --git a/msg/vehicle_land_detected.msg b/msg/vehicle_land_detected.msg index 1552967a8c..a044cb224e 100644 --- a/msg/vehicle_land_detected.msg +++ b/msg/vehicle_land_detected.msg @@ -4,3 +4,4 @@ bool freefall # true if vehicle is currently in free-fall bool ground_contact # true if vehicle has ground contact but is not landed bool maybe_landed # true if the vehicle might have landed float32 alt_max # maximum altitude in [m] that can be reached +bool in_ground_effect # indicates if from the perspective of the landing detector the vehicle might be in ground effect (baro). This flag will become true if the vehicle is not moving horizontally and is descending (crude assumption that user is landing). diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 7093f6a25a..ca40771381 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -1250,6 +1250,7 @@ void Ekf2::run() if (vehicle_land_detected_updated) { if (orb_copy(ORB_ID(vehicle_land_detected), _vehicle_land_detected_sub, &vehicle_land_detected) == PX4_OK) { _ekf.set_in_air_status(!vehicle_land_detected.landed); + //_ekf.set_gnd_effect_flag(vehicle_land_detected.in_ground_effect); } } diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index c63bcf79ff..1aa977112a 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -125,6 +125,8 @@ void LandDetector::_cycle() const bool ground_contactDetected = (_state == LandDetectionState::GROUND_CONTACT); const float alt_max = _get_max_altitude() > 0.0f ? _get_max_altitude() : INFINITY; + const bool in_ground_effect = _ground_effect_hysteresis.get_state(); + const hrt_abstime now = hrt_absolute_time(); // publish at 1 Hz, very first time, or when the result has changed @@ -134,6 +136,7 @@ void LandDetector::_cycle() (_landDetected.freefall != freefallDetected) || (_landDetected.maybe_landed != maybe_landedDetected) || (_landDetected.ground_contact != ground_contactDetected) || + (_landDetected.in_ground_effect != in_ground_effect) || (fabsf(_landDetected.alt_max - alt_max) > FLT_EPSILON)) { if (!landDetected && _landDetected.landed) { @@ -147,6 +150,7 @@ void LandDetector::_cycle() _landDetected.maybe_landed = maybe_landedDetected; _landDetected.ground_contact = ground_contactDetected; _landDetected.alt_max = alt_max; + _landDetected.in_ground_effect = in_ground_effect; int instance; orb_publish_auto(ORB_ID(vehicle_land_detected), &_landDetectedPub, &_landDetected, @@ -207,6 +211,7 @@ void LandDetector::_update_state() _landed_hysteresis.set_state_and_update(_get_landed_state()); _maybe_landed_hysteresis.set_state_and_update(_get_maybe_landed_state()); _ground_contact_hysteresis.set_state_and_update(_get_ground_contact_state()); + _ground_effect_hysteresis.set_state_and_update(_get_ground_effect_state()); if (_freefall_hysteresis.get_state()) { _state = LandDetectionState::FREEFALL; diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index 1ee7e4557a..a60c2a9eb4 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -137,6 +137,11 @@ protected: */ virtual float _get_max_altitude() = 0; + /** + * @return true if vehicle could be in ground effect (close to ground) + */ + virtual bool _get_ground_effect_state() { return false; } + /** * Convenience function for polling uORB subscriptions. * @@ -156,6 +161,7 @@ protected: systemlib::Hysteresis _landed_hysteresis{true}; systemlib::Hysteresis _maybe_landed_hysteresis{true}; systemlib::Hysteresis _ground_contact_hysteresis{true}; + systemlib::Hysteresis _ground_effect_hysteresis{false}; struct actuator_armed_s _arming {}; diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 3e54f3d0c9..31d3b5194e 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -179,17 +179,17 @@ bool MulticopterLandDetector::_get_ground_contact_state() } // Check if we are moving horizontally. - bool horizontalMovement = sqrtf(_vehicleLocalPosition.vx * _vehicleLocalPosition.vx - + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy) > _params.maxVelocity; + _horizontalMovement = sqrtf(_vehicleLocalPosition.vx * _vehicleLocalPosition.vx + + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy) > _params.maxVelocity; // if we have a valid velocity setpoint and the vehicle is demanded to go down but no vertical movement present, // we then can assume that the vehicle hit ground - bool in_descend = _is_climb_rate_enabled() - && (_vehicleLocalPositionSetpoint.vz >= land_speed_threshold); - bool hit_ground = in_descend && !verticalMovement; + _in_descend = _is_climb_rate_enabled() + && (_vehicleLocalPositionSetpoint.vz >= land_speed_threshold); + bool hit_ground = _in_descend && !verticalMovement; // TODO: we need an accelerometer based check for vertical movement for flying without GPS - if ((_has_low_thrust() || hit_ground) && (!horizontalMovement || !_has_position_lock()) + if ((_has_low_thrust() || hit_ground) && (!_horizontalMovement || !_has_position_lock()) && (!verticalMovement || !_has_altitude_lock())) { return true; } @@ -334,4 +334,14 @@ bool MulticopterLandDetector::_has_minimal_thrust() return _actuators.control[actuator_controls_s::INDEX_THROTTLE] <= sys_min_throttle; } +bool MulticopterLandDetector::_get_ground_effect_state() +{ + if (_in_descend && !_horizontalMovement) { + return true; + + } else { + return false; + } +} + } // namespace land_detector diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index ea470cd501..fb89d0b278 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -76,6 +76,7 @@ protected: bool _get_ground_contact_state() override; bool _get_maybe_landed_state() override; bool _get_freefall_state() override; + bool _get_ground_effect_state() override; float _get_max_altitude() override; private: @@ -140,6 +141,9 @@ private: hrt_abstime _min_trust_start{0}; ///< timestamp when minimum trust was applied first hrt_abstime _landed_time{0}; + bool _in_descend{false}; ///< vehicle is desending + bool _horizontalMovement{false}; ///< vehicle is moving horizontally + /* get control mode dependent pilot throttle threshold with which we should quit landed state and take off */ float _get_takeoff_throttle(); bool _has_altitude_lock();