PX4着陆检测

为什么要着陆检测

对于开发一款飞控而言，起飞和落地的检测是必不可少的，甚至占有举足轻重的地位。比如，飞控内部起飞前控制器的一些初始化设置，落地及时自动加锁等功能都必须要基于这个功能，如果出现判定不准确或者误判的情况，会导致炸鸡的风险。在PX4的代码中着落检测和起飞检测写在了一起，判断函数为_get_landed_state()，如果飞机当前在地面上(还没有起飞或者已经着陆)，则返回true，如果在空中则返回false。着陆检测的代码在Firmware\src\modules\land_detector中。下面将结合代码分析。

旋翼的着陆检测

旋翼的着陆检测主要根据解锁情况、油门、GPS的情况以及水平速度、垂直速度、旋转情况来判断。一下的代码摘自MulticopterLandDetector.cpp

	// 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;
	}

 首先根据当前的飞行模式，是手动模式还是姿态控制模式得到一个系统最小的油门值sys_min_throttle(maxThrottle和minManThrottle代表的是什么意思？)。然后判断当前油门是否小于这个系统最小的油门值，记录下符合条件的最早时间_min_trust_start，这个时间后面要用到。

	// only trigger flight conditions if we are armed
	if (!_arming.armed) {
		_arming_time = 0;

		return true;

	} else if (_arming_time == 0) {
		_arming_time = now;
	}

根据解锁状态来判断。如果还没有解锁，肯定在地面上，返回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 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;
	}

在手动模式下，如果油门值大于0.15(这个0.15是怎么来的？)，则认定为在空中，返回false。如果有拉杆的动作，油门值超过0.15说明正要起飞了，或者已经在空中(反正不可能在地面上)。

	// 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 true;

		} else {
			return false;
		}
	}

这里是根据GPS位置的一个判断。如果没有GPS的数据，或者丢失时间已经超过5秒了，执行下面的判断，如果距离_min_trust_start已经超过8秒，说明这8秒内的油门都是低于系统油门阈值的，认定为在地面上，否则认为在空中。

	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_US) {
		armThresholdFactor = 2.5f;
	}

这里设定一个 解锁的因子，如果距离解锁的时间在LAND_DETECTOR_ARM_PHASE_TIME_US的时间内，这个因子大一点，否则的话小一点。我猜这是因为在解锁到拉杆的这段时间内，着地检测的要求要严格一点，如果超过一定时间了，可以松一点，这样可以防止起飞的时候飞机在地面上移动带来的误判。

	// 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.
		return false;
	}

	return true;

下面是根据上述因子计算得到的阈值。主要有水平方向的移动，垂直方向的移动(这个移动值得应该是速度) 以及旋转角速度的一个阈值。只要有一个超过阈值则认为是起飞状态，否则认定为着陆状态

固定翼的着陆检测

固定翼的着陆检测好像简单点，如果未解锁，返回true，已经在地面上，主要从水平面的速度_velocity_xy_filtered，爬升速度_velocity_z_filtered，空速计速度_airspeed_filtered，水平加速度大小_accel_horz_lp4个指标来判断。只有这4个指标同时小于参数的阈值才认定为除于陆地状态，否则认为起飞。

	// only trigger flight conditions if we are armed
	if (!_arming.armed) {
		return true;
	}

	bool landDetected = false;

	if (hrt_elapsed_time(&_controlState.timestamp) < 500 * 1000) {
		float val = 0.97f * _velocity_xy_filtered + 0.03f * sqrtf(_controlState.x_vel *
				_controlState.x_vel + _controlState.y_vel * _controlState.y_vel);

		if (PX4_ISFINITE(val)) {
			_velocity_xy_filtered = val;
		}

		val = 0.99f * _velocity_z_filtered + 0.01f * fabsf(_controlState.z_vel);

		if (PX4_ISFINITE(val)) {
			_velocity_z_filtered = val;
		}

		_airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s;

		// a leaking lowpass prevents biases from building up, but
		// gives a mostly correct response for short impulses
		_accel_horz_lp = _accel_horz_lp * 0.8f + _controlState.horz_acc_mag * 0.18f;

		// crude land detector for fixedwing
		if (_velocity_xy_filtered < _params.maxVelocity
		    && _velocity_z_filtered < _params.maxClimbRate
		    && _airspeed_filtered < _params.maxAirSpeed
		    && _accel_horz_lp < _params.maxIntVelocity) {

			landDetected = true;

		} else {
			landDetected = false;
		}

	} else {
		// Control state topic has timed out and we need to assume we're landed.
		landDetected = true;
	}

	return landDetected;
}

需要注意的是，这几个参数的计算都采用了一个低通滤波，但是0.97,0.95,0.99这种参数又是怎么来的呢？ 同时这样的判断我觉得也存在一些缺陷，飞机在地面上起飞阶段和降落阶段的滑行会被认为是飞行状态而不是着陆状态，一定要飞机除于比较静止的状态才认为是着陆。

扑翼的着陆检测(设想)

扑翼的起飞是需要用手抛出的，着陆的时候也需要用手接。任何一个最基本的判断是解锁，如果未解锁肯定在陆地上。手抛起飞的检测可以参考固定翼的，不过可能参数要稍微修改下，扑翼的移动速度没有固定翼那么快。最难的部分是手接落地的检测，需要识别到那部分时间是人接住了这个飞机，认定为已经着陆，因为接住飞机之后通常都是发生移动，单纯使用速度和加速度来识别的话容易引起误判。这时可以加上油门的阈值判断，一般手接时候油门都会达到底来让飞机的翅膀不拍动。所以整理流程规划如下：

1、判断是否解锁，没有解锁认为在陆地上。

2、得到飞机的水平移动速度，爬升速度，以及z轴的加速度，只要超过阈值且油门大于一定阈值认定为起飞，否则的话认定为着陆(飞机在手上)

参考资料

https://www.sohu.com/a/135335731_466960