|
发表于 2008-4-26 23:16:29
|
显示全部楼层
void Piep(unsigned char Anzahl)
{
while(Anzahl--)
{
if(MotorenEin) return; //auf keinen Fall im Flug!
beeptime = 100;
Delay_ms(250);
}
}
蜂鸣器叫,当时间不断减小时,让其叫起来,直到完成或发动机启动为止。
函数:SetNeutral设定传感器发出参数的中立数值,因为有漂移所以要使其每次工作都要测量出来。
//############################################################################
// Nullwerte ermitteln
void SetNeutral(void)
//############################################################################
{
NeutralAccX = 0; //X轴(欧美坐标系)加速度置0
NeutralAccY = 0;//Y轴(欧美坐标系)加速度置0
NeutralAccZ = 0;//Z轴(欧美坐标系)加速度置0
AdNeutralNick = 0;//俯仰角速率传感器置0
AdNeutralRoll = 0;//滚转。。。
AdNeutralGier = 0;//偏航。。。
Parameter_AchsKopplung1 = 0;//轴的耦合系数定为0,
Parameter_AchsGegenKopplung1 = 0;。。。
CalibrierMittelwert(); //校准参数数值
Delay_ms_Mess(100);//等待时间为100
CalibrierMittelwert();//再次校准参数数值
if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) //如果高度需要控制
{
if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();//如果气压表输出在950外750内,则设定气压初始的偏差。
}
AdNeutralNick= AdWertNick; //俯仰速率中立数值被当前俯仰数值赋值。
AdNeutralRoll= AdWertRoll; //滚转。。。
AdNeutralGier= AdWertGier;//偏航...
StartNeutralRoll = AdNeutralRoll;//开始滚转中立数值为上面赋值的内容。
StartNeutralNick = AdNeutralNick;//滚转。。。
if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) > 4) //从eeprom中读取出俯仰方向的数值大于4,说明区别很大,因此直接用测量值=》
{
NeutralAccY = abs(Mittelwert_AccRoll) / ACC_AMPLIFY;//平均滚轴加速度数值除以放大倍数。
NeutralAccX = abs(Mittelwert_AccNick) / ACC_AMPLIFY;//平均俯仰轴加速度数值除以放大倍数。
NeutralAccZ = Aktuell_az;//法向加速度数值赋值给它。
}
else //否则说明变化不大,因此还按上次的来。估计上次应该是很好的
{
NeutralAccX = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK+1]);
NeutralAccY = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL+1]);
NeutralAccZ = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_Z]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_Z+1]);
}
Mess_IntegralNick = 0;//测量的俯仰积分为0
Mess_IntegralNick2 = 0;//测量的俯仰积分2为0:积分2是中间所需要的变量,没其他意思
Mess_IntegralRoll = 0;//滚。。。
Mess_IntegralRoll2 = 0;
Mess_Integral_Gier = 0; //偏航。。。
MesswertNick = 0;//测量数值俯仰置0
MesswertRoll = 0;//测量数值滚置0
MesswertGier = 0;//测量数值偏置0
StartLuftdruck = Luftdruck;//开始气压数值
HoeheD = 0;//高度改变率为0
Mess_Integral_Hoch = 0;//高度积分数值为0
KompassStartwert = KompassValue;//罗盘开始数值为当前罗盘数值。
GPS_Neutral();//gps系统中立
beeptime = 50; //
Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L;
ExternHoehenValue = 0;
}
///////////////////////////////
函数描述 :求参数的平均数值
//////////////////////////////
//############################################################################
// Bearbeitet die Messwerte
void Mittelwert(void)
//############################################################################
{
static signed long tmpl,tmpl2;//静态局部变量在使用中记录上次的结果
MesswertGier = (signed int) AdNeutralGier - AdWertGier;//偏航平均值=中立偏航角速率-偏航速率数值
MesswertRoll = (signed int) AdWertRoll - AdNeutralRoll;//滚转。。。
MesswertNick = (signed int) AdWertNick - AdNeutralNick;//俯仰。。。
//DebugOut.Analog[26] = MesswertNick;
DebugOut.Analog[28] = MesswertRoll;//调试信息输出
//加速度传感器输出
// Beschleunigungssensor ++++++++++++++++++++++++++++++++++++++++++++++++
Mittelwert_AccNick = ((long)Mittelwert_AccNick * 1 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 2L;//具有滤波功能的方法,用当前加速度和上次的加速度平均
Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 1 + ((ACC_AMPLIFY * (long)AdWertAccRoll))) / 2L;
Mittelwert_AccHoch = ((long)Mittelwert_AccHoch * 1 + ((long)AdWertAccHoch)) / 2L;
IntegralAccNick += ACC_AMPLIFY * AdWertAccNick;//加速度直接赋值
IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll;
IntegralAccZ += Aktuell_az - NeutralAccZ;
// Gier ++++++++++++++++++++++++++++++++++++++++++++++++//偏航部分直接积分计算(把噪声当作零均值)
Mess_Integral_Gier += MesswertGier;
Mess_Integral_Gier2 += MesswertGier;
// Kopplungsanteil +++++++++++++++++++++++++++++++++++++//耦合计算,因为
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; //109
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;//如果角速率小于15,那就让滚转平均值为-1000。
if(AdWertRoll < 7) MesswertRoll = -2000;//如果角速率小于7,就让滚转平均值为-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;
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; //重新开始模拟量的采集
//++++++++++++++++++++++++++++++++++++++++++++++++
Integral_Gier = Mess_Integral_Gier;//积分赋值。
IntegralNick = Mess_IntegralNick;
IntegralRoll = Mess_IntegralRoll;
IntegralNick2 = Mess_IntegralNick2;
IntegralRoll2 = Mess_IntegralRoll2;
if(EE_Parameter.GlobalConfig & CFG_DREHRATEN_BEGRENZER && !Looping_Nick && !Looping_Roll)
//非正常调节过程,就要让测量值满足如下的计算方法,超过200则重记录,有可能上面的计算过程中出现了问题。
{
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;
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;
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(); //用总线发送数据。
}
//函数:参数分配
//############################################################################
// Tr鋑t ggf. das Poti als Parameter ein
void ParameterZuordnung(void)
//############################################################################
{
//
#define CHK_POTI(b,a,min,max) { if(a > 250) { if(a == 251) b = Poti1; else if(a == 252) b = Poti2; else if(a == 253) b = Poti3; else if(a == 254) b = Poti4;} else b = a; if(b <= min) b = min; else if(b >= max) b = max;}
CHK_POTI(Parameter_MaxHoehe,EE_Parameter.MaxHoehe,0,255);
CHK_POTI(Parameter_Luftdruck_D,EE_Parameter.Luftdruck_D,0,100);
CHK_POTI(Parameter_Hoehe_P,EE_Parameter.Hoehe_P,0,100);
CHK_POTI(Parameter_Hoehe_ACC_Wirkung,EE_Parameter.Hoehe_ACC_Wirkung,0,255);
CHK_POTI(Parameter_KompassWirkung,EE_Parameter.KompassWirkung,0,255);
CHK_POTI(Parameter_Gyro_P,EE_Parameter.Gyro_P,10,255);
CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I,0,255);
CHK_POTI(Parameter_I_Faktor,EE_Parameter.I_Faktor,0,255);
CHK_POTI(Parameter_UserParam1,EE_Parameter.UserParam1,0,255);
CHK_POTI(Parameter_UserParam2,EE_Parameter.UserParam2,0,255);
CHK_POTI(Parameter_UserParam3,EE_Parameter.UserParam3,0,255);
CHK_POTI(Parameter_UserParam4,EE_Parameter.UserParam4,0,255);
CHK_POTI(Parameter_UserParam5,EE_Parameter.UserParam5,0,255);
CHK_POTI(Parameter_UserParam6,EE_Parameter.UserParam6,0,255);
CHK_POTI(Parameter_UserParam7,EE_Parameter.UserParam7,0,255);
CHK_POTI(Parameter_UserParam8,EE_Parameter.UserParam8,0,255);
CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255);
CHK_POTI(Parameter_LoopGasLimit,EE_Parameter.LoopGasLimit,0,255);
CHK_POTI(Parameter_AchsKopplung1, EE_Parameter.AchsKopplung1,0,255);
CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255);
CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability,0,255);
Ki = (float) Parameter_I_Faktor * 0.0001;
MAX_GAS = EE_Parameter.Gas_Max;
MIN_GAS = EE_Parameter.Gas_Min;
}
//电机控制周期工作
//############################################################################
//
void MotorRegler(void)
//############################################################################
{
int motorwert,pd_ergebnis,h,tmp_int;//电机数值,结果数值,临时变量
int GierMischanteil,GasMischanteil;//偏航混合数值,油门混和数值
static long SummeNick=0,SummeRoll=0;//俯仰总和,滚转总和
static long sollGier = 0,tmp_long,tmp_long2;//标准偏航值,
static long IntegralFehlerNick = 0;//俯仰误差积分
static long IntegralFehlerRoll = 0;//滚转误差积分
static unsigned int RcLostTimer;//传输时间变化
static unsigned char delay_neutral = 0;//延迟中立数值
static unsigned char delay_einschalten = 0,delay_ausschalten = 0;//延迟接通,延迟关闭
static unsigned int modell_fliegt = 0;//飞机飞行时间
static int hoehenregler = 0;//高度调节
static char TimerWerteausgabe = 0;//时间数值
static char NeueKompassRichtungMerken = 0;//罗盘方向调整中立值
static long ausgleichNick, ausgleichRoll;//俯仰均衡,滚转均衡
Mittelwert(); //求平均值
GRN_ON;//打开端口
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gaswert ermitteln//判断油门数值
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
GasMischanteil = StickGas;
if(GasMischanteil < 0) GasMischanteil = 0;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Emfang schlecht//无线电故障,不好
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(SenderOkay < 100)
{
if(!PcZugriff)
{
if(BeepMuster == 0xffff) //开始叫
{
beeptime = 15000;
BeepMuster = 0x0c00;
}
}
if(RcLostTimer) RcLostTimer--; //关机了,就停机,但是是一个慢动作
else
{
MotorenEin = 0;
Notlandung = 0;
}
ROT_ON;
if(modell_fliegt > 2000) // wahrscheinlich in der Luft --> langsam absenken
{
GasMischanteil = EE_Parameter.NotGas;
Notlandung = 1;
PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] = 0;
PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;
PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] = 0;
}
else MotorenEin = 0;
}
else
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Emfang gut//好的时候
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(SenderOkay > 140)//无线电好
{
Notlandung = 0;//飞行
RcLostTimer = EE_Parameter.NotGasZeit * 50;//保持延迟时间为1000个tick
if(GasMischanteil > 40)
{
if(modell_fliegt < 0xffff) modell_fliegt++;
}
if((modell_fliegt < 200) || (GasMischanteil < 40))//没飞就停了吧
{
SummeNick = 0;
SummeRoll = 0;
Mess_Integral_Gier = 0;
Mess_Integral_Gier2 = 0;
}
if((PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] > 80) && MotorenEin == 0)//输入油门大了要飞了
{
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// auf Nullwerte kalibrieren
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75) // Neutralwerte//中立数值
{
if(++delay_neutral > 200) // nicht sofort//否则的化 马上行动
{
GRN_OFF;
MotorenEin = 0;//要飞了,马上将参数置零
delay_neutral = 0;
modell_fliegt = 0;
if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70 || abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) > 70)
{
unsigned char setting=1;
if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 1;
if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 2;
if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 3;
if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 4;
if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 5;
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], setting); // aktiven Datensatz merken//储存当前数据。
}
if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // H鰄enregelung aktiviert?
{
if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
}
ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
SetNeutral();//设定中性点。
Piep(GetActiveParamSetNumber());
}
}
else
if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75) // ACC Neutralwerte speichern存储加速度中立数据,因为航向偏差太大,因此需要更新
//说明的问题是前后电机组和左右电机组之间的扭矩抵消的不够?
{
if(++delay_neutral > 200) // nicht sofort
{
GRN_OFF;
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK],0xff); // Werte l鰏chen
MotorenEin = 0;
delay_neutral = 0;
modell_fliegt = 0;
SetNeutral();//设立中性点。
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK],NeutralAccX / 256); // ACC-NeutralWerte speichern
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK+1],NeutralAccX % 256); // ACC-NeutralWerte speichern
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL],NeutralAccY / 256);
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL+1],NeutralAccY % 256);
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_Z],(int)NeutralAccZ / 256);
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_Z+1],(int)NeutralAccZ % 256);
Piep(GetActiveParamSetNumber());
}
}
else delay_neutral = 0;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gas ist unten
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] < 35-120)//如果油门收回来了,分两种情况
{
// Starten
if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75)//偏航大
{
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Einschalten
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(++delay_einschalten > 200)
{
delay_einschalten = 200;
modell_fliegt = 1;
MotorenEin = 1;
sollGier = 0;
Mess_Integral_Gier = 0;
Mess_Integral_Gier2 = 0;
Mess_IntegralNick = 0;
Mess_IntegralRoll = 0;
Mess_IntegralNick2 = IntegralNick;
Mess_IntegralRoll2 = IntegralRoll;
SummeNick = 0;
SummeRoll = 0;
}
}
else delay_einschalten = 0;//没事,就让其延迟关闭为0
//Auf Neutralwerte setzen
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Auschalten
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)//同样道理
{
if(++delay_ausschalten > 200) // nicht sofort
{
MotorenEin = 0;
delay_ausschalten = 200;
modell_fliegt = 0;
}
}
else delay_ausschalten = 0;
}
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// neue Werte von der Funke
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(!NewPpmData-- || Notlandung) //非着陆和有新数据传输的情况下
{
int tmp_int;
static int stick_nick,stick_roll;//俯仰杆,倾斜杆
ParameterZuordnung();//分配参数
StickNick = (StickNick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4; //新数据和老数据混合起滤波作用
StickNick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;//增加上微分量,用于提高反应的快速性。
StickRoll = (StickRoll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4;
StickRoll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
StickGas = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]) > MaxStickNick) //记录最近历史上的最大给杆量,这个量会逐渐的减小下去,直到下一次给杆量很大为止,此量用于限制飞机的机动性,否则容易出事。
MaxStickNick = abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]); else MaxStickNick--;
if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) > MaxStickRoll)
MaxStickRoll = abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]); else MaxStickRoll--;
if(Notlandung) {MaxStickNick = 0; MaxStickRoll = 0;}//如果是空中就算了。
GyroFaktor = ((float)Parameter_Gyro_P + 10.0) / 256.0;//陀螺倍数
IntegralFaktor = ((float) Parameter_Gyro_I) / 44000;//积分倍数
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+ Digitale Steuerung per DubWise
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#define KEY_VALUE (Parameter_UserParam1 * 4) //(Poti3 * 8)//为了增加杆的输入的丰富性,提供了扩展的杆的描述,对最终杆的描述更加丰富。
if(DubWiseKeys[1]) beeptime = 10;
if(DubWiseKeys[1] & DUB_KEY_UP) tmp_int = KEY_VALUE; else
if(DubWiseKeys[1] & DUB_KEY_DOWN) tmp_int = -KEY_VALUE; else tmp_int = 0;
ExternStickNick = (ExternStickNick * 7 + tmp_int) / 8;
if(DubWiseKeys[1] & DUB_KEY_LEFT) tmp_int = KEY_VALUE; else
if(DubWiseKeys[1] & DUB_KEY_RIGHT) tmp_int = -KEY_VALUE; else tmp_int = 0;
ExternStickRoll = (ExternStickRoll * 7 + tmp_int) / 8;
if(DubWiseKeys[0] & 8) ExternStickGier = 50;else
if(DubWiseKeys[0] & 4) ExternStickGier =-50;else ExternStickGier = 0;
if(DubWiseKeys[0] & 2) ExternHoehenValue++;
if(DubWiseKeys[0] & 16) ExternHoehenValue--;
StickNick += ExternStickNick / 8;
StickRoll += ExternStickRoll / 8;
StickGier += ExternStickGier;
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+ Analoge Steuerung per Seriell
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(ExternControl.Config & 0x01 && Parameter_UserParam1 > 128)//同上,具有扩展功能的控制输入
{
StickNick += (int) ExternControl.Nick * (int) EE_Parameter.Stick_P;
StickRoll += (int) ExternControl.Roll * (int) EE_Parameter.Stick_P;
StickGier += ExternControl.Gier;
ExternHoehenValue = (int) ExternControl.Hight * (int)EE_Parameter.Hoehe_Verstaerkung;
if(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas;
}
if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor = 0;
if(GyroFaktor < 0) GyroFaktor = 0;
if(IntegralFaktor < 0) IntegralFaktor = 0;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Looping?//处于循环状态,指,受到了驾驶杆的控制,
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_LINKS) Looping_Links = 1;
else
{
{
if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < (EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese))) Looping_Links = 0;
}
}
if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_RECHTS) Looping_Rechts = 1;
else
{
if(Looping_Rechts) // Hysterese
{
if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > -(EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese)) Looping_Rechts = 0;
}
}
if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_OBEN) Looping_Oben = 1;
else
{
if(Looping_Oben) // Hysterese
{
if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < (EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese))) Looping_Oben = 0;
}
}
if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_UNTEN) Looping_Unten = 1;
else
{
if(Looping_Unten) // Hysterese
{
if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > -(EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese)) Looping_Unten = 0;
}
}
if(Looping_Links || Looping_Rechts) Looping_Roll = 1; else Looping_Roll = 0;//不能同时处于滚转和俯仰状态,否则系统将不可控制
if(Looping_Oben || Looping_Unten) {Looping_Nick = 1; Looping_Roll = 0; Looping_Links = 0; Looping_Rechts = 0;} else Looping_Nick = 0;
} // Ende neue Funken-Werte
if(Looping_Roll) beeptime = 100;//如果滚循环开始,则叫声为100,
if(Looping_Roll || Looping_Nick)//如果循环开始则让油门总和受到限制。
{
if(GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Bei Empfangsausfall im Flug
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(Notlandung)//如果急需降落,则按这个来。
{
StickGier = 0;
StickNick = 0;
StickRoll = 0;
GyroFaktor = 0.1;
IntegralFaktor = 0.005;
Looping_Roll = 0;
Looping_Nick = 0;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Integrale auf ACC-Signal abgleichen//加速度信号的积分校准
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#define ABGLEICH_ANZAHL 256L
MittelIntegralNick += IntegralNick; // F黵 die Mittelwertbildung aufsummieren//平均值的积累
MittelIntegralRoll += IntegralRoll;
MittelIntegralNick2 += IntegralNick2;
MittelIntegralRoll2 += IntegralRoll2;
if(Looping_Nick || Looping_Roll)//机动过程中,让所有的积分项都为零,因为机动过程会产生很大的误差,因此需要尽快结束其控制,然后自动调平。
{
IntegralAccNick = 0;
IntegralAccRoll = 0;
MittelIntegralNick = 0;
MittelIntegralRoll = 0;
MittelIntegralNick2 = 0;
MittelIntegralRoll2 = 0;
Mess_IntegralNick2 = Mess_IntegralNick;//有待理解2的含义,测量积分的处理方法慢慢来
Mess_IntegralRoll2 = Mess_IntegralRoll;
ZaehlMessungen = 0;//每当测量一次航向就让其增加一,在这里让其为0,可能是要重新考虑航向的事情了
LageKorrekturNick = 0;//存放正确俯仰角为0
LageKorrekturRoll = 0;//。。。
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(!Looping_Nick && !Looping_Roll)//水平过程中,处理目的是为了将陀螺加计交联项处理掉
{
long tmp_long, tmp_long2;
tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick);//这部分可能就是高科技了,有效的补偿了耦合项目
tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll); //
tmp_long /= 16;
tmp_long2 /= 16;
if((MaxStickNick > 15) || (MaxStickRoll > 15))
{
tmp_long /= 3;
tmp_long2 /= 3;
}
if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)
{
tmp_long /= 3;
tmp_long2 /= 3;
}
#define AUSGLEICH 32//补偿项
if(tmp_long > AUSGLEICH) tmp_long = AUSGLEICH;
if(tmp_long < -AUSGLEICH) tmp_long =-AUSGLEICH;
if(tmp_long2 > AUSGLEICH) tmp_long2 = AUSGLEICH;
if(tmp_long2 <-AUSGLEICH) tmp_long2 =-AUSGLEICH;
Mess_IntegralNick -= tmp_long;
Mess_IntegralRoll -= tmp_long2;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(ZaehlMessungen >= ABGLEICH_ANZAHL)//关于时间积累的处理过程
{
static int cnt = 0;
static char last_n_p,last_n_n,last_r_p,last_r_n;
static long MittelIntegralNick_Alt,MittelIntegralRoll_Alt;//alt是老的意思Old
if(!Looping_Nick && !Looping_Roll)//此时不在机动
{
MittelIntegralNick /= ABGLEICH_ANZAHL;//计算平均的俯仰积分
MittelIntegralRoll /= ABGLEICH_ANZAHL;
IntegralAccNick = (EE_Parameter.GyroAccFaktor * IntegralAccNick) / ABGLEICH_ANZAHL;//计算加计的积分(因为加计在不机动时应为0,)
IntegralAccRoll = (EE_Parameter.GyroAccFaktor * IntegralAccRoll) / ABGLEICH_ANZAHL;
IntegralAccZ = IntegralAccZ / ABGLEICH_ANZAHL;
#define MAX_I 0//(Poti2/10)
// Nick ++++++++++++++++++++++++++++++++++++++++++++++++
IntegralFehlerNick = (long)(MittelIntegralNick - (long)IntegralAccNick);//俯仰积分误差等于平均积分减加计积分
ausgleichNick = IntegralFehlerNick / EE_Parameter.GyroAccAbgleich;//均衡项等于积分误差除以陀螺加计平衡项(abgleich:平衡和调整的意思)
// Roll ++++++++++++++++++++++++++++++++++++++++++++++++
IntegralFehlerRoll = (long)(MittelIntegralRoll - (long)IntegralAccRoll);
ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich;
LageKorrekturNick = ausgleichNick / ABGLEICH_ANZAHL;//正确值储存
LageKorrekturRoll = ausgleichRoll / ABGLEICH_ANZAHL;
if((MaxStickNick > 15) || (MaxStickRoll > 15) || (abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25))//有过一段时间的大的操作,不精密了,就不管
{
LageKorrekturNick /= 2;
LageKorrekturNick /= 2;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gyro-Drift ermitteln//陀螺漂移的确定
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
MittelIntegralNick2 /= ABGLEICH_ANZAHL;
MittelIntegralRoll2 /= ABGLEICH_ANZAHL;
tmp_long = IntegralNick2 - IntegralNick; //按照有校正和没校正之间的差值,作为误差积分。
tmp_long2 = IntegralRoll2 - IntegralRoll;
//DebugOut.Analog[25] = MittelIntegralRoll2 / 26;
IntegralFehlerNick = tmp_long;
IntegralFehlerRoll = tmp_long2;
Mess_IntegralNick2 -= IntegralFehlerNick;
Mess_IntegralRoll2 -= IntegralFehlerRoll;
// IntegralFehlerNick = (IntegralFehlerNick * 1 + tmp_long) / 2;
// IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2;
DebugOut.Analog[17] = IntegralAccNick / 26;
DebugOut.Analog[18] = IntegralAccRoll / 26;
DebugOut.Analog[19] = IntegralFehlerNick;// / 26;
DebugOut.Analog[20] = IntegralFehlerRoll;// / 26;
DebugOut.Analog[21] = MittelIntegralNick / 26;
DebugOut.Analog[22] = MittelIntegralRoll / 26;
//DebugOut.Analog[28] = ausgleichNick;
DebugOut.Analog[29] = ausgleichRoll;
DebugOut.Analog[30] = LageKorrekturRoll * 10;
#define FEHLER_LIMIT (ABGLEICH_ANZAHL * 4)
#define FEHLER_LIMIT2 (ABGLEICH_ANZAHL * 16)
#define BEWEGUNGS_LIMIT 20000
// Nick +++++++++++++++++++++++++++++++++++++++++++++++++
cnt = 1;// + labs(IntegralFehlerNick) / 4096;
if(labs(MittelIntegralNick_Alt - MittelIntegralNick) < BEWEGUNGS_LIMIT)//运动限制
{
if(IntegralFehlerNick > FEHLER_LIMIT2) //误差太大
{
if(last_n_p)
{
cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2; //误差越大增加越大
ausgleichNick = IntegralFehlerNick / 8;
if(ausgleichNick > 5000) ausgleichNick = 5000;
LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL;
}
else last_n_p = 1;
} else last_n_p = 0;
if(IntegralFehlerNick < -FEHLER_LIMIT2)
{
if(last_n_n)
{
cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2;
ausgleichNick = IntegralFehlerNick / 8;
if(ausgleichNick < -5000) ausgleichNick = -5000;
LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL;
}
else last_n_n = 1;
} else last_n_n = 0;
} else cnt = 0;
if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;//超过了需要校正的范围后,就重新更正中立数据
if(IntegralFehlerNick > FEHLER_LIMIT) AdNeutralNick += cnt;
if(IntegralFehlerNick < -FEHLER_LIMIT) AdNeutralNick -= cnt;
// Roll +++++++++++++++++++++++++++++++++++++++++++++++++
cnt = 1;// + labs(IntegralFehlerNick) / 4096;
ausgleichRoll = 0;
if(labs(MittelIntegralRoll_Alt - MittelIntegralRoll) < BEWEGUNGS_LIMIT)
{
if(IntegralFehlerRoll > FEHLER_LIMIT2)
{
if(last_r_p)
{
cnt += labs(IntegralFehlerRoll) / FEHLER_LIMIT2;
ausgleichRoll = IntegralFehlerRoll / 8;
if(ausgleichRoll > 5000) ausgleichRoll = 5000;
LageKorrekturRoll += ausgleichRoll / ABGLEICH_ANZAHL;
}
else last_r_p = 1;
} else last_r_p = 0;
if(IntegralFehlerRoll < -FEHLER_LIMIT2)
{
if(last_r_n)
{
cnt += labs(IntegralFehlerRoll) / FEHLER_LIMIT2;
ausgleichRoll = IntegralFehlerRoll / 8;
if(ausgleichRoll < -5000) ausgleichRoll = -5000;
LageKorrekturRoll += ausgleichRoll / ABGLEICH_ANZAHL;
}
else last_r_n = 1;
} else last_r_n = 0;
} else
{
cnt = 0;
}
if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
if(IntegralFehlerRoll > FEHLER_LIMIT) AdNeutralRoll += cnt;//与上部分同理
if(IntegralFehlerRoll < -FEHLER_LIMIT) AdNeutralRoll -= cnt;
DebugOut.Analog[27] = ausgleichRoll;
DebugOut.Analog[23] = AdNeutralNick;//10*(AdNeutralNick - StartNeutralNick);
DebugOut.Analog[24] = 10*(AdNeutralRoll - StartNeutralRoll);
}
else
{
LageKorrekturRoll = 0;
LageKorrekturNick = 0;
}
if(!IntegralFaktor) { LageKorrekturRoll = 0; LageKorrekturNick = 0;} // z.B. bei HH
// +++++++++++++++++++++++++++++++++++++++++++++++++++++
MittelIntegralNick_Alt = MittelIntegralNick;
MittelIntegralRoll_Alt = MittelIntegralRoll;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++
IntegralAccNick = 0;
IntegralAccRoll = 0;
IntegralAccZ = 0;
MittelIntegralNick = 0;
MittelIntegralRoll = 0;
MittelIntegralNick2 = 0;
MittelIntegralRoll2 = 0;
ZaehlMessungen = 0;
}
//DebugOut.Analog[31] = StickRoll / (26*IntegralFaktor);
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gieren//偏航
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(abs(StickGier) > 20) // war 35
{
if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1;//节外生枝的罗盘方向注意到
}
tmp_int = (long) EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo y = ax + bx?
tmp_int += (EE_Parameter.Gier_P * StickGier) / 4;
sollGier = tmp_int;
Mess_Integral_Gier -= tmp_int; //由于是控制的原因,所以要减去一些积分的
if(Mess_Integral_Gier > 50000) Mess_Integral_Gier = 50000; // begrenzen约束和限制
if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Kompass
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV))
{
int w,v;
static int SignalSchlecht = 0;
w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln//根据有害的倾向而扩大相应的节流控制
v = abs(IntegralRoll /512);
if(v > w) w = v; // gr鰏ste Neigung ermitteln//倾向方面的决定
if(w < 25 && NeueKompassRichtungMerken && !SignalSchlecht) //如果数值小于25,且有控值,而且信号传输正常
{
KompassStartwert = KompassValue;//重新设定开始航向
NeueKompassRichtungMerken = 0;//再有就属于节外生枝了
}
w = (w * Parameter_KompassWirkung) / 64; // auf die Wirkung normieren//影响规范化
w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln//影响对应的控制节流数值
if(w > 0)
{
if(!SignalSchlecht) Mess_Integral_Gier += (KompassRichtung * w) / 32; // nach Kompass ausrichten//以后罗盘调整值
if(SignalSchlecht) SignalSchlecht--;//信号快有问题了?
}
else SignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek//延迟判断信号有问题。延迟,信号,聋的, 发问
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Debugwerte zuordnen//调试数值的归并
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(!TimerWerteausgabe--)
{
TimerWerteausgabe = 24;
DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor;
DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor;
DebugOut.Analog[2] = Mittelwert_AccNick;
DebugOut.Analog[3] = Mittelwert_AccRoll;
DebugOut.Analog[4] = MesswertGier;
DebugOut.Analog[5] = HoehenWert;
DebugOut.Analog[6] =(Mess_Integral_Hoch / 512);
DebugOut.Analog[8] = KompassValue;
DebugOut.Analog[9] = UBat;
DebugOut.Analog[10] = SenderOkay;
DebugOut.Analog[16] = Mittelwert_AccHoch;
/* DebugOut.Analog[16] = motor_rx[0];
DebugOut.Analog[17] = motor_rx[1];
DebugOut.Analog[18] = motor_rx[2];
DebugOut.Analog[19] = motor_rx[3];
DebugOut.Analog[20] = motor_rx[0] + motor_rx[1] + motor_rx[2] + motor_rx[3];
DebugOut.Analog[20] /= 14;
DebugOut.Analog[21] = motor_rx[4];
DebugOut.Analog[22] = motor_rx[5];
DebugOut.Analog[23] = motor_rx[6];
DebugOut.Analog[24] = motor_rx[7];
DebugOut.Analog[25] = motor_rx[4] + motor_rx[5] + motor_rx[6] + motor_rx[7];
*/
// DebugOut.Analog[9] = MesswertNick;
// DebugOut.Analog[9] = SollHoehe;
// DebugOut.Analog[10] = Mess_Integral_Gier / 128;
// DebugOut.Analog[11] = KompassStartwert;
// DebugOut.Analog[10] = Parameter_Gyro_I;
// DebugOut.Analog[10] = EE_Parameter.Gyro_I;
// DebugOut.Analog[9] = KompassRichtung;
// DebugOut.Analog[10] = GasMischanteil;
// DebugOut.Analog[3] = HoeheD * 32;
// DebugOut.Analog[4] = hoehenregler;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen//角速度和角度变化的归纳部分
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//DebugOut.Analog[26] = MesswertNick;
//DebugOut.Analog[28] = MesswertRoll;
if(Looping_Nick) MesswertNick = MesswertNick * GyroFaktor;//机动时,测量值仅仅包含陀螺输出 d增稳
else MesswertNick = IntegralNick * IntegralFaktor + MesswertNick * GyroFaktor;//稳定时包含角速度和角度,pd控制
if(Looping_Roll) MesswertRoll = MesswertRoll * GyroFaktor;
else MesswertRoll = IntegralRoll * IntegralFaktor + MesswertRoll * GyroFaktor;
MesswertGier = MesswertGier * (2 * GyroFaktor) + Integral_Gier * IntegralFaktor / 2;
DebugOut.Analog[25] = IntegralRoll * IntegralFaktor;
DebugOut.Analog[31] = StickRoll;// / (26*IntegralFaktor);
DebugOut.Analog[28] = MesswertRoll;
// Maximalwerte abfangen
#define MAX_SENSOR 2048
if(MesswertNick > MAX_SENSOR) MesswertNick = MAX_SENSOR;//对控制器最后输出进行幅度限制
if(MesswertNick < -MAX_SENSOR) MesswertNick = -MAX_SENSOR;
if(MesswertRoll > MAX_SENSOR) MesswertRoll = MAX_SENSOR;
if(MesswertRoll < -MAX_SENSOR) MesswertRoll = -MAX_SENSOR;
if(MesswertGier > MAX_SENSOR) MesswertGier = MAX_SENSOR;
if(MesswertGier < -MAX_SENSOR) MesswertGier = -MAX_SENSOR;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// H鰄enregelung
// Die H鰄enregelung schw鋍ht lediglich das Gas ab, erh鰄t es allerdings nicht
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//OCR0B = 180 - (Poti1 + 120) / 4;
//DruckOffsetSetting = OCR0B;
if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // H鰄enregelung//高度调节器工作
{
int tmp_int;
if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER) // Regler wird 黚er Schalter gesteuert
{
if(Parameter_MaxHoehe < 50)
{
SollHoehe = HoehenWert - 20; // Parameter_MaxHoehe ist der PPM-Wert des Schalters//考虑了误差,最大高度-20为真实高度
HoehenReglerAktiv = 0;
}
else
HoehenReglerAktiv = 1;
}
else //否则真是高度是外部高度数值和最大高度数值和加固的高度参数乘积后-20
{
SollHoehe = ((int) ExternHoehenValue + (int) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung - 20;
HoehenReglerAktiv = 1;
}
if(Notlandung) SollHoehe = 0;
h = HoehenWert;
if((h > SollHoehe) && HoehenReglerAktiv) // zu hoch --> drosseln//如果高度大于真实高度
{ h = ((h - SollHoehe) * (int) Parameter_Hoehe_P) / 16; // Differenz bestimmen --> P-Anteil//给出给定和实际间差值,作为差分信号
h = GasMischanteil - h; // vom Gas abziehen//油门混合值最终结果
h -= (HoeheD * Parameter_Luftdruck_D)/8; // D-Anteil//微分份额
tmp_int = ((Mess_Integral_Hoch / 512) * (signed long) Parameter_Hoehe_ACC_Wirkung) / 32;//加速度份额,过载限制
if(tmp_int > 50) tmp_int = 50;
else if(tmp_int < -50) tmp_int = -50;
h -= tmp_int;
hoehenregler = (hoehenregler*15 + h) / 16; //更新数值与老数据加权滤波
if(hoehenregler < EE_Parameter.Hoehe_MinGas) // nicht unter MIN//太小了的话,让油门混合值保证最小值
{
if(GasMischanteil >= EE_Parameter.Hoehe_MinGas) hoehenregler = EE_Parameter.Hoehe_MinGas;
if(GasMischanteil < EE_Parameter.Hoehe_MinGas) hoehenregler = GasMischanteil;
}
if(hoehenregler > GasMischanteil) hoehenregler = GasMischanteil; // nicht mehr als Gas//判断最后的结果,取其合适值
GasMischanteil = hoehenregler;
}
}
if(GasMischanteil > MAX_GAS - 20) GasMischanteil = MAX_GAS - 20;//最后再让油门混合处于最大-20处。
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Mischer und PI-Regler 在PI控制器下的混合数值
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
DebugOut.Analog[7] = GasMischanteil;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gier-Anteil//偏航部分
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#define MUL_G 1.0
GierMischanteil = MesswertGier - sollGier; // Regler f黵 Gier//首先是得到误差,偏航混合部分的误差值
// GierMischanteil = 0;
if(GierMischanteil > (GasMischanteil / 2)) GierMischanteil = GasMischanteil / 2; //如果偏航混合大于油门混合的二分之一,则限制为二分之一
if(GierMischanteil < -(GasMischanteil / 2)) GierMischanteil = -(GasMischanteil / 2);
if(GierMischanteil > ((MAX_GAS - GasMischanteil))) GierMischanteil = ((MAX_GAS - GasMischanteil)); //如果超了最大,则限制在最大
if(GierMischanteil < -((MAX_GAS - GasMischanteil))) GierMischanteil = -((MAX_GAS - GasMischanteil));
if(GasMischanteil < 20) GierMischanteil = 0;//油门本身如果太小了,就限制偏航为0
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Nick-Achse俯仰轴
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
DiffNick = MesswertNick - (StickNick - GPS_Nick); // Differenz bestimmen//计算俯仰的微分情况,用测量值减去控制输入值
if(IntegralFaktor) SummeNick += IntegralNick * IntegralFaktor - (StickNick - GPS_Nick); // I-Anteil bei Winkelregelung
//如果角度控制器参与控制,则让俯仰总和
//增加角度份额,并刨掉控制部分
else SummeNick += DiffNick; // I-Anteil bei HH 否则就让俯仰总和增加微分量。
if(SummeNick > 16000) SummeNick = 16000;
if(SummeNick < -16000) SummeNick = -16000;
pd_ergebnis = DiffNick + Ki * SummeNick; // PI-Regler f黵 Nick PD控制结果为比例+积分控制
// Motor Vorn
tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;//将油门总和及偏航部分记入临时变量
if(pd_ergebnis > tmp_int) pd_ergebnis = tmp_int; //如果PD控制部分太大,就限幅
if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int;
motorwert = GasMischanteil + pd_ergebnis + GierMischanteil; // Mischer//最后的混合值为油门混合值 + PD份额+偏航混合份额给后机
if ((motorwert < 0)) motorwert = 0;
else if(motorwert > MAX_GAS) motorwert = MAX_GAS;
if (motorwert < MIN_GAS) motorwert = MIN_GAS;
Motor_Vorne = motorwert;
// Motor Heck
motorwert = GasMischanteil - pd_ergebnis + GierMischanteil;// Mischer//最后的混合值为油门混合值 - PD份额+偏航混合份额给前机
if ((motorwert < 0)) motorwert = 0;
else if(motorwert > MAX_GAS) motorwert = MAX_GAS;
if (motorwert < MIN_GAS) motorwert = MIN_GAS;
Motor_Hinten = motorwert;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Roll-Achse滚转轴 道理一样。
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
DiffRoll = MesswertRoll - (StickRoll - GPS_Roll); // Differenz bestimmen
if(IntegralFaktor) SummeRoll += IntegralRoll * IntegralFaktor - (StickRoll - GPS_Roll);// I-Anteil bei Winkelregelung
else SummeRoll += DiffRoll; // I-Anteil bei HH
if(SummeRoll > 16000) SummeRoll = 16000;
if(SummeRoll < -16000) SummeRoll = -16000;
pd_ergebnis = DiffRoll + Ki * SummeRoll; // PI-Regler f黵 Roll
tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
if(pd_ergebnis > tmp_int) pd_ergebnis = tmp_int;
if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int;
// Motor Links
motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;
#define GRENZE Poti1
if ((motorwert < 0)) motorwert = 0;
else if(motorwert > MAX_GAS) motorwert = MAX_GAS;
if (motorwert < MIN_GAS) motorwert = MIN_GAS;
Motor_Links = motorwert;
// Motor Rechts
motorwert = GasMischanteil - pd_ergebnis - GierMischanteil;
if ((motorwert < 0)) motorwert = 0;
else if(motorwert > MAX_GAS) motorwert = MAX_GAS;
if (motorwert < MIN_GAS) motorwert = MIN_GAS;
Motor_Rechts = motorwert;
// +++++++++++++++++++++++++++++++++++++++++++++++
} |
|