Skip to content

Commit

Permalink
Regelung auf PID umgestellt und integriert.
Browse files Browse the repository at this point in the history 
Roll-Teil aufgeräumt und einigermaßen funktionierende Parameter eingestellt.
  • Loading branch information
@slagon
slagon committed Jan 29, 2013
1 parent c974748 commit c6e4a96
Show file tree
Hide file tree
Showing 6 changed files with 6,092 additions and 11,747 deletions.
12,321 changes: 4,228 additions & 8,093 deletions Project/ARMCM4-STM32F407-DISCOVERY/keil/ch_Demo.dep
View file

Large diffs are not rendered by default.

5,394 changes: 1,822 additions & 3,572 deletions Project/ARMCM4-STM32F407-DISCOVERY/keil/lst/ch.map
View file

Large diffs are not rendered by default.

15 changes: 4 additions & 11 deletions Project/ARMCM4-STM32F407-DISCOVERY/keil/lst/chcoreasm_v7m.lst
View file
Expand Up @@ -1151,16 +1151,9 @@ RMCMx\\chcoreasm_v7m.s"
114 00000024 END
Command Line: --debug --xref --cpreproc --cpu=Cortex-M4.fp --apcs=interwork --d
epend=.\obj\chcoreasm_v7m.d -o.\obj\chcoreasm_v7m.o -I..\ -I..\..\..\boards\ST_
<<<<<<< HEAD
STM32F4_DISCOVERY -I..\..\..\os\ports\RVCT\ARMCMx\STM32F4xx -I"D:\Program Files
\Keil\ARM\RV31\INC" -I"D:\Program Files\Keil\ARM\CMSIS\Include" -I"D:\Program F
iles\Keil\ARM\Inc\ST\STM32F4xx" --list=.\lst\chcoreasm_v7m.lst ..\..\..\os\port
s\RVCT\ARMCMx\chcoreasm_v7m.s
=======
STM32F4_DISCOVERY -I..\..\..\os\ports\RVCT\ARMCMx\STM32F4xx -IC:\Keil\ARM\RV31\
INC -IC:\Keil\ARM\CMSIS\Include -IC:\Keil\ARM\Inc\ST\STM32F4xx --list=.\lst\chc
oreasm_v7m.lst ..\..\..\os\ports\RVCT\ARMCMx\chcoreasm_v7m.s
>>>>>>> 4168db3... Repair
STM32F4_DISCOVERY -I..\..\..\os\ports\RVCT\ARMCMx\STM32F4xx -IC:\Keil\ARM\CMSIS
\Include -IC:\Keil\ARM\INC\ST\STM32F4xx --list=.\lst\chcoreasm_v7m.lst ..\..\..
\os\ports\RVCT\ARMCMx\chcoreasm_v7m.s



Expand Down Expand Up @@ -1263,4 +1256,4 @@ Symbol: chThdExit
At line 84 in file ..\\..\\..\\os\\ports\\RVCT\\ARMCMx\\chcoreasm_v7m.s
Comment: chThdExit used once
2 symbols
341 symbols in table
334 symbols in table
15 changes: 4 additions & 11 deletions Project/ARMCM4-STM32F407-DISCOVERY/keil/lst/cstartup.lst
View file
Expand Up @@ -174,16 +174,9 @@ ck_size)
00000000
Command Line: --debug --xref --cpreproc --cpu=Cortex-M4.fp --apcs=interwork --d
epend=.\obj\cstartup.d -o.\obj\cstartup.o -I..\ -I..\..\..\boards\ST_STM32F4_DI
<<<<<<< HEAD
SCOVERY -I..\..\..\os\ports\RVCT\ARMCMx\STM32F4xx -I"D:\Program Files\Keil\ARM\
RV31\INC" -I"D:\Program Files\Keil\ARM\CMSIS\Include" -I"D:\Program Files\Keil\
ARM\Inc\ST\STM32F4xx" --list=.\lst\cstartup.lst ..\..\..\os\ports\RVCT\ARMCMx\c
startup.s
=======
SCOVERY -I..\..\..\os\ports\RVCT\ARMCMx\STM32F4xx -IC:\Keil\ARM\RV31\INC -IC:\K
eil\ARM\CMSIS\Include -IC:\Keil\ARM\Inc\ST\STM32F4xx --list=.\lst\cstartup.lst
..\..\..\os\ports\RVCT\ARMCMx\cstartup.s
>>>>>>> 4168db3... Repair
SCOVERY -I..\..\..\os\ports\RVCT\ARMCMx\STM32F4xx -IC:\Keil\ARM\CMSIS\Include -
IC:\Keil\ARM\INC\ST\STM32F4xx --list=.\lst\cstartup.lst ..\..\..\os\ports\RVCT\
ARMCMx\cstartup.s



