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;
}
Exemplo n.º 2
0
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;
}
Exemplo n.º 3
0
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;
}
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;
}
Exemplo n.º 5
0
bool MulticopterLandDetector::_has_position_lock()
{
	return _has_altitude_lock() && _vehicleLocalPosition.xy_valid;
}
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;
}