/branches/GPS_BETA_chris2798_hallo2/GPS.c |
---|
1,89 → 1,114 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Copyright (c) 08.2007 by Christopher Hartmann / Daniel Schmitz |
// + Copyright (c) 10.2007 by Christopher Hartmann / Daniel Schmitz |
// + |
// + Bitte die read_me Datei beachten! Es handelt sich hierbei um eine erste Probeversion! |
// + Bitte die read_me Datei beachten! Es handelt sich hierbei um eine BETAVersion! |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#include "main.h" |
#include "math.h" |
// GPS feste Variablen++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//volatile int loop = 0; |
//long gps_northing = 0, gps_easting = 0, gps_altitude = 0; |
long target_x = 0, target_y = 0, target_z = 0; |
volatile int alpha = 0; |
long zwn = 0, zwe = 0, zwn1 = 0, zwe1 = 0, zwn2 = 0, zwe2 = 0; |
volatile int gps_getpos = 5; |
long gps_home_x = 0; |
long gps_home_y = 0; |
long zwn = 0, zwe = 0, zwn1 = 0, zwe1 = 0, zwn2 = 0, zwe2 = 0, entf_x = 0, entf_y = 0; |
// GPS Einstellungen +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
volatile int komp_dreh = 0; // verdrehten Kompasseinbau kompensieren (+/-Grad) |
volatile int gpsmax = 35; //maximal zulässiger "GPS-Steuerausschlag" |
int x=0; |
int hoehedurchlauf=0; |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
float gps_sin=0; |
float gps_cos=0; |
signed int GPS_Nick = 0; |
signed int GPS_Roll = 0; |
long alpha2 = 900; |
long GPS_Nick2 = 0; |
long GPS_Roll2 = 0; |
void gps_main(void) |
{ |
/* |
if (MotorenEin = 1 && gps_gethome == 0 && actualPos.state != 0){ //speichert GPS-Home-Position |
gps_home_x = actualPos.x; |
gps_home_y = actualPos.y; |
beeptime = 80; |
gps_gethome = 1; |
}*/ |
if (StickNick>-15 && StickNick<15 && StickRoll>-15 && StickRoll<15){ |
stick_time++; |
if (stick_time==2){ |
sticks_centered=1;} |
else if (stick_time>35){ |
stick_time=0; |
}} |
else |
{stick_time=0; sticks_centered=0;} |
if (Poti1>0 && actualPos.state != 0){ //Beginn GPS-Position-Hold |
if (actualPos.state != 0 && gps_new==1 && Poti1>0){ //Beginn GPS-Position-Hold |
if (gps_getpos != 0){ //Postion mit Schalter loggen |
target_x = actualPos.x; |
target_y = actualPos.y; |
target_z = actualPos.z; |
beeptime = 50; |
if (gps_getpos != 0 && (Poti2 == 0||gps_gethome==5) && sticks_centered==1){ //PH aktivieren |
target_x = actualPos.northing; |
target_y = actualPos.easting; |
if (gps_gethome!=5){beeptime = 50;} |
gps_getpos = 0;} |
if (Poti2>0 && gps_gethome==1){ // Home anfliegen |
target_x = gps_home_x; |
target_y = gps_home_y; |
if ((actualPos.northing < target_x+200 && actualPos.northing > target_x-200) && (actualPos.easting < target_y+200 && actualPos.easting > target_y-200) && homing_state !=1) |
{beeptime = 100; homing_state=1; gps_getpos = 0;}else {beeptime = 50;} |
//Höhenreglung für Home-Anflug |
if (SollHoehe > home_height && hoehedurchlauf==2){SollHoehe=SollHoehe-1; |
hoehedurchlauf=0;} |
if (SollHoehe < home_height && hoehedurchlauf==2){SollHoehe=SollHoehe+1; |
hoehedurchlauf=0;} |
else if ((SollHoehe > home_height || SollHoehe < home_height) && hoehedurchlauf<2) {hoehedurchlauf++;}} |
if (sticks_centered==1){ |
//Regler ########################################################################################################################## |
//P-Regler |
zwn = ((sqrt(target_x^2+target_z^2)-sqrt(actualPos.x^2+actualPos.z^2))*gps_p)/10; //8 |
zwe = ((target_y-actualPos.y)*gps_p)/10; |
zwn = (target_x-actualPos.northing)*(gps_p); |
zwe = (target_y-actualPos.easting)*(gps_p); |
//D-Regler |
zwn2= (gps_d*actualPos.vx)/-3; //-2 |
zwe2= (gps_d*actualPos.vy)/-3; |
zwn2 = (actualPos.velNorth)*(-gps_d); |
zwe2 = (actualPos.velEast)*(-gps_d); |
GPS_Nick = (zwn+zwn2); // skal; |
GPS_Roll = (zwe+zwe2); // skal; |
//Rotationsmatrix + Kompass######################################################################################################## |
//GPS-Mixer######################################################################################################################## |
if (GPS_Nick>gpsmax){GPS_Nick=gpsmax;} else if (GPS_Nick<(-1*gpsmax)){GPS_Nick=(-1*gpsmax);} //min-max Wert überprüfen |
if (GPS_Roll>gpsmax){GPS_Roll=gpsmax;} else if (GPS_Roll<(-1*gpsmax)){GPS_Roll=(-1*gpsmax);} |
/* |
//Rotationsmatrix################################################################################################################## |
//Kompass ++++++++++++++++++++++++++++ |
alpha=0; |
alpha = komp_dreh+KompassValue; |
if (KompassValue>300) {beeptime=50;} |
if (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV){ |
alpha = komp_dreh+KompassStartwert; |
if (alpha>359) {alpha=alpha-360;} |
if (alpha != alpha2){ |
gps_sin =floor(sin(alpha*M_PI/180)*100)/100; |
gps_cos =floor(cos(alpha*M_PI/180)*100)/100; |
alpha2 = alpha; |
} |
GPS_Nick=(sin(alpha)*GPS_Roll+cos(alpha)*GPS_Nick); |
GPS_Roll=(cos(alpha)*GPS_Roll-sin(alpha)*GPS_Nick); |
*/ |
GPS_Nick2 = (zwn+zwn2)/skal; |
GPS_Roll2 = (zwe+zwe2)/skal; |
GPS_Nick= (gps_sin*GPS_Roll2)+(gps_cos*GPS_Nick2); |
GPS_Roll= (gps_cos*GPS_Roll2)-(gps_sin*GPS_Nick2); |
}else { |
GPS_Nick = (zwn+zwn2)/skal; |
GPS_Roll = (zwe+zwe2)/skal; |
} |
GPS_Nick=GPS_Nick*-1; |
//GPS-Min-Max Prüfung############################################################################################################### |
if (GPS_Nick>gpsmax){GPS_Nick=gpsmax;} else if (GPS_Nick<(-1*gpsmax)){GPS_Nick=(-1*gpsmax);} |
if (GPS_Roll>gpsmax){GPS_Roll=gpsmax;} else if (GPS_Roll<(-1*gpsmax)){GPS_Roll=(-1*gpsmax);} |
gps_new=0; |
}else{ |
gps_getpos=5; |
GPS_Nick=0; |
GPS_Roll=0; |
GPS_Roll=0;}} |
} |
} |
/branches/GPS_BETA_chris2798_hallo2/fc.c |
---|
56,7 → 56,7 |
#include "main.h" |
unsigned char h,m,s; |
volatile unsigned char Timeout = 0; |
volatile unsigned int I2CTimeout = 100; |
volatile int MesswertNick,MesswertRoll,MesswertGier; |
volatile int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0; |
volatile int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch, NeutralAccX=0, NeutralAccY=0; |
88,6 → 88,7 |
char MotorenEin = 0; |
int HoehenWert = 0; |
int SollHoehe = 0; |
unsigned char Looping_Nick = 0,Looping_Roll = 0; |
float Kp = FAKTOR_P; |
float Ki = FAKTOR_I; |
97,7 → 98,7 |
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_Gyro_P = 50; // Wert : 10-250 |
unsigned char Parameter_Gyro_P = 150; // Wert : 10-250 |
unsigned char Parameter_Gyro_I = 150; // Wert : 0-250 |
unsigned char Parameter_Gier_P = 2; // Wert : 1-20 |
unsigned char Parameter_I_Faktor = 10; // Wert : 1-20 |
106,6 → 107,7 |
unsigned char Parameter_UserParam3 = 0; |
unsigned char Parameter_UserParam4 = 0; |
unsigned char Parameter_ServoNickControl = 100; |
unsigned char Parameter_LoopGasLimit = 70; |
struct mk_param_struct EE_Parameter; |
void Piep(unsigned char Anzahl) |
138,9 → 140,19 |
{ |
if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset(); |
} |
if(PlatinenVersion == 10) |
{ |
AdNeutralNick= abs(MesswertNick); |
AdNeutralRoll= abs(MesswertRoll); |
AdNeutralGier= abs(MesswertGier); |
} |
else |
{ |
AdNeutralNick= abs(MesswertNick) / 2; |
AdNeutralRoll= abs(MesswertRoll) / 2; |
AdNeutralGier= abs(MesswertGier) / 2; |
} |
NeutralAccY = abs(Mittelwert_AccRoll) / ACC_AMPLIFY; |
NeutralAccX = abs(Mittelwert_AccNick) / ACC_AMPLIFY; |
NeutralAccZ = Aktuell_az; |
157,6 → 169,7 |
HoeheD = 0; |
Mess_Integral_Hoch = 0; |
KompassStartwert = KompassValue; |
//GPS_Neutral(); |
beeptime = 50; |
} |
188,15 → 201,16 |
// ADC einschalten |
ANALOG_ON; |
/* |
//------------------------------------------------------------------------------ |
if(MesswertNick > 200) MesswertNick += 4 * (MesswertNick - 200); |
else |
if(MesswertNick < -200) MesswertNick += 4 * (MesswertNick + 200); |
if(MesswertRoll > 200) MesswertRoll += 4 * (MesswertRoll - 200); |
else |
if(MesswertRoll < -200) MesswertRoll += 4 * (MesswertRoll + 200); |
//------------------------------------------------------------------------------ |
*/ |
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--; |
287,9 → 301,9 |
EE_Parameter.Hoehe_MinGas = 30; |
EE_Parameter.MaxHoehe = 251; // Wert : 0-32 251 -> Poti1 |
EE_Parameter.Hoehe_P = 10; // Wert : 0-32 |
EE_Parameter.Luftdruck_D = 70; // Wert : 0-250 |
EE_Parameter.Hoehe_ACC_Wirkung = 30; // Wert : 0-250 |
EE_Parameter.Hoehe_Verstaerkung = 2; // Wert : 0-50 |
EE_Parameter.Luftdruck_D = 50; // Wert : 0-250 |
EE_Parameter.Hoehe_ACC_Wirkung = 50; // Wert : 0-250 |
EE_Parameter.Hoehe_Verstaerkung = 4; // Wert : 0-50 |
EE_Parameter.Stick_P = 4; //2 // Wert : 1-6 |
EE_Parameter.Stick_D = 8; //8 // Wert : 0-64 |
EE_Parameter.Gier_P = 16; // Wert : 1-20 |
299,11 → 313,11 |
EE_Parameter.KompassWirkung = 128; // Wert : 0-250 |
EE_Parameter.Gyro_P = 120; //80 // Wert : 0-250 |
EE_Parameter.Gyro_I = 150; // Wert : 0-250 |
EE_Parameter.UnterspannungsWarnung = 90; // Wert : 0-250 |
EE_Parameter.UnterspannungsWarnung = 94; // Wert : 0-250 |
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 = 5; |
EE_Parameter.I_Faktor = 0; |
EE_Parameter.UserParam1 = 0; //zur freien Verwendung |
EE_Parameter.UserParam2 = 0; //zur freien Verwendung |
EE_Parameter.UserParam3 = 0; //zur freien Verwendung |
314,7 → 328,10 |
EE_Parameter.ServoNickMin = 50; // Wert : 0-250 // Anschlag |
EE_Parameter.ServoNickMax = 150; // Wert : 0-250 // Anschlag |
EE_Parameter.ServoNickRefresh = 5; |
memcpy(EE_Parameter.Name, "Normal\0", 12); |
EE_Parameter.LoopGasLimit = 50; |
EE_Parameter.LoopThreshold = 90; // Wert: 0-250 Schwelle für Stickausschlag |
EE_Parameter.LoopConfig = 0; // Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts / wird getrennt behandelt |
memcpy(EE_Parameter.Name, "Sport\0", 12); |
} |
void DefaultKonstanten2(void) |
326,6 → 343,7 |
EE_Parameter.Kanalbelegung[K_POTI1] = 5; |
EE_Parameter.Kanalbelegung[K_POTI2] = 6; |
EE_Parameter.Kanalbelegung[K_POTI3] = 7; |
EE_Parameter.Kanalbelegung[K_POTI4] = 8; |
EE_Parameter.GlobalConfig = 0;//CFG_HOEHENREGELUNG | /*CFG_HOEHEN_SCHALTER |*/ CFG_KOMPASS_AKTIV;//0x01; |
EE_Parameter.Hoehe_MinGas = 30; |
EE_Parameter.MaxHoehe = 251; // Wert : 0-32 251 -> Poti1 |
342,17 → 360,15 |
EE_Parameter.KompassWirkung = 128; // Wert : 0-250 |
EE_Parameter.Gyro_P = 175; //80 // Wert : 0-250 |
EE_Parameter.Gyro_I = 175; // Wert : 0-250 |
EE_Parameter.UnterspannungsWarnung = 90; // Wert : 0-250 |
EE_Parameter.UnterspannungsWarnung = 94; // Wert : 0-250 |
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 = 5; |
EE_Parameter.I_Faktor = 0; |
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.UserParam3 = 0; //zur freien Verwendung |
EE_Parameter.UserParam4 = 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.ServoNickCompInvert = 0; // Wert : 0-250 // Richtung Einfluss Gyro/Servo |
359,6 → 375,9 |
EE_Parameter.ServoNickMin = 50; // Wert : 0-250 // Anschlag |
EE_Parameter.ServoNickMax = 150; // Wert : 0-250 // Anschlag |
EE_Parameter.ServoNickRefresh = 5; |
EE_Parameter.LoopGasLimit = 50; |
EE_Parameter.LoopThreshold = 90; // Wert: 0-250 Schwelle für Stickausschlag |
EE_Parameter.LoopConfig = 0; // Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts |
memcpy(EE_Parameter.Name, "Kamera\0", 12); |
} |
382,16 → 401,10 |
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); |
unsigned char ServoNickComp; // Wert : 0-250 // Einfluss Gyro/Servo |
unsigned char ServoNickCompInvert; // Wert : 0-250 // Richtung Einfluss Gyro/Servo |
unsigned char ServoNickMin; // Wert : 0-250 // Anschlag |
unsigned char ServoNickMax; // Wert : 0-250 // Anschlag |
CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255); |
CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255); |
CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255); |
CHK_POTI(Parameter_LoopGasLimit,EE_Parameter.LoopGasLimit,0,255); |
Ki = (float) Parameter_I_Faktor * 0.0001; |
MAX_GAS = EE_Parameter.Gas_Max; |
408,8 → 421,8 |
int GierMischanteil,GasMischanteil; |
static long SummeNick=0,SummeRoll=0; |
static long sollGier = 0,tmp_long,tmp_long2; |
static int IntegralFehlerNick = 0; |
static int IntegralFehlerRoll = 0; |
long IntegralFehlerNick = 0; |
long IntegralFehlerRoll = 0; |
static unsigned int RcLostTimer; |
static unsigned char delay_neutral = 0; |
static unsigned char delay_einschalten = 0,delay_ausschalten = 0; |
426,13 → 439,19 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
GasMischanteil = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120; |
if(GasMischanteil < 0) GasMischanteil = 0; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Emfang schlecht |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(SenderOkay < 100) |
{ |
if(!PcZugriff) beeptime = 500; |
if(!PcZugriff) |
{ |
if(BeepMuster == 0xffff) |
{ |
beeptime = 15000; |
BeepMuster = 0x0c00; |
} |
} |
if(RcLostTimer) RcLostTimer--; |
else |
{ |
447,10 → 466,7 |
PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] = 0; |
PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] = 0; |
PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] = 0; |
/* Poti1 = 65; |
Poti2 = 48; |
Poti3 = 0; |
*/ } |
} |
else MotorenEin = 0; |
} |
else |
483,7 → 499,6 |
if(++delay_neutral > 200) // nicht sofort |
{ |
GRN_OFF; |
SetNeutral(); |
MotorenEin = 0; |
delay_neutral = 0; |
modell_fliegt = 0; |
496,12 → 511,13 |
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 |
} |
ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE); |
Piep(GetActiveParamSetNumber()); |
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 delay_neutral = 0; |
562,7 → 578,6 |
StickRoll = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P; |
StickRoll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; |
StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]; |
GyroFaktor = ((float)Parameter_Gyro_P + 10.0) / 256.0; |
IntegralFaktor = ((float) Parameter_Gyro_I) / 44000; |
569,8 → 584,57 |
if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor = 0; |
if(GyroFaktor < 0) GyroFaktor = 0; |
if(IntegralFaktor < 0) IntegralFaktor = 0; |
// greift in den Stick ein, um ungewolltes überschlagen zu verhindern |
if(!(EE_Parameter.LoopConfig & CFG_LOOP_LINKS) && !(EE_Parameter.LoopConfig & CFG_LOOP_RECHTS)) |
{ |
if(IntegralNick > 60000) |
{ |
StickNick -= 8 * EE_Parameter.Stick_P; |
if(IntegralNick > 80000) StickNick -= 16 * EE_Parameter.Stick_P; |
} |
else |
if(IntegralNick < -60000) |
{ |
StickNick += 8 * EE_Parameter.Stick_P; |
if(IntegralNick > 80000) StickNick -= 16 * EE_Parameter.Stick_P; |
} |
if(IntegralRoll > 60000) |
{ |
StickRoll -= 8 * EE_Parameter.Stick_P; |
if(IntegralRoll > 80000) StickRoll -= 16 * EE_Parameter.Stick_P; |
} |
else |
if(IntegralRoll < -60000) |
{ |
StickRoll += 8 * EE_Parameter.Stick_P; |
if(IntegralRoll > 80000) StickRoll -= 16 * EE_Parameter.Stick_P; |
} |
} |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Looping? |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_LINKS) || |
((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_RECHTS)) |
{ |
Looping_Roll = 1; |
if(GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit; |
} |
else Looping_Roll = 0; |
if(((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_OBEN) || |
((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_UNTEN)) |
{ |
Looping_Nick = 1; |
Looping_Roll = 0; |
if(GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit; |
} |
else Looping_Nick = 0; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Bei Empfangsausfall im Flug |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(Notlandung) |
580,6 → 644,8 |
StickRoll = 0; |
GyroFaktor = 0.1; |
IntegralFaktor = 0.005; |
Looping_Roll = 0; |
Looping_Nick = 0; |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Gyro-Drift kompensieren |
594,8 → 660,8 |
if(IntegralFehlerNick < -500/DRIFT_FAKTOR) AdNeutralNick--; |
if(IntegralFehlerRoll > 500/DRIFT_FAKTOR) AdNeutralRoll++; |
if(IntegralFehlerRoll < -500/DRIFT_FAKTOR) AdNeutralRoll--; |
if(Mess_Integral_Gier2 > 500/DRIFT_FAKTOR) AdNeutralGier--; |
if(Mess_Integral_Gier2 <-500/DRIFT_FAKTOR) AdNeutralGier++; |
// if(Mess_Integral_Gier2 > 500/DRIFT_FAKTOR) AdNeutralGier--; // macht nur mit Referenz (Kompass Sinn) |
// if(Mess_Integral_Gier2 <-500/DRIFT_FAKTOR) AdNeutralGier++; // macht nur mit Referenz (Kompass Sinn) |
ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
Mess_IntegralNick2 = IntegralNick; |
Mess_IntegralRoll2 = IntegralRoll; |
605,6 → 671,10 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Integrale auf ACC-Signal abgleichen |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(EE_Parameter.GlobalConfig & CFG_GPS_AKTIV){ |
tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick) / 16; |
tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll) / 16; |
#define AUSGLEICH 500 |
615,18 → 685,45 |
ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
Mess_IntegralNick -= tmp_long; |
Mess_IntegralRoll -= tmp_long2; |
}else{ |
if(IntegralFaktor && !Looping_Nick && !Looping_Roll) |
{ |
tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick); |
if(labs(Mittelwert_AccNick) < 200) tmp_long /= 8; |
else tmp_long /= 16; |
tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll); |
if(labs(Mittelwert_AccRoll) < 200) tmp_long2 /= 8; |
else tmp_long2 /= 16; |
#define AUSGLEICH 500 |
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; |
} |
else |
{ |
tmp_long = 0; |
tmp_long2 = 0; |
} |
ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
Mess_IntegralNick -= tmp_long; |
Mess_IntegralRoll -= tmp_long2;} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Gieren |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
sollGier = StickGier; |
if(abs(StickGier) > 35) |
if(abs(StickGier) > 20) // war 35 |
{ |
if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1; |
} |
tmp_int = EE_Parameter.Gier_P * (sollGier * abs(sollGier)) / 256; // expo |
tmp_int = EE_Parameter.Gier_P * (StickGier * abs(StickGier)) / 512; // expo y = ax + bx² |
tmp_int += (EE_Parameter.Gier_P * StickGier) / 4; |
sollGier = tmp_int; |
Mess_Integral_Gier -= tmp_int; |
if(Mess_Integral_Gier > 30000) Mess_Integral_Gier = 30000; // begrenzen |
if(Mess_Integral_Gier <-30000) Mess_Integral_Gier =-30000; |
if(Mess_Integral_Gier > 25000) Mess_Integral_Gier = 25000; // begrenzen |
if(Mess_Integral_Gier <-25000) Mess_Integral_Gier =-25000; |
ANALOG_ON; // ADC einschalten |
633,7 → 730,6 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Kompass |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//KompassValue = 12; |
if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)) |
{ |
int w,v; |
658,35 → 754,55 |
else SignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// GPS |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(EE_Parameter.GlobalConfig & CFG_GPS_AKTIV) |
{ |
gps_p = Parameter_UserParam1; |
gps_d = Parameter_UserParam2; |
skal = Parameter_UserParam3; |
komp_dreh = Parameter_UserParam4*45; |
if(EE_Parameter.GlobalConfig & CFG_GPS_AKTIV){ |
gpsmax=40; //maximale GPS Steuerausschläge |
if (MotorenEin == 1 && gps_gethome == 0 && actualPos.state != 0){ //speichert GPS-Home-Position |
gps_home_x = actualPos.northing; |
gps_home_y = actualPos.easting; |
home_height = HoehenWert+50; //30 |
beeptime = 80; |
gps_gethome = 1; |
} |
if (MotorenEin ==0 && gps_gethome ==1) |
{gps_gethome=0;} |
if (Poti1>0) { |
gps_main(); |
} |
else |
{ |
}else if (Poti1>0 && actualPos.state==0){beeptime = 80;} |
if (Poti1==0){ |
gps_getpos=5; |
GPS_Nick = 0; |
GPS_Roll = 0; |
} |
if (Poti2==0){homing_state=0;} |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Debugwerte zuordnen |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
DebugOut.Sekunden++; |
if(!TimerWerteausgabe--) |
{ |
TimerWerteausgabe = 49; |
// DebugOut.Analog[0] = MesswertNick; |
// DebugOut.Analog[1] = MesswertRoll; |
// DebugOut.Analog[2] = MesswertGier; |
DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor; |
DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor; |
DebugOut.Analog[2] = Mittelwert_AccNick; |
694,8 → 810,23 |
DebugOut.Analog[4] = MesswertGier; |
DebugOut.Analog[5] = HoehenWert; |
DebugOut.Analog[6] = (Mess_Integral_Hoch / 512); |
DebugOut.Analog[7] = GasMischanteil; |
DebugOut.Analog[8] = KompassValue; |
DebugOut.Analog[10] = gps_h_d1; |
DebugOut.Analog[11] = gps_h_d2; |
/* DebugOut.Analog[16] = motor_rx[0]; |
DebugOut.Analog[17] = motor_rx[1]; |
DebugOut.Analog[18] = motor_rx[2]; |
DebugOut.Analog[19] = motor_rx[3]; |
DebugOut.Analog[20] = motor_rx[0] + motor_rx[1] + motor_rx[2] + motor_rx[3]; |
DebugOut.Analog[20] /= 14; |
DebugOut.Analog[21] = motor_rx[4]; |
DebugOut.Analog[22] = motor_rx[5]; |
DebugOut.Analog[23] = motor_rx[6]; |
DebugOut.Analog[24] = motor_rx[7]; |
DebugOut.Analog[25] = motor_rx[4] + motor_rx[5] + motor_rx[6] + motor_rx[7]; |
*/ |
// DebugOut.Analog[9] = MesswertNick; |
// DebugOut.Analog[9] = SollHoehe; |
// DebugOut.Analog[10] = Mess_Integral_Gier / 128; |
// DebugOut.Analog[11] = KompassStartwert; |
707,13 → 838,15 |
// DebugOut.Analog[4] = hoehenregler; |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
MesswertNick = IntegralNick * IntegralFaktor + MesswertNick * GyroFaktor; |
MesswertRoll = IntegralRoll * IntegralFaktor + MesswertRoll * GyroFaktor; |
MesswertGier = MesswertGier * (GyroFaktor/2) + Integral_Gier * IntegralFaktor; |
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 * (GyroFaktor/2) + Integral_Gier * IntegralFaktor; |
MesswertGier = MesswertGier * (GyroFaktor) + Integral_Gier * IntegralFaktor/2; |
// Maximalwerte abfangen |
#define MAX_SENSOR 2048 |
772,35 → 905,46 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + 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 > (MUL_G * GasMischanteil)) GierMischanteil = MUL_G * GasMischanteil; |
if(GierMischanteil < -(MUL_G * GasMischanteil)) GierMischanteil = -(MUL_G * GasMischanteil); |
if(GierMischanteil > 100) GierMischanteil = 100; |
if(GierMischanteil < -100) GierMischanteil = -100; |
if(GasMischanteil < 20) GierMischanteil = 0; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Nick-Achse |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
DiffNick = Kp * (MesswertNick - (StickNick - GPS_Nick)); // Differenz bestimmen |
SummeNick += DiffNick; // I-Anteil |
if(SummeNick > 0) SummeNick-= (abs(SummeNick)/256 + 1); else SummeNick += abs(SummeNick)/256 + 1; |
if(SummeNick > 16000) SummeNick = 16000; |
if(SummeNick < -16000) SummeNick = -16000; |
pd_ergebnis = DiffNick + Ki * SummeNick; // PI-Regler für Nick |
pd_ergebnis = DiffNick;// + Ki * SummeNick; // PI-Regler für Nick |
// Motor Vorn |
#define MUL 2 |
if(pd_ergebnis > MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis = MUL * (GasMischanteil + abs(GierMischanteil)); |
if(pd_ergebnis < -MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis = -MUL * (GasMischanteil + abs(GierMischanteil)); |
motorwert = GasMischanteil + pd_ergebnis + GierMischanteil; // Mischer |
if ((motorwert < 0) | (GasMischanteil < 10)) motorwert = 0; |
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) | (GasMischanteil < 10)) motorwert = 0; |
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 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
809,20 → 953,21 |
if(SummeRoll > 0) SummeRoll-= (abs(SummeRoll)/256 + 1); else SummeRoll += abs(SummeRoll)/256 + 1; |
if(SummeRoll > 16000) SummeRoll = 16000; |
if(SummeRoll < -16000) SummeRoll = -16000; |
pd_ergebnis = DiffRoll + Ki * SummeRoll; // PI-Regler für Roll |
pd_ergebnis = DiffRoll;// + Ki * SummeRoll; // PI-Regler für Roll |
if(pd_ergebnis > MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis = MUL * (GasMischanteil + abs(GierMischanteil)); |
if(pd_ergebnis < -MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis = -MUL * (GasMischanteil + abs(GierMischanteil)); |
// Motor Links |
motorwert = GasMischanteil + pd_ergebnis - GierMischanteil; |
if ((motorwert < 0) | (GasMischanteil < 10)) motorwert = 0; |
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) | (GasMischanteil < 10)) motorwert = 0; |
if ((motorwert < 0)) motorwert = 0; |
else if(motorwert > MAX_GAS) motorwert = MAX_GAS; |
if (motorwert < MIN_GAS) motorwert = MIN_GAS; |
Motor_Rechts = motorwert; |
// +++++++++++++++++++++++++++++++++++++++++++++++ |
} |
/branches/GPS_BETA_chris2798_hallo2/fc.h |
---|
5,7 → 5,7 |
#ifndef _FC_H |
#define _FC_H |
extern volatile unsigned char Timeout; |
extern volatile unsigned int I2CTimeout; |
extern unsigned char Sekunde,Minute; |
extern volatile long IntegralNick,IntegralNick2; |
extern volatile long IntegralRoll,IntegralRoll2; |
81,15 → 81,28 |
unsigned char ServoNickComp; // Wert : 0-250 // Einfluss Gyro/Servo |
unsigned char ServoNickMin; // Wert : 0-250 // Anschlag |
unsigned char ServoNickMax; // Wert : 0-250 // Anschlag |
unsigned char ServoNickRefresh; // Wert : 0-250 // Richtung Einfluss Gyro/Servo |
unsigned char ServoNickCompInvert; // Wert : 0-250 // Richtung Einfluss Gyro/Servo |
unsigned char Reserved[7]; |
unsigned char ServoNickRefresh; // |
unsigned char LoopGasLimit; // Wert: 0-250 max. Gas während Looping |
unsigned char LoopThreshold; // Wert: 0-250 Schwelle für Stickausschlag |
//------------------------------------------------ |
unsigned char LoopConfig; // Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts / wird getrennt behandelt |
unsigned char ServoNickCompInvert; // Wert : 0-250 0 oder 1 // WICHTIG!!! am Ende lassen |
unsigned char Reserved[4]; |
char Name[12]; |
}; |
/* |
unsigned char ServoNickMax; // Wert : 0-250 |
unsigned char ServoNickRefresh; // |
unsigned char LoopGasLimit; // Wert: 0-250 max. Gas während Looping |
unsigned char LoopThreshold; // Wert: 0-250 Schwelle für Stickausschlag |
//------------------------------------------------ |
unsigned char LoopConfig; // Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts / wird getrennt behandelt |
unsigned char ServoNickCompInvert; // Wert : 0-250 0 oder 1 // WICHTIG!!! am Ende lassen |
unsigned char Reserved[4]; |
char Name[12]; |
*/ |
extern struct mk_param_struct EE_Parameter; |
extern unsigned char Parameter_Luftdruck_D; |
/branches/GPS_BETA_chris2798_hallo2/main.c |
---|
52,6 → 52,7 |
#include "main.h" |
unsigned char EEPromArray[E2END+1] EEMEM; |
unsigned char PlatinenVersion = 10; |
// -- Parametersatz aus EEPROM lesen --- |
// number [0..5] |
75,8 → 76,15 |
unsigned char GetActiveParamSetNumber(void) |
{ |
return(eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET])); |
unsigned char set; |
set = eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET]); |
if(set > 5) |
{ |
set = 2; |
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], set); // diesen Parametersatz als aktuell merken |
} |
return(set); |
} |
//############################################################################ |
//Hauptprogramm |
85,8 → 93,11 |
{ |
unsigned int timer; |
unsigned int timer2 = 0; |
DDRC = 0x01; // SCL |
DDRB = 0x00; |
PORTB = 0x00; |
for(timer = 0; timer < 1000; timer++); // verzögern |
if(PINB & 0x01) PlatinenVersion = 11; else PlatinenVersion = 10; |
DDRC = 0x81; // SCL |
PORTC = 0xff; // Pullup SDA |
DDRB = 0x1B; // LEDs und Druckoffset |
PORTB = 0x01; // LED_Rot |
118,11 → 129,12 |
VersionInfo.Nebenversion = VERSION_NEBENVERSION; |
VersionInfo.PCKompatibel = VERSION_KOMPATIBEL; |
printf("\n\rFlightControl V%d.%d ", VERSION_HAUPTVERSION, VERSION_NEBENVERSION); |
printf("\n\rFlightControl\n\rHardware:%d.%d\n\rSoftware:V%d.%d ",PlatinenVersion/10,PlatinenVersion%10, VERSION_HAUPTVERSION, VERSION_NEBENVERSION); |
printf("\n\r=============================="); |
GRN_ON; |
if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_VALID]) != 59) // seit V 0.60 |
#define EE_DATENREVISION 62 // wird angepasst, wenn sich die EEPROM-Daten geändert haben |
if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_VALID]) != EE_DATENREVISION) |
{ |
printf("\n\rInit. EEPROM: Generiere Default-Parameter..."); |
DefaultKonstanten1(); |
131,8 → 143,8 |
if(i==2) DefaultKonstanten2(); |
WriteParameterSet(i, (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE); |
} |
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], 1); |
eeprom_write_byte(&EEPromArray[EEPROM_ADR_VALID], 59); |
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], 2); |
eeprom_write_byte(&EEPromArray[EEPROM_ADR_VALID], EE_DATENREVISION); |
} |
ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE); |
142,7 → 154,7 |
if(EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG) |
{ |
printf("\n\rAbgleich Luftdrucksensor.."); |
timer = SetDelay(2500); |
timer = SetDelay(1000); |
SucheLuftruckOffset(); |
while (!CheckDelay(timer)); |
printf("OK\n\r"); |
164,9 → 176,9 |
printf("\n\n\r"); |
LcdClear(); |
I2CTimeout = 5000; |
while (1) |
{ |
if (UpdateMotor) // ReglerIntervall |
{ |
UpdateMotor=0; |
174,25 → 186,23 |
SendMotorData(); |
ROT_OFF; |
if(PcZugriff) PcZugriff--; |
if(SenderOkay) SenderOkay--; |
if (UBat < EE_Parameter.UnterspannungsWarnung) |
if(!I2CTimeout) |
{ |
beeptime = 2000; |
} |
if(!Timeout) |
I2CTimeout = 5; |
i2c_reset(); |
if((BeepMuster == 0xffff) && MotorenEin) |
{ |
i2c_init(); |
beeptime = 10000; |
BeepMuster = 0x0080; |
} |
} |
else |
{ |
I2CTimeout--; |
ROT_OFF; |
} |
} |
if(SIO_DEBUG) |
{ |
DatenUebertragung(); |
199,12 → 209,19 |
BearbeiteRxDaten(); |
} |
else BearbeiteRxDaten(); |
if(CheckDelay(timer2)) |
if(CheckDelay(timer)) |
{ |
if(MotorenEin) PORTC ^= 0x10; else PORTC &= ~0x10; |
timer = SetDelay(500); |
if(UBat < EE_Parameter.UnterspannungsWarnung) |
{ |
if(BeepMuster == 0xffff) |
{ |
beeptime = 6000; |
BeepMuster = 0x0300; |
} |
} |
timer = SetDelay(100); |
} |
} |
return (1); |
} |
/branches/GPS_BETA_chris2798_hallo2/main.h |
---|
11,14 → 11,13 |
//#define SYSCLK 16000000L //Quarz Frequenz in Hz |
#endif |
// neue Hardware |
#define ROT_OFF PORTB &=~0x01 |
#define ROT_ON PORTB |= 0x01 |
#define ROT_OFF {if(PlatinenVersion == 10) PORTB &=~0x01; else PORTB |= 0x01;} |
#define ROT_ON {if(PlatinenVersion == 10) PORTB |= 0x01; else PORTB &=~0x01;} |
#define ROT_FLASH PORTB ^= 0x01 |
#define GRN_OFF PORTB &=~0x02 |
#define GRN_ON PORTB |= 0x02 |
#define GRN_FLASH PORTD ^= 0x02 |
#define GRN_FLASH PORTB ^= 0x02 |
//#ifndef F_CPU |
//#error ################## F_CPU nicht definiert oder ungültig ############# |
30,6 → 29,7 |
#define EEPROM_ADR_VALID 1 |
#define EEPROM_ADR_ACTIVE_SET 2 |
#define EEPROM_ADR_LAST_OFFSET 3 |
#define EEPROM_ADR_PARAM_BEGIN 100 |
40,12 → 40,51 |
#define CFG_KOMPASS_FIX 0x10 |
#define CFG_GPS_AKTIV 0x20 |
#define CFG_LOOP_OBEN 0x01 |
#define CFG_LOOP_UNTEN 0x02 |
#define CFG_LOOP_LINKS 0x04 |
#define CFG_LOOP_RECHTS 0x08 |
long durchschnitt_x; |
long durchschnitt_y; |
long durchschnitt_z; |
int gps_h_d1; |
int gps_h_d2; |
long target_x, target_y, target_z; |
volatile int gps_error; |
volatile int gps_getpos; |
volatile int gps_p; //P-Anteil |
volatile int gps_d; //D-Anteil |
volatile int gps_acc; //Acc-Anteil |
int sticks_centered; |
int stick_time; |
long gps_home_x; |
long gps_home_y; |
long home_height; |
int homing_state; |
int way, way1, wayn, way2; |
volatile int gpsmax; |
volatile int komp_dreh; // verdrehten Kompasseinbau kompensieren (+/-Grad) |
volatile int gps_new; |
volatile int skal; //Skalierungsfaktor der Regelung (alter Wert: 10) (20) |
volatile int gps_gethome; |
//#define SYSCLK |
//extern unsigned long SYSCLK; |
extern volatile int i_Nick[20],i_Roll[20],DiffNick,DiffRoll; |
extern volatile unsigned char SenderOkay; |
extern unsigned char CosinusNickWinkel, CosinusRollWinkel; |
extern unsigned char PlatinenVersion; |
extern void ReadParameterSet (unsigned char number, unsigned char *buffer, unsigned char length); |
extern void WriteParameterSet(unsigned char number, unsigned char *buffer, unsigned char length); |
52,15 → 91,6 |
extern unsigned char GetActiveParamSetNumber(void); |
extern unsigned char EEPromArray[]; |
long durchschnitt_northing; |
long durchschnitt_easting; |
volatile int gps_p; //P-Anteil (10) |
volatile int gps_d; //D-Anteil (4) |
volatile int skal; //Skalierungsfaktor der Regelung (alter Wert: 10) (20) |
volatile int gps_gethome; |
#include <stdlib.h> |
#include <string.h> |
#include <avr/io.h> |
72,7 → 102,7 |
#include "old_macros.h" |
#include "_Settings.h" |
#include "_settings.h" |
#include "printf_P.h" |
#include "timer0.h" |
#include "uart.h" |
/branches/GPS_BETA_chris2798_hallo2/makefile |
---|
4,8 → 4,8 |
F_CPU = 20000000 |
#------------------------------------------------------------------- |
HAUPT_VERSION = 0 |
NEBEN_VERSION = 60 |
VERSION_KOMPATIBEL = 4 # PC-Kompatibilität |
NEBEN_VERSION = 64 |
VERSION_KOMPATIBEL = 5 # PC-Kompatibilität |
#------------------------------------------------------------------- |
ifeq ($(MCU), atmega32) |
/branches/GPS_BETA_chris2798_hallo2/menu.c |
---|
27,7 → 27,7 |
void Menu(void) |
{ |
static unsigned char MaxMenue = 10,MenuePunkt=0; |
static unsigned char MaxMenue = 12,MenuePunkt=0; |
if(RemoteTasten & KEY1) { if(MenuePunkt) MenuePunkt--; else MenuePunkt = MaxMenue; LcdClear(); } |
if(RemoteTasten & KEY2) { MenuePunkt++; LcdClear(); } |
35,15 → 35,49 |
LCD_printfxy(17,0,"[%i]",MenuePunkt); |
switch(MenuePunkt) |
{ |
case 0: |
LCD_printfxy(0,0,"GPS Modification"); |
LCD_printfxy(0,1,"based on V0.64 , GPS V0.74"); |
LCD_printfxy(0,2,"(c) Hartmann"); |
LCD_printfxy(0,3,"Schmitz"); |
break; |
case 1: |
LCD_printfxy(0,0,"++ MikroKopter ++"); |
LCD_printfxy(0,1,"V%d.%d",VERSION_HAUPTVERSION, VERSION_NEBENVERSION); |
LCD_printfxy(0,1,"HW:V%d.%d SW:%d.%d",PlatinenVersion/10,PlatinenVersion%10,VERSION_HAUPTVERSION, VERSION_NEBENVERSION); |
LCD_printfxy(0,2,"Setting: %d ",GetActiveParamSetNumber()); |
LCD_printfxy(0,3,"(c) Holger Buss"); |
// if(RemoteTasten & KEY3) TestInt--; |
// if(RemoteTasten & KEY4) TestInt++; |
break; |
case 1: |
case 2: |
if(EE_Parameter.GlobalConfig & CFG_GPS_AKTIV){ |
if(actualPos.state == 2){ |
LCD_printfxy(0,0,"2D-Fix"); |
LCD_printfxy(0,1,"Koordinaten:"); |
LCD_printfxy(0,2,"Nord: %5i",actualPos.northing); |
LCD_printfxy(0,3,"Nord: %5i",actualPos.easting); |
} |
else if (actualPos.state ==3){ |
LCD_printfxy(0,0,"3D-Fix"); |
LCD_printfxy(0,1,"Koordinaten:"); |
LCD_printfxy(0,2,"Nord: %5i",actualPos.northing); |
LCD_printfxy(0,3,"Nord: %5i",actualPos.easting);} |
else |
{ |
LCD_printfxy(0,1,"Kein "); |
LCD_printfxy(0,2,"Empfang"); |
}} |
else{ |
LCD_printfxy(0,1,"GPS "); |
LCD_printfxy(0,2,"deaktiviert");} |
break; |
case 3: |
if(EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG) |
{ |
LCD_printfxy(0,0,"Hoehe: %5i",HoehenWert); |
58,53 → 92,53 |
} |
break; |
case 2: |
case 4: |
LCD_printfxy(0,0,"akt. Lage"); |
LCD_printfxy(0,1,"Nick: %5i",IntegralNick/1024); |
LCD_printfxy(0,2,"Roll: %5i",IntegralRoll/1024); |
LCD_printfxy(0,3,"Kompass: %5i",KompassValue); |
break; |
case 3: |
case 5: |
LCD_printfxy(0,0,"K1:%4i K2:%4i ",PPM_in[1],PPM_in[2]); |
LCD_printfxy(0,1,"K3:%4i K4:%4i ",PPM_in[3],PPM_in[4]); |
LCD_printfxy(0,2,"K5:%4i K6:%4i ",PPM_in[5],PPM_in[6]); |
LCD_printfxy(0,3,"K7:%4i Kanäle ",PPM_in[7]); |
LCD_printfxy(0,3,"K7:%4i K8:%4i ",PPM_in[7],PPM_in[8]); |
break; |
case 4: |
case 6: |
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 Kanäle ",PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]]); |
LCD_printfxy(0,3,"P3:%4i P4:%4i ",PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]],PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]]); |
break; |
case 5: |
case 7: |
LCD_printfxy(0,0,"Gyro - Sensor"); |
LCD_printfxy(0,1,"Nick %4i (%3i)",AccumulateNick / MessanzahlNick, AdNeutralNick); |
LCD_printfxy(0,2,"Roll %4i (%3i)",AccumulateRoll / MessanzahlRoll, AdNeutralRoll); |
LCD_printfxy(0,3,"Gier %4i (%3i)",AccumulateGier / MessanzahlGier, AdNeutralGier); |
break; |
case 6: |
case 8: |
LCD_printfxy(0,0,"ACC - Sensor"); |
LCD_printfxy(0,1,"Nick %4i (%3i)",accumulate_AccNick / messanzahl_AccNick,NeutralAccX); |
LCD_printfxy(0,2,"Roll %4i (%3i)",accumulate_AccRoll / messanzahl_AccRoll,NeutralAccY); |
LCD_printfxy(0,3,"Hoch %4i (%3i)",Aktuell_az/*accumulate_AccHoch / messanzahl_AccHoch*/,(int)NeutralAccZ); |
break; |
case 7: |
case 9: |
LCD_printfxy(0,1,"Spannung: %5i",UBat); |
LCD_printfxy(0,2,"Empf.Pegel:%5i",SenderOkay); |
break; |
case 8: |
case 10: |
LCD_printfxy(0,0,"Kompass "); |
LCD_printfxy(0,1,"Richtung: %5i",KompassRichtung); |
LCD_printfxy(0,2,"Messwert: %5i",KompassValue); |
LCD_printfxy(0,3,"Start: %5i",KompassStartwert); |
break; |
case 9: |
case 11: |
LCD_printfxy(0,0,"Poti1: %3i",Poti1); |
LCD_printfxy(0,1,"Poti2: %3i",Poti2); |
LCD_printfxy(0,2,"Poti3: %3i",Poti3); |
LCD_printfxy(0,3,"Poti4: %3i",Poti4); |
break; |
case 10: |
case 12: |
LCD_printfxy(0,0,"Servo " ); |
LCD_printfxy(0,1,"Setpoint %3i",Parameter_ServoNickControl); |
LCD_printfxy(0,2,"Stellung: %3i",ServoValue); |
/branches/GPS_BETA_chris2798_hallo2/rc.c |
---|
23,7 → 23,6 |
{ |
TCCR1B=(1<<CS11)|(1<<CS10)|(1<<ICES1)|(1<<ICNC1);//|(1 << WGM12); //timer1 prescale 64 |
// PWM |
//TCCR1A = (1 << COM1B1) | (1 << WGM11) | (1 << WGM10); |
//TCCR1B |= (1 << WGM12); |
43,7 → 42,7 |
{ |
static unsigned int AltICR=0; |
signed int signal = 0; |
signed int signal = 0,tmp; |
static int index; |
signal = (unsigned int) ICR1 - AltICR; |
66,15 → 65,16 |
signal -= 466; |
// Stabiles Signal |
if(abs(signal - PPM_in[index]) < 6) { if(SenderOkay < 200) SenderOkay += 10;} |
signal = (3 * (PPM_in[index]) + signal) / 4; |
//373 entspricht ca. 1.5ms also Mittelstellung |
PPM_diff[index] = signal - PPM_in[index]; |
PPM_in[index] = signal; |
tmp = (3 * (PPM_in[index]) + signal) / 4; |
// if(tmp > signal+1) tmp--; else |
// if(tmp < signal-1) tmp++; |
PPM_diff[index] = tmp - PPM_in[index]; |
PPM_in[index] = tmp; |
} |
index++; |
/* if(index == 5) PORTD |= 0x20; else PORTD &= ~0x20; // Servosignal an J3 anlegen |
if(index == 5) PORTD |= 0x20; else PORTD &= ~0x20; // Servosignal an J3 anlegen |
if(index == 6) PORTD |= 0x10; else PORTD &= ~0x10; // Servosignal an J4 anlegen |
if(index == 7) PORTD |= 0x08; else PORTD &= ~0x08; // Servosignal an J5 anlegen */ |
if(index == 7) PORTD |= 0x08; else PORTD &= ~0x08; // Servosignal an J5 anlegen |
} |
} |
} |
/branches/GPS_BETA_chris2798_hallo2/timer0.c |
---|
5,6 → 5,7 |
volatile unsigned char UpdateMotor = 0; |
volatile unsigned int cntKompass = 0; |
volatile unsigned int beeptime = 0; |
unsigned int BeepMuster = 0xffff; |
int ServoValue = 0; |
enum { |
22,6 → 23,7 |
SIGNAL (SIG_OVERFLOW0) // 8kHz |
{ |
static unsigned char cnt_1ms = 1,cnt = 0; |
unsigned char pieper_ein = 0; |
// TCNT0 -= 250;//TIMER_RELOAD_VALUE; |
if(!cnt--) |
31,17 → 33,35 |
cnt_1ms %= 2; |
if(!cnt_1ms) UpdateMotor = 1; |
CountMilliseconds++; |
if(Timeout) Timeout--; |
} |
if(beeptime > 1) |
{ |
beeptime--; |
PORTD |= (1<<2); |
if(beeptime & BeepMuster) |
{ |
pieper_ein = 1; |
} |
else pieper_ein = 0; |
} |
else |
PORTD &= ~(1<<2); |
{ |
pieper_ein = 0; |
BeepMuster = 0xffff; |
} |
if(pieper_ein) |
{ |
if(PlatinenVersion == 10) PORTD |= (1<<2); // Speaker an PORTD.2 |
else PORTC |= (1<<7); // Speaker an PORTC.7 |
} |
else |
{ |
if(PlatinenVersion == 10) PORTD &= ~(1<<2); |
else PORTC &= ~(1<<7); |
} |
if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) |
{ |
if(PINC & 0x10) |
127,7 → 147,6 |
if(ServoValue < EE_Parameter.ServoNickMin) ServoValue = EE_Parameter.ServoNickMin; |
else if(ServoValue > EE_Parameter.ServoNickMax) ServoValue = EE_Parameter.ServoNickMax; |
DebugOut.Analog[10] = ServoValue; |
OCR2A = ServoValue;// + 75; |
timer = EE_Parameter.ServoNickRefresh; |
} |
/branches/GPS_BETA_chris2798_hallo2/timer0.h |
---|
11,4 → 11,5 |
extern volatile unsigned char UpdateMotor; |
extern volatile unsigned int beeptime; |
extern volatile unsigned int cntKompass; |
extern int ServoValue; |
extern int ServoValue; |
extern unsigned int BeepMuster; |
/branches/GPS_BETA_chris2798_hallo2/twimaster.c |
---|
34,7 → 34,25 |
TWCR = (1<<TWEN) | (1<<TWSTO) | (1<<TWINT); |
} |
void i2c_reset(void) |
//############################################################################ |
{ |
i2c_stop(); |
twi_state = 0; |
motor = TWDR; |
motor = 0; |
TWCR = 0x80; |
TWAMR = 0; |
TWAR = 0; |
TWDR = 0; |
TWSR = 0; |
TWBR = 0; |
i2c_init(); |
i2c_start(); |
i2c_write_byte(0); |
} |
//############################################################################ |
//Start I2C |
char i2c_write_byte(char byte) |
//############################################################################ |
104,6 → 122,7 |
break; |
case 5: //1 Byte vom Motor lesen |
motor_rx[motorread] = TWDR; |
case 6: |
switch(motorread) |
{ |
126,6 → 145,8 |
motorread++; |
if (motorread>3) motorread=0; |
i2c_stop(); |
I2CTimeout = 10; |
twi_state = 0; |
} |
TWCR |= 0x80; |
} |
/branches/GPS_BETA_chris2798_hallo2/uart.c |
---|
3,10 → 3,6 |
// + only for non-profit use |
// + www.MikroKopter.com |
// + see the File "License.txt" for further Informations |
// + |
// + GPS read out: |
// + modified Version of the Pitschu Brushless Ufo - (c) Peter Schulten, Mülheim, Germany |
// + only for non-profit use |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#include "main.h" |
42,8 → 38,6 |
#define GPS_CKA 7 |
#define GPS_CKB 8 |
//gpsInfo_t gpsPoints[5]; // stored position to fly to (currently only 1 target supported) |
//gpsInfo_t *gpsTarget; // points to one of the targets |
gpsInfo_t actualPos; // measured position (last gps record) |
#define SYNC_CHAR1 0xb5 |
50,12 → 44,9 |
#define SYNC_CHAR2 0x62 |
#define CLASS_NAV 0x01 |
#define MSGID_POSECEF 0x01 |
#define MSGID_STATUS 0x03 |
//#define MSGID_POSLLH 0x02 |
#define MSGID_VELECEF 0x11 |
//#define MSGID_POSUTM 0x08 |
//#define MSGID_VELNED 0x12 |
#define MSGID_POSUTM 0x08 |
#define MSGID_VELNED 0x12 |
70,19 → 61,7 |
uint8_t packetStatus; |
} NAV_STATUS_t; |
/* |
typedef struct { |
unsigned long ITOW; // time of week |
long LON; // longitude in 1e-07 deg |
long LAT; // lattitude |
long HEIGHT; // height in mm |
long HMSL; // height above mean sea level im mm |
unsigned long Hacc; // horizontal accuracy in mm |
unsigned long Vacc; // vertical accuracy in mm |
uint8_t packetStatus; |
} NAV_POSLLH_t; |
typedef struct { |
unsigned long ITOW; // time of week |
long EAST; // cm UTM Easting |
106,35 → 85,11 |
unsigned long CAcc; // deg Course / Heading Accuracy Estimate |
uint8_t packetStatus; |
} NAV_VELNED_t; |
*/ |
typedef struct { |
unsigned long ITOW; // ms GPS Millisecond Time of Week |
long ECEF_X; // ecef x / cm |
long ECEF_Y; // ecef y / cm |
long ECEF_Z; // ecef z / cm |
unsigned long Pacc; // Abweichung |
uint8_t packetStatus; |
} NAV_POSECEF_t ; |
typedef struct { |
unsigned long ITOW; // ms GPS Millisecond Time of Week |
long ECEFVX; // ecef x velocity cm/s |
long ECEFVY; // ecef y velocity cm/s |
long ECEFVZ; // ecef z velocity cm/s |
unsigned long SAcc; // Abweichung |
uint8_t packetStatus; |
} NAV_VELECEF_t; |
NAV_STATUS_t navStatus; |
NAV_POSECEF_t navPosECEF; |
NAV_VELECEF_t navVelECEF; |
NAV_POSUTM_t navPosUtm; |
NAV_VELNED_t navVelNed; |
//NAV_POSLLH_t navPosLlh; |
//NAV_POSUTM_t navPosUtm; |
//NAV_VELECEF avVelNed; |
volatile char *ubxP, *ubxEp, *ubxSp; // pointers to packet currently transfered |
volatile uint8_t CK_A, CK_B; // UBX checksum bytes |
volatile unsigned short msgLen; |
181,15 → 136,7 |
{ |
if (navPosECEF.packetStatus == 1) // valid packet |
{ |
actualPos.x = navPosECEF.ECEF_X; //ECEF X in cm |
actualPos.y = navPosECEF.ECEF_Y; //ECEF Y in cm |
actualPos.z = navPosECEF.ECEF_Z; //ECEF Z in cm |
navPosECEF.packetStatus = 0; |
} |
if (navStatus.packetStatus == 1) // valid packet |
{ |
actualPos.state = navStatus.GPSfix; |
196,36 → 143,22 |
navStatus.packetStatus = 0; |
} |
if (navVelECEF.packetStatus == 1) // valid packet |
{ |
actualPos.vx = navVelECEF.ECEFVX; //ECEF VEL X in cm/s |
actualPos.vy = navVelECEF.ECEFVY; //ECEF VEL Y in cm/s |
actualPos.vz = navVelECEF.ECEFVZ; //ECEF VEL Z in cm/s |
navVelECEF.packetStatus = 0; |
} |
/* |
if (navPosUtm.packetStatus == 1) // valid packet |
{ |
actualPos.northing = navPosUtm.NORTH; ///10; // in 10cm; |
actualPos.northing = navPosUtm.NORTH; ///10; |
actualPos.easting = navPosUtm.EAST; //10; |
actualPos.altitude = navPosUtm.ALT; //10; |
navPosUtm.packetStatus = 0; |
} |
if (navPosLlh.packetStatus == 1) |
navPosLlh.packetStatus = 0; |
if (navVelNed.packetStatus == 1){ |
actualPos.velNorth = navVelNed.VEL_N; |
actualPos.velEast = navVelNed.VEL_E; |
actualPos.velDown = navVelNed.VEL_D; |
navVelNed.packetStatus = 0;} |
navPosLlh and navVelNed currently not used |
*/ |
if (actualPos.state != 0){ROT_ON;} //-> Rot blinkt mit 4Hz wenn GPS Signal brauchbar |
if (actualPos.state == 2){ROT_ON; gps_new=1;} //-> Rot blinkt mit 4Hz wenn GPS 2D-Fix |
else if (actualPos.state == 3){GRN_OFF; gps_new=1;} //-> Grün blinkt mit 4Hz wenn GPS 3D-Fix |
} |
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
240,9 → 173,6 |
SioTmp = UDR; |
uint8_t c; |
uint8_t re; |
296,26 → 226,6 |
ubxSp = (char*)&navStatus.packetStatus; |
ignorePacket = navStatus.packetStatus; |
break; |
case MSGID_POSECEF: |
ubxP = (char*)&navPosECEF; |
ubxEp = (char*)(&navPosECEF + sizeof(NAV_POSECEF_t)); |
ubxSp = (char*)&navPosECEF.packetStatus; |
ignorePacket = navPosECEF.packetStatus; |
break; |
case MSGID_VELECEF: |
ubxP = (char*)&navVelECEF; |
ubxEp = (char*)(&navVelECEF + sizeof(NAV_VELECEF_t)); |
ubxSp = (char*)&navVelECEF.packetStatus; |
ignorePacket = navVelECEF.packetStatus; |
break; |
/* |
case MSGID_POSLLH: |
ubxP = (char*)&navPosLlh; |
ubxEp = (char*)(&navPosLlh + sizeof(NAV_POSLLH_t)); |
ubxSp = (char*)&navPosLlh.packetStatus; |
ignorePacket = navPosLlh.packetStatus; |
break; |
case MSGID_POSUTM: |
ubxP = (char*)&navPosUtm; |
ubxEp = (char*)(&navPosUtm + sizeof(NAV_POSUTM_t)); |
328,8 → 238,8 |
ubxSp = (char*)&navVelNed.packetStatus; |
ignorePacket = navVelNed.packetStatus; |
break; |
*/ |
default: |
ignorePacket = 1; |
ubxSp = (char*)0; |
381,11 → 291,6 |
GPSscanData (); |
if(buf_ptr >= MAX_EMPFANGS_BUFF) UartState = 0; |
if(SioTmp == '\r' && UartState == 2) |
{ |
402,7 → 307,7 |
NeuerDatensatzEmpfangen = 1; |
AnzahlEmpfangsBytes = buf_ptr; |
RxdBuffer[buf_ptr] = '\r'; |
if(/*(RxdBuffer[1] == MeineSlaveAdresse || (RxdBuffer[1] == 'a')) && */(RxdBuffer[2] == 'R')) wdt_enable(WDTO_250MS); // Reset-Commando |
if(RxdBuffer[2] == 'R') wdt_enable(WDTO_250MS); // Reset-Commando |
} |
} |
else |
449,6 → 354,7 |
} |
// -------------------------------------------------------------------------- |
void SendOutData(unsigned char cmd,unsigned char modul, unsigned char *snd, unsigned char len) |
{ |
521,7 → 427,7 |
EE_CheckAndWrite(&EE_Buffer[EE_DEBUGWERTE + i*2], DebugIn.Analog[i]); |
EE_CheckAndWrite(&EE_Buffer[EE_DEBUGWERTE + i*2 + 1], DebugIn.Analog[i] >> 8); |
}*/ |
RemoteTasten |= DebugIn.RemoteTasten; |
//RemoteTasten |= DebugIn.RemoteTasten; |
DebugDataAnforderung = 1; |
break; |
616,8 → 522,6 |
//fdevopen (uart_putchar, 0); |
//sbi(PORTD,4); |
Debug_Timer = SetDelay(200); |
gpsState = GPS_EMPTY; |
} |
//--------------------------------------------------------------------------------------------- |
643,9 → 547,13 |
{ |
Menu(); |
DebugDisplayAnforderung = 0; |
if(++dis_zeile == 4) dis_zeile = 0; |
SendOutData('0' + dis_zeile,0,&DisplayBuff[20 * dis_zeile],20); // DisplayZeile übertragen |
if(++dis_zeile == 4) |
{ |
SendOutData('4',0,&PPM_in,sizeof(PPM_in)); // DisplayZeile übertragen |
dis_zeile = -1; |
} |
else SendOutData('0' + dis_zeile,0,&DisplayBuff[20 * dis_zeile],20); // DisplayZeile übertragen |
} |
if(GetVersionAnforderung && UebertragungAbgeschlossen) |
{ |
SendOutData('V',MeineSlaveAdresse,(unsigned char *) &VersionInfo,sizeof(VersionInfo)); |
/branches/GPS_BETA_chris2798_hallo2/uart.h |
---|
21,11 → 21,8 |
extern unsigned char MotorTest[4]; |
struct str_DebugOut |
{ |
unsigned char Digital[13]; |
unsigned int AnzahlZyklen; |
unsigned int Zeit; |
unsigned char Sekunden; |
unsigned int Analog[16]; // Debugwerte |
unsigned char Digital[2]; |
unsigned int Analog[32]; // Debugwerte |
}; |
extern struct str_DebugOut DebugOut; |
34,12 → 31,12 |
#define _B0(bit) (0 << (bit)) |
typedef struct { |
long x; // in cm (+ = north) |
long y; // in cm (+ = east) |
long z; // in cm |
long vx; |
long vy; |
long vz; |
long northing; // in cm (+ = north) |
long easting; // in cm (+ = east) |
long altitude; // in cm |
long velNorth; |
long velEast; |
long velDown; |
//long groundSpeed; |
//long heading; |