Expand Down Expand Up @@ -424,4 +417,4 @@ Symbol: __use_two_region_memory
None
Comment: __use_two_region_memory unused
2 symbols
359 symbols in table
352 symbols in table
15 changes: 4 additions & 11 deletions Project/ARMCM4-STM32F407-DISCOVERY/keil/lst/vectors.lst
View file
Expand Up @@ -595,16 +595,9 @@ ARM Macro Assembler Page 10
345 00000002 END
Command Line: --debug --xref --cpreproc --cpu=Cortex-M4.fp --apcs=interwork --d
epend=.\obj\vectors.d -o.\obj\vectors.o -I..\ -I..\..\..\boards\ST_STM32F4_DISC
<<<<<<< HEAD
OVERY -I..\..\..\os\ports\RVCT\ARMCMx\STM32F4xx -I"D:\Program Files\Keil\ARM\RV
31\INC" -I"D:\Program Files\Keil\ARM\CMSIS\Include" -I"D:\Program Files\Keil\AR
M\Inc\ST\STM32F4xx" --list=.\lst\vectors.lst ..\..\..\os\ports\RVCT\ARMCMx\STM3
2F4xx\vectors.s
=======
OVERY -I..\..\..\os\ports\RVCT\ARMCMx\STM32F4xx -IC:\Keil\ARM\RV31\INC -IC:\Kei
l\ARM\CMSIS\Include -IC:\Keil\ARM\Inc\ST\STM32F4xx --list=.\lst\vectors.lst ..\
..\..\os\ports\RVCT\ARMCMx\STM32F4xx\vectors.s
>>>>>>> 4168db3... Repair
OVERY -I..\..\..\os\ports\RVCT\ARMCMx\STM32F4xx -IC:\Keil\ARM\CMSIS\Include -IC
:\Keil\ARM\INC\ST\STM32F4xx --list=.\lst\vectors.lst ..\..\..\os\ports\RVCT\ARM
CMx\STM32F4xx\vectors.s



Expand Down Expand Up @@ -1952,4 +1945,4 @@ s.s
s.s
Comment: __initial_msp used once
2 symbols
433 symbols in table
426 symbols in table
79 changes: 30 additions & 49 deletions Quad/Regelung.cpp
View file
Expand Up @@ -52,6 +52,9 @@ float ei_Roll_alt = 0;
float ea_Roll = 0;
float Soll_v_Roll = 0;
float aRoll;
float Roll_counter = 0;
float Roll_v_min = 0;
float Roll_v_max = 0;

