Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 818 → Rev 819

/trunk/fc.c
87,7 → 87,6
volatile int DiffNick,DiffRoll;
int Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0;
volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count;
unsigned char MotorWert[5];
volatile unsigned char SenderOkay = 0;
int StickNick = 0,StickRoll = 0,StickGier = 0,StickGas = 0;
char MotorenEin = 0;
412,6 → 411,8
}
 
 
 
 
//############################################################################
//
void MotorRegler(void)
515,13 → 516,22
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?
else
if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) < 20 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -70)
{
WinkelOut.CalcState = 1;
beeptime = 1000;
}
else
{
ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert?
{
if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
}
ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
SetNeutral();
Piep(GetActiveParamSetNumber());
}
SetNeutral();
Piep(GetActiveParamSetNumber());
}
}
}
else
823,7 → 833,6
 
// IntegralFehlerNick = (IntegralFehlerNick * 1 + tmp_long) / 2;
// IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2;
DebugOut.Analog[10] = GierGyroFehler;
if(GierGyroFehler > ABGLEICH_ANZAHL/2) AdNeutralGier++;
if(GierGyroFehler <-ABGLEICH_ANZAHL/2) AdNeutralGier--;
 
948,7 → 957,7
// if(GasMischanteil < 35) { if(StickGier > 10) StickGier = 10; else if(StickGier < -10) StickGier = -10;};
if(abs(StickGier) > 20) // war 35
{
if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) { NeueKompassRichtungMerken = 1; KompassSignalSchlecht = 500;};
if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) { NeueKompassRichtungMerken = 1; KompassSignalSchlecht = 250;};
}
tmp_int = (long) EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo y = ax + bx²
tmp_int += (EE_Parameter.Gier_P * StickGier) / 4;
960,9 → 969,11
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Kompass
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
DebugOut.Analog[29] = (MaxStickNick + MaxStickRoll);
 
if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV))
{
int w,v,fehler,korrektur;
int w,v,r,fehler,korrektur;
static int KompassSignalSchlecht = 0;
w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln
v = abs(IntegralRoll /512);
969,6 → 980,7
//v /= 4;
//w /= 4;
if(v > w) w = v; // grösste Neigung ermitteln
// if((MaxStickNick > 32) || (MaxStickRoll > 32)) w *= 2;
korrektur = w + 4;
if(w < 25 && NeueKompassRichtungMerken && !KompassSignalSchlecht)
{
986,8 → 998,12
if(!KompassSignalSchlecht)
{
GierGyroFehler += fehler;
v = (KompassRichtung * w) / 32; // nach Kompass ausrichten
w = Parameter_KompassWirkung;
v = 64 + ((MaxStickNick + MaxStickRoll)) / 8;
//v = 32;
//r = ((540 + (ErsatzKompass/GIER_GRAD_FAKTOR) - KompassStartwert) % 360) - 180;
r = KompassRichtung;
v = (r * w) / v; // nach Kompass ausrichten
w = 3 * Parameter_KompassWirkung;
if(v > w) v = w; // Begrenzen
else
if(v < -w) v = -w;
995,7 → 1011,7
}
if(KompassSignalSchlecht) KompassSignalSchlecht--;
}
else KompassSignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek
else KompassSignalSchlecht = 250; // so lange das Signal taub stellen --> ca. 0,5 sek
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
1021,6 → 1037,7
 
DebugOut.Analog[17] = FromNaviCtrl_Value.Distance;
DebugOut.Analog[18] = (int)FromNaviCtrl_Value.OsdBar;
DebugOut.Analog[19] = WinkelOut.CalcState;
 
DebugOut.Analog[30] = GPS_Nick;
DebugOut.Analog[31] = GPS_Roll;
/trunk/fc.h
47,7 → 47,6
extern volatile int DiffNick,DiffRoll;
extern int Poti1, Poti2, Poti3, Poti4;
extern volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count;
extern unsigned char MotorWert[5];
extern volatile unsigned char SenderOkay;
extern int StickNick,StickRoll,StickGier;
extern char MotorenEin;
/trunk/main.c
84,12 → 84,32
return(set);
}
 
