func InitializeObjects()
{
CreateObject(Ambience);
var Rule_BaseRespawn001 = CreateObject(Rule_BaseRespawn);
Rule_BaseRespawn001->SetInventoryTransfer(true);
Rule_BaseRespawn001->SetFreeCrew(true);
CreateObject(Rule_NoPowerNeed);
CreateObject(Rule_TeamAccount);
CreateObjectAbove(EnvPack_Scarecrow, 1218, 440);
CreateObjectAbove(EnvPack_Guidepost, 835, 369);
CreateObject(EnvPack_TreeTrunks, 808, 368);
CreateObjectAbove(SproutBerryBush, 1154, 445);
var Branch001 = CreateObject(Branch, 1509, 657);
Branch001->SetR(-26);
CreateObjectAbove(Trunk, 1194, 454);
CreateObjectAbove(Tree_Coconut, 1487, 669);
var Tree_Coniferous001 = CreateObject(Tree_Coniferous, 1068, 363);
Tree_Coniferous001->SetR(6);
CreateObjectAbove(Tree_Coniferous, 1371, 576);
CreateObjectAbove(Tree_Coniferous, 1258, 470);
var Tree_Coniferous002 = CreateObject(Tree_Coniferous, 1302, 464);
Tree_Coniferous002->SetR(30);
CreateObjectAbove(Tree_Coniferous, 1219, 441);
CreateObjectAbove(EnvPack_Guidepost, 2054, 521);
CreateObjectAbove(Trunk, 2631, 589);
CreateObjectAbove(SproutBerryBush, 2599, 590);
CreateObjectAbove(SproutBerryBush, 2521, 582);
CreateObject(SproutBerryBush, 3332, 645);
CreateObjectAbove(SproutBerryBush, 2674, 593);
var Branch002 = CreateObject(Branch, 2335, 600);
Branch002->SetR(21);
CreateObjectAbove(BigRock, 3273, 609);
var LargeCaveMushroom001 = CreateObjectAbove(LargeCaveMushroom, 2877, 1342);
LargeCaveMushroom001->SetClrModulation(0xffe1dde0);
var LargeCaveMushroom002 = CreateObjectAbove(LargeCaveMushroom, 3101, 1371);
LargeCaveMushroom002->SetClrModulation(0xffdde4da);
var LargeCaveMushroom003 = CreateObjectAbove(LargeCaveMushroom, 2971, 1339);
LargeCaveMushroom003->SetClrModulation(0xffe0eef5);
var LargeCaveMushroom004 = CreateObjectAbove(LargeCaveMushroom, 2793, 1261);
LargeCaveMushroom004->SetClrModulation(0xffdcd2ed);
CreateObjectAbove(Tree_Coconut, 1822, 679);
CreateObjectAbove(Tree_Coniferous2, 2567, 583);
CreateObjectAbove(Tree_Coniferous2, 2107, 528);
var Tree_Coniferous2001 = CreateObject(Tree_Coniferous2, 1409, 544);
Tree_Coniferous2001->SetR(20);
CreateObjectAbove(Tree_Coniferous2, 1157, 449);
CreateObjectAbove(Tree_Coniferous, 1102, 432);
var Tree_Coniferous2002 = CreateObject(Tree_Coniferous2, 1032, 358);
Tree_Coniferous2002->SetR(-8);
CreateObjectAbove(Tree_Coniferous3, 2359, 624);
CreateObjectAbove(Tree_Coniferous3, 2424, 609);
CreateObjectAbove(Flower, 907, 399);
CreateObjectAbove(Flower, 997, 408);
CreateObjectAbove(Flower, 938, 407);
CreateObjectAbove(Flower, 1404, 599);
var Flower005 = CreateObject(Flower, 1322, 533);
Flower005->SetR(50);
var Flower006 = CreateObject(Flower, 1328, 541);
Flower006->SetR(50);
var Flower007 = CreateObject(Flower, 1311, 523);
Flower007->SetR(50);
var Flower008 = CreateObjectAbove(Flower, 2600, 592);
Flower008->SetSkin(2);
var Flower009 = CreateObject(Flower, 2578, 574);
Flower009->SetR(20);
Flower009->SetSkin(2);
CreateObject(Basement, 758, 370);
CreateObject(Basement, 464, 368);
CreateObject(Basement, 93, 391);
CreateObject(Basement, 618, 370);
CreateObject(Basement, 733, 369);
CreateObject(Basement, 581, 370);
CreateObject(Basement, 2758, 612);
CreateObject(Basement, 2796, 613);
CreateObject(Basement, 2837, 612);
CreateObject(Basement, 3233, 603);
CreateObject(Basement, 3205, 604);
CreateObject(Basement, 3171, 603);
CreateObject(Basement, 3061, 620);
CreateObject(Basement, 3019, 618);
CreateObject(Basement, 2967, 600);
CreateObject(Basement, 2935, 600);
CreateObject(Basement, 2897, 599);
CreateObject(Basement, 2720, 611);
var Foundry002 = CreateObjectAbove(Foundry, 944, 782);
var Foundry001 = CreateObjectAbove(Foundry, 2958, 596);
g_chemical = CreateObjectAbove(ChemicalLab, 734, 365);
g_chemical.StaticSaveVar = "g_chemical";
g_cabin = CreateObjectAbove(WoodenCabin, 546, 367);
g_cabin.StaticSaveVar = "g_cabin";
g_sawmill = CreateObjectAbove(Sawmill, 782, 366);
g_sawmill.StaticSaveVar = "g_sawmill";
CreateObjectAbove(Pump, 466, 363);
g_workshop = CreateObjectAbove(ToolsWorkshop, 609, 365);
g_workshop.StaticSaveVar = "g_workshop";
var ToolsWorkshop001 = CreateObjectAbove(ToolsWorkshop, 2905, 595);
CreateObjectAbove(Castle_ConstructionSite, 281, 343);
CreateObjectAbove(Kitchen, 3030, 615);
CreateObjectAbove(InventorsLab, 3212, 599);
var Shipyard001 = CreateObjectAbove(Shipyard, 2763, 608);
CreateObjectAbove(Loom, 3080, 616);
var StoneDoor001 = CreateObject(StoneDoor, 540, 1244);
StoneDoor001->SetComDir(COMD_Down);
var SpinWheel001 = CreateObjectAbove(SpinWheel, 571, 1263);
SpinWheel001->SetStoneDoor(StoneDoor001);
g_windmill = CreateObjectAbove(Windmill, 665, 351);
g_windmill->SetCategory(C4D_StaticBack);
g_windmill.StaticSaveVar = "g_windmill";
g_flagpole = CreateObjectAbove(Flagpole, 502, 369);
g_flagpole.StaticSaveVar = "g_flagpole";
g_flagpole->SetNeutral(true);
CreateObjectAbove(WindGenerator, 3163, 599);
var Elevator001 = CreateObjectAbove(Elevator, 76, 387);
Elevator001->SetDir(DIR_Right);
Elevator001->CreateShaft(530);
Elevator001->SetCasePosition(905);
var Elevator002 = CreateObjectAbove(Elevator, 985, 782);
Elevator002->CreateShaft(30);
Elevator002->SetCasePosition(800);
var Lorry001 = CreateObject(Lorry, 25, 771);
Lorry001->SetR(11);
var Lorry002 = CreateObject(Lorry, 3188, 591);
Lorry002->SetR(6);
Lorry002->SetRDir(7);
var Catapult001 = CreateObjectAbove(Catapult, 2795, 608);
Catapult001->SetRDir(-5);
Catapult001->SetObjectLayer(Catapult001);
npc_newton = CreateObjectAbove(Clonk, 226, 321);
npc_newton->SetColor(0xffff);
npc_newton->SetName("Newton");
npc_newton->SetObjectLayer(npc_newton);
npc_newton.StaticSaveVar = "npc_newton";
npc_newton->SetDir(DIR_Left);
npc_lara = CreateObjectAbove(Clonk, 300, 337);
npc_lara->SetColor(0xff0000);
npc_lara.StaticSaveVar = "npc_lara";
npc_lara->SetName("Lara");
npc_lara->SetObjectLayer(npc_lara);
npc_lara->SetSkin(1);
npc_lara->SetDir(DIR_Left);
npc_lisa = CreateObjectAbove(Clonk, 496, 367);
npc_lisa->SetColor(0xff00);
npc_lisa.StaticSaveVar = "npc_lisa";
npc_lisa->SetName("Lisa");
npc_lisa->SetObjectLayer(npc_lisa);
npc_lisa->SetSkin(3);
npc_lisa->SetDir(DIR_Left);
npc_woody = CreateObjectAbove(Clonk, 782, 367);
npc_woody->SetColor(0x808000);
npc_woody.StaticSaveVar = "npc_woody";
npc_woody->SetName("Woody");
npc_woody->SetObjectLayer(npc_woody);
npc_woody->SetSkin(2);
npc_woody->SetDir(DIR_Left);
npc_rocky = CreateObjectAbove(Clonk, 98, 774);
npc_rocky->SetDir(DIR_Right);
npc_rocky->SetColor(0x808080);
npc_rocky.StaticSaveVar = "npc_rocky";
npc_rocky->SetName("Rocky");
npc_rocky->SetObjectLayer(npc_rocky);
npc_rocky->SetSkin(2);
npc_mave = CreateObjectAbove(Clonk, 973, 783);
npc_mave->SetDir(DIR_Right);
npc_mave->SetColor(0xff8000);
npc_mave->SetName("Mave");
npc_mave->SetObjectLayer(npc_mave);
npc_mave.StaticSaveVar = "npc_mave";
npc_pyrit = CreateObjectAbove(Clonk, 2816, 607);
npc_pyrit->SetColor(0xff0000);
npc_pyrit.StaticSaveVar = "npc_pyrit";
npc_pyrit->SetName("Pyrit");
npc_pyrit->SetObjectLayer(npc_pyrit);
npc_pyrit->SetSkin(2);
npc_pyrit->SetDir(DIR_Left);
npc_clonko = CreateObjectAbove(Clonk, 2934, 595);
npc_clonko->SetDir(DIR_Right);
npc_clonko->SetColor(0xff8000);
npc_clonko->SetName("Clonko");
npc_clonko->SetObjectLayer(npc_clonko);
npc_clonko.StaticSaveVar = "npc_clonko";
npc_matthi = CreateObjectAbove(Clonk, 3002, 613);
npc_matthi->SetColor(0x80ff00);
npc_matthi->SetName("Matthi");
npc_matthi->SetObjectLayer(npc_matthi);
npc_matthi.StaticSaveVar = "npc_matthi";
npc_matthi->SetDir(DIR_Left);
npc_dora = CreateObjectAbove(Clonk, 3178, 1367);
npc_dora->SetDir(DIR_Right);
npc_dora->SetColor(0xffff20);
npc_dora.StaticSaveVar = "npc_dora";
npc_dora->SetName("Dora");
npc_dora->SetObjectLayer(npc_dora);
npc_dora->SetSkin(3);
CreateObjectAbove(Fish, 1889, 728);
CreateObjectAbove(Fish, 1879, 726);
var Mosquito001 = CreateObjectAbove(Mosquito, 2292, 514);
Mosquito001->SetXDir(-6);
Mosquito001->SetCommand("Call", Mosquito001, nil, 0, nil, "MissionComplete");
var Sickle001 = npc_matthi->CreateContents(Sickle);
npc_matthi->SetDialogue("Matthi",true);
var Hammer001 = npc_clonko->CreateContents(Hammer);
npc_clonko->SetDialogue("Clonko",true);
var Hammer002 = npc_pyrit->CreateContents(Hammer);
var Hammer003 = npc_pyrit->CreateContents(Hammer);
npc_pyrit->SetDialogue("Pyrit",true);
var Bucket001 = npc_mave->CreateContents(Bucket);
npc_mave->SetDialogue("Mave",false);
var Hammer004 = npc_newton->CreateContents(Hammer);
npc_newton->SetDialogue("Newton",true);
npc_lara->SetDialogue("Lara",true);
npc_lisa->SetDialogue("Lisa",true);
var Axe001 = npc_woody->CreateContents(Axe);
npc_woody->SetDialogue("Woody",true);
var Pickaxe001 = npc_rocky->CreateContents(Pickaxe);
npc_rocky->SetDialogue("Rocky",true);
npc_dora->SetDialogue("Dora",true);
CreateObjectAbove(Skull, 1566, 703);
CreateObjectAbove(Skull, 3124, 1378);
CreateObject(Rock, 685, 593);
CreateObject(Rock, 793, 487);
CreateObject(Rock, 1244, 514);
CreateObject(Rock, 1480, 734);
CreateObject(Rock, 794, 894);
CreateObject(Rock, 311, 446);
CreateObject(Rock, 1243, 791);
CreateObject(Rock, 1231, 736);
CreateObject(Rock, 563, 921);
CreateObject(Rock, 1219, 1087);
CreateObject(Rock, 428, 1318);
CreateObject(Rock, 2100, 950);
CreateObject(Rock, 1922, 1160);
CreateObject(Rock, 2277, 967);
CreateObject(Rock, 2405, 695);
CreateObject(Rock, 2893, 988);
CreateObject(Rock, 3131, 688);
CreateObject(Rock, 3266, 1378);
CreateObjectAbove(Ore, 2226, 943);
Foundry001->CreateContents(Ore, 3);
CreateObject(Loam, 1030, 446);
CreateObject(Loam, 1122, 917);
CreateObject(Loam, 1492, 801);
CreateObjectAbove(Loam, 926, 1166);
CreateObject(Loam, 456, 1001);
CreateObject(Loam, 2315, 629);
CreateObject(Loam, 2582, 787);
CreateObject(Loam, 3056, 722);
CreateObject(Loam, 3235, 789);
CreateObject(Loam, 3167, 946);
CreateObject(Loam, 2630, 1049);
CreateObject(Loam, 3238, 1147);
CreateObject(Loam, 2734, 1242);
CreateObject(Loam, 3003, 1342);
Foundry002->CreateContents(Loam, 2);
CreateObject(Loam, 951, 1330);
CreateObjectAbove(Loam, 970, 1355);
CreateObjectAbove(Loam, 952, 1399);
CreateObject(Loam, 808, 1333);
CreateObject(Loam, 737, 1319);
CreateObject(Loam, 652, 1277);
CreateObject(Loam, 797, 1388);
CreateObjectAbove(Loam, 1012, 1391);
CreateObjectAbove(Loam, 492, 1263);
CreateObjectAbove(Loam, 504, 1263);
CreateObjectAbove(Loam, 500, 1263);
CreateObjectAbove(Metal, 2217, 942);
ToolsWorkshop001->CreateContents(Metal, 3);
Shipyard001->CreateContents(Metal, 4);
Lorry002->CreateContents(Metal, 2);
CreateObject(Moss, 1529, 680);
ToolsWorkshop001->CreateContents(Wood, 3);
Shipyard001->CreateContents(Wood, 4);
ToolsWorkshop001->CreateContents(Pickaxe, 2);
Lorry001->CreateContents(Axe, 3);
var Crate001 = CreateObjectAbove(Crate, 2836, 607);
Crate001->CreateContents(Hammer);
ToolsWorkshop001->CreateContents(Hammer, 2);
Foundry002->CreateContents(Bucket, 3);
CreateObjectAbove(Bucket, 435, 1271);
CreateObjectAbove(Crate, 2849, 607);
CreateObjectAbove(Crate, 444, 1271);
CreateObjectAbove(Crate, 473, 1263);
CreateObjectAbove(Crate, 403, 1271);
var Barrel001 = Foundry002->CreateContents(Barrel);
Barrel001->AddRestoreMode(Foundry002, 944, 757);
Barrel001->SetColor(0xff000000);
var Barrel002 = Foundry002->CreateContents(Barrel);
Barrel002->AddRestoreMode(Foundry002, 944, 757);
Barrel002->SetColor(0xff000000);
var Barrel003 = CreateObject(Barrel, 484, 361);
Barrel003->SetR(23);
Barrel003->SetColor(0xff000000);
Barrel003->SetObjectLayer(Barrel003);
var Barrel004 = CreateObject(Barrel, 648, 345);
Barrel004->SetR(-22);
Barrel004->SetColor(0xff000000);
Barrel004->SetObjectLayer(Barrel004);
var Barrel005 = CreateObjectAbove(Barrel, 244, 321);
Barrel005->SetColor(0xff000000);
Barrel005->SetObjectLayer(Barrel005);
var Barrel006 = CreateObjectAbove(Barrel, 396, 343);
Barrel006->SetColor(0xff000000);
Barrel006->SetObjectLayer(Barrel006);
CreateObjectAbove(Mushroom, 1192, 448);
CreateObjectAbove(Mushroom, 1170, 439);
CreateObjectAbove(Mushroom, 1492, 663);
CreateObjectAbove(Mushroom, 1131, 434);
CreateObjectAbove(Mushroom, 1523, 674);
CreateObjectAbove(Mushroom, 1163, 438);
CreateObjectAbove(Mushroom, 1070, 414);
CreateObjectAbove(Mushroom, 1010, 399);
CreateObjectAbove(Mushroom, 960, 400);
CreateObjectAbove(Mushroom, 1175, 440);
CreateObjectAbove(Mushroom, 1120, 432);
CreateObjectAbove(Mushroom, 989, 398);
CreateObjectAbove(Mushroom, 968, 398);
CreateObjectAbove(Mushroom, 1013, 399);
CreateObjectAbove(Mushroom, 1496, 662);
CreateObjectAbove(Mushroom, 1088, 423);
CreateObjectAbove(Mushroom, 1545, 696);
CreateObjectAbove(Mushroom, 1223, 438);
CreateObjectAbove(Mushroom, 943, 399);
CreateObjectAbove(Mushroom, 1006, 399);
CreateObjectAbove(Seaweed, 1952, 903);
CreateObjectAbove(Seaweed, 2013, 911);
CreateObjectAbove(Seaweed, 1903, 887);
CreateObjectAbove(Seaweed, 1983, 911);
CreateObjectAbove(Seaweed, 2207, 942);
CreateObjectAbove(Seaweed, 2127, 895);
CreateObjectAbove(Seaweed, 2227, 943);
CreateObjectAbove(Seaweed, 2191, 927);
CreateObjectAbove(Seaweed, 2232, 943);
CreateObjectAbove(Seaweed, 2269, 927);
CreateObjectAbove(Seaweed, 2249, 935);
CreateObjectAbove(DynamiteBox, 2844, 607);
CreateObjectAbove(DynamiteBox, 452, 1271);
var DynamiteBox001 = CreateObject(DynamiteBox, 430, 1269);
DynamiteBox001->SetR(10);
var MetalBarrel001 = CreateObjectAbove(MetalBarrel, 395, 1271);
MetalBarrel001->SetColor(0xff000000);
var MetalBarrel002 = CreateObject(MetalBarrel, 421, 1268);
MetalBarrel002->SetR(-104);
MetalBarrel002->SetColor(0xff000000);
var MetalBarrel003 = CreateObjectAbove(MetalBarrel, 411, 1271);
MetalBarrel003->SetColor(0xff000000);
var MetalBarrel004 = CreateObjectAbove(MetalBarrel, 385, 1271);
MetalBarrel004->SetColor(0xff000000);
var PowderKeg001 = CreateObject(PowderKeg, 378, 1268);
PowderKeg001->SetR(99);
var WindBag001 = CreateObject(WindBag, 382, 1268);
WindBag001->SetR(-29);
CreateObject(Firestone, 1272, 961);
CreateObject(Firestone, 1763, 900);
CreateObject(Firestone, 1415, 708);
CreateObject(Firestone, 772, 621);
CreateObject(Firestone, 1196, 493);
CreateObject(Firestone, 345, 692);
Lorry002->CreateContents(Firestone, 2);
CreateObject(Firestone, 2460, 1366);
CreateObject(Firestone, 2893, 671);
CreateObject(Firestone, 2998, 959);
CreateObject(Firestone, 3266, 1172);
CreateObject(Firestone, 2653, 1129);
CreateObject(Firestone, 2410, 1165);
CreateObject(Firestone, 2853, 1378);
return true;
}
//############################################################################
//主程序
int main (void)
//############################################################################
{
unsigned int timer;
//unsigned int timer2 = 0;
DDRB = 0x00;
PORTB = 0x00;
for(timer = 0; timer < 1000; timer++);
if(PINB & 0x01) PlatinenVersion = 11;
else PlatinenVersion = 10;
DDRC = 0x81; // SCL
PORTC = 0xff; // Pullup SDA
DDRB = 0x1B; // LEDs und offset
PORTB = 0x01; // LED_Rot
DDRD = 0x3E; // Speaker & TXD & J3 J4 J5
DDRD |=0x80; // J7
PORTD = 0xF7; // LED
MCUSR &=~(1<<WDRF);
WDTCSR |= (1<<WDCE)|(1<<WDE);
WDTCSR = 0;
beeptime = 2000;
StickGier = 0;
PPM_in[K_GAS] = 0;
StickRoll = 0;
StickNick = 0;
ROT_OFF;
Timer_Init();
UART_Init();
rc_sum_init();
ADC_Init();
i2c_init();
SPI_MasterInit();
sei();
VersionInfo.Hauptversion = VERSION_HAUPTVERSION;
VersionInfo.Nebenversion = VERSION_NEBENVERSION;
VersionInfo.PCKompatibel = VERSION_KOMPATIBEL;
printf("\n\rFlightControl\n\rHardware:%d.%d\n\rSoftware:V%d.%d%c ",PlatinenVersion/10,PlatinenVersion%10, VERSION_HAUPTVERSION, VERSION_NEBENVERSION,VERSION_INDEX + 'a');
printf("\n\r==============================");
GRN_ON;
#define EE_DATENREVISION 69 // wird angepasst, wenn sich die EEPROM-Daten ge鋘dert haben
if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_VALID]) != EE_DATENREVISION)
{
printf("\n\rInit. EEPROM: Generiere Default-Parameter...");
DefaultKonstanten1();
for (unsigned char i=0; i<6; i++)
{
if(i==2) DefaultKonstanten2(); // 相机
if(i==3) DefaultKonstanten3(); // 初学者
if(i>3) DefaultKonstanten2(); // 相机
WriteParameterSet(i, (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
}
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], 3); // default-Setting
eeprom_write_byte(&EEPromArray[EEPROM_ADR_VALID], EE_DATENREVISION);
}
if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) > 4)
{
printf("\n\rACC nicht abgeglichen!");
}
ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
printf("\n\rBenutze Parametersatz %d", GetActiveParamSetNumber());
if(EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)
{
printf("\n\rAbgleich Luftdrucksensor..");
timer = SetDelay(1000);
SucheLuftruckOffset();
while (!CheckDelay(timer));
printf("OK\n\r");
}
SetNeutral();
ROT_OFF;
beeptime = 2000;
ExternControl.Digital[0] = 0x55;
printf("\n\rSteuerung: ");
if (EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) printf("HeadingHold");
else printf("Neutral");
printf("\n\n\r");
LcdClear();
I2CTimeout = 5000;
while (1)
{
if(UpdateMotor) // 间隙调整
{
SPI_TransmitByte(); //#
UpdateMotor=0;
MotorRegler();
SendMotorData();
ROT_OFF;
if(PcZugriff) PcZugriff--;
else
{
DubWiseKeys[0] = 0;
DubWiseKeys[1] = 0;
ExternControl.Config = 0;
ExternStickNick = 0;
ExternStickRoll = 0;
ExternStickGier = 0;
}
if(SenderOkay) SenderOkay--;
if(!I2CTimeout)
{
I2CTimeout = 5;
i2c_reset();
if((BeepMuster == 0xffff) && MotorenEin)
{
beeptime = 10000;
BeepMuster = 0x0080;
}
}
else
{
I2CTimeout--;
ROT_OFF;
}
if(SIO_DEBUG && !UpdateMotor)
{
DatenUebertragung();
BearbeiteRxDaten();
}
else BearbeiteRxDaten();
if(CheckDelay(timer))
{
if(UBat < EE_Parameter.UnterspannungsWarnung)
{
if(BeepMuster == 0xffff)
{
beeptime = 6000;
BeepMuster = 0x0300;
}
}
SPI_StartTransmitPacket();//#
timer = SetDelay(100);
}
}
}
return (1);
}
//############################################################################
//
void MotorRegler(void)
//############################################################################
{
int motorwert,pd_ergebnis,h,tmp_int;
int GierMischanteil,GasMischanteil;
static long SummeNick=0,SummeRoll=0;
static long sollGier = 0,tmp_long,tmp_long2;
static long IntegralFehlerNick = 0;
static long IntegralFehlerRoll = 0;
static unsigned int RcLostTimer;
static unsigned char delay_neutral = 0;
static unsigned char delay_einschalten = 0,delay_ausschalten = 0;
static unsigned int modell_fliegt = 0;
static int hoehenregler = 0;
static char TimerWerteausgabe = 0;
static char NeueKompassRichtungMerken = 0;
static long ausgleichNick, ausgleichRoll;
Mittelwert();
GRN_ON;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gaswert ermitteln
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
GasMischanteil = StickGas;
if(GasMischanteil < 0) GasMischanteil = 0;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Emfang schlecht
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(SenderOkay < 100)
{
if(!PcZugriff)
{
if(BeepMuster == 0xffff)
{
beeptime = 15000;
BeepMuster = 0x0c00;
}
}
if(RcLostTimer) RcLostTimer--;
else
{
MotorenEin = 0;
Notlandung = 0;
}
ROT_ON;
if(modell_fliegt > 2000) // wahrscheinlich in der Luft --> langsam absenken
{
GasMischanteil = EE_Parameter.NotGas;
Notlandung = 1;
PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] = 0;
PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;
PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] = 0;
}
else MotorenEin = 0;
}
else
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Emfang gut
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(SenderOkay > 140)
{
Notlandung = 0;
RcLostTimer = EE_Parameter.NotGasZeit * 50;
if(GasMischanteil > 40)
{
if(modell_fliegt < 0xffff) modell_fliegt++;
}
if((modell_fliegt < 200) || (GasMischanteil < 40))
{
SummeNick = 0;
SummeRoll = 0;
Mess_Integral_Gier = 0;
Mess_Integral_Gier2 = 0;
}
if((PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] > 80) && MotorenEin == 0)
{
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// auf Nullwerte kalibrieren
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75) // Neutralwerte
{
if(++delay_neutral > 200) // nicht sofort
{
GRN_OFF;
MotorenEin = 0;
delay_neutral = 0;
modell_fliegt = 0;
if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70 || abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) > 70)
{
unsigned char setting=1;
if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 1;
if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 2;
if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 3;
if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 4;
if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 5;
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], setting); // aktiven Datensatz merken
}
if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert?
{
if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
}
ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
SetNeutral();
Piep(GetActiveParamSetNumber());
}
}
else
if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75) // ACC Neutralwerte speichern
{
if(++delay_neutral > 200) // nicht sofort
{
GRN_OFF;
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK],0xff); // Werte löschen
MotorenEin = 0;
delay_neutral = 0;
modell_fliegt = 0;
SetNeutral();
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK],NeutralAccX / 256); // ACC-NeutralWerte speichern
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK+1],NeutralAccX % 256); // ACC-NeutralWerte speichern
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL],NeutralAccY / 256);
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL+1],NeutralAccY % 256);
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_Z],(int)NeutralAccZ / 256);
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_Z+1],(int)NeutralAccZ % 256);
Piep(GetActiveParamSetNumber());
}
}
else delay_neutral = 0;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gas ist unten
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] < 35-120)
{
// Starten
if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75)
{
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Einschalten
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(++delay_einschalten > 200)
{
delay_einschalten = 200;
modell_fliegt = 1;
MotorenEin = 1;
sollGier = 0;
Mess_Integral_Gier = 0;
Mess_Integral_Gier2 = 0;
Mess_IntegralNick = 0;
Mess_IntegralRoll = 0;
Mess_IntegralNick2 = IntegralNick;
Mess_IntegralRoll2 = IntegralRoll;
SummeNick = 0;
SummeRoll = 0;
}
}
else delay_einschalten = 0;
//Auf Neutralwerte setzen
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Auschalten
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)
{
if(++delay_ausschalten > 200) // nicht sofort
{
MotorenEin = 0;
delay_ausschalten = 200;
modell_fliegt = 0;
}
}
else delay_ausschalten = 0;
}
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// neue Werte von der Funke
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(!NewPpmData-- || Notlandung)
{
int tmp_int;
static int stick_nick,stick_roll;
ParameterZuordnung();
StickNick = (StickNick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4;
StickNick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
StickRoll = (StickRoll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4;
StickRoll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
StickGas = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]) > MaxStickNick)
MaxStickNick = abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]); else MaxStickNick--;
if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) > MaxStickRoll)
MaxStickRoll = abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]); else MaxStickRoll--;
if(Notlandung) {MaxStickNick = 0; MaxStickRoll = 0;}
GyroFaktor = ((float)Parameter_Gyro_P + 10.0) / 256.0;
IntegralFaktor = ((float) Parameter_Gyro_I) / 44000;
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+ Digitale Steuerung per DubWise
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#define KEY_VALUE (Parameter_UserParam1 * 4) //(Poti3 * 8)
if(DubWiseKeys[1]) beeptime = 10;
if(DubWiseKeys[1] & DUB_KEY_UP) tmp_int = KEY_VALUE; else
if(DubWiseKeys[1] & DUB_KEY_DOWN) tmp_int = -KEY_VALUE; else tmp_int = 0;
ExternStickNick = (ExternStickNick * 7 + tmp_int) / 8;
if(DubWiseKeys[1] & DUB_KEY_LEFT) tmp_int = KEY_VALUE; else
if(DubWiseKeys[1] & DUB_KEY_RIGHT) tmp_int = -KEY_VALUE; else tmp_int = 0;
ExternStickRoll = (ExternStickRoll * 7 + tmp_int) / 8;
if(DubWiseKeys[0] & 8) ExternStickGier = 50;else
if(DubWiseKeys[0] & 4) ExternStickGier =-50;else ExternStickGier = 0;
if(DubWiseKeys[0] & 2) ExternHoehenValue++;
if(DubWiseKeys[0] & 16) ExternHoehenValue--;
StickNick += ExternStickNick / 8;
StickRoll += ExternStickRoll / 8;
StickGier += ExternStickGier;
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+ Analoge Steuerung per Seriell
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(ExternControl.Config & 0x01 && Parameter_UserParam1 > 128)
{
StickNick += (int) ExternControl.Nick * (int) EE_Parameter.Stick_P;
StickRoll += (int) ExternControl.Roll * (int) EE_Parameter.Stick_P;
StickGier += ExternControl.Gier;
ExternHoehenValue = (int) ExternControl.Hight * (int)EE_Parameter.Hoehe_Verstaerkung;
if(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas;
}
if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor = 0;
if(GyroFaktor < 0) GyroFaktor = 0;
if(IntegralFaktor < 0) IntegralFaktor = 0;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Looping?
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_LINKS) Looping_Links = 1;
else
{
{
if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < (EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese))) Looping_Links = 0;
}
}
if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_RECHTS) Looping_Rechts = 1;
else
{
if(Looping_Rechts) // Hysterese
{
if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > -(EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese)) Looping_Rechts = 0;
}
}
if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_OBEN) Looping_Oben = 1;
else
{
if(Looping_Oben) // Hysterese
{
if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < (EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese))) Looping_Oben = 0;
}
}
if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_UNTEN) Looping_Unten = 1;
else
{
if(Looping_Unten) // Hysterese
{
if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > -(EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese)) Looping_Unten = 0;
}
}
if(Looping_Links || Looping_Rechts) Looping_Roll = 1; else Looping_Roll = 0;
if(Looping_Oben || Looping_Unten) {Looping_Nick = 1; Looping_Roll = 0; Looping_Links = 0; Looping_Rechts = 0;} else Looping_Nick = 0;
} // Ende neue Funken-Werte
if(Looping_Roll) beeptime = 100;
if(Looping_Roll || Looping_Nick)
{
if(GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Bei Empfangsausfall im Flug
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(Notlandung)
{
StickGier = 0;
StickNick = 0;
StickRoll = 0;
GyroFaktor = 0.1;
IntegralFaktor = 0.005;
Looping_Roll = 0;
Looping_Nick = 0;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Integrale auf ACC-Signal abgleichen
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#define ABGLEICH_ANZAHL 256L
MittelIntegralNick += IntegralNick; // Für die Mittelwertbildung aufsummieren
MittelIntegralRoll += IntegralRoll;
MittelIntegralNick2 += IntegralNick2;
MittelIntegralRoll2 += IntegralRoll2;
if(Looping_Nick || Looping_Roll)
{
IntegralAccNick = 0;
IntegralAccRoll = 0;
MittelIntegralNick = 0;
MittelIntegralRoll = 0;
MittelIntegralNick2 = 0;
MittelIntegralRoll2 = 0;
Mess_IntegralNick2 = Mess_IntegralNick;
Mess_IntegralRoll2 = Mess_IntegralRoll;
ZaehlMessungen = 0;
LageKorrekturNick = 0;
LageKorrekturRoll = 0;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(!Looping_Nick && !Looping_Roll)
{
long tmp_long, tmp_long2;
tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick);
tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll);
tmp_long /= 16;
tmp_long2 /= 16;
if((MaxStickNick > 15) || (MaxStickRoll > 15))
{
tmp_long /= 3;
tmp_long2 /= 3;
}
if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)
{
tmp_long /= 3;
tmp_long2 /= 3;
}
#define AUSGLEICH 32
if(tmp_long > AUSGLEICH) tmp_long = AUSGLEICH;
if(tmp_long < -AUSGLEICH) tmp_long =-AUSGLEICH;
if(tmp_long2 > AUSGLEICH) tmp_long2 = AUSGLEICH;
if(tmp_long2 <-AUSGLEICH) tmp_long2 =-AUSGLEICH;
Mess_IntegralNick -= tmp_long;
Mess_IntegralRoll -= tmp_long2;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(ZaehlMessungen >= ABGLEICH_ANZAHL)
{
static int cnt = 0;
static char last_n_p,last_n_n,last_r_p,last_r_n;
static long MittelIntegralNick_Alt,MittelIntegralRoll_Alt;
if(!Looping_Nick && !Looping_Roll)
{
MittelIntegralNick /= ABGLEICH_ANZAHL;
MittelIntegralRoll /= ABGLEICH_ANZAHL;
IntegralAccNick = (EE_Parameter.GyroAccFaktor * IntegralAccNick) / ABGLEICH_ANZAHL;
IntegralAccRoll = (EE_Parameter.GyroAccFaktor * IntegralAccRoll) / ABGLEICH_ANZAHL;
IntegralAccZ = IntegralAccZ / ABGLEICH_ANZAHL;
#define MAX_I 0//(Poti2/10)
// Nick ++++++++++++++++++++++++++++++++++++++++++++++++
IntegralFehlerNick = (long)(MittelIntegralNick - (long)IntegralAccNick);
ausgleichNick = IntegralFehlerNick / EE_Parameter.GyroAccAbgleich;
// Roll ++++++++++++++++++++++++++++++++++++++++++++++++
IntegralFehlerRoll = (long)(MittelIntegralRoll - (long)IntegralAccRoll);
ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich;
LageKorrekturNick = ausgleichNick / ABGLEICH_ANZAHL;
LageKorrekturRoll = ausgleichRoll / ABGLEICH_ANZAHL;
if((MaxStickNick > 15) || (MaxStickRoll > 15) || (abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25))
{
LageKorrekturNick /= 2;
LageKorrekturNick /= 2;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gyro-Drift ermitteln
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
MittelIntegralNick2 /= ABGLEICH_ANZAHL;
MittelIntegralRoll2 /= ABGLEICH_ANZAHL;
tmp_long = IntegralNick2 - IntegralNick;
tmp_long2 = IntegralRoll2 - IntegralRoll;
//DebugOut.Analog[25] = MittelIntegralRoll2 / 26;
IntegralFehlerNick = tmp_long;
IntegralFehlerRoll = tmp_long2;
Mess_IntegralNick2 -= IntegralFehlerNick;
Mess_IntegralRoll2 -= IntegralFehlerRoll;
// IntegralFehlerNick = (IntegralFehlerNick * 1 + tmp_long) / 2;
// IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2;
DebugOut.Analog[17] = IntegralAccNick / 26;
DebugOut.Analog[18] = IntegralAccRoll / 26;
DebugOut.Analog[19] = IntegralFehlerNick;// / 26;
DebugOut.Analog[20] = IntegralFehlerRoll;// / 26;
DebugOut.Analog[21] = MittelIntegralNick / 26;
DebugOut.Analog[22] = MittelIntegralRoll / 26;
//DebugOut.Analog[28] = ausgleichNick;
DebugOut.Analog[29] = ausgleichRoll;
DebugOut.Analog[30] = LageKorrekturRoll * 10;
#define FEHLER_LIMIT (ABGLEICH_ANZAHL * 4)
#define FEHLER_LIMIT2 (ABGLEICH_ANZAHL * 16)
#define BEWEGUNGS_LIMIT 20000
// Nick +++++++++++++++++++++++++++++++++++++++++++++++++
cnt = 1;// + labs(IntegralFehlerNick) / 4096;
if(labs(MittelIntegralNick_Alt - MittelIntegralNick) < BEWEGUNGS_LIMIT)
{
if(IntegralFehlerNick > FEHLER_LIMIT2)
{
if(last_n_p)
{
cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2;
ausgleichNick = IntegralFehlerNick / 8;
if(ausgleichNick > 5000) ausgleichNick = 5000;
LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL;
}
else last_n_p = 1;
} else last_n_p = 0;
if(IntegralFehlerNick < -FEHLER_LIMIT2)
{
if(last_n_n)
{
cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2;
ausgleichNick = IntegralFehlerNick / 8;
if(ausgleichNick < -5000) ausgleichNick = -5000;
LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL;
}
else last_n_n = 1;
} else last_n_n = 0;
} else cnt = 0;
if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
if(IntegralFehlerNick > FEHLER_LIMIT) AdNeutralNick += cnt;
if(IntegralFehlerNick < -FEHLER_LIMIT) AdNeutralNick -= cnt;
// Roll +++++++++++++++++++++++++++++++++++++++++++++++++
cnt = 1;// + labs(IntegralFehlerNick) / 4096;
ausgleichRoll = 0;
if(labs(MittelIntegralRoll_Alt - MittelIntegralRoll) < BEWEGUNGS_LIMIT)
{
if(IntegralFehlerRoll > FEHLER_LIMIT2)
{
if(last_r_p)
{
cnt += labs(IntegralFehlerRoll) / FEHLER_LIMIT2;
ausgleichRoll = IntegralFehlerRoll / 8;
if(ausgleichRoll > 5000) ausgleichRoll = 5000;
LageKorrekturRoll += ausgleichRoll / ABGLEICH_ANZAHL;
}
else last_r_p = 1;
} else last_r_p = 0;
if(IntegralFehlerRoll < -FEHLER_LIMIT2)
{
if(last_r_n)
{
cnt += labs(IntegralFehlerRoll) / FEHLER_LIMIT2;
ausgleichRoll = IntegralFehlerRoll / 8;
if(ausgleichRoll < -5000) ausgleichRoll = -5000;
LageKorrekturRoll += ausgleichRoll / ABGLEICH_ANZAHL;
}
else last_r_n = 1;
} else last_r_n = 0;
} else
{
cnt = 0;
}
if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
if(IntegralFehlerRoll > FEHLER_LIMIT) AdNeutralRoll += cnt;
if(IntegralFehlerRoll < -FEHLER_LIMIT) AdNeutralRoll -= cnt;
DebugOut.Analog[27] = ausgleichRoll;
DebugOut.Analog[23] = AdNeutralNick;//10*(AdNeutralNick - StartNeutralNick);
DebugOut.Analog[24] = 10*(AdNeutralRoll - StartNeutralRoll);
}
else
{
LageKorrekturRoll = 0;
LageKorrekturNick = 0;
}
if(!IntegralFaktor) { LageKorrekturRoll = 0; LageKorrekturNick = 0;} // z.B. bei HH
// +++++++++++++++++++++++++++++++++++++++++++++++++++++
MittelIntegralNick_Alt = MittelIntegralNick;
MittelIntegralRoll_Alt = MittelIntegralRoll;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++
IntegralAccNick = 0;
IntegralAccRoll = 0;
IntegralAccZ = 0;
MittelIntegralNick = 0;
MittelIntegralRoll = 0;
MittelIntegralNick2 = 0;
MittelIntegralRoll2 = 0;
ZaehlMessungen = 0;
}
//DebugOut.Analog[31] = StickRoll / (26*IntegralFaktor);
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gieren
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(abs(StickGier) > 20) // war 35
{
if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1;
}
tmp_int = (long) EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo y = ax + bx²
tmp_int += (EE_Parameter.Gier_P * StickGier) / 4;
sollGier = tmp_int;
Mess_Integral_Gier -= tmp_int;
if(Mess_Integral_Gier > 50000) Mess_Integral_Gier = 50000; // begrenzen
if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Kompass
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV))
{
int w,v;
static int SignalSchlecht = 0;
w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln
v = abs(IntegralRoll /512);
if(v > w) w = v; // grösste Neigung ermitteln
if(w < 25 && NeueKompassRichtungMerken && !SignalSchlecht)
{
KompassStartwert = KompassValue;
NeueKompassRichtungMerken = 0;
}
w = (w * Parameter_KompassWirkung) / 64; // auf die Wirkung normieren
w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln
if(w > 0)
{
if(!SignalSchlecht) Mess_Integral_Gier += (KompassRichtung * w) / 32; // nach Kompass ausrichten
if(SignalSchlecht) SignalSchlecht--;
}
else SignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Debugwerte zuordnen
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(!TimerWerteausgabe--)
{
TimerWerteausgabe = 24;
DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor;
DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor;
DebugOut.Analog[2] = Mittelwert_AccNick;
DebugOut.Analog[3] = Mittelwert_AccRoll;
DebugOut.Analog[4] = MesswertGier;
DebugOut.Analog[5] = HoehenWert;
DebugOut.Analog[6] =(Mess_Integral_Hoch / 512);
DebugOut.Analog[8] = KompassValue;
DebugOut.Analog[9] = UBat;
DebugOut.Analog[10] = SenderOkay;
DebugOut.Analog[16] = Mittelwert_AccHoch;
if(Looping_Nick) MesswertNick = MesswertNick * GyroFaktor;
else MesswertNick = IntegralNick * IntegralFaktor + MesswertNick * GyroFaktor;
if(Looping_Roll) MesswertRoll = MesswertRoll * GyroFaktor;
else MesswertRoll = IntegralRoll * IntegralFaktor + MesswertRoll * GyroFaktor;
MesswertGier = MesswertGier * (2 * GyroFaktor) + Integral_Gier * IntegralFaktor / 2;
DebugOut.Analog[25] = IntegralRoll * IntegralFaktor;
DebugOut.Analog[31] = StickRoll;// / (26*IntegralFaktor);
DebugOut.Analog[28] = MesswertRoll;
// Maximalwerte abfangen
#define MAX_SENSOR 2048
if(MesswertNick > MAX_SENSOR) MesswertNick = MAX_SENSOR;
if(MesswertNick < -MAX_SENSOR) MesswertNick = -MAX_SENSOR;
if(MesswertRoll > MAX_SENSOR) MesswertRoll = MAX_SENSOR;
if(MesswertRoll < -MAX_SENSOR) MesswertRoll = -MAX_SENSOR;
if(MesswertGier > MAX_SENSOR) MesswertGier = MAX_SENSOR;
if(MesswertGier < -MAX_SENSOR) MesswertGier = -MAX_SENSOR;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Höhenregelung
// Die Höhenregelung schwächt lediglich das Gas ab, erhöht es allerdings nicht
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//OCR0B = 180 - (Poti1 + 120) / 4;
//DruckOffsetSetting = OCR0B;
if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung
{
int tmp_int;
if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER) // Regler wird über Schalter gesteuert
{
if(Parameter_MaxHoehe < 50)
{
SollHoehe = HoehenWert - 20; // Parameter_MaxHoehe ist der PPM-Wert des Schalters
HoehenReglerAktiv = 0;
}
else
HoehenReglerAktiv = 1;
}
else
{
SollHoehe = ((int) ExternHoehenValue + (int) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung - 20;
HoehenReglerAktiv = 1;
}
if(Notlandung) SollHoehe = 0;
h = HoehenWert;
if((h > SollHoehe) && HoehenReglerAktiv) // zu hoch --> drosseln
{ h = ((h - SollHoehe) * (int) Parameter_Hoehe_P) / 16; // Differenz bestimmen --> P-Anteil
h = GasMischanteil - h; // vom Gas abziehen
h -= (HoeheD * Parameter_Luftdruck_D)/8; // D-Anteil
tmp_int = ((Mess_Integral_Hoch / 512) * (signed long) Parameter_Hoehe_ACC_Wirkung) / 32;
if(tmp_int > 50) tmp_int = 50;
else if(tmp_int < -50) tmp_int = -50;
h -= tmp_int;
hoehenregler = (hoehenregler*15 + h) / 16;
if(hoehenregler < EE_Parameter.Hoehe_MinGas) // nicht unter MIN
{
if(GasMischanteil >= EE_Parameter.Hoehe_MinGas) hoehenregler = EE_Parameter.Hoehe_MinGas;
if(GasMischanteil < EE_Parameter.Hoehe_MinGas) hoehenregler = GasMischanteil;
}
if(hoehenregler > GasMischanteil) hoehenregler = GasMischanteil; // nicht mehr als Gas
GasMischanteil = hoehenregler;
}
}
if(GasMischanteil > MAX_GAS - 20) GasMischanteil = MAX_GAS - 20;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Mischer und PI-Regler
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
DebugOut.Analog[7] = GasMischanteil;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gier-Anteil
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#define MUL_G 1.0
GierMischanteil = MesswertGier - sollGier; // Regler für Gier
// GierMischanteil = 0;
if(GierMischanteil > (GasMischanteil / 2)) GierMischanteil = GasMischanteil / 2;
if(GierMischanteil < -(GasMischanteil / 2)) GierMischanteil = -(GasMischanteil / 2);
if(GierMischanteil > ((MAX_GAS - GasMischanteil))) GierMischanteil = ((MAX_GAS - GasMischanteil));
if(GierMischanteil < -((MAX_GAS - GasMischanteil))) GierMischanteil = -((MAX_GAS - GasMischanteil));
if(GasMischanteil < 20) GierMischanteil = 0;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Nick-Achse
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
DiffNick = MesswertNick - (StickNick - GPS_Nick); // Differenz bestimmen
if(IntegralFaktor) SummeNick += IntegralNick * IntegralFaktor - (StickNick - GPS_Nick); // I-Anteil bei Winkelregelung
else SummeNick += DiffNick; // I-Anteil bei HH
if(SummeNick > 16000) SummeNick = 16000;
if(SummeNick < -16000) SummeNick = -16000;
pd_ergebnis = DiffNick + Ki * SummeNick; // PI-Regler für Nick
// Motor Vorn
tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
if(pd_ergebnis > tmp_int) pd_ergebnis = tmp_int;
if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int;
motorwert = GasMischanteil + pd_ergebnis + GierMischanteil; // Mischer
if ((motorwert < 0)) motorwert = 0;
else if(motorwert > MAX_GAS) motorwert = MAX_GAS;
if (motorwert < MIN_GAS) motorwert = MIN_GAS;
Motor_Vorne = motorwert;
// Motor Heck
motorwert = GasMischanteil - pd_ergebnis + GierMischanteil;
if ((motorwert < 0)) motorwert = 0;
else if(motorwert > MAX_GAS) motorwert = MAX_GAS;
if (motorwert < MIN_GAS) motorwert = MIN_GAS;
Motor_Hinten = motorwert;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Roll-Achse
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
DiffRoll = MesswertRoll - (StickRoll - GPS_Roll); // Differenz bestimmen
if(IntegralFaktor) SummeRoll += IntegralRoll * IntegralFaktor - (StickRoll - GPS_Roll);// I-Anteil bei Winkelregelung
else SummeRoll += DiffRoll; // I-Anteil bei HH
if(SummeRoll > 16000) SummeRoll = 16000;
if(SummeRoll < -16000) SummeRoll = -16000;
pd_ergebnis = DiffRoll + Ki * SummeRoll; // PI-Regler für Roll
tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
if(pd_ergebnis > tmp_int) pd_ergebnis = tmp_int;
if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int;
// Motor Links
motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;
#define GRENZE Poti1
if ((motorwert < 0)) motorwert = 0;
else if(motorwert > MAX_GAS) motorwert = MAX_GAS;
if (motorwert < MIN_GAS) motorwert = MIN_GAS;
Motor_Links = motorwert;
// Motor Rechts
motorwert = GasMischanteil - pd_ergebnis - GierMischanteil;
if ((motorwert < 0)) motorwert = 0;
else if(motorwert > MAX_GAS) motorwert = MAX_GAS;
if (motorwert < MIN_GAS) motorwert = MIN_GAS;
Motor_Rechts = motorwert;
//############################################################################
//Hauptprogramm
int main (void)
//############################################################################
{
unsigned int timer,i,timer2 = 0, timerPolling;
DDRB = 0x00;
PORTB = 0x00;
for(timer = 0; timer < 1000; timer++); // verzögern
#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
PlatinenVersion = 21;
#else
if(PINB & 0x01)
{
if(PINB & 0x02) PlatinenVersion = 13;
else PlatinenVersion = 11;
}
else
{
if(PINB & 0x02) PlatinenVersion = 20;
else PlatinenVersion = 10;
}
#endif
DDRC = 0x81; // SCL
DDRC |=0x40; // HEF4017 Reset
PORTC = 0xff; // Pullup SDA
DDRB = 0x1B; // LEDs und Druckoffset
PORTB = 0x01; // LED_Rot
DDRD = 0x3E; // Speaker & TXD & J3 J4 J5
PORTD = 0x47; // LED
HEF4017R_ON;
MCUSR &=~(1<<WDRF);
WDTCSR |= (1<<WDCE)|(1<<WDE);
WDTCSR = 0;
beeptime = 2500;
StickGier = 0; PPM_in[K_GAS] = 0; StickRoll = 0; StickNick = 0;
if(PlatinenVersion >= 20) GIER_GRAD_FAKTOR = 1220; else GIER_GRAD_FAKTOR = 1291; // unterschiedlich für ME und ENC
ROT_OFF;
Timer_Init();
TIMER2_Init();
UART_Init();
rc_sum_init();
ADC_Init();
I2C_Init(1);
SPI_MasterInit();
Capacity_Init();
LIBFC_Init();
GRN_ON;
sei();
ParamSet_Init();
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Check connected BL-Ctrls
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Check connected BL-Ctrls
BLFlags |= BLFLAG_READ_VERSION;
motor_read = 0; // read the first I2C-Data
SendMotorData();
timer = SetDelay(500);
while(!(BLFlags & BLFLAG_TX_COMPLETE) && !CheckDelay(timer)); //wait for complete transfer
printf("\n\rFound BL-Ctrl: ");
timer = SetDelay(4000);
for(i=0; i < MAX_MOTORS; i++)
{
SendMotorData();
while(!(BLFlags & BLFLAG_TX_COMPLETE) && !CheckDelay(timer)); //wait for complete transfer
if(Mixer.Motor[i][0] > 0) // wait max 4 sec for the BL-Ctrls to wake up
{
while(!CheckDelay(timer) && !(Motor[i].State & MOTOR_STATE_PRESENT_MASK) )
{
SendMotorData();
while(!(BLFlags & BLFLAG_TX_COMPLETE) && !CheckDelay(timer)); //wait for complete transfer
}
}
if(Motor[i].State & MOTOR_STATE_PRESENT_MASK)
{
printf("%d",i+1);
FoundMotors++;
// if(Motor[i].Version & MOTOR_STATE_NEW_PROTOCOL_MASK) printf("(new) ");
}
}
for(i=0; i < MAX_MOTORS; i++)
{
if(!(Motor[i].State & MOTOR_STATE_PRESENT_MASK) && Mixer.Motor[i][0] > 0)
{
printf("\n\r\n\r!! MISSING BL-CTRL: %d !!",i+1);
ServoActive = 2; // just in case the FC would be used as camera-stabilizer
}
Motor[i].State &= ~MOTOR_STATE_ERROR_MASK; // clear error counter
}
printf("\n\r===================================");
if(RequiredMotors < FoundMotors) VersionInfo.HardwareError[1] |= FC_ERROR1_MIXER;
//if(EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)
{
printf("\n\rCalibrating pressure sensor..");
timer = SetDelay(1000);
SucheLuftruckOffset();
while (!CheckDelay(timer));
printf("OK\n\r");
}
SetNeutral(0);
ROT_OFF;
beeptime = 2000;
ExternControl.Digital[0] = 0x55;
FlugMinuten = (unsigned int)GetParamByte(PID_FLIGHT_MINUTES) * 256 + (unsigned int)GetParamByte(PID_FLIGHT_MINUTES + 1);
FlugMinutenGesamt = (unsigned int)GetParamByte(PID_FLIGHT_MINUTES_TOTAL) * 256 + (unsigned int)GetParamByte(PID_FLIGHT_MINUTES_TOTAL + 1);
if((FlugMinutenGesamt == 0xFFFF) || (FlugMinuten == 0xFFFF))
{
FlugMinuten = 0;
FlugMinutenGesamt = 0;
}
printf("\n\rFlight-time %u min Total:%u min", FlugMinuten, FlugMinutenGesamt);
printf("\n\rControl: ");
if (EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) printf("HeadingHold");
else printf("Normal (ACC-Mode)");
LcdClear();
I2CTimeout = 5000;
WinkelOut.Orientation = 1;
LipoDetection(1);
LIBFC_ReceiverInit(EE_Parameter.Receiver);
printf("\n\r===================================\n\r");
//SpektrumBinding();
timer = SetDelay(2000);
timerPolling = SetDelay(250);
Debug(ANSI_CLEAR "FC-Start!\n\rFlugzeit: %d min", FlugMinutenGesamt); // Note: this won't waste flash memory, if #DEBUG is not active
DebugOut.Status[0] = 0x01 | 0x02;
JetiBeep = 0;
while (1)
{
if (JetiUpdateModeActive) while (1);
if(CheckDelay(timerPolling))
{
timerPolling = SetDelay(100);
LIBFC_Polling();
}
if(UpdateMotor && AdReady) // ReglerIntervall
{
UpdateMotor=0;
if(WinkelOut.CalcState) CalMk3Mag();
else MotorRegler();
SendMotorData();
ROT_OFF;
if(SenderOkay) { SenderOkay--; VersionInfo.HardwareError[1] &= ~FC_ERROR1_PPM; }
else
{
TIMSK1 |= _BV(ICIE1); // enable PPM-Input
PPM_in[0] = 0; // set RSSI to zero on data timeout
VersionInfo.HardwareError[1] |= FC_ERROR1_PPM;
}
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//if(HoehenReglerAktiv && NaviDataOkay && SenderOkay < 160 && SenderOkay > 10 && FromNaviCtrl_Value.SerialDataOkay > 220) SenderOkay = 160;
//if(HoehenReglerAktiv && NaviDataOkay && SenderOkay < 101 && SenderOkay > 10 && FromNaviCtrl_Value.SerialDataOkay > 1) SenderOkay = 101;
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(!--I2CTimeout || MissingMotor)
{
if(!I2CTimeout)
{
I2C_Reset();
I2CTimeout = 5;
DebugOut.Analog[28]++; // I2C-Error
VersionInfo.HardwareError[1] |= FC_ERROR1_I2C;
DebugOut.Status[1] |= 0x02; // BL-Error-Status
}
if((BeepMuster == 0xffff) && MotorenEin)
{
beeptime = 10000;
BeepMuster = 0x0080;
}
}
else
{
ROT_OFF;
if(!beeptime)
{
VersionInfo.HardwareError[1] &= ~FC_ERROR1_I2C;
}
}
if(!UpdateMotor)
{
if(CalculateServoSignals) CalculateServo();
DatenUebertragung();
BearbeiteRxDaten();
if(CheckDelay(timer))
{
static unsigned char second;
timer += 20; // 20 ms interval
if(MissingMotor)
{
VersionInfo.HardwareError[1] |= FC_ERROR1_BL_MISSING;
DebugOut.Status[1] |= 0x02; // BL-Error-Status
}
else
{
VersionInfo.HardwareError[1] &= ~FC_ERROR1_BL_MISSING;
if(I2CTimeout > 6) DebugOut.Status[1] &= ~0x02; // BL-Error-Status
}
if(I2CTimeout > 6) VersionInfo.HardwareError[1] &= ~FC_ERROR1_I2C;
if(PcZugriff) PcZugriff--;
else
{
ExternControl.Config = 0;
ExternStickNick = 0;
ExternStickRoll = 0;
ExternStickGier = 0;
if(BeepMuster == 0xffff && SenderOkay == 0)
{
beeptime = 15000;
BeepMuster = 0x0c00;
}
}
if(NaviDataOkay > 200)
{
NaviDataOkay--;
VersionInfo.HardwareError[1] &= ~FC_ERROR1_SPI_RX;
}
else
{
if(NC_Version.Compatible)
{
VersionInfo.HardwareError[1] |= FC_ERROR1_SPI_RX;
if(BeepMuster == 0xffff && MotorenEin)
{
beeptime = 15000;
BeepMuster = 0xA800;
}
}
GPS_Nick = 0;
GPS_Roll = 0;
//if(!beeptime)
FromNaviCtrl.CompassValue = -1;
NaviDataOkay = 0;
}
if(UBat < BattLowVoltageWarning)
{
FC_StatusFlags |= FC_STATUS_LOWBAT;
if(BeepMuster == 0xffff)
{
beeptime = 6000;
BeepMuster = 0x0300;
}
}
else if(!beeptime) FC_StatusFlags &= ~FC_STATUS_LOWBAT;
SPI_StartTransmitPacket();
SendSPI = 4;
if(!MotorenEin) timer2 = 1450; // 0,5 Minuten aufrunden
else
if(++second == 49)
{
second = 0;
FlugSekunden++;
}
if(++timer2 == 2930) // eine Minute
{
timer2 = 0;
FlugMinuten++;
FlugMinutenGesamt++;
SetParamByte(PID_FLIGHT_MINUTES,FlugMinuten / 256);
SetParamByte(PID_FLIGHT_MINUTES+1,FlugMinuten % 256);
SetParamByte(PID_FLIGHT_MINUTES_TOTAL,FlugMinutenGesamt / 256);
SetParamByte(PID_FLIGHT_MINUTES_TOTAL+1,FlugMinutenGesamt % 256);
timer = SetDelay(20); // falls "timer += 20;" mal nicht geht
}
}
LED_Update();
Capacity_Update();
} //else DebugOut.Analog[26]++;
}
if(!SendSPI) { SPI_TransmitByte(); }
}
return (1);
}