float v_Yaw_tp1 = 0;
float v_Yaw_tp1_alt = 0;
Expand Down Expand Up @@ -106,7 +109,7 @@ static msg_t RegelungSyncthread(void *arg)
}
else if(rc_datalog != FR_OK)
{
//chprintf((BaseChannel *) &SD2, "SD Quadregelung.TXT: f_open() failed %d\r\n", rc_datalog);
chprintf((BaseChannel *) &SD2, "SD Quadregelung.TXT: f_open() failed %d\r\n", rc_datalog);
chThdSleepMilliseconds(10);
}
else
Expand All @@ -115,12 +118,12 @@ static msg_t RegelungSyncthread(void *arg)
rc_datalog = f_sync(&Fil_regelung);
if(rc_datalog != FR_OK)
{
//chprintf((BaseChannel *) &SD2, "SD Quadregelung.TXT: initial f_sync() failed %d\r\n", rc_datalog);
chprintf((BaseChannel *) &SD2, "SD Quadregelung.TXT: initial f_sync() failed %d\r\n", rc_datalog);
chThdSleepMilliseconds(10);
}
else
{
//chprintf((BaseChannel *) &SD2, "SD Quadregelung.TXT: opened successfull\r\n");
chprintf((BaseChannel *) &SD2, "SD Quadregelung.TXT: opened successfull\r\n");
chThdSleepMilliseconds(10);
datalog_regelung_opened = TRUE;
}
Expand All @@ -133,11 +136,11 @@ static msg_t RegelungSyncthread(void *arg)
rc_datalog = f_sync(&Fil_regelung);
if(rc_datalog != FR_OK)
{
//chprintf((BaseChannel *) &SD2, "SD Quadregelung.TXT: datalog f_sync() failed %d\r\n", rc_datalog);
chprintf((BaseChannel *) &SD2, "SD Quadregelung.TXT: datalog f_sync() failed %d\r\n", rc_datalog);
}
else
{
//chprintf((BaseChannel *) &SD2, "SD Quadregelung.TXT: datalog f_sync() succeeded\r\n");
chprintf((BaseChannel *) &SD2, "SD Quadregelung.TXT: datalog f_sync() succeeded\r\n");
}
datalog_regelung_syncing = 0;
chThdSleepMilliseconds(1000);
Expand All @@ -163,7 +166,7 @@ static msg_t RegelungPrintthread(void *arg)
if(readcounter!=writecounter && Datalogger_ready() && datalog_regelung_opened && datalog_regelung_syncing==0)
{
systime_t time = chTimeNow(); // Tnow
//chprintf((BaseChannel *) &SD2, "Printing:%d\r\n",time);
chprintf((BaseChannel *) &SD2, "Printing:%d\r\n",time);
f_printf(&Fil_regelung, "%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d\r\n",
regelung_timebuffer[readcounter],
regelung_databuffer[readcounter][0],
Expand Down Expand Up @@ -269,9 +272,11 @@ void Regelung(void)
inRollSollLage = get_euler_roll_soll();
inYawSollLage = 0;

inYawIstLage = get_euler_yaw_ist();
inNickIstLage = get_euler_nick_ist();
inRollIstLage = get_euler_roll_ist();
v_Roll_tp1 = ((get_ypr_roll_ist() - inRollIstLage)*100)*0.05+v_Roll_tp1*0.95;

inYawIstLage = get_ypr_yaw_ist();
inNickIstLage = get_ypr_nick_ist();
inRollIstLage = get_ypr_roll_ist();

inRollIstV = get_rate_roll_ist();
inNickIstV = get_rate_nick_ist();
Expand All @@ -293,7 +298,7 @@ void Regelung(void)
if(inSchub > 0.1 && inSchub <=1 /*&& inNickIstLage < 0.5 && inNickIstLage > -0.5*/)
ii_Nick = 0.08 * ei_Nick + ii_Nick; //I-Anteil nur nahe der Nulllage ver�ndern

di_Nick = (ei_Nick - ei_Nick_alt)*10; //d-Anteil
di_Nick = ((ei_Nick - ei_Nick_alt)*10*0.1)+di_Nick*0.9; //d-Anteil
if (di_Nick > 1.5)
di_Nick = 1.5;
else if(di_Nick < -1.5)
Expand All @@ -303,50 +308,26 @@ void Regelung(void)

/////////////////////////// Roll-Regler berechnen //////////////////////////////////////////

/*ea_Roll = (inRollSollLage) - (inRollIstLage+0.08*inRollIstV); // Eingang in den �u�eren Regler
if(inSchub > 0.1 && inSchub <=1 && inRollIstLage < 0.2 && inRollIstLage > -0.2)
ia_Roll = 0.01 * ea_Roll + ia_Roll; //I-Anteil nur nahe der Nulllage ver�ndern
Soll_v_Roll = ea_Roll*2 + ia_Roll*0.02; //Sollgeschwindigkeit des �u�eren Reglers
//Soll_v_Roll = inRollSollLage;
//innerer Regler
v_Roll_tp1 = 0.95*v_Roll_tp1 + 0.05*inRollIstV; // Tiefpass-gefilterter Gyrowert
ei_Roll= Soll_v_Roll - v_Roll_tp1; //Eingang in die innere Regelung
pi_Roll = ei_Roll * 0.5; //p-Anteil
if(inSchub > 0.1 && inSchub <=1 && inRollIstLage < 0.2 && inRollIstLage > -0.2)
ii_Roll = 0.1 * ei_Roll + ii_Roll; //I-Anteil nur nahe der Nulllage ver�ndern
di_Roll = (ei_Roll - ei_Roll_alt)*20; //d-Anteil
if (di_Roll > 1.5)
di_Roll = 1.5;
else if(di_Roll < -1.5)
di_Roll = -1.5; //Saturierung des D-Anteils
aRoll = (pi_Roll + (ii_Roll+37)*0.02 + di_Roll)*567; //Ausgang des inneren Reglers
ea_Roll = (inRollSollLage) - (inRollIstLage+0.08*inRollIstV); // Eingang in den �u�eren Regler
if(inSchub > 0.1 && inSchub <=1 && inRollIstLage < 0.2 && inRollIstLage > -0.2)
ia_Roll = 0.01 * ea_Roll + ia_Roll; //I-Anteil nur nahe der Nulllage ver�ndern
Soll_v_Roll = ea_Roll*2 + ia_Roll*0.02; //Sollgeschwindigkeit des �u�eren Reglers
Soll_v_Roll = inRollSollLage;

//innerer Regler
v_Roll_tp1 = 0.95*v_Roll_tp1 + 0.05*inRollIstV; // Tiefpass-gefilterter Gyrowert
ei_Roll= Soll_v_Roll - v_Roll_tp1; //Eingang in die innere Regelung
pi_Roll = ei_Roll * 0.1; //p-Anteil
if(inSchub > 0.1 && inSchub <=1 /*&& inRollIstLage < 0.2 && inRollIstLage > -0.2)
ii_Roll = 0.01 * ei_Roll + ii_Roll; //I-Anteil nur nahe der Nulllage ver�ndern
ea_Roll = (inRollSollLage) - (inRollIstLage); //Reglereingang
ei_Roll = ea_Roll;

pi_Roll = ei_Roll * 0.7; //P-Regler
di_Roll = (ei_Roll - ei_Roll_alt)*40; //D-Regler
if(inSchub > 0.1)
ii_Roll = 0.05 * ei_Roll + ii_Roll; //I-Regler

di_Roll = (ei_Roll - ei_Roll_alt)*10; //d-Anteil
if (di_Roll > 1.5)
di_Roll = 1.5;
else if(di_Roll < -1.5)
di_Roll = -1.5; //Saturierung des D-Anteils
if (di_Roll > 2) //Saturierung D
di_Roll = 2;
else if(di_Roll < -2)
di_Roll = -2;

aRoll = (pi_Roll + (ii_Roll)*0.005 + di_Roll)*567; //Ausgang des inneren Reglers */
if (ii_Roll > 2) //Saturierung I
ii_Roll = 2;
else if(ii_Roll < -2)
ii_Roll = -2;

aRoll = ((inRollSollLage) - (inRollIstLage))*567*-0.5;
aRoll = (pi_Roll + (ii_Roll)*0.1 + di_Roll)*567;

/////////////////////////// Yaw-Regler berechnen //////////////////////////////////////////

Expand Down

0 comments on commit c6e4a96

Please sign in to comment.