| /branches/V0.70d_flinkflash/src/GPS.c |
|---|
| 0,0 → 1,32 |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Copyright (c) 04.2007 Holger Buss |
| // + only for non-profit use |
| // + www.MikroKopter.com |
| // + see the File "License.txt" for further Informations |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| #include "main.h" |
| signed int GPS_Nick = 0; |
| signed int GPS_Roll = 0; |
| signed int GPS_Nick2 = 0; |
| signed int GPS_Roll2 = 0; |
| long GpsAktuell_X = 0; |
| long GpsAktuell_Y = 0; |
| long GpsZiel_X = 0; |
| long GpsZiel_Y = 0; |
| void GPS_Neutral(void) |
| { |
| GpsZiel_X = GpsAktuell_X; |
| GpsZiel_Y = GpsAktuell_Y; |
| } |
| void GPS_BerechneZielrichtung(void) |
| { |
| GPS_Nick = 0; |
| GPS_Roll = 0; |
| } |
| /branches/V0.70d_flinkflash/src/GPS.d |
|---|
| 0,0 → 1,4 |
| GPS.o GPS.d : src/GPS.c include/main.h include/old_macros.h include/_Settings.h \ |
| include/printf_P.h include/timer0.h include/uart.h include/analog.h \ |
| include/twimaster.h include/menu.h include/rc.h include/fc.h \ |
| include/gps.h include/spi.h include/led.h |
| /branches/V0.70d_flinkflash/src/analog.c |
|---|
| 0,0 → 1,181 |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Copyright (c) 04.2007 Holger Buss |
| // + only for non-profit use |
| // + www.MikroKopter.com |
| // + see the File "License.txt" for further Informations |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| #include "main.h" |
| volatile int Aktuell_Nick,Aktuell_Roll,Aktuell_Gier,Aktuell_ax, Aktuell_ay,Aktuell_az, UBat = 100; |
| volatile int AdWertNick = 0, AdWertRoll = 0, AdWertGier = 0; |
| volatile int AdWertAccRoll = 0,AdWertAccNick = 0,AdWertAccHoch = 0; |
| volatile char MessanzahlNick = 0, MessanzahlRoll = 0, MessanzahlGier = 0; |
| volatile char messanzahl_AccNick = 0, messanzahl_AccRoll = 0, messanzahl_AccHoch = 0; |
| volatile long Luftdruck = 32000; |
| volatile int StartLuftdruck; |
| volatile unsigned int MessLuftdruck = 1023; |
| unsigned char DruckOffsetSetting; |
| volatile int HoeheD = 0; |
| volatile char messanzahl_Druck; |
| volatile int tmpLuftdruck; |
| volatile unsigned int ZaehlMessungen = 0; |
| unsigned char AnalogOffsetNick = 115,AnalogOffsetRoll = 115,AnalogOffsetGier = 115; |
| unsigned char GyroDefektN = 0,GyroDefektR = 0,GyroDefektG = 0; |
| //####################################################################################### |
| // |
| void ADC_Init(void) |
| //####################################################################################### |
| { |
| ADMUX = 0;//Referenz ist extern |
| ADCSRA=(1<<ADEN)|(1<<ADSC)|(1<<ADATE)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0)|(1<<ADIE); |
| //Free Running Mode, Division Factor 128, Interrupt on |
| } |
| void SucheLuftruckOffset(void) |
| { |
| unsigned int off; |
| off = eeprom_read_byte(&EEPromArray[EEPROM_ADR_LAST_OFFSET]); |
| if(off > 20) off -= 10; |
| OCR0A = off; |
| Delay_ms_Mess(100); |
| if(MessLuftdruck < 850) off = 0; |
| for(; off < 250;off++) |
| { |
| OCR0A = off; |
| Delay_ms_Mess(50); |
| printf("."); |
| if(MessLuftdruck < 900) break; |
| } |
| eeprom_write_byte(&EEPromArray[EEPROM_ADR_LAST_OFFSET], off); |
| DruckOffsetSetting = off; |
| Delay_ms_Mess(300); |
| } |
| void SucheGyroOffset(void) |
| { |
| unsigned char i, ready = 0; |
| GyroDefektN = 0; GyroDefektR = 0; GyroDefektG = 0; |
| for(i=140; i != 0; i--) |
| { |
| if(ready == 3 && i > 10) i = 9; |
| ready = 0; |
| if(AdWertNick < 1020) AnalogOffsetNick--; else if(AdWertNick > 1030) AnalogOffsetNick++; else ready++; |
| if(AdWertRoll < 1020) AnalogOffsetRoll--; else if(AdWertRoll > 1030) AnalogOffsetRoll++; else ready++; |
| if(AdWertGier < 1020) AnalogOffsetGier--; else if(AdWertGier > 1030) AnalogOffsetGier++; else ready++; |
| twi_state = 8; |
| i2c_start(); |
| if(AnalogOffsetNick < 10) { GyroDefektN = 1; AnalogOffsetNick = 10;}; if(AnalogOffsetNick > 245) { GyroDefektN = 1; AnalogOffsetNick = 245;}; |
| if(AnalogOffsetRoll < 10) { GyroDefektR = 1; AnalogOffsetRoll = 10;}; if(AnalogOffsetRoll > 245) { GyroDefektR = 1; AnalogOffsetRoll = 245;}; |
| if(AnalogOffsetGier < 10) { GyroDefektG = 1; AnalogOffsetGier = 10;}; if(AnalogOffsetGier > 245) { GyroDefektG = 1; AnalogOffsetGier = 245;}; |
| while(twi_state); |
| messanzahl_Druck = 0; |
| ANALOG_ON; |
| while(messanzahl_Druck == 0); |
| if(i<10) Delay_ms_Mess(10); |
| } |
| Delay_ms_Mess(70); |
| } |
| //####################################################################################### |
| // |
| SIGNAL(SIG_ADC) |
| //####################################################################################### |
| { |
| static unsigned char kanal=0,state = 0; |
| static unsigned int gier1, roll1, nick1; |
| ANALOG_OFF; |
| switch(state++) |
| { |
| case 0: |
| gier1 = ADC; |
| kanal = 1; |
| ZaehlMessungen++; |
| break; |
| case 1: |
| roll1 = ADC; |
| kanal = 2; |
| break; |
| case 2: |
| nick1 = ADC; |
| kanal = 4; |
| break; |
| case 3: |
| UBat = (3 * UBat + ADC / 3) / 4;//(UBat + ((ADC * 39) / 256) + 19) / 2; |
| kanal = 6; |
| break; |
| case 4: |
| Aktuell_ay = NeutralAccY - ADC; |
| AdWertAccRoll = Aktuell_ay; |
| kanal = 7; |
| break; |
| case 5: |
| Aktuell_ax = ADC - NeutralAccX; |
| AdWertAccNick = Aktuell_ax; |
| kanal = 0; |
| break; |
| case 6: |
| if(PlatinenVersion == 10) AdWertGier = (ADC + gier1) / 2; |
| else AdWertGier = ADC + gier1; |
| kanal = 1; |
| break; |
| case 7: |
| if(PlatinenVersion == 10) AdWertRoll = (ADC + roll1) / 2; |
| else AdWertRoll = ADC + roll1; |
| kanal = 2; |
| break; |
| case 8: |
| if(PlatinenVersion == 10) AdWertNick = (ADC + nick1) / 2; |
| else AdWertNick = ADC + nick1; |
| //AdWertNick = 0; |
| //AdWertNick += Poti2; |
| kanal = 5; |
| break; |
| case 9: |
| AdWertAccHoch = (signed int) ADC - NeutralAccZ; |
| // AdWertAccHoch += abs(Aktuell_ay) / 4 + abs(Aktuell_ax) / 4; |
| if(AdWertAccHoch > 1) |
| { |
| if(NeutralAccZ < 750) |
| { |
| NeutralAccZ += 0.02; |
| if(modell_fliegt < 500) NeutralAccZ += 0.1; |
| } |
| } |
| else if(AdWertAccHoch < -1) |
| { |
| if(NeutralAccZ > 550) |
| { |
| NeutralAccZ-= 0.02; |
| if(modell_fliegt < 500) NeutralAccZ -= 0.1; |
| } |
| } |
| messanzahl_AccHoch = 1; |
| Aktuell_az = ADC; |
| Mess_Integral_Hoch += AdWertAccHoch; // Integrieren |
| Mess_Integral_Hoch -= Mess_Integral_Hoch / 1024; // dämfen |
| kanal = 3; |
| break; |
| case 10: |
| tmpLuftdruck += ADC; |
| if(++messanzahl_Druck >= 5) |
| { |
| MessLuftdruck = ADC; |
| messanzahl_Druck = 0; |
| HoeheD = (7 * HoeheD + (int) Parameter_Luftdruck_D * (int)(StartLuftdruck - tmpLuftdruck - HoehenWert))/8; // D-Anteil = neuerWert - AlterWert |
| Luftdruck = (tmpLuftdruck + 3 * Luftdruck) / 4; |
| HoehenWert = StartLuftdruck - Luftdruck; |
| tmpLuftdruck = 0; |
| } |
| kanal = 0; |
| state = 0; |
| break; |
| default: |
| kanal = 0; |
| state = 0; |
| break; |
| } |
| ADMUX = kanal; |
| if(state != 0) ANALOG_ON; |
| } |
| /branches/V0.70d_flinkflash/src/analog.d |
|---|
| 0,0 → 1,4 |
| analog.o analog.d : src/analog.c include/main.h include/old_macros.h \ |
| include/_Settings.h include/printf_P.h include/timer0.h include/uart.h \ |
| include/analog.h include/twimaster.h include/menu.h include/rc.h \ |
| include/fc.h include/gps.h include/spi.h include/led.h |
| /branches/V0.70d_flinkflash/src/eeprom.c |
|---|
| 0,0 → 1,218 |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Konstanten |
| // + 0-250 -> normale Werte |
| // + 251 -> Poti1 |
| // + 252 -> Poti2 |
| // + 253 -> Poti3 |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| void DefaultKonstanten1(void) |
| { |
| EE_Parameter.Kanalbelegung[K_NICK] = 1; |
| EE_Parameter.Kanalbelegung[K_ROLL] = 2; |
| EE_Parameter.Kanalbelegung[K_GAS] = 3; |
| EE_Parameter.Kanalbelegung[K_GIER] = 4; |
| EE_Parameter.Kanalbelegung[K_POTI1] = 5; |
| EE_Parameter.Kanalbelegung[K_POTI2] = 6; |
| EE_Parameter.Kanalbelegung[K_POTI3] = 7; |
| EE_Parameter.Kanalbelegung[K_POTI4] = 8; |
| EE_Parameter.GlobalConfig = CFG_ACHSENKOPPLUNG_AKTIV | CFG_KOMPASS_AKTIV | CFG_GPS_AKTIV;//CFG_HOEHEN_SCHALTER |
| EE_Parameter.Hoehe_MinGas = 30; |
| EE_Parameter.MaxHoehe = 251; // Wert : 0-250 251 -> Poti1 |
| EE_Parameter.Hoehe_P = 10; // Wert : 0-32 |
| EE_Parameter.Luftdruck_D = 30; // Wert : 0-250 |
| EE_Parameter.Hoehe_ACC_Wirkung = 30; // Wert : 0-250 |
| EE_Parameter.Hoehe_Verstaerkung = 4; // Wert : 0-50 |
| EE_Parameter.Stick_P = 15; // Wert : 1-6 |
| EE_Parameter.Stick_D = 30; // Wert : 0-64 |
| EE_Parameter.Gier_P = 12; // Wert : 1-20 |
| EE_Parameter.Gas_Min = 8; // Wert : 0-32 |
| EE_Parameter.Gas_Max = 230; // Wert : 33-250 |
| EE_Parameter.GyroAccFaktor = 30; // Wert : 1-64 |
| EE_Parameter.KompassWirkung = 128; // Wert : 0-250 |
| EE_Parameter.Gyro_P = 80; // Wert : 0-250 |
| EE_Parameter.Gyro_I = 150; // Wert : 0-250 |
| EE_Parameter.UnterspannungsWarnung = 94; // Wert : 0-250 |
| EE_Parameter.NotGas = 35; // Wert : 0-250 // Gaswert bei Empangsverlust |
| EE_Parameter.NotGasZeit = 30; // Wert : 0-250 // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen |
| EE_Parameter.UfoAusrichtung = 0; // X oder + Formation |
| EE_Parameter.I_Faktor = 32; |
| EE_Parameter.UserParam1 = 0; // zur freien Verwendung |
| EE_Parameter.UserParam2 = 0; // zur freien Verwendung |
| EE_Parameter.UserParam3 = 0; // zur freien Verwendung |
| EE_Parameter.UserParam4 = 0; // zur freien Verwendung |
| EE_Parameter.UserParam5 = 0; // zur freien Verwendung |
| EE_Parameter.UserParam6 = 0; // zur freien Verwendung |
| EE_Parameter.UserParam7 = 0; // zur freien Verwendung |
| EE_Parameter.UserParam8 = 0; // zur freien Verwendung |
| EE_Parameter.ServoNickControl = 100; // Wert : 0-250 // Stellung des Servos |
| EE_Parameter.ServoNickComp = 40; // Wert : 0-250 // Einfluss Gyro/Servo |
| EE_Parameter.ServoNickCompInvert = 0; // Wert : 0-250 // Richtung Einfluss Gyro/Servo |
| EE_Parameter.ServoNickMin = 50; // Wert : 0-250 // Anschlag |
| EE_Parameter.ServoNickMax = 150; // Wert : 0-250 // Anschlag |
| EE_Parameter.ServoNickRefresh = 5; |
| EE_Parameter.LoopGasLimit = 50; |
| EE_Parameter.LoopThreshold = 90; // Wert: 0-250 Schwelle für Stickausschlag |
| EE_Parameter.LoopHysterese = 50; |
| EE_Parameter.LoopConfig = 0; // Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts / wird getrennt behandelt |
| EE_Parameter.AchsKopplung1 = 90; |
| EE_Parameter.AchsGegenKopplung1 = 5; |
| EE_Parameter.WinkelUmschlagNick = 85; |
| EE_Parameter.WinkelUmschlagRoll = 85; |
| EE_Parameter.GyroAccAbgleich = 16; // 1/k |
| EE_Parameter.Driftkomp = 4; |
| EE_Parameter.DynamicStability = 100; |
| EE_Parameter.J16Bitmask = 95; |
| EE_Parameter.J17Bitmask = 243; |
| EE_Parameter.J16Timing = 15; |
| EE_Parameter.J17Timing = 15; |
| EE_Parameter.NaviGpsModeControl = 253; |
| EE_Parameter.NaviGpsGain = 100; |
| EE_Parameter.NaviGpsP = 90; |
| EE_Parameter.NaviGpsI = 90; |
| EE_Parameter.NaviGpsD = 90; |
| EE_Parameter.NaviGpsACC = 0; |
| EE_Parameter.NaviGpsMinSat = 6; |
| EE_Parameter.NaviStickThreshold = 8; |
| memcpy(EE_Parameter.Name, "Sport\0", 12); |
| } |
| void DefaultKonstanten2(void) |
| { |
| EE_Parameter.Kanalbelegung[K_NICK] = 1; |
| EE_Parameter.Kanalbelegung[K_ROLL] = 2; |
| EE_Parameter.Kanalbelegung[K_GAS] = 3; |
| EE_Parameter.Kanalbelegung[K_GIER] = 4; |
| EE_Parameter.Kanalbelegung[K_POTI1] = 5; |
| EE_Parameter.Kanalbelegung[K_POTI2] = 6; |
| EE_Parameter.Kanalbelegung[K_POTI3] = 7; |
| EE_Parameter.Kanalbelegung[K_POTI4] = 8; |
| EE_Parameter.GlobalConfig = CFG_ACHSENKOPPLUNG_AKTIV | CFG_KOMPASS_AKTIV | CFG_GPS_AKTIV;///*CFG_HOEHEN_SCHALTER |*/ CFG_KOMPASS_AKTIV;//0x01; |
| EE_Parameter.Hoehe_MinGas = 30; |
| EE_Parameter.MaxHoehe = 251; // Wert : 0-250 251 -> Poti1 |
| EE_Parameter.Hoehe_P = 10; // Wert : 0-32 |
| EE_Parameter.Luftdruck_D = 30; // Wert : 0-250 |
| EE_Parameter.Hoehe_ACC_Wirkung = 30; // Wert : 0-250 |
| EE_Parameter.Hoehe_Verstaerkung = 3; // Wert : 0-50 |
| EE_Parameter.Stick_P = 12; // Wert : 1-6 |
| EE_Parameter.Stick_D = 16; // Wert : 0-64 |
| EE_Parameter.Gier_P = 6; // Wert : 1-20 |
| EE_Parameter.Gas_Min = 8; // Wert : 0-32 |
| EE_Parameter.Gas_Max = 230; // Wert : 33-250 |
| EE_Parameter.GyroAccFaktor = 30; // Wert : 1-64 |
| EE_Parameter.KompassWirkung = 128; // Wert : 0-250 |
| EE_Parameter.Gyro_P = 80; // Wert : 0-250 |
| EE_Parameter.Gyro_I = 120; // Wert : 0-250 |
| EE_Parameter.UnterspannungsWarnung = 94; // Wert : 0-250 |
| EE_Parameter.NotGas = 35; // Wert : 0-250 // Gaswert bei Empangsverlust |
| EE_Parameter.NotGasZeit = 30; // Wert : 0-250 // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen |
| EE_Parameter.UfoAusrichtung = 0; // X oder + Formation |
| EE_Parameter.I_Faktor = 32; |
| EE_Parameter.UserParam1 = 0; // zur freien Verwendung |
| EE_Parameter.UserParam2 = 0; // zur freien Verwendung |
| EE_Parameter.UserParam3 = 0; // zur freien Verwendung |
| EE_Parameter.UserParam4 = 0; // zur freien Verwendung |
| EE_Parameter.UserParam5 = 0; // zur freien Verwendung |
| EE_Parameter.UserParam6 = 0; // zur freien Verwendung |
| EE_Parameter.UserParam7 = 0; // zur freien Verwendung |
| EE_Parameter.UserParam8 = 0; // zur freien Verwendung |
| EE_Parameter.ServoNickControl = 100; // Wert : 0-250 // Stellung des Servos |
| EE_Parameter.ServoNickComp = 40; // Wert : 0-250 // Einfluss Gyro/Servo |
| EE_Parameter.ServoNickCompInvert = 0; // Wert : 0-250 // Richtung Einfluss Gyro/Servo |
| EE_Parameter.ServoNickMin = 50; // Wert : 0-250 // Anschlag |
| EE_Parameter.ServoNickMax = 150; // Wert : 0-250 // Anschlag |
| EE_Parameter.ServoNickRefresh = 5; |
| EE_Parameter.LoopGasLimit = 50; |
| EE_Parameter.LoopThreshold = 90; // Wert: 0-250 Schwelle für Stickausschlag |
| EE_Parameter.LoopHysterese = 50; |
| EE_Parameter.LoopConfig = 0; // Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts |
| EE_Parameter.AchsKopplung1 = 90; // Faktor, mit dem Gier die Achsen Roll und Nick verkoppelt |
| EE_Parameter.AchsGegenKopplung1 = 5; |
| EE_Parameter.WinkelUmschlagNick = 85; |
| EE_Parameter.WinkelUmschlagRoll = 85; |
| EE_Parameter.GyroAccAbgleich = 32; // 1/k |
| EE_Parameter.Driftkomp = 4; |
| EE_Parameter.DynamicStability = 75; |
| EE_Parameter.J16Bitmask = 95; |
| EE_Parameter.J17Bitmask = 243; |
| EE_Parameter.J16Timing = 20; |
| EE_Parameter.J17Timing = 20; |
| EE_Parameter.NaviGpsModeControl = 253; |
| EE_Parameter.NaviGpsGain = 100; |
| EE_Parameter.NaviGpsP = 90; |
| EE_Parameter.NaviGpsI = 90; |
| EE_Parameter.NaviGpsD = 90; |
| EE_Parameter.NaviGpsACC = 0; |
| EE_Parameter.NaviGpsMinSat = 6; |
| EE_Parameter.NaviStickThreshold = 8; |
| memcpy(EE_Parameter.Name, "Normal\0", 12); |
| } |
| void DefaultKonstanten3(void) |
| { |
| EE_Parameter.Kanalbelegung[K_NICK] = 1; |
| EE_Parameter.Kanalbelegung[K_ROLL] = 2; |
| EE_Parameter.Kanalbelegung[K_GAS] = 3; |
| EE_Parameter.Kanalbelegung[K_GIER] = 4; |
| EE_Parameter.Kanalbelegung[K_POTI1] = 5; |
| EE_Parameter.Kanalbelegung[K_POTI2] = 6; |
| EE_Parameter.Kanalbelegung[K_POTI3] = 7; |
| EE_Parameter.Kanalbelegung[K_POTI4] = 8; |
| EE_Parameter.GlobalConfig = CFG_DREHRATEN_BEGRENZER | CFG_ACHSENKOPPLUNG_AKTIV | CFG_KOMPASS_AKTIV | CFG_GPS_AKTIV;///*CFG_HOEHEN_SCHALTER |*/ CFG_KOMPASS_AKTIV;//0x01; |
| EE_Parameter.Hoehe_MinGas = 30; |
| EE_Parameter.MaxHoehe = 251; // Wert : 0-250 251 -> Poti1 |
| EE_Parameter.Hoehe_P = 10; // Wert : 0-32 |
| EE_Parameter.Luftdruck_D = 30; // Wert : 0-250 |
| EE_Parameter.Hoehe_ACC_Wirkung = 30; // Wert : 0-250 |
| EE_Parameter.Hoehe_Verstaerkung = 3; // Wert : 0-50 |
| EE_Parameter.Stick_P = 8; // Wert : 1-6 |
| EE_Parameter.Stick_D = 16; // Wert : 0-64 |
| EE_Parameter.Gier_P = 6; // Wert : 1-20 |
| EE_Parameter.Gas_Min = 8; // Wert : 0-32 |
| EE_Parameter.Gas_Max = 230; // Wert : 33-250 |
| EE_Parameter.GyroAccFaktor = 30; // Wert : 1-64 |
| EE_Parameter.KompassWirkung = 128; // Wert : 0-250 |
| EE_Parameter.Gyro_P = 100; // Wert : 0-250 |
| EE_Parameter.Gyro_I = 120; // Wert : 0-250 |
| EE_Parameter.UnterspannungsWarnung = 94; // Wert : 0-250 |
| EE_Parameter.NotGas = 35; // Wert : 0-250 // Gaswert bei Empangsverlust |
| EE_Parameter.NotGasZeit = 20; // Wert : 0-250 // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen |
| EE_Parameter.UfoAusrichtung = 0; // X oder + Formation |
| EE_Parameter.I_Faktor = 16; |
| EE_Parameter.UserParam1 = 0; // zur freien Verwendung |
| EE_Parameter.UserParam2 = 0; // zur freien Verwendung |
| EE_Parameter.UserParam3 = 0; // zur freien Verwendung |
| EE_Parameter.UserParam4 = 0; // zur freien Verwendung |
| EE_Parameter.UserParam5 = 0; // zur freien Verwendung |
| EE_Parameter.UserParam6 = 0; // zur freien Verwendung |
| EE_Parameter.UserParam7 = 0; // zur freien Verwendung |
| EE_Parameter.UserParam8 = 0; // zur freien Verwendung |
| EE_Parameter.ServoNickControl = 100; // Wert : 0-250 // Stellung des Servos |
| EE_Parameter.ServoNickComp = 40; // Wert : 0-250 // Einfluss Gyro/Servo |
| EE_Parameter.ServoNickCompInvert = 0; // Wert : 0-250 // Richtung Einfluss Gyro/Servo |
| EE_Parameter.ServoNickMin = 50; // Wert : 0-250 // Anschlag |
| EE_Parameter.ServoNickMax = 150; // Wert : 0-250 // Anschlag |
| EE_Parameter.ServoNickRefresh = 5; |
| EE_Parameter.LoopGasLimit = 50; |
| EE_Parameter.LoopThreshold = 90; // Wert: 0-250 Schwelle für Stickausschlag |
| EE_Parameter.LoopHysterese = 50; |
| EE_Parameter.LoopConfig = 0; // Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts |
| EE_Parameter.AchsKopplung1 = 90; // Faktor, mit dem Gier die Achsen Roll und Nick verkoppelt |
| EE_Parameter.AchsGegenKopplung1 = 5; |
| EE_Parameter.WinkelUmschlagNick = 85; |
| EE_Parameter.WinkelUmschlagRoll = 85; |
| EE_Parameter.GyroAccAbgleich = 32; // 1/k |
| EE_Parameter.Driftkomp = 4; |
| EE_Parameter.DynamicStability = 50; |
| EE_Parameter.J16Bitmask = 95; |
| EE_Parameter.J17Bitmask = 243; |
| EE_Parameter.J16Timing = 30; |
| EE_Parameter.J17Timing = 30; |
| EE_Parameter.NaviGpsModeControl = 253; |
| EE_Parameter.NaviGpsGain = 100; |
| EE_Parameter.NaviGpsP = 90; |
| EE_Parameter.NaviGpsI = 90; |
| EE_Parameter.NaviGpsD = 90; |
| EE_Parameter.NaviGpsACC = 0; |
| EE_Parameter.NaviGpsMinSat = 6; |
| EE_Parameter.NaviStickThreshold = 8; |
| memcpy(EE_Parameter.Name, "Beginner\0", 12); |
| } |
| /branches/V0.70d_flinkflash/src/fc.c |
|---|
| 0,0 → 1,1280 |
| /*####################################################################################### |
| Flight Control |
| #######################################################################################*/ |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Copyright (c) 04.2007 Holger Buss |
| // + Nur für den privaten Gebrauch |
| // + www.MikroKopter.com |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation), |
| // + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist. |
| // + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt |
| // + bzgl. der Nutzungsbedingungen aufzunehmen. |
| // + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen, |
| // + Verkauf von Luftbildaufnahmen, usw. |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht, |
| // + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts |
| // + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
| // + eindeutig als Ursprung verlinkt werden |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion |
| // + Benutzung auf eigene Gefahr |
| // + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
| // + mit unserer Zustimmung zulässig |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
| // + this list of conditions and the following disclaimer. |
| // + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived |
| // + from this software without specific prior written permission. |
| // + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet |
| // + for non-commercial use (directly or indirectly) |
| // + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
| // + with our written permission |
| // + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
| // + clearly linked as origin |
| // + * porting to systems other than hardware from www.mikrokopter.de is not allowed |
| // + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
| // + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
| // + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
| // + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
| // + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
| // + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
| // + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
| // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
| // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
| // + POSSIBILITY OF SUCH DAMAGE. |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| #include "main.h" |
| #include "eeprom.c" |
| unsigned char h,m,s; |
| volatile unsigned int I2CTimeout = 100; |
| volatile int MesswertNick,MesswertRoll,MesswertGier,MesswertGierBias; |
| int AdNeutralGierBias; |
| int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0; |
| int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch, NeutralAccX=0, NeutralAccY=0; |
| int NaviAccNick, NaviAccRoll,NaviCntAcc = 0; |
| volatile float NeutralAccZ = 0; |
| unsigned char CosinusNickWinkel = 0, CosinusRollWinkel = 0; |
| long IntegralNick = 0,IntegralNick2 = 0; |
| long IntegralRoll = 0,IntegralRoll2 = 0; |
| long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0; |
| long Integral_Gier = 0; |
| long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0; |
| long Mess_IntegralRoll = 0,Mess_IntegralRoll2 = 0; |
| long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0; |
| long MittelIntegralNick,MittelIntegralRoll,MittelIntegralNick2,MittelIntegralRoll2; |
| volatile long Mess_Integral_Hoch = 0; |
| volatile int KompassValue = 0; |
| volatile int KompassStartwert = 0; |
| volatile int KompassRichtung = 0; |
| unsigned int KompassSignalSchlecht = 500; |
| unsigned char MAX_GAS,MIN_GAS; |
| unsigned char Notlandung = 0; |
| unsigned char HoehenReglerAktiv = 0; |
| unsigned char TrichterFlug = 0; |
| long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L; |
| long ErsatzKompass; |
| int ErsatzKompassInGrad; // Kompasswert in Grad |
| int GierGyroFehler = 0; |
| float GyroFaktor; |
| float IntegralFaktor; |
| volatile int DiffNick,DiffRoll; |
| int Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0; |
| volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count; |
| volatile unsigned char SenderOkay = 0; |
| int StickNick = 0,StickRoll = 0,StickGier = 0,StickGas = 0; |
| char MotorenEin = 0; |
| int HoehenWert = 0; |
| int SollHoehe = 0; |
| int LageKorrekturRoll = 0,LageKorrekturNick = 0; |
| float Ki = FAKTOR_I; |
| unsigned char Looping_Nick = 0,Looping_Roll = 0; |
| unsigned char Looping_Links = 0, Looping_Rechts = 0, Looping_Unten = 0, Looping_Oben = 0; |
| unsigned char Parameter_Luftdruck_D = 48; // Wert : 0-250 |
| unsigned char Parameter_MaxHoehe = 251; // Wert : 0-250 |
| unsigned char Parameter_Hoehe_P = 16; // Wert : 0-32 |
| unsigned char Parameter_Hoehe_ACC_Wirkung = 58; // Wert : 0-250 |
| unsigned char Parameter_KompassWirkung = 64; // Wert : 0-250 |
| unsigned char Parameter_Gyro_P = 150; // Wert : 10-250 |
| unsigned char Parameter_Gyro_I = 150; // Wert : 0-250 |
| unsigned char Parameter_Gier_P = 2; // Wert : 1-20 |
| unsigned char Parameter_I_Faktor = 10; // Wert : 1-20 |
| unsigned char Parameter_UserParam1 = 0; |
| unsigned char Parameter_UserParam2 = 0; |
| unsigned char Parameter_UserParam3 = 0; |
| unsigned char Parameter_UserParam4 = 0; |
| unsigned char Parameter_UserParam5 = 0; |
| unsigned char Parameter_UserParam6 = 0; |
| unsigned char Parameter_UserParam7 = 0; |
| unsigned char Parameter_UserParam8 = 0; |
| unsigned char Parameter_ServoNickControl = 100; |
| unsigned char Parameter_LoopGasLimit = 70; |
| unsigned char Parameter_AchsKopplung1 = 0; |
| unsigned char Parameter_AchsGegenKopplung1 = 0; |
| unsigned char Parameter_DynamicStability = 100; |
| unsigned char Parameter_J16Bitmask; // for the J16 Output |
| unsigned char Parameter_J16Timing; // for the J16 Output |
| unsigned char Parameter_J17Bitmask; // for the J17 Output |
| unsigned char Parameter_J17Timing; // for the J17 Output |
| unsigned char Parameter_NaviGpsModeControl; // Parameters for the Naviboard |
| unsigned char Parameter_NaviGpsGain; |
| unsigned char Parameter_NaviGpsP; |
| unsigned char Parameter_NaviGpsI; |
| unsigned char Parameter_NaviGpsD; |
| unsigned char Parameter_NaviGpsACC; |
| unsigned char Parameter_ExternalControl; |
| struct mk_param_struct EE_Parameter; |
| signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20; |
| int MaxStickNick = 0,MaxStickRoll = 0; |
| unsigned int modell_fliegt = 0; |
| unsigned char MikroKopterFlags = 0; |
| void Piep(unsigned char Anzahl) |
| { |
| while(Anzahl--) |
| { |
| if(MotorenEin) return; //auf keinen Fall im Flug! |
| beeptime = 100; |
| Delay_ms(250); |
| } |
| } |
| //############################################################################ |
| // Nullwerte ermitteln |
| void SetNeutral(void) |
| //############################################################################ |
| { |
| NeutralAccX = 0; |
| NeutralAccY = 0; |
| NeutralAccZ = 0; |
| AdNeutralNick = 0; |
| AdNeutralRoll = 0; |
| AdNeutralGier = 0; |
| AdNeutralGierBias = 0; |
| Parameter_AchsKopplung1 = 0; |
| Parameter_AchsGegenKopplung1 = 0; |
| CalibrierMittelwert(); |
| Delay_ms_Mess(100); |
| CalibrierMittelwert(); |
| if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert? |
| { |
| if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset(); |
| } |
| AdNeutralNick= AdWertNick; |
| AdNeutralRoll= AdWertRoll; |
| AdNeutralGier= AdWertGier; |
| AdNeutralGierBias = AdWertGier; |
| StartNeutralRoll = AdNeutralRoll; |
| StartNeutralNick = AdNeutralNick; |
| if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) > 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; |
| Mess_IntegralNick2 = 0; |
| Mess_IntegralRoll = 0; |
| Mess_IntegralRoll2 = 0; |
| Mess_Integral_Gier = 0; |
| MesswertNick = 0; |
| MesswertRoll = 0; |
| MesswertGier = 0; |
| Delay_ms_Mess(100); |
| StartLuftdruck = Luftdruck; |
| HoeheD = 0; |
| Mess_Integral_Hoch = 0; |
| KompassStartwert = KompassValue; |
| GPS_Neutral(); |
| beeptime = 50; |
| Umschlag180Nick = ((long) EE_Parameter.WinkelUmschlagNick * 2500L) + 15000L; |
| Umschlag180Roll = ((long) EE_Parameter.WinkelUmschlagRoll * 2500L) + 15000L; |
| ExternHoehenValue = 0; |
| ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR; |
| GierGyroFehler = 0; |
| SendVersionToNavi = 1; |
| LED_Init(); |
| MikroKopterFlags |= FLAG_CALIBRATE; |
| } |
| //############################################################################ |
| // Bearbeitet die Messwerte |
| void Mittelwert(void) |
| //############################################################################ |
| { |
| static signed long tmpl,tmpl2; |
| MesswertGier = (signed int) AdNeutralGier - AdWertGier; |
| MesswertGierBias = (signed int) AdNeutralGierBias - 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; |
| NaviAccNick += AdWertAccNick; |
| NaviAccRoll += AdWertAccRoll; |
| NaviCntAcc++; |
| IntegralAccZ += Aktuell_az - NeutralAccZ; |
| // Gier ++++++++++++++++++++++++++++++++++++++++++++++++ |
| ErsatzKompass += MesswertGier; |
| Mess_Integral_Gier += MesswertGier; |
| // Mess_Integral_Gier2 += MesswertGier; |
| if(ErsatzKompass >= (360L * GIER_GRAD_FAKTOR)) ErsatzKompass -= 360L * GIER_GRAD_FAKTOR; // 360° Umschlag |
| if(ErsatzKompass < 0) ErsatzKompass += 360L * GIER_GRAD_FAKTOR; |
| // Kopplungsanteil +++++++++++++++++++++++++++++++++++++ |
| if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV)) |
| { |
| tmpl = (MesswertGierBias * Mess_IntegralNick) / 2048L; |
| tmpl *= Parameter_AchsKopplung1; //125 |
| tmpl /= 4096L; |
| tmpl2 = (MesswertGierBias * Mess_IntegralRoll) / 2048L; |
| tmpl2 *= Parameter_AchsKopplung1; |
| tmpl2 /= 4096L; |
| if(labs(tmpl) > 128 || labs(tmpl2) > 128) TrichterFlug = 1; |
| } |
| 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 - 25000L); |
| Mess_IntegralRoll2 = Mess_IntegralRoll; |
| } |
| if(Mess_IntegralRoll <-Umschlag180Roll) |
| { |
| Mess_IntegralRoll = (Umschlag180Roll - 25000L); |
| 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; |
| Mess_IntegralNick += MesswertNick - LageKorrekturNick; |
| if(Mess_IntegralNick > Umschlag180Nick) |
| { |
| Mess_IntegralNick = -(Umschlag180Nick - 25000L); |
| Mess_IntegralNick2 = Mess_IntegralNick; |
| } |
| if(Mess_IntegralNick <-Umschlag180Nick) |
| { |
| Mess_IntegralNick = (Umschlag180Nick - 25000L); |
| 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) |
| { |
| 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) |
| //############################################################################ |
| { |
| if(PlatinenVersion >= 13) SucheGyroOffset(); |
| // ADC auschalten, damit die Werte sich nicht während der Berechnung ändern |
| 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.WinkelUmschlagRoll * 2500L; |
| } |
| //############################################################################ |
| // Senden der Motorwerte per I2C-Bus |
| void SendMotorData(void) |
| //############################################################################ |
| { |
| if(!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]; |
| MikroKopterFlags &= ~(FLAG_MOTOR_RUN | FLAG_FLY); |
| } else MikroKopterFlags |= FLAG_MOTOR_RUN; |
| 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ägt ggf. das Poti als Parameter ein |
| void ParameterZuordnung(void) |
| //############################################################################ |
| { |
| #define CHK_POTI_MM(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;} |
| #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; } |
| CHK_POTI(Parameter_MaxHoehe,EE_Parameter.MaxHoehe,0,255); |
| CHK_POTI_MM(Parameter_Luftdruck_D,EE_Parameter.Luftdruck_D,0,100); |
| CHK_POTI_MM(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_MM(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); |
| CHK_POTI_MM(Parameter_J16Timing,EE_Parameter.J16Timing,1,255); |
| CHK_POTI_MM(Parameter_J17Timing,EE_Parameter.J17Timing,1,255); |
| CHK_POTI(Parameter_NaviGpsModeControl,EE_Parameter.NaviGpsModeControl,0,255); |
| CHK_POTI(Parameter_NaviGpsGain,EE_Parameter.NaviGpsGain,0,255); |
| CHK_POTI(Parameter_NaviGpsP,EE_Parameter.NaviGpsP,0,255); |
| CHK_POTI(Parameter_NaviGpsI,EE_Parameter.NaviGpsI,0,255); |
| CHK_POTI(Parameter_NaviGpsD,EE_Parameter.NaviGpsD,0,255); |
| CHK_POTI(Parameter_NaviGpsACC,EE_Parameter.NaviGpsACC,0,255); |
| CHK_POTI(Parameter_ExternalControl,EE_Parameter.ExternalControl,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 int hoehenregler = 0; |
| static char TimerWerteausgabe = 0; |
| static char NeueKompassRichtungMerken = 0; |
| static long ausgleichNick, ausgleichRoll; |
| Mittelwert(); |
| GRN_ON; |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // Gaswert ermitteln |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| GasMischanteil = StickGas; |
| if(GasMischanteil < MIN_GAS + 10) GasMischanteil = MIN_GAS + 10; |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // Empfang 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 > 1000) // wahrscheinlich in der Luft --> langsam absenken |
| { |
| GasMischanteil = EE_Parameter.NotGas; |
| Notlandung = 1; |
| PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] = 0; |
| PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] = 0; |
| 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; |
| if(GasMischanteil > 40 && MotorenEin) |
| { |
| if(modell_fliegt < 0xffff) modell_fliegt++; |
| } |
| if((modell_fliegt < 256)) |
| { |
| SummeNick = 0; |
| SummeRoll = 0; |
| if(modell_fliegt == 250) |
| { |
| NeueKompassRichtungMerken = 1; |
| sollGier = 0; |
| Mess_Integral_Gier = 0; |
| // Mess_Integral_Gier2 = 0; |
| } |
| } else MikroKopterFlags |= FLAG_FLY; |
| 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 |
| } |
| // else |
| if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) < 20 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -70) |
| { |
| WinkelOut.CalcState = 1; |
| beeptime = 1000; |
| } |
| else |
| { |
| ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE); |
| if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert? |
| { |
| if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset(); |
| } |
| 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öschen |
| 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; |
| MikroKopterFlags |= FLAG_START; |
| } |
| } |
| else delay_einschalten = 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(); |
| stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4; |
| stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D; |
| StickNick = stick_nick - (GPS_Nick + GPS_Nick2); |
| // StickNick = (StickNick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4; |
| stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4; |
| stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; |
| StickRoll = stick_roll - (GPS_Roll + GPS_Roll2); |
| // StickRoll = (StickRoll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4; |
| 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--; |
| */ |
| GyroFaktor = ((float)Parameter_Gyro_P + 10.0) / (256.0/STICK_GAIN); |
| IntegralFaktor = ((float) Parameter_Gyro_I) / (44000 / STICK_GAIN); |
| //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| //+ Digitale Steuerung per DubWise |
| //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| #define KEY_VALUE (Parameter_ExternalControl * 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 += (STICK_GAIN * ExternStickNick) / 8; |
| StickRoll += (STICK_GAIN * ExternStickRoll) / 8; |
| StickGier += STICK_GAIN * ExternStickGier; |
| //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| //+ Analoge Steuerung per Seriell |
| //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| if(ExternControl.Config & 0x01 && Parameter_ExternalControl > 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(StickGas < 0) StickGas = 0; |
| if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor = 0; |
| if(GyroFaktor < 0) GyroFaktor = 0; |
| if(IntegralFaktor < 0) IntegralFaktor = 0; |
| if(abs(StickNick/STICK_GAIN) > MaxStickNick) |
| { |
| MaxStickNick = abs(StickNick)/STICK_GAIN; |
| if(MaxStickNick > 100) MaxStickNick = 100; |
| } |
| else MaxStickNick--; |
| if(abs(StickRoll/STICK_GAIN) > MaxStickRoll) |
| { |
| MaxStickRoll = abs(StickRoll)/STICK_GAIN; |
| if(MaxStickRoll > 100) MaxStickRoll = 100; |
| } |
| else MaxStickRoll--; |
| if(Notlandung) {MaxStickNick = 0; MaxStickRoll = 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 || Looping_Nick) |
| { |
| if(GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit; |
| } |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // Bei Empfangsausfall im Flug |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| if(Notlandung) |
| { |
| StickGier = 0; |
| StickNick = 0; |
| StickRoll = 0; |
| GyroFaktor = (float) 100 / (256.0 / STICK_GAIN); |
| IntegralFaktor = (float) 120 / (44000 / STICK_GAIN); |
| Looping_Roll = 0; |
| Looping_Nick = 0; |
| } |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // Integrale auf ACC-Signal abgleichen |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| #define ABGLEICH_ANZAHL 256L |
| MittelIntegralNick += IntegralNick; // Für 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; |
| Mess_IntegralRoll2 = Mess_IntegralRoll; |
| ZaehlMessungen = 0; |
| LageKorrekturNick = 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 > 64) || (MaxStickRoll > 64)) |
| { |
| 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; |
| if(!Looping_Nick && !Looping_Roll && !TrichterFlug) |
| { |
| MittelIntegralNick /= ABGLEICH_ANZAHL; |
| MittelIntegralRoll /= ABGLEICH_ANZAHL; |
| IntegralAccNick = (EE_Parameter.GyroAccFaktor * IntegralAccNick) / ABGLEICH_ANZAHL; |
| 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; |
| // Roll ++++++++++++++++++++++++++++++++++++++++++++++++ |
| IntegralFehlerRoll = (long)(MittelIntegralRoll - (long)IntegralAccRoll); |
| ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich; |
| LageKorrekturNick = ausgleichNick / ABGLEICH_ANZAHL; |
| LageKorrekturRoll = ausgleichRoll / ABGLEICH_ANZAHL; |
| if((MaxStickNick > 64) || (MaxStickRoll > 64) || (abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)) |
| { |
| LageKorrekturNick /= 2; |
| LageKorrekturRoll /= 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; |
| if(GierGyroFehler > ABGLEICH_ANZAHL/2) { AdNeutralGier++; AdNeutralGierBias++; } |
| if(GierGyroFehler <-ABGLEICH_ANZAHL/2) { AdNeutralGier--; AdNeutralGierBias--; } |
| DebugOut.Analog[22] = MittelIntegralRoll / 26; |
| //DebugOut.Analog[24] = GierGyroFehler; |
| GierGyroFehler = 0; |
| /*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; |
| //MittelIntegralRoll = MittelIntegralRoll; |
| //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; |
| KompassSignalSchlecht = 1000; |
| } |
| 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; |
| KompassSignalSchlecht = 1000; |
| } |
| if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp; |
| if(IntegralFehlerRoll > FEHLER_LIMIT) AdNeutralRoll += cnt; |
| if(IntegralFehlerRoll < -FEHLER_LIMIT) AdNeutralRoll -= cnt; |
| } |
| else |
| { |
| LageKorrekturRoll = 0; |
| LageKorrekturNick = 0; |
| TrichterFlug = 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(GasMischanteil < 35) { if(StickGier > 10) StickGier = 10; else if(StickGier < -10) StickGier = -10;}; |
| if(abs(StickGier) > 15) // war 35 |
| { |
| KompassSignalSchlecht = 1000; |
| 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 |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| //DebugOut.Analog[29] = (MaxStickNick + MaxStickRoll); |
| if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)) |
| { |
| int w,v,r,fehler,korrektur; |
| w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln |
| v = abs(IntegralRoll /512); |
| if(v > w) w = v; // grösste Neigung ermitteln |
| korrektur = w / 8 + 1; |
| fehler = ((540 + KompassValue - (ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180; |
| //DebugOut.Analog[25] = KompassSignalSchlecht; |
| if(!KompassSignalSchlecht && w < 25) |
| { |
| GierGyroFehler += fehler; |
| if(NeueKompassRichtungMerken) |
| { |
| beeptime = 200; |
| // KompassStartwert = KompassValue; |
| KompassStartwert = (ErsatzKompass/GIER_GRAD_FAKTOR); |
| NeueKompassRichtungMerken = 0; |
| } |
| } |
| ErsatzKompass += (fehler * 8) / korrektur; |
| w = (w * Parameter_KompassWirkung) / 32; // auf die Wirkung normieren |
| w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln |
| if(w >= 0) |
| { |
| if(!KompassSignalSchlecht) |
| { |
| v = 64 + ((MaxStickNick + MaxStickRoll)) / 8; |
| r = ((540 + (ErsatzKompass/GIER_GRAD_FAKTOR) - KompassStartwert) % 360) - 180; |
| // r = KompassRichtung; |
| v = (r * w) / v; // nach Kompass ausrichten |
| w = 3 * Parameter_KompassWirkung; |
| if(v > w) v = w; // Begrenzen |
| else |
| if(v < -w) v = -w; |
| Mess_Integral_Gier += v; |
| } |
| if(KompassSignalSchlecht) KompassSignalSchlecht--; |
| } |
| else KompassSignalSchlecht = 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[11] = ErsatzKompass / GIER_GRAD_FAKTOR; |
| DebugOut.Analog[10] = SenderOkay; |
| //DebugOut.Analog[16] = Mittelwert_AccHoch; |
| // DebugOut.Analog[17] = FromNaviCtrl_Value.Distance; |
| // DebugOut.Analog[18] = (int)FromNaviCtrl_Value.OsdBar; |
| DebugOut.Analog[19] = WinkelOut.CalcState; |
| DebugOut.Analog[20] = ServoValue; |
| DebugOut.Analog[30] = GPS_Nick; |
| DebugOut.Analog[31] = GPS_Roll; |
| // DebugOut.Analog[19] -= DebugOut.Analog[19]/128; |
| // if(DebugOut.Analog[19] > 0) DebugOut.Analog[19]--; else DebugOut.Analog[19]++; |
| /* 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 |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| if(Looping_Nick) MesswertNick = MesswertNick * GyroFaktor; |
| else MesswertNick = IntegralNick * IntegralFaktor + MesswertNick * GyroFaktor; |
| if(Looping_Roll) MesswertRoll = MesswertRoll * GyroFaktor; |
| else MesswertRoll = IntegralRoll * IntegralFaktor + MesswertRoll * GyroFaktor; |
| MesswertGier = MesswertGier * (2 * GyroFaktor) + Integral_Gier * IntegralFaktor / 2; |
| DebugOut.Analog[21] = MesswertNick; |
| DebugOut.Analog[22] = MesswertRoll; |
| // Maximalwerte abfangen |
| #define MAX_SENSOR (4096*STICK_GAIN) |
| 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öhenregelung |
| // Die Höhenregelung schwächt lediglich das Gas ab, erhöht es allerdings nicht |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| //OCR0B = 180 - (Poti1 + 120) / 4; |
| //DruckOffsetSetting = OCR0B; |
| GasMischanteil *= STICK_GAIN; |
| if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung |
| { |
| int tmp_int; |
| if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER) // Regler wird über Schalter gesteuert |
| { |
| if(Parameter_MaxHoehe < 50) |
| { |
| SollHoehe = HoehenWert - 20; // Parameter_MaxHoehe ist der PPM-Wert des Schalters |
| HoehenReglerAktiv = 0; |
| } |
| else |
| HoehenReglerAktiv = 1; |
| } |
| else |
| { |
| 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 / STICK_GAIN); // Differenz bestimmen --> P-Anteil |
| h = GasMischanteil - h; // vom Gas abziehen |
| // h -= (HoeheD * Parameter_Luftdruck_D)/(8/STICK_GAIN); // D-Anteil |
| h -= (HoeheD)/(8/STICK_GAIN); // D-Anteil |
| tmp_int = ((Mess_Integral_Hoch / 128) * (signed long) Parameter_Hoehe_ACC_Wirkung) / (128 / STICK_GAIN); |
| if(tmp_int > 70*STICK_GAIN) tmp_int = 70*STICK_GAIN; |
| else if(tmp_int < -(70*STICK_GAIN)) tmp_int = -(70*STICK_GAIN); |
| h -= tmp_int; |
| hoehenregler = (hoehenregler*15 + h) / 16; |
| if(hoehenregler < EE_Parameter.Hoehe_MinGas * STICK_GAIN) // nicht unter MIN |
| { |
| if(GasMischanteil >= EE_Parameter.Hoehe_MinGas * STICK_GAIN) hoehenregler = EE_Parameter.Hoehe_MinGas * STICK_GAIN; |
| if(GasMischanteil < EE_Parameter.Hoehe_MinGas * STICK_GAIN) hoehenregler = GasMischanteil; |
| } |
| if(hoehenregler > GasMischanteil) hoehenregler = GasMischanteil; // nicht mehr als Gas |
| GasMischanteil = hoehenregler; |
| } |
| } |
| if(GasMischanteil > (MAX_GAS - 20) * STICK_GAIN) GasMischanteil = (MAX_GAS - 20) * STICK_GAIN; |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Mischer und PI-Regler |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| DebugOut.Analog[7] = GasMischanteil; |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // Gier-Anteil |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| #define MUL_G 1.0 |
| GierMischanteil = MesswertGier - sollGier * STICK_GAIN; // Regler für Gier |
| // GierMischanteil = 0; |
| #define MIN_GIERGAS (40*STICK_GAIN) // unter diesem Gaswert trotzdem Gieren |
| if(GasMischanteil > MIN_GIERGAS) |
| { |
| if(GierMischanteil > (GasMischanteil / 2)) GierMischanteil = GasMischanteil / 2; |
| if(GierMischanteil < -(GasMischanteil / 2)) GierMischanteil = -(GasMischanteil / 2); |
| } |
| else |
| { |
| if(GierMischanteil > (MIN_GIERGAS / 2)) GierMischanteil = MIN_GIERGAS / 2; |
| if(GierMischanteil < -(MIN_GIERGAS / 2)) GierMischanteil = -(MIN_GIERGAS / 2); |
| } |
| tmp_int = MAX_GAS*STICK_GAIN; |
| if(GierMischanteil > ((tmp_int - GasMischanteil))) GierMischanteil = ((tmp_int - GasMischanteil)); |
| if(GierMischanteil < -((tmp_int - GasMischanteil))) GierMischanteil = -((tmp_int - GasMischanteil)); |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // Nick-Achse |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| DiffNick = MesswertNick - StickNick; // Differenz bestimmen |
| if(IntegralFaktor) SummeNick += IntegralNick * IntegralFaktor - StickNick; // I-Anteil bei Winkelregelung |
| else SummeNick += DiffNick; // I-Anteil bei HH |
| if(SummeNick > (STICK_GAIN * 16000L)) SummeNick = (STICK_GAIN * 16000L); |
| if(SummeNick < -(16000L * STICK_GAIN)) SummeNick = -(16000L * STICK_GAIN); |
| pd_ergebnis = DiffNick + Ki * SummeNick; // PI-Regler für Nick |
| // Motor Vorn |
| 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; |
| motorwert = GasMischanteil + pd_ergebnis + GierMischanteil; // Mischer |
| motorwert /= STICK_GAIN; |
| 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; |
| motorwert /= STICK_GAIN; |
| 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; // Differenz bestimmen |
| if(IntegralFaktor) SummeRoll += IntegralRoll * IntegralFaktor - StickRoll;// I-Anteil bei Winkelregelung |
| else SummeRoll += DiffRoll; // I-Anteil bei HH |
| if(SummeRoll > (STICK_GAIN * 16000L)) SummeRoll = (STICK_GAIN * 16000L); |
| if(SummeRoll < -(16000L * STICK_GAIN)) SummeRoll = -(16000L * STICK_GAIN); |
| pd_ergebnis = DiffRoll + Ki * SummeRoll; // PI-Regler für 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; |
| motorwert /= STICK_GAIN; |
| 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; |
| motorwert /= STICK_GAIN; |
| if ((motorwert < 0)) motorwert = 0; |
| else if(motorwert > MAX_GAS) motorwert = MAX_GAS; |
| if (motorwert < MIN_GAS) motorwert = MIN_GAS; |
| Motor_Rechts = motorwert; |
| // +++++++++++++++++++++++++++++++++++++++++++++++ |
| } |
| /branches/V0.70d_flinkflash/src/fc.d |
|---|
| 0,0 → 1,4 |
| fc.o fc.d : src/fc.c include/main.h include/old_macros.h include/_Settings.h \ |
| include/printf_P.h include/timer0.h include/uart.h include/analog.h \ |
| include/twimaster.h include/menu.h include/rc.h include/fc.h \ |
| include/gps.h include/spi.h include/led.h src/eeprom.c |
| /branches/V0.70d_flinkflash/src/led.c |
|---|
| 0,0 → 1,49 |
| #include <inttypes.h> |
| #include "main.h" |
| uint16_t LED1_Timing = 0; |
| uint16_t LED2_Timing = 0; |
| unsigned char J16Blinkcount = 0, J16Mask = 1; |
| unsigned char J17Blinkcount = 0, J17Mask = 1; |
| // initializes the LED control outputs J16, J17 |
| void LED_Init(void) |
| { |
| // set PC2 & PC3 as output (control of J16 & J17) |
| DDRC |= (1<<DDC2)|(1<<DDC3); |
| J16_OFF; |
| J17_OFF; |
| J16Blinkcount = 0; J16Mask = 128; |
| J17Blinkcount = 0; J17Mask = 128; |
| } |
| // called in UpdateMotors() every 2ms |
| void LED_Update(void) |
| { |
| static char delay = 0; |
| if(!delay--) // 10ms Intervall |
| { |
| delay = 4; |
| if((EE_Parameter.J16Timing > 250) && (Parameter_J16Timing > 230)) {if(EE_Parameter.J16Bitmask & 128) J16_ON; else J16_OFF;} |
| else |
| if((EE_Parameter.J16Timing > 250) && (Parameter_J16Timing < 10)) {if(EE_Parameter.J16Bitmask & 128) J16_OFF; else J16_ON;} |
| else |
| if(!J16Blinkcount--) |
| { |
| J16Blinkcount = Parameter_J16Timing-1; |
| if(J16Mask == 1) J16Mask = 128; else J16Mask /= 2; |
| if(J16Mask & EE_Parameter.J16Bitmask) J16_ON; else J16_OFF; |
| } |
| if((EE_Parameter.J17Timing > 250) && (Parameter_J17Timing > 230)) {if(EE_Parameter.J17Bitmask & 128) J17_ON; else J17_OFF;} |
| else |
| if((EE_Parameter.J17Timing > 250) && (Parameter_J17Timing < 10)) {if(EE_Parameter.J17Bitmask & 128) J17_OFF; else J17_ON;} |
| else |
| if(!J17Blinkcount--) |
| { |
| J17Blinkcount = Parameter_J17Timing-1; |
| if(J17Mask == 1) J17Mask = 128; else J17Mask /= 2; |
| if(J17Mask & EE_Parameter.J17Bitmask) J17_ON; else J17_OFF; |
| } |
| } |
| } |
| /branches/V0.70d_flinkflash/src/led.d |
|---|
| 0,0 → 1,4 |
| led.o led.d : src/led.c include/main.h include/old_macros.h include/_Settings.h \ |
| include/printf_P.h include/timer0.h include/uart.h include/analog.h \ |
| include/twimaster.h include/menu.h include/rc.h include/fc.h \ |
| include/gps.h include/spi.h include/led.h |
| /branches/V0.70d_flinkflash/src/main.c |
|---|
| 0,0 → 1,289 |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Copyright (c) 04.2007 Holger Buss |
| // + Nur für den privaten Gebrauch |
| // + www.MikroKopter.com |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation), |
| // + dass eine Nutzung (auch auszugsweise) nur für den privaten und nicht-kommerziellen Gebrauch zulässig ist. |
| // + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt |
| // + bzgl. der Nutzungsbedingungen aufzunehmen. |
| // + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen, |
| // + Verkauf von Luftbildaufnahmen, usw. |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht, |
| // + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts |
| // + auf anderen Webseiten oder Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
| // + eindeutig als Ursprung verlinkt und genannt werden |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion |
| // + Benutzung auf eigene Gefahr |
| // + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
| // + mit unserer Zustimmung zulässig |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
| // + this list of conditions and the following disclaimer. |
| // + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived |
| // + from this software without specific prior written permission. |
| // + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet |
| // + for non-commercial use (directly or indirectly) |
| // + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
| // + with our written permission |
| // + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
| // + clearly linked as origin |
| // + * porting to systems other than hardware from www.mikrokopter.de is not allowed |
| // + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
| // + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
| // + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
| // + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
| // + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
| // + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
| // + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
| // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
| // + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
| // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
| // + POSSIBILITY OF SUCH DAMAGE. |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| #include "main.h" |
| unsigned char EEPromArray[E2END+1] EEMEM; |
| unsigned char PlatinenVersion = 10; |
| unsigned char SendVersionToNavi = 1; |
| // -- Parametersatz aus EEPROM lesen --- |
| // number [0..5] |
| void ReadParameterSet(unsigned char number, unsigned char *buffer, unsigned char length) |
| { |
| if (number > 5) number = 5; |
| eeprom_read_block(buffer, &EEPromArray[EEPROM_ADR_PARAM_BEGIN + length * number], length); |
| LED_Init(); |
| } |
| // -- Parametersatz ins EEPROM schreiben --- |
| // number [0..5] |
| void WriteParameterSet(unsigned char number, unsigned char *buffer, unsigned char length) |
| { |
| if(number > 5) number = 5; |
| eeprom_write_block(buffer, &EEPromArray[EEPROM_ADR_PARAM_BEGIN + length * number], length); |
| eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], number); // diesen Parametersatz als aktuell merken |
| LED_Init(); |
| } |
| unsigned char GetActiveParamSetNumber(void) |
| { |
| unsigned char set; |
| set = eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET]); |
| if(set > 5) |
| { |
| set = 2; |
| eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], set); // diesen Parametersatz als aktuell merken |
| } |
| return(set); |
| } |
| void CalMk3Mag(void) |
| { |
| static unsigned char stick = 1; |
| if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > -20) stick = 0; |
| if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -70) && !stick) |
| { |
| stick = 1; |
| WinkelOut.CalcState++; |
| if(WinkelOut.CalcState > 4) |
| { |
| // WinkelOut.CalcState = 0; // in Uart.c |
| beeptime = 1000; |
| } |
| else Piep(WinkelOut.CalcState); |
| } |
| DebugOut.Analog[19] = WinkelOut.CalcState; |
| } |
| //############################################################################ |
| //Hauptprogramm |
| int main (void) |
| //############################################################################ |
| { |
| unsigned int timer; |
| //unsigned int timer2 = 0; |
| DDRB = 0x00; |
| PORTB = 0x00; |
| for(timer = 0; timer < 1000; timer++); // verzögern |
| if(PINB & 0x01) |
| { |
| if(PINB & 0x02) PlatinenVersion = 13; |
| else PlatinenVersion = 11; |
| } |
| else PlatinenVersion = 10; |
| DDRC = 0x81; // SCL |
| PORTC = 0xff; // Pullup SDA |
| DDRB = 0x1B; // LEDs und Druckoffset |
| PORTB = 0x01; // LED_Rot |
| DDRD = 0x3E; // Speaker & TXD & J3 J4 J5 |
| DDRD |=0x80; // J7 |
| PORTD = 0xF7; // LED |
| MCUSR &=~(1<<WDRF); |
| WDTCSR |= (1<<WDCE)|(1<<WDE); |
| WDTCSR = 0; |
| beeptime = 2000; |
| StickGier = 0; PPM_in[K_GAS] = 0;StickRoll = 0; StickNick = 0; |
| ROT_OFF; |
| Timer_Init(); |
| UART_Init(); |
| rc_sum_init(); |
| ADC_Init(); |
| i2c_init(); |
| SPI_MasterInit(); |
| sei(); |
| VersionInfo.Hauptversion = VERSION_HAUPTVERSION; |
| VersionInfo.Nebenversion = VERSION_NEBENVERSION; |
| VersionInfo.PCKompatibel = VERSION_KOMPATIBEL; |
| VersionInfo.Hardware = 1; // FlightCtrl |
| printf("\n\rFlightControl\n\rHardware:%d.%d\n\rSoftware:V%d.%d%c ",PlatinenVersion/10,PlatinenVersion%10, VERSION_HAUPTVERSION, VERSION_NEBENVERSION,VERSION_INDEX + 'a'); |
| printf("\n\r=============================="); |
| GRN_ON; |
| #define EE_DATENREVISION 71 // wird angepasst, wenn sich die EEPROM-Daten geändert haben |
| if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_VALID]) != EE_DATENREVISION) |
| { |
| printf("\n\rInit. EEPROM: Generiere Default-Parameter..."); |
| DefaultKonstanten1(); |
| for (unsigned char i=0;i<6;i++) |
| { |
| if(i==2) DefaultKonstanten2(); // Kamera |
| if(i==3) DefaultKonstanten3(); // Beginner |
| if(i>3) DefaultKonstanten2(); // Kamera |
| WriteParameterSet(i, (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE); |
| } |
| eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], 3); // default-Setting |
| eeprom_write_byte(&EEPromArray[EEPROM_ADR_VALID], EE_DATENREVISION); |
| } |
| if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) > 4) |
| { |
| printf("\n\rACC nicht abgeglichen!"); |
| } |
| ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE); |
| printf("\n\rBenutze Parametersatz %d", GetActiveParamSetNumber()); |
| if(EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG) |
| { |
| printf("\n\rAbgleich Luftdrucksensor.."); |
| timer = SetDelay(1000); |
| SucheLuftruckOffset(); |
| while (!CheckDelay(timer)); |
| printf("OK\n\r"); |
| } |
| SetNeutral(); |
| ROT_OFF; |
| beeptime = 2000; |
| ExternControl.Digital[0] = 0x55; |
| printf("\n\rSteuerung: "); |
| if (EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) printf("HeadingHold"); |
| else printf("Neutral"); |
| printf("\n\n\r"); |
| LcdClear(); |
| I2CTimeout = 5000; |
| WinkelOut.Orientation = 1; |
| while (1) |
| { |
| if(UpdateMotor) // ReglerIntervall |
| { |
| UpdateMotor=0; |
| //PORTD |= 0x08; |
| if(WinkelOut.CalcState) CalMk3Mag(); |
| else MotorRegler(); |
| //PORTD &= ~0x08; |
| SendMotorData(); |
| ROT_OFF; |
| if(PcZugriff) PcZugriff--; |
| else |
| { |
| DubWiseKeys[0] = 0; |
| DubWiseKeys[1] = 0; |
| ExternControl.Config = 0; |
| ExternStickNick = 0; |
| ExternStickRoll = 0; |
| ExternStickGier = 0; |
| } |
| if(SenderOkay) SenderOkay--; |
| if(!I2CTimeout) |
| { |
| I2CTimeout = 5; |
| i2c_reset(); |
| if((BeepMuster == 0xffff) && MotorenEin) |
| { |
| beeptime = 10000; |
| BeepMuster = 0x0080; |
| } |
| } |
| else |
| { |
| I2CTimeout--; |
| ROT_OFF; |
| } |
| if(SIO_DEBUG && (!UpdateMotor || !MotorenEin)) |
| { |
| DatenUebertragung(); |
| BearbeiteRxDaten(); |
| } |
| else BearbeiteRxDaten(); |
| if(CheckDelay(timer)) |
| { |
| if(UBat < EE_Parameter.UnterspannungsWarnung) |
| { |
| if(BeepMuster == 0xffff) |
| { |
| beeptime = 6000; |
| BeepMuster = 0x0300; |
| } |
| } |
| /* if(SendVersionToNavi) |
| { |
| SPI_StartTransmitPacket(SPI_CMD_VERSION);//# |
| SendVersionToNavi = 0; |
| } |
| else SPI_StartTransmitPacket(SPI_CMD_VALUE);//# |
| */ |
| SPI_StartTransmitPacket();//# |
| SendSPI = 4; |
| timer = SetDelay(20); |
| } |
| //if(UpdateMotor) DebugOut.Analog[26]++; |
| LED_Update(); |
| } |
| if(!SendSPI) { SPI_TransmitByte(); } |
| } |
| return (1); |
| } |
| /branches/V0.70d_flinkflash/src/main.d |
|---|
| 0,0 → 1,4 |
| main.o main.d : src/main.c include/main.h include/old_macros.h \ |
| include/_Settings.h include/printf_P.h include/timer0.h include/uart.h \ |
| include/analog.h include/twimaster.h include/menu.h include/rc.h \ |
| include/fc.h include/gps.h include/spi.h include/led.h |
| /branches/V0.70d_flinkflash/src/menu.c |
|---|
| 0,0 → 1,141 |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Copyright (c) 04.2007 Holger Buss |
| // + only for non-profit use |
| // + www.MikroKopter.com |
| // + see the File "License.txt" for further Informations |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| #include "main.h" |
| unsigned int TestInt = 0; |
| #define ARRAYGROESSE 10 |
| unsigned char Array[ARRAYGROESSE] = {1,2,3,4,5,6,7,8,9,10}; |
| char DisplayBuff[80] = "Hallo Welt"; |
| unsigned char DispPtr = 0; |
| unsigned char RemoteTasten = 0; |
| #define KEY1 0x01 |
| #define KEY2 0x02 |
| #define KEY3 0x04 |
| #define KEY4 0x08 |
| #define KEY5 0x10 |
| void LcdClear(void) |
| { |
| unsigned char i; |
| for(i=0;i<80;i++) DisplayBuff[i] = ' '; |
| } |
| void Menu(void) |
| { |
| static unsigned char MaxMenue = 11,MenuePunkt=0; |
| if(RemoteTasten & KEY1) { if(MenuePunkt) MenuePunkt--; else MenuePunkt = MaxMenue; LcdClear(); RemotePollDisplayLine = -1; } |
| if(RemoteTasten & KEY2) { MenuePunkt++; LcdClear(); RemotePollDisplayLine = -1;} |
| if((RemoteTasten & KEY1) && (RemoteTasten & KEY2)) MenuePunkt = 0; |
| if(MenuePunkt < 10) {LCD_printfxy(17,0,"[%i]",MenuePunkt);} else {LCD_printfxy(16,0,"[%i]",MenuePunkt);}; |
| switch(MenuePunkt) |
| { |
| case 0: |
| LCD_printfxy(0,0,"+ MikroKopter +"); |
| LCD_printfxy(0,1,"HW:V%d.%d SW:%d.%d%c",PlatinenVersion/10,PlatinenVersion%10,VERSION_HAUPTVERSION, VERSION_NEBENVERSION,VERSION_INDEX+'a'); |
| LCD_printfxy(0,2,"Setting: %d ",GetActiveParamSetNumber()); |
| LCD_printfxy(0,3,"(c) Holger Buss"); |
| // if(RemoteTasten & KEY3) TestInt--; |
| // if(RemoteTasten & KEY4) TestInt++; |
| break; |
| case 1: |
| if(EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG) |
| { |
| LCD_printfxy(0,0,"Hoehe: %5i",HoehenWert); |
| LCD_printfxy(0,1,"SollHoehe: %5i",SollHoehe); |
| LCD_printfxy(0,2,"Luftdruck: %5i",MessLuftdruck); |
| LCD_printfxy(0,3,"Off : %5i",DruckOffsetSetting); |
| } |
| else |
| { |
| LCD_printfxy(0,1,"Keine "); |
| LCD_printfxy(0,2,"Höhenregelung"); |
| } |
| break; |
| case 2: |
| LCD_printfxy(0,0,"akt. Lage"); |
| LCD_printfxy(0,1,"Nick: %5i",IntegralNick/1024); |
| LCD_printfxy(0,2,"Roll: %5i",IntegralRoll/1024); |
| LCD_printfxy(0,3,"Kompass: %5i",KompassValue); |
| break; |
| case 3: |
| LCD_printfxy(0,0,"K1:%4i K2:%4i ",PPM_in[1],PPM_in[2]); |
| LCD_printfxy(0,1,"K3:%4i K4:%4i ",PPM_in[3],PPM_in[4]); |
| LCD_printfxy(0,2,"K5:%4i K6:%4i ",PPM_in[5],PPM_in[6]); |
| LCD_printfxy(0,3,"K7:%4i K8:%4i ",PPM_in[7],PPM_in[8]); |
| break; |
| case 4: |
| LCD_printfxy(0,0,"Ni:%4i Ro:%4i ",PPM_in[EE_Parameter.Kanalbelegung[K_NICK]],PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]); |
| LCD_printfxy(0,1,"Gs:%4i Gi:%4i ",PPM_in[EE_Parameter.Kanalbelegung[K_GAS]],PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]); |
| LCD_printfxy(0,2,"P1:%4i P2:%4i ",PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]],PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]]); |
| LCD_printfxy(0,3,"P3:%4i P4:%4i ",PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]],PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]]); |
| break; |
| case 5: |
| LCD_printfxy(0,0,"Gyro - Sensor"); |
| if(PlatinenVersion == 10) |
| { |
| LCD_printfxy(0,1,"Nick %4i (%3i)",AdWertNick - AdNeutralNick, AdNeutralNick); |
| LCD_printfxy(0,2,"Roll %4i (%3i)",AdWertRoll - AdNeutralRoll, AdNeutralRoll); |
| LCD_printfxy(0,3,"Gier %4i (%3i)",AdNeutralGier - AdWertGier, AdNeutralGier); |
| } |
| else |
| if(PlatinenVersion == 11) |
| { |
| LCD_printfxy(0,1,"Nick %4i (%3i)",AdWertNick - AdNeutralNick, AdNeutralNick/2); |
| LCD_printfxy(0,2,"Roll %4i (%3i)",AdWertRoll - AdNeutralRoll, AdNeutralRoll/2); |
| LCD_printfxy(0,3,"Gier %4i (%3i)",AdNeutralGier - AdWertGier, AdNeutralGier/2); |
| } |
| else |
| { |
| LCD_printfxy(0,1,"Nick %4i (%3i)(%3i)",AdWertNick - AdNeutralNick, AdNeutralNick/2,AnalogOffsetNick); |
| LCD_printfxy(0,2,"Roll %4i (%3i)(%3i)",AdWertRoll - AdNeutralRoll, AdNeutralRoll/2,AnalogOffsetRoll); |
| LCD_printfxy(0,3,"Gier %4i (%3i)(%3i)",AdNeutralGier - AdWertGier, AdNeutralGier/2,AnalogOffsetGier); |
| } |
| break; |
| case 6: |
| LCD_printfxy(0,0,"ACC - Sensor"); |
| LCD_printfxy(0,1,"Nick %4i (%3i)",AdWertAccNick,NeutralAccX); |
| LCD_printfxy(0,2,"Roll %4i (%3i)",AdWertAccRoll,NeutralAccY); |
| LCD_printfxy(0,3,"Hoch %4i (%3i)",Mittelwert_AccHoch/*accumulate_AccHoch / messanzahl_AccHoch*/,(int)NeutralAccZ); |
| break; |
| case 7: |
| LCD_printfxy(0,1,"Spannung: %5i",UBat); |
| LCD_printfxy(0,2,"Empf.Pegel:%5i",SenderOkay); |
| break; |
| case 8: |
| LCD_printfxy(0,0,"Kompass "); |
| LCD_printfxy(0,1,"Richtung: %5i",KompassRichtung); |
| LCD_printfxy(0,2,"Messwert: %5i",KompassValue); |
| LCD_printfxy(0,3,"Start: %5i",KompassStartwert); |
| break; |
| case 9: |
| LCD_printfxy(0,0,"Poti1: %3i",Poti1); |
| LCD_printfxy(0,1,"Poti2: %3i",Poti2); |
| LCD_printfxy(0,2,"Poti3: %3i",Poti3); |
| LCD_printfxy(0,3,"Poti4: %3i",Poti4); |
| break; |
| case 10: |
| LCD_printfxy(0,0,"Servo " ); |
| LCD_printfxy(0,1,"Setpoint %3i",Parameter_ServoNickControl); |
| LCD_printfxy(0,2,"Stellung: %3i",ServoValue); |
| LCD_printfxy(0,3,"Range:%3i-%3i",EE_Parameter.ServoNickMin,EE_Parameter.ServoNickMax); |
| break; |
| case 11: |
| LCD_printfxy(0,0,"ExternControl " ); |
| LCD_printfxy(0,1,"Ni:%4i Ro:%4i ",ExternControl.Nick,ExternControl.Roll); |
| LCD_printfxy(0,2,"Gs:%4i Gi:%4i ",ExternControl.Gas,ExternControl.Gier); |
| LCD_printfxy(0,3,"Hi:%4i Cf:%4i ",ExternControl.Hight,ExternControl.Config); |
| break; |
| default: MaxMenue = MenuePunkt - 1; |
| MenuePunkt = 0; |
| break; |
| } |
| RemoteTasten = 0; |
| } |
| /branches/V0.70d_flinkflash/src/menu.d |
|---|
| 0,0 → 1,4 |
| menu.o menu.d : src/menu.c include/main.h include/old_macros.h \ |
| include/_Settings.h include/printf_P.h include/timer0.h include/uart.h \ |
| include/analog.h include/twimaster.h include/menu.h include/rc.h \ |
| include/fc.h include/gps.h include/spi.h include/led.h |
| /branches/V0.70d_flinkflash/src/printf_P.c |
|---|
| 0,0 → 1,480 |
| // Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist nicht von der Lizenz für den MikroKopter-Teil unterstellt |
| /* |
| Copyright (C) 1993 Free Software Foundation |
| This file is part of the GNU IO Library. This library is free |
| software; you can redistribute it and/or modify it under the |
| terms of the GNU General Public License as published by the |
| Free Software Foundation; either version 2, or (at your option) |
| any later version. |
| This library is distributed in the hope that it will be useful, |
| but WITHOUT ANY WARRANTY; without even the implied warranty of |
| MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| GNU General Public License for more details. |
| You should have received a copy of the GNU General Public License |
| along with this library; see the file COPYING. If not, write to the Free |
| Software Foundation, 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. |
| As a special exception, if you link this library with files |
| compiled with a GNU compiler to produce an executable, this does not cause |
| the resulting executable to be covered by the GNU General Public License. |
| This exception does not however invalidate any other reasons why |
| the executable file might be covered by the GNU General Public License. */ |
| /* |
| * Copyright (c) 1990 Regents of the University of California. |
| * All rights reserved. |
| * |
| * Redistribution and use in source and binary forms, with or without |
| * modification, are permitted provided that the following conditions |
| * are met: |
| * 1. Redistributions of source code must retain the above copyright |
| * notice, this list of conditions and the following disclaimer. |
| * 2. Redistributions in binary form must reproduce the above copyright |
| * notice, this list of conditions and the following disclaimer in the |
| * documentation and/or other materials provided with the distribution. |
| * 3. [rescinded 22 July 1999] |
| * 4. Neither the name of the University nor the names of its contributors |
| * may be used to endorse or promote products derived from this software |
| * without specific prior written permission. |
| * |
| * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND |
| * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
| * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
| * ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE |
| * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL |
| * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS |
| * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) |
| * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
| * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY |
| * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF |
| * SUCH DAMAGE. |
| */ |
| /****************************************************************************** |
| This file is a patched version of printf called _printf_P |
| It is made to work with avr-gcc for Atmel AVR MCUs. |
| There are some differences from standard printf: |
| 1. There is no floating point support (with fp the code is about 8K!) |
| 2. Return type is void |
| 3. Format string must be in program memory (by using macro printf this is |
| done automaticaly) |
| 4. %n is not implemented (just remove the comment around it if you need it) |
| 5. If LIGHTPRINTF is defined, the code is about 550 bytes smaller and the |
| folowing specifiers are disabled : |
| space # * . - + p s o O |
| 6. A function void uart_sendchar(char c) is used for output. The UART must |
| be initialized before using printf. |
| Alexander Popov |
| sasho@vip.orbitel.bg |
| ******************************************************************************/ |
| /* |
| * Actual printf innards. |
| * |
| * This code is large and complicated... |
| */ |
| #include <string.h> |
| #ifdef __STDC__ |
| #include <stdarg.h> |
| #else |
| #include <varargs.h> |
| #endif |
| #include "main.h" |
| //#define LIGHTPRINTF |
| char PrintZiel; |
| char Putchar(char zeichen) |
| { |
| if(PrintZiel == OUT_LCD) { DisplayBuff[DispPtr++] = zeichen; return(1);} |
| else return(uart_putchar(zeichen)); |
| } |
| void PRINT(const char * ptr, unsigned int len) |
| { |
| for(;len;len--) Putchar(*ptr++); |
| } |
| void PRINTP(const char * ptr, unsigned int len) |
| { |
| for(;len;len--) Putchar(pgm_read_byte(ptr++)); |
| } |
| void PAD_SP(signed char howmany) |
| { |
| for(;howmany>0;howmany--) Putchar(' '); |
| } |
| void PAD_0(signed char howmany) |
| { |
| for(;howmany>0;howmany--) Putchar('0'); |
| } |
| #define BUF 40 |
| /* |
| * Macros for converting digits to letters and vice versa |
| */ |
| #define to_digit(c) ((c) - '0') |
| #define is_digit(c) ((c)<='9' && (c)>='0') |
| #define to_char(n) ((n) + '0') |
| /* |
| * Flags used during conversion. |
| */ |
| #define LONGINT 0x01 /* long integer */ |
| #define LONGDBL 0x02 /* long double; unimplemented */ |
| #define SHORTINT 0x04 /* short integer */ |
| #define ALT 0x08 /* alternate form */ |
| #define LADJUST 0x10 /* left adjustment */ |
| #define ZEROPAD 0x20 /* zero (as opposed to blank) pad */ |
| #define HEXPREFIX 0x40 /* add 0x or 0X prefix */ |
| void _printf_P (char ziel,char const *fmt0, ...) /* Works with string from FLASH */ |
| { |
| va_list ap; |
| register const char *fmt; /* format string */ |
| register char ch; /* character from fmt */ |
| register int n; /* handy integer (short term usage) */ |
| register char *cp; /* handy char pointer (short term usage) */ |
| const char *fmark; /* for remembering a place in fmt */ |
| register unsigned char flags; /* flags as above */ |
| signed char width; /* width from format (%8d), or 0 */ |
| signed char prec; /* precision from format (%.3d), or -1 */ |
| char sign; /* sign prefix (' ', '+', '-', or \0) */ |
| unsigned long _ulong=0; /* integer arguments %[diouxX] */ |
| #define OCT 8 |
| #define DEC 10 |
| #define HEX 16 |
| unsigned char base; /* base for [diouxX] conversion */ |
| signed char dprec; /* a copy of prec if [diouxX], 0 otherwise */ |
| signed char dpad; /* extra 0 padding needed for integers */ |
| signed char fieldsz; /* field size expanded by sign, dpad etc */ |
| /* The initialization of 'size' is to suppress a warning that |
| 'size' might be used unitialized. It seems gcc can't |
| quite grok this spaghetti code ... */ |
| signed char size = 0; /* size of converted field or string */ |
| char buf[BUF]; /* space for %c, %[diouxX], %[eEfgG] */ |
| char ox[2]; /* space for 0x hex-prefix */ |
| PrintZiel = ziel; // bestimmt, LCD oder UART |
| va_start(ap, fmt0); |
| fmt = fmt0; |
| /* |
| * Scan the format for conversions (`%' character). |
| */ |
| for (;;) { |
| for (fmark = fmt; (ch = pgm_read_byte(fmt)) != '\0' && ch != '%'; fmt++) |
| /* void */; |
| if ((n = fmt - fmark) != 0) { |
| PRINTP(fmark, n); |
| } |
| if (ch == '\0') |
| goto done; |
| fmt++; /* skip over '%' */ |
| flags = 0; |
| dprec = 0; |
| width = 0; |
| prec = -1; |
| sign = '\0'; |
| rflag: ch = PRG_RDB(fmt++); |
| reswitch: |
| #ifdef LIGHTPRINTF |
| if (ch=='o' || ch=='u' || (ch|0x20)=='x') { |
| #else |
| if (ch=='u' || (ch|0x20)=='x') { |
| #endif |
| if (flags&LONGINT) { |
| _ulong=va_arg(ap, unsigned long); |
| } else { |
| register unsigned int _d; |
| _d=va_arg(ap, unsigned int); |
| _ulong = flags&SHORTINT ? (unsigned long)(unsigned short)_d : (unsigned long)_d; |
| } |
| } |
| #ifndef LIGHTPRINTF |
| if(ch==' ') { |
| /* |
| * ``If the space and + flags both appear, the space |
| * flag will be ignored.'' |
| * -- ANSI X3J11 |
| */ |
| if (!sign) |
| sign = ' '; |
| goto rflag; |
| } else if (ch=='#') { |
| flags |= ALT; |
| goto rflag; |
| } else if (ch=='*'||ch=='-') { |
| if (ch=='*') { |
| /* |
| * ``A negative field width argument is taken as a |
| * - flag followed by a positive field width.'' |
| * -- ANSI X3J11 |
| * They don't exclude field widths read from args. |
| */ |
| if ((width = va_arg(ap, int)) >= 0) |
| goto rflag; |
| width = -width; |
| } |
| flags |= LADJUST; |
| flags &= ~ZEROPAD; /* '-' disables '0' */ |
| goto rflag; |
| } else if (ch=='+') { |
| sign = '+'; |
| goto rflag; |
| } else if (ch=='.') { |
| if ((ch = PRG_RDB(fmt++)) == '*') { |
| n = va_arg(ap, int); |
| prec = n < 0 ? -1 : n; |
| goto rflag; |
| } |
| n = 0; |
| while (is_digit(ch)) { |
| n = n*10 + to_digit(ch); |
| ch = PRG_RDB(fmt++); |
| } |
| prec = n < 0 ? -1 : n; |
| goto reswitch; |
| } else |
| #endif /* LIGHTPRINTF */ |
| if (ch=='0') { |
| /* |
| * ``Note that 0 is taken as a flag, not as the |
| * beginning of a field width.'' |
| * -- ANSI X3J11 |
| */ |
| if (!(flags & LADJUST)) |
| flags |= ZEROPAD; /* '-' disables '0' */ |
| goto rflag; |
| } else if (ch>='1' && ch<='9') { |
| n = 0; |
| do { |
| n = 10 * n + to_digit(ch); |
| ch = PRG_RDB(fmt++); |
| } while (is_digit(ch)); |
| width = n; |
| goto reswitch; |
| } else if (ch=='h') { |
| flags |= SHORTINT; |
| goto rflag; |
| } else if (ch=='l') { |
| flags |= LONGINT; |
| goto rflag; |
| } else if (ch=='c') { |
| *(cp = buf) = va_arg(ap, int); |
| size = 1; |
| sign = '\0'; |
| } else if (ch=='D'||ch=='d'||ch=='i') { |
| if(ch=='D') |
| flags |= LONGINT; |
| if (flags&LONGINT) { |
| _ulong=va_arg(ap, long); |
| } else { |
| register int _d; |
| _d=va_arg(ap, int); |
| _ulong = flags&SHORTINT ? (long)(short)_d : (long)_d; |
| } |
| if ((long)_ulong < 0) { |
| _ulong = -_ulong; |
| sign = '-'; |
| } |
| base = DEC; |
| goto number; |
| } else |
| /* |
| if (ch=='n') { |
| if (flags & LONGINT) |
| *va_arg(ap, long *) = ret; |
| else if (flags & SHORTINT) |
| *va_arg(ap, short *) = ret; |
| else |
| *va_arg(ap, int *) = ret; |
| continue; // no output |
| } else |
| */ |
| #ifndef LIGHTPRINTF |
| if (ch=='O'||ch=='o') { |
| if (ch=='O') |
| flags |= LONGINT; |
| base = OCT; |
| goto nosign; |
| } else if (ch=='p') { |
| /* |
| * ``The argument shall be a pointer to void. The |
| * value of the pointer is converted to a sequence |
| * of printable characters, in an implementation- |
| * defined manner.'' |
| * -- ANSI X3J11 |
| */ |
| /* NOSTRICT */ |
| _ulong = (unsigned int)va_arg(ap, void *); |
| base = HEX; |
| flags |= HEXPREFIX; |
| ch = 'x'; |
| goto nosign; |
| } else if (ch=='s') { // print a string from RAM |
| if ((cp = va_arg(ap, char *)) == NULL) { |
| cp=buf; |
| cp[0] = '('; |
| cp[1] = 'n'; |
| cp[2] = 'u'; |
| cp[4] = cp[3] = 'l'; |
| cp[5] = ')'; |
| cp[6] = '\0'; |
| } |
| if (prec >= 0) { |
| /* |
| * can't use strlen; can only look for the |
| * NUL in the first `prec' characters, and |
| * strlen() will go further. |
| */ |
| char *p = (char*)memchr(cp, 0, prec); |
| if (p != NULL) { |
| size = p - cp; |
| if (size > prec) |
| size = prec; |
| } else |
| size = prec; |
| } else |
| size = strlen(cp); |
| sign = '\0'; |
| } else |
| #endif /* LIGHTPRINTF */ |
| if(ch=='U'||ch=='u') { |
| if (ch=='U') |
| flags |= LONGINT; |
| base = DEC; |
| goto nosign; |
| } else if (ch=='X'||ch=='x') { |
| base = HEX; |
| /* leading 0x/X only if non-zero */ |
| if (flags & ALT && _ulong != 0) |
| flags |= HEXPREFIX; |
| /* unsigned conversions */ |
| nosign: sign = '\0'; |
| /* |
| * ``... diouXx conversions ... if a precision is |
| * specified, the 0 flag will be ignored.'' |
| * -- ANSI X3J11 |
| */ |
| number: if ((dprec = prec) >= 0) |
| flags &= ~ZEROPAD; |
| /* |
| * ``The result of converting a zero value with an |
| * explicit precision of zero is no characters.'' |
| * -- ANSI X3J11 |
| */ |
| cp = buf + BUF; |
| if (_ulong != 0 || prec != 0) { |
| register unsigned char _d,notlastdigit; |
| do { |
| notlastdigit=(_ulong>=base); |
| _d = _ulong % base; |
| if (_d<10) { |
| _d+='0'; |
| } else { |
| _d+='a'-10; |
| if (ch=='X') _d&=~0x20; |
| } |
| *--cp=_d; |
| _ulong /= base; |
| } while (notlastdigit); |
| #ifndef LIGHTPRINTF |
| // handle octal leading 0 |
| if (base==OCT && flags & ALT && *cp != '0') |
| *--cp = '0'; |
| #endif |
| } |
| size = buf + BUF - cp; |
| } else { //default |
| /* "%?" prints ?, unless ? is NUL */ |
| if (ch == '\0') |
| goto done; |
| /* pretend it was %c with argument ch */ |
| cp = buf; |
| *cp = ch; |
| size = 1; |
| sign = '\0'; |
| } |
| /* |
| * All reasonable formats wind up here. At this point, |
| * `cp' points to a string which (if not flags&LADJUST) |
| * should be padded out to `width' places. If |
| * flags&ZEROPAD, it should first be prefixed by any |
| * sign or other prefix; otherwise, it should be blank |
| * padded before the prefix is emitted. After any |
| * left-hand padding and prefixing, emit zeroes |
| * required by a decimal [diouxX] precision, then print |
| * the string proper, then emit zeroes required by any |
| * leftover floating precision; finally, if LADJUST, |
| * pad with blanks. |
| */ |
| /* |
| * compute actual size, so we know how much to pad. |
| */ |
| fieldsz = size; |
| dpad = dprec - size; |
| if (dpad < 0) |
| dpad = 0; |
| if (sign) |
| fieldsz++; |
| else if (flags & HEXPREFIX) |
| fieldsz += 2; |
| fieldsz += dpad; |
| /* right-adjusting blank padding */ |
| if ((flags & (LADJUST|ZEROPAD)) == 0) |
| PAD_SP(width - fieldsz); |
| /* prefix */ |
| if (sign) { |
| PRINT(&sign, 1); |
| } else if (flags & HEXPREFIX) { |
| ox[0] = '0'; |
| ox[1] = ch; |
| PRINT(ox, 2); |
| } |
| /* right-adjusting zero padding */ |
| if ((flags & (LADJUST|ZEROPAD)) == ZEROPAD) |
| PAD_0(width - fieldsz); |
| /* leading zeroes from decimal precision */ |
| PAD_0(dpad); |
| /* the string or number proper */ |
| PRINT(cp, size); |
| /* left-adjusting padding (always blank) */ |
| if (flags & LADJUST) |
| PAD_SP(width - fieldsz); |
| } |
| done: |
| va_end(ap); |
| } |
| /branches/V0.70d_flinkflash/src/printf_P.d |
|---|
| 0,0 → 1,4 |
| printf_P.o printf_P.d : src/printf_P.c include/main.h include/old_macros.h \ |
| include/_Settings.h include/printf_P.h include/timer0.h include/uart.h \ |
| include/analog.h include/twimaster.h include/menu.h include/rc.h \ |
| include/fc.h include/gps.h include/spi.h include/led.h |
| /branches/V0.70d_flinkflash/src/rc.c |
|---|
| 0,0 → 1,85 |
| /*####################################################################################### |
| Decodieren eines RC Summen Signals |
| #######################################################################################*/ |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Copyright (c) 04.2007 Holger Buss |
| // + only for non-profit use |
| // + www.MikroKopter.com |
| // + see the File "License.txt" for further Informations |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| #include "rc.h" |
| #include "main.h" |
| volatile int PPM_in[11]; |
| volatile int PPM_diff[11]; // das diffenzierte Stick-Signal |
| volatile unsigned char NewPpmData = 1; |
| //############################################################################ |
| //zum decodieren des PPM-Signals wird Timer1 mit seiner Input |
| //Capture Funktion benutzt: |
| void rc_sum_init (void) |
| //############################################################################ |
| { |
| TCCR1B=(1<<CS11)|(1<<CS10)|(1<<ICES1)|(1<<ICNC1);//|(1 << WGM12); //timer1 prescale 64 |
| // PWM |
| //TCCR1A = (1 << COM1B1) | (1 << WGM11) | (1 << WGM10); |
| //TCCR1B |= (1 << WGM12); |
| //OCR1B = 55; |
| TIMSK1 |= _BV(ICIE1); |
| AdNeutralGier = 0; |
| AdNeutralRoll = 0; |
| AdNeutralNick = 0; |
| return; |
| } |
| //############################################################################ |
| //Diese Routine startet und inizialisiert den Timer für RC |
| SIGNAL(SIG_INPUT_CAPTURE1) |
| //############################################################################ |
| { |
| static unsigned int AltICR=0; |
| signed int signal = 0,tmp; |
| static int index; |
| signal = (unsigned int) ICR1 - AltICR; |
| AltICR = ICR1; |
| //Syncronisationspause? |
| // if((signal > (int) Parameter_UserParam2 * 10) && (signal < 8000)) |
| if((signal > 1100) && (signal < 8000)) |
| { |
| if(index >= 4) NewPpmData = 0; // Null bedeutet: Neue Daten |
| index = 1; |
| } |
| else |
| { |
| if(index < 10) |
| { |
| if((signal > 250) && (signal < 687)) |
| { |
| signal -= 466; |
| // Stabiles Signal |
| if(abs(signal - PPM_in[index]) < 6) { if(SenderOkay < 200) SenderOkay += 10;} |
| // tmp = (7 * (PPM_in[index]) + signal) / 8; |
| tmp = (3 * (PPM_in[index]) + signal) / 4; |
| if(tmp > signal+1) tmp--; else |
| if(tmp < signal-1) tmp++; |
| if(SenderOkay >= 195) PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; |
| else PPM_diff[index] = 0; |
| PPM_in[index] = tmp; |
| } |
| index++; |
| if(index == 5) PORTD |= 0x20; else PORTD &= ~0x20; // Servosignal an J3 anlegen |
| if(index == 6) PORTD |= 0x10; else PORTD &= ~0x10; // Servosignal an J4 anlegen |
| if(index == 7) PORTD |= 0x08; else PORTD &= ~0x08; // Servosignal an J5 anlegen |
| } |
| } |
| } |
| /branches/V0.70d_flinkflash/src/rc.d |
|---|
| 0,0 → 1,4 |
| rc.o rc.d : src/rc.c include/rc.h include/main.h include/old_macros.h \ |
| include/_Settings.h include/printf_P.h include/timer0.h include/uart.h \ |
| include/analog.h include/twimaster.h include/menu.h include/rc.h \ |
| include/fc.h include/gps.h include/spi.h include/led.h |
| /branches/V0.70d_flinkflash/src/spi.c |
|---|
| 0,0 → 1,268 |
| // ######################## SPI - FlightCtrl ################### |
| #include "main.h" |
| //struct str_ToNaviCtrl_Version ToNaviCtrl_Version; |
| //struct str_FromNaviCtrl_Version FromNaviCtrl_Version; |
| struct str_ToNaviCtrl ToNaviCtrl; |
| struct str_FromNaviCtrl FromNaviCtrl; |
| unsigned char SPI_BufferIndex; |
| unsigned char SPI_RxBufferIndex; |
| volatile unsigned char SPI_Buffer[sizeof(FromNaviCtrl)]; |
| unsigned char *SPI_TX_Buffer; |
| unsigned char SPITransferCompleted, SPI_ChkSum; |
| unsigned char SPI_RxDataValid; |
| unsigned char SPI_CommandSequence[] = { SPI_CMD_USER, SPI_CMD_STICK, SPI_CMD_PARAMETER1, SPI_CMD_STICK, SPI_CMD_CAL_COMPASS}; |
| unsigned char SPI_CommandCounter = 0; |
| #ifdef USE_SPI_COMMUNICATION |
| //------------------------------------------------------ |
| void SPI_MasterInit(void) |
| { |
| DDR_SPI |= (1<<DD_MOSI)|(1<<DD_SCK); // Set MOSI and SCK output, all others input |
| SLAVE_SELECT_DDR_PORT |= (1 << SPI_SLAVE_SELECT); |
| SPCR = (1<<SPE)|(1<<MSTR)|(1<<SPR1)|(0<<SPR0)|(0<<SPIE); // Enable SPI, Master, set clock rate fck/64 |
| SPSR = 0;//(1<<SPI2X); |
| SLAVE_SELECT_PORT |= (1 << SPI_SLAVE_SELECT); |
| SPITransferCompleted = 1; |
| //SPDR = 0x00; // dummy write |
| ToNaviCtrl.Sync1 = 0xAA; |
| ToNaviCtrl.Sync2 = 0x83; |
| ToNaviCtrl.Command = SPI_CMD_USER; |
| ToNaviCtrl.IntegralNick = 0; |
| ToNaviCtrl.IntegralRoll = 0; |
| SPI_RxDataValid = 0; |
| } |
| //------------------------------------------------------ |
| void SPI_StartTransmitPacket(void) |
| { |
| //if ((SLAVE_SELECT_PORT & (1 << SPI_SLAVE_SELECT)) == 0) return; // transfer of prev. packet not completed |
| if (!SPITransferCompleted) return; |
| // _delay_us(30); |
| SLAVE_SELECT_PORT &= ~(1 << SPI_SLAVE_SELECT); // SelectSlave |
| SPI_TX_Buffer = (unsigned char *) &ToNaviCtrl; |
| ToNaviCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++]; |
| if (SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0; |
| SPITransferCompleted = 0; |
| UpdateSPI_Buffer(); // update buffer |
| SPI_BufferIndex = 1; |
| //ebugOut.Analog[16]++; |
| // -- Debug-Output --- |
| //---- |
| asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); |
| asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); |
| asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); |
| ToNaviCtrl.Chksum = ToNaviCtrl.Sync1; |
| SPDR = ToNaviCtrl.Sync1; // Start transmission |
| // SLAVE_SELECT_PORT |= (1 << SPI_SLAVE_SELECT); // DeselectSlave |
| } |
| //------------------------------------------------------ |
| //SIGNAL(SIG_SPI) |
| void SPI_TransmitByte(void) |
| { |
| static unsigned char SPI_RXState = 0; |
| unsigned char rxdata; |
| static unsigned char rxchksum; |
| if (SPITransferCompleted) return; |
| if (!(SPSR & (1 << SPIF))) return; |
| SendSPI = 4; |
| // _delay_us(30); |
| SLAVE_SELECT_PORT |= (1 << SPI_SLAVE_SELECT); // DeselectSlave |
| rxdata = SPDR; |
| switch ( SPI_RXState) |
| { |
| case 0: |
| SPI_RxBufferIndex = 0; |
| //DebugOut.Analog[17]++; |
| rxchksum = rxdata; |
| if (rxdata == 0x81 ) { SPI_RXState = 1; } // 1. Syncbyte ok |
| break; |
| case 1: |
| if (rxdata == 0x55) { rxchksum += rxdata; SPI_RXState = 2; } // 2. Syncbyte ok |
| else SPI_RXState = 0; |
| //DebugOut.Analog[18]++; |
| break; |
| case 2: |
| SPI_Buffer[SPI_RxBufferIndex++]= rxdata; // get data |
| //DebugOut.Analog[19]++; |
| if (SPI_RxBufferIndex >= sizeof(FromNaviCtrl)) |
| { |
| if (rxdata == rxchksum) |
| { |
| unsigned char *ptr = (unsigned char *)&FromNaviCtrl; |
| memcpy(ptr, (unsigned char *) SPI_Buffer, sizeof(SPI_Buffer)); |
| SPI_RxDataValid = 1; |
| } |
| else SPI_RxDataValid = 0; |
| SPI_RXState = 0; |
| } |
| else rxchksum += rxdata; |
| break; |
| } |
| if (SPI_BufferIndex < sizeof(ToNaviCtrl)) |
| { |
| SLAVE_SELECT_PORT &= ~(1 << SPI_SLAVE_SELECT); // SelectSlave |
| asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); |
| asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); |
| asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); |
| SPDR = SPI_TX_Buffer[SPI_BufferIndex]; |
| ToNaviCtrl.Chksum += SPI_TX_Buffer[SPI_BufferIndex]; |
| // SLAVE_SELECT_PORT |= (1 << SPI_SLAVE_SELECT); // DeselectSlave |
| } |
| else SPITransferCompleted = 1; |
| SPI_BufferIndex++; |
| } |
| //------------------------------------------------------ |
| void UpdateSPI_Buffer(void) |
| { |
| static unsigned char i =0; |
| signed int tmp; |
| cli(); |
| ToNaviCtrl.IntegralNick = (int) (IntegralNick / 108); |
| ToNaviCtrl.IntegralRoll = (int) (IntegralRoll / 108); |
| ToNaviCtrl.GyroCompass = ErsatzKompass / GIER_GRAD_FAKTOR; |
| ToNaviCtrl.AccNick = (int) ACC_AMPLIFY * (NaviAccNick / NaviCntAcc); |
| ToNaviCtrl.AccRoll = (int) ACC_AMPLIFY * (NaviAccRoll / NaviCntAcc); |
| NaviCntAcc = 0; NaviAccNick = 0; NaviAccRoll = 0; |
| // ToNaviCtrl.User8 = Parameter_UserParam8; |
| // ToNaviCtrl.CalState = WinkelOut.CalcState; |
| switch(ToNaviCtrl.Command) // |
| { |
| case SPI_CMD_USER: |
| ToNaviCtrl.Param.Byte[0] = Parameter_UserParam1; |
| ToNaviCtrl.Param.Byte[1] = Parameter_UserParam2; |
| ToNaviCtrl.Param.Byte[2] = Parameter_UserParam3; |
| ToNaviCtrl.Param.Byte[3] = Parameter_UserParam4; |
| ToNaviCtrl.Param.Byte[4] = Parameter_UserParam5; |
| ToNaviCtrl.Param.Byte[5] = Parameter_UserParam6; |
| ToNaviCtrl.Param.Byte[6] = Parameter_UserParam7; |
| ToNaviCtrl.Param.Byte[7] = Parameter_UserParam8; |
| ToNaviCtrl.Param.Byte[8] = (unsigned char) MikroKopterFlags; |
| MikroKopterFlags &= ~(FLAG_CALIBRATE | FLAG_START); |
| ToNaviCtrl.Param.Byte[9] = (unsigned char) UBat; |
| ToNaviCtrl.Param.Byte[10] =(unsigned char) EE_Parameter.UnterspannungsWarnung; |
| ToNaviCtrl.Param.Byte[11] =(unsigned char) eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET]); |
| break; |
| case SPI_CMD_PARAMETER1: |
| ToNaviCtrl.Param.Byte[0] = Parameter_NaviGpsModeControl; // Parameters for the Naviboard |
| ToNaviCtrl.Param.Byte[1] = Parameter_NaviGpsGain; |
| ToNaviCtrl.Param.Byte[2] = Parameter_NaviGpsP; |
| ToNaviCtrl.Param.Byte[3] = Parameter_NaviGpsI; |
| ToNaviCtrl.Param.Byte[4] = Parameter_NaviGpsD; |
| ToNaviCtrl.Param.Byte[5] = Parameter_NaviGpsACC; |
| ToNaviCtrl.Param.Byte[6] = EE_Parameter.NaviGpsMinSat; |
| ToNaviCtrl.Param.Byte[7] = EE_Parameter.NaviStickThreshold; |
| ToNaviCtrl.Param.Byte[8] = 15; // MaxRadius |
| break; |
| case SPI_CMD_STICK: |
| tmp = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]]; if(tmp > 127) tmp = 127; else if(tmp < -127) tmp = -127; |
| ToNaviCtrl.Param.Byte[0] = (char) tmp; |
| tmp = PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]; if(tmp > 127) tmp = 127; else if(tmp < -127) tmp = -127; |
| ToNaviCtrl.Param.Byte[1] = (char) tmp; |
| tmp = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]; if(tmp > 127) tmp = 127; else if(tmp < -127) tmp = -127; |
| ToNaviCtrl.Param.Byte[2] = (char) tmp; |
| tmp = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]; if(tmp > 127) tmp = 127; else if(tmp < -127) tmp = -127; |
| ToNaviCtrl.Param.Byte[3] = (char) tmp; |
| ToNaviCtrl.Param.Byte[4] = (unsigned char) Poti1; |
| ToNaviCtrl.Param.Byte[5] = (unsigned char) Poti2; |
| ToNaviCtrl.Param.Byte[6] = (unsigned char) Poti3; |
| ToNaviCtrl.Param.Byte[7] = (unsigned char) Poti4; |
| ToNaviCtrl.Param.Byte[8] = (unsigned char) SenderOkay; |
| break; |
| case SPI_CMD_CAL_COMPASS: |
| if(WinkelOut.CalcState > 5) |
| { |
| WinkelOut.CalcState = 0; |
| ToNaviCtrl.Param.Byte[0] = 5; |
| } |
| else ToNaviCtrl.Param.Byte[0] = WinkelOut.CalcState; |
| break; |
| } |
| sei(); |
| if (SPI_RxDataValid) |
| { |
| if(abs(FromNaviCtrl.GPS_Nick) < 512 && abs(FromNaviCtrl.GPS_Roll) < 512 && (EE_Parameter.GlobalConfig & CFG_GPS_AKTIV)) |
| { |
| GPS_Nick = FromNaviCtrl.GPS_Nick; |
| GPS_Roll = FromNaviCtrl.GPS_Roll; |
| } |
| if(FromNaviCtrl.CompassValue <= 360) KompassValue = FromNaviCtrl.CompassValue; |
| KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180; |
| if(FromNaviCtrl.BeepTime > beeptime && !WinkelOut.CalcState) beeptime = FromNaviCtrl.BeepTime; |
| switch (FromNaviCtrl.Command) |
| { |
| case SPI_CMD_OSD_DATA: |
| // ToFlightCtrl.Param.Byte[0] = OsdBar; |
| // ToFlightCtrl.Param.Int[1] = Distance; |
| break; |
| case SPI_CMD_GPS_POS: |
| // ToFlightCtrl.Param.Long[0] = GPS_Data.Longitude; |
| // ToFlightCtrl.Param.Long[1] = GPS_Data.Latitude; |
| break; |
| case SPI_CMD_GPS_TARGET: |
| // ToFlightCtrl.Param.Long[0] = GPS_Data.TargetLongitude; |
| // ToFlightCtrl.Param.Long[1] = GPS_Data.TargetLatitude; |
| break; |
| default: |
| break; |
| } |
| } |
| else |
| { |
| // KompassValue = 0; |
| // KompassRichtung = 0; |
| GPS_Nick = 0; |
| GPS_Roll = 0; |
| } |
| } |
| #endif |
| /branches/V0.70d_flinkflash/src/spi.d |
|---|
| 0,0 → 1,4 |
| spi.o spi.d : src/spi.c include/main.h include/old_macros.h include/_Settings.h \ |
| include/printf_P.h include/timer0.h include/uart.h include/analog.h \ |
| include/twimaster.h include/menu.h include/rc.h include/fc.h \ |
| include/gps.h include/spi.h include/led.h |
| /branches/V0.70d_flinkflash/src/timer0.c |
|---|
| 0,0 → 1,198 |
| #include "main.h" |
| volatile unsigned int CountMilliseconds = 0; |
| volatile static unsigned int tim_main; |
| volatile unsigned char UpdateMotor = 0; |
| volatile unsigned int cntKompass = 0; |
| volatile unsigned int beeptime = 0; |
| volatile unsigned char SendSPI = 0; |
| volatile unsigned int ServoState = 40; |
| unsigned int BeepMuster = 0xffff; |
| unsigned int ServoValue = 0; |
| enum { |
| STOP = 0, |
| CK = 1, |
| CK8 = 2, |
| CK64 = 3, |
| CK256 = 4, |
| CK1024 = 5, |
| T0_FALLING_EDGE = 6, |
| T0_RISING_EDGE = 7 |
| }; |
| SIGNAL (SIG_OVERFLOW0) // 8kHz |
| { |
| static unsigned char cnt_1ms = 1,cnt = 0; |
| unsigned char pieper_ein = 0; |
| // TCNT0 -= 250;//TIMER_RELOAD_VALUE; |
| if(SendSPI) SendSPI--; |
| if(!cnt--) |
| { |
| cnt = 9; |
| cnt_1ms++; |
| cnt_1ms %= 2; |
| if(!cnt_1ms) UpdateMotor = 1; |
| CountMilliseconds++; |
| } |
| if(beeptime > 1) |
| { |
| beeptime--; |
| if(beeptime & BeepMuster) |
| { |
| pieper_ein = 1; |
| } |
| else pieper_ein = 0; |
| } |
| else |
| { |
| pieper_ein = 0; |
| BeepMuster = 0xffff; |
| } |
| if(pieper_ein) |
| { |
| if(PlatinenVersion == 10) PORTD |= (1<<2); // Speaker an PORTD.2 |
| else PORTC |= (1<<7); // Speaker an PORTC.7 |
| } |
| else |
| { |
| if(PlatinenVersion == 10) PORTD &= ~(1<<2); |
| else PORTC &= ~(1<<7); |
| } |
| if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) |
| { |
| if(PINC & 0x10) |
| { |
| cntKompass++; |
| } |
| else |
| { |
| if((cntKompass) && (cntKompass < 362)) |
| { |
| cntKompass += cntKompass / 41; |
| if(cntKompass > 10) KompassValue = cntKompass - 10; else KompassValue = 0; |
| } |
| // if(cntKompass < 10) cntKompass = 10; |
| // KompassValue = (unsigned long)((unsigned long)(cntKompass-10)*720L + 1L) / 703L; |
| KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180; |
| cntKompass = 0; |
| } |
| } |
| } |
| //---------------------------- |
| void Timer_Init(void) |
| { |
| tim_main = SetDelay(10); |
| TCCR0B = CK8; |
| TCCR0A = (1<<COM0A1)|(1<<COM0B1)|3;//fast PWM |
| OCR0A = 0; |
| OCR0B = 120; |
| TCNT0 = (unsigned char)-TIMER_RELOAD_VALUE; // reload |
| //OCR1 = 0x00; |
| TCCR2A=(1<<COM2A1)|(1<<COM2A0)|3; |
| // TCCR2B=(0<<CS20)|(1<<CS21)|(1<<CS22); // clk/256 |
| TCCR2B=(0<<CS20)|(0<<CS21)|(1<<CS22); // clk/64 |
| TIMSK2 |= _BV(OCIE2A); |
| TIMSK0 |= _BV(TOIE0); |
| OCR2A = 10; |
| TCNT2 = 0; |
| } |
| // ----------------------------------------------------------------------- |
| unsigned int SetDelay (unsigned int t) |
| { |
| // TIMSK0 &= ~_BV(TOIE0); |
| return(CountMilliseconds + t + 1); |
| // TIMSK0 |= _BV(TOIE0); |
| } |
| // ----------------------------------------------------------------------- |
| char CheckDelay(unsigned int t) |
| { |
| // TIMSK0 &= ~_BV(TOIE0); |
| return(((t - CountMilliseconds) & 0x8000) >> 9); |
| // TIMSK0 |= _BV(TOIE0); |
| } |
| // ----------------------------------------------------------------------- |
| void Delay_ms(unsigned int w) |
| { |
| unsigned int akt; |
| akt = SetDelay(w); |
| while (!CheckDelay(akt)); |
| } |
| void Delay_ms_Mess(unsigned int w) |
| { |
| unsigned int akt; |
| akt = SetDelay(w); |
| while (!CheckDelay(akt)) ANALOG_ON; |
| } |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // Servo ansteuern |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| SIGNAL(SIG_OVERFLOW2) |
| { |
| if (ServoState > 0) PORTD |= 0x80; |
| else PORTD &= ~0x80; |
| TCCR2A =3; |
| TIMSK2 &= ~_BV(TOIE2); |
| } |
| SIGNAL(SIG_OUTPUT_COMPARE2A) |
| { |
| static unsigned char postPulse = 0x80; |
| static int filterServo = 100; |
| #define MULTIPLIER 4 |
| if(ServoState == 4) |
| { |
| ServoValue = 0x0030; // Offset Part1 |
| filterServo = (filterServo * 3 + (int) Parameter_ServoNickControl * 2)/4; |
| ServoValue += filterServo; |
| if(EE_Parameter.ServoNickCompInvert & 0x01) ServoValue += ((long) ((long)EE_Parameter.ServoNickComp * IntegralNick) / 128L )/ (512L/MULTIPLIER); |
| else ServoValue -= ((long) ((long)EE_Parameter.ServoNickComp * IntegralNick) / 128L) / (512L/MULTIPLIER); |
| if((ServoValue) < ((int)EE_Parameter.ServoNickMin*3)) ServoValue = (int)EE_Parameter.ServoNickMin*3; |
| else if((ServoValue) > ((int)EE_Parameter.ServoNickMax*3)) ServoValue = (int)EE_Parameter.ServoNickMax*3; |
| DebugOut.Analog[20] = ServoValue; |
| if ((ServoValue % 255) < 45) { ServoValue+= 77; postPulse = 0x60 - 77; } else postPulse = 0x60; |
| OCR2A = 255-(ServoValue % 256); |
| TCCR2A=(1<<COM2A1)|(1<<COM2A0)|3; |
| } |
| else if ((ServoState > 0) && (ServoState < 4)) |
| { |
| if(ServoValue > 255) |
| { PORTD |= 0x80; |
| TCCR2A =3; |
| ServoValue -= 255; |
| } |
| else |
| { |
| TCCR2A=(1<<COM2A1)|(0<<COM2A0)|3; |
| OCR2A = postPulse; // Offset Part2 |
| ServoState = 1; |
| } |
| } |
| else if (ServoState == 0) |
| { |
| ServoState = (int) EE_Parameter.ServoNickRefresh * MULTIPLIER; |
| PORTD&=~0x80; |
| TCCR2A = 3; |
| } |
| ServoState--; |
| } |
| /branches/V0.70d_flinkflash/src/timer0.d |
|---|
| 0,0 → 1,4 |
| timer0.o timer0.d : src/timer0.c include/main.h include/old_macros.h \ |
| include/_Settings.h include/printf_P.h include/timer0.h include/uart.h \ |
| include/analog.h include/twimaster.h include/menu.h include/rc.h \ |
| include/fc.h include/gps.h include/spi.h include/led.h |
| /branches/V0.70d_flinkflash/src/twimaster.c |
|---|
| 0,0 → 1,204 |
| /*############################################################################ |
| ############################################################################*/ |
| #include "main.h" |
| volatile unsigned char twi_state = 0; |
| unsigned char motor = 0; |
| unsigned char motorread = 0; |
| unsigned char motor_rx[8]; |
| //############################################################################ |
| //Initzialisieren der I2C (TWI) Schnittstelle |
| void i2c_init(void) |
| //############################################################################ |
| { |
| TWSR = 0; |
| TWBR = ((SYSCLK/SCL_CLOCK)-16)/2; |
| } |
| //############################################################################ |
| //Start I2C |
| char i2c_start(void) |
| //############################################################################ |
| { |
| TWCR = (1<<TWSTA) | (1<<TWEN) | (1<<TWINT) | (1<<TWIE); |
| return(0); |
| } |
| //############################################################################ |
| //Start I2C |
| void i2c_stop(void) |
| //############################################################################ |
| { |
| TWCR = (1<<TWEN) | (1<<TWSTO) | (1<<TWINT); |
| } |
| void i2c_reset(void) |
| //############################################################################ |
| { |
| i2c_stop(); |
| twi_state = 0; |
| motor = TWDR; |
| motor = 0; |
| TWCR = 0x80; |
| TWAMR = 0; |
| TWAR = 0; |
| TWDR = 0; |
| TWSR = 0; |
| TWBR = 0; |
| i2c_init(); |
| i2c_start(); |
| i2c_write_byte(0); |
| } |
| //############################################################################ |
| //Start I2C |
| char i2c_write_byte(char byte) |
| //############################################################################ |
| { |
| TWSR = 0x00; |
| TWDR = byte; |
| TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE); |
| return(0); |
| } |
| //############################################################################ |
| //Start I2C |
| SIGNAL (TWI_vect) |
| //############################################################################ |
| { |
| switch (twi_state++) |
| { |
| case 0: |
| i2c_write_byte(0x52+(motor*2)); |
| break; |
| case 1: |
| switch(motor++) |
| { |
| case 0: |
| i2c_write_byte(Motor_Vorne); |
| break; |
| case 1: |
| i2c_write_byte(Motor_Hinten); |
| break; |
| case 2: |
| i2c_write_byte(Motor_Rechts); |
| break; |
| case 3: |
| i2c_write_byte(Motor_Links); |
| break; |
| } |
| break; |
| case 2: |
| i2c_stop(); |
| if (motor<4) twi_state = 0; |
| else motor = 0; |
| i2c_start(); |
| break; |
| //Liest Daten von Motor |
| case 3: |
| i2c_write_byte(0x53+(motorread*2)); |
| break; |
| case 4: |
| switch(motorread) |
| { |
| case 0: |
| i2c_write_byte(Motor_Vorne); |
| break; |
| case 1: |
| i2c_write_byte(Motor_Hinten); |
| break; |
| case 2: |
| i2c_write_byte(Motor_Rechts); |
| break; |
| case 3: |
| i2c_write_byte(Motor_Links); |
| break; |
| } |
| break; |
| case 5: //1 Byte vom Motor lesen |
| motor_rx[motorread] = TWDR; |
| case 6: |
| switch(motorread) |
| { |
| case 0: |
| i2c_write_byte(Motor_Vorne); |
| break; |
| case 1: |
| i2c_write_byte(Motor_Hinten); |
| break; |
| case 2: |
| i2c_write_byte(Motor_Rechts); |
| break; |
| case 3: |
| i2c_write_byte(Motor_Links); |
| break; |
| } |
| break; |
| case 7: //2 Byte vom Motor lesen |
| motor_rx[motorread+4] = TWDR; |
| motorread++; |
| if (motorread>3) motorread=0; |
| i2c_stop(); |
| I2CTimeout = 10; |
| twi_state = 0; |
| break; |
| case 8: // Gyro-Offset |
| i2c_write_byte(0x98); // Address of the DAC |
| break; |
| case 9: |
| i2c_write_byte(0x10); // Update Channel A |
| break; |
| case 10: |
| i2c_write_byte(AnalogOffsetNick); // Value |
| break; |
| case 11: |
| i2c_write_byte(0x80); // Value |
| break; |
| case 12: |
| i2c_stop(); |
| I2CTimeout = 10; |
| i2c_start(); |
| break; |
| case 13: |
| i2c_write_byte(0x98); // Address of the DAC |
| break; |
| case 14: |
| i2c_write_byte(0x12); // Update Channel B |
| break; |
| case 15: |
| i2c_write_byte(AnalogOffsetRoll); // Value |
| break; |
| case 16: |
| i2c_write_byte(0x80); // Value |
| break; |
| case 17: |
| i2c_stop(); |
| I2CTimeout = 10; |
| i2c_start(); |
| break; |
| case 18: |
| i2c_write_byte(0x98); // Address of the DAC |
| break; |
| case 19: |
| i2c_write_byte(0x14); // Update Channel C |
| break; |
| case 20: |
| i2c_write_byte(AnalogOffsetGier); // Value |
| break; |
| case 21: |
| i2c_write_byte(0x80); // Value |
| break; |
| case 22: |
| i2c_stop(); |
| I2CTimeout = 10; |
| twi_state = 0; |
| break; |
| } |
| TWCR |= 0x80; |
| } |
| /branches/V0.70d_flinkflash/src/twimaster.d |
|---|
| 0,0 → 1,4 |
| twimaster.o twimaster.d : src/twimaster.c include/main.h include/old_macros.h \ |
| include/_Settings.h include/printf_P.h include/timer0.h include/uart.h \ |
| include/analog.h include/twimaster.h include/menu.h include/rc.h \ |
| include/fc.h include/gps.h include/spi.h include/led.h |
| /branches/V0.70d_flinkflash/src/uart.c |
|---|
| 0,0 → 1,423 |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Copyright (c) 04.2007 Holger Buss |
| // + only for non-profit use |
| // + www.MikroKopter.com |
| // + see the File "License.txt" for further Informations |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| #include "main.h" |
| #include "uart.h" |
| unsigned char DebugGetAnforderung = 0,DebugDisplayAnforderung = 0,DebugDataAnforderung = 0,GetVersionAnforderung = 0; |
| unsigned volatile char SioTmp = 0; |
| unsigned volatile char SendeBuffer[MAX_SENDE_BUFF]; |
| unsigned volatile char RxdBuffer[MAX_EMPFANGS_BUFF]; |
| unsigned volatile char NMEABuffer[MAX_EMPFANGS_BUFF]; |
| unsigned volatile char NeuerDatensatzEmpfangen = 0; |
| unsigned volatile char NeueKoordinateEmpfangen = 0; |
| unsigned volatile char UebertragungAbgeschlossen = 1; |
| unsigned volatile char CntCrcError = 0; |
| unsigned volatile char AnzahlEmpfangsBytes = 0; |
| unsigned volatile char PC_DebugTimeout = 0; |
| unsigned char RemotePollDisplayLine = 0; |
| unsigned char NurKanalAnforderung = 0; |
| unsigned char DebugTextAnforderung = 255; |
| unsigned char PcZugriff = 100; |
| unsigned char MotorTest[4] = {0,0,0,0}; |
| unsigned char DubWiseKeys[4] = {0,0,0,0}; |
| unsigned char MeineSlaveAdresse; |
| unsigned char ConfirmFrame; |
| struct str_DebugOut DebugOut; |
| struct str_ExternControl ExternControl; |
| struct str_VersionInfo VersionInfo; |
| struct str_WinkelOut WinkelOut; |
| int Debug_Timer,Kompass_Timer; |
| const unsigned char ANALOG_TEXT[32][16] = |
| { |
| //1234567890123456 |
| "IntegralNick ", //0 |
| "IntegralRoll ", |
| "AccNick ", |
| "AccRoll ", |
| "GyroGier ", |
| "HoehenWert ", //5 |
| "AccZ ", |
| "Gas ", |
| "KompassValue ", |
| "Spannung ", |
| "Empfang ", //10 |
| "Ersatzkompass ", |
| "Motor_Vorne ", |
| "Motor_Hinten ", |
| "Motor_Links ", |
| "Motor_Rechts ", //15 |
| " ", |
| "Distance ", |
| "OsdBar ", |
| "MK3Mag CalState ", |
| "Servo ", //20 |
| "Nick ", |
| "Roll ", |
| " ", |
| " ", |
| " ", //25 |
| " ", |
| " ", |
| " ", |
| " ", |
| "GPS_Nick ", //30 |
| "GPS_Roll " |
| }; |
| //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| //++ Sende-Part der Datenübertragung |
| //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| SIGNAL(INT_VEC_TX) |
| { |
| static unsigned int ptr = 0; |
| unsigned char tmp_tx; |
| if(!UebertragungAbgeschlossen) |
| { |
| ptr++; // die [0] wurde schon gesendet |
| tmp_tx = SendeBuffer[ptr]; |
| if((tmp_tx == '\r') || (ptr == MAX_SENDE_BUFF)) |
| { |
| ptr = 0; |
| UebertragungAbgeschlossen = 1; |
| } |
| UDR = tmp_tx; |
| } |
| else ptr = 0; |
| } |
| //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| //++ Empfangs-Part der Datenübertragung, incl. CRC-Auswertung |
| //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| SIGNAL(INT_VEC_RX) |
| { |
| static unsigned int crc; |
| static unsigned char crc1,crc2,buf_ptr; |
| static unsigned char UartState = 0; |
| unsigned char CrcOkay = 0; |
| SioTmp = UDR; |
| if(buf_ptr >= MAX_EMPFANGS_BUFF) UartState = 0; |
| if(SioTmp == '\r' && UartState == 2) |
| { |
| UartState = 0; |
| crc -= RxdBuffer[buf_ptr-2]; |
| crc -= RxdBuffer[buf_ptr-1]; |
| crc %= 4096; |
| crc1 = '=' + crc / 64; |
| crc2 = '=' + crc % 64; |
| CrcOkay = 0; |
| if((crc1 == RxdBuffer[buf_ptr-2]) && (crc2 == RxdBuffer[buf_ptr-1])) CrcOkay = 1; else { CrcOkay = 0; CntCrcError++;}; |
| if(!NeuerDatensatzEmpfangen && CrcOkay) // Datensatz schon verarbeitet |
| { |
| NeuerDatensatzEmpfangen = 1; |
| AnzahlEmpfangsBytes = buf_ptr; |
| RxdBuffer[buf_ptr] = '\r'; |
| if(RxdBuffer[2] == 'R') wdt_enable(WDTO_250MS); // Reset-Commando |
| } |
| } |
| else |
| switch(UartState) |
| { |
| case 0: |
| if(SioTmp == '#' && !NeuerDatensatzEmpfangen) UartState = 1; // Startzeichen und Daten schon verarbeitet |
| buf_ptr = 0; |
| RxdBuffer[buf_ptr++] = SioTmp; |
| crc = SioTmp; |
| break; |
| case 1: // Adresse auswerten |
| UartState++; |
| RxdBuffer[buf_ptr++] = SioTmp; |
| crc += SioTmp; |
| break; |
| case 2: // Eingangsdaten sammeln |
| RxdBuffer[buf_ptr] = SioTmp; |
| if(buf_ptr < MAX_EMPFANGS_BUFF) buf_ptr++; |
| else UartState = 0; |
| crc += SioTmp; |
| break; |
| default: |
| UartState = 0; |
| break; |
| } |
| } |
| // -------------------------------------------------------------------------- |
| void AddCRC(unsigned int wieviele) |
| { |
| unsigned int tmpCRC = 0,i; |
| for(i = 0; i < wieviele;i++) |
| { |
| tmpCRC += SendeBuffer[i]; |
| } |
| tmpCRC %= 4096; |
| SendeBuffer[i++] = '=' + tmpCRC / 64; |
| SendeBuffer[i++] = '=' + tmpCRC % 64; |
| SendeBuffer[i++] = '\r'; |
| UebertragungAbgeschlossen = 0; |
| UDR = SendeBuffer[0]; |
| } |
| // -------------------------------------------------------------------------- |
| void SendOutData(unsigned char cmd,unsigned char modul, unsigned char *snd, unsigned char len) |
| { |
| unsigned int pt = 0; |
| unsigned char a,b,c; |
| unsigned char ptr = 0; |
| SendeBuffer[pt++] = '#'; // Startzeichen |
| SendeBuffer[pt++] = modul; // Adresse (a=0; b=1,...) |
| SendeBuffer[pt++] = cmd; // Commando |
| while(len) |
| { |
| if(len) { a = snd[ptr++]; len--;} else a = 0; |
| if(len) { b = snd[ptr++]; len--;} else b = 0; |
| if(len) { c = snd[ptr++]; len--;} else c = 0; |
| SendeBuffer[pt++] = '=' + (a >> 2); |
| SendeBuffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4)); |
| SendeBuffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6)); |
| SendeBuffer[pt++] = '=' + ( c & 0x3f); |
| } |
| AddCRC(pt); |
| } |
| // -------------------------------------------------------------------------- |
| void Decode64(unsigned char *ptrOut, unsigned char len, unsigned char ptrIn,unsigned char max) // Wohin mit den Daten; Wie lang; Wo im RxdBuffer |
| { |
| unsigned char a,b,c,d; |
| unsigned char ptr = 0; |
| unsigned char x,y,z; |
| while(len) |
| { |
| a = RxdBuffer[ptrIn++] - '='; |
| b = RxdBuffer[ptrIn++] - '='; |
| c = RxdBuffer[ptrIn++] - '='; |
| d = RxdBuffer[ptrIn++] - '='; |
| if(ptrIn > max - 2) break; // nicht mehr Daten verarbeiten, als empfangen wurden |
| x = (a << 2) | (b >> 4); |
| y = ((b & 0x0f) << 4) | (c >> 2); |
| z = ((c & 0x03) << 6) | d; |
| if(len--) ptrOut[ptr++] = x; else break; |
| if(len--) ptrOut[ptr++] = y; else break; |
| if(len--) ptrOut[ptr++] = z; else break; |
| } |
| } |
| // -------------------------------------------------------------------------- |
| void BearbeiteRxDaten(void) |
| { |
| if(!NeuerDatensatzEmpfangen) return; |
| unsigned int tmp_int_arr1[1]; |
| // unsigned int tmp_int_arr2[2]; |
| // unsigned int tmp_int_arr3[3]; |
| unsigned char tmp_char_arr2[2]; |
| // unsigned char tmp_char_arr3[3]; |
| // unsigned char tmp_char_arr4[4]; |
| //if(!MotorenEin) |
| switch(RxdBuffer[2]) |
| { |
| case 'K':// Kompasswert |
| Decode64((unsigned char *) &tmp_int_arr1[0],sizeof(tmp_int_arr1),3,AnzahlEmpfangsBytes); |
| KompassValue = tmp_int_arr1[0]; |
| KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180; |
| break; |
| case 'a':// Texte der Analogwerte |
| Decode64((unsigned char *) &tmp_char_arr2[0],sizeof(tmp_char_arr2),3,AnzahlEmpfangsBytes); |
| DebugTextAnforderung = tmp_char_arr2[0]; |
| PcZugriff = 255; |
| break; |
| case 'b': |
| Decode64((unsigned char *) &ExternControl,sizeof(ExternControl),3,AnzahlEmpfangsBytes); |
| RemoteTasten |= ExternControl.RemoteTasten; |
| ConfirmFrame = ExternControl.Frame; |
| PcZugriff = 255; |
| break; |
| case 'c': |
| Decode64((unsigned char *) &ExternControl,sizeof(ExternControl),3,AnzahlEmpfangsBytes); |
| RemoteTasten |= ExternControl.RemoteTasten; |
| ConfirmFrame = ExternControl.Frame; |
| DebugDataAnforderung = 1; |
| PcZugriff = 255; |
| break; |
| case 'h':// x-1 Displayzeilen |
| Decode64((unsigned char *) &tmp_char_arr2[0],sizeof(tmp_char_arr2),3,AnzahlEmpfangsBytes); |
| RemoteTasten |= tmp_char_arr2[0]; |
| if(tmp_char_arr2[1] == 255) NurKanalAnforderung = 1; else NurKanalAnforderung = 0; // keine Displaydaten |
| DebugDisplayAnforderung = 1; |
| break; |
| case 't':// Motortest |
| Decode64((unsigned char *) &MotorTest[0],sizeof(MotorTest),3,AnzahlEmpfangsBytes); |
| PcZugriff = 255; |
| break; |
| case 'k':// Keys von DubWise |
| Decode64((unsigned char *) &DubWiseKeys[0],sizeof(DubWiseKeys),3,AnzahlEmpfangsBytes); |
| ConfirmFrame = DubWiseKeys[3]; |
| PcZugriff = 255; |
| break; |
| case 'v': // Version-Anforderung und Ausbaustufe |
| GetVersionAnforderung = 1; |
| break; |
| case 'g':// "Get"-Anforderung für Debug-Daten |
| // Bei Get werden die vom PC einstellbaren Werte vom PC zurückgelesen |
| DebugGetAnforderung = 1; |
| break; |
| case 'q':// "Get"-Anforderung für Settings |
| // Bei Get werden die vom PC einstellbaren Werte vom PC zurückgelesen |
| Decode64((unsigned char *) &tmp_char_arr2[0],sizeof(tmp_char_arr2),3,AnzahlEmpfangsBytes); |
| while(!UebertragungAbgeschlossen); |
| if(tmp_char_arr2[0] != 0xff) |
| { |
| if(tmp_char_arr2[0] > 5) tmp_char_arr2[0] = 5; |
| ReadParameterSet(tmp_char_arr2[0], (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE); |
| SendOutData('L' + tmp_char_arr2[0] -1,MeineSlaveAdresse,(unsigned char *) &EE_Parameter.Kanalbelegung[0],STRUCT_PARAM_LAENGE); |
| } |
| else |
| SendOutData('L' + GetActiveParamSetNumber()-1,MeineSlaveAdresse,(unsigned char *) &EE_Parameter.Kanalbelegung[0],STRUCT_PARAM_LAENGE); |
| break; |
| case 'l': |
| case 'm': |
| case 'n': |
| case 'o': |
| case 'p': // Parametersatz speichern |
| Decode64((unsigned char *) &EE_Parameter.Kanalbelegung[0],STRUCT_PARAM_LAENGE,3,AnzahlEmpfangsBytes); |
| WriteParameterSet(RxdBuffer[2] - 'l' + 1, (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE); |
| eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], RxdBuffer[2] - 'l' + 1); // aktiven Datensatz merken |
| Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L; |
| Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L; |
| Piep(GetActiveParamSetNumber()); |
| break; |
| } |
| // DebugOut.AnzahlZyklen = Debug_Timer_Intervall; |
| NeuerDatensatzEmpfangen = 0; |
| } |
| //############################################################################ |
| //Routine für die Serielle Ausgabe |
| int uart_putchar (char c) |
| //############################################################################ |
| { |
| if (c == '\n') |
| uart_putchar('\r'); |
| //Warten solange bis Zeichen gesendet wurde |
| loop_until_bit_is_set(USR, UDRE); |
| //Ausgabe des Zeichens |
| UDR = c; |
| return (0); |
| } |
| // -------------------------------------------------------------------------- |
| void WriteProgramData(unsigned int pos, unsigned char wert) |
| { |
| //if (ProgramLocation == IN_RAM) Buffer[pos] = wert; |
| // else eeprom_write_byte(&EE_Buffer[pos], wert); |
| // Buffer[pos] = wert; |
| } |
| //############################################################################ |
| //INstallation der Seriellen Schnittstelle |
| void UART_Init (void) |
| //############################################################################ |
| { |
| //Enable TXEN im Register UCR TX-Data Enable & RX Enable |
| UCR=(1 << TXEN) | (1 << RXEN); |
| // UART Double Speed (U2X) |
| USR |= (1<<U2X); |
| // RX-Interrupt Freigabe |
| UCSRB |= (1<<RXCIE); |
| // TX-Interrupt Freigabe |
| UCSRB |= (1<<TXCIE); |
| //Teiler wird gesetzt |
| UBRR=(SYSCLK / (BAUD_RATE * 8L) - 1); |
| //UBRR = 33; |
| //öffnet einen Kanal für printf (STDOUT) |
| //fdevopen (uart_putchar, 0); |
| //sbi(PORTD,4); |
| Debug_Timer = SetDelay(200); |
| Kompass_Timer = SetDelay(220); |
| } |
| //--------------------------------------------------------------------------------------------- |
| void DatenUebertragung(void) |
| { |
| if(!UebertragungAbgeschlossen) return; |
| if(DebugGetAnforderung && UebertragungAbgeschlossen) // Bei Get werden die vom PC einstellbaren Werte vom PC zurückgelesen |
| { |
| SendOutData('G',MeineSlaveAdresse,(unsigned char *) &ExternControl,sizeof(ExternControl)); |
| DebugGetAnforderung = 0; |
| } |
| if((CheckDelay(Kompass_Timer)) && UebertragungAbgeschlossen) |
| { |
| WinkelOut.Winkel[0] = (int) (IntegralNick / 108); // etwa in 0,1 Grad |
| WinkelOut.Winkel[1] = (int) (IntegralRoll / 108); // etwa in 0,1 Grad |
| WinkelOut.UserParameter[0] = Parameter_UserParam1; |
| WinkelOut.UserParameter[1] = Parameter_UserParam2; |
| SendOutData('w',MeineSlaveAdresse,(unsigned char *) &WinkelOut,sizeof(WinkelOut)); |
| if(WinkelOut.CalcState > 4) WinkelOut.CalcState = 6; // wird dann in SPI auf Null gesetzt |
| Kompass_Timer = SetDelay(99); |
| } |
| if((CheckDelay(Debug_Timer) || DebugDataAnforderung) && UebertragungAbgeschlossen) |
| { |
| SendOutData('D',MeineSlaveAdresse,(unsigned char *) &DebugOut,sizeof(DebugOut)); |
| DebugDataAnforderung = 0; |
| Debug_Timer = SetDelay(MIN_DEBUG_INTERVALL); |
| } |
| if(DebugTextAnforderung != 255) // Texte für die Analogdaten |
| { |
| SendOutData('A',DebugTextAnforderung + '0',(unsigned char *) ANALOG_TEXT[DebugTextAnforderung],16); |
| DebugTextAnforderung = 255; |
| } |
| if(ConfirmFrame && UebertragungAbgeschlossen) // Datensatz ohne CRC bestätigen |
| { |
| SendeBuffer[0] = '#'; |
| SendeBuffer[1] = ConfirmFrame; |
| SendeBuffer[2] = '\r'; |
| UebertragungAbgeschlossen = 0; |
| ConfirmFrame = 0; |
| UDR = SendeBuffer[0]; |
| } |
| if(DebugDisplayAnforderung && UebertragungAbgeschlossen) |
| { |
| Menu(); |
| DebugDisplayAnforderung = 0; |
| if(++RemotePollDisplayLine == 4 || NurKanalAnforderung) |
| { |
| SendOutData('4',0,(unsigned char *)&PPM_in,sizeof(PPM_in)); // DisplayZeile übertragen |
| RemotePollDisplayLine = -1; |
| } |
| else SendOutData('0' + RemotePollDisplayLine,0,(unsigned char *)&DisplayBuff[20 * RemotePollDisplayLine],20); // DisplayZeile übertragen |
| } |
| if(GetVersionAnforderung && UebertragungAbgeschlossen) |
| { |
| SendOutData('V',MeineSlaveAdresse,(unsigned char *) &VersionInfo,sizeof(VersionInfo)); |
| GetVersionAnforderung = 0; |
| } |
| } |
| /branches/V0.70d_flinkflash/src/uart.d |
|---|
| 0,0 → 1,4 |
| uart.o uart.d : src/uart.c include/main.h include/old_macros.h \ |
| include/_Settings.h include/printf_P.h include/timer0.h include/uart.h \ |
| include/analog.h include/twimaster.h include/menu.h include/rc.h \ |
| include/fc.h include/gps.h include/spi.h include/led.h include/uart.h |