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

1、说明

上一篇内容中，我通过判断油门摇杆是否从非中位跳到中位这个事件，作为是否将当前位置更新到期望位置中的条件，从而解决期望位置一直被赋值的bug；但是实际上，程序还是存在逻辑问题，也就当油门在中位的时候，Z轴方向的模式不断的被切换成速度模式与位置模式，这是不应该的；

2、code解读

（1）原有错误的code逻辑

如下code所示，当油门居于中位，Z轴方向的控制模式不断的被切换成速度模式与位置模式；而正常的逻辑是，当油门居于中位，Z轴方向的控制模式只应该是位置模式；

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

（2）正确的控制code修改

code修改成正常的逻辑是，当油门居于中位，Z轴方向的控制模式只有位置模式；修改的地方就是最后一个else判断加入一个条件，即rcThrottleAdjustment是否小于0；

		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 if(rcThrottleAdjustment < 0)
		{
			setpoint->mode.z = modeVelocity;
			setpoint->velocity.z = rcThrottleAdjustment * CLIMB_RATE_DOWN / (altHoldThrottleBase - RC_MIN - ALT_HOLD_DEADBAND);
			isAdjustAlttitude = true;
		}

（3）最终效果

飞机一推油门就抽风，起飞失败；

3、关于模式的复位

（1）debug

从上面的波形可以看到，这里的期望速度在油门推杆过中点的时候就下来了，但是此时的融合的速度突然变高，说明系统给了很大的油门，此时飞机就抽风了；

（2）bug原因

在进入定高模式的时候，系统会先初始化所有的控制模式；
而上面修改成正常逻辑的code中，当油门居于中位，则不再重新设置控制模式，而这里的复位所有控制模式，就将Z轴本来的位置控制模式复位；

//复位控制模式
static void resetSetpointMode(setpoint_t *setpoint)
{
	setpoint->mode.roll = modeDisable;
	setpoint->mode.pitch = modeDisable;
	setpoint->mode.yaw = modeDisable;
	
	setpoint->mode.x = modeDisable;
	setpoint->mode.y = modeDisable;
	setpoint->mode.z = modeDisable;
}

（3）bug修改

//复位控制模式
static void resetSetpointMode(setpoint_t *setpoint)
{
	setpoint->mode.roll = modeDisable;
	setpoint->mode.pitch = modeDisable;
	setpoint->mode.yaw = modeDisable;
	
	setpoint->mode.x = modeDisable;
	setpoint->mode.y = modeDisable;
//	setpoint->mode.z = modeDisable;
}

（4）飞行效果

飞行正常！

git地址