LandDetectionResult MulticopterLandDetector::update()
{
// first poll for new data from our subscriptions
updateSubscriptions();
updateParameterCache(false);
if (get_freefall_state()) {
_state = LANDDETECTION_RES_FREEFALL;
} else if (get_landed_state()) {
_state = LANDDETECTION_RES_LANDED;
} else {
_state = LANDDETECTION_RES_FLYING;
}
return _state;
}
bool MulticopterLandDetector::get_landed_state()
{
// Time base for this function
const uint64_t now = hrt_absolute_time();
// Check if thrust output is less than max throttle param.
bool minimalThrust = (_actuators.control[3] <= (_params.maxThrottle + 0.05f));
if (minimalThrust && _min_trust_start == 0) {
_min_trust_start = now;
} else if (!minimalThrust) {
_min_trust_start = 0;
}
// only trigger flight conditions if we are armed
if (!_arming.armed) {
_arming_time = 0;
return true;
} else if (_arming_time == 0) {
_arming_time = now;
}
// Return status based on armed state and throttle if no position lock is available.
if (_vehicleLocalPosition.timestamp == 0 ||
hrt_elapsed_time(&_vehicleLocalPosition.timestamp) > 500000 ||
!_vehicleLocalPosition.xy_valid ||
!_vehicleLocalPosition.z_valid) {
// Check if user commands throttle and if so, report not landed
if (_manual.z > _params.minManThrottle + 0.01f) {
return false;
}
if ((_min_trust_start > 0) &&
(hrt_elapsed_time(&_min_trust_start) > 5 * 1000 * 1000)) {
return !get_freefall_state();
} else {
return false;
}
}
float armThresholdFactor = 1.0f;
// Widen acceptance thresholds for landed state right after arming
// so that motor spool-up and other effects do not trigger false negatives.
if (hrt_elapsed_time(&_arming_time) < LAND_DETECTOR_ARM_PHASE_TIME) {
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;
// Check if we are moving horizontally.
bool horizontalMovement = sqrtf(_vehicleLocalPosition.vx * _vehicleLocalPosition.vx
+ _vehicleLocalPosition.vy * _vehicleLocalPosition.vy) > _params.maxVelocity;
// Next look if all rotation angles are not moving.
float maxRotationScaled = _params.maxRotation_rad_s * armThresholdFactor;
bool rotating = (fabsf(_vehicleAttitude.rollspeed) > maxRotationScaled) ||
(fabsf(_vehicleAttitude.pitchspeed) > maxRotationScaled) ||
(fabsf(_vehicleAttitude.yawspeed) > maxRotationScaled);
if (verticalMovement || rotating || !minimalThrust || horizontalMovement) {
// Sensed movement or thottle high, so reset the land detector.
_landTimer = now;
return false;
}
return (now - _landTimer > LAND_DETECTOR_TRIGGER_TIME);
}
bool MulticopterLandDetector::get_landed_state()
{
// Time base for this function
const uint64_t now = hrt_absolute_time();
float sys_min_throttle = (_params.maxThrottle + 0.01f);
// Determine the system min throttle based on flight mode
if (!_ctrl_mode.flag_control_altitude_enabled) {
sys_min_throttle = (_params.minManThrottle + 0.01f);
}
// Check if thrust output is less than the minimum auto throttle param.
bool minimalThrust = (_actuators.control[3] <= sys_min_throttle);
if (minimalThrust && _min_trust_start == 0) {
_min_trust_start = now;
} else if (!minimalThrust) {
_min_trust_start = 0;
}
// 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 not landed based on
// the user intent to take off (even if the system might physically still have
// ground contact at this point).
if (_manual.timestamp > 0 && _manual.z > 0.15f && _ctrl_mode.flag_control_manual_enabled) {
return false;
}
// Return status based on armed state and throttle if no position lock is available.
if (_vehicleLocalPosition.timestamp == 0 ||
hrt_elapsed_time(&_vehicleLocalPosition.timestamp) > 500000 ||
!_vehicleLocalPosition.xy_valid ||
!_vehicleLocalPosition.z_valid) {
// 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) > 8 * 1000 * 1000)) {
return !get_freefall_state();
} else {
return false;
}
}
float armThresholdFactor = 1.0f;
// Widen acceptance thresholds for landed state right after arming
// so that motor spool-up and other effects do not trigger false negatives.
if (hrt_elapsed_time(&_arming_time) < LAND_DETECTOR_ARM_PHASE_TIME) {
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;
// Check if we are moving horizontally.
bool horizontalMovement = sqrtf(_vehicleLocalPosition.vx * _vehicleLocalPosition.vx
+ _vehicleLocalPosition.vy * _vehicleLocalPosition.vy) > _params.maxVelocity;
// Next look if all rotation angles are not moving.
float maxRotationScaled = _params.maxRotation_rad_s * armThresholdFactor;
bool rotating = (fabsf(_vehicleAttitude.rollspeed) > maxRotationScaled) ||
(fabsf(_vehicleAttitude.pitchspeed) > maxRotationScaled) ||
(fabsf(_vehicleAttitude.yawspeed) > maxRotationScaled);
if (verticalMovement || rotating || !minimalThrust || horizontalMovement) {
// Sensed movement or thottle high, so reset the land detector.
_landTimer = now;
return false;
}
return (now - _landTimer > LAND_DETECTOR_TRIGGER_TIME);
}
