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