【record】tof定高（三）——定高模式bug与解决方案

翁庆年的六便士

已于 2022-10-23 10:45:58 修改

阅读量438

点赞数

分类专栏：【drone】Alientek_wukongFPV 文章标签：算法

于 2022-10-21 22:32:10 首次发布

本文链接：https://blog.csdn.net/weixin_43705900/article/details/127454746

版权

【drone】Alientek_wukongFPV 专栏收录该内容

10 篇文章 6 订阅

订阅专栏

本文介绍了正点原子无人机在定高模式下出现的一个bug，即油门居中时飞机缓慢下降的问题。问题源于代码逻辑错误，当rcThrottleAdjustment为0且isAdjustAlttitude为真时，期望位置被不断更新。为解决此问题，增加了油门变化方向的判断，只有在油门从高到中或从低到中时才更新期望高度。修改后的代码确保了在油门中位时期望高度保持固定，从而改善了飞行效果。

摘要由CSDN通过智能技术生成

1、说明

上篇讲到，将tof的数据融合加速度融合得到飞机的实时高度和实时速度，就可以实现定高模式；在正点原子最新发布的固件中，就已经有定高模式的实现，只是之前用的是气压计的融合结果，飞行效果强差人意；而tof输出的精度更好，定高的效果将会优于前者；
但是在实际测试的过程，发现了正点原子定高模式下的一个bug;该bug就是在油门居于中位的时候，期望的位置居然在随着融合的高度在变，而不是一个固定的值，所以飞机就会缓慢下降；

2、问题现象

可以看到，当我把油门居于中位时，期望的高度居然没有固定住，而是随着融合的高度在变化，飞机的实际飞行效果就是会出现缓慢下降；
在这里插入图片描述

3、主要原因

（1）code逻辑问题

		//调整设定高度
		int16_t rcThrottleAdjustment = applyDeadband(rcCommand[THROTTLE] - altHoldThrottleBase, ALT_HOLD_DEADBAND);
		if (rcThrottleAdjustment == 0 && isAdjustAlttitude == true)
		{
			setpoint->mode.z = modeAbs;
			setpoint->position.z = state->position.z;
			isAdjustAlttitude = false;
		}
		else if (rcThrottleAdjustment > 0)
		{
			setpoint->mode.z = modeVelocity;
			setpoint->velocity.z = rcThrottleAdjustment * CLIMB_RATE_UP / (RC_MAX - altHoldThrottleBase - ALT_HOLD_DEADBAND);
			isAdjustAlttitude = true;
		}
		else
		{
			setpoint->mode.z = modeVelocity;
			setpoint->velocity.z = rcThrottleAdjustment * CLIMB_RATE_DOWN / (altHoldThrottleBase - RC_MIN - ALT_HOLD_DEADBAND);
			isAdjustAlttitude = true;
		}

问题就是出在最后一个else时，当rcThrottleAdjustment == 0 && isAdjustAlttitude == true条件满足的时候，期望的位置就会被赋予当前位置这个值，然后将isAdjustAlttitude = false;
而下一次执行到这里的时候，就会去执行最后一个else，导致isAdjustAlttitude = true;
此时只要油门还是在中位时，期望的高度就会不断的被赋予实际的高度，而不是一个固定的值；

4、解决方案

//调整设定高度
		int16_t rcThrottleAdjustment = applyDeadband(rcCommand[THROTTLE] - altHoldThrottleBase, ALT_HOLD_DEADBAND);
		static int pre_level = -1;
		static int now_level = -1;
		static bool level_Jump = false;
		if(rcThrottleAdjustment > 0)
		{
			now_level = 1;
		}
		else if(rcThrottleAdjustment == 0)
		{
			now_level = 0;
		}
		else
		{
			now_level = -1;
		}
		if(now_level == 0 && pre_level == -1 || now_level == 0 && pre_level == 1)
		{
			level_Jump = true;
		}
		else
		{
			level_Jump = false;
		}
		pre_level = now_level;

		if (rcThrottleAdjustment == 0 && isAdjustAlttitude == true)
		{
			setpoint->mode.z = modeAbs;
			if(level_Jump == true)
			{
				setpoint->position.z = state->position.z;
			}
			isAdjustAlttitude = false;
		}
		else if (rcThrottleAdjustment > 0)
		{
			setpoint->mode.z = modeVelocity;
			setpoint->velocity.z = rcThrottleAdjustment * CLIMB_RATE_UP / (RC_MAX - altHoldThrottleBase - ALT_HOLD_DEADBAND);
			isAdjustAlttitude = true;
		}
		else
		{
			setpoint->mode.z = modeVelocity;
			setpoint->velocity.z = rcThrottleAdjustment * CLIMB_RATE_DOWN / (altHoldThrottleBase - RC_MIN - ALT_HOLD_DEADBAND);
			isAdjustAlttitude = true;
		}
	}