德国开源代码的四轴飞行(3)

//函数：参数分配

//############################################################################

// Tr鋑t ggf. das Poti als Parameter ein void ParameterZuordnung(void)

//############################################################################ { //

/*这个宏定义的作用是：将a中的值赋给b，并将b限制在max和min之间*/

#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;//电机数值，PI算法的计算数值 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) {

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;

} // end of if(SenderOkay < 100)

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // Emfang gut//

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ else if(SenderOkay > 140) {

Notlandung = 0;

RcLostTimer = EE_Parameter.NotGasZeit * 50; 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;

}

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // auf Nullwerte kalibrieren

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

if((PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] > 80) && MotorenEin == 0) {

if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75) {

if(++delay_neutral > 200) {

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); }

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) {

if(++delay_neutral > 200) // nicht sofort { GRN_OFF;

eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK],0xff); // Werte lchen

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;

} // end if of if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // Gas ist unten

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

if(PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] < 35-120)

鰏

百度搜索“77cn”或“免费范文网”即可找到本站免费阅读全部范文。收藏本站方便下次阅读，免费范文网，提供经典小说综合文库德国开源代码的四轴飞行(3)在线全文阅读。