Browse Source

land_detector: refactored helper method names

sbg
Matthias Grob 8 years ago committed by Lorenz Meier
parent
commit
9963cf532d
  1. 20
      src/modules/land_detector/MulticopterLandDetector.cpp
  2. 6
      src/modules/land_detector/MulticopterLandDetector.h

20
src/modules/land_detector/MulticopterLandDetector.cpp

@ -158,7 +158,7 @@ bool MulticopterLandDetector::_get_ground_contact_state()
// Check if user commands throttle and if so, report no ground contact based on // 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 // the user intent to take off (even if the system might physically still have
// ground contact at this point). // ground contact at this point).
const bool manual_control_move_down = _get_manual_control_present() && _manual.z < 0.05f; const bool manual_control_move_down = _has_manual_control_present() && _manual.z < 0.05f;
// Widen acceptance thresholds for landed state right after arming // Widen acceptance thresholds for landed state right after arming
// so that motor spool-up and other effects do not trigger false negatives. // so that motor spool-up and other effects do not trigger false negatives.
@ -175,8 +175,8 @@ bool MulticopterLandDetector::_get_ground_contact_state()
// If pilots commands fully down or already below minimal thrust because of auto land and we do not move down we assume ground contact // If pilots commands fully down or already below minimal thrust because of auto land 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_move_down || !_control_mode.flag_control_manual_enabled) && _get_minimal_thrust()) && if (((manual_control_move_down || !_control_mode.flag_control_manual_enabled) && _has_minimal_thrust()) &&
(!verticalMovement || !_get_position_lock_available())) { (!verticalMovement || !_has_position_lock())) {
return true; return true;
} }
@ -194,7 +194,7 @@ bool MulticopterLandDetector::_get_landed_state()
} }
// If we control manually and are still landed, we want to stay idle until the pilot rises the throttle for takeoff // 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 && _get_manual_control_present()) { if (_state == LandDetectionState::LANDED && _has_manual_control_present()) {
if (_manual.z < _get_takeoff_throttle()) { if (_manual.z < _get_takeoff_throttle()) {
return true; return true;
@ -204,7 +204,7 @@ bool MulticopterLandDetector::_get_landed_state()
} }
} }
if (_get_minimal_thrust()) { if (_has_minimal_thrust()) {
if (_min_trust_start == 0) { if (_min_trust_start == 0) {
_min_trust_start = now; _min_trust_start = now;
} }
@ -214,7 +214,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 (!_get_position_lock_available()) { if (!_has_position_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
@ -248,7 +248,7 @@ 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() && _get_minimal_thrust() && !rotating && !horizontalMovement) { if (_ground_contact_hysteresis.get_state() && _has_minimal_thrust() && !rotating && !horizontalMovement) {
// Ground contact, no thrust and no movement -> landed // Ground contact, no thrust and no movement -> landed
return true; return true;
} }
@ -276,7 +276,7 @@ float MulticopterLandDetector::_get_takeoff_throttle()
return 0.0f; return 0.0f;
} }
bool MulticopterLandDetector::_get_position_lock_available() bool MulticopterLandDetector::_has_position_lock()
{ {
return !(_vehicleLocalPosition.timestamp == 0 || return !(_vehicleLocalPosition.timestamp == 0 ||
hrt_elapsed_time(&_vehicleLocalPosition.timestamp) > 500000 || hrt_elapsed_time(&_vehicleLocalPosition.timestamp) > 500000 ||
@ -284,12 +284,12 @@ bool MulticopterLandDetector::_get_position_lock_available()
!_vehicleLocalPosition.z_valid); !_vehicleLocalPosition.z_valid);
} }
bool MulticopterLandDetector::_get_manual_control_present() bool MulticopterLandDetector::_has_manual_control_present()
{ {
return _control_mode.flag_control_manual_enabled && _manual.timestamp > 0; return _control_mode.flag_control_manual_enabled && _manual.timestamp > 0;
} }
bool MulticopterLandDetector::_get_minimal_thrust() bool MulticopterLandDetector::_has_minimal_thrust()
{ {
// 10% of throttle range between min and hover // 10% of throttle range between min and hover
float sys_min_throttle = _params.minThrottle + (_params.hoverThrottle - _params.minThrottle) * _params.throttleRange; float sys_min_throttle = _params.minThrottle + (_params.hoverThrottle - _params.minThrottle) * _params.throttleRange;

6
src/modules/land_detector/MulticopterLandDetector.h

@ -124,9 +124,9 @@ 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 _get_position_lock_available(); bool _has_position_lock();
bool _get_manual_control_present(); bool _has_manual_control_present();
bool _get_minimal_thrust(); bool _has_minimal_thrust();
}; };

Loading…
Cancel
Save