/branches/MicroMag3_Nick666/V0.69k/mymath.h |
---|
File deleted |
/branches/MicroMag3_Nick666/V0.69k/mymath.c |
---|
File deleted |
/branches/MicroMag3_Nick666/V0.69k/compass.c |
---|
File deleted |
/branches/MicroMag3_Nick666/V0.69k/compass.h |
---|
File deleted |
/branches/MicroMag3_Nick666/V0.69k/fc.c |
---|
71,12 → 71,9 |
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; |
uint8_t updKompass; |
volatile int KompassValue = 0; |
volatile int KompassStartwert = 0; |
volatile int KompassRichtung = 0; |
unsigned int KompassSignalSchlecht = 500; |
unsigned char MAX_GAS,MIN_GAS; |
unsigned char Notlandung = 0; |
161,12 → 158,6 |
acc_neutral.Y = abs(Mittelwert_AccRoll) / ACC_AMPLIFY; |
acc_neutral.Z = Aktuell_az; |
Piep(2); |
while (PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > -100) //Warten, bis Benutzer den Kopter neu ausgerichtet hat |
Delay_ms_Mess(100); |
acc_neutral.compass = Aktuell_az; |
eeprom_write_block(&acc_neutral,&ee_acc_neutral,sizeof(struct acc_neutral_struct)); |
} |
996,50 → 987,44 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//DebugOut.Analog[29] = (MaxStickNick + MaxStickRoll); |
if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) |
{ |
int w,v,r,fehler,korrektur; |
if (!updKompass--) // Aufruf mit ~20 Hz |
{ |
updKompass = 25; |
KompassValue = heading_MM3(); |
KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180; |
} |
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; |
if(w < 25 && NeueKompassRichtungMerken && !KompassSignalSchlecht) |
{ |
beeptime = 200; |
//KompassStartwert = KompassValue; |
KompassStartwert = (ErsatzKompass/GIER_GRAD_FAKTOR); |
NeueKompassRichtungMerken = 0; |
} |
fehler = ((540 + KompassValue - (ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180; |
ErsatzKompass += (fehler * 8) / korrektur; |
w = (w * Parameter_KompassWirkung) / 64; // auf die Wirkung normieren |
w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln |
if(w > 0) |
{ |
if(!KompassSignalSchlecht) |
{ |
GierGyroFehler += fehler; |
v = 64 + ((MaxStickNick + MaxStickRoll)) / 8; |
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; |
Mess_Integral_Gier += v; |
} |
if(KompassSignalSchlecht) KompassSignalSchlecht--; |
} |
if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)) |
{ |
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; |
if(w < 25 && NeueKompassRichtungMerken && !KompassSignalSchlecht) |
{ |
beeptime = 200; |
// KompassStartwert = KompassValue; |
KompassStartwert = (ErsatzKompass/GIER_GRAD_FAKTOR); |
NeueKompassRichtungMerken = 0; |
} |
fehler = ((540 + KompassValue - (ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180; |
ErsatzKompass += (fehler * 8) / korrektur; |
w = (w * Parameter_KompassWirkung) / 64; // auf die Wirkung normieren |
w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln |
if(w > 0) |
{ |
if(!KompassSignalSchlecht) |
{ |
GierGyroFehler += fehler; |
v = 64 + ((MaxStickNick + MaxStickRoll)) / 8; |
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; |
Mess_Integral_Gier += v; |
} |
if(KompassSignalSchlecht) KompassSignalSchlecht--; |
} |
else KompassSignalSchlecht = 250; // so lange das Signal taub stellen --> ca. 0,5 sek |
} |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
/branches/MicroMag3_Nick666/V0.69k/fc.h |
---|
16,11 → 16,9 |
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 volatile int KompassValue; |
extern volatile int KompassStartwert; |
extern volatile int KompassRichtung; |
extern long ErsatzKompass; |
extern int ErsatzKompassInGrad; // Kompasswert in Grad |
extern int HoehenWert; |
61,7 → 59,6 |
int X; |
int Y; |
float Z; |
int compass; |
}; |
extern struct acc_neutral_struct acc_neutral; |
/branches/MicroMag3_Nick666/V0.69k/flight.pnproj |
---|
1,0 → 0,0 |
<Project name="Flight-Ctrl"><File path="uart.h"></File><File path="main.c"></File><File path="main.h"></File><File path="makefile"></File><File path="uart.c"></File><File path="printf_P.h"></File><File path="printf_P.c"></File><File path="timer0.c"></File><File path="timer0.h"></File><File path="old_macros.h"></File><File path="twimaster.c"></File><File path="version.txt"></File><File path="twimaster.h"></File><File path="rc.c"></File><File path="rc.h"></File><File path="fc.h"></File><File path="fc.c"></File><File path="menu.h"></File><File path="menu.c"></File><File path="_Settings.h"></File><File path="analog.c"></File><File path="analog.h"></File><File path="GPS.c"></File><File path="gps.h"></File><File path="License.txt"></File><File path="eeprom.c"></File><File path="spi.h"></File><File path="spi.c"></File><File path="mymath.h"></File><File path="compass.c"></File><File path="compass.h"></File><File path="mymath.c"></File></Project> |
<Project name="Flight-Ctrl"><File path="uart.h"></File><File path="main.c"></File><File path="main.h"></File><File path="makefile"></File><File path="uart.c"></File><File path="printf_P.h"></File><File path="printf_P.c"></File><File path="timer0.c"></File><File path="timer0.h"></File><File path="old_macros.h"></File><File path="twimaster.c"></File><File path="version.txt"></File><File path="twimaster.h"></File><File path="rc.c"></File><File path="rc.h"></File><File path="fc.h"></File><File path="fc.c"></File><File path="menu.h"></File><File path="menu.c"></File><File path="_Settings.h"></File><File path="analog.c"></File><File path="analog.h"></File><File path="GPS.c"></File><File path="gps.h"></File><File path="License.txt"></File><File path="eeprom.c"></File><File path="spi.h"></File><File path="spi.c"></File></Project> |
/branches/MicroMag3_Nick666/V0.69k/main.c |
---|
147,8 → 147,7 |
rc_sum_init(); |
ADC_Init(); |
i2c_init(); |
init_MM3(); |
//SPI_MasterInit(); |
SPI_MasterInit(); |
sei(); |
182,13 → 181,6 |
//kurze Wartezeit (sonst reagiert die "Kompass kalibrieren?"-Abfrage nicht |
Delay_ms(500); |
//Kompass kalibrieren? |
if(PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] > 100 && PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 100) |
{ |
printf("\n\rKalibriere Kompass"); |
calib_MM3(); |
} |
//Neutrallage kalibrieren? |
if(PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] > 100 && PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -100) |
{ |
283,7 → 275,7 |
} |
else SPI_StartTransmitPacket(SPI_CMD_VALUE);//# |
*/ |
//SPI_StartTransmitPacket();//# |
SPI_StartTransmitPacket();//# |
SendSPI = 4; |
timer = SetDelay(20); |
290,7 → 282,7 |
} |
//if(UpdateMotor) DebugOut.Analog[26]++; |
} |
//if(!SendSPI) { SPI_TransmitByte(); } |
if(!SendSPI) { SPI_TransmitByte(); } |
} |
return (1); |
} |
/branches/MicroMag3_Nick666/V0.69k/main.h |
---|
76,10 → 76,9 |
#include "rc.h" |
#include "fc.h" |
#include "gps.h" |
// #include "spi.h" |
#include "compass.h" |
#include "mymath.h" |
#include "spi.h" |
#ifndef EEMEM |
#define EEMEM __attribute__ ((section (".eeprom"))) |
#endif |
/branches/MicroMag3_Nick666/V0.69k/makefile |
---|
84,7 → 84,7 |
########################################################################################################## |
# List C source files here. (C dependencies are automatically generated.) |
SRC = main.c uart.c printf_P.c timer0.c analog.c menu.c |
SRC += twimaster.c rc.c fc.c GPS.c compass.c mymath.c |
SRC += twimaster.c rc.c fc.c GPS.c spi.c |
########################################################################################################## |
/branches/MicroMag3_Nick666/V0.69k/menu.c |
---|
27,7 → 27,7 |
void Menu(void) |
{ |
static unsigned char MaxMenue = 13,MenuePunkt=0; |
static unsigned char MaxMenue = 11,MenuePunkt=0; |
if(RemoteTasten & KEY1) { if(MenuePunkt) MenuePunkt--; else MenuePunkt = MaxMenue; LcdClear(); RemotePollDisplayLine = -1; } |
if(RemoteTasten & KEY2) { MenuePunkt++; LcdClear(); RemotePollDisplayLine = -1;} |
125,18 → 125,6 |
LCD_printfxy(0,2,"Gs:%4i Gi:%4i ",ExternControl.Gas,ExternControl.Gier); |
LCD_printfxy(0,3,"Hi:%4i Cf:%4i ",ExternControl.Hight,ExternControl.Config); |
break; |
case 12: |
LCD_printfxy(0,0,"MM3 Off"); |
LCD_printfxy(0,1,"X_Offset: %3i",MM3_calib.X_off); |
LCD_printfxy(0,2,"Y_Offset: %3i",MM3_calib.Y_off); |
LCD_printfxy(0,3,"Z_Offset: %3i",MM3_calib.Z_off); |
break; |
case 13: |
LCD_printfxy(0,0,"MM3 Range"); |
LCD_printfxy(0,1,"X_Range: %4i",MM3_calib.X_range); |
LCD_printfxy(0,2,"Y_Range: %4i",MM3_calib.Y_range); |
LCD_printfxy(0,3,"Z_Range: %4i",MM3_calib.Z_range); |
break; |
default: MaxMenue = MenuePunkt - 1; |
MenuePunkt = 0; |
break; |
/branches/MicroMag3_Nick666/V0.69k/timer0.c |
---|
3,6 → 3,7 |
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; |
63,7 → 64,25 |
else PORTC &= ~(1<<7); |
} |
if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) timer0_MM3(); // Kompass auslesen |
if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) |
{ |
if(PINC & 0x10) |
{ |
cntKompass++; |
} |
else |
{ |
if((cntKompass) && (cntKompass < 362)) |
{ |
cntKompass += cntKompass / 41; |
if(cntKompass > 10) KompassValue = cntKompass - 10; else KompassValue = 0; |
} |
// if(cntKompass < 10) cntKompass = 10; |
// KompassValue = (unsigned long)((unsigned long)(cntKompass-10)*720L + 1L) / 703L; |
KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180; |
cntKompass = 0; |
} |
} |
} |
/branches/MicroMag3_Nick666/V0.69k/timer0.h |
---|
11,6 → 11,7 |
extern volatile unsigned int CountMilliseconds; |
extern volatile unsigned char UpdateMotor; |
extern volatile unsigned int beeptime; |
extern volatile unsigned int cntKompass; |
extern int ServoValue; |
extern unsigned int BeepMuster; |
extern volatile unsigned char SendSPI; |