Browse Source

VtolLandDetector: remove airspeed check

This commit removes the additional airspeed check (airspeed for VTOLs in
hover below LNDFW_AIRSPD_MAX), as it is not a required condition in the
landed state (headwind blowing into the airspeed sensor won't stop
once on the gruond). In FW mode the check would make more sense, but there
the land detector is currently simply disabled.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
main
Silvan Fuhrer 3 years ago
parent
commit
85a621303d
  1. 26
      src/modules/land_detector/VtolLandDetector.cpp
  2. 19
      src/modules/land_detector/VtolLandDetector.h

26
src/modules/land_detector/VtolLandDetector.cpp

@ -69,31 +69,7 @@ bool VtolLandDetector::_get_landed_state() @@ -69,31 +69,7 @@ bool VtolLandDetector::_get_landed_state()
return !_armed;
}
// this is returned from the mutlicopter land detector
bool landed = MulticopterLandDetector::_get_landed_state();
// for vtol we additionally consider airspeed
airspeed_validated_s airspeed_validated{};
_airspeed_validated_sub.copy(&airspeed_validated);
if (hrt_elapsed_time(&airspeed_validated.timestamp) < 1_s && PX4_ISFINITE(airspeed_validated.true_airspeed_m_s)) {
_airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * airspeed_validated.true_airspeed_m_s;
} else {
// if airspeed does not update, set it to zero and rely on multicopter land detector
_airspeed_filtered = 0.0f;
}
// only consider airspeed if we have been in air before to avoid false
// detections in the case of wind on the ground
if (_was_in_air && (_airspeed_filtered > _param_lndfw_airspd_max.get())) {
landed = false;
}
_was_in_air = !landed;
return landed;
return MulticopterLandDetector::_get_landed_state();
}
bool VtolLandDetector::_get_freefall_state()

19
src/modules/land_detector/VtolLandDetector.h

@ -1,6 +1,6 @@ @@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
* Copyright (c) 2013-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -33,7 +33,9 @@ @@ -33,7 +33,9 @@
/**
* @file VtolLandDetector.h
* Land detection implementation for VTOL also called hybrids.
* Land detection implementation for VTOLs.
* It uses the MC land detector in hover, while land detection in FW
* is 1:1 linked to the (boolean) armed state.
*
* @author Roman Bapst <bapstr@gmail.com>
* @author Julian Oes <julian@oes.ch>
@ -41,8 +43,6 @@ @@ -41,8 +43,6 @@
#pragma once
#include <uORB/topics/airspeed_validated.h>
#include "MulticopterLandDetector.h"
namespace land_detector
@ -59,17 +59,6 @@ protected: @@ -59,17 +59,6 @@ protected:
bool _get_landed_state() override;
bool _get_maybe_landed_state() override;
bool _get_freefall_state() override;
private:
uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)};
bool _was_in_air{false}; /**< indicates whether the vehicle was in the air in the previous iteration */
float _airspeed_filtered{0.0f}; /**< low pass filtered airspeed */
DEFINE_PARAMETERS_CUSTOM_PARENT(
MulticopterLandDetector,
(ParamFloat<px4::params::LNDFW_AIRSPD_MAX>) _param_lndfw_airspd_max
);
};
} // namespace land_detector

Loading…
Cancel
Save