diff -ur ./V0.82b_orig//fc.c ./V0.82b_cyphy//fc.c --- ./V0.82b_orig//fc.c 2011-08-16 16:58:27.000000000 +1000 +++ ./V0.82b_cyphy//fc.c 2011-06-21 11:54:11.000000000 +1000 @@ -78,6 +78,7 @@ long SummeNick=0,SummeRoll=0; volatile long Mess_Integral_Hoch = 0; int KompassValue = 0; +int ExternOn =0; int KompassStartwert = 0; int KompassRichtung = 0; unsigned int KompassSignalSchlecht = 500; @@ -164,6 +165,10 @@ #define LIMIT_MAX(value, max) {if(value >= max) value = max;} #define LIMIT_MIN_MAX(value, min, max) {if(value <= min) value = min; else if(value >= max) value = max;} +signed int My_Pitch; +signed int My_Roll; + + // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // Debugwerte zuordnen @@ -176,20 +181,35 @@ DebugOut.Analog[3] = Mittelwert_AccRoll / 4; DebugOut.Analog[4] = (signed int) AdNeutralGier - AdWertGier; DebugOut.Analog[5] = HoehenWert/5; - DebugOut.Analog[6] = AdWertAccHoch;//(Mess_Integral_Hoch / 512);// Aktuell_az; - DebugOut.Analog[8] = KompassValue; - DebugOut.Analog[9] = UBat; + //DebugOut.Analog[5] = StickGas; + DebugOut.Analog[11] = AdWertAccHoch;//(Mess_Integral_Hoch / 512);// Aktuell_az; AD value acc high + DebugOut.Analog[6] = Aktuell_az; //AD value acc high + + DebugOut.Analog[8] = UBat; + //DebugOut.Analog[8] = ExternOn; + //DebugOut.Analog[9] = UBat; + DebugOut.Analog[9] = StickGas; + //DebugOut.Analog[9] = 100; + DebugOut.Analog[10] = SenderOkay; - DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR; +// DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR; DebugOut.Analog[12] = Motor[0].SetPoint; DebugOut.Analog[13] = Motor[1].SetPoint; DebugOut.Analog[14] = Motor[2].SetPoint; DebugOut.Analog[15] = Motor[3].SetPoint; - DebugOut.Analog[20] = ServoNickValue; + //DebugOut.Analog[20] = ServoNickValue; + DebugOut.Analog[20] = UBat; DebugOut.Analog[22] = Capacity.ActualCurrent; - DebugOut.Analog[23] = Capacity.UsedCapacity; + //DebugOut.Analog[23] = Capacity.UsedCapacity; + // DebugOut.Analog[22] = FromNaviCtrl_Value.GpsZ; // DebugOut.Analog[29] = FromNaviCtrl_Value.SerialDataOkay; + DebugOut.Analog[23] = StickGier; + DebugOut.Analog[24] = StickNick; + DebugOut.Analog[25] = StickRoll; + + + DebugOut.Analog[29] = Capacity.MinOfMaxPWM; DebugOut.Analog[30] = GPS_Nick; DebugOut.Analog[31] = GPS_Roll; @@ -368,9 +388,9 @@ static signed long tmpl,tmpl2,tmpl3,tmpl4; static signed int oldNick, oldRoll, d2Roll, d2Nick; signed long winkel_nick, winkel_roll; - unsigned char i; MesswertGier = (signed int) AdNeutralGier - AdWertGier; MesswertNick = (signed int) AdWertNickFilter / 8; +// printf("MesswertNick=%d\r\n",MesswertNick); MesswertRoll = (signed int) AdWertRollFilter / 8; RohMesswertNick = MesswertNick; RohMesswertRoll = MesswertRoll; @@ -643,7 +663,7 @@ static char NeueKompassRichtungMerken = 0; static long ausgleichNick, ausgleichRoll; int IntegralNickMalFaktor,IntegralRollMalFaktor; - unsigned char i; + unsigned long i=0; Mittelwert(); GRN_ON; // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ @@ -828,9 +848,13 @@ if(!NewPpmData-- || (FC_StatusFlags & FC_STATUS_EMERGENCY_LANDING)) { static int stick_nick,stick_roll; + static int extern_stick_nick,extern_stick_roll; + static struct str_error previous_error; ParameterZuordnung(); stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4; stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D; +// printf("PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] = %d\r\n",PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]]); +// printf("stick_nick =%d\r\n",stick_nick); stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4; stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; @@ -851,8 +875,37 @@ FromNC_Rotate_S = sintab[EE_Parameter.OrientationAngle]; StickNick = ((FromNC_Rotate_C * stick_nick) + (FromNC_Rotate_S * stick_roll)) / 8; StickRoll = ((FromNC_Rotate_C * stick_roll) - (FromNC_Rotate_S * stick_nick)) / 8; +// printf("stick_nick = %d stick_Roll=%d\r\n",StickNick,StickRoll); } + +//============================================================================================= +// Simple PD Position control part by enddl22 Jan.10.2011 +//============================================================================================= + +#if 0 + + error.y = CurrentPosition.y-TargetPosition.y; + error.x = CurrentPosition.x-TargetPosition.x; + My_Pitch = MY_PITCH_P*(CurrentPosition.y-TargetPosition.y)+MY_PITCH_D*(error.y-previous_error.y); + My_Roll = MY_ROLL_P*(CurrentPosition.x-TargetPosition.x)+MY_ROLL_D*(error.x-previous_error.x); + + My_Pitch = My_Pitch/PITCH_SCALE; + My_Roll = My_Roll/PITCH_SCALE; + + //printf("My_Pitch = %d My_Roll =%d \n\r",My_Pitch,My_Roll); + + + previous_error = error; + + StickNick -= My_Pitch; + StickRoll += My_Roll; + +#endif + +//============================================================================================= + + StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]; if(StickGier > 2) StickGier -= 2; else if(StickGier < -2) StickGier += 2; else StickGier = 0; @@ -861,22 +914,91 @@ StickRoll -= (GPS_Roll + GPS_Roll2); StickGas = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120; + //printf("StickNick =%d\r\n",StickNick); + GyroFaktor = (Parameter_Gyro_P + 10.0); IntegralFaktor = Parameter_Gyro_I; GyroFaktorGier = (Parameter_Gyro_Gier_P + 10.0); IntegralFaktorGier = Parameter_Gyro_Gier_I; + //StickNick += (int) ExternControl.Nick * (int) EE_Parameter.Stick_P; + //StickRoll += (int) ExternControl.Roll * (int) EE_Parameter.Stick_P; + //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ //+ Analoge Steuerung per Seriell //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ - if(ExternControl.Config & 0x01 && Parameter_ExternalControl > 128) +// if(ExternControl.Config & 0x01 && Parameter_ExternalControl > 128) + if(PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] > 128) + +// if((ExternControl.Config ==1) && (PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] > 128)) { + + ExternOn =1; // to get the initial position of the stick_gas value in the air. +//enddl22_21.12.2010[[ +//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ +// Original code from Mikrokopter +//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ StickNick += (int) ExternControl.Nick * (int) EE_Parameter.Stick_P; StickRoll += (int) ExternControl.Roll * (int) EE_Parameter.Stick_P; + +// StickNick += (int) ExternControl.Nick; +// StickRoll += (int) ExternControl.Roll; + + + // 2Bytes resolution nick/roll + // printf("ExternControl.Nick=%\n\r",ExternControl.Nick); + // printf("ExternControl.Roll=%\n\r",ExternControl.Roll); + //enddl22_03.02.2011 Nick/Roll input data range extension -4096 ~ 4096. + // The range was -128 ~ 128 +// printf("ExternControl.Nick=%d,ExternControl.Roll=%d\n\r",ExternControl.Nick,ExternControl.Roll); +// printf("StickNick=%d,StickRoll=%d\n\r",StickNick,StickRoll); + + //StickNick += (int) ExternControl.Nick; + //StickRoll += (int) ExternControl.Roll; + +// extern_stick_nick = (extern_stick_nick * 3 + (int) ExternControl.Nick * (EE_Parameter.Stick_P-2)) / 4; +// extern_stick_nick += (ExternControl.Nick-post_ExternControl.Nick)* (EE_Parameter.Stick_D-4); +// printf("Ext =%d , Ext diff = %d \r\n",ExternControl.Nick,ExternControl.Nick-post_ExternControl.Nick); + +// extern_stick_roll = (extern_stick_roll * 3 + (int) ExternControl.Roll * (EE_Parameter.Stick_P-2)) / 4; +// extern_stick_roll += (ExternControl.Roll-post_ExternControl.Roll)* (EE_Parameter.Stick_D-4); + +// printf("ExtStickNick =%d , ExtStickRoll = %d \r\n",stick_ExternControl.Nick,ExternControl.Nick-post_ExternControl.Nick); + #if 0 + stick_roll = (stick_roll * 3 + (int) ExternControl.Roll * EE_Parameter.Stick_P) / 4; + stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; + #endif + +// StickNick=extern_stick_nick; +// StickRoll=extern_stick_roll; + +// printf("extern_stick_nick =%d , extern_stick_roll = %d \r\n",extern_stick_nick,extern_stick_roll); +// post_ExternControl = ExternControl; +//enddl22_21.12.2010]] + +#if 0 + printf("pitch=%d roll=%d yaw=%d height=%d \n", + ExternControl.Nick, + ExternControl.Roll, + ExternControl.Gier, + ExternControl.Hight); +#endif + + //printf("GasMischanteil=%d PPM=%d\n",GasMischanteil , PPM_in[EE_Parameter.Kanalbelegung[K_GAS]]); + StickGier += ExternControl.Gier; ExternHoehenValue = (int) ExternControl.Hight * (int)EE_Parameter.Hoehe_Verstaerkung; - if(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas; + //StickGas = ExternControl.Gas; + + if(ExternControl.Gas>=130) StickGas = 130; + if(ExternControl.Gas<=0) StickGas=0; + StickGas = ExternControl.Gas; + + //if(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas; } + + else ExternOn =0; + if(StickGas < 0) StickGas = 0; if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor = 0; @@ -1280,7 +1402,8 @@ // Die Höhenregelung schwächt lediglich das Gas ab, erhöht es allerdings nicht // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ if(UBat > BattLowVoltageWarning) GasMischanteil = ((unsigned int)GasMischanteil * BattLowVoltageWarning) / UBat; // Gas auf das aktuelle Spannungvieveau beziehen - GasMischanteil *= STICK_GAIN; +// GasMischanteil *= STICK_GAIN; + GasMischanteil *= 1; // if height control is activated if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG) && !(Looping_Roll || Looping_Nick)) // Höhenregelung { @@ -1626,6 +1749,7 @@ LIMIT_MIN(GasMischanteil, (MIN_GAS + 10) * STICK_GAIN); if(GasMischanteil > (MAX_GAS - 20) * STICK_GAIN) GasMischanteil = (MAX_GAS - 20) * STICK_GAIN; +//printf("MIN_GAS=%d MAX_GAS=%d \n",MIN_GAS,MAX_GAS); // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // all BL-Ctrl connected? // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ @@ -1638,7 +1762,8 @@ // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // + Mischer und PI-Regler // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ - DebugOut.Analog[7] = GasMischanteil; + DebugOut.Analog[7] = GasMischanteil; // Gas Mixer Input + //printf("GasMischanteil=%d\n",GasMischanteil); // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // Gier-Anteil // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ diff -ur ./V0.82b_orig//fc.h ./V0.82b_cyphy//fc.h --- ./V0.82b_orig//fc.h 2011-08-16 16:58:27.000000000 +1000 +++ ./V0.82b_cyphy//fc.h 2011-01-11 16:21:41.000000000 +1000 @@ -22,6 +22,27 @@ extern volatile unsigned char FC_StatusFlags; +//[[ enddl22_0110.2011 +#define MY_PITCH_P 50 +#define MY_PITCH_D 25 +#define MY_ROLL_P 50 +#define MY_ROLL_D 25 + +#define PITCH_SCALE 255 +#define ROLL_SCALE 255 + +struct str_error +{ + int x; + int y; + int yaw; +}; +struct str_error error; + + + +//]] + #define Poti1 Poti[0] #define Poti2 Poti[1] diff -ur ./V0.82b_orig//GPS.c ./V0.82b_cyphy//GPS.c --- ./V0.82b_orig//GPS.c 2011-08-16 16:58:27.000000000 +1000 +++ ./V0.82b_cyphy//GPS.c 2011-01-10 23:06:21.000000000 +1000 @@ -8,6 +8,7 @@ signed int GPS_Nick = 0; signed int GPS_Roll = 0; + signed int GPS_Nick2 = 0; signed int GPS_Roll2 = 0; long GpsAktuell_X = 0; Only in ./V0.82b_orig/: License.txt Only in ./V0.82b_cyphy/: LICENSE.TXT diff -ur ./V0.82b_orig//main.c ./V0.82b_cyphy//main.c --- ./V0.82b_orig//main.c 2011-08-16 16:58:27.000000000 +1000 +++ ./V0.82b_cyphy//main.c 2011-03-10 17:40:48.000000000 +1000 @@ -141,6 +141,19 @@ WDTCSR |= (1<= 20) GIER_GRAD_FAKTOR = 1220; else GIER_GRAD_FAKTOR = 1291; // unterschiedlich für ME und ENC @@ -328,9 +341,11 @@ else { ExternControl.Config = 0; + ExternControl.Gas = 0; ExternStickNick = 0; ExternStickRoll = 0; ExternStickGier = 0; + if(BeepMuster == 0xffff && SenderOkay == 0) { beeptime = 15000; diff -ur ./V0.82b_orig//makefile ./V0.82b_cyphy//makefile --- ./V0.82b_orig//makefile 2011-08-16 16:58:27.000000000 +1000 +++ ./V0.82b_cyphy//makefile 2010-12-21 19:43:12.000000000 +1000 @@ -1,7 +1,7 @@ #-------------------------------------------------------------------- # MCU name -#MCU = atmega1284p -MCU = atmega644p +MCU = atmega1284p +#MCU = atmega644p F_CPU = 20000000 #------------------------------------------------------------------- VERSION_MAJOR = 0 diff -ur ./V0.82b_orig//uart.c ./V0.82b_cyphy//uart.c --- ./V0.82b_orig//uart.c 2011-08-16 16:58:27.000000000 +1000 +++ ./V0.82b_cyphy//uart.c 2011-06-20 23:41:24.000000000 +1000 @@ -68,6 +68,9 @@ unsigned char ConfirmFrame; struct str_DebugOut DebugOut; struct str_ExternControl ExternControl; +struct str_Position CurrentPosition; +struct str_Position TargetPosition; +struct str_Position PostPosition; struct str_VersionInfo VersionInfo; struct str_WinkelOut WinkelOut; struct str_Data3D Data3D; @@ -104,8 +107,8 @@ "Hovergas ", "Current [0.1A] ", "Capacity [mAh] ", - "24 ", - "25 ", //25 + "StickNick ", + "StickRoll ", //25 "26 ", "27 ", "I2C-Error ", @@ -335,7 +338,16 @@ { case FC_ADDRESS: // FC special commands switch(RxdBuffer[2]) + + { + case 'e': // Retrieve the current position of the quadrotor. + memcpy((unsigned char *)&CurrentPosition , (unsigned char *)pRxData, sizeof(CurrentPosition)); +// printf("FC_x=%d FC_y=%d FC_yaw=%d \r\n",CurrentPosition.x,CurrentPosition.y,CurrentPosition.yaw); + // printf("FC_sizeof=%d , FC_sizeof CurrentPosition=%d\r\n",sizeof(CurrentPosition.x),sizeof(CurrentPosition)); + + break; + case 'K':// Kompasswert memcpy((unsigned char *)&KompassValue , (unsigned char *)pRxData, sizeof(KompassValue)); KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180; @@ -554,6 +566,11 @@ break; case 'b': memcpy((unsigned char *)&ExternControl, (unsigned char *)pRxData, sizeof(ExternControl)); + //printf("Ext.Nick=%d Ext.Roll=%d,StickNick=%d,StickRoll=%d\n\r",ExternControl.Nick,ExternControl.Roll,StickNick,StickRoll); + //GPS_Nick=ExternControl.Nick; + //GPS_Roll=ExternControl.Roll; + + ConfirmFrame = ExternControl.Frame; PcZugriff = 255; break; @@ -568,7 +585,6 @@ if(DebugDataIntervall > 0) DebugDataAnforderung = 1; AboTimeOut = SetDelay(ABO_TIMEOUT); break; - case 'h':// x-1 Displayzeilen PcZugriff = 255; if((pRxData[0] & 0x80) == 0x00) // old format @@ -667,6 +683,7 @@ Intervall3D = 0; } +#if 1 if(((Display_Interval>0 && CheckDelay(Display_Timer)) || DebugDisplayAnforderung) && UebertragungAbgeschlossen) { if(DisplayLine > 3)// new format @@ -711,13 +728,17 @@ if(WinkelOut.CalcState > 4) WinkelOut.CalcState = 6; // wird dann in SPI auf Null gesetzt Kompass_Timer = SetDelay(99); } + #endif if(((DebugDataIntervall>0 && CheckDelay(Debug_Timer)) || DebugDataAnforderung) && UebertragungAbgeschlossen) { CopyDebugValues(); - SendOutData('D', FC_ADDRESS, 1, (unsigned char *) &DebugOut,sizeof(DebugOut)); + SendOutData('D', FC_ADDRESS, 1, (unsigned char *) &DebugOut,sizeof(DebugOut)-38); +// SendOutData('D', FC_ADDRESS, 1, (unsigned char *) &DebugOut,sizeof(DebugOut)-18); + DebugDataAnforderung = 0; if(DebugDataIntervall>0) Debug_Timer = SetDelay(DebugDataIntervall); } + #if 1 if(Intervall3D > 0 && CheckDelay(Timer3D) && UebertragungAbgeschlossen) { Data3D.Winkel[0] = (int) (IntegralNick / (EE_Parameter.GyroAccFaktor * 4)); // etwa in 0.1 Grad @@ -756,6 +777,8 @@ } #endif +#endif + } diff -ur ./V0.82b_orig//uart.h ./V0.82b_cyphy//uart.h --- ./V0.82b_orig//uart.h 2011-08-16 16:58:27.000000000 +1000 +++ ./V0.82b_cyphy//uart.h 2011-06-21 00:12:29.000000000 +1000 @@ -49,6 +49,7 @@ }; extern struct str_Data3D Data3D; +#if 0 struct str_ExternControl { unsigned char Digital[2]; @@ -62,8 +63,56 @@ unsigned char Frame; unsigned char Config; }; +#endif + +#if 0 +struct str_ExternControl +{ + unsigned char Digital[2]; + unsigned char RemoteTasten; + signed char Nick; + signed char Roll; + signed char Gier; + unsigned char Gas; + signed char Hight; + unsigned char free; + unsigned char Frame; + unsigned char Config; +}; + +#endif + +struct str_ExternControl +{ + unsigned int Gas; + unsigned char Digital[2]; + unsigned char RemoteTasten; + signed char Nick; + signed char Roll; + signed char Gier; + unsigned char Hight; + unsigned char free; + unsigned char Frame; + unsigned char Config; + unsigned char dummy; +}; + + + extern struct str_ExternControl ExternControl; +struct str_Position +{ + int x; + int y; + int yaw; +}; +extern struct str_Position CurrentPosition; +extern struct str_Position TargetPosition; +extern struct str_Position PostPosition; + + + // FC hardware errors // bitmask for UART_VersionInfo_t.HardwareError[0] @@ -102,6 +151,7 @@ //#define USART0_BAUD 28800 //#define USART0_BAUD 38400 #define USART0_BAUD 57600 +//#define USART0_BAUD 115200 #endif //_UART_H