/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 |
//-------------------------------------------------- |