void CalMk3Mag(void)
{
static unsigned char stick = 1;
 
if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > -20) stick = 0;
if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -70) && !stick)
{
stick = 1;
WinkelOut.CalcState++;
if(WinkelOut.CalcState > 4)
{
// WinkelOut.CalcState = 0; // in Uart.c
beeptime = 1000;
}
else Piep(WinkelOut.CalcState);
}
DebugOut.Analog[19] = WinkelOut.CalcState;
}
 
 
//############################################################################
//Hauptprogramm
int main (void)
//############################################################################
{
unsigned int timer, alt_ms;
unsigned int timer;
 
//unsigned int timer2 = 0;
DDRB = 0x00;
183,6 → 203,7
LcdClear();
I2CTimeout = 5000;
WinkelOut.Orientation = 1;
while (1)
{
190,7 → 211,8
{
UpdateMotor=0;
//PORTD |= 0x08;
MotorRegler();
if(WinkelOut.CalcState) CalMk3Mag();
else MotorRegler();
//PORTD &= ~0x08;
SendMotorData();
ROT_OFF;
/trunk/rc.c
47,7 → 47,6
signal = (unsigned int) ICR1 - AltICR;
AltICR = ICR1;
//Syncronisationspause?
// if((signal > (int) Parameter_UserParam2 * 10) && (signal < 8000))
if((signal > 1100) && (signal < 8000))
/trunk/spi.c
10,7 → 10,7
unsigned char SPI_BufferIndex;
unsigned char SPI_RxBufferIndex;
 
volatile unsigned char SPI_Buffer[sizeof(FromNaviCtrl_Value)];
volatile unsigned char SPI_Buffer[sizeof(FromNaviCtrl_Value) + 8];
unsigned char *SPI_TX_Buffer, *ToNaviCtrl_Chksum;
 
unsigned char SPITransferCompleted, SPI_ChkSum;
36,12 → 36,12
ToNaviCtrl_Value.Sync2 = 0x83;
ToNaviCtrl_Value.Command = SPI_CMD_VALUE;
ToNaviCtrl_Value.IntegralNick = 12345;
ToNaviCtrl_Value.IntegralRoll = 56789;
ToNaviCtrl_Value.StickNick = 100;
ToNaviCtrl_Value.StickRoll = 150;//(char) StickRoll;
ToNaviCtrl_Value.StickGier = 200;//(char) StickGier;
ToNaviCtrl_Value.IntegralNick = 0;
ToNaviCtrl_Value.IntegralRoll = 0;
ToNaviCtrl_Value.StickNick = 0;
ToNaviCtrl_Value.StickRoll = 0;//(char) StickRoll;
ToNaviCtrl_Value.StickGier = 0;//(char) StickGier;
ToNaviCtrl_Value.CalState = 0;
SPI_RxDataValid = 0;
}
 
164,13 → 164,13
ToNaviCtrl_Value.Command = SPI_CMD_VALUE;
ToNaviCtrl_Value.IntegralNick = (int) (IntegralNick / 1024);
ToNaviCtrl_Value.IntegralRoll = (int) (IntegralRoll / 1024);
ToNaviCtrl_Value.AccNick = (int) ACC_AMPLIFY * (NaviAccNick / NaviCntAcc);
ToNaviCtrl_Value.AccRoll = (int) ACC_AMPLIFY * (Mittelwert_AccRoll / NaviCntAcc);
NaviCntAcc = 0; NaviAccNick = 0; NaviAccRoll = 0;
ToNaviCtrl_Value.StickNick = (char) PPM_in[EE_Parameter.Kanalbelegung[K_NICK]];
ToNaviCtrl_Value.StickRoll = (char) PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]];
ToNaviCtrl_Value.StickGier = (char) PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
ToNaviCtrl_Value.GyroCompass = ErsatzKompass / GIER_GRAD_FAKTOR;
ToNaviCtrl_Value.AccNick = (int) ACC_AMPLIFY * (NaviAccNick / NaviCntAcc);
ToNaviCtrl_Value.AccRoll = (int) ACC_AMPLIFY * (Mittelwert_AccRoll / NaviCntAcc);
NaviCntAcc = 0; NaviAccNick = 0; NaviAccRoll = 0;
ToNaviCtrl_Value.User1 = Parameter_UserParam1;
ToNaviCtrl_Value.User2 = Parameter_UserParam2;
ToNaviCtrl_Value.User3 = Parameter_UserParam3;
178,7 → 178,8
ToNaviCtrl_Value.User5 = Parameter_UserParam5;
ToNaviCtrl_Value.User6 = Parameter_UserParam6;
ToNaviCtrl_Value.User7 = Parameter_UserParam7;
ToNaviCtrl_Value.User8 = Parameter_UserParam8;
// ToNaviCtrl_Value.User8 = Parameter_UserParam8;
ToNaviCtrl_Value.CalState = WinkelOut.CalcState;
sei();
if (SPI_RxDataValid)
192,8 → 193,8
}
else
{
KompassValue = 0;
KompassRichtung = 0;
// KompassValue = 0;
// KompassRichtung = 0;
GPS_Nick = 0;
GPS_Roll = 0;
/trunk/spi.h
57,10 → 57,10
signed int IntegralRoll;
signed int AccNick;
signed int AccRoll;
char StickNick,StickRoll,
StickGier, StickGas;
char StickNick,StickRoll, StickGier, StickGas;
signed int GyroCompass;
char User1, User2, User3, User4, User5, User6, User7, User8;
unsigned char User1, User2, User3, User4, User5, User6, User7;//, User8;
unsigned char CalState;
unsigned char Chksum;
};
 
/trunk/uart.c
56,7 → 56,7
"Acc_Z ",
"Distance ",
"OsdBar ",
" ",
"MK3Mag CalState ",
" ", //20
" ",
" ",
371,13 → 371,12
 
if((CheckDelay(Kompass_Timer)) && UebertragungAbgeschlossen)
{
WinkelOut.Winkel[0] = IntegralNick / 1024; // etwa in Grad
WinkelOut.Winkel[1] = IntegralRoll / 1024; // etwa in Grad
WinkelOut.Winkel[0] = (int) (IntegralNick / 1024); // etwa in Grad
WinkelOut.Winkel[1] = (int) (IntegralRoll / 1024); // etwa in Grad
WinkelOut.UserParameter[0] = Parameter_UserParam1;
WinkelOut.UserParameter[1] = Parameter_UserParam2;
WinkelOut.UserParameter[2] = Parameter_UserParam3;
WinkelOut.UserParameter[3] = Parameter_UserParam4;
SendOutData('w',MeineSlaveAdresse,(unsigned char *) &WinkelOut,sizeof(WinkelOut));
if(WinkelOut.CalcState > 4) WinkelOut.CalcState = 0;
Kompass_Timer = SetDelay(99);
}
 
/trunk/uart.h
40,11 → 40,12
struct str_WinkelOut
{
unsigned int Winkel[2];
unsigned char UserParameter[4];
unsigned char UserParameter[2];
unsigned char CalcState;
unsigned char Orientation;
};
extern struct str_WinkelOut WinkelOut;
 
 
struct str_ExternControl
{
unsigned char Digital[2];