bool MulticopterLandDetector::_get_ground_contact_state()
{
// only trigger flight conditions if we are armed
if (!_arming.armed) {
return true;
}
// 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;
// land speed threshold
float land_speed_threshold = 0.9f * math::max(_params.landSpeed, 0.1f);
// 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;
if (hrt_elapsed_time(&_landed_time) < LAND_DETECTOR_LAND_PHASE_TIME_US) {
// Widen acceptance thresholds for landed state right after arming
// so that motor spool-up and other effects do not trigger false negatives.
verticalMovement = fabsf(_vehicleLocalPosition.vz) > _params.maxClimbRate * 2.5f;
} else {
// Adjust maxClimbRate if land_speed is lower than 2x maxClimbrate
float maxClimbRate = ((land_speed_threshold * 0.5f) < _params.maxClimbRate) ? (0.5f * land_speed_threshold) :
_params.maxClimbRate;
verticalMovement = fabsf(_vehicleLocalPosition.vz) > maxClimbRate;
}
// Check if we are moving horizontally.
bool horizontalMovement = sqrtf(_vehicleLocalPosition.vx * _vehicleLocalPosition.vx
+ _vehicleLocalPosition.vy * _vehicleLocalPosition.vy) > _params.maxVelocity;
// if we have a valid velocity setpoint and the vehicle is demanded to go down but no vertical movement present,
// we then can assume that the vehicle hit ground
bool in_descend = _is_climb_rate_enabled()
&& (_vehicleLocalPositionSetpoint.vz >= land_speed_threshold);
bool hit_ground = in_descend && !verticalMovement;
// 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_low_thrust() || hit_ground) && (!horizontalMovement || !_has_position_lock())
&& (!verticalMovement || !_has_altitude_lock())) {
return true;
}
return false;
}
bool MulticopterLandDetector::_get_ground_contact_state()
{
// When not armed, consider to have ground-contact
if (!_arming.armed) {
return true;
}
// land speed threshold
float land_speed_threshold = 0.9f * math::max(_params.landSpeed, 0.1f);
// 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;
if (hrt_elapsed_time(&_landed_time) < LAND_DETECTOR_LAND_PHASE_TIME_US) {
// Widen acceptance thresholds for landed state right after arming
// so that motor spool-up and other effects do not trigger false negatives.
verticalMovement = fabsf(_vehicleLocalPosition.vz) > _params.maxClimbRate * 2.5f;
} else {
// Adjust maxClimbRate if land_speed is lower than 2x maxClimbrate
float maxClimbRate = ((land_speed_threshold * 0.5f) < _params.maxClimbRate) ? (0.5f * land_speed_threshold) :
_params.maxClimbRate;
verticalMovement = fabsf(_vehicleLocalPosition.vz) > maxClimbRate;
}
// Check if we are moving horizontally.
_horizontal_movement = sqrtf(_vehicleLocalPosition.vx * _vehicleLocalPosition.vx
+ _vehicleLocalPosition.vy * _vehicleLocalPosition.vy) > _params.maxVelocity;
// if we have a valid velocity setpoint and the vehicle is demanded to go down but no vertical movement present,
// we then can assume that the vehicle hit ground
_in_descend = _is_climb_rate_enabled()
&& (_vehicleLocalPositionSetpoint.vz >= land_speed_threshold);
bool hit_ground = _in_descend && !verticalMovement;
// TODO: we need an accelerometer based check for vertical movement for flying without GPS
if ((_has_low_thrust() || hit_ground) && (!_horizontal_movement || !_has_position_lock())
&& (!verticalMovement || !_has_altitude_lock())) {
return true;
}
return false;
}
