| /branches/V0.76g_dk9nw_balancekopter/794 MK FC V0.76g balance/led.c |
|---|
| File deleted |
| /branches/V0.76g_dk9nw_balancekopter/794 MK FC V0.76g balance/eeprom.h |
|---|
| File deleted |
| /branches/V0.76g_dk9nw_balancekopter/794 MK FC V0.76g balance/eeprom.c |
|---|
| File deleted |
| /branches/V0.76g_dk9nw_balancekopter/794 MK FC V0.76g balance/uart.h |
|---|
| File deleted |
| /branches/V0.76g_dk9nw_balancekopter/794 MK FC V0.76g balance/old_macros.h |
|---|
| File deleted |
| /branches/V0.76g_dk9nw_balancekopter/794 MK FC V0.76g balance/led.h |
|---|
| File deleted |
| /branches/V0.76g_dk9nw_balancekopter/794 MK FC V0.76g balance/twimaster.c |
|---|
| File deleted |
| /branches/V0.76g_dk9nw_balancekopter/794 MK FC V0.76g balance/gps.h |
|---|
| File deleted |
| /branches/V0.76g_dk9nw_balancekopter/794 MK FC V0.76g balance/rc.c |
|---|
| File deleted |
| /branches/V0.76g_dk9nw_balancekopter/794 MK FC V0.76g balance/rc.h |
|---|
| File deleted |
| /branches/V0.76g_dk9nw_balancekopter/794 MK FC V0.76g balance/isqrt.h |
|---|
| File deleted |
| /branches/V0.76g_dk9nw_balancekopter/794 MK FC V0.76g balance/twimaster.h |
|---|
| File deleted |
| /branches/V0.76g_dk9nw_balancekopter/794 MK FC V0.76g balance/mymath.h |
|---|
| File deleted |
| /branches/V0.76g_dk9nw_balancekopter/794 MK FC V0.76g balance/mymath.c |
|---|
| File deleted |
| /branches/V0.76g_dk9nw_balancekopter/794 MK FC V0.76g balance/main.c |
|---|
| File deleted |
| /branches/V0.76g_dk9nw_balancekopter/794 MK FC V0.76g balance/timer0.c |
|---|
| File deleted |
| /branches/V0.76g_dk9nw_balancekopter/794 MK FC V0.76g balance/timer0.h |
|---|
| File deleted |
| /branches/V0.76g_dk9nw_balancekopter/794 MK FC V0.76g balance/menu.c |
|---|
| File deleted |
| /branches/V0.76g_dk9nw_balancekopter/794 MK FC V0.76g balance/isqrt.S |
|---|
| File deleted |
| /branches/V0.76g_dk9nw_balancekopter/794 MK FC V0.76g balance/main.h |
|---|
| File deleted |
| /branches/V0.76g_dk9nw_balancekopter/794 MK FC V0.76g balance/printf_P.c |
|---|
| File deleted |
| /branches/V0.76g_dk9nw_balancekopter/794 MK FC V0.76g balance/menu.h |
|---|
| File deleted |
| /branches/V0.76g_dk9nw_balancekopter/794 MK FC V0.76g balance/fc.h |
|---|
| File deleted |
| /branches/V0.76g_dk9nw_balancekopter/794 MK FC V0.76g balance/fc.c |
|---|
| File deleted |
| /branches/V0.76g_dk9nw_balancekopter/794 MK FC V0.76g balance/printf_P.h |
|---|
| File deleted |
| /branches/V0.76g_dk9nw_balancekopter/794 MK FC V0.76g balance/uart.c |
|---|
| File deleted |
| /branches/V0.76g_dk9nw_balancekopter/eeprom.c |
|---|
| 0,0 → 1,412 |
| /***************************************************************************************************************************** |
| * File: eeprom.c |
| * |
| * Purpose: storing or loading the EEPROM values to RAM or writing them to EE_Parameter |
| * |
| * Functions: void DefaultStickMapping(void) // stick setting at RC control for incoming sum signal chain |
| * void DefaultKonstanten3(void) // |
| * void DefaultKonstanten2(void) // |
| * void DefaultKonstanten1(void) // |
| * void ReadParameterSet(unsigned char number, unsigned char *buffer, unsigned char length) |
| * void WriteParameterSet(unsigned char number, unsigned char *buffer, unsigned char length) |
| * unsigned char GetActiveParamSetNumber(void) |
| * void SetActiveParamSetNumber(unsigned char number) |
| * |
| *****************************************************************************************************************************/ |
| #include "eeprom.h" |
| unsigned char EEPromArray[E2END+1] EEMEM; // E2END = 2047 |
| // wird im EEPROM deklariert |
| // in den I/O-Headern wird die letzte EEPROM-Adresse als E2END definiert |
| // #define EEMEM __attribute__ ((section (".eeprom"))) |
| // EEPROM Size = 2 Kbytes // Page Size = 8 bytes // No. of Pages = 256 // siehe Atmega644 manual S287 |
| // --------------------------------------------------------------------------------------------------------------------------- |
| // + Konstanten |
| // + 0-250 -> normale Werte |
| // + 251 -> Poti1 |
| // + 252 -> Poti2 |
| // + 253 -> Poti3 |
| // --------------------------------------------------------------------------------------------------------------------------- |
| //----------------------------------------------------------------------------------------------------------------------------- |
| // stick setting at RC control for incoming sum signal chain |
| //----------------------------------------------------------------------------------------------------------------------------- |
| void DefaultStickMapping(void) // remote control is set to mode 2 |
| { |
| EE_Parameter.Kanalbelegung[K_NICK] = 3; // #define K_NICK 0 // Koptertool -> Einstellungen -> Kanäle -> "NICK" |
| EE_Parameter.Kanalbelegung[K_ROLL] = 2; // #define K_ROLL 1 // Koptertool -> Einstellungen -> Kanäle -> "ROLL" |
| EE_Parameter.Kanalbelegung[K_GAS] = 1; // #define K_GAS 2 // Koptertool -> Einstellungen -> Kanäle -> "GAS" |
| EE_Parameter.Kanalbelegung[K_GIER] = 4; // #define K_GIER 3 // Koptertool -> Einstellungen -> Kanäle -> "Gier" |
| EE_Parameter.Kanalbelegung[K_POTI1] = 5; // #define K_POTI1 4 // Koptertool -> Einstellungen -> Kanäle -> "POTI1" |
| EE_Parameter.Kanalbelegung[K_POTI2] = 6; // #define K_POTI2 5 // Koptertool -> Einstellungen -> Kanäle -> "POTI2" |
| EE_Parameter.Kanalbelegung[K_POTI3] = 7; // #define K_POTI3 6 // Koptertool -> Einstellungen -> Kanäle -> "POTI3" |
| EE_Parameter.Kanalbelegung[K_POTI4] = 8; // #define K_POTI4 7 // Koptertool -> Einstellungen -> Kanäle -> "POTI4" |
| } |
| //----------------------------------------------------------------------------------------------------------------------------- |
| //----------------------------------------------------------------------------------------------------------------------------- |
| void DefaultKonstanten3(void) |
| { |
| EE_Parameter.GlobalConfig = CFG_KOMPASS_AKTIV; |
| // |
| // #define CFG_HOEHENREGELUNG 0x01 |
| // #define CFG_HOEHEN_SCHALTER 0x02 |
| // #define CFG_HEADING_HOLD 0x04 |
| // #define CFG_KOMPASS_AKTIV 0x08 |
| // #define CFG_KOMPASS_FIX 0x10 |
| // #define CFG_GPS_AKTIV 0x20 |
| // #define CFG_ACHSENKOPPLUNG_AKTIV 0x40 |
| // #define CFG_DREHRATEN_BEGRENZER 0x80 |
| EE_Parameter.ExtraConfig = CFG2_HEIGHT_LIMIT; |
| // |
| // #define CFG2_HEIGHT_LIMIT 0x01 // Koptertool -> Einstellungen -> Höhe -> Höhenregelung aktiv -> "Höhenbegrenzung" |
| // #define CFG2_VARIO_BEEP 0x02 // Koptertool -> Einstellungen -> Höhe -> Höhenregelung aktiv -> "akustisches Variometer" |
| // #define CFG_SENSITIVE_RC 0x04 // Koptertool -> Einstellungen -> Konfiguration -> "Erweiterte Empfangssignalprüfung" |
| // #define CFG_3_3V_REFERENCE 0x08 // Koptertool -> Einstellungen -> Verschiedenes -> "Voltage Reference" 3.0V |
| // #define CFG_NO_RCOFF_BEEPING 0x10 // Koptertool -> Einstellungen -> Verschiedenes -> "kein Signalton ohne aktiven Empfänger" |
| // #define CFG_GPS_AID 0x20 // Koptertool -> ??? |
| // #define CFG_LEARNABLE_CAREFREE 0x40 // Koptertool -> Einstellungen -> Easy Setup -> "Teachable Carefree" |
| // #define CFG_IGNORE_MAG_ERR_AT_STARTUP 0x80 |
| 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_HoverBand = 5; // Wert : 0-250 |
| EE_Parameter.Hoehe_GPS_Z = 64; // Wert : 0-250 |
| EE_Parameter.Hoehe_StickNeutralPoint = 0; // Wert : 0-250 (0 = Hoover-Estimation) |
| EE_Parameter.Hoehe_Verstaerkung = 15; // 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.Gyro_D = 3; // Wert : 0-250 |
| EE_Parameter.Gyro_Gier_P = 100; // Wert : 0-250 |
| EE_Parameter.Gyro_Gier_I = 120; // Wert : 0-250 |
| EE_Parameter.UnterspannungsWarnung = 33; // Wert : 0-250 // Automatische Zellenerkennung bei < 50 |
| 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.ServoCompInvert = 1; // Wert : 0-250 // Richtung Einfluss Gyro/Servo |
| EE_Parameter.ServoNickMin = 0; // Wert : 0-250 // Anschlag |
| EE_Parameter.ServoNickMax = 250; // Wert : 0-250 // Anschlag |
| EE_Parameter.ServoNickRefresh = 6; |
| EE_Parameter.ServoRollControl = 100; // Wert : 0-250 // Stellung des Servos |
| EE_Parameter.ServoRollComp = 40; // Wert : 0-250 // Einfluss Gyro/Servo |
| EE_Parameter.ServoRollMin = 0; // Wert : 0-250 // Anschlag |
| EE_Parameter.ServoRollMax = 250; // Wert : 0-250 // Anschlag |
| EE_Parameter.LoopGasLimit = 50; |
| EE_Parameter.LoopThreshold = 90; // Wert: 0-250 Schwelle für Stickausschlag |
| EE_Parameter.LoopHysterese = 50; |
| EE_Parameter.BitConfig = 0; // Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts |
| EE_Parameter.AchsKopplung1 = 90; |
| EE_Parameter.AchsKopplung2 = 80; |
| EE_Parameter.CouplingYawCorrection = 70; |
| EE_Parameter.WinkelUmschlagNick = 85; |
| EE_Parameter.WinkelUmschlagRoll = 85; |
| EE_Parameter.GyroAccAbgleich = 32; // 1/k |
| EE_Parameter.Driftkomp = 32; |
| EE_Parameter.DynamicStability = 50; |
| EE_Parameter.J16Bitmask = 95; |
| EE_Parameter.J17Bitmask = 243; |
| EE_Parameter.WARN_J16_Bitmask = 0xAA; |
| EE_Parameter.WARN_J17_Bitmask = 0xAA; |
| EE_Parameter.J16Timing = 30; |
| EE_Parameter.J17Timing = 30; |
| EE_Parameter.NaviGpsModeControl = 252; |
| EE_Parameter.NaviGpsGain = 100; |
| EE_Parameter.NaviGpsP = 90; |
| EE_Parameter.NaviGpsI = 90; |
| EE_Parameter.NaviGpsD = 90; |
| EE_Parameter.NaviGpsPLimit = 75; |
| EE_Parameter.NaviGpsILimit = 75; |
| EE_Parameter.NaviGpsDLimit = 75; |
| EE_Parameter.NaviGpsACC = 0; |
| EE_Parameter.NaviGpsMinSat = 6; |
| EE_Parameter.NaviStickThreshold = 8; |
| EE_Parameter.NaviWindCorrection = 90; |
| EE_Parameter.NaviSpeedCompensation = 30; |
| EE_Parameter.NaviOperatingRadius = 100; |
| EE_Parameter.NaviAngleLimitation = 100; |
| EE_Parameter.NaviPH_LoginTime = 4; |
| memcpy(EE_Parameter.Name, "Beginner\0", 12); |
| } |
| //----------------------------------------------------------------------------------------------------------------------------- |
| //----------------------------------------------------------------------------------------------------------------------------- |
| void DefaultKonstanten2(void) |
| { |
| EE_Parameter.GlobalConfig = CFG_ACHSENKOPPLUNG_AKTIV | CFG_KOMPASS_AKTIV | CFG_GPS_AKTIV | CFG_HOEHEN_SCHALTER; |
| EE_Parameter.ExtraConfig = CFG2_HEIGHT_LIMIT;// | CFG2_VARIO_BEEP | CFG_SENSITIVE_RC |
| 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_HoverBand = 5; // Wert : 0-250 |
| EE_Parameter.Hoehe_GPS_Z = 64; // Wert : 0-250 |
| EE_Parameter.Hoehe_StickNeutralPoint = 0; // Wert : 0-250 (0 = Hoover-Estimation) |
| EE_Parameter.Hoehe_Verstaerkung = 15; // Wert : 0-50 |
| EE_Parameter.Stick_P = 10; // 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 = 90; // Wert : 0-250 |
| EE_Parameter.Gyro_I = 120; // Wert : 0-250 |
| EE_Parameter.Gyro_D = 3; // Wert : 0-250 |
| EE_Parameter.Gyro_Gier_P = 90; // Wert : 0-250 |
| EE_Parameter.Gyro_Gier_I = 120; // Wert : 0-250 |
| EE_Parameter.UnterspannungsWarnung = 33; // Wert : 0-250 ( Automatische Zellenerkennung bei < 50) |
| 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.ServoCompInvert = 1; // Wert : 0-250 // Richtung Einfluss Gyro/Servo |
| EE_Parameter.ServoNickMin = 0; // Wert : 0-250 // Anschlag |
| EE_Parameter.ServoNickMax = 250; // Wert : 0-250 // Anschlag |
| EE_Parameter.ServoNickRefresh = 6; |
| EE_Parameter.ServoRollControl = 100; // Wert : 0-250 // Stellung des Servos |
| EE_Parameter.ServoRollComp = 40; // Wert : 0-250 // Einfluss Gyro/Servo |
| EE_Parameter.ServoRollMin = 0; // Wert : 0-250 // Anschlag |
| EE_Parameter.ServoRollMax = 250; // Wert : 0-250 // Anschlag |
| EE_Parameter.LoopGasLimit = 50; |
| EE_Parameter.LoopThreshold = 90; // Wert: 0-250 Schwelle für Stickausschlag |
| EE_Parameter.LoopHysterese = 50; |
| EE_Parameter.BitConfig = 0; // Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts |
| EE_Parameter.AchsKopplung1 = 90; |
| EE_Parameter.AchsKopplung2 = 80; |
| EE_Parameter.CouplingYawCorrection = 60; |
| EE_Parameter.WinkelUmschlagNick = 85; |
| EE_Parameter.WinkelUmschlagRoll = 85; |
| EE_Parameter.GyroAccAbgleich = 32; // 1/k |
| EE_Parameter.Driftkomp = 32; |
| EE_Parameter.DynamicStability = 75; |
| EE_Parameter.J16Bitmask = 95; |
| EE_Parameter.J17Bitmask = 243; |
| EE_Parameter.WARN_J16_Bitmask = 0xAA; |
| EE_Parameter.WARN_J17_Bitmask = 0xAA; |
| EE_Parameter.J16Timing = 20; |
| EE_Parameter.J17Timing = 20; |
| EE_Parameter.NaviGpsModeControl = 252; |
| EE_Parameter.NaviGpsGain = 100; |
| EE_Parameter.NaviGpsP = 90; |
| EE_Parameter.NaviGpsI = 90; |
| EE_Parameter.NaviGpsD = 90; |
| EE_Parameter.NaviGpsPLimit = 75; |
| EE_Parameter.NaviGpsILimit = 75; |
| EE_Parameter.NaviGpsDLimit = 75; |
| EE_Parameter.NaviGpsACC = 0; |
| EE_Parameter.NaviGpsMinSat = 6; |
| EE_Parameter.NaviStickThreshold = 8; |
| EE_Parameter.NaviWindCorrection = 90; |
| EE_Parameter.NaviSpeedCompensation = 30; |
| EE_Parameter.NaviOperatingRadius = 100; |
| EE_Parameter.NaviAngleLimitation = 100; |
| EE_Parameter.NaviPH_LoginTime = 4; |
| memcpy(EE_Parameter.Name, "Normal\0", 12); |
| } |
| //----------------------------------------------------------------------------------------------------------------------------- |
| //----------------------------------------------------------------------------------------------------------------------------- |
| void DefaultKonstanten1(void) |
| { |
| EE_Parameter.GlobalConfig = CFG_ACHSENKOPPLUNG_AKTIV | CFG_KOMPASS_AKTIV | CFG_GPS_AKTIV | CFG_HOEHEN_SCHALTER; |
| EE_Parameter.ExtraConfig = CFG2_HEIGHT_LIMIT;// | CFG2_VARIO_BEEP | CFG_SENSITIVE_RC |
| 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_HoverBand = 5; // Wert : 0-250 |
| EE_Parameter.Hoehe_GPS_Z = 64; // Wert : 0-250 |
| EE_Parameter.Hoehe_StickNeutralPoint = 0; // Wert : 0-250 (0 = Hoover-Estimation) |
| EE_Parameter.Hoehe_Verstaerkung = 20; // Wert : 0-50 |
| EE_Parameter.Stick_P = 14; // Wert : 1-6 |
| EE_Parameter.Stick_D = 16; // 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.Gyro_D = 3; // Wert : 0-250 |
| EE_Parameter.Gyro_Gier_P = 80; // Wert : 0-250 |
| EE_Parameter.Gyro_Gier_I = 150; // Wert : 0-250 |
| EE_Parameter.UnterspannungsWarnung = 33; // Wert : 0-250 ( Automatische Zellenerkennung bei < 50) |
| 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.ServoCompInvert = 1; // Wert : 0-250 // Richtung Einfluss Gyro/Servo |
| EE_Parameter.ServoNickMin = 0; // Wert : 0-250 // Anschlag |
| EE_Parameter.ServoNickMax = 250; // Wert : 0-250 // Anschlag |
| EE_Parameter.ServoNickRefresh = 6; |
| EE_Parameter.ServoRollControl = 100; // Wert : 0-250 // Stellung des Servos |
| EE_Parameter.ServoRollComp = 40; // Wert : 0-250 // Einfluss Gyro/Servo |
| EE_Parameter.ServoRollMin = 0; // Wert : 0-250 // Anschlag |
| EE_Parameter.ServoRollMax = 250; // Wert : 0-250 // Anschlag |
| EE_Parameter.LoopGasLimit = 50; |
| EE_Parameter.LoopThreshold = 90; // Wert: 0-250 Schwelle für Stickausschlag |
| EE_Parameter.LoopHysterese = 50; |
| EE_Parameter.BitConfig = 0; // Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts / wird getrennt behandelt |
| EE_Parameter.AchsKopplung1 = 90; |
| EE_Parameter.AchsKopplung2 = 80; |
| EE_Parameter.CouplingYawCorrection = 1; |
| EE_Parameter.WinkelUmschlagNick = 85; |
| EE_Parameter.WinkelUmschlagRoll = 85; |
| EE_Parameter.GyroAccAbgleich = 16; // 1/k |
| EE_Parameter.Driftkomp = 32; |
| EE_Parameter.DynamicStability = 100; |
| EE_Parameter.J16Bitmask = 95; |
| EE_Parameter.J17Bitmask = 243; |
| EE_Parameter.WARN_J16_Bitmask = 0xAA; |
| EE_Parameter.WARN_J17_Bitmask = 0xAA; |
| EE_Parameter.J16Timing = 15; |
| EE_Parameter.J17Timing = 15; |
| EE_Parameter.NaviGpsModeControl = 252; |
| EE_Parameter.NaviGpsGain = 100; |
| EE_Parameter.NaviGpsP = 90; |
| EE_Parameter.NaviGpsI = 90; |
| EE_Parameter.NaviGpsD = 90; |
| EE_Parameter.NaviGpsPLimit = 75; |
| EE_Parameter.NaviGpsILimit = 75; |
| EE_Parameter.NaviGpsDLimit = 75; |
| EE_Parameter.NaviGpsACC = 0; |
| EE_Parameter.NaviGpsMinSat = 6; |
| EE_Parameter.NaviStickThreshold = 8; |
| EE_Parameter.NaviWindCorrection = 90; |
| EE_Parameter.NaviSpeedCompensation = 30; |
| EE_Parameter.NaviOperatingRadius = 100; |
| EE_Parameter.NaviAngleLimitation = 100; |
| EE_Parameter.NaviPH_LoginTime = 4; |
| memcpy(EE_Parameter.Name, "Sport\0", 12); |
| } |
| //----------------------------------------------------------------------------------------------------------------------------- |
| //----------------------------------------------------------------------------------------------------- |
| // Parametersatz aus dem EEPROM lesen |
| // Nummer [1..5] möglich |
| //----------------------------------------------------------------------------------------------------- |
| void ReadParameterSet(unsigned char number, unsigned char *buffer, unsigned char length) |
| { |
| if((number > 5)||(number < 1)) number = 3; |
| eeprom_read_block(buffer, &EEPromArray[EEPROM_ADR_PARAM_BEGIN + length * (number - 1)], length); // #define EEPROM_ADR_PARAM_BEGIN 100 |
| LED_Init (); // initialisiert die Schalt-Ausgänge PORTC2 und PORTC3 an SV2 (control of J16 & J17) |
| } |
| //----------------------------------------------------------------------------------------------------- |
| //----------------------------------------------------------------------------------------------------- |
| // Parametersatz vom ROM ins EEPROM schreiben |
| // Nummer [1..5] ist die mögliche Nummer des Paramtersatzes |
| // *buffer ist die Adresse, woher die Bytes kommen, die kopiert werden sollen |
| // length ist die Anzahl der zu kopierenden Bytes |
| //------------------------------------------------------------------------------------------------------------------------------------ |
| void WriteParameterSet(unsigned char number, unsigned char *buffer, unsigned char length) |
| { |
| if(number > 5) number = 5; |
| if(number < 1) return; |
| eeprom_write_block(buffer, &EEPromArray[EEPROM_ADR_PARAM_BEGIN + length * (number - 1)], length); // #define EEPROM_ADR_PARAM_BEGIN 100 |
| // eeprom_write_block(buffer, &EEPromArray[100 + length * (number - 1)], length); // aufgelöst |
| eeprom_write_byte(&EEPromArray[EEPROM_ADR_PARAM_LENGTH], length); // Länge der Datensätze merken // #define EEPROM_ADR_PARAM_LENGTH 98 |
| // eeprom_write_byte(&EEPromArray[98], length); // aufgelöst // lenght = 101 |
| eeprom_write_block(buffer, &EEPromArray[EEPROM_ADR_CHANNELS], 8); // 8 Kanäle merken // #define EEPROM_ADR_CHANNELS 80 |
| // eeprom_write_block(buffer, &EEPromArray[80], 8); // aufgelöst |
| SetActiveParamSetNumber(number); |
| LED_Init(); // initialisiert die Schalt-Ausgänge PORTC2 und PORTC3 an SV2 (control of J16 & J17) |
| } |
| //------------------------------------------------------------------------------------------------------------------------------------ |
| //------------------------------------------------------------------------------------------------------------------------------------- |
| unsigned char GetActiveParamSetNumber(void) // hier wird herausgelesen, mit welchem Parametersatz gefolgen werden soll |
| { // Beginner=3,Normal=2,Sport=1 |
| unsigned char set; |
| set = eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET]); // #define EEPROM_ADR_ACTIVE_SET 2 |
| if((set > 5) || (set < 1)) |
| { |
| set = 3; // default = Beginner |
| SetActiveParamSetNumber(set); // diesen Parametersatz als aktuell merken |
| } |
| return(set); |
| } |
| //------------------------------------------------------------------------------------------------------------------------------------- |
| //------------------------------------------------------------------------------------------------------------------------------------- |
| void SetActiveParamSetNumber(unsigned char number) |
| { |
| if(number > 5) number = 5; |
| if(number < 1) return; // #define EEPROM_ADR_ACTIVE_SET 2 |
| eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], number); // diesen Parametersatz im EEPROM als aktuell merken |
| } |
| //------------------------------------------------------------------------------------------------------------------------------------- |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| /branches/V0.76g_dk9nw_balancekopter/eeprom.h |
|---|
| 0,0 → 1,39 |
| /***************************************************************************************************************************** |
| * File: eeprom.h |
| * Purpose: header of eeprom.c |
| *****************************************************************************************************************************/ |
| #ifndef _EEPROM_H |
| #define _EEPROM_H |
| #define EE_DATENREVISION 80 // Parameter fürs Koptertool; entspricht den EEPROM-Daten von FlightCtrl Version V0.76g |
| #define MIXER_REVISION 1 // wird angepasst, wenn sich die Mixer-Daten geändert haben |
| #define EEPROM_ADR_VALID 1 // für EEPROM Datenrevision = 80 bei V0.76g |
| #define EEPROM_ADR_ACTIVE_SET 2 // für den aktiven Parametersatz 3=Beginner 2=Normal 1=Sport |
| #define EEPROM_ADR_LAST_OFFSET 3 // Luftdruck Offset siehe analog.c Zeile 44 |
| #define EEPROM_ADR_ACC_NICK 4 |
| #define EEPROM_ADR_ACC_ROLL 6 |
| #define EEPROM_ADR_ACC_Z 8 |
| #define EEPROM_ADR_MINUTES 10 // FlugMinuten Gesamt über alle Flüge |
| #define EEPROM_ADR_MINUTES2 14 // FlugMinuten Einzelflug |
| #define EEPROM_ADR_CHANNELS 80 // Ablageadresse der Kanäle |
| #define EEPROM_ADR_PARAM_LENGTH 98 // Länge des structs mk_param_struct verwendet für EE_Parameter = 101 |
| #define EEPROM_ADR_PARAM_BEGIN 100 // Startadresse der Abbilder vom struct mk_param_struct also EE_Parameter |
| #define EEPROM_ADR_MIXER_TABLE 1000 // 1001 - 1100 |
| #ifndef EEMEM |
| #define EEMEM __attribute__ ((section (".eeprom"))) |
| #endif |
| //----------------------------------------------------------------------------------------------------------------------------------- |
| extern unsigned char EEPromArray[]; |
| //----------------------------------------- declaration of functions -------------------------- |
| void ReadParameterSet (unsigned char number, unsigned char *buffer, unsigned char length); |
| void WriteParameterSet(unsigned char number, unsigned char *buffer, unsigned char length); |
| extern unsigned char GetActiveParamSetNumber(void); |
| void SetActiveParamSetNumber(unsigned char number); |
| #endif |
| //*** EOF: __EEPROM_H *********************************************************************************************************** |
| /branches/V0.76g_dk9nw_balancekopter/fc.c |
|---|
| 0,0 → 1,1817 |
| /***************************************************************************************************************************** |
| * File: fc.c |
| * |
| * Purpose: Flight Control |
| * |
| * Functions: int MotorSmoothing(int neu, int alt) |
| * void Piep(unsigned char Anzahl, unsigned int dauer) |
| * void SetNeutral(void) |
| * void Mittelwert(void) |
| * void CalibrierMittelwert(void) |
| * void SendMotorData(void) |
| * void ParameterZuordnung(void) |
| * void MotorRegler(void) |
| * |
| *****************************************************************************************************************************/ |
| #include "fc.h" |
| #include "main.h" |
| #include "eeprom.c" |
| #include "mymath.h" |
| #include "isqrt.h" // wird nur gebraucht wegen Zeile 1265 // CosAttitude = (int16_t)ihypot(tmp_int, tmp_int2); |
| unsigned char h,m,s; |
| unsigned int BaroExpandActive = 0; |
| volatile unsigned int I2CTimeout = 100; |
| int MesswertNick,MesswertRoll,MesswertGier,MesswertGierBias, RohMesswertNick,RohMesswertRoll; |
| int TrimNick, TrimRoll; |
| 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; // dieser Wert wird im 3D Koptertool angezeigt |
| long IntegralNick2 = 0; |
| long IntegralRoll = 0; |
| long 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; |
| int KompassValue = 0; |
| int KompassStartwert = 0; |
| int KompassRichtung = 0; |
| unsigned int KompassSignalSchlecht = 500; |
| long ErsatzKompass; |
| int ErsatzKompassInGrad; // Kompasswert in Grad |
| char MotorenEin = 0; |
| unsigned char MAX_GAS,MIN_GAS; |
| unsigned char HoehenReglerAktiv = 0; |
| unsigned char TrichterFlug = 0; |
| long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L; |
| int GierGyroFehler = 0; |
| char GyroFaktor,GyroFaktorGier; |
| char IntegralFaktor,IntegralFaktorGier; |
| int DiffNick,DiffRoll; |
| int Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0; |
| volatile unsigned char SenderOkay = 0; |
| volatile unsigned char SenderRSSI = 0; |
| int StickNick = 0,StickRoll = 0,StickGier = 0,StickGas = 0; // Stick aus der Fernsteuerung |
| long HoehenWert = 0; |
| long SollHoehe = 0; |
| int LageKorrekturRoll = 0,LageKorrekturNick = 0; |
| int Ki = 10300 / 33; |
| 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_Hoehe_GPS_Z = 64; // Wert : 0-250 |
| unsigned char Parameter_Gyro_D = 8; // Wert : 0-250 |
| unsigned char Parameter_Gyro_P = 150; // Wert : 10-250 |
| unsigned char Parameter_Gyro_I = 150; // Wert : 0-250 |
| unsigned char Parameter_Gyro_Gier_P = 150; // Wert : 10-250 |
| unsigned char Parameter_Gyro_Gier_I = 150; // Wert : 10-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_ServoRollControl = 100; |
| unsigned char Parameter_LoopGasLimit = 70; |
| unsigned char Parameter_AchsKopplung1 = 90; |
| unsigned char Parameter_AchsKopplung2 = 65; |
| unsigned char Parameter_CouplingYawCorrection = 64; |
| 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_NaviOperatingRadius; |
| unsigned char Parameter_NaviWindCorrection; |
| unsigned char Parameter_NaviSpeedCompensation; |
| unsigned char Parameter_ExternalControl; |
| struct mk_param_struct EE_Parameter; // definiert in fc.h Zeile 80 |
| signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20; |
| int MaxStickNick = 0,MaxStickRoll = 0; |
| unsigned int modell_fliegt = 0; |
| volatile unsigned char MikroKopterFlags = 0; |
| long GIER_GRAD_FAKTOR = 1291; |
| signed int KopplungsteilNickRoll,KopplungsteilRollNick; |
| unsigned char RequiredMotors = 4; |
| unsigned char Motor[MAX_MOTORS]; |
| signed int tmp_motorwert[MAX_MOTORS]; |
| unsigned char LoadHandler = 0; |
| //------------------------------------------------------------ Definitionen -------------------------------------- |
| #define LIMIT_MIN(value, min) {if(value < min) value = min;} |
| #define LIMIT_MAX(value, max) {if(value > max) value = max;} |
| #define LIMIT_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;} |
| // ***************************************************************************************************************** |
| int MotorSmoothing(int neu, int alt) |
| { |
| int motor; |
| if(neu > alt) motor = (1*(int)alt + neu) / 2; |
| else motor = neu - (alt - neu)*1; |
| return(motor); |
| } |
| //------------------------------------------------------ |
| // *********************************************************** |
| // erzeugt einen Piepton |
| //------------------------------------------------------ |
| void Piep(unsigned char Anzahl, unsigned int dauer) |
| { |
| if(MotorenEin) return; //auf keinen Fall im Flug! |
| while(Anzahl--) |
| { |
| beeptime = dauer; |
| while(beeptime); |
| Delay_ms(dauer * 2); |
| } |
| } |
| //------------------------------------------------------ |
| // ******************************************************************************************************************* |
| // Nullwerte ermitteln und Startwerte festlegen |
| // ----------------------------------------------- |
| void SetNeutral(void) |
| { |
| unsigned char i; |
| unsigned int gier_neutral=0, nick_neutral=0, roll_neutral=0; |
| HEF4017R_ON; |
| NeutralAccX = 0; |
| NeutralAccY = 0; |
| NeutralAccZ = 0; |
| AdNeutralNick = 0; |
| AdNeutralRoll = 0; |
| AdNeutralGier = 0; |
| AdNeutralGierBias = 0; |
| Parameter_AchsKopplung1 = 0; |
| Parameter_AchsKopplung2 = 0; |
| ExpandBaro = 0; |
| CalibrierMittelwert(); |
| Delay_ms_Mess(100); |
| CalibrierMittelwert(); |
| if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert? |
| { |
| if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset(); |
| } |
| #define NEUTRAL_FILTER 32 |
| for(i=0; i<NEUTRAL_FILTER; i++) |
| { |
| Delay_ms_Mess(10); |
| gier_neutral += AdWertGier; |
| nick_neutral += AdWertNick; |
| roll_neutral += AdWertRoll; |
| } |
| AdNeutralNick= (nick_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER / 8); |
| AdNeutralRoll= (roll_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER / 8); |
| AdNeutralGier= (gier_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER); |
| AdNeutralGierBias = AdNeutralGier; |
| StartNeutralRoll = AdNeutralRoll; |
| StartNeutralNick = AdNeutralNick; |
| if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) > 4) |
| { |
| NeutralAccY = abs(Mittelwert_AccRoll) / (2*ACC_AMPLIFY); // #define ACC_AMPLIFY 6 |
| NeutralAccX = abs(Mittelwert_AccNick) / (2*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]); |
| } |
| MesswertNick = 0; |
| MesswertRoll = 0; |
| MesswertGier = 0; |
| Delay_ms_Mess(100); |
| Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick; // #define ACC_AMPLIFY 6 |
| Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll; |
| IntegralNick = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccNick; |
| IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll; |
| Mess_IntegralNick2 = IntegralNick; |
| Mess_IntegralRoll2 = IntegralRoll; |
| Mess_Integral_Gier = 0; |
| StartLuftdruck = Luftdruck; |
| VarioMeter = 0; |
| Mess_Integral_Hoch = 0; |
| KompassStartwert = KompassValue; |
| //GPS_Neutral(); // no GPS available |
| beeptime = 50; |
| Umschlag180Nick = ((long) EE_Parameter.WinkelUmschlagNick * 2500L) + 15000L; |
| Umschlag180Roll = ((long) EE_Parameter.WinkelUmschlagRoll * 2500L) + 15000L; |
| ExternHoehenValue = 0; |
| ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR; // long GIER_GRAD_FAKTOR = 1291; |
| GierGyroFehler = 0; |
| SendVersionToNavi = 1; |
| LED_Init(); |
| MikroKopterFlags |= FLAG_CALIBRATE; |
| Poti1 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110; |
| Poti2 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110; |
| Poti3 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110; |
| Poti4 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110; |
| SenderOkay = 100; |
| //---------------------------------------------- |
| if(ServoActive) |
| { |
| HEF4017R_ON; |
| DDRD |=0x80; // enable J7 -> Servo signal |
| } |
| //---------------------------------------------- |
| } |
| //*** EOF: SetNeutral(void) ******************************************************************************************************* |
| // ******************************************************************************************************************************** |
| // Bearbeitet die Messwerte und legt die Mittelwerte fest |
| // ---------------------------------------------------------- |
| void Mittelwert(void) |
| { |
| static signed long tmpl,tmpl2,tmpl3,tmpl4; |
| static signed int oldNick, oldRoll, d2Roll, d2Nick; |
| signed long winkel_nick, winkel_roll; |
| // MesswertGier = (signed int) AdNeutralGier - AdWertGier; // Orginalcode vorher |
| MesswertGier = AdWertGier - (signed int) AdNeutralGier; // Gyro ist um 180 Grad gedreht eingebaut |
| // MesswertGierBias = (signed int) AdNeutralGierBias - AdWertGier; |
| MesswertNick = (signed int) AdWertNickFilter / 8; |
| MesswertRoll = (signed int) AdWertRollFilter / 8; |
| RohMesswertNick = MesswertNick; |
| RohMesswertRoll = MesswertRoll; |
| // Beschleunigungssensor -------------------------------------------------------------------------- |
| Mittelwert_AccNick = ((long)Mittelwert_AccNick * 3 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 4L; // #define ACC_AMPLIFY 6 |
| Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 3 + ((ACC_AMPLIFY * (long)AdWertAccRoll))) / 4L; |
| Mittelwert_AccHoch = ((long)Mittelwert_AccHoch * 3 + ((long)AdWertAccHoch)) / 4L; |
| IntegralAccNick += ACC_AMPLIFY * AdWertAccNick; // #define ACC_AMPLIFY 6 |
| IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll; // #define ACC_AMPLIFY 6 |
| NaviAccNick += AdWertAccNick; |
| NaviAccRoll += AdWertAccRoll; |
| NaviCntAcc++; |
| IntegralAccZ += Aktuell_az - NeutralAccZ; |
| //------------------------------------------------------------ |
| // Single Conversion at ADC |
| // |
| // ADCSRA = Analog Digital Conversion Status Register A -> ADEN ADSC ADATE ADIF ADIE ADPS2 ADPS1 ADPS0 |
| // --------------------------------------------------------------------------------------------------- |
| // ADEN = Writing this bit to one enables the ADC |
| // ADSC = start conversion |
| // ADATE = When this bit is written to one, Auto Triggering of the ADC is enabled |
| // ADIE = ADC Interrupt Enable. When this bit is written to one and the I-bit in SREG is set, the ADC Conversion Complete Interrupt is activated. |
| // ADPS = These bits determine the division factor between the XTAL frequency and the input clock to the ADC. |
| // |
| ANALOG_ON; // #define ANALOG_ON ADCSRA=(1<<ADEN)|(1<<ADSC)|(0<<ADATE)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0)|(1<<ADIE) |
| AdReady = 0; |
| //------------------------------------------------------------ |
| if(Mess_IntegralRoll > 93000L) winkel_roll = 93000L; |
| else if(Mess_IntegralRoll <-93000L) winkel_roll = -93000L; |
| else winkel_roll = Mess_IntegralRoll; |
| if(Mess_IntegralNick > 93000L) winkel_nick = 93000L; |
| else if(Mess_IntegralNick <-93000L) winkel_nick = -93000L; |
| else winkel_nick = Mess_IntegralNick; |
| // Gier ----------------------------------------------------------------------------------------- |
| Mess_Integral_Gier += MesswertGier; |
| ErsatzKompass += MesswertGier; |
| // Kopplungsanteil ----------------------------------------------------------------------------- |
| if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV)) |
| { |
| tmpl3 = (MesswertRoll * winkel_nick) / 2048L; |
| tmpl3 *= Parameter_AchsKopplung2; //65 |
| tmpl3 /= 4096L; |
| tmpl4 = (MesswertNick * winkel_roll) / 2048L; |
| tmpl4 *= Parameter_AchsKopplung2; //65 |
| tmpl4 /= 4096L; |
| KopplungsteilNickRoll = tmpl3; |
| KopplungsteilRollNick = tmpl4; |
| tmpl4 -= tmpl3; |
| ErsatzKompass += tmpl4; |
| if(!Parameter_CouplingYawCorrection) Mess_Integral_Gier -= tmpl4/2; // Gier nachhelfen |
| tmpl = ((MesswertGier + tmpl4) * winkel_nick) / 2048L; |
| tmpl *= Parameter_AchsKopplung1; // 90 |
| tmpl /= 4096L; |
| tmpl2 = ((MesswertGier + tmpl4) * winkel_roll) / 2048L; |
| tmpl2 *= Parameter_AchsKopplung1; |
| tmpl2 /= 4096L; |
| if(abs(MesswertGier) > 64) if(labs(tmpl) > 128 || labs(tmpl2) > 128) TrichterFlug = 1; |
| // MesswertGier += (Parameter_CouplingYawCorrection * tmpl4) / 256; |
| } |
| else tmpl = tmpl2 = KopplungsteilNickRoll = KopplungsteilRollNick = 0; |
| TrimRoll = tmpl - tmpl2 / 100L; |
| TrimNick = -tmpl2 + tmpl / 100L; |
| // Kompasswert begrenzen ----------------------------------------------------------------------------------------------- |
| if(ErsatzKompass >= (360L * GIER_GRAD_FAKTOR)) ErsatzKompass -= 360L * GIER_GRAD_FAKTOR; // 360° Umschlag |
| if(ErsatzKompass < 0) ErsatzKompass += 360L * GIER_GRAD_FAKTOR; |
| // Roll ------------------------------------------------------------ |
| Mess_IntegralRoll2 += MesswertRoll + TrimRoll; |
| Mess_IntegralRoll += MesswertRoll + TrimRoll - 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; |
| } |
| // Nick ------------------------------------------------------------------- |
| Mess_IntegralNick2 += MesswertNick + TrimNick; |
| Mess_IntegralNick += MesswertNick + TrimNick - 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; |
| } |
| Integral_Gier = Mess_Integral_Gier; |
| IntegralNick = Mess_IntegralNick; |
| IntegralRoll = Mess_IntegralRoll; |
| IntegralNick2 = Mess_IntegralNick2; |
| IntegralRoll2 = Mess_IntegralRoll2; |
| #define D_LIMIT 128 |
| MesswertNick = HiResNick / 8; |
| MesswertRoll = HiResRoll / 8; |
| 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 > 2000) MesswertNick = +1000; if(AdWertNick > 2015) MesswertNick = +2000; } |
| 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 > 2000) MesswertRoll = +1000; if(AdWertRoll > 2015) MesswertRoll = +2000; } |
| if(Parameter_Gyro_D) |
| { |
| d2Nick = HiResNick - oldNick; |
| oldNick = (oldNick + HiResNick)/2; |
| if(d2Nick > D_LIMIT) d2Nick = D_LIMIT; |
| else if(d2Nick < -D_LIMIT) d2Nick = -D_LIMIT; |
| MesswertNick += (d2Nick * (signed int) Parameter_Gyro_D) / 16; |
| d2Roll = HiResRoll - oldRoll; |
| oldRoll = (oldRoll + HiResRoll)/2; |
| if(d2Roll > D_LIMIT) d2Roll = D_LIMIT; |
| else if(d2Roll < -D_LIMIT) d2Roll = -D_LIMIT; |
| MesswertRoll += (d2Roll * (signed int) Parameter_Gyro_D) / 16; |
| HiResNick += (d2Nick * (signed int) Parameter_Gyro_D); |
| HiResRoll += (d2Roll * (signed int) Parameter_Gyro_D); |
| } |
| if(RohMesswertRoll > 0) TrimRoll += ((long) abs(KopplungsteilNickRoll) * Parameter_CouplingYawCorrection) / 64L; |
| else TrimRoll -= ((long) abs(KopplungsteilNickRoll) * Parameter_CouplingYawCorrection) / 64L; |
| if(RohMesswertNick > 0) TrimNick += ((long) abs(KopplungsteilRollNick) * Parameter_CouplingYawCorrection) / 64L; |
| else TrimNick -= ((long) abs(KopplungsteilRollNick) * Parameter_CouplingYawCorrection) / 64L; |
| if(EE_Parameter.GlobalConfig & CFG_DREHRATEN_BEGRENZER && !Looping_Nick && !Looping_Roll) |
| { |
| if(RohMesswertNick > 256) MesswertNick += 1 * (RohMesswertNick - 256); |
| else if(RohMesswertNick < -256) MesswertNick += 1 * (RohMesswertNick + 256); |
| if(RohMesswertRoll > 256) MesswertRoll += 1 * (RohMesswertRoll - 256); |
| else if(RohMesswertRoll < -256) MesswertRoll += 1 * (RohMesswertRoll + 256); |
| } |
| 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; |
| } |
| // *** EOF: Mittelwert(void) ******************************************************************************************************** |
| // ********************************************************************************************************************************** |
| // Messwerte beim Ermitteln der Nullage |
| // ---------------------------------------- |
| void CalibrierMittelwert(void) |
| { |
| if(PlatinenVersion == 13) SucheGyroOffset(); |
| ANALOG_OFF; // ADC auschalten, damit die Werte sich nicht während der Berechnung ändern |
| MesswertNick = AdWertNick; |
| MesswertRoll = AdWertRoll; |
| MesswertGier = AdWertGier; |
| Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick; // #define ACC_AMPLIFY 6 |
| Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll; |
| Mittelwert_AccHoch = (long)AdWertAccHoch; |
| // ADCSRA = Analog Digital Conversion Status Register A -> ADEN ADSC ADATE ADIF ADIE ADPS2 ADPS1 ADPS0 |
| // --------------------------------------------------------------------------------------------------- |
| // ADEN = Writing this bit to one enables the ADC |
| // ADSC = start conversion |
| // ADATE = When this bit is written to one, Auto Triggering of the ADC is enabled |
| // ADIE = ADC Interrupt Enable. When this bit is written to one and the I-bit in SREG is set, the ADC Conversion Complete Interrupt is activated. |
| // ADPS = These bits determine the division factor between the XTAL frequency and the input clock to the ADC. |
| // |
| ANALOG_ON; // swich on ADC // #define ANALOG_ON ADCSRA=(1<<ADEN)|(1<<ADSC)|(0<<ADATE)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0)|(1<<ADIE) |
| 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; |
| } |
| // *** EOF: CalibrierMittelwert() *************************************************************************************************** |
| // *************************************************************************************************************************** |
| // take over the motor values from Motor[] and start I2C-Bus |
| // ---------------------------------------------------------- |
| void SendMotorData(void) |
| { |
| unsigned char i; |
| if(!MotorenEin) |
| { |
| MikroKopterFlags &= ~(FLAG_MOTOR_RUN | FLAG_FLY); |
| for(i=0; i < MAX_MOTORS; i++) // #define MAX_MOTORS 4 |
| { |
| if(!PC_MotortestActive) MotorTest[i] = 0; |
| Motor[i] = MotorTest[i]; |
| } |
| if(PC_MotortestActive) PC_MotortestActive--; |
| } |
| else MikroKopterFlags |= FLAG_MOTOR_RUN; // #define FLAG_MOTOR_RUN 1 |
| DebugOut.Analog[12] = Motor[0]; // see also fc.c line 1270 |
| DebugOut.Analog[13] = Motor[1]; |
| twi_state = 0; // it is assumed that all Motor[] have ther new value now -> see line 1645 |
| motor = 0; |
| i2c_start(); // start I2C Interrupt Mode |
| } |
| // *************************************************************************************************************************** |
| // ******************************************************************************************************************************** |
| // Trägt gegebenfalls 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_Hoehe_GPS_Z,EE_Parameter.Hoehe_GPS_Z,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_Gyro_D,EE_Parameter.Gyro_D,0,255); |
| CHK_POTI(Parameter_Gyro_Gier_P,EE_Parameter.Gyro_Gier_P,10,255); |
| CHK_POTI(Parameter_Gyro_Gier_I,EE_Parameter.Gyro_Gier_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_ServoRollControl,EE_Parameter.ServoRollControl,0,255); |
| CHK_POTI(Parameter_LoopGasLimit,EE_Parameter.LoopGasLimit,0,255); |
| CHK_POTI(Parameter_AchsKopplung1, EE_Parameter.AchsKopplung1,0,255); |
| CHK_POTI(Parameter_AchsKopplung2, EE_Parameter.AchsKopplung2,0,255); |
| CHK_POTI(Parameter_CouplingYawCorrection,EE_Parameter.CouplingYawCorrection,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_ExternalControl,EE_Parameter.ExternalControl,0,255); |
| Ki = 10300 / (Parameter_I_Faktor + 1); |
| MAX_GAS = EE_Parameter.Gas_Max; // bei Beginner3 = 230 |
| MIN_GAS = EE_Parameter.Gas_Min; // bei Beginner3 = 8 |
| } |
| //----------------------------------------------------------------------------------------------------------- |
| // ------------------------------------------only for balance ------------------------------------ |
| unsigned char ucflg1=0x01, ucflg2=0x01, ucflg3=0x01; |
| int ipk[3] = {0,0,0}; |
| int angle, desiredAngle, motorOutFront, motorOutRear; |
| int thrust; |
| int kp=0, kd=0, kdd=0; |
| int controllerP = 0, maxcontrollerP = 0, mincontrollerP = 0; |
| int controllerD = 0, maxcontrollerD = 0 , mincontrollerD = 0; |
| int controllerDD = 0, maxcontrollerDD = 0, mincontrollerDD = 0; |
| int gyroScaledOld = 0; |
| int gyroScaled = 0; |
| int filtersum = 0; |
| int filterDD = 0; |
| // EOF: for balance ---------------------------------------------------------------------------- |
| // ********************************************************************************************************************** |
| // automatic control function - called from main.c |
| // ------------------------------------------------ |
| void MotorRegler(void) |
| { |
| int pd_ergebnis_nick,pd_ergebnis_roll,tmp_int, tmp_int2; |
| 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 char TimerWerteausgabe = 0; |
| static char NeueKompassRichtungMerken = 0; |
| static long ausgleichNick, ausgleichRoll; |
| int IntegralNickMalFaktor,IntegralRollMalFaktor; |
| // unsigned char i; |
| if(--LoadHandler == 0) LoadHandler = 5; // verteilt die Prozessorlast |
| Mittelwert(); // see fc.c line 273 |
| GRN_ON; // switch on green LED |
| // ------------------------------------------------------------------- |
| // find out gas value |
| // ------------------------------------------------------------------- |
| GasMischanteil = StickGas; |
| if(GasMischanteil < MIN_GAS + 10) GasMischanteil = MIN_GAS + 10; |
| // ---------------------------------------------------------------------------------- |
| // radio control receiving is bad |
| // ---------------------------------------------------------------------------------- |
| if(SenderOkay < 100) |
| { |
| if(RcLostTimer) RcLostTimer--; |
| else |
| { |
| MotorenEin = 0; |
| MikroKopterFlags &= ~FLAG_NOTLANDUNG; |
| } |
| ROT_ON; |
| if(modell_fliegt > 1000) // wahrscheinlich in der Luft --> langsam absenken |
| { |
| GasMischanteil = EE_Parameter.NotGas; |
| MikroKopterFlags |= FLAG_NOTLANDUNG; |
| PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] = 0; // #define K_NICK 0 |
| PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] = 0; // #define K_ROLL 1 |
| PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] = 0; // #define K_GAS 2 |
| PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] = 0; |
| PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] = 0; // #define K_GIER 3 |
| } |
| else MotorenEin = 0; |
| } // EOF : receiving radio control is bad |
| else |
| // ----------------------------------------------------------------------------------- |
| // radio control receiving is fine |
| // ----------------------------------------------------------------------------------- |
| if(SenderOkay > 140) |
| { |
| MikroKopterFlags &= ~FLAG_NOTLANDUNG; |
| 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; |
| } |
| } |
| else MikroKopterFlags |= FLAG_FLY; |
| if((PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] > 80) && MotorenEin == 0) |
| { |
| //---------------------------------------------------------------------------- |
| // calibrate to zero |
| //---------------------------------------------------------------------------- |
| if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75) // neutral values |
| { |
| if(++delay_neutral > 200) // during the first 200 ms |
| { |
| GRN_OFF; |
| MotorenEin = 0; // switch or keep motor in status "off" |
| delay_neutral = 0; |
| modell_fliegt = 0; |
| //---------------------------------------------------------------------------------------------------------------- |
| // Bei ausgeschalteten Motoren Settings per Sender einstellen: |
| // |
| // Gas/Gier-Knüppel oben-links und gleichzeitig... |
| // |
| // Nick/Roll-Knüppel -> Links Mitte = Setting 1; Sport, sehr agil |
| // Nick/Roll-Knüppel -> Links Oben = Setting 2; Normal |
| // Nick/Roll-Knüppel -> Mitte Oben = Setting 3; Beginner, empfohlen für erste Versuche |
| // Nick/Roll-Knüppel -> Rechts Oben = Setting 4 |
| // Nick/Roll-Knüppel -> Rechts Mitte = Setting 5 |
| // |
| // Setting 1 = Gas rauf und Gier links dann Roll links und Nick mitte |
| // Setting 2 = Gas rauf und Gier links dann Roll links und Nick rauf |
| // Setting 3 = Gas rauf und Gier links dann Roll mitte und Nick rauf |
| // Setting 4 = Gas rauf und Gier links dann Roll rechts und Nick rauf |
| // Setting 5 = Gas rauf und Gier links dann Roll rechts und Nick mitte |
| //---------------------------------------------------------------------------------------------------------------- |
| 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; |
| SetActiveParamSetNumber(setting); // aktiven Datensatz merken |
| } |
| if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) < 30 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -70) |
| { |
| WinkelOut.CalcState = 1; // Kompass wird kalibriert |
| beeptime = 1000; |
| } |
| else |
| { |
| //------------------------------------------------------------------------------------------------------------------- |
| // Hier werden die Werte aus dem EEPROM entsprechend Setting eingelesen |
| //------------------------------------------------------------------------------------------------------------------- |
| ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE); // #define STRUCT_PARAM_LAENGE sizeof(EE_Parameter) |
| if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert? |
| { |
| if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset(); |
| } |
| // Vor jedem Starten des MK sollten die Gyros neu kalibriert werden. |
| // Dazu wird der Gas/Gier-Knüppel einige Zeit in die obere linke Ecke gedrückt |
| ServoActive = 0; |
| SetNeutral(); |
| ServoActive = 1; |
| DDRD |=0x80; // enable J7 -> Servo signal |
| Piep(GetActiveParamSetNumber(),120); |
| } |
| } // Ende von "nicht sofort, sondern erst nach 200 ms" |
| } |
| 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(),120); |
| } |
| } |
| else delay_neutral = 0; |
| } //Ende Gas ist oben und Motoren aus |
| //--------------------------------------------------------------------------------------------------------------------- |
| // gas is at bottom for switing on or off |
| //--------------------------------------------------------------------------------------------------------------------- |
| if(PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] < -85) // Gas unten |
| { |
| if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75) // Starten - Gierknüppel unten ganz rechts |
| { |
| // ------------------------------------------------------------------------------------- |
| // switch on motor |
| // ------------------------------------------------------------------------------------- |
| if(++delay_einschalten > 200) |
| { |
| delay_einschalten = 200; |
| modell_fliegt = 1; |
| MotorenEin = 1; |
| sollGier = 0; |
| Mess_Integral_Gier = 0; |
| Mess_Integral_Gier2 = 0; |
| Mess_IntegralNick = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccNick; |
| Mess_IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll; |
| Mess_IntegralNick2 = IntegralNick; |
| Mess_IntegralRoll2 = IntegralRoll; |
| SummeNick = 0; |
| SummeRoll = 0; |
| MikroKopterFlags |= FLAG_START; |
| } |
| } |
| else delay_einschalten = 0; // reset delay |
| //---------------------------------------------------------------------------------------------- |
| // switch off |
| //---------------------------------------------------------------------------------------------- |
| 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; |
| } // EOF : gas is at bottom |
| } // EOF : receiving radio control is good |
| //------------------------------------------------------------------------------------------------------------------- |
| // neue Werte von der Funke |
| //------------------------------------------------------------------------------------------------------------------- |
| if(!NewPpmData-- || (MikroKopterFlags & FLAG_NOTLANDUNG)) |
| { |
| 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; |
| 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; |
| StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]; |
| if(StickGier > 2) StickGier -= 2; else |
| if(StickGier < -2) StickGier += 2; else StickGier = 0; |
| StickGas = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120; |
| GyroFaktor = (Parameter_Gyro_P + 10.0); |
| IntegralFaktor = Parameter_Gyro_I; |
| GyroFaktorGier = (Parameter_Gyro_Gier_P + 10.0); |
| IntegralFaktorGier = Parameter_Gyro_Gier_I; |
| //------------------------------------------------------------------------------------------ |
| // Analoge Steuerung nicht per Fersteuerung, sondern per serieller Schnittstelle |
| //------------------------------------------------------------------------------------------ |
| 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(MikroKopterFlags & FLAG_NOTLANDUNG) {MaxStickNick = 0; MaxStickRoll = 0;} |
| // ----------------------------------------------------------------------------------------------------------------------------------- |
| // Looping? |
| // ----------------------------------------------------------------------------------------------------------------------------------- |
| if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > EE_Parameter.LoopThreshold) && EE_Parameter.BitConfig & 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.BitConfig & 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.BitConfig & 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.BitConfig & 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; |
| } // EOF : "neue Funksteuerungswerte" |
| if(Looping_Roll || Looping_Nick) |
| { |
| if(GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit; |
| TrichterFlug = 1; |
| } |
| // --------------------------------------------------------------------------------------------- |
| // Bei Empfangsausfall im Flug Notlandung einleiten |
| // --------------------------------------------------------------------------------------------- |
| if(MikroKopterFlags & FLAG_NOTLANDUNG) |
| { |
| StickGier = 0; |
| StickNick = 0; |
| StickRoll = 0; |
| GyroFaktor = 90; |
| IntegralFaktor = 120; |
| GyroFaktorGier = 90; |
| IntegralFaktorGier = 120; |
| Looping_Roll = 0; |
| Looping_Nick = 0; |
| } // ------------------- Ende : Notlandung ----------------------------------------------------- |
| //--------------------------------------------------------------------------------------------- |
| // 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; // counts one up as all ADC cases have been passed through -> see analog.c line 288 |
| LageKorrekturNick = 0; |
| LageKorrekturRoll = 0; |
| } |
| //------------------------------------------------------------------------------------------------ |
| if(!Looping_Nick && !Looping_Roll && (Aktuell_az > 512 || MotorenEin)) |
| { |
| 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; |
| //if(Poti2 > 20) { tmp_long = 0; tmp_long2 = 0;} |
| Mess_IntegralNick -= tmp_long; |
| Mess_IntegralRoll -= tmp_long2; |
| } // --------------------------------- EOF: Looping ----------------------------------- |
| // ------------------------------------------------------------------------------------------------- |
| // nach 256 ADC Durchläufen wird abgeglichen |
| // ------------------------------------------------------------------------------------------------- |
| if(ZaehlMessungen >= ABGLEICH_ANZAHL) // #define ABGLEICH_ANZAHL 256L |
| { // counts one up as all ADC cases have been passed through -> see analog.c line 288 |
| 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 && EE_Parameter.Driftkomp) |
| { |
| MittelIntegralNick /= ABGLEICH_ANZAHL; // #define ABGLEICH_ANZAHL 256L |
| 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; // #define ABGLEICH_ANZAHL 256L |
| 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; // #define ABGLEICH_ANZAHL 256L |
| 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; |
| if(EE_Parameter.Driftkomp) |
| { |
| if(GierGyroFehler > ABGLEICH_ANZAHL/2) { AdNeutralGier++; AdNeutralGierBias++; } // #define ABGLEICH_ANZAHL 256L |
| if(GierGyroFehler <-ABGLEICH_ANZAHL/2) { AdNeutralGier--; AdNeutralGierBias--; } |
| } |
| GierGyroFehler = 0; |
| #define FEHLER_LIMIT (ABGLEICH_ANZAHL / 2) // #define ABGLEICH_ANZAHL 256L |
| #define FEHLER_LIMIT1 (ABGLEICH_ANZAHL * 2) //4 |
| #define FEHLER_LIMIT2 (ABGLEICH_ANZAHL * 16) //16 |
| #define BEWEGUNGS_LIMIT 20000 |
| // --------------- Nick ------------------------------------------------------------------ |
| cnt = 1; // + labs(IntegralFehlerNick) / 4096; |
| if(labs(IntegralFehlerNick) > FEHLER_LIMIT1) cnt = 4; |
| if( labs(MittelIntegralNick_Alt - MittelIntegralNick) < BEWEGUNGS_LIMIT ) |
| { |
| if(IntegralFehlerNick > FEHLER_LIMIT2) |
| { |
| if(last_n_p) |
| { |
| cnt += labs(IntegralFehlerNick) / (FEHLER_LIMIT2 / 8); |
| ausgleichNick = IntegralFehlerNick / 8; |
| if(ausgleichNick > 5000) ausgleichNick = 5000; |
| LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL; // #define ABGLEICH_ANZAHL 256L |
| } |
| else last_n_p = 1; |
| } |
| else last_n_p = 0; |
| if(IntegralFehlerNick < -FEHLER_LIMIT2) |
| { |
| if(last_n_n) |
| { |
| cnt += labs(IntegralFehlerNick) / (FEHLER_LIMIT2 / 8); |
| ausgleichNick = IntegralFehlerNick / 8; |
| if(ausgleichNick < -5000) ausgleichNick = -5000; |
| LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL; // #define ABGLEICH_ANZAHL 256L |
| } |
| 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; |
| if(labs(IntegralFehlerRoll) > FEHLER_LIMIT1) cnt = 4; |
| ausgleichRoll = 0; |
| if(labs(MittelIntegralRoll_Alt - MittelIntegralRoll) < BEWEGUNGS_LIMIT ) |
| { |
| if(IntegralFehlerRoll > FEHLER_LIMIT2) |
| { |
| if(last_r_p) |
| { |
| cnt += labs(IntegralFehlerRoll) / (FEHLER_LIMIT2 / 8); |
| ausgleichRoll = IntegralFehlerRoll / 8; |
| if(ausgleichRoll > 5000) ausgleichRoll = 5000; |
| LageKorrekturRoll += ausgleichRoll / ABGLEICH_ANZAHL; // #define ABGLEICH_ANZAHL 256L |
| } |
| else last_r_p = 1; |
| } else last_r_p = 0; |
| if(IntegralFehlerRoll < -FEHLER_LIMIT2) |
| { |
| if(last_r_n) |
| { |
| cnt += labs(IntegralFehlerRoll) / (FEHLER_LIMIT2 / 8); |
| ausgleichRoll = IntegralFehlerRoll / 8; |
| if(ausgleichRoll < -5000) ausgleichRoll = -5000; |
| LageKorrekturRoll += ausgleichRoll / ABGLEICH_ANZAHL; // #define ABGLEICH_ANZAHL 256L |
| } |
| 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) // z.B. bei Heading Hold wird nicht ausgeglichen |
| { |
| LageKorrekturRoll = 0; |
| LageKorrekturNick = 0; |
| } |
| // ---------------------------------------------------------------------------------- |
| MittelIntegralNick_Alt = MittelIntegralNick; |
| MittelIntegralRoll_Alt = MittelIntegralRoll; |
| // ---------------------------------------------------------------------------------- |
| IntegralAccNick = 0; |
| IntegralAccRoll = 0; |
| IntegralAccZ = 0; |
| MittelIntegralNick = 0; |
| MittelIntegralRoll = 0; |
| MittelIntegralNick2 = 0; |
| MittelIntegralRoll2 = 0; |
| ZaehlMessungen = 0; // counts one up as all ADC cases have been passed through -> see analog.c line 288 |
| } |
| // EOF: if(ZaehlMessungen >= ABGLEICH_ANZAHL) |
| // ------------------------------------------------------------------------------------------------------------- |
| // -------- Gieren ----------- |
| // |
| // Für eine Drehung im Uhrzeigersinn wird die Drehzahl des linken und des rechten Propellers erhöht, |
| // während die des vorderen und hinteren gesenkt wird |
| // ------------------------------------------------------------------------------------------------------------- |
| if(abs(StickGier) > 15) // war 35 |
| { |
| KompassSignalSchlecht = 1000; |
| if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) // Orientation fix |
| { |
| NeueKompassRichtungMerken = 1; |
| } |
| } |
| tmp_int = (long) EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo y = ax + bx² |
| tmp_int += (EE_Parameter.Gier_P * StickGier) / 4; |
| sollGier = tmp_int; |
| Mess_Integral_Gier -= tmp_int; |
| if(Mess_Integral_Gier > 50000) Mess_Integral_Gier = 50000; // begrenzen |
| if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000; |
| // ----------------------------------------------------------------------------------------------------- |
| // Kompass |
| // ------------------------------------------------------------------------------------------------------------------ |
| if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)) // CFG_KOMPASS_AKTIV = 0x08 |
| { |
| 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; // long GIER_GRAD_FAKTOR = 1291; |
| if(abs(MesswertGier) > 128) |
| { |
| fehler = 0; |
| } |
| if(!KompassSignalSchlecht && w < 25) |
| { |
| GierGyroFehler += fehler; |
| if(NeueKompassRichtungMerken) |
| { |
| ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR; // long GIER_GRAD_FAKTOR = 1291; |
| 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; // long GIER_GRAD_FAKTOR = 1291; |
| // // 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 |
| } // EOF: Kompass |
| //--------------------------------------------------------------------------------------------------------------------------------- |
| //---------------------------------------------------------------------------------------------------------------------------------- |
| // Debugwerte zuordnen / die zugehörige Texte stehen unter -> uart.c Zeile 46 |
| //---------------------------------------------------------------------------------------------------------------------------------- |
| if(!TimerWerteausgabe--) |
| { |
| TimerWerteausgabe = 24; |
| DebugOut.Analog[0] = (int) (IntegralNick / (EE_Parameter.GyroAccFaktor * 4)); |
| DebugOut.Analog[1] = Mittelwert_AccNick / 4; |
| DebugOut.Analog[2] = maxcontrollerDD; |
| DebugOut.Analog[3] = mincontrollerDD; |
| DebugOut.Analog[4] = MesswertNick; |
| DebugOut.Analog[5] = HiResNick; |
| DebugOut.Analog[6] = AdWertAccNick; |
| // siehe Zeile fc.c Zeile 1660 |
| DebugOut.Analog[8] = 0; |
| DebugOut.Analog[9] = UBat; |
| DebugOut.Analog[10] = SenderOkay; |
| DebugOut.Analog[11] = controllerP; |
| // siehe fc.c Zeile 476 |
| DebugOut.Analog[14] = controllerD; |
| DebugOut.Analog[15] = controllerDD; |
| DebugOut.Analog[16] = PPM_in[3]; |
| DebugOut.Analog[17] = ipk[0]; |
| DebugOut.Analog[18] = ipk[1]; |
| DebugOut.Analog[19] = ipk[2]; |
| DebugOut.Analog[20] = ucflg1; // 0,1,2 |
| DebugOut.Analog[21] = PPM_in[5]; // switch chanel 5 |
| DebugOut.Analog[22] = MesswertNick; |
| DebugOut.Analog[23] = maxcontrollerD; |
| DebugOut.Analog[24] = mincontrollerD; |
| DebugOut.Analog[25] = AdWertNick; |
| DebugOut.Analog[26] = maxcontrollerP; // = 727 |
| DebugOut.Analog[27] = mincontrollerP; // = 730 |
| DebugOut.Analog[28] = PPM_in[4]; // Gier |
| DebugOut.Analog[29] = PPM_in[3]; // Nick |
| DebugOut.Analog[30] = PPM_in[2]; // Roll |
| DebugOut.Analog[31] = PPM_in[1]; // Gas |
| } |
| // ----------------------------------------------------------------------- |
| // Drehgeschwindigkeit und Drehwinkel zu einem Istwert zusammenfassen |
| // ----------------------------------------------------------------------- |
| if(TrichterFlug) {SummeRoll = 0; SummeNick = 0;}; |
| if(!Looping_Nick) IntegralNickMalFaktor = (IntegralNick * IntegralFaktor) / (44000 / STICK_GAIN); else IntegralNickMalFaktor = 0; |
| if(!Looping_Roll) IntegralRollMalFaktor = (IntegralRoll * IntegralFaktor) / (44000 / STICK_GAIN); else IntegralRollMalFaktor = 0; |
| #define TRIM_MAX 200 |
| if(TrimNick > TRIM_MAX) TrimNick = TRIM_MAX; else if(TrimNick <-TRIM_MAX) TrimNick =-TRIM_MAX; |
| if(TrimRoll > TRIM_MAX) TrimRoll = TRIM_MAX; else if(TrimRoll <-TRIM_MAX) TrimRoll =-TRIM_MAX; |
| MesswertNick = IntegralNickMalFaktor + (long)((long)MesswertNick * GyroFaktor + (long)TrimNick * 128L) / (256L / STICK_GAIN); |
| MesswertRoll = IntegralRollMalFaktor + (long)((long)MesswertRoll * GyroFaktor + (long)TrimRoll * 128L) / (256L / STICK_GAIN); |
| MesswertGier = (long)(MesswertGier * 2 * (long)GyroFaktorGier) / (256L / STICK_GAIN) + (long)(Integral_Gier * IntegralFaktorGier) / (2 * (44000 / STICK_GAIN)); |
| // Maximalwerte abfangen |
| // #define MAX_SENSOR (4096*STICK_GAIN) |
| #define MAX_SENSOR (4096*4) |
| 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 |
| // ------------------------------------------------------------------------------------------------------------------------ |
| if(UBat > BattLowVoltageWarning) GasMischanteil = ((unsigned int)GasMischanteil * BattLowVoltageWarning) / UBat; // Gas auf das aktuelle Spannungvieveau beziehen |
| GasMischanteil *= STICK_GAIN; |
| // if height control is activated |
| if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG) && !(Looping_Roll || Looping_Nick)) // Höhenregelung |
| { |
| //--------------------------------- definitions------------------------------------------- |
| #define HOOVER_GAS_AVERAGE 4096L // 4096 * 2ms = 8.2s averaging |
| #define HC_GAS_AVERAGE 4 // 4 * 2ms= 8ms averaging |
| #define OPA_OFFSET_STEP 10 |
| //------------------------------- variables ---------------------------------------------- |
| int HCGas, HeightDeviation; |
| static int HeightTrimming = 0; // rate for change of height setpoint |
| static int FilterHCGas = 0; |
| static int StickGasHoover = 120, HooverGas = 0, HooverGasMin = 0, HooverGasMax = 1023; |
| static unsigned long HooverGasFilter = 0; |
| static unsigned char delay = 100, BaroAtUpperLimit = 0, BaroAtLowerLimit = 0; |
| int CosAttitude; // for projection of hoover gas |
| // ------------------------------- get the current hooverpoint ---------------------------------------------------------- |
| // if(LoadHandler == 1) |
| { |
| // DebugOut.Analog[21] = HooverGas; |
| // DebugOut.Analog[18] = VarioMeter; |
| // Expand the measurement |
| // measurement of air pressure close to upper limit and no overflow in correction of the new OCR0A value occurs |
| if(!BaroExpandActive) |
| { |
| if(MessLuftdruck > 920) |
| { // increase offset |
| if(OCR0A < (255 - OPA_OFFSET_STEP)) |
| { |
| ExpandBaro -= 1; |
| OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // increase offset to shift ADC down |
| beeptime = 300; |
| BaroExpandActive = 350; |
| } |
| else |
| { |
| BaroAtLowerLimit = 1; |
| } |
| } |
| // measurement of air pressure close to lower limit and |
| else |
| if(MessLuftdruck < 100) |
| { // decrease offset |
| if(OCR0A > OPA_OFFSET_STEP) |
| { |
| ExpandBaro += 1; |
| OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // decrease offset to shift ADC up |
| beeptime = 300; |
| BaroExpandActive = 350; |
| } |
| else |
| { |
| BaroAtUpperLimit = 1; |
| } |
| } |
| else |
| { |
| BaroAtUpperLimit = 0; |
| BaroAtLowerLimit = 0; |
| } |
| } |
| else // delay, because of expanding the Baro-Range |
| { |
| // now clear the D-values |
| SummenHoehe = HoehenWert * SM_FILTER; // #define SM_FILTER 16 |
| VarioMeter = 0; |
| BaroExpandActive--; |
| } |
| // if height control is activated by an rc channel |
| if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER) // Regler wird über Schalter gesteuert |
| { // check if parameter is less than activation threshold |
| if(Parameter_MaxHoehe < 50) // for 3 or 2-state switch height control is disabled in lowest position |
| { //height control not active |
| if(!delay--) |
| { |
| HoehenReglerAktiv = 0; // disable height control |
| SollHoehe = HoehenWert; // update SetPoint with current reading |
| delay = 1; |
| } |
| } |
| else |
| { //height control is activated |
| HoehenReglerAktiv = 1; // enable height control |
| delay = 200; |
| } |
| } |
| else // no switchable height control |
| { |
| SollHoehe = ((int16_t) ExternHoehenValue + (int16_t) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung; |
| HoehenReglerAktiv = 1; |
| } |
| // calculate cos of nick and roll angle used for projection of the vertical hoover gas |
| tmp_int = (int)(IntegralNick/GIER_GRAD_FAKTOR); // nick angle in deg |
| tmp_int2 = (int)(IntegralRoll/GIER_GRAD_FAKTOR); // roll angle in deg |
| CosAttitude = (int16_t)ihypot(tmp_int, tmp_int2); // phytagoras gives effective attitude angle in deg // xxx |
| LIMIT_MAX(CosAttitude, 60); // limit effective attitude angle |
| CosAttitude = c_cos_8192(CosAttitude); // cos of actual attitude |
| if(HoehenReglerAktiv && !(MikroKopterFlags & FLAG_NOTLANDUNG)) |
| { |
| #define HEIGHT_TRIM_UP 0x01 |
| #define HEIGHT_TRIM_DOWN 0x02 |
| static unsigned char HeightTrimmingFlag = 0x00; |
| #define HEIGHT_CONTROL_STICKTHRESHOLD 15 |
| // Holger original version |
| // start of height control algorithm |
| // the height control is only an attenuation of the actual gas stick. |
| // I.e. it will work only if the gas stick is higher than the hover gas |
| // and the hover height will be allways larger than height setpoint. |
| if((EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT) || !(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER)) // Regler wird über Schalter gesteuert) |
| { // old version |
| HCGas = GasMischanteil; // take current stick gas as neutral point for the height control |
| HeightTrimming = 0; |
| } |
| else |
| { |
| // alternative height control |
| // PD-Control with respect to hoover point |
| // the thrust loss out of horizontal attitude is compensated |
| // the setpoint will be fine adjusted with the gas stick position |
| if(MikroKopterFlags & FLAG_FLY) // trim setpoint only when flying |
| { // gas stick is above hoover point |
| if(StickGas > (StickGasHoover + HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtUpperLimit) |
| { |
| if(HeightTrimmingFlag & HEIGHT_TRIM_DOWN) |
| { |
| HeightTrimmingFlag &= ~HEIGHT_TRIM_DOWN; |
| SollHoehe = HoehenWert; // update setpoint to current heigth |
| } |
| HeightTrimmingFlag |= HEIGHT_TRIM_UP; |
| HeightTrimming += abs(StickGas - (StickGasHoover + HEIGHT_CONTROL_STICKTHRESHOLD)); |
| } // gas stick is below hoover point |
| else if(StickGas < (StickGasHoover - HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtLowerLimit ) |
| { |
| if(HeightTrimmingFlag & HEIGHT_TRIM_UP) |
| { |
| HeightTrimmingFlag &= ~HEIGHT_TRIM_UP; |
| SollHoehe = HoehenWert; // update setpoint to current heigth |
| } |
| HeightTrimmingFlag |= HEIGHT_TRIM_DOWN; |
| HeightTrimming -= abs(StickGas - (StickGasHoover - HEIGHT_CONTROL_STICKTHRESHOLD)); |
| } |
| else // Gas Stick in Hoover Range |
| { |
| if(HeightTrimmingFlag & (HEIGHT_TRIM_UP | HEIGHT_TRIM_DOWN)) |
| { |
| HeightTrimmingFlag &= ~(HEIGHT_TRIM_UP | HEIGHT_TRIM_DOWN); |
| HeightTrimming = 0; |
| SollHoehe = HoehenWert; // update setpoint to current height |
| if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 500; |
| } |
| } |
| // Trim height set point |
| if(abs(HeightTrimming) > 512) |
| { |
| SollHoehe += (HeightTrimming * EE_Parameter.Hoehe_Verstaerkung)/(5 * 512 / 2); // move setpoint |
| HeightTrimming = 0; |
| if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 75; |
| //update hoover gas stick value when setpoint is shifted |
| if(!EE_Parameter.Hoehe_StickNeutralPoint) |
| { |
| StickGasHoover = HooverGas/STICK_GAIN; //rescale back to stick value |
| StickGasHoover = (StickGasHoover * UBat) / BattLowVoltageWarning; |
| if(StickGasHoover < 70) StickGasHoover = 70; |
| else if(StickGasHoover > 150) StickGasHoover = 150; |
| } |
| } |
| if(BaroExpandActive) SollHoehe = HoehenWert; // update setpoint to current altitude if Expanding is active |
| } //if MikroKopterFlags & MKFLAG_FLY |
| else |
| { |
| SollHoehe = HoehenWert - 400; |
| if(EE_Parameter.Hoehe_StickNeutralPoint) StickGasHoover = EE_Parameter.Hoehe_StickNeutralPoint; |
| else StickGasHoover = 120; |
| } |
| HCGas = HooverGas; // take hoover gas (neutral point) |
| } |
| if(HoehenWert > SollHoehe || !(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT)) |
| { |
| // ------------------------- P-Part ---------------------------- |
| HeightDeviation = (int)(HoehenWert - SollHoehe); // positive when too high |
| tmp_int = (HeightDeviation * (int)Parameter_Hoehe_P) / 16; // p-part |
| HCGas -= tmp_int; |
| // ------------------------- D-Part 1: Vario Meter ---------------------------- |
| tmp_int = VarioMeter / 8; |
| if(tmp_int > 8) tmp_int = 8; // limit quadratic part on upward movement to avoid to much gas reduction |
| if(tmp_int > 0) tmp_int = VarioMeter + (tmp_int * tmp_int) / 4; |
| else tmp_int = VarioMeter - (tmp_int * tmp_int) / 4; |
| tmp_int = (Parameter_Luftdruck_D * (long)(tmp_int)) / 128L; // scale to d-gain parameter |
| LIMIT_MIN_MAX(tmp_int, -127, 255); |
| HCGas -= tmp_int; |
| // ------------------------ D-Part 2: ACC-Z Integral ------------------------ |
| tmp_int = ((Mess_Integral_Hoch / 128) * (long) Parameter_Hoehe_ACC_Wirkung) / (128 / STICK_GAIN); |
| LIMIT_MIN_MAX(tmp_int, -127, 255); |
| HCGas -= tmp_int; |
| // --------- limit deviation from hoover point within the target region --------------------------------- |
| if( (abs(HeightDeviation) < 150) && (!HeightTrimming) && (HooverGas > 0)) // height setpoint is not changed and hoover gas not zero |
| { |
| LIMIT_MIN_MAX(HCGas, HooverGasMin, HooverGasMax); // limit gas around the hoover point |
| } |
| if(BaroExpandActive) HCGas = HooverGas; |
| // strech control output by inverse attitude projection 1/cos |
| // + 1/cos(angle) ++++++++++++++++++++++++++ |
| tmp_long2 = (int32_t)HCGas; |
| tmp_long2 *= 8192L; |
| tmp_long2 /= CosAttitude; |
| HCGas = (int16_t)tmp_long2; |
| // update height control gas averaging |
| FilterHCGas = (FilterHCGas * (HC_GAS_AVERAGE - 1) + HCGas) / HC_GAS_AVERAGE; // #define HC_GAS_AVERAGE 4 |
| // limit height control gas pd-control output |
| LIMIT_MIN_MAX(FilterHCGas, EE_Parameter.Hoehe_MinGas * STICK_GAIN, (MAX_GAS - 20) * STICK_GAIN); |
| // set GasMischanteil to HeightControlGasFilter |
| if(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT) // old version |
| { |
| if(FilterHCGas > GasMischanteil) FilterHCGas = GasMischanteil; // nicht mehr als Gas |
| } |
| GasMischanteil = FilterHCGas; |
| } |
| }// EOF height control active |
| else // HC not active |
| { |
| //update hoover gas stick value when HC is not active |
| if(!EE_Parameter.Hoehe_StickNeutralPoint) |
| { |
| StickGasHoover = HooverGas/STICK_GAIN; // rescale back to stick value |
| StickGasHoover = (StickGasHoover * UBat) / BattLowVoltageWarning; |
| } |
| else StickGasHoover = EE_Parameter.Hoehe_StickNeutralPoint; |
| if(StickGasHoover < 70) StickGasHoover = 70; |
| else if(StickGasHoover > 150) StickGasHoover = 150; |
| FilterHCGas = GasMischanteil; |
| } |
| // Hoover gas estimation by averaging gas control output on small z-velocities |
| // this is done only if height contol option is selected in global config and aircraft is flying |
| if((MikroKopterFlags & FLAG_FLY) && !(MikroKopterFlags & FLAG_NOTLANDUNG)) |
| { |
| if(HooverGasFilter == 0) HooverGasFilter = HOOVER_GAS_AVERAGE * (unsigned long)(GasMischanteil); // init estimation |
| if(abs(VarioMeter) < 100) // only on small vertical speed |
| { |
| tmp_long2 = (int32_t)GasMischanteil; // take current thrust |
| tmp_long2 *= CosAttitude; // apply attitude projection |
| tmp_long2 /= 8192; |
| // average vertical projected thrust |
| if(modell_fliegt < 2000) // the first 4 seconds |
| { // reduce the time constant of averaging by factor of 8 to get much faster a stable value |
| HooverGasFilter -= HooverGasFilter/(HOOVER_GAS_AVERAGE/8L); |
| HooverGasFilter += 8L * tmp_long2; |
| } |
| else if(modell_fliegt < 4000) // the first 8 seconds |
| { // reduce the time constant of averaging by factor of 4 to get much faster a stable value |
| HooverGasFilter -= HooverGasFilter/(HOOVER_GAS_AVERAGE/4L); |
| HooverGasFilter += 4L * tmp_long2; |
| } |
| else if(modell_fliegt < 8000) // the first 16 seconds |
| { // reduce the time constant of averaging by factor of 2 to get much faster a stable value |
| HooverGasFilter -= HooverGasFilter/(HOOVER_GAS_AVERAGE/2L); |
| HooverGasFilter += 2L * tmp_long2; |
| } |
| else //later |
| { |
| HooverGasFilter -= HooverGasFilter/HOOVER_GAS_AVERAGE; |
| HooverGasFilter += tmp_long2; |
| } |
| HooverGas = (int16_t)(HooverGasFilter/HOOVER_GAS_AVERAGE); |
| if(EE_Parameter.Hoehe_HoverBand) |
| { |
| int16_t band; |
| band = HooverGas / EE_Parameter.Hoehe_HoverBand; // the higher the parameter the smaller the range |
| HooverGasMin = HooverGas - band; |
| HooverGasMax = HooverGas + band; |
| } |
| else |
| { // no limit |
| HooverGasMin = 0; |
| HooverGasMax = 1023; |
| } |
| } |
| } |
| } |
| // EOF: |
| } // EOF: ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL |
| // limit gas to parameter setting |
| LIMIT_MIN(GasMischanteil, (MIN_GAS + 10) * STICK_GAIN); |
| if(GasMischanteil > (MAX_GAS - 20) * STICK_GAIN) GasMischanteil = (MAX_GAS - 20) * STICK_GAIN; |
| // ----------------------------------------------------------------------------------------------- |
| // sind alle BL-Ctrl angeschlossen ? ist eventuell ein Motor defekt ? |
| // ----------------------------------------------------------------------------------------------- |
| if(MissingMotor) // see twimaster.c line 184 |
| if(modell_fliegt > 1 && modell_fliegt < 50 && GasMischanteil > 0) |
| { |
| modell_fliegt = 1; |
| GasMischanteil = MIN_GAS; |
| } |
| //----------------------------------------------------------------------------------------------- |
| // Mischer und PI-Regler |
| //----------------------------------------------------------------------------------------------- |
| //----------------------------------------------------------------------------------------------- |
| // Gier-Anteil (yaw) |
| //----------------------------------------------------------------------------------------------- |
| GierMischanteil = MesswertGier - sollGier * STICK_GAIN; // Regler für Gier |
| #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 (pitch axis) |
| // ------------------------------------------------------------------------------------------------------ |
| DiffNick = MesswertNick - StickNick; // Differenz bestimmen |
| if(IntegralFaktor) SummeNick += IntegralNickMalFaktor - StickNick; // I-Anteil bei Winkelregelung |
| else SummeNick += DiffNick; // I-Anteil bei Heading Hold |
| if(SummeNick > (STICK_GAIN * 16000L)) SummeNick = (STICK_GAIN * 16000L); |
| if(SummeNick < -(16000L * STICK_GAIN)) SummeNick = -(16000L * STICK_GAIN); |
| pd_ergebnis_nick = DiffNick + SummeNick / Ki; // PI-Regler für Nick |
| // Motor Vorn |
| tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |
| if(pd_ergebnis_nick > tmp_int) pd_ergebnis_nick = tmp_int; |
| if(pd_ergebnis_nick < -tmp_int) pd_ergebnis_nick = -tmp_int; |
| // ------------------------------------------------------------------------------------------------------ |
| // Roll-Achse (roll axis) |
| // ------------------------------------------------------------------------------------------------------ |
| DiffRoll = MesswertRoll - StickRoll; // Differenz bestimmen |
| if(IntegralFaktor) SummeRoll += IntegralRollMalFaktor - StickRoll; // I-Anteil bei Winkelregelung |
| else SummeRoll += DiffRoll; // I-Anteil bei Heading Hold |
| if(SummeRoll > (STICK_GAIN * 16000L)) SummeRoll = (STICK_GAIN * 16000L); |
| if(SummeRoll < -(16000L * STICK_GAIN)) SummeRoll = -(16000L * STICK_GAIN); |
| pd_ergebnis_roll = DiffRoll + SummeRoll / Ki; // PI-Regler für Roll |
| tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |
| if(pd_ergebnis_roll > tmp_int) pd_ergebnis_roll = tmp_int; |
| if(pd_ergebnis_roll < -tmp_int) pd_ergebnis_roll = -tmp_int; |
| // ----------------------------------------------------------------------------------- |
| // kopterbalance |
| // ----------------------------------------------------------------------------------- |
| //------------------------------------------------------------------------------------ |
| // set the parameters with radio control |
| //------------------------------------------------------------------------------------ |
| if((PPM_in[2] < -60) && (ucflg2==0x01)) |
| { |
| ucflg2=0x00; |
| if(ucflg1 == 0x02) |
| { |
| ucflg1=0x00; |
| } |
| else ucflg1++; |
| } |
| if((PPM_in[2] > -20) && (PPM_in[2] < 20)) |
| { |
| ucflg2=0x01; |
| } |
| // ----------------------------------------------------------------------------------- |
| // set analog value at according parameter |
| // ----------------------------------------------------------------------------------- |
| if((PPM_in[5] < -80L) && (ucflg3==0x01)) |
| { |
| ucflg3=0x00; |
| if((ucflg1==0x00) || (ucflg1==0x01) || (ucflg1==0x02)) ipk[ucflg1] += PPM_in[3]; |
| } |
| if(PPM_in[5] > 80L) |
| { |
| ucflg3=0x01; |
| } |
| kp = ipk[0]; |
| kd = ipk[1]; |
| kdd = ipk[2]; |
| // ------------------------------------------------------------------------------------- |
| // set trust with radio control |
| // ----------------------------------------------------------------------------------- |
| thrust = PPM_in[1] + 127; // set turnrate speed |
| if(thrust<10) // limit min thrust |
| { |
| thrust = 10; |
| } |
| if(thrust>50) // limit max thrust |
| { |
| thrust = 50; |
| } |
| DebugOut.Analog[7] = thrust; |
| // -------------------------------------------------------------------------------------- |
| desiredAngle = 0; |
| angle = (int) (IntegralNick / (EE_Parameter.GyroAccFaktor * 4)); |
| gyroScaled = HiResNick; // Gyro |
| // -------------------------------------------------------------------------------------- |
| controllerP = (PPM_in[2]/4) + (kp * ((desiredAngle - angle)/16)); // proportional & manual control |
| if(controllerP > maxcontrollerP) maxcontrollerP = controllerP; |
| if(controllerP < mincontrollerP) mincontrollerP = controllerP; |
| // -------------------------------------------------------------------------------------- |
| controllerD = (-gyroScaled * kd)/512; |
| if(controllerD > maxcontrollerD) maxcontrollerD = controllerD; |
| if(controllerD < mincontrollerD) mincontrollerD = controllerD; |
| // -------------------------------------------------------------------------------------- |
| controllerDD = (gyroScaledOld - gyroScaled) * kdd; |
| filtersum = filtersum - filterDD + controllerDD; |
| filterDD = filtersum/8; |
| controllerDD = filterDD / 64; |
| gyroScaledOld = gyroScaled; |
| if(controllerDD > maxcontrollerDD) maxcontrollerDD = controllerDD; |
| if(controllerDD < mincontrollerDD) mincontrollerDD = controllerDD; |
| // -------------------------------------------------------------------------------------- |
| motorOutRear = thrust + controllerD + controllerDD + controllerP; |
| motorOutFront = thrust - controllerD - controllerDD - controllerP; |
| if(motorOutFront>50) motorOutFront = 50; |
| if(motorOutFront<1) motorOutFront = 0; |
| if(motorOutRear>50) motorOutRear = 50; |
| if(motorOutRear<1) motorOutRear = 0; |
| Motor[1] = motorOutRear; |
| Motor[0] = motorOutFront; |
| } |
| // *** EOF: MotorRegler() ******************************************************************************************************** |
| /branches/V0.76g_dk9nw_balancekopter/fc.h |
|---|
| 0,0 → 1,212 |
| /***************************************************************************************************************************** |
| * File: fc.h |
| * Purpose: header of fc.c |
| *****************************************************************************************************************************/ |
| #ifndef _FC_H |
| #define _FC_H |
| // ---------------------- definitions ---------------------- |
| #define STICK_GAIN 4 |
| #define FLAG_MOTOR_RUN 1 // diese Flags werden |
| #define FLAG_FLY 2 // bei Bedarf |
| #define FLAG_CALIBRATE 4 // auf die |
| #define FLAG_START 8 // Variable |
| #define FLAG_NOTLANDUNG 16 // "MikroKopterFlags" |
| #define FLAG_LOWBAT 32 // abgebildet |
| #define MAX_MOTORS 2 // gesetzt für Waage (max 12 möglich) |
| #define CHECK_MIN_MAX(wert,min,max) {if(wert < min) wert = min; else if(wert > max) wert = max;} |
| extern long GIER_GRAD_FAKTOR; // Abhängigkeit zwischen GyroIntegral und Winkel |
| extern volatile unsigned char MikroKopterFlags; |
| extern volatile unsigned int I2CTimeout; |
| extern unsigned char Sekunde,Minute; |
| extern unsigned int BaroExpandActive; |
| extern long IntegralNick,IntegralNick2; |
| extern long IntegralRoll,IntegralRoll2; |
| extern long Mess_IntegralNick,Mess_IntegralNick2; |
| extern long Mess_IntegralRoll,Mess_IntegralRoll2; |
| extern long IntegralAccNick,IntegralAccRoll; |
| extern volatile long Mess_Integral_Hoch; |
| extern long Integral_Gier,Mess_Integral_Gier,Mess_Integral_Gier2; |
| extern int KompassValue; |
| extern int KompassStartwert; |
| extern int KompassRichtung; |
| extern int TrimNick, TrimRoll; |
| extern long ErsatzKompass; |
| extern int ErsatzKompassInGrad; // Kompasswert in Grad |
| extern long HoehenWert; |
| extern long SollHoehe; |
| extern int MesswertNick,MesswertRoll,MesswertGier; |
| extern int AdNeutralNick,AdNeutralRoll,AdNeutralGier, Mittelwert_AccNick, Mittelwert_AccRoll; |
| extern int NeutralAccX, NeutralAccY,Mittelwert_AccHoch; |
| extern unsigned char HoehenReglerAktiv; |
| extern volatile float NeutralAccZ; |
| extern long Umschlag180Nick, Umschlag180Roll; |
| extern signed int ExternStickNick,ExternStickRoll,ExternStickGier; |
| extern unsigned char Parameter_UserParam1,Parameter_UserParam2,Parameter_UserParam3,Parameter_UserParam4,Parameter_UserParam5,Parameter_UserParam6,Parameter_UserParam7,Parameter_UserParam8; |
| extern int NaviAccNick,NaviAccRoll,NaviCntAcc; |
| extern unsigned int modell_fliegt; |
| //------------------------- functions --------------- |
| void MotorRegler(void); |
| void SendMotorData(void); |
| void CalibrierMittelwert(void); |
| void Mittelwert(void); |
| void SetNeutral(void); |
| void Piep(unsigned char Anzahl, unsigned int dauer); |
| extern unsigned char h,m,s; |
| extern volatile unsigned char Timeout ; |
| extern unsigned char CosinusNickWinkel, CosinusRollWinkel; |
| extern int DiffNick,DiffRoll; |
| extern int Poti1, Poti2, Poti3, Poti4; |
| extern volatile unsigned char SenderOkay; |
| extern volatile unsigned char SenderRSSI; |
| extern unsigned char RequiredMotors; |
| extern int StickNick,StickRoll,StickGier; |
| extern char MotorenEin; |
| extern void DefaultKonstanten1(void); |
| extern void DefaultKonstanten2(void); |
| extern void DefaultKonstanten3(void); |
| extern void DefaultStickMapping(void); |
| #define STRUCT_PARAM_LAENGE sizeof(EE_Parameter) |
| struct mk_param_struct |
| { |
| unsigned char Kanalbelegung[8]; // 01-08 Nick[0]=3, Roll[1]=2,Gas[2]=1, Gier[3]=4, K_POTI1[4], K_POTI2[5], K_POTI3[6], K_POTI4[7] steht auf EEPROM auf Adresse 80-87 |
| unsigned char GlobalConfig; // 09 0x80=Rotationrate Limiter, 0x40=ACHSENKOPPLUNG_AKTIV, 0x20=GPS aktiv, 0x10=Orientation fix, 0x08=Kompass_aktiv, 0x04=Heading Hold aktiv, 0x02=?, 0x01=Höhenregler aktiv |
| unsigned char Hoehe_MinGas; // 10 Wert : 0-100 |
| unsigned char Luftdruck_D; // 11 Wert : 0-250 |
| unsigned char MaxHoehe; // 12 Wert : 0-32 |
| unsigned char Hoehe_P; // 13 Wert : 0-32 |
| unsigned char Hoehe_Verstaerkung; // 14 Wert : 0-50 |
| unsigned char Hoehe_ACC_Wirkung; // 15 Wert : 0-250 |
| unsigned char Hoehe_HoverBand; // 16 Wert : 0-250 |
| unsigned char Hoehe_GPS_Z; // 17 Wert : 0-250 |
| unsigned char Hoehe_StickNeutralPoint; // 18 Wert : 0-250 |
| unsigned char Stick_P; // 19 Wert : 1-6 |
| unsigned char Stick_D; // 20 Wert : 0-64 |
| unsigned char Gier_P; // 21 Wert : 1-20 |
| unsigned char Gas_Min; // 22 Wert : 0-32 |
| unsigned char Gas_Max; // 23 Wert : 33-250 |
| unsigned char GyroAccFaktor; // 24 Wert : 1-64 |
| unsigned char KompassWirkung; // 25 Wert : 0-32 |
| unsigned char Gyro_P; // 26 Wert : 10-250 |
| unsigned char Gyro_I; // 27 Wert : 0-250 |
| unsigned char Gyro_D; // 28 Wert : 0-250 |
| unsigned char Gyro_Gier_P; // 29 Wert : 10-250 |
| unsigned char Gyro_Gier_I; // 30 Wert : 0-250 |
| unsigned char UnterspannungsWarnung; // 31 Wert : 0-250 |
| unsigned char NotGas; // 32 Wert : 0-250 // Gaswert bei Empangsverlust |
| unsigned char NotGasZeit; // 33 Wert : 0-250 // Zeit, bis auf NotGas geschaltet wird, wegen Empfänger-Problemen |
| unsigned char UfoAusrichtung; // 34 X oder + Formation |
| unsigned char I_Faktor; // 35 Wert : 0-250 |
| unsigned char UserParam1; // 36 Wert : 0-250 |
| unsigned char UserParam2; // 37 Wert : 0-250 |
| unsigned char UserParam3; // 38 Wert : 0-250 |
| unsigned char UserParam4; // 39 Wert : 0-250 |
| unsigned char ServoNickControl; // 40 Wert : 0-250 // Stellung des Servos |
| unsigned char ServoNickComp; // 41 Wert : 0-250 // Einfluss Gyro/Servo |
| unsigned char ServoNickMin; // 42 Wert : 0-250 // Anschlag |
| unsigned char ServoNickMax; // 43 Wert : 0-250 // Anschlag |
| //---------------------------------- // --------------------- Seit V0.75 ------------------------------------- |
| unsigned char ServoRollControl; // 44 Wert : 0-250 // Stellung des Servos |
| unsigned char ServoRollComp; // 45 Wert : 0-250 |
| unsigned char ServoRollMin; // 46 Wert : 0-250 |
| unsigned char ServoRollMax; // 47 Wert : 0-250 |
| unsigned char ServoNickRefresh; // 48 |
| unsigned char LoopGasLimit; // 49 Wert: 0-250 max. Gas während Looping |
| unsigned char LoopThreshold; // 50 Wert: 0-250 Schwelle für Stickausschlag |
| unsigned char LoopHysterese; // 51 Wert: 0-250 Hysterese für Stickausschlag |
| unsigned char AchsKopplung1; // 52 Wert: 0-250 Faktor, mit dem Gier die Achsen Roll und Nick koppelt (NickRollMitkopplung) |
| unsigned char AchsKopplung2; // 53 Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
| unsigned char CouplingYawCorrection; // 54 Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
| unsigned char WinkelUmschlagNick; // 55 Wert: 0-250 180°-Punkt // EE_Parameter.WinkelUmschlagNick = 85; // TurnOverNick |
| unsigned char WinkelUmschlagRoll; // 56 Wert: 0-250 180°-Punkt // EE_Parameter.WinkelUmschlagRoll = 85; // TurnOverRoll |
| unsigned char GyroAccAbgleich; // 57 1/k (Koppel_ACC_Wirkung) |
| unsigned char Driftkomp; // 58 |
| unsigned char DynamicStability; // 59 |
| unsigned char UserParam5; // 60 Wert : 0-250 |
| unsigned char UserParam6; // 61 Wert : 0-250 |
| unsigned char UserParam7; // 62 Wert : 0-250 |
| unsigned char UserParam8; // 63 Wert : 0-250 |
| //----------------------------------- // ---------------------- Output ----------------------------------------- |
| unsigned char J16Bitmask; // 64 for the J16 Output |
| unsigned char J16Timing; // 65 for the J16 Output |
| unsigned char J17Bitmask; // 66 for the J17 Output |
| unsigned char J17Timing; // 67 for the J17 Output |
| // ---------------------------------- // ---------------------- seit version V0.75c ---------------------------- |
| unsigned char WARN_J16_Bitmask; // 68 for the J16 Output |
| unsigned char WARN_J17_Bitmask; // 69 for the J17 Output |
| //----------------------------------- // ------------NaviCtrl---------------------------------------------------- |
| unsigned char NaviGpsModeControl; // 70 Parameter für das Naviboard |
| unsigned char NaviGpsGain; // 71 |
| unsigned char NaviGpsP; // 72 |
| unsigned char NaviGpsI; // 73 |
| unsigned char NaviGpsD; // 74 |
| unsigned char NaviGpsPLimit; // 75 |
| unsigned char NaviGpsILimit; // 76 |
| unsigned char NaviGpsDLimit; // 77 |
| unsigned char NaviGpsACC; // 78 |
| unsigned char NaviGpsMinSat; // 79 |
| unsigned char NaviStickThreshold; // 80 |
| unsigned char NaviWindCorrection; // 81 |
| unsigned char NaviSpeedCompensation; // 82 |
| unsigned char NaviOperatingRadius; // 83 |
| unsigned char NaviAngleLimitation; // 84 |
| unsigned char NaviPH_LoginTime; // 85 |
| //---------------------------------- // ---Ext.Ctrl------------- |
| unsigned char ExternalControl; // 86 for serial Control |
| unsigned char BitConfig; // 87 (war Loop-Cfg) Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts / wird getrennt behandelt |
| unsigned char ServoCompInvert; // 88 0x01 = Nick, 0x02 = Roll 0 oder 1 // WICHTIG!!! am Ende lassen |
| unsigned char ExtraConfig; // 89 bitcodiert |
| char Name[12]; // 90-101 |
| }; |
| //-------------------------------------------------------------------------------------------------------------------------------------------------- |
| extern struct mk_param_struct EE_Parameter; // ist in fc.c Zeile 112 deklariert worden |
| //-------------------------------------------------------------------------------------------------------------------------------------------------- |
| //---------------------------------------------------------------------------------------------------------------------- |
| struct |
| { |
| char Revision; // 1 |
| char Name[12]; // 2-13 siehe main.c Zeile 229 memcpy(Mixer.Name, "Quadro\0", 11); |
| signed char Motor[16][4]; // 14-77 für jeden Motor gibt es einen 4er Datensatz |
| } Mixer; |
| //---------------------------------------------------------------------------------------------------------------------- |
| extern unsigned char Parameter_Luftdruck_D; |
| extern unsigned char Parameter_MaxHoehe; |
| extern unsigned char Parameter_Hoehe_P; |
| extern unsigned char Parameter_Hoehe_ACC_Wirkung; |
| extern unsigned char Parameter_KompassWirkung; |
| extern unsigned char Parameter_Gyro_P; |
| extern unsigned char Parameter_Gyro_I; |
| extern unsigned char Parameter_Gier_P; |
| extern unsigned char Parameter_ServoNickControl; |
| extern unsigned char Parameter_ServoRollControl; |
| extern unsigned char Parameter_AchsKopplung1; |
| extern unsigned char Parameter_AchsKopplung2; |
| extern unsigned char Parameter_J16Bitmask; // for the J16 Output |
| extern unsigned char Parameter_J16Timing; // for the J16 Output |
| extern unsigned char Parameter_J17Bitmask; // for the J17 Output |
| extern unsigned char Parameter_J17Timing; // for the J17 Output |
| extern signed char MixerTable[MAX_MOTORS][4]; |
| extern unsigned char Motor[MAX_MOTORS]; |
| #endif |
| // *** EOF: _FC_H ******************************************************************************************************************** |
| /branches/V0.76g_dk9nw_balancekopter/gps.h |
|---|
| 0,0 → 1,12 |
| //----------------------------------------------------------------------------------------------------- |
| // soll entfernt werden |
| // gps.h |
| //----------------------------------------------------------------------------------------------------- |
| //extern signed int GPS_Nick; |
| //extern signed int GPS_Roll; |
| //extern signed int GPS_Nick2; |
| //extern signed int GPS_Roll2; |
| //void GPS_Neutral(void); |
| //void GPS_BerechneZielrichtung(void); |
| /branches/V0.76g_dk9nw_balancekopter/isqrt.S |
|---|
| 0,0 → 1,201 |
| ;-----------------------------------------------------------------------------; |
| ; Fast integer squareroot routines for avr-gcc project (C)ChaN, 2008 |
| ; http://elm-chan.org/docs/avrlib/sqrt32.S |
| ;-----------------------------------------------------------------------------; |
| ; uint16_t isqrt32 (uint32_t n); |
| ; uint8_t isqrt16 (uint16_t n); |
| ; uint16_t ihypot (int16_t x, int16_t y); |
| ;-----------------------------------------------------------------------------: |
| ; 32bit integer squareroot |
| ;-----------------------------------------------------------------------------; |
| ; uint16_t isqrt32 ( |
| ; uint32_t n |
| ; ); |
| ; |
| ; Return Value: |
| ; Squareroot of n. |
| ; |
| ; Size = 53 words |
| ; Clock = 532..548 cycles |
| ; Stack = 0 byte |
| .global isqrt32 |
| .func isqrt32 |
| isqrt32: |
| clr r0 |
| clr r18 |
| clr r19 |
| clr r20 |
| ldi r21, 1 |
| clr r27 |
| clr r30 |
| clr r31 |
| ldi r26, 16 |
| 1: lsl r22 |
| rol r23 |
| rol r24 |
| rol r25 |
| rol r0 |
| rol r18 |
| rol r19 |
| rol r20 |
| lsl r22 |
| rol r23 |
| rol r24 |
| rol r25 |
| rol r0 |
| rol r18 |
| rol r19 |
| rol r20 |
| brpl 2f |
| add r0, r21 |
| adc r18, r27 |
| adc r19, r30 |
| adc r20, r31 |
| rjmp 3f |
| 2: sub r0, r21 |
| sbc r18, r27 |
| sbc r19, r30 |
| sbc r20, r31 |
| 3: lsl r21 |
| rol r27 |
| rol r30 |
| andi r21, 0b11111000 |
| ori r21, 0b00000101 |
| sbrc r20, 7 |
| subi r21, 2 |
| dec r26 |
| brne 1b |
| lsr r30 |
| ror r27 |
| ror r21 |
| lsr r30 |
| ror r27 |
| ror r21 |
| mov r24, r21 |
| mov r25, r27 |
| ret |
| .endfunc |
| ;-----------------------------------------------------------------------------: |
| ; 16bit integer squareroot |
| ;-----------------------------------------------------------------------------; |
| ; uint8_t isqrt16 ( uint16_t n ); |
| ; |
| ; Return Value: |
| ; Squareroot of n. |
| ; |
| ; Size = 33 words |
| ; Clock = 181..189 cycles |
| ; Stack = 0 byte |
| .global isqrt16 |
| .func isqrt16 |
| isqrt16: |
| clr r18 |
| clr r19 |
| ldi r20, 1 |
| clr r21 |
| ldi r22, 8 |
| 1: lsl r24 |
| rol r25 |
| rol r18 |
| rol r19 |
| lsl r24 |
| rol r25 |
| rol r18 |
| rol r19 |
| brpl 2f |
| add r18, r20 |
| adc r19, r21 |
| rjmp 3f |
| 2: sub r18, r20 |
| sbc r19, r21 |
| 3: lsl r20 |
| rol r21 |
| andi r20, 0b11111000 |
| ori r20, 0b00000101 |
| sbrc r19, 7 |
| subi r20, 2 |
| dec r22 |
| brne 1b |
| lsr r21 |
| ror r20 |
| lsr r21 |
| ror r20 |
| mov r24, r20 |
| ret |
| .endfunc |
| ;-----------------------------------------------------------------------------: |
| ; 16bit integer hypot (megaAVR is required) |
| ;-----------------------------------------------------------------------------; |
| ; uint16_t ihypot ( |
| ; int16_t x, |
| ; int16_t y |
| ; ); |
| ; |
| ; Return Value: |
| ; Squareroot of (x*x + y*y) |
| ; |
| ; Size = 42 words |
| ; Clock = 581..597 cycles |
| ; Stack = 0 byte |
| .global ihypot |
| .func ihypot |
| ihypot: |
| clr r26 |
| sbrs r25, 7 |
| rjmp 1f |
| com r24 |
| com r25 |
| adc r24, r26 |
| adc r25, r26 |
| 1: sbrs r23, 7 |
| rjmp 2f |
| com r22 |
| com r23 |
| adc r22, r26 |
| adc r23, r26 |
| 2: mul r22, r22 |
| movw r18, r0 |
| mul r23, r23 |
| movw r20, r0 |
| mul r22, r23 |
| add r19, r0 |
| adc r20, r1 |
| adc r21, r26 |
| add r19, r0 |
| adc r20, r1 |
| adc r21, r26 |
| mul r24, r24 |
| movw r30, r0 |
| mul r25, r25 |
| add r18, r30 |
| adc r19, r31 |
| adc r20, r0 |
| adc r21, r1 |
| mul r24, r25 |
| add r19, r0 |
| adc r20, r1 |
| adc r21, r26 |
| add r19, r0 |
| adc r20, r1 |
| adc r21, r26 |
| movw r24, r20 |
| movw r22, r18 |
| clr r1 |
| rjmp isqrt32 |
| .endfunc |
| /branches/V0.76g_dk9nw_balancekopter/isqrt.h |
|---|
| 0,0 → 1,11 |
| #ifndef _ISQRT_H |
| #define _ISQRT_H |
| #include <inttypes.h> |
| // coded in assembler file |
| extern uint16_t isqrt32(uint32_t n); |
| extern uint8_t isqrt16(uint16_t n); |
| extern uint16_t ihypot(int16_t x, int16_t y); |
| #endif // _ISQRT_H |
| /branches/V0.76g_dk9nw_balancekopter/led.c |
|---|
| 0,0 → 1,89 |
| /***************************************************************************************************************************** |
| * File: led.c |
| * |
| * Purpose: controlling LEDs |
| * |
| * Functions: void LED_Init(void) |
| * void LED_Update(void) |
| * |
| *****************************************************************************************************************************/ |
| #include "led.h" |
| #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; |
| //------------------------------------------------------------------------------------------------------ |
| // initialisiert die Schalt-Ausgänge PORTC2 und PORTC3 an SV2 (control of J16 & J17) |
| //------------------------------------------------------------------------------------------------------ |
| void LED_Init(void) |
| { |
| DDRC |= (1<<DDC2)|(1<<DDC3); |
| J16_OFF; |
| J17_OFF; |
| J16Blinkcount = 0; J16Mask = 128; |
| J17Blinkcount = 0; J17Mask = 128; |
| } |
| //------------------------------------------------------------------------------------------------------ |
| //-------------------------------------------------------------------------------------------------------------------------------- |
| // wird aus UpdateMotors() alle 2ms aufgerufen |
| // damit können am Mikrokopter aussen angebrachte LEDs gesteuert werden |
| // deren Ankoppelung geschieht über PORTC2 und PORTC3 |
| //-------------------------------------------------------------------------------------------------------------------------------- |
| void LED_Update(void) |
| { |
| static char delay = 0; |
| static unsigned char J16Bitmask = 0; |
| static unsigned char J17Bitmask = 0; |
| if(!delay--) // 10ms Intervall |
| { |
| delay = 4; |
| if(MikroKopterFlags & FLAG_LOWBAT || SenderOkay < 128) |
| { |
| J16Bitmask = EE_Parameter.WARN_J16_Bitmask; |
| J17Bitmask = EE_Parameter.WARN_J17_Bitmask; |
| } |
| else |
| { |
| J16Bitmask = EE_Parameter.J16Bitmask; |
| J17Bitmask = EE_Parameter.J17Bitmask; |
| } |
| if((EE_Parameter.BitConfig & CFG_MOTOR_BLINK) && !MotorenEin) {if(EE_Parameter.BitConfig & CFG_MOTOR_OFF_LED1) J16_ON; else J16_OFF;} |
| else |
| if((EE_Parameter.J16Timing > 250) && (Parameter_J16Timing > 220)) {if(J16Bitmask & 128) J16_ON; else J16_OFF;} |
| else |
| if((EE_Parameter.J16Timing > 250) && (Parameter_J16Timing < 10)) {if(J16Bitmask & 128) J16_OFF; else J16_ON;} |
| else |
| if(!J16Blinkcount--) |
| { |
| J16Blinkcount = Parameter_J16Timing-1; |
| if(J16Mask == 1) J16Mask = 128; else J16Mask /= 2; |
| if(J16Mask & J16Bitmask) J16_ON; else J16_OFF; |
| } |
| if((EE_Parameter.BitConfig & CFG_MOTOR_BLINK) && !MotorenEin) {if(EE_Parameter.BitConfig & CFG_MOTOR_OFF_LED2) J17_ON; else J17_OFF;} |
| else |
| if((EE_Parameter.J17Timing > 250) && (Parameter_J17Timing > 230)) {if(J17Bitmask & 128) J17_ON; else J17_OFF;} |
| else |
| if((EE_Parameter.J17Timing > 250) && (Parameter_J17Timing < 10)) {if(J17Bitmask & 128) J17_OFF; else J17_ON;} |
| else |
| if(!J17Blinkcount--) |
| { |
| J17Blinkcount = Parameter_J17Timing-1; |
| if(J17Mask == 1) J17Mask = 128; else J17Mask /= 2; |
| if(J17Mask & J17Bitmask) J17_ON; else J17_OFF; |
| } |
| } // Ende : 10ms Intervall |
| } |
| //-------------------------------------------------------------------------------------------------------------------------------- |
| /branches/V0.76g_dk9nw_balancekopter/led.h |
|---|
| 0,0 → 1,21 |
| /***************************************************************************************************************************** |
| * File: led.h |
| * Purpose: header of fc.c |
| *****************************************************************************************************************************/ |
| #ifndef _LED_H |
| #define _LED_H |
| #include <avr/io.h> |
| #define J16_ON PORTC |= (1<<PORTC2) |
| #define J16_OFF PORTC &= ~(1<<PORTC2) |
| #define J16_TOGGLE PORTC ^= (1<<PORTC2) |
| #define J17_ON PORTC |= (1<<PORTC3) |
| #define J17_OFF PORTC &= ~(1<<PORTC3) |
| #define J17_TOGGLE PORTC ^= (1<<PORTC3) |
| extern void LED_Init(void); |
| extern void LED_Update(void); |
| #endif |
| //------------------------------------------------------------------------------------------------------------------------------------- |
| /branches/V0.76g_dk9nw_balancekopter/main.c |
|---|
| 0,0 → 1,491 |
| /************************************************************************************************************************************** |
| * File: main.c |
| * |
| * Purpose: main function fot Flight Ctrl |
| * |
| * Functions: void CalMk3Mag(void) |
| * void LipoDetection(unsigned char print) |
| * int main(void) |
| * |
| * hardware: Flight Ctrl V1.3 |
| * |
| * Created: Feb 2013 |
| * |
| * Revisions: 1.00 experimental subversion for a balancekopter |
| * Achtung: nicht flugfähige Experimentalversion für eine Balenwaage mit Flight-CTRL |
| * siehe: http://forum.mikrokopter.de/topic-39231.html |
| * |
| * Copyright: (c)2013 www.mikrokopter.de |
| * All rights reserved. This software is available only for non-commercial or educational applications. |
| * Other uses are prohibited. This software may be modified only if |
| * the resulting code be made available publicly and the original author(s) given credit. |
| * |
| ************************************************************************************************************************************/ |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Copyright (c) Holger Buss, Ingo Busker |
| // + Nur für den privaten Gebrauch / NON-COMMERCIAL USE ONLY |
| // + 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 oder Nutzung 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 the sources to other systems or using the software on other systems (except 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 PlatinenVersion = 13; |
| unsigned char SendVersionToNavi = 1; |
| unsigned char BattLowVoltageWarning = 94; |
| unsigned int FlugMinuten = 0,FlugMinutenGesamt = 0; |
| //----------------------------------------------------------------------------------------------------- |
| // calibrate magnetic compass |
| //----------------------------------------------------------------------------------------------------- |
| 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) |
| { |
| beeptime = 1000; |
| } |
| else Piep(WinkelOut.CalcState,150); |
| } |
| DebugOut.Analog[19] = WinkelOut.CalcState; |
| } |
| //----------------------------------------------------------------------------------------------------- |
| //----------------------------------------------------------------------------------------------------- |
| // recognize the LiPo accumulators |
| // char print can be 0 or 1 |
| //----------------------------------------------------------------------------------------------------- |
| void LipoDetection(unsigned char print) |
| { |
| unsigned int timer; |
| if(print) printf("\n\rBatt:"); |
| if(EE_Parameter.UnterspannungsWarnung < 50) // automatic recognition of lipo cells -> default = 33 |
| { |
| timer = SetDelay(500); // |
| if(print) while (!CheckDelay(timer)); |
| if(UBat < 130) |
| { |
| BattLowVoltageWarning = 3 * EE_Parameter.UnterspannungsWarnung; |
| if(print) |
| { |
| Piep(3,200); |
| printf(" 3 Cells "); |
| } |
| } |
| else |
| { |
| BattLowVoltageWarning = 4 * EE_Parameter.UnterspannungsWarnung; |
| if(print) |
| { |
| Piep(4,200); |
| printf(" 4 Cells "); |
| } |
| } |
| } |
| else BattLowVoltageWarning = EE_Parameter.UnterspannungsWarnung; |
| if(print) printf(" Low warning level: %d.%d",BattLowVoltageWarning/10,BattLowVoltageWarning%10); |
| } |
| //----------------------------------------------------------------------------------------------------- |
| // -------------------------------------------------------------------------------------------------------------------------- |
| // main program starting here |
| // |
| // INPUT: None |
| // OUTPUT: None |
| // RETURN: 1 |
| // -------------------------------------------------------------------------------------------------------------------------- |
| int main(void) |
| { |
| unsigned int timer,i,timer2 = 0; |
| DDRB = 0x00; |
| PORTB = 0x00; |
| for(timer = 0; timer < 1000; timer++); // verzögern |
| PlatinenVersion = 13; |
| DDRC = 0x81; // SCL |
| PORTC = 0xFF; // Pullup SDA |
| DDRB = 0x1B; // LEDs und Druckoffset |
| PORTB = 0x01; // LED_Rot |
| DDRD = 0x3E; // Speaker & TXD & J3 J4 J5 |
| PORTD = 0x47; // LED |
| HEF4017R_OFF; // #define HEF4017R_OFF PORTC &= ~(1<<PORTC6) |
| MCUSR &=~(1<<WDRF); // MCUSR MCU Status Register provides information on which reset source caused an MCU reset |
| WDTCSR |= (1<<WDCE)|(1<<WDE); // WDTCSR Watchdog Timer Control Register |
| WDTCSR = 0; |
| beeptime = 2000; |
| StickGier = 0; PPM_in[K_GAS] = 0; StickRoll = 0; StickNick = 0; |
| GIER_GRAD_FAKTOR = 1291; |
| ROT_OFF; // rote LED aus // PORTB |= 0x01; |
| Timer_Init(); // goto timer0.c Zeile 40 |
| TIMER2_Init(); // goto timer0.c Zeile 170 |
| UART_Init(); // goto uart.c Zeile 488 |
| rc_sum_init(); // goto rc.c line 17 |
| ADC_Init(); // goto analog.c Zeile 26 |
| i2c_init(); // goto twimaster.c Zeile 16 |
| //--------------------------------------------------------------------------------------------------------------------------------- |
| // passes one time through the following code before arriving at the forever loop |
| //--------------------------------------------------------------------------------------------------------------------------------- |
| sei(); |
| printf("\n\r==================================="); |
| printf("\n\rFlightControl\n\rHardware:%d.%d\n\rSoftware:V%d.%d%c ",PlatinenVersion/10,PlatinenVersion%10, VERSION_MAJOR, VERSION_MINOR,VERSION_PATCH + 'a'); |
| printf("\n\rexperimental version"); |
| printf("\n\r==================================="); |
| GRN_ON; // switch green LED on // PORTB &=~0x02; |
| ReadParameterSet(3, (unsigned char *) &EE_Parameter.Kanalbelegung[0], 9); // read first 9 Bytes = chanal setup of radio control |
| // --------------------------------------------------------------------------------------------------------------------------- |
| // setup of mixer |
| // --------------------------------------------------------------------------------------------------------------------------- |
| // check MIXER-Revision at first EEPROM Byte = 1000 // #define EEPROM_ADR_MIXER_TABLE 1000 |
| // if((eeprom_read_byte(&EEPromArray[EEPROM_ADR_MIXER_TABLE]) == MIXER_REVISION) && (eeprom_read_byte(&EEPromArray[EEPROM_ADR_VALID]) != 0xff)) |
| if((eeprom_read_byte(&EEPromArray[1000]) == 1) && (eeprom_read_byte(&EEPromArray[1]) != 0xFF)) // aufgelöst |
| { |
| unsigned char i; // Settings via Koptertool zurücksetzen |
| RequiredMotors = 0; |
| eeprom_read_block(&Mixer, &EEPromArray[EEPROM_ADR_MIXER_TABLE], sizeof(Mixer)); |
| for(i=0; i<16;i++) { if(Mixer.Motor[i][0] > 0) RequiredMotors++;} |
| } |
| else // default |
| { |
| unsigned char i; |
| printf("\n\rerzeugt default Mixer Table"); |
| for(i=0; i<16;i++) { Mixer.Motor[i][0] = 0;Mixer.Motor[i][1] = 0;Mixer.Motor[i][2] = 0;Mixer.Motor[i][3] = 0;}; |
| // default = Quadro |
| // number 64 is equivalent to 100% |
| // GasMischanteil pd_ergebnis_nick pd_ergebnis_roll GierMischanteil |
| Mixer.Motor[0][0] = 64; Mixer.Motor[0][1] = +64; Mixer.Motor[0][2] = 0; Mixer.Motor[0][3] = +64; // vorne |
| Mixer.Motor[1][0] = 64; Mixer.Motor[1][1] = -64; Mixer.Motor[1][2] = 0; Mixer.Motor[1][3] = +64; // hinten |
| Mixer.Motor[2][0] = 64; Mixer.Motor[2][1] = 0; Mixer.Motor[2][2] = -64; Mixer.Motor[2][3] = -64; // rechts |
| Mixer.Motor[3][0] = 64; Mixer.Motor[3][1] = 0; Mixer.Motor[3][2] = +64; Mixer.Motor[3][3] = -64; // links |
| Mixer.Revision = MIXER_REVISION; // #define MIXER_REVISION 1 |
| memcpy(Mixer.Name, "Quadro\0", 11); |
| eeprom_write_block(&Mixer, &EEPromArray[EEPROM_ADR_MIXER_TABLE], sizeof(Mixer)); // sizeof(Mixer) = 77 |
| } |
| printf("\n\rMixer-Config: '%s' (%u Motors)",Mixer.Name,RequiredMotors); |
| // --------------------------------------------------------------------------------------------------------------------------- |
| // how many BL-Ctrls are connected ? |
| // --------------------------------------------------------------------------------------------------------------------------- |
| printf("\n\r...BL-Ctrl...."); |
| motorread = 0; |
| UpdateMotor = 0; |
| SendMotorData(); |
| while(!UpdateMotor); |
| motorread = 0; // read the first I2C-Datasets |
| timer = SetDelay(2000); // sets |
| for(i=0; i < MAX_MOTORS; i++) // #define MAX_MOTORS 4 |
| { |
| UpdateMotor = 0; |
| SendMotorData(); // goto fc.c line 460 |
| while(!UpdateMotor); |
| if(Mixer.Motor[i][0] > 0) // wait maximum 2 sec to wake up the BL-Ctrls |
| { |
| while(!CheckDelay(timer) && !MotorPresent[i]) |
| { |
| UpdateMotor = 0; |
| SendMotorData(); |
| while(!UpdateMotor); |
| } |
| } |
| if(MotorPresent[i]) printf("%d ",i+1); |
| } |
| for(i=0; i < MAX_MOTORS; i++) |
| { |
| if(!MotorPresent[i] && Mixer.Motor[i][0] > 0) // #define MAX_MOTORS 4 |
| { |
| printf("\n\r\n\r!! missing BL-CTRL: %d !!",i+1); |
| ServoActive = 1; // just in case the FlightCtrl would be used as camera-stabilizer |
| } |
| MotorError[i] = 0; |
| } |
| printf("\n\r==================================="); |
| SendMotorData(); |
| // --------------------------------------------------------------------------------------------------------------------------- |
| // check, that the revision in EEPROM fits to actual software |
| // --------------------------------------------------------------------------------------------------------------------------- |
| if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_VALID]) != EE_DATENREVISION) // #define EEPROM_ADR_VALID 1 |
| { // #define EE_DATENREVISION 80 |
| DefaultKonstanten1(); // Funktion aus eeprom.c |
| printf("\n\rInit. EEPROM"); |
| for (unsigned char i=1;i<6;i++) // es gibt 5 verschiedene Settings |
| { |
| if(i==2) DefaultKonstanten2(); // Kamera |
| if(i==3) DefaultKonstanten3(); // Anfänger |
| if(i>3) DefaultKonstanten2(); // Kamera |
| // --------------------------------------------------------------------------------------------------------------------------- |
| // valid Setting ? |
| // --------------------------------------------------------------------------------------------------------------------------- |
| if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS]) < 12 && eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+1]) < 12 && eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+2]) < 12 && eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+3]) < 12) |
| { |
| EE_Parameter.Kanalbelegung[0] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+0]); // Nick // #define EEPROM_ADR_CHANNELS 80 |
| EE_Parameter.Kanalbelegung[1] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+1]); // Roll |
| EE_Parameter.Kanalbelegung[2] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+2]); // Gas |
| EE_Parameter.Kanalbelegung[3] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+3]); // Gier |
| EE_Parameter.Kanalbelegung[4] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+4]); // Poti 1 |
| EE_Parameter.Kanalbelegung[5] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+5]); // Poti 2 |
| EE_Parameter.Kanalbelegung[6] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+6]); // Poti 3 |
| EE_Parameter.Kanalbelegung[7] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+7]); // Poti 4 |
| if(i==1) printf(": Generating Default-Parameter using old Stick Settings"); |
| } else DefaultStickMapping(); |
| WriteParameterSet(i, (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE); // #define STRUCT_PARAM_LAENGE sizeof(EE_Parameter) |
| // WriteParameterSet(i, (unsigned char *) &EE_Parameter.Kanalbelegung[0], 101)); |
| } |
| SetActiveParamSetNumber(3); // default Setting ist Beginner |
| eeprom_write_byte(&EEPromArray[EEPROM_ADR_VALID], EE_DATENREVISION); // #define EE_DATENREVISION 80 |
| // eeprom_write_byte(&EEPromArray[1], 80); // aufgelöst |
| } // EOF : check existing revision at EEPROM |
| FlugMinuten = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_MINUTES2]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_MINUTES2+1]); |
| FlugMinutenGesamt = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_MINUTES]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_MINUTES+1]); |
| if(FlugMinutenGesamt == 0xFFFF || FlugMinuten == 0xFFFF) // Flugminuten sind am überlaufen - zurücksetzen |
| { |
| FlugMinuten = 0; |
| FlugMinutenGesamt = 0; |
| } |
| printf("\n\rFlight-time %u min Total:%u min" ,FlugMinuten,FlugMinutenGesamt); |
| if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) > 4) |
| { |
| printf("\n\rACC noch nicht calibriert !"); |
| } |
| ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE); // #define STRUCT_PARAM_LAENGE sizeof(EE_Parameter) |
| printf("\n\rUsing parameterset %d", GetActiveParamSetNumber()); |
| if(EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG) |
| { |
| printf("\n\rKalibrieren des Drucksensors."); |
| timer = SetDelay(1000); // |
| SucheLuftruckOffset(); |
| while (!CheckDelay(timer)); // pause 1 sec |
| printf("OK\n\r"); |
| } |
| SetNeutral(); // Nullwerte ermitteln und Startwerte festlegen - goto fc.c line 162 |
| ROT_OFF; // redLED off // PORTB |= 0x01; |
| beeptime = 2000; |
| ExternControl.Digital[0] = 0x55; // externe Steuerung per serieller Schnittstelle - siehe uart.h |
| printf("\n\rControl: "); |
| if (EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) printf("HeadingHold"); |
| else printf("normaler (ACC-Mode)"); |
| LcdClear(); |
| I2CTimeout = 5000; // watchdog set up to 10 sec |
| WinkelOut.Orientation = 1; |
| LipoDetection(1); // Lipos should be detected now |
| printf("\n\r===================================\n\r"); |
| timer = SetDelay(2000); |
| // -------------------------------- end of main() prelude --------------------------------------------------------------------- |
| //---------------------------------------------------------------------------------------------------------------------- |
| // forever loop of main program |
| //---------------------------------------------------------------------------------------------------------------------- |
| while(1) |
| { |
| if(UpdateMotor && AdReady) // motor is updated every 2 ms and ADC is already passed |
| { |
| UpdateMotor=0; // reset and wait fpr the next 2ms timed trigger from timer0 IR |
| if(WinkelOut.CalcState) CalMk3Mag(); // In diesem Spezial-Fall soll der Kompass kalibriert werden |
| else MotorRegler(); // Im Normalfall Sollwerte für die Motoren berechnen = goto fc.c line 541 |
| SendMotorData(); // Sollwerte an die Motorren senden -> fc.c Zeile 465 |
| ROT_OFF; // switch red LED off // PORTB |= 0x01; |
| if(SenderOkay) SenderOkay--; // ICIE1: Timer/Counter1, Input Capture Interrupt Enable |
| else TIMSK1 |= _BV(ICIE1); // enable PPM-Input // TIMSK1 Timer/Counter1 Interrupt Mask Register -> ICIE1 OCIE1B OCIE1A TOIE1 |
| if(!--I2CTimeout || MissingMotor) // counting down I2CTimeout or motor is missing |
| { |
| if(!I2CTimeout) |
| { |
| i2c_reset(); |
| I2CTimeout = 5; |
| DebugOut.Analog[28]++; // I2C-Error |
| } |
| if((BeepMuster == 0xffff) && MotorenEin) |
| { |
| beeptime = 10000; |
| BeepMuster = 0x0080; |
| } |
| } |
| else |
| { |
| ROT_OFF; // switch red LED off // PORTB |= 0x01; |
| } |
| if( 1 && (!UpdateMotor || !MotorenEin)) |
| { |
| DatenUebertragung(); |
| BearbeiteRxDaten(); |
| } |
| else BearbeiteRxDaten(); |
| // DatenUebertragung(); // where ist that contained? |
| // BearbeiteRxDaten(); // where ist that contained? |
| if(CheckDelay(timer)) // goto timer0.c line 65 |
| { |
| timer += 20; |
| if(PcZugriff) PcZugriff--; // flight-CTRL controlled by external PC |
| else |
| { |
| ExternControl.Config = 0; |
| ExternStickNick = 0; |
| ExternStickRoll = 0; |
| ExternStickGier = 0; |
| if(BeepMuster == 0xffff && SenderOkay == 0) |
| { |
| beeptime = 15000; |
| BeepMuster = 0x0c00; |
| } |
| } |
| if(UBat < BattLowVoltageWarning) // low battery |
| { |
| MikroKopterFlags |= FLAG_LOWBAT; |
| if(BeepMuster == 0xffff) |
| { |
| beeptime = 6000; |
| BeepMuster = 0x0300; |
| } |
| } |
| else MikroKopterFlags &= ~FLAG_LOWBAT; |
| //SPI_StartTransmitPacket(); // where ist that contained? |
| SendSPI = 4; |
| if(!MotorenEin) timer2 = 1450; // round it up to 30 sec |
| if(++timer2 == 2930) // one minute |
| { |
| timer2 = 0; |
| FlugMinuten++; |
| FlugMinutenGesamt++; |
| eeprom_write_byte(&EEPromArray[EEPROM_ADR_MINUTES2],FlugMinuten / 256); |
| eeprom_write_byte(&EEPromArray[EEPROM_ADR_MINUTES2+1],FlugMinuten % 256); |
| eeprom_write_byte(&EEPromArray[EEPROM_ADR_MINUTES],FlugMinutenGesamt / 256); |
| eeprom_write_byte(&EEPromArray[EEPROM_ADR_MINUTES+1],FlugMinutenGesamt % 256); |
| timer = SetDelay(20); // delay 20 ms |
| } |
| } |
| LED_Update(); // junmps erery 2ms to led.c line 32 |
| } // *** EOF : if(UpdateMotor && AdReady) |
| // *** EOF: if(!SendSPI) { SPI_TransmitByte(); } |
| } // End of endlessloop |
| return (1); |
| } |
| // *** EOF: main(void) ************************************************************************************************************ |
| /branches/V0.76g_dk9nw_balancekopter/main.h |
|---|
| 0,0 → 1,113 |
| /***************************************************************************************************************************** |
| * File: main.h |
| * Purpose: header of main.c |
| *****************************************************************************************************************************/ |
| #ifndef _MAIN_H |
| #define _MAIN_H |
| #define QUADRO |
| //--------------------------------------------------------------------------------- |
| // Quadro: |
| // 1 |
| // 4 3 |
| // 2 |
| //--------------------------------------------------------------------------------- |
| // Hier wird die Quarz Frequenz angegeben = 20 MHz |
| #if defined (__AVR_ATmega644__) |
| #define SYSCLK 20000000L //Quarz Frequenz in Hz |
| #endif |
| #if defined (__AVR_ATmega644P__) |
| #define SYSCLK 20000000L //Quarz Frequenz in Hz |
| #endif |
| // Definitionen für Hardware und LEDs |
| #define ROT_OFF {if((PlatinenVersion == 10)||(PlatinenVersion == 20)) PORTB &=~0x01; else PORTB |= 0x01;} // rote LED aus // PORTB0 aus |
| #define ROT_ON {if((PlatinenVersion == 10)||(PlatinenVersion == 20)) PORTB |= 0x01; else PORTB &=~0x01;} // rote LED an // PORTB0 ein |
| #define ROT_FLASH PORTB ^= 0x01 |
| #define GRN_OFF {if((PlatinenVersion < 12)) PORTB &=~0x02; else PORTB |= 0x02;} // grüne LED aus // PORTB1 aus |
| #define GRN_ON {if((PlatinenVersion < 12)) PORTB |= 0x02; else PORTB &=~0x02;} // grüne LED an // PORTB1 ein |
| #define GRN_FLASH PORTB ^= 0x02 |
| #define CFG_HOEHENREGELUNG 0x01 // Höhenregler aktiv |
| #define CFG_HOEHEN_SCHALTER 0x02 // Höhenschalter - wo wird dieser Parameter geschaltet ? |
| #define CFG_HEADING_HOLD 0x04 // Heading Hold aktiv |
| #define CFG_KOMPASS_AKTIV 0x08 // Kompass aktiv |
| #define CFG_KOMPASS_FIX 0x10 // Orientation fix |
| #define CFG_GPS_AKTIV 0x20 // GPS aktiv |
| #define CFG_ACHSENKOPPLUNG_AKTIV 0x40 // Axis Decoupling |
| #define CFG_DREHRATEN_BEGRENZER 0x80 // Rotationraten Limiter |
| #define CFG_LOOP_OBEN 0x01 |
| #define CFG_LOOP_UNTEN 0x02 |
| #define CFG_LOOP_LINKS 0x04 |
| #define CFG_LOOP_RECHTS 0x08 |
| #define CFG_MOTOR_BLINK 0x10 |
| #define CFG_MOTOR_OFF_LED1 0x20 |
| #define CFG_MOTOR_OFF_LED2 0x40 |
| #define CFG_RES4 0x80 |
| #define CFG2_HEIGHT_LIMIT 0x01 |
| #define CFG2_VARIO_BEEP 0x02 |
| #define CFG_SENSITIVE_RC 0x04 |
| #define J3High PORTD |= 0x20 // Servosausgang PORTPD5 |
| #define J3Low PORTD &= ~0x20 |
| #define J4High PORTD |= 0x10 // Servosausgang PORTPD4 |
| #define J4Low PORTD &= ~0x10 |
| #define J5High PORTD |= 0x08 // Servosausgang PORTPD3 |
| #define J5Low PORTD &= ~0x08 |
| //------------------------------------------ extern Variables -------------------------------- |
| extern volatile unsigned char SenderOkay; |
| extern unsigned char BattLowVoltageWarning; |
| extern unsigned char CosinusNickWinkel, CosinusRollWinkel; |
| extern unsigned char PlatinenVersion; |
| extern unsigned char SendVersionToNavi; |
| extern unsigned int FlugMinuten,FlugMinutenGesamt; |
| //----------------------------------------- declaration of functions -------------------------- |
| void LipoDetection(unsigned char print); |
| //---------------------------------------------------------------------------------------------- |
| #include <stdlib.h> |
| #include <string.h> |
| #include <avr/io.h> |
| #include <avr/pgmspace.h> |
| #include <avr/interrupt.h> |
| #include <avr/eeprom.h> |
| #include <avr/boot.h> |
| #include <avr/wdt.h> |
| #include "old_macros.h" |
| #include "_Settings.h" |
| #include "printf_P.h" |
| #include "timer0.h" |
| #include "uart.h" |
| #include "analog.h" |
| #include "eeprom.h" |
| #include "twimaster.h" |
| #include "menu.h" |
| #include "rc.h" |
| #include "fc.h" |
| #include "led.h" |
| // #define DEBUG_DISPLAY_INTERVALL 123 // in ms |
| #define DELAY_US(x) ((unsigned int)( (x) * 1e-6 * F_CPU )) |
| #endif |
| // *** EOF: main.h *********************************************************************************************************** |
| /branches/V0.76g_dk9nw_balancekopter/menu.c |
|---|
| 0,0 → 1,207 |
| /***************************************************************************************************************************** |
| * File: menu.c |
| * |
| * Purpose: collecting ADC analog inputs from PORTA |
| * |
| * Functions: void ADC_Init(void) |
| * void SucheLuftruckOffset(void) |
| * void SucheGyroOffset(void) |
| * ISR(ADC_vect) |
| * |
| *****************************************************************************************************************************/ |
| #include "main.h" |
| #include "menu.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] = "cq de dk9nw"; |
| unsigned char DispPtr = 0; |
| unsigned char MaxMenue = 14; |
| unsigned char MenuePunkt = 0; |
| unsigned char RemoteKeys = 0; |
| #define KEY1 0x01 |
| #define KEY2 0x02 |
| #define KEY3 0x04 |
| #define KEY4 0x08 |
| #define KEY5 0x10 |
| // ---------------------------------------------------------------------------------------------------------------------------- |
| // leert die LCD Anzeige |
| // ---------------------------------------------------------------------------------------------------------------------------- |
| void LcdClear(void) |
| { |
| unsigned char i; |
| for(i=0;i<80;i++) DisplayBuff[i] = ' '; |
| } |
| // ---------------------------------------------------------------------------------------------------------------------------- |
| // --------------------------------------------------------------------------------------------------------------------------- |
| // Anzeigen auf dem MK Toll auf dem dortigen LCD Display |
| // --------------------------------------------------------------------------------------------------------------------------- |
| void Menu(void) |
| { |
| if(MenuePunkt > MaxMenue) MenuePunkt = MaxMenue; // MaxMenue = 14; |
| if(RemoteKeys & KEY1) { if(MenuePunkt) MenuePunkt--; else MenuePunkt = MaxMenue;} // go down |
| if(RemoteKeys & KEY2) { if(MenuePunkt == MaxMenue) MenuePunkt = 0; else MenuePunkt++;} // go up |
| if((RemoteKeys & KEY1) && (RemoteKeys & KEY2)) MenuePunkt = 0; // both keys pressed |
| LcdClear(); |
| 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_MAJOR, VERSION_MINOR, VERSION_PATCH +'a'); |
| LCD_printfxy(0,2,"Setting:%d %s",GetActiveParamSetNumber(),Mixer.Name); |
| if(I2CTimeout < 6) LCD_printfxy(0,3,"I2C ERROR!!!") |
| else |
| if(MissingMotor) LCD_printfxy(0,3,"Missing BL-Ctrl:%d!!",MissingMotor) |
| else LCD_printfxy(0,3,"----BL-Ctrl ok---"); |
| break; |
| case 1: |
| LCD_printfxy(0,0,"DAC Offset"); |
| LCD_printfxy(0,1,"%5i",AnalogOffsetNick); // = 113 |
| LCD_printfxy(0,2,"%5i",AnalogOffsetRoll); // = 114 |
| LCD_printfxy(0,3,"%5i",AnalogOffsetGier); // = 149 |
| break; |
| case 2: |
| LCD_printfxy(0,0,"GlobalConfig"); // |
| LCD_printfxy(0,1,"%5i",EE_Parameter.GlobalConfig); // |
| LCD_printfxy(0,2,"EEPROM310"); // |
| unsigned char c47 = eeprom_read_byte(&EEPromArray[310]); // |
| LCD_printfxy(0,3,"%5i",c47); // |
| break; |
| case 3: |
| LCD_printfxy(0,0,"K1:%4i K2:%4i ",PPM_in[1],PPM_in[2]); // K1 = -125 kann -125 bis 120 = Gas K2 = 2 kann 115 bis -102 = Roll |
| LCD_printfxy(0,1,"K3:%4i K4:%4i ",PPM_in[3],PPM_in[4]); // K3 = 0 kann -117 bis 120 = Nick K4 = 2 kann -120 bis +120 = Gier |
| LCD_printfxy(0,2,"K5:%4i ",PPM_in[5]); // K5 = -117 oder +120 = Kippschalter |
| /* |
| 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 4: |
| LCD_printfxy(0,0,"AdNeutral..."); |
| LCD_printfxy(0,1,"Nick: %5i",AdNeutralNick); // 8461 ständig steigend |
| LCD_printfxy(0,2,"Roll: %5i",AdNeutralRoll); // 8463 ständig steigend |
| LCD_printfxy(0,3,"Gier: %5i",AdNeutralGier); // 1039 |
| break; |
| case 5: |
| LCD_printfxy(0,0,"Gyro - Sensor"); |
| LCD_printfxy(0,1,"Nick %4i (%3i)",AdWertNick - AdNeutralNick/8, AdNeutralNick/16); // = 1 (564) |
| LCD_printfxy(0,2,"Roll %4i (%3i)",AdWertRoll - AdNeutralRoll/8, AdNeutralRoll/16); // = 1 (555) |
| LCD_printfxy(0,3,"Gier %4i (%3i)",AdNeutralGier - AdWertGier, AdNeutralGier/2); // = 3 (322) |
| break; |
| case 6: |
| LCD_printfxy(0,0,"ACC - Sensor"); |
| LCD_printfxy(0,1,"Nick %4i (%3i)",AdWertAccNick,NeutralAccX); // = -4 (504) |
| LCD_printfxy(0,2,"Roll %4i (%3i)",AdWertAccRoll,NeutralAccY); // = 7 (487) |
| LCD_printfxy(0,3,"Gier %4i (%3i)",Mittelwert_AccHoch/*accumulate_AccHoch / messanzahl_AccHoch*/,(int)NeutralAccZ); // = 0 (713) |
| break; |
| case 7: |
| LCD_printfxy(0,1,"Spannung: %5i",UBat); // = 120 müsst 12V sein |
| LCD_printfxy(0,2,"Empf.Pegel:%5i",SenderOkay); // = 200 bei gutem Empfang = 0 bei keinem Empfang |
| break; |
| case 8: |
| LCD_printfxy(0,0,"Kompass..."); |
| LCD_printfxy(0,1,"Richtung: %4i",KompassRichtung); // = -75 wurde nach dem Start nach links gedreht |
| LCD_printfxy(0,2,"Value: %4i",KompassValue); // = 318 Anzeigewert des Kompass |
| LCD_printfxy(0,3,"Startwert: %4i",KompassStartwert); // = 34 ist die Differenz zwischen beiden Werten |
| break; |
| case 9: |
| LCD_printfxy(0,0,"Poti1: %3i",Poti1); // = 0 |
| LCD_printfxy(0,1,"Poti2: %3i",Poti2); // = 154 |
| LCD_printfxy(0,2,"Poti3: %3i",Poti3); // = 110 |
| LCD_printfxy(0,3,"Poti4: %3i",Poti4); // = 110 |
| break; |
| case 10: |
| LCD_printfxy(0,0,"Servo " ); |
| LCD_printfxy(0,1,"Setpoint %3i",Parameter_ServoNickControl); // = 100 |
| //LCD_printfxy(0,2,"Stellung: %3i",ServoValue); |
| LCD_printfxy(0,3,"Range:%3i-%3i",EE_Parameter.ServoNickMin,EE_Parameter.ServoNickMax); // 50-150 |
| break; |
| case 11: |
| 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 12: |
| LCD_printfxy(0,0,"BL-Ctrl Errors " ); |
| LCD_printfxy(0,1," %3d %3d %3d %3d ",MotorError[0],MotorError[1],MotorError[2],MotorError[3]); |
| LCD_printfxy(0,2," %3d %3d %3d %3d ",MotorError[4],MotorError[5],MotorError[6],MotorError[7]); |
| LCD_printfxy(0,3," %3d %3d %3d %3d ",MotorError[8],MotorError[9],MotorError[10],MotorError[11]); |
| break; |
| case 13: |
| LCD_printfxy(0,0,"BL-Ctrl found " ); |
| LCD_printfxy(0,1," %c %c %c %c ",MotorPresent[0] + '-',MotorPresent[1] + '-',MotorPresent[2] + '-',MotorPresent[3] + '-'); |
| LCD_printfxy(0,2," %c %c %c %c ",MotorPresent[4] + '-',MotorPresent[5] + '-',MotorPresent[6] + '-',MotorPresent[7] + '-'); |
| LCD_printfxy(0,3," %c - - -",MotorPresent[8] + '-'); |
| if(MotorPresent[9]) LCD_printfxy(4,3,"10"); |
| if(MotorPresent[10]) LCD_printfxy(8,3,"11"); |
| if(MotorPresent[11]) LCD_printfxy(12,3,"12"); |
| break; |
| case 14: |
| LCD_printfxy(0,0,"Flight-Time " ); |
| LCD_printfxy(0,1," %5umin",FlugMinuten); |
| LCD_printfxy(0,2,"Total:%5umin",FlugMinutenGesamt); |
| LCD_printfxy(13,3,"(reset)"); |
| if(RemoteKeys & KEY4) // #define KEY4 0x08 |
| { |
| FlugMinuten = 0; |
| eeprom_write_byte(&EEPromArray[EEPROM_ADR_MINUTES2],FlugMinuten / 256); // FlugMinuten gesamt auf Null setzen |
| eeprom_write_byte(&EEPromArray[EEPROM_ADR_MINUTES2+1],FlugMinuten % 256); |
| } |
| break; |
| default: |
| MaxMenue = MenuePunkt - 1; |
| MenuePunkt = 0; |
| break; |
| } |
| RemoteKeys = 0; |
| } |
| //------------------------------------------------------------------------------------------------------------------------------------ |
| /branches/V0.76g_dk9nw_balancekopter/menu.h |
|---|
| 0,0 → 1,19 |
| /***************************************************************************************************************************** |
| * File: menu.h |
| * Purpose: header of menu.c |
| *****************************************************************************************************************************/ |
| #ifndef _MENU_H |
| #define _MENU_H |
| extern void Menu(void); |
| extern void LcdClear(void); |
| extern char DisplayBuff[80]; |
| extern unsigned char DispPtr; |
| extern unsigned char MaxMenue; |
| extern unsigned char MenuePunkt; |
| extern unsigned char RemoteKeys; |
| #endif |
| //*** EOF: _MENU_H *********************************************************************************************************** |
| /branches/V0.76g_dk9nw_balancekopter/mymath.c |
|---|
| 0,0 → 1,134 |
| /************************************************************************************************************************************** |
| * File: mymath.c |
| * |
| * Purpose: mathematic functions |
| * |
| * Functions: int16_t c_sin_8192(int16_t angle) |
| * int16_t c_cos_8192(int16_t angle) |
| * int16_t c_atan2(int16_t y, int16_t x) |
| * uint32_t c_sqrt(uint32_t a) |
| * |
| * hardware: Flight Ctrl V1.3 |
| * |
| * Created: Feb 2013 |
| * |
| *************************************************************************************************************************************/ |
| #include "mymath.h" |
| #include <stdlib.h> |
| #include <avr/pgmspace.h> |
| const uint16_t pgm_sinlookup[91] PROGMEM = {0, 143, 286, 429, 571, 714, 856, 998, 1140, 1282, 1423, 1563, 1703, 1843, 1982, 2120, 2258, 2395, 2531, 2667, 2802, 2936, 3069, 3201, 3332, 3462, 3591, 3719, 3846, 3972, 4096, 4219, 4341, 4462, 4581, 4699, 4815, 4930, 5043, 5155, 5266, 5374, 5482, 5587, 5691, 5793, 5893, 5991, 6088, 6183, 6275, 6366, 6455, 6542, 6627, 6710, 6791, 6870, 6947, 7022, 7094, 7165, 7233, 7299, 7363, 7424, 7484, 7541, 7595, 7648, 7698, 7746, 7791, 7834, 7875, 7913, 7949, 7982, 8013, 8041, 8068, 8091, 8112, 8131, 8147, 8161, 8172, 8181, 8187, 8191, 8192}; |
| //---------------------------------------------------------------------------------------------------------------------------------- |
| // Sinus with argument in degree at an angular resolution of 1 degree and a discretisation of 13 bit. |
| //---------------------------------------------------------------------------------------------------------------------------------- |
| int16_t c_sin_8192(int16_t angle) |
| { |
| int8_t m,n; |
| int16_t sinus; |
| // avoid negative angles |
| if (angle < 0) |
| { |
| m = -1; |
| angle = abs(angle); |
| } |
| else m = +1; |
| // fold angle to intervall 0 to 359 |
| angle %= 360; |
| // check quadrant |
| if (angle <= 90) n=1; // first quadrant |
| else if ((angle > 90) && (angle <= 180)) {angle = 180 - angle; n = 1;} // second quadrant |
| else if ((angle > 180) && (angle <= 270)) {angle = angle - 180; n = -1;} // third quadrant |
| else {angle = 360 - angle; n = -1;} //fourth quadrant |
| // get lookup value |
| sinus = pgm_read_word(&pgm_sinlookup[angle]); |
| // calculate sinus value |
| return (sinus * m * n); |
| } |
| // ---------------------------------------------------------------------------------------------------------------------------------- |
| //---------------------------------------------------------------------------------------------------------------------------------- |
| // Cosinus with argument in degree at an angular resolution of 1 degree and a discretisation of 13 bit. |
| //---------------------------------------------------------------------------------------------------------------------------------- |
| int16_t c_cos_8192(int16_t angle) |
| { |
| return (c_sin_8192(90 - angle)); |
| } |
| // ---------------------------------------------------------------------------------------------------------------------------------- |
| const uint8_t pgm_atanlookup[346] PROGMEM = {0,1,2,3,4,4,5,6,7,8,9,10,11,11,12,13,14,15,16,17,17,18,19,20,21,21,22,23,24,24,25,26,27,27,28,29,29,30,31,31,32,33,33,34,35,35,36,36,37,37,38,39,39,40,40,41,41,42,42,43,43,44,44,45,45,45,46,46,47,47,48,48,48,49,49,50,50,50,51,51,51,52,52,52,53,53,53,54,54,54,55,55,55,55,56,56,56,57,57,57,57,58,58,58,58,59,59,59,59,60,60,60,60,60,61,61,61,61,62,62,62,62,62,63,63,63,63,63,63,64,64,64,64,64,64,65,65,65,65,65,65,66,66,66,66,66,66,66,67,67,67,67,67,67,67,68,68,68,68,68,68,68,68,69,69,69,69,69,69,69,69,69,70,70,70,70,70,70,70,70,70,71,71,71,71,71,71,71,71,71,71,71,72,72,72,72,72,72,72,72,72,72,72,73,73,73,73,73,73,73,73,73,73,73,73,73,73,74,74,74,74,74,74,74,74,74,74,74,74,74,74,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,76,76,76,76,76,76,76,76,76,76,76,76,76,76,76,76,76,76,76,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79}; |
| //---------------------------------------------------------------------------------------------------------------------------------- |
| // arcustangens returns degree in a range of +/. 180 deg |
| //---------------------------------------------------------------------------------------------------------------------------------- |
| int16_t c_atan2(int16_t y, int16_t x) |
| { |
| int16_t index, angle; |
| int8_t m; |
| if (!x && !y) return 0; //atan2(0, 0) is undefined |
| if (y < 0) m = -1; |
| else m = 1; |
| if (!x) return (90 * m); // atan2(y,0) = +/- 90 deg |
| index = (int16_t)(((int32_t)y * 64) / x);// calculate index for lookup table |
| if (index < 0) index = -index; |
| if (index < 346) angle = pgm_read_byte(&pgm_atanlookup[index]); // lookup for 0 deg to 79 deg |
| else if (index > 7334) angle = 90; // limit is 90 deg |
| else if (index > 2444) angle = 89; // 89 deg to 80 deg is mapped via intervalls |
| else if (index > 1465) angle = 88; |
| else if (index > 1046) angle = 87; |
| else if (index > 813) angle = 86; |
| else if (index > 664) angle = 85; |
| else if (index > 561) angle = 84; |
| else if (index > 486) angle = 83; |
| else if (index > 428) angle = 82; |
| else if (index > 382) angle = 81; |
| else angle = 80; // (index>345) |
| if (x > 0) return (angle * m); // 1st and 4th quadrant |
| else if ((x < 0) && (m > 0)) return (180 - angle); // 2nd quadrant |
| else return (angle - 180); // ( (x < 0) && (y < 0)) 3rd quadrant |
| } |
| // ---------------------------------------------------------------------------------------------------------------------------------- |
| //---------------------------------------------------------------------------------------------------------------------------------- |
| // Integer square root |
| //---------------------------------------------------------------------------------------------------------------------------------- |
| uint32_t c_sqrt(uint32_t a) |
| { |
| uint32_t rem = 0; |
| uint32_t root = 0; |
| uint8_t i; |
| for(i = 0; i < 16; i++) |
| { |
| root <<= 1; |
| rem = ((rem << 2) + (a >> 30)); |
| a <<= 2; |
| root++; |
| if(root <= rem) |
| { |
| rem -= root; |
| root++; |
| } |
| else root--; |
| } |
| return (root >> 1); |
| } |
| // ---------------------------------------------------------------------------------------------------------------------------------- |
| // *** EOF: ************************************************************************************************************************ |
| /branches/V0.76g_dk9nw_balancekopter/mymath.h |
|---|
| 0,0 → 1,16 |
| /***************************************************************************************************************************** |
| * File: mymath.h |
| * Purpose: header of mymath.c |
| *****************************************************************************************************************************/ |
| #ifndef _MYMATH_H |
| #define _MYMATH_H |
| #include <inttypes.h> |
| extern int16_t c_sin_8192(int16_t angle); |
| extern int16_t c_cos_8192(int16_t angle); |
| extern int16_t c_atan2(int16_t y, int16_t x); |
| extern uint32_t c_sqrt(uint32_t a); |
| #endif |
| // EOF: _MYMATH_H ************************************************************************************************************* |
| /branches/V0.76g_dk9nw_balancekopter/old_macros.h |
|---|
| 0,0 → 1,46 |
| //---------------------------------------------------------------------------------------------------------------------------------- |
| // For backwards compatibility only. |
| //---------------------------------------------------------------------------------------------------------------------------------- |
| #ifndef cbi |
| #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit)) |
| #endif |
| #ifndef sbi |
| #define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit)) |
| #endif |
| #ifndef inb |
| #define inb(sfr) _SFR_BYTE(sfr) |
| #endif |
| #ifndef outb |
| #define outb(sfr, val) (_SFR_BYTE(sfr) = (val)) |
| #endif |
| #ifndef inw |
| #define inw(sfr) _SFR_WORD(sfr) |
| #endif |
| #ifndef outw |
| #define outw(sfr, val) (_SFR_WORD(sfr) = (val)) |
| #endif |
| #ifndef outp |
| #define outp(val, sfr) outb(sfr, val) |
| #endif |
| #ifndef inp |
| #define inp(sfr) inb(sfr) |
| #endif |
| #ifndef BV |
| #define BV(bit) _BV(bit) |
| #endif |
| #ifndef PRG_RDB |
| #define PRG_RDB pgm_read_byte |
| #endif |
| //---------------------------------------------------------------------------------------------------------------------------------- |
| /branches/V0.76g_dk9nw_balancekopter/printf_P.c |
|---|
| 0,0 → 1,482 |
| //---------------------------------------------------------------------------------------------------------------------------------- |
| // 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.76g_dk9nw_balancekopter/printf_P.h |
|---|
| 0,0 → 1,24 |
| //---------------------------------------------------------------------------------------------------------------------------------- |
| // printf_P.h |
| //---------------------------------------------------------------------------------------------------------------------------------- |
| #ifndef _PRINTF_P_H_ |
| #define _PRINTF_P_H_ |
| #include <avr/pgmspace.h> |
| #define OUT_V24 0 |
| #define OUT_LCD 1 |
| void _printf_P (char, char const *fmt0, ...); |
| extern char PrintZiel; |
| #define printf_P(format, args...) _printf_P(OUT_V24,format , ## args) |
| #define printf(format, args...) _printf_P(OUT_V24,PSTR(format) , ## args) |
| #define LCD_printfxy(x,y,format, args...) { DispPtr = y * 20 + x; _printf_P(OUT_LCD,PSTR(format) , ## args);} |
| #define LCD_printf(format, args...) { _printf_P(OUT_LCD,PSTR(format) , ## args);} |
| #endif |
| //---------------------------------------------------------------------------------------------------------------------------------- |
| /branches/V0.76g_dk9nw_balancekopter/rc.c |
|---|
| 0,0 → 1,104 |
| /************************************************************************************************************************************** |
| * File: rc.c |
| * |
| * Purpose: Decoding of the radio control sum signal |
| * |
| * Functions: void rc_sum_init(void) |
| * ISR(TIMER1_CAPT_vect) |
| * |
| *************************************************************************************************************************************/ |
| #include "rc.h" |
| #include "main.h" |
| volatile int PPM_in[11]; // containing the stick signal of the according radio control chanel |
| volatile int PPM_diff[11]; // the differential stick signal of the appropriate radio control chanel |
| volatile char Channels; // containing the number of channels |
| volatile char tmpChannels = 0; // |
| volatile unsigned char NewPpmData = 1; // |
| //---------------------------------------------------------------------------------------------------------------------------------- |
| // The input capture function of Timer1 is used to decode the PPM sum signal |
| //---------------------------------------------------------------------------------------------------------------------------------- |
| void rc_sum_init(void) |
| { |
| //------------------------------------------------------------------------------------------------ |
| // TCCR1B = Timer/Counter1 Control Register B -> ICNC1 ICES1 WGM13 WGM12 CS12 CS11 CS10 |
| // |
| // ICES1: Input Capture Edge Select = activated |
| // ICNC1: Input Capture Noise Canceler = activated |
| // CSn2:0 Clock Select = 1:64 from prescaler |
| // |
| TCCR1B= (1<<ICES1)|(1<<ICNC1)|(1<<CS11)|(1<<CS10); |
| //------------------------------------------------------------------------------------------------ |
| // TIMSK1 = Timer/Counter1 Interrupt Mask Register -> ICIE1 OCIE1B OCIE1A TOIE1 |
| // ICIE1: Timer/Counter1, Input Capture Interrupt Enable |
| // |
| TIMSK1 |= (1<<ICIE1); |
| AdNeutralGier = 0; |
| AdNeutralRoll = 0; |
| AdNeutralNick = 0; |
| return; |
| } |
| //---------------------------------------------------------------------------------------------------------------------------------- |
| //---------------------------------------------------------------------------------------------------------------------------------- |
| // This function works with Timer1 to decode the radio control |
| //---------------------------------------------------------------------------------------------------------------------------------- |
| ISR(TIMER1_CAPT_vect) |
| { |
| static unsigned int AltICR=0; |
| signed int signal = 0; |
| signed int tmp; |
| static int index; |
| signal = (unsigned int) ICR1 - AltICR; // ICR1H and ICR1L Input Capture Register 1 |
| AltICR = ICR1; |
| if((signal > 1100) && (signal < 8000)) // Syncronisation break? (3.52 ms < signal < 25.6 ms) |
| { |
| Channels = index; |
| if(index >= 4) NewPpmData = 0; // zero is equal to new Data |
| index = 1; |
| } |
| else |
| { |
| if(index < 10) |
| { |
| if((signal > 250) && (signal < 687)) |
| { |
| signal -= 466; |
| if(abs(signal - PPM_in[index]) < 6) // good signal |
| { |
| if(SenderOkay < 200) SenderOkay += 10; |
| else SenderOkay = 200; |
| } |
| 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) J3High; else J3Low; // Servosignal an J3 anlegen |
| if(index == 6) J4High; else J4Low; // Servosignal an J4 anlegen |
| if(index == 7) J5High; else J5Low; // Servosignal an J5 anlegen |
| } |
| } |
| } |
| //---------------------------------------------------------------------------------------------------------------------------------- |
| /branches/V0.76g_dk9nw_balancekopter/rc.h |
|---|
| 0,0 → 1,25 |
| //------------------------------------------------------------------------------------------------------------------------------------- |
| // rc.c |
| //------------------------------------------------------------------------------------------------------------------------------------- |
| #ifndef _RC_H |
| #define _RC_H |
| #if defined (__AVR_ATmega644__) |
| #define TIMER_RELOAD_VALUE 250 |
| #endif |
| #if defined (__AVR_ATmega644P__) |
| #define TIMER_RELOAD_VALUE 250 |
| #endif |
| #define GAS PPM_in[2] |
| extern void rc_sum_init (void); |
| extern volatile int PPM_in[11]; // das Fernsteuer-Summensignal - decodiert // PPM_in[1] = Gas // PPM_in[2] = Roll // PPM_in[3] = Nick // PPM_in[4] = Gier |
| extern volatile int PPM_diff[11]; // das diffenzierte Stick-Signal |
| extern volatile unsigned char NewPpmData; |
| extern volatile char Channels,tmpChannels; |
| #endif //_RC_H |
| //------------------------------------------------------------------------------------------------------------------------------------- |
| /branches/V0.76g_dk9nw_balancekopter/timer0.c |
|---|
| 0,0 → 1,380 |
| /***************************************************************************************************************************** |
| * File: timer0.c |
| * |
| * Purpose: setup of timer0 and timer2 |
| * and delay functions |
| * |
| * Functions: void Timer_Init(void); |
| * void TIMER2_Init(void); |
| * void Delay_ms(unsigned int); |
| * void Delay_ms_Mess(unsigned int); |
| * |
| *****************************************************************************************************************************/ |
| // |
| #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 char ServoActive = 0; |
| unsigned int BeepMuster = 0xFFFF; |
| volatile int16_t ServoNickValue = 0; |
| volatile int16_t ServoRollValue = 0; |
| //--------------------------- |
| enum |
| { |
| STOP = 0, |
| CK = 1, |
| CK8 = 2, |
| CK64 = 3, |
| CK256 = 4, |
| CK1024 = 5, |
| T0_FALLING_EDGE = 6, |
| T0_RISING_EDGE = 7 |
| }; |
| //--------------------------- |
| //******************************************************************************************************************** |
| // Timer0 is running with "fast PWM" at OC0A=PORTB3 and OC0B=PORTB4 |
| // setup of Offset P_OFF1 and P_OFF2 for the air pressure sensor |
| //-------------------------------------------------------------------------------------------------------------------- |
| void Timer_Init(void) |
| { |
| tim_main = SetDelay(10); |
| // ---------------------------------------------------------------------------------------------------------------- |
| // TCCR0B Timer/Counter Control Register B containing: FOC0A FOC0B WGM02 CS02 CS01 CS0 |
| // |
| TCCR0B = CK8; // CK8 = 2 = clk/8 (From prescaler) one turnarround takes 0.1024ms |
| // ---------------------------------------------------------------------------------------------------------------- |
| // TCCR0A Timer/Counter Control Register A containing: COM0A1 COM0A0 COM0B1 COM0B0 WGM01 WGM00 |
| // |
| TCCR0A = (1<<COM0A1)|(1<<COM0B1)|3; // WGM02 WGM01 WGM00 = 011 = Fast PWM |
| OCR0A = 0; // output compare Register A for Counter 0 |
| OCR0B = 120; // output compare Register B for Counter 0 |
| TCNT0 = (unsigned char)-TIMER_RELOAD_VALUE; // reload // #define TIMER_RELOAD_VALUE 250 |
| // ---------------------------------------------------------------------------------------------------------------- |
| // TIMSK0 Timer/Counter Interrupt Mask Register containing: OCIE0B OCIE0A TOIE0 |
| // TOIE0: Timer/Counter0 Overflow Interrupt Enable - When the TOIE0 bit is written to one, and the I-bit in the Status Register is set |
| // |
| TIMSK0 |= (1<<TOIE0); |
| } |
| // **************************** EOF: void Timer_Init(void) ******************************************************************* |
| // ************************************************************************************************************************* |
| // this funktions returns the variable "CountMilliseconds" as it will be after the waiting time hase been exipired |
| // ------------------------------------------------------------------------------------------------------------------------- |
| unsigned int SetDelay(unsigned int t) |
| { |
| return(CountMilliseconds + t + 1); |
| } |
| // ************************************************************************************************************************* |
| // ************************************************************************************************************************* |
| // Hier wird also die Zeitdifferenz (t-CountsMilliseconds) mit der Maske 0x08000 maskiert. |
| // 0x8000 = 1000 0000 0000 0000 setzt also bis auf das höchste Bit der 16-bit Zeitdifferenz alle Bits auf 0. |
| // Bei einer Integerzahl codiert das höchst bit das Vorzeichen. Ist also t < CountMillisconds wird, |
| // so flippt das Vorzeichen-Bit auf 1 sonst ist es 0. Diese Eigenschaft wird hier genutzt. |
| // Das Ganze wird dann noch um 9 Bits nach rechts geshiftet, damit die Information im unteren Byte liegt, |
| // um beim Typecast des Rückgabewertes auf 8-Bit die Ändeurng im oberen Byte nicht abgeschnitten wird. |
| // |
| // Man könnte den Code auch so aufschreiben: |
| /* |
| char CheckDelay(unsigned int t) |
| { |
| int diff; |
| diff = int(t-CountMilliseconds); |
| if (diff < 0) return 255; |
| else return 0; |
| } |
| */ |
| // |
| char CheckDelay(unsigned int t) |
| { |
| return(((t - CountMilliseconds) & 0x8000) >> 9); // 0x8000 = 0b 1000 0000 0000 0000 |
| } |
| // ************************************************************************************************************************* |
| // ************************************************************************************************************************* |
| 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)) if(AdReady) {AdReady = 0; ANALOG_ON;} // #define ANALOG_ON ADCSRA=(1<<ADEN)|(1<<ADSC)|(0<<ADATE)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0)|(1<<ADIE) |
| } |
| // ************************************************************************************************************************* |
| // ************************************************************************************************************************* |
| // Interrupt due to overflow of Timer0 |
| // -------------------------------------- |
| ISR(TIMER0_OVF_vect) |
| { |
| static unsigned char cnt_1ms=1, cnt=0; // one turnarround of Timer0 takes 0.1024ms at 256 steps |
| unsigned char pieper_ein = 0; |
| if(SendSPI) SendSPI--; // |
| if(!cnt--) |
| { |
| cnt=9; // 10 steps * 0.1024ms -> 1ms |
| cnt_1ms++; |
| cnt_1ms %= 2; // modulo division |
| if(!cnt_1ms) UpdateMotor = 1; // all 2 ms the motor is updated |
| CountMilliseconds++; |
| } |
| if(beeptime >= 1) |
| { |
| beeptime--; |
| if(beeptime & BeepMuster) |
| { |
| pieper_ein = 1; |
| } |
| else pieper_ein = 0; |
| } |
| else |
| { |
| pieper_ein = 0; |
| BeepMuster = 0xFFFF; |
| } |
| if(pieper_ein) |
| { |
| PORTC |= (1<<7); // beeper is connected to PORTC.7 |
| } |
| else |
| { |
| PORTC &= ~(1<<7); |
| } |
| //------------------------------------------------------------------------------------------------------------------------ |
| // Compass works per PWM at PORTC4 |
| //------------------------------------------------------------------------------------------------------------------------ |
| if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) |
| { |
| if(PINC & 0x10) // PORTC4 is up |
| { |
| cntKompass++; |
| } |
| else // PORTC4 is doing down now -> check lenght of PWM pulse |
| { |
| if((cntKompass) && (cntKompass < 362)) // if value is positiv and less then 362 |
| { |
| cntKompass += cntKompass / 41; |
| if(cntKompass > 10) KompassValue = cntKompass - 10; else KompassValue = 0; // indicated value of compass |
| } |
| KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180; // differential direction |
| cntKompass = 0; // reset PWM counter |
| } |
| } |
| } |
| // ************************************************************************************************************************* |
| // ************************************************************************************************************************* |
| // Purpose: Initialize Timer 2 |
| // |
| // Timer 2 is used to generate the PWM at PD7 (J7) to control a camera servo for nick compensation. |
| // |
| //--------------------------------------------------------------------------------------------------------------------------------------- |
| void TIMER2_Init(void) |
| { |
| unsigned char sreg = SREG; // copy SREG Status Register |
| cli(); // disable all interrupts before reconfiguration |
| PORTD &= ~(1<<PORTD7); // set PD7 = OC2A to low = Servo PPM |
| //------------------------------------------------------------------------------------------ |
| // DDRC Port C Data Direction Register containing: DDC7 DDC6 DDC5 DDC4 DDC3 DDC2 DDC1 DDC0 |
| DDRC |= (1<<DDC6); // set PC6 as output (Reset for HEF4017) |
| HEF4017R_ON; // #define HEF4017R_ON PORTC |= (1<<PORTC6) |
| //------------------------------------------------------------------------------------------ |
| // TCCR2A = Timer/Counter 2 Control Register A containing: COM2A1 COM2A0 COM2B1 COM2B0 WGM21 WGM20 |
| // COM2A1:0: Compare Match Output A Mode. These bits control the Output Compare pin (OC2A) behavior |
| // COM2B1:0: Compare Match Output B Mode. These bits control the Output Compare pin (OC2B) behavior |
| TCCR2A &= ~((1<<COM2A1)|(1<<COM2A0)|(1<<COM2B1)|(1<<COM2B0)); // Normal port operation |
| // |
| // The counter counts from BOTTOM to TOP then restarts from BOTTOM. |
| // TOP is defined as OCR2A asxxx, MGM22:0 = 111. |
| TCCR2A |= (1<<WGM21)|(1<<WGM20); |
| //------------------------------------------------------------------------------------------ |
| // TCCR2B = Timer/Counter 2 Control Register B containing: FOC2A FOC2B WGM22 CS22 CS21 CS20 |
| // FOC2A: Force Output Compare A. The FOC2A bit is only active when the WGM bits specify a non-PWM mode |
| // FOC2B: Force Output Compare B. The FOC2B bit is only active when the WGM bits specify a non-PWM mode. |
| // CS22:0: Clock Select. The three Clock Select bits select the clock source to be used by the Timer/Counter |
| // Clock Select = 011 = SYSKLOCK/32 at 20MHz / 32 = 625 kHz |
| // The timer increments from 0x00 to OCR2A with an update rate of 625 kHz or 0.0016 ms |
| // |
| TCCR2B &= ~((1<<FOC2A)|(1<<FOC2B)|(1<<CS22)); |
| TCCR2B |= (1<<CS21)|(1<<CS20)|(1<<WGM22); |
| //------------------------------------------------------------------------------------------ |
| // Reset the Timer/Counter 2 |
| TCNT2 = 0; |
| OCR2A = 255; // Initialize the Output Compare Register A used for PWM generation on port PD7. |
| TCCR2A |= (1<<COM2A1); // Clear OC2A on Compare Match when up-counting. Set OC2A on Compare Match when down-counting |
| //------------------------------------------------------------------------------------------ |
| // TIMSK2 Timer/Counter2 Interrupt Mask Register containing: OCIE2B OCIE2A TOIE2 |
| // OCIE2B: Timer/Counter2 Output Compare Match B Interrupt Enable |
| // TOIE2: Timer/Counter2 Overflow Interrupt Enable |
| // |
| TIMSK2 &= ~((1<<OCIE2B)|(1<<TOIE2)); // Output Compare Match B Interrupt is dissabled as Overflow Interrupt |
| TIMSK2 |= (1<<OCIE2A); // Timer/Counter2 Output Compare Match A Interrupt is enabled |
| SREG = sreg; // recopy SREG Status Register |
| } |
| // ************************************************************************************************************************* |
| // ************************************************************************************************************************* |
| // Timer2 controlls the Position of Control Servos at |
| // OC2A=PORTD7 |
| //--------------------------------------------------------------------------------------------------------------------------------------------- |
| ISR(TIMER2_COMPA_vect) |
| { |
| // frame len 22.5 ms = 14063 * 1.6 us |
| // stop pulse: 0.3 ms = 188 * 1.6 us |
| // min servo pulse: 0.6 ms = 375 * 1.6 us |
| // max servo pulse: 2.4 ms = 1500 * 1.6 us |
| // resolution: 1500 - 375 = 1125 steps |
| #define IRS_RUNTIME 127 |
| #define PPM_STOPPULSE 188 |
| #define PPM_FRAMELEN (1757 * EE_Parameter.ServoNickRefresh) |
| #define MINSERVOPULSE 375 |
| #define MAXSERVOPULSE 1500 |
| #define SERVORANGE (MAXSERVOPULSE - MINSERVOPULSE) |
| static uint8_t PulseOutput = 0; |
| static uint16_t RemainingPulse = 0; |
| static uint16_t ServoFrameTime = 0; |
| //static uint8_t ServoIndex = 0; |
| #define MULTIPLYER 4 |
| static int16_t ServoNickOffset = (255 / 2) * MULTIPLYER; // initial value near center positon |
| //static int16_t ServoRollOffset = (255 / 2) * MULTIPLYER; // initial value near center positon |
| //------------------------------------------------------------------------------------------------------------------------ |
| // Nick servo state machine |
| //------------------------------------------------------------------------------------------------------------------------ |
| if(!PulseOutput) // pulse output complete |
| { |
| if(TCCR2A & (1<<COM2A0)) // we had a low pulse |
| { |
| TCCR2A &= ~(1<<COM2A0); // make a high pulse |
| RemainingPulse = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms |
| ServoNickOffset = (ServoNickOffset * 3 + (int16_t)Parameter_ServoNickControl * MULTIPLYER) / 4; // lowpass offset |
| ServoNickValue = ServoNickOffset; // offset (Range from 0 to 255 * 3 = 765) |
| if(EE_Parameter.ServoCompInvert & 0x01) |
| { // inverting movement of servo |
| ServoNickValue += (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) ); |
| } |
| else |
| { // non inverting movement of servo |
| ServoNickValue -= (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) ); |
| } |
| // limit servo value to its parameter range definition |
| if(ServoNickValue < ((int16_t)EE_Parameter.ServoNickMin * MULTIPLYER) ) |
| { |
| ServoNickValue = (int16_t)EE_Parameter.ServoNickMin * MULTIPLYER; |
| } |
| else if(ServoNickValue > ((int16_t)EE_Parameter.ServoNickMax * MULTIPLYER) ) |
| { |
| ServoNickValue = (int16_t)EE_Parameter.ServoNickMax * MULTIPLYER; |
| } |
| RemainingPulse += ServoNickValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position |
| ServoNickValue /= MULTIPLYER; |
| // range servo pulse width |
| if(RemainingPulse > MAXSERVOPULSE ) RemainingPulse = MAXSERVOPULSE; // upper servo pulse limit |
| else if(RemainingPulse < MINSERVOPULSE ) RemainingPulse = MINSERVOPULSE; // lower servo pulse limit |
| ServoFrameTime = RemainingPulse; // accumulate time for correct update rate |
| } |
| else // we had a high pulse |
| { |
| TCCR2A |= (1<<COM2A0); // make a low pulse |
| RemainingPulse = PPM_FRAMELEN - ServoFrameTime; |
| } |
| PulseOutput = 1; // set pulse output active |
| } |
| if(RemainingPulse > (255 + IRS_RUNTIME)) // General pulse output generator |
| { |
| OCR2A = 255; |
| RemainingPulse -= 255; |
| } |
| else |
| { |
| if(RemainingPulse > 255) // this is the 2nd last part |
| { |
| if((RemainingPulse - 255) < IRS_RUNTIME) |
| { |
| OCR2A = 255 - IRS_RUNTIME; |
| RemainingPulse -= 255 - IRS_RUNTIME; |
| } |
| else // last part > ISR_RUNTIME |
| { |
| OCR2A = 255; |
| RemainingPulse -= 255; |
| } |
| } |
| else // this is the last part |
| { |
| OCR2A = RemainingPulse; |
| RemainingPulse = 0; |
| PulseOutput = 0; // trigger to stop pulse |
| } |
| } // EOF: general pulse output generator |
| } |
| // *** EOF: ISR(TIMER2_COMPA_vect) *************************************************************************************** |
| /branches/V0.76g_dk9nw_balancekopter/timer0.h |
|---|
| 0,0 → 1,30 |
| /***************************************************************************************************************************** |
| * File: timer0.h |
| * Purpose: header of timer0.c |
| *****************************************************************************************************************************/ |
| #ifndef _TIMER0 |
| #define _TIMER0 |
| #define TIMER_TEILER CK8 |
| #define TIMER_RELOAD_VALUE 250 |
| #define HEF4017R_ON PORTC |= (1<<PORTC6) |
| #define HEF4017R_OFF PORTC &= ~(1<<PORTC6) |
| void Timer_Init(void); |
| void TIMER2_Init(void); |
| void Delay_ms(unsigned int); |
| void Delay_ms_Mess(unsigned int); |
| unsigned int SetDelay (unsigned int t); |
| char CheckDelay (unsigned int t); |
| extern volatile unsigned int CountMilliseconds; |
| extern volatile unsigned char UpdateMotor; |
| extern volatile unsigned int beeptime; |
| extern volatile unsigned int cntKompass; |
| extern unsigned int BeepMuster; |
| extern volatile unsigned char SendSPI, ServoActive; |
| extern volatile int16_t ServoNickValue; |
| extern volatile int16_t ServoRollValue; |
| #endif |
| //*** EOF: _TIMER0_H *********************************************************************************************************** |
| /branches/V0.76g_dk9nw_balancekopter/twimaster.c |
|---|
| 0,0 → 1,308 |
| /***************************************************************************************************************************** |
| * File: twimaster.c |
| * |
| * Purpose: setup of I2C (TWI) |
| * |
| * Functions: extern void i2c_init (void); // initialize I2C |
| * extern void i2c_start (void); // Start I2C |
| * extern void i2c_reset(void); // resert I2C |
| * extern void i2c_stop (void); // Stop I2C |
| * extern void i2c_write_byte (char byte); // write 1 Byte |
| * |
| *****************************************************************************************************************************/ |
| #include "twimaster.h" |
| #include "main.h" |
| volatile unsigned char twi_state = 0; |
| unsigned char motor=0; |
| unsigned char motorread=0, MissingMotor=0; |
| unsigned char motor_rx[16], motor_rx2[16]; |
| unsigned char MotorPresent[MAX_MOTORS]; // #define MAX_MOTORS 4 |
| unsigned char MotorError[MAX_MOTORS]; |
| unsigned int I2CError=0; |
| // ************************************************************************************************************************************* |
| // Initzialize I2C (TWI) |
| // |
| void i2c_init(void) |
| { |
| //-------------------------------------------------------------------------------------------------------------------- |
| // TWSR = TWI Status Register containing: TWS7 TWS6 TWS5 TWS4 TWS3 TWPS1 TWPS0 |
| // |
| TWSR = 0; // TWSR TWI Status Register |
| //-------------------------------------------------------------------------------------------------------------------- |
| // TWBR = TWI Bit Rate Register containing: TWBR7 TWBR6 TWBR5 TWBR4 TWBR3 TWBR2 TWBR1 TWBR0 |
| // set TWI Bit Rate Register // SYSCLK = 16000000 and SCL_CLOCK = 200000 |
| TWBR = ((SYSCLK/SCL_CLOCK)-16)/2; // selects the division factor for the bit rate generator |
| } |
| // ************************************************************************************************************************************* |
| //------------------------------------------------------------------------------------------------------------------------------------- |
| // Start I2C |
| // |
| void i2c_start(void) |
| { |
| // --------------------------------------------------------------------------- |
| // TWCR = TWI Control Register -> TWINT TWEA TWSTA TWSTO TWWC TWEN TWIE |
| // TWINT = TWI interrupt flag - to be set to one, to reset the TWINT Flag |
| // TWSTA = TWI start and should be set to one, that TWI is started as Master. |
| // TWEN = TWI enable -> now TWI/I2C is working properly. |
| // TWIE = TWI Interrupt enable is the TWIE Bit -> should be set |
| // |
| TWCR = (1<<TWINT)|(1<<TWSTA)|(1<<TWEN)|(1<<TWIE); |
| } |
| //------------------------------------------------------------------------------------------------------------------------------------- |
| //------------------------------------------------------------------------------------------------------------------------------------- |
| // Stop I2C |
| // |
| void i2c_stop(void) |
| { |
| // --------------------------------------------------------------------------- |
| // TWCR = TWI Control Register -> TWINT TWEA TWSTA TWSTO TWWC TWEN TWIE |
| // TWINT = TWI interrupt flag - to be set to one, to reset the TWINT Flag |
| // TWSTO = TWI STOP Condition Bit (TWSTO = 1) -> now will be stoped |
| // TWEN = TWI enable -> TWI/I2C is working properly |
| // |
| TWCR = (1<<TWINT)|(1<<TWSTO)|(1<<TWEN); |
| } |
| //------------------------------------------------------------------------------------------------------------------------------------- |
| //------------------------------------------------------------------------------------------------------------------------------------- |
| // I2C Reset |
| // |
| void i2c_reset(void) |
| { |
| i2c_stop(); |
| twi_state = 0; |
| motor = TWDR; // TWI Data Register (TWDR) siehe S 227 |
| motor = 0; // ist das letzte gesendete oder empfangene Byte |
| TWCR = 0x80; |
| TWAMR = 0; |
| TWAR = 0; |
| TWDR = 0; |
| TWSR = 0; |
| TWBR = 0; |
| i2c_init(); |
| i2c_start(); |
| i2c_write_byte(0); |
| } |
| //------------------------------------------------------------------------------------------------------------------------------------- |
| //--------------------------------------------------------------------------------------------------------------------------------------------- |
| // write via I2C |
| // |
| void i2c_write_byte(char byte) |
| { |
| TWSR = 0x00; |
| TWDR = byte; // sende dieses Byte via TWI Data Register |
| TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE); |
| } |
| //--------------------------------------------------------------------------------------------------------------------------------------------- |
| //--------------------------------------------------------------------------------------------------------------------------------------------- |
| // write byte via I2C |
| // |
| void I2C_WriteByte(int8_t byte) |
| { |
| // move byte to send into TWI Data Register |
| TWDR = byte; |
| // clear interrupt flag (TWINT = 1) |
| // enable i2c bus (TWEN = 1) |
| // enable interrupt (TWIE = 1) |
| TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE); |
| } |
| //--------------------------------------------------------------------------------------------------------------------------------------------- |
| //--------------------------------------------------------------------------------------------------------------------------------------------- |
| // ein Byte gelesen und sendet ACK |
| // |
| void I2C_ReceiveByte(void) |
| { |
| TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE) | (1<<TWEA); |
| } |
| //--------------------------------------------------------------------------------------------------------------------------------------------- |
| //--------------------------------------------------------------------------------------------------------------------------------------------- |
| // I2C empfing das letzte Byte und sendet kein ACK |
| // |
| void I2C_ReceiveLastByte(void) |
| { |
| TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE); |
| } |
| //--------------------------------------------------------------------------------------------------------------------------------------------- |
| //----------------------------------------------------------------------------------------------------------------------------------------------- |
| // Interrupt I2C |
| // |
| ISR (TWI_vect) |
| { |
| static unsigned char missing_motor; |
| switch(twi_state++) |
| { |
| //---------------------------------------------------------------------------------------------------------------------------- |
| // write Motor-Data |
| //---------------------------------------------------------------------------------------------------------------------------- |
| case 0: |
| while(Mixer.Motor[motor][0] <= 0 && motor < MAX_MOTORS) motor++; // #define MAX_MOTORS 2 |
| if(motor == MAX_MOTORS) // writing finished -> now read |
| { |
| motor = 0; |
| twi_state = 3; |
| if(motorread==0) i2c_write_byte(0x53); // Read-Adress of: Motor 1 = 0x53 / 2 = 0x55 / 3 = 0x57 / 4 = 0x59 |
| if(motorread==1) i2c_write_byte(0x57); |
| } |
| else |
| { |
| if(motor==0) i2c_write_byte(0x52); // Write-Adress of: Motor 1 = 0x52 / 2 = 0x54 / 3 = 0x56 / 4 = 0x58 |
| if(motor==1) i2c_write_byte(0x56); |
| } |
| break; |
| case 1: |
| i2c_write_byte(Motor[motor++]); |
| break; |
| case 2: |
| // TWSR TWI Status Register -> TWS7 TWS6 TWS5 TWS4 TWS3 - TWPS1 TWPS0 |
| // Data byte has been transmitted; NOT ACK has been received |
| if(TWSR == 0x30) |
| { |
| if(!missing_motor) missing_motor = motor; // |
| if(++MotorError[motor-1] == 0) MotorError[motor-1] = 255; // |
| } |
| i2c_stop(); |
| I2CTimeout = 10; // counted down every 2ms = watchdog |
| twi_state = 0; |
| i2c_start(); |
| break; |
| //---------------------------------------------------------------------------------------------------------- |
| // read Motor-Data |
| //---------------------------------------------------------------------------------------------------------- |
| case 3: |
| if(TWSR != 0x40) // Error? |
| { // send 1. Byte to be read |
| MotorPresent[motorread] = 0; |
| motorread++; |
| if(motorread >= MAX_MOTORS) motorread = 0; // #define MAX_MOTORS 2 |
| i2c_stop(); |
| twi_state = 0; |
| } |
| else |
| { |
| MotorPresent[motorread] = ('1' - '-') + motorread; |
| I2C_ReceiveByte(); |
| } |
| //MissingMotor = missing_motor; |
| MissingMotor = 0; |
| missing_motor = 0; |
| break; |
| case 4: // read 1. Byte and send 2. Byte |
| motor_rx[motorread] = TWDR; |
| I2C_ReceiveLastByte(); // no acknowledge |
| break; |
| case 5: |
| motor_rx2[motorread++] = TWDR; // read 2. Byte |
| if(motorread >= MAX_MOTORS) motorread = 0; // #define MAX_MOTORS 4 |
| i2c_stop(); |
| twi_state = 0; |
| break; |
| //---------------------------------------------------------------------------------------------------------------------------------- |
| // write Gyro-Offset |
| //---------------------------------------------------------------------------------------------------------------------------------- |
| case 8: |
| i2c_write_byte(0x98); // Address of DAC see DAC Manual page 17 |
| break; |
| case 9: |
| i2c_write_byte(0x10); // byte sent to chanel A |
| break; |
| case 10: |
| i2c_write_byte(AnalogOffsetNick); // offset sent to DAC |
| break; |
| case 11: |
| i2c_write_byte(0x80); // The 2nd Byte does not care = 0x80 (Dummy) |
| break; |
| case 12: |
| i2c_stop(); |
| I2CTimeout = 10; |
| i2c_start(); |
| break; |
| case 13: |
| i2c_write_byte(0x98); // Address of DAC see DAC Manual page 17 |
| break; |
| case 14: |
| i2c_write_byte(0x12); // byte sent to chanel B |
| break; |
| case 15: |
| i2c_write_byte(AnalogOffsetRoll); // offset sent to DAC |
| break; |
| case 16: |
| i2c_write_byte(0x80); // The 2nd Byte does not care = 0x80 (Dummy) |
| break; |
| case 17: |
| i2c_stop(); |
| I2CTimeout = 10; |
| i2c_start(); |
| break; |
| case 18: |
| i2c_write_byte(0x98); // Address of DAC see DAC Manual page 17 |
| break; |
| case 19: |
| i2c_write_byte(0x14); // byte sent to chanel C |
| break; |
| case 20: |
| i2c_write_byte(AnalogOffsetGier); // offset sent to DAC |
| break; |
| case 21: |
| i2c_write_byte(0x80); // The 2nd Byte does not care = 0x80 (Dummy) |
| break; |
| case 22: |
| i2c_stop(); |
| I2CTimeout = 10; |
| twi_state = 0; |
| break; |
| default: twi_state = 0; |
| break; |
| } |
| TWCR |= 0x80; // TWWC: TWI Write Collision Flag is set to 1 |
| } |
| //----------------------------------------------------------------------------------------------------------------------------------- |
| /branches/V0.76g_dk9nw_balancekopter/twimaster.h |
|---|
| 0,0 → 1,35 |
| /***************************************************************************************************************************** |
| * File: twimaster.h |
| * Purpose: header of twimaster.c |
| *****************************************************************************************************************************/ |
| #ifndef _I2C_MASTER_H |
| #define _I2C_MASTER_H |
| // ------------------------ definions for I2C --- |
| #define SCL_CLOCK 200000L |
| #define I2C_TIMEOUT 30000 |
| #define I2C_START 0x08 |
| #define I2C_REPEATED_START 0x10 |
| #define I2C_TX_SLA_ACK 0x18 |
| #define I2C_TX_DATA_ACK 0x28 |
| #define I2C_RX_SLA_ACK 0x40 |
| #define I2C_RX_DATA_ACK 0x50 |
| extern volatile unsigned char twi_state; |
| extern unsigned char motor,MissingMotor; |
| extern unsigned char motorread; |
| extern unsigned char motor_rx[]; |
| extern unsigned char MotorPresent[]; |
| extern unsigned char MotorError[]; |
| extern void i2c_init(void); // initialize I2C |
| extern void i2c_start(void); // start I2C |
| extern void i2c_stop(void); // stop I2C |
| extern void i2c_write_byte(char byte); // write 1 Byte |
| extern void i2c_reset(void); // reset |
| #endif |
| // *** EOF: I2C_MASTER_H ****************************************************************************************************** |
| /branches/V0.76g_dk9nw_balancekopter/uart.c |
|---|
| 0,0 → 1,625 |
| /************************************************************************************************************************************** |
| * File: uart.c |
| * |
| * Purpose: This program treats the RS232 USRAT interface |
| * |
| * Functions: void UART_Init(void) |
| * ISR(USART0_TX_vect) |
| * ISR(USART0_RX_vect) |
| * void AddCRC(unsigned int wieviele) |
| * void SendOutData(unsigned char cmd,unsigned char address, unsigned char BufferAnzahl, ...) |
| * void Decode64(void) |
| * void BearbeiteRxDaten(void) |
| * int uart_putchar(char c) |
| * void DatenUebertragung(void) |
| * |
| ******************************************************************************************************************************************/ |
| #include "main.h" |
| #include "uart.h" |
| #include <stdarg.h> |
| #include <string.h> |
| //------------------------ Definitionen --------- |
| #define FC_ADDRESS 1 |
| #define NC_ADDRESS 2 |
| #define MK3MAG_ADDRESS 3 |
| //---------------------------------------------Variables------- |
| unsigned char GetExternalControl = 0; |
| unsigned char DebugDisplayAnforderung1 = 0; |
| unsigned char DebugDisplayAnforderung = 0; |
| unsigned char DebugDataAnforderung = 0; |
| unsigned char GetVersionAnforderung = 0; |
| unsigned char GetPPMChannelAnforderung = 0; |
| unsigned char DisplayLine = 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 char *pRxData = 0; |
| unsigned char RxDataLen = 0; |
| unsigned volatile char PC_DebugTimeout = 0; |
| unsigned volatile char PC_MotortestActive = 0; |
| unsigned char DebugTextAnforderung = 255; |
| unsigned char PcZugriff = 100; |
| unsigned char MotorTest[16]; |
| unsigned char MeineSlaveAdresse = 1; // Flight-Ctrl |
| unsigned char ConfirmFrame; |
| int Debug_Timer,Kompass_Timer,Timer3D; |
| unsigned int DebugDataIntervall = 200, Intervall3D = 0; |
| //---------------------------------------structs---------- |
| struct str_DebugOut DebugOut; |
| struct str_ExternControl ExternControl; |
| struct str_VersionInfo VersionInfo; |
| struct str_WinkelOut WinkelOut; |
| struct str_Data3D Data3D; |
| const unsigned char ANALOG_TEXT[32][16] = |
| { |
| //1234567890123456 // -------------------------------------------------------------// siehe "Debugwerte zuordnen" in fc.c Zeile 1270 |
| "3D Nick-Winkel ", // 0 = IntegralNick / (EE_Parameter.GyroAccFaktor * 4); // = Winkel * 10 |
| "AccNick ", // 1 = Mittelwert_AccNick / 4; // = so um die Null |
| "maxcontrollerDD ", // 2 = // = so um die Null |
| "mincontrollerDD ", // 3 = // = so um die Null |
| "MesswertNick ", // 4 = MesswertGier; // = -450 |
| "HiResNick ", // 5 = auf Null bezogener Gyro Nickwert // = so um die Null |
| "AdWertAccNick ", // 6 = // = 713 |
| "Gas = thrust ", // 7 = Gas // = min 72 max 840 entspricht der Gashebelstellung |
| " ", // 8 = // = 0 bis 360 Grad jenachdem wie der MK steht |
| "Spannung V ", // 9 = UBat; // = 120 bei 12V |
| "Empfang ", // 10 = SenderOkay; // = 0 wenn Sender aus und ca 200 wenn Sender an |
| "controllerP ", // 11 = controllerP // = dreht sich im Kreis - Umlauf 40 sec |
| "Motor vorne ", // 12 = Motor[0] // = 0 // wird in SendMotorData(void) hier in fc.c zugewiesen |
| "Motor hinten ", // 13 = Motor[1] // = 0 |
| "controllerD ", // 14 = // = 0 |
| "controllerDD ", // 15 = // = 0 |
| "PPM_in[3] Nick ", // 16 = // = -120...+120 // ab hier beginjnt der zweite Teil der Anzeige ------------ |
| "ipk[0] ", // 17 = // = |
| "ipk[1] ", // 18 = // = |
| "ipk[2] ", // 19 = // = |
| "ucflg1 ", // 20 = // = |
| "Stick Kanal 5 ", // 21 = PPM_in[5] // = ca -120 oder +120 |
| "MesswertNick ", // 22 = MesswertNick; // = 0 |
| "maxcontrollerD ", // 23 = // = 0 |
| "mincontrollerD ", // 24 = // = -560 |
| "AdWertNick ", // 25 = AdWertNick; // = 1130 |
| "maxcontrollerP ", // 26 = // |
| "mincontrollerP ", // 27 = // |
| "PPM_in[4] Gier ", // 28 = PPM_in[4]; // = ca -120 bis +120 |
| "PPM_in[3] Nick ", // 29 = PPM_in[3]; // = ca -120 bis +120 |
| "PPM_in[2] Roll ", // 30 = PPM_in[2]; // = ca -120 bis +120 |
| "PPM_in[1] Gas " // 31 = PPM_in[1]; // = ca -120 bis +120 |
| }; |
| //----------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
| //------------------------------------------------------------------------------------------------------------------ |
| // Installation of the USART COM Port |
| //------------------------------------------------------------------------------------------------------------------ |
| void UART_Init(void) |
| { |
| //------------------------------------------------------------------------------------------------------------ |
| // UCSR0B USART Control and Status Register 0 B containing RXCIE0 TXCIE0 UDRIE0 RXEN0 TXEN0 UCSZ02 RXB80 TXB80 |
| // |
| UCSR0B = (1 << TXEN0) | (1 << RXEN0); // Writing this bit to one enables the USART Transmitter |
| //------------------------------------------------------------------------------------------------------------ |
| // UCSR0A USART Control and Status Register A containing RXC0 TXC0 UDRE0 FE0 DORn UPEn U2X0 MPCM0 |
| // |
| UCSR0A |= (1<<U2X0); // Double the USART Transmission Speed |
| //------------------------------------------------------------------------------------------------------------ |
| // UCSR0B USART Control and Status Register 0 B containing RXCIE0 TXCIE0 UDRIE0 RXEN0 TXEN0 UCSZ02 RXB80 TXB80 |
| // |
| UCSR0B |= (1<<RXCIE); // RX Complete Interrupt Enable |
| UCSR0B |= (1<<TXCIE); // TX Complete Interrupt Enable |
| //------------------------------------------------------------------------------------------------------------ |
| // UBRRnL and UBRRnH USART Baud Rate Registers |
| // |
| UBRR0L = (SYSCLK / (BAUD_RATE * 8L) - 1); // set Baudrate to 57600 |
| Debug_Timer = SetDelay(DebugDataIntervall); |
| Kompass_Timer = SetDelay(220); |
| VersionInfo.SWMajor = VERSION_MAJOR; |
| VersionInfo.SWMinor = VERSION_MINOR; |
| VersionInfo.SWPatch = VERSION_PATCH; |
| VersionInfo.ProtoMajor = VERSION_SERIAL_MAJOR; |
| VersionInfo.ProtoMinor = VERSION_SERIAL_MINOR; |
| pRxData = 0; |
| RxDataLen = 0; |
| } |
| //------------------------------------------------------------------------------------------------------------------ |
| //----------------------------------------------------------------------------------------------------- |
| // Interrupt due to USART0 TX Complete |
| //----------------------------------------------------------------------------------------------------- |
| ISR(USART0_TX_vect) |
| { |
| 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; |
| } |
| //----------------------------------------------------------------------------------------------------- |
| //----------------------------------------------------------------------------------------------------- |
| // Interrupt due to USART0 RX Complete |
| //----------------------------------------------------------------------------------------------------- |
| ISR(USART0_RX_vect) |
| { |
| 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 + 1; |
| RxdBuffer[buf_ptr] = '\r'; |
| if(RxdBuffer[2] == 'R') |
| { |
| wdt_enable(WDTO_250MS); // Reset-Commando |
| ServoActive = 0; |
| } |
| } |
| } |
| 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; |
| } |
| } |
| //----------------------------------------------------------------------------------------------------- |
| // -------------------------------------------------------------------------- |
| // checksum |
| // -------------------------------------------------------------------------- |
| 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]; |
| } |
| //----------------------------------------------------------------------------------------------------- |
| // --------------------------------------------------------------------------------------------------------------------------- |
| //unsigned char *snd, unsigned char len) |
| // --------------------------------------------------------------------------------------------------------------------------- |
| void SendOutData(unsigned char cmd,unsigned char address, unsigned char BufferAnzahl, ...) |
| { |
| va_list ap; |
| unsigned int pt = 0; |
| unsigned char a,b,c; |
| unsigned char ptr = 0; |
| unsigned char *snd = 0; |
| int len = 0; |
| SendeBuffer[pt++] = '#'; // Startzeichen |
| SendeBuffer[pt++] = 'a' + address; // Adresse (a=0; b=1,...) |
| SendeBuffer[pt++] = cmd; // Commando |
| va_start(ap, BufferAnzahl); |
| if(BufferAnzahl) |
| { |
| snd = va_arg(ap, unsigned char*); |
| len = va_arg(ap, int); |
| ptr = 0; |
| BufferAnzahl--; |
| } |
| while(len) |
| { |
| if(len) |
| { |
| a = snd[ptr++]; |
| len--; |
| if((!len) && BufferAnzahl) |
| { |
| snd = va_arg(ap, unsigned char*); |
| len = va_arg(ap, int); |
| ptr = 0; |
| BufferAnzahl--; |
| } |
| } |
| else a = 0; |
| if(len) |
| { |
| b = snd[ptr++]; |
| len--; |
| if((!len) && BufferAnzahl) |
| { |
| snd = va_arg(ap, unsigned char*); |
| len = va_arg(ap, int); |
| ptr = 0; |
| BufferAnzahl--; |
| } |
| } |
| else b = 0; |
| if(len) |
| { |
| c = snd[ptr++]; |
| len--; |
| if((!len) && BufferAnzahl) |
| { |
| snd = va_arg(ap, unsigned char*); |
| len = va_arg(ap, int); |
| ptr = 0; |
| BufferAnzahl--; |
| } |
| } |
| 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); |
| } |
| va_end(ap); |
| AddCRC(pt); |
| } |
| // EOF : SendOutData() ---------------------------------------------------------------------------------------------------- |
| // ---------------------------------------------------------------------------------------------------------------------- |
| // die daten werden im rx buffer dekodiert, das geht nur, weil aus 4 byte immer 3 gemacht werden. |
| // ---------------------------------------------------------------------------------------------------------------------- |
| void Decode64(void) |
| { |
| unsigned char a,b,c,d; |
| unsigned char x,y,z; |
| unsigned char ptrIn = 3; // start at begin of data block |
| unsigned char ptrOut = 3; |
| unsigned char len = AnzahlEmpfangsBytes - 6; // von der Gesamtbytezahl eines Frames gehen 3 Bytes des Headers ('#',Addr, Cmd) und 3 Bytes des Footers (CRC1, CRC2, '\r') ab. |
| while(len) |
| { |
| a = RxdBuffer[ptrIn++] - '='; |
| b = RxdBuffer[ptrIn++] - '='; |
| c = RxdBuffer[ptrIn++] - '='; |
| d = RxdBuffer[ptrIn++] - '='; |
| x = (a << 2) | (b >> 4); |
| y = ((b & 0x0f) << 4) | (c >> 2); |
| z = ((c & 0x03) << 6) | d; |
| if(len--) RxdBuffer[ptrOut++] = x; else break; |
| if(len--) RxdBuffer[ptrOut++] = y; else break; |
| if(len--) RxdBuffer[ptrOut++] = z; else break; |
| } |
| pRxData = (unsigned char*)&RxdBuffer[3]; // decodierte Daten beginnen beim 4. Byte |
| RxDataLen = ptrOut - 3; // wie viele Bytes wurden dekodiert? |
| } |
| //----------------------------------------------------------------------------------------------------- |
| // ---------------------------------------------------------------------------------------------------------------------- |
| void BearbeiteRxDaten(void) |
| { |
| if(!NeuerDatensatzEmpfangen) return; |
| unsigned char tempchar1, tempchar2; |
| Decode64(); // dekodiere datenblock im Empfangsbuffer |
| switch(RxdBuffer[1]-'a') // check for Slave Address |
| { |
| case FC_ADDRESS: // FC special commands |
| switch(RxdBuffer[2]) |
| { |
| case 'K': // Kompasswert |
| memcpy((unsigned char *)&KompassValue , (unsigned char *)pRxData, sizeof(KompassValue)); |
| KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180; |
| break; |
| case 't': // Motortest |
| if(AnzahlEmpfangsBytes > 20) memcpy(&MotorTest[0], (unsigned char *)pRxData, sizeof(MotorTest)); |
| else memcpy(&MotorTest[0], (unsigned char *)pRxData, 4); |
| PC_MotortestActive = 240; |
| //while(!UebertragungAbgeschlossen); |
| //SendOutData('T', MeineSlaveAdresse, 0); |
| PcZugriff = 255; |
| break; |
| case 'n': // "Get Mixer |
| while(!UebertragungAbgeschlossen); |
| SendOutData('N', FC_ADDRESS, 1, (unsigned char *) &Mixer,sizeof(Mixer)); |
| break; |
| case 'm': // "Write Mixer |
| while(!UebertragungAbgeschlossen); |
| if(pRxData[0] == MIXER_REVISION) |
| { |
| memcpy(&Mixer, (unsigned char *)pRxData, sizeof(Mixer)); |
| eeprom_write_block(&Mixer, &EEPromArray[EEPROM_ADR_MIXER_TABLE], sizeof(Mixer)); |
| tempchar1 = 1; |
| } |
| else tempchar1 = 0; |
| SendOutData('M', FC_ADDRESS, 1, &tempchar1, sizeof(tempchar1)); |
| break; |
| case 'p': // get PPM Channels |
| GetPPMChannelAnforderung = 1; |
| break; |
| case 'q': // "Get"-Anforderung für Settings |
| // Bei Get werden die vom PC einstellbaren Werte vom PC zurückgelesen |
| if(pRxData[0] == 0xFF) |
| { |
| pRxData[0] = GetActiveParamSetNumber(); |
| } |
| // limit settings range |
| if(pRxData[0] < 1) pRxData[0] = 1; // limit to 5 |
| else if(pRxData[0] > 5) pRxData[0] = 5; // limit to 5 |
| // load requested parameter set |
| ReadParameterSet(pRxData[0], (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE); // #define STRUCT_PARAM_LAENGE sizeof(EE_Parameter) |
| while(!UebertragungAbgeschlossen); |
| tempchar1 = pRxData[0]; |
| tempchar2 = EE_DATENREVISION; // #define EE_DATENREVISION 80 // Parameter fürs Koptertool; entspricht den EEPROM-Daten von FlightCtrl Version V0.76g |
| SendOutData('Q', FC_ADDRESS, 3, &tempchar1, sizeof(tempchar1), &tempchar2, sizeof(tempchar2), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE); // #define STRUCT_PARAM_LAENGE sizeof(EE_Parameter) |
| break; |
| case 's': // Parametersatz speichern |
| if((1 <= pRxData[0]) && (pRxData[0] <= 5) && (pRxData[1] == EE_DATENREVISION)) // check for setting to be in range |
| { |
| memcpy((unsigned char *) &EE_Parameter.Kanalbelegung[0], (unsigned char *)&pRxData[2], STRUCT_PARAM_LAENGE); // #define STRUCT_PARAM_LAENGE sizeof(EE_Parameter) |
| WriteParameterSet(pRxData[0], (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE); |
| Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L; |
| Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L; |
| SetActiveParamSetNumber(pRxData[0]); |
| tempchar1 = GetActiveParamSetNumber(); |
| LipoDetection(0); // jump tp main.c line 59 |
| Piep(tempchar1,110); |
| } |
| else |
| { |
| tempchar1 = 0; // mark in response an invlid setting |
| } |
| while(!UebertragungAbgeschlossen); |
| SendOutData('S', FC_ADDRESS, 1, &tempchar1, sizeof(tempchar1)); |
| break; |
| } // EOF : case FC_ADDRESS |
| default: // any Slave Address |
| switch(RxdBuffer[2]) |
| { |
| // 't' comand placed here only for compatibility to BL |
| case 't': // Motortest |
| if(AnzahlEmpfangsBytes > 20) memcpy(&MotorTest[0], (unsigned char *)pRxData, sizeof(MotorTest)); |
| else memcpy(&MotorTest[0], (unsigned char *)pRxData, 4); |
| while(!UebertragungAbgeschlossen); |
| SendOutData('T', MeineSlaveAdresse, 0); |
| PC_MotortestActive = 250; |
| PcZugriff = 255; |
| break; |
| // 'K' comand placed here only for compatibility to old MK3MAG software, that does not send the right Slave Address |
| case 'K': // Kompasswert |
| memcpy((unsigned char *)&KompassValue , (unsigned char *)pRxData, sizeof(KompassValue)); |
| KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180; |
| break; |
| case 'a': // Texte der Analogwerte |
| DebugTextAnforderung = pRxData[0]; |
| if (DebugTextAnforderung > 31) DebugTextAnforderung = 31; |
| PcZugriff = 255; |
| break; |
| case 'b': |
| memcpy((unsigned char *)&ExternControl, (unsigned char *)pRxData, sizeof(ExternControl)); |
| ConfirmFrame = ExternControl.Frame; |
| PcZugriff = 255; |
| break; |
| case 'c': // Poll the 3D-Data |
| if(!Intervall3D) { if(pRxData[0]) Timer3D = SetDelay(pRxData[0] * 10);} |
| Intervall3D = pRxData[0] * 10; |
| break; |
| case 'd': // Poll the debug data |
| DebugDataIntervall = pRxData[0] * 10; |
| if(DebugDataIntervall > 0) DebugDataAnforderung = 1; |
| break; |
| case 'h': // x-1 Displayzeilen |
| PcZugriff = 255; |
| RemoteKeys |= pRxData[0]; |
| if(RemoteKeys) DisplayLine = 0; |
| DebugDisplayAnforderung = 1; |
| break; |
| case 'l': // x-1 Displayzeilen |
| PcZugriff = 255; |
| MenuePunkt = pRxData[0]; |
| DebugDisplayAnforderung1 = 1; |
| break; |
| case 'v': // Version-Anforderung und Ausbaustufe |
| GetVersionAnforderung = 1; |
| break; |
| case 'g': // |
| GetExternalControl = 1; |
| break; |
| } |
| break; // default: |
| } |
| NeuerDatensatzEmpfangen = 0; |
| pRxData = 0; |
| RxDataLen = 0; |
| } |
| //----------------------------------------------------------------------------------------------------- |
| // -------------------------------------------------------------------------- |
| // function serial output |
| // -------------------------------------------------------------------------- |
| int uart_putchar(char c) |
| { |
| if (c == '\n') uart_putchar('\r'); |
| loop_until_bit_is_set(USR, UDRE); // Warten solange bis Zeichen gesendet wurde |
| UDR = c; // Ausgabe des Zeichens |
| return (0); |
| } |
| //----------------------------------------------------------------------------------------------------- |
| //--------------------------------------------------------------------------------------------- |
| // Datenübertragung |
| //--------------------------------------------------------------------------------------------- |
| void DatenUebertragung(void) |
| { |
| if(!UebertragungAbgeschlossen) return; |
| if(DebugDisplayAnforderung && UebertragungAbgeschlossen) |
| { |
| Menu(); |
| SendOutData('H', FC_ADDRESS, 2, &DisplayLine, sizeof(DisplayLine), &DisplayBuff[DisplayLine * 20], 20); |
| DisplayLine++; |
| if(DisplayLine >= 4) DisplayLine = 0; |
| DebugDisplayAnforderung = 0; |
| } |
| if(DebugDisplayAnforderung1 && UebertragungAbgeschlossen) |
| { |
| Menu(); |
| SendOutData('L', FC_ADDRESS, 3, &MenuePunkt, sizeof(MenuePunkt), &MaxMenue, sizeof(MaxMenue), DisplayBuff, sizeof(DisplayBuff)); |
| DebugDisplayAnforderung1 = 0; |
| } |
| if(GetVersionAnforderung && UebertragungAbgeschlossen) |
| { |
| SendOutData('V', FC_ADDRESS, 1, (unsigned char *) &VersionInfo, sizeof(VersionInfo)); |
| GetVersionAnforderung = 0; |
| } |
| if(GetExternalControl && UebertragungAbgeschlossen) // Bei Get werden die vom PC einstellbaren Werte vom PC zurückgelesen |
| { |
| SendOutData('G',MeineSlaveAdresse, 1, (unsigned char *) &ExternControl, sizeof(ExternControl)); |
| GetExternalControl = 0; |
| } |
| if((CheckDelay(Kompass_Timer)) && UebertragungAbgeschlossen) |
| { |
| WinkelOut.Winkel[0] = (int) (IntegralNick / (EE_Parameter.GyroAccFaktor * 4)); // etwa in 0.1 Grad |
| WinkelOut.Winkel[1] = (int) (IntegralRoll / (EE_Parameter.GyroAccFaktor * 4)); // etwa in 0.1 Grad |
| WinkelOut.UserParameter[0] = Parameter_UserParam1; |
| WinkelOut.UserParameter[1] = Parameter_UserParam2; |
| SendOutData('w', MK3MAG_ADDRESS, 1, (unsigned char *) &WinkelOut,sizeof(WinkelOut)); |
| if(WinkelOut.CalcState > 4) WinkelOut.CalcState = 6; // wird dann in SPI auf Null gesetzt |
| Kompass_Timer = SetDelay(99); |
| } |
| if(((DebugDataIntervall>0 && CheckDelay(Debug_Timer)) || DebugDataAnforderung) && UebertragungAbgeschlossen) |
| { |
| //if(Poti3 > 64) |
| SendOutData('D', FC_ADDRESS, 1, (unsigned char *) &DebugOut,sizeof(DebugOut)); |
| DebugDataAnforderung = 0; |
| if(DebugDataIntervall>0) Debug_Timer = SetDelay(DebugDataIntervall); |
| } |
| if(Intervall3D > 0 && CheckDelay(Timer3D) && UebertragungAbgeschlossen) |
| { |
| Data3D.Winkel[0] = (int) (IntegralNick / (EE_Parameter.GyroAccFaktor * 4)); // etwa in 0.1 Grad |
| Data3D.Winkel[1] = (int) (IntegralRoll / (EE_Parameter.GyroAccFaktor * 4)); // etwa in 0.1 Grad |
| Data3D.Winkel[2] = (int) ((10 * ErsatzKompass) / GIER_GRAD_FAKTOR); |
| SendOutData('C', FC_ADDRESS, 1, (unsigned char *) &Data3D,sizeof(Data3D)); |
| Timer3D = SetDelay(Intervall3D); |
| } |
| if(DebugTextAnforderung != 255) // Texte für die Analogdaten |
| { |
| SendOutData('A', FC_ADDRESS, 2, (unsigned char *)&DebugTextAnforderung, sizeof(DebugTextAnforderung),(unsigned char *) ANALOG_TEXT[DebugTextAnforderung], 16); |
| DebugTextAnforderung = 255; |
| } |
| if(ConfirmFrame && UebertragungAbgeschlossen) // Datensatz bestätigen |
| { |
| SendOutData('B', FC_ADDRESS, 1, (uint8_t*)&ConfirmFrame, sizeof(ConfirmFrame)); |
| ConfirmFrame = 0; |
| } |
| if(GetPPMChannelAnforderung && UebertragungAbgeschlossen) |
| { |
| SendOutData('P', FC_ADDRESS, 1, (unsigned char *) &PPM_in, sizeof(PPM_in)); |
| GetPPMChannelAnforderung = 0; |
| } |
| } |
| //---------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
| /branches/V0.76g_dk9nw_balancekopter/uart.h |
|---|
| 0,0 → 1,124 |
| // ------------------------------------------------------------------------------------------------------------------------------------- |
| // uart.h |
| // ------------------------------------------------------------------------------------------------------------------------------------- |
| #ifndef _UART_H |
| #define _UART_H |
| #define MAX_SENDE_BUFF 150 |
| #define MAX_EMPFANGS_BUFF 150 |
| void BearbeiteRxDaten(void); |
| extern unsigned char DebugGetAnforderung; |
| extern unsigned volatile char SendeBuffer[MAX_SENDE_BUFF]; |
| extern unsigned volatile char RxdBuffer[MAX_EMPFANGS_BUFF]; |
| extern unsigned volatile char UebertragungAbgeschlossen; |
| extern unsigned volatile char PC_DebugTimeout; |
| extern unsigned volatile char NeueKoordinateEmpfangen; |
| extern unsigned volatile char PC_MotortestActive; |
| extern unsigned char MeineSlaveAdresse; |
| extern unsigned char PcZugriff; |
| extern unsigned char RemotePollDisplayLine; |
| extern int Debug_Timer,Kompass_Timer; |
| extern void UART_Init (void); |
| extern int uart_putchar(char c); |
| extern void boot_program_page(uint32_t page, uint8_t *buf); |
| extern void DatenUebertragung(void); |
| extern void BearbeiteRxDaten(void); |
| extern unsigned char MotorTest[16]; |
| struct str_DebugOut |
| { |
| unsigned char Digital[2]; |
| signed int Analog[32]; // Debugwerte |
| }; |
| extern struct str_DebugOut DebugOut; |
| struct str_WinkelOut |
| { |
| signed int Winkel[2]; |
| unsigned char UserParameter[2]; |
| unsigned char CalcState; // wird als vierstufiges Flag in spi.c uart.c fc.c und main.c verwendet |
| unsigned char Orientation; |
| }; |
| extern struct str_WinkelOut WinkelOut; |
| struct str_Data3D |
| { |
| signed int Winkel[3]; // nick, roll, compass in 0,1° |
| signed char reserve[8]; |
| }; |
| extern struct str_Data3D Data3D; |
| struct str_ExternControl |
| { |
| unsigned char Digital[2]; |
| unsigned char RemoteTasten; |
| signed char Nick; |
| signed char Roll; |
| signed char Gier; |
| unsigned char Gas; |
| signed char Hight; |
| unsigned char free; |
| unsigned char Frame; |
| unsigned char Config; |
| }; |
| extern struct str_ExternControl ExternControl; |
| struct str_VersionInfo |
| { |
| unsigned char SWMajor; |
| unsigned char SWMinor; |
| unsigned char ProtoMajor; |
| unsigned char ProtoMinor; |
| unsigned char SWPatch; |
| unsigned char Reserved[5]; |
| }; |
| extern struct str_VersionInfo VersionInfo; |
| #define BAUD_RATE 57600 //Die Baud_Rate der Seriellen Schnittstelle ist 57600 Baud |
| //Anpassen der seriellen Schnittstellen Register----- |
| #if defined (__AVR_ATmega644__) |
| # define USR UCSR0A |
| // # define UCR UCSR0B |
| # define UDR UDR0 |
| # define UBRR UBRR0L |
| # define EICR EICR0B |
| # define TXEN TXEN0 |
| # define RXEN RXEN0 |
| # define RXCIE RXCIE0 |
| # define TXCIE TXCIE0 |
| # define U2X U2X0 |
| # define UCSRB UCSR0B |
| # define UDRE UDRE0 |
| # define INT_VEC_RX SIG_USART_RECV |
| # define INT_VEC_TX SIG_USART_TRANS |
| #endif |
| //-------------------------------------- |
| #if defined (__AVR_ATmega644P__) |
| # define USR UCSR0A |
| # define UCR UCSR0B |
| # define UDR UDR0 |
| # define UBRR UBRR0L |
| # define EICR EICR0B |
| # define TXEN TXEN0 |
| # define RXEN RXEN0 |
| # define RXCIE RXCIE0 |
| # define TXCIE TXCIE0 |
| # define U2X U2X0 |
| # define UCSRB UCSR0B |
| # define UDRE UDRE0 |
| # define INT_VEC_RX SIG_USART_RECV |
| # define INT_VEC_TX SIG_USART_TRANS |
| #endif |
| #endif //_UART_H |
| //-------------------------------------------------- |