|
|
今天还把陀螺仪温飘的修正代码加上了，不过在试飞的时候并没有出现过调整动作。
// deviation of gyro nick integral (IntegralNick is corrected by averaged acc sensor)
IntegralErrorNick = IntegralNick2 - IntegralNick;
Reading_IntegralGyroNick2 -= IntegralErrorNick;
cnt = 1;// + labs(IntegralErrorNick) / 4096;
CorrectionNick = 0;
if((labs(MeanIntegralNick_old - MeanIntegralNick) < MOVEMENT_LIMIT) || (FCParam.Kalman_MaxDrift > 3* 16))
{
if(IntegralErrorNick > ERROR_LIMIT2)
{
if(last_n_p)
{
cnt += labs(IntegralErrorNick) / ERROR_LIMIT2;
CorrectionNick = IntegralErrorNick / 8;
if(CorrectionNick > 5000) CorrectionNick = 5000;
AttitudeCorrectionNick += CorrectionNick / BALANCE_NUMBER;
}
else last_n_p = 1;
}
else last_n_p = 0;
if(IntegralErrorNick < -ERROR_LIMIT2)
{
if(last_n_n)
{
cnt += labs(IntegralErrorNick) / ERROR_LIMIT2;
CorrectionNick = IntegralErrorNick / 8;
if(CorrectionNick < -5000) CorrectionNick = -5000;
AttitudeCorrectionNick += CorrectionNick / BALANCE_NUMBER;
}
else last_n_n = 1;
}
else last_n_n = 0;
}
else
{
cnt = 0;
BadCompassHeading = 1000;
}
if(cnt > ParamSet.DriftComp) cnt = ParamSet.DriftComp;
if(cnt * 16 > FCParam.Kalman_MaxDrift) cnt = FCParam.Kalman_MaxDrift / 16;
// correct Gyro Offsets
if(IntegralErrorNick > ERROR_LIMIT) AdNeutralNick += cnt;
if(IntegralErrorNick < -ERROR_LIMIT) AdNeutralNick -= cnt;
这是MK的0点调整代码，再解说之前，先看看为什么需要进行温飘调整。
还是这个图：
通过这个图可以很明确地看出来，如果不进行温飘调整，到后来陀螺仪会越偏越大，直到超过加速度计的调整范围。
在实际测试中，如果让马达转2分钟，在没有0点调整代码的情况下，会明显感觉四轴开始慢慢地向一边歪，而程序则将这个歪的位置认为是平衡位置。
看到这个图，我们大概也可以明白如何进行0点调整了。那就是我们需要比较陀螺仪的原始数据的积分和我们经过调整后的积分值之间的差异，如果这个差异过大，那么说明我们需要动一下0点了。MK的流程也差不多是这样，只是他写的比较复杂。
// deviation of gyro nick integral (IntegralNick is corrected by averaged acc sensor)
IntegralErrorNick = IntegralNick2 - IntegralNick;
Reading_IntegralGyroNick2 -= IntegralErrorNick;
这部分，就是计算陀螺仪原始数据积分和调整后积分的差异的。IntegralNick2，Reading_IntegralGyroNick2 是陀螺仪原始数据积分，IntegralNick是经过加速度计调整后的积分，IntegralErrorNick 就是他们的差异。
第一行求出差异，第二行将这个差异从陀螺仪的原始积分里去掉，Reading_IntegralGyroNick2 这个是真正在AD部分用的陀螺仪原始数据积分。这个去掉的动作很重要，否则的话一旦积分差异超过上限，就会一直调整下去。
下面的部分就是根据这个差异进行判断，其实很多时候代码执行起来是这样的：
cnt = 1;
// correct Gyro Offsets
if(IntegralErrorNick > ERROR_LIMIT) AdNeutralNick += cnt;
if(IntegralErrorNick < -ERROR_LIMIT) AdNeutralNick -= cnt;
关键部分就是：
if(IntegralErrorNick > ERROR_LIMIT) AdNeutralNick += cnt;
if(IntegralErrorNick < -ERROR_LIMIT) AdNeutralNick -= cnt;
其中ERROR_LIMIT是(BALANCE_NUMBER * 4)，意思是如果我们每次进行积分，2个数据差异都有4，那么说明我们确实应该调整一下中立点了。
代码简化成上面的形式就容易理解多了，就是看一下差异，如果超过ERROR_LIMIT那么就对陀螺仪中立点增加1，如果小于-ERROR_LIMIT，那么就减1。
而这个差异是执行了BALANCE_NUMBER次计算后的，也就是积分要在0.5秒内差出这么多才可以。条件还是很严格的。
不过MK还多想了一些，还想到了如果在这0.5秒内一下子差出(BALANCE_NUMBER * 16)的情况：
cnt = 1;// + labs(IntegralErrorNick) / 4096;
CorrectionNick = 0;
if((labs(MeanIntegralNick_old - MeanIntegralNick) < MOVEMENT_LIMIT) || (FCParam.Kalman_MaxDrift > 3* 16))
{
if(IntegralErrorNick > ERROR_LIMIT2)
{
if(last_n_p)
{
cnt += labs(IntegralErrorNick) / ERROR_LIMIT2;
CorrectionNick = IntegralErrorNick / 8;
if(CorrectionNick > 5000) CorrectionNick = 5000;
AttitudeCorrectionNick += CorrectionNick / BALANCE_NUMBER;
}
else last_n_p = 1;
}
else last_n_p = 0;
if(IntegralErrorNick < -ERROR_LIMIT2)
{
if(last_n_n)
{
cnt += labs(IntegralErrorNick) / ERROR_LIMIT2;
CorrectionNick = IntegralErrorNick / 8;
if(CorrectionNick < -5000) CorrectionNick = -5000;
AttitudeCorrectionNick += CorrectionNick / BALANCE_NUMBER;
}
else last_n_n = 1;
}
else last_n_n = 0;
}
首先，如果上次的陀螺仪积分平均值和这次的差异不大，那么去判断是否需要调整中立点。具体为什么，不知道，有待后续研究。
下面就是判断的流程，如果积分差异超过ERROR_LIMIT2，也就是(BALANCE_NUMBER * 16)，且出现了2次（last_n_p标志位），那么我们根据这个积分差异来计算cnt这个调整量的数值。
然后根据差异算出AttitudeCorrectionNick 的调整量。
不过我比较怀疑有多大几率陀螺仪0点会一下子飘很多，一般情况下都是慢慢飘的，所以我怀疑这部分复杂逻辑在实际飞行时有没有起到很多的作用。 |
|
/1 
发表于 2009-5-20 21:31
收藏
楼主
