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)飞行效果
飞行正常!