候所进行的补偿*/
/*假设目前的俯仰角是30°，而横滚角是0°，这时候如保持俯仰和横滚轴没有任何运动，而将偏航轴转动90°
，那么实际的俯仰角就会变为0°，横滚角就会变为30°
但是，按照目前的算法，由于俯仰和横滚方向没有运动，因此就不会有陀螺仪的积分，俯仰和横滚角是不变的
，这就是采用陀螺仪直接积分测角度的不完善性，这时候
使用加速度计对姿态进行修正能够起到作用，但是需要一段时间，使用下面的这段话，就是将偏航轴的运动耦
合在另外两个轴上，使姿态角度能够迅速收敛到真实值上*/
/*注：使用四元数法进行姿态结算可以避免出现这种问题，但这种方法需要有准确的陀螺仪和加表的数学模型，
四元数法还需要进行大量的矩阵计算，
而且对四元数姿态进行加速度计的融合不太方便*/
if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig &
CFG_ACHSENKOPPLUNG_AKTIV))//不在俯仰滚转控制循环中 {
tmpl = Mess_IntegralNick / 4096L; tmpl *= MesswertGier;
tmpl *= Parameter_AchsKopplung1; //125 tmpl /= 2048L;
tmpl2 = Mess_IntegralRoll / 4096L; tmpl2 *= MesswertGier;
tmpl2 *= Parameter_AchsKopplung1; tmpl2 /= 2048L; }
else tmpl = tmpl2 = 0;
// Roll ++++++++++++++++++++++++++++++++++++++++++++++++ MesswertRoll += tmpl;
MesswertRoll += (tmpl2*Parameter_AchsGegenKopplung1)/512L; Mess_IntegralRoll2 += MesswertRoll;
Mess_IntegralRoll += MesswertRoll - LageKorrekturRoll; /*积分超过半圈的情况*/
if(Mess_IntegralRoll > Umschlag180Roll) {
Mess_IntegralRoll = -(Umschlag180Roll - 10000L); Mess_IntegralRoll2 = Mess_IntegralRoll; }
if(Mess_IntegralRoll <-Umschlag180Roll)//一样
{
Mess_IntegralRoll = (Umschlag180Roll - 10000L); Mess_IntegralRoll2 = Mess_IntegralRoll; }
if(AdWertRoll < 15) MesswertRoll = -1000; if(AdWertRoll < 7) MesswertRoll = -2000; if(PlatinenVersion == 10) {
if(AdWertRoll > 1010) MesswertRoll = +1000; if(AdWertRoll > 1017) MesswertRoll = +2000; } else {
if(AdWertRoll > 2020) MesswertRoll = +1000; if(AdWertRoll > 2034) MesswertRoll = +2000; }
// Nick ++++++++++++++++++++++++++++++++++++++++++++++++ MesswertNick -= tmpl2;
MesswertNick -= (tmpl*Parameter_AchsGegenKopplung1)/512L; Mess_IntegralNick2 += MesswertNick;
/*LageKorrekturNick是通过加速度计积分和角速率积分的积分进行做差比较得到 的,*/
Mess_IntegralNick += MesswertNick - LageKorrekturNick; if(Mess_IntegralNick > Umschlag180Nick) {
Mess_IntegralNick = -(Umschlag180Nick - 10000L); Mess_IntegralNick2 = Mess_IntegralNick; }
if(Mess_IntegralNick <-Umschlag180Nick) {
Mess_IntegralNick = (Umschlag180Nick - 10000L); Mess_IntegralNick2 = Mess_IntegralNick; }
if(AdWertNick < 15) MesswertNick = -1000; if(AdWertNick < 7) MesswertNick = -2000; if(PlatinenVersion == 10) {
if(AdWertNick > 1010) MesswertNick = +1000; if(AdWertNick > 1017) MesswertNick = +2000; } else {
if(AdWertNick > 2020) MesswertNick = +1000;
if(AdWertNick > 2034) MesswertNick = +2000; }
//++++++++++++++++++++++++++++++++++++++++++++++++ // ADC einschalten
ANALOG_ON; //重新开始模拟量的采集
//++++++++++++++++++++++++++++++++++++++++++++++++
/*上一步计算完了积分之后，现在将积分赋值，因此后面使用的就将是IntegralNick，IntegralNick2等数据了 */
Integral_Gier = Mess_Integral_Gier; IntegralNick = Mess_IntegralNick; IntegralRoll = Mess_IntegralRoll; IntegralNick2 = Mess_IntegralNick2; IntegralRoll2 = Mess_IntegralRoll2;
/*这部分代码不执行，因为在EEPROM中CFG_DREHRATEN_BEGRENZER这一位为0*/
if(EE_Parameter.GlobalConfig & CFG_DREHRATEN_BEGRENZER && !Looping_Nick && !Looping_Roll) {
if(MesswertNick > 200) MesswertNick += 4 * (MesswertNick - 200); else if(MesswertNick < -200) MesswertNick += 4 * (MesswertNick + 200); if(MesswertRoll > 200) MesswertRoll += 4 * (MesswertRoll - 200); else if(MesswertRoll < -200) MesswertRoll += 4 * (MesswertRoll + 200); }
if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++;
else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--; if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in
[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--;
if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in
[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--;
if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in
[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--; if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255; if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255; if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255; if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
}
//函数：校正平均值
//############################################################################
// Messwerte beim Ermitteln der Nullage /*确定零位*/ /*记录中立点*/
void CalibrierMittelwert(void)
//############################################################################ {
// ADC auschalten, damit die Werte sich nicht w鋒rend der Berechnung 鋘dern ANALOG_OFF;
MesswertNick = AdWertNick; MesswertRoll = AdWertRoll; MesswertGier = AdWertGier;
/*要乘以 ACC_AMPLIFY 是为了进行数值的匹配*/
Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick; Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll; Mittelwert_AccHoch = (long)AdWertAccHoch; // ADC einschalten ANALOG_ON; //开模拟量
if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++;
else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--;
if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++;
else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--;
if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++;
else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--;
if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++;
else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--;
if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
if(Poti3 < 0) Poti3 = 0;
else if(Poti3 > 255) Poti3 = 255;
if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
/*这两个数据是在对陀螺仪积分区域进行的限制，如果超过这个范围，说明就超出了+-90°的范围，则需要相应
的改变*/
Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L; Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagNick * 2500L; }
//发送电机数据
//############################################################################
// Senden der Motorwerte per I2C-Bus void SendMotorData(void)
//############################################################################ {
if(MOTOR_OFF || !MotorenEin)//关机或未工作 {
Motor_Hinten = 0; Motor_Vorne = 0; Motor_Rechts = 0; Motor_Links = 0;//都置零
if(MotorTest[0]) Motor_Vorne = MotorTest[0]; if(MotorTest[1]) Motor_Hinten = MotorTest[1]; if(MotorTest[2]) Motor_Links = MotorTest[2];
if(MotorTest[3]) Motor_Rechts = MotorTest[3];//如果是试验就干。 }
DebugOut.Analog[12] = Motor_Vorne; DebugOut.Analog[13] = Motor_Hinten; DebugOut.Analog[14] = Motor_Links; DebugOut.Analog[15] = Motor_Rechts;
//Start I2C Interrupt Mode twi_state = 0; motor = 0; i2c_start(); }