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;
}
}
解决思路很简单,就是设置一个判断逻辑,记录油门的变化方向,当油门从高到中,或者从低到中;
就置起一个标志位,最后根据这个标志位来判断是否要刷新期望的高度;
4、飞行效果(波形)
5、git地址
git@gitee.com:ggxxd/wukong_dronefly.git
6、视频效果
https://www.bilibili.com/video/BV1Em4y1w7Uj/?vd_source=6d49f1af36ee59119860204ad2fae52f