mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-19 02:23:34 +08:00
land_detector: treat altitude mode like position mode with vertical checks but without horizontal checks
because in altitude mode we have a baro available and can therefore check vertical movement we can not check horizontal movement but I consider the checks for landing still pretty safe unlike in manual mode we are not allowed to disarm before land detection in altitude mode
This commit is contained in:
@@ -187,7 +187,7 @@ bool MulticopterLandDetector::_get_ground_contact_state()
|
|||||||
// If pilots commands down or in auto mode and we are already below minimal thrust and we do not move down we assume ground contact
|
// If pilots commands down or in auto mode and we are already below minimal thrust and we do not move down we assume ground contact
|
||||||
// TODO: we need an accelerometer based check for vertical movement for flying without GPS
|
// TODO: we need an accelerometer based check for vertical movement for flying without GPS
|
||||||
if (manual_control_idle_or_auto && _has_minimal_thrust() &&
|
if (manual_control_idle_or_auto && _has_minimal_thrust() &&
|
||||||
(!verticalMovement || !_has_position_lock())) {
|
(!verticalMovement || !_has_altitude_lock())) {
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -226,7 +226,7 @@ bool MulticopterLandDetector::_get_landed_state()
|
|||||||
}
|
}
|
||||||
|
|
||||||
// Return status based on armed state and throttle if no position lock is available.
|
// Return status based on armed state and throttle if no position lock is available.
|
||||||
if (!_has_position_lock()) {
|
if (!_has_altitude_lock()) {
|
||||||
// The system has minimum trust set (manual or in failsafe)
|
// The system has minimum trust set (manual or in failsafe)
|
||||||
// if this persists for 8 seconds AND the drone is not
|
// if this persists for 8 seconds AND the drone is not
|
||||||
// falling consider it to be landed. This should even sustain
|
// falling consider it to be landed. This should even sustain
|
||||||
@@ -260,7 +260,8 @@ bool MulticopterLandDetector::_get_landed_state()
|
|||||||
(fabsf(_vehicleAttitude.pitchspeed) > maxRotationScaled) ||
|
(fabsf(_vehicleAttitude.pitchspeed) > maxRotationScaled) ||
|
||||||
(fabsf(_vehicleAttitude.yawspeed) > maxRotationScaled);
|
(fabsf(_vehicleAttitude.yawspeed) > maxRotationScaled);
|
||||||
|
|
||||||
if (_ground_contact_hysteresis.get_state() && _has_minimal_thrust() && !rotating && !horizontalMovement) {
|
if (_ground_contact_hysteresis.get_state() && _has_minimal_thrust() && !rotating &&
|
||||||
|
(!horizontalMovement || !_has_position_lock())) {
|
||||||
// Ground contact, no thrust and no movement -> landed
|
// Ground contact, no thrust and no movement -> landed
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
@@ -308,12 +309,16 @@ float MulticopterLandDetector::_get_max_altitude()
|
|||||||
return valid_altitude_max;
|
return valid_altitude_max;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
bool MulticopterLandDetector::_has_altitude_lock()
|
||||||
|
{
|
||||||
|
return _vehicleLocalPosition.timestamp != 0 &&
|
||||||
|
hrt_elapsed_time(&_vehicleLocalPosition.timestamp) < 500000 &&
|
||||||
|
_vehicleLocalPosition.z_valid;
|
||||||
|
}
|
||||||
|
|
||||||
bool MulticopterLandDetector::_has_position_lock()
|
bool MulticopterLandDetector::_has_position_lock()
|
||||||
{
|
{
|
||||||
return !(_vehicleLocalPosition.timestamp == 0 ||
|
return _has_altitude_lock() && _vehicleLocalPosition.xy_valid;
|
||||||
hrt_elapsed_time(&_vehicleLocalPosition.timestamp) > 500000 ||
|
|
||||||
!_vehicleLocalPosition.xy_valid ||
|
|
||||||
!_vehicleLocalPosition.z_valid);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
bool MulticopterLandDetector::_has_manual_control_present()
|
bool MulticopterLandDetector::_has_manual_control_present()
|
||||||
|
|||||||
@@ -135,6 +135,7 @@ private:
|
|||||||
|
|
||||||
/* get control mode dependent pilot throttle threshold with which we should quit landed state and take off */
|
/* get control mode dependent pilot throttle threshold with which we should quit landed state and take off */
|
||||||
float _get_takeoff_throttle();
|
float _get_takeoff_throttle();
|
||||||
|
bool _has_altitude_lock();
|
||||||
bool _has_position_lock();
|
bool _has_position_lock();
|
||||||
bool _has_manual_control_present();
|
bool _has_manual_control_present();
|
||||||
bool _has_minimal_thrust();
|
bool _has_minimal_thrust();
|
||||||
|
|||||||
Reference in New Issue
Block a user