bool MulticopterLandDetector::_get_maybe_landed_state()
{
// Time base for this function
const hrt_abstime now = hrt_absolute_time();
// When not armed, consider to be maybe-landed
if (!_arming.armed) {
return true;
}
if (_has_minimal_thrust()) {
if (_min_trust_start == 0) {
_min_trust_start = now;
}
} else {
_min_trust_start = 0;
}
float landThresholdFactor = 1.0f;
// Widen acceptance thresholds for landed state right after landed
if (hrt_elapsed_time(&_landed_time) < LAND_DETECTOR_LAND_PHASE_TIME_US) {
landThresholdFactor = 2.5f;
}
// Next look if all rotation angles are not moving.
float maxRotationScaled = _params.maxRotation_rad_s * landThresholdFactor;
bool rotating = (fabsf(_vehicleAttitude.rollspeed) > maxRotationScaled) ||
(fabsf(_vehicleAttitude.pitchspeed) > maxRotationScaled) ||
(fabsf(_vehicleAttitude.yawspeed) > maxRotationScaled);
// Return status based on armed state and throttle if no position lock is available.
if (!_has_altitude_lock() && !rotating) {
// The system has minimum trust set (manual or in failsafe)
// if this persists for 8 seconds AND the drone is not
// falling consider it to be landed. This should even sustain
// quite acrobatic flight.
return (_min_trust_start > 0) && (hrt_elapsed_time(&_min_trust_start) > 8_s);
}
if (_ground_contact_hysteresis.get_state() && _has_minimal_thrust() && !rotating) {
// Ground contact, no thrust and no movement -> landed
return true;
}
return false;
}
bool MulticopterLandDetector::_get_ground_contact_state()
{
// Time base for this function
const uint64_t now = hrt_absolute_time();
// only trigger flight conditions if we are armed
if (!_arming.armed) {
_arming_time = 0;
return true;
} else if (_arming_time == 0) {
_arming_time = now;
}
// If in manual flight mode never report landed if the user has more than idle throttle
// 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
// ground contact at this point).
const bool manual_control_idle = (_has_manual_control_present() && _manual.z < _params.manual_stick_down_threshold);
const bool manual_control_idle_or_auto = manual_control_idle || !_control_mode.flag_control_manual_enabled;
// Widen acceptance thresholds for landed state right after arming
// so that motor spool-up and other effects do not trigger false negatives.
float armThresholdFactor = 1.0f;
if (hrt_elapsed_time(&_arming_time) < LAND_DETECTOR_ARM_PHASE_TIME_US) {
armThresholdFactor = 2.5f;
}
// Check if we are moving vertically - this might see a spike after arming due to
// throttle-up vibration. If accelerating fast the throttle thresholds will still give
// an accurate in-air indication.
bool verticalMovement = fabsf(_vehicleLocalPosition.vz) > _params.maxClimbRate * armThresholdFactor;
// 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
if (manual_control_idle_or_auto && _has_minimal_thrust() &&
(!verticalMovement || !_has_altitude_lock())) {
return true;
}
return false;
}
bool MulticopterLandDetector::_get_maybe_landed_state()
{
// Time base for this function
const uint64_t now = hrt_absolute_time();
// only trigger flight conditions if we are armed
if (!_arming.armed) {
return true;
}
// 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 && _has_manual_control_present()) {
if (_manual.z < _get_takeoff_throttle()) {
return true;
} else {
// Pilot wants to take off, assume no groundcontact anymore and therefore allow thrust
_ground_contact_hysteresis.set_state_and_update(false);
return false;
}
}
if (_has_minimal_thrust()) {
if (_min_trust_start == 0) {
_min_trust_start = now;
}
} else {
_min_trust_start = 0;
}
float landThresholdFactor = 1.0f;
// Widen acceptance thresholds for landed state right after landed
// so that motor spool-up and other effects do not trigger false negatives.
if (hrt_elapsed_time(&_landed_time) < LAND_DETECTOR_LAND_PHASE_TIME_US) {
landThresholdFactor = 2.5f;
}
// Next look if all rotation angles are not moving.
float maxRotationScaled = _params.maxRotation_rad_s * landThresholdFactor;
bool rotating = (fabsf(_vehicleAttitude.rollspeed) > maxRotationScaled) ||
(fabsf(_vehicleAttitude.pitchspeed) > maxRotationScaled) ||
(fabsf(_vehicleAttitude.yawspeed) > maxRotationScaled);
// Return status based on armed state and throttle if no position lock is available.
if (!_has_altitude_lock() && !rotating) {
// The system has minimum trust set (manual or in failsafe)
// if this persists for 8 seconds AND the drone is not
// falling consider it to be landed. This should even sustain
// quite acrobatic flight.
if ((_min_trust_start > 0) &&
(hrt_elapsed_time(&_min_trust_start) > 8000000)) {
return true;
} else {
return false;
}
}
if (_ground_contact_hysteresis.get_state() && _has_minimal_thrust() && !rotating) {
// Ground contact, no thrust and no movement -> landed
return true;
}
return false;
}
