Brushless Motor per Arduino steuern

[das_copter_198]

Also als Auflösung dann 65535 und als Bereich dann 4000 wenn ich MPU6050_GYRO_FS_2000 nehme.

 

[donvido]

ja, genau so

 

[das_copter_198]

Ich hab gestern Abend noch probiert die Range zuändern und es ging nur als ichs in void loop() gepackt hab. Außerdem bin ich mir nicht sicher ob meine Werte stimmen, die kommen mir sehr niedrig vor.

 

[das_copter_198]

Ich hab gerade noch nen Forumeintrag zu dem Thema gefunden: http://forum.arduino.cc/index.php?topic=183417.0
Dort wird die Gyro Rate so ausgerechnet: rate = (raw - Zero Rate Offset) / Sensitivität
Das wäre dann, laut Datasheet 16,4 als Sensitivität. Kann ich so auch die Rate ausrechnen?

 

[donvido]

Öhm, das ist genau das gleiche wie das, was ich dir gegeben habe.
32768/250[°/2]=131=16-Bit/500[°/s]

edit: 32768/2000=16,4

doubleedit: den Offset kannst du auch nach der Umrechnung rausrechnen

 

[das_copter_198]

Oh, gar nicht bemerkt Ich hab nen gemessene Offset von ca. -13 soll ich das nehmen, oder ausrechnen bzw. wie rechne ich das dann aus?

 

[donvido]

Naja, einfach soviel dazuaddieren oder abziehen, bis du bei Null Bewegung auch den Messwert Null hast. Bei großsen Offsets musst du aber n Hardware-Offset einstellen, weil du sonst in die Anschläge kommst.

 

[das_copter_198]

Ich hab bei den Rohwerten -13 bei keiner Bewegung. Die Berechnung wäre dann ja:
rate = (raw + 13) /16.4
Die Off Set sind auch Rohdaten?

 

[donvido]

Hängt davon ab, ob du vor oder nach der Umrechnung deinen Offset bestimmst.

 

[das_copter_198]

Hallo,

ich habe gestern den Kalmanfilter geschrieben und mir ist aufgefallen, dass die Werte am Anfang noch nicht richtig sind, erst nach einigen Durchläufen stimmen die Messungen. Woran liegt das und wie kann ich das umgehen?

 

[donvido]

Klingt als würden deine Startwerte nicht mit den realen Werten übereinstimmen, und der Filter sich erst mal einschwingen muss.

 

[das_copter_198]

Ja genau so ist es, er brauch erste einmal eine kurze Startphase bevor es wirklich geht.

 

[das_copter_198]

Kann ich dagegen was machen? Oder ist das normal?

 

[donvido]

Naja, ein Kalmanfilter ist schon deutlich komplizierter, als das simple Auslesen eines Sensors. Und wenn es bereits beim Sensor hakt, dann wird das beim Filter garantiert nicht einfacher. Ein bisschen mehr Input wäre dann auch nicht schlecht.
Wie sieht dein Modell aus, was sind deine Zustände, was deine Eingänge, mit welchen Werten wird dein Kalmanfilter initialisiert, etc.

 

[das_copter_198]

Grunsätzlich ist es dieser Code:

https://github.com/TKJElectronics/E...cluding-Kalman-filter/tree/master/IMU/MPU6050

D.h ist hab die Kalmanfunktion und die berechnet dann den Wert. Die 3 anpassbaren Werte sind bei mir wie in dem Code.

 

[donvido]

Versuchs mal damit:

void kalmanRoll(float *ym1, float *ym2, float *xH_1, float *xH_2, float *dt){

float xS_1, xS_2;

float pS_11, pS_12,
float pS_21, pS_22;

float lS_11, lS_12;
float lS_21, lS_22;

static float pH_11=1;
static float pH_12=0;
static float pH_21=0;
static float pH_22=1;



xS_1 = *xH_1+*dt**xH_2;
xS_2 = *xH_2;

pS_11 = pH_11+*dt*pH_21+*dt*(pH_12+*dt*pH_22)+1.0E-4;
pS_12 = pH_12+*dt*pH_22;
pS_21 = pH_21+*dt*pH_22;
pS_22 = pH_22+1.0E-4;

lS_11 = -(pS_12*pS_21*1.0-pS_11*(pS_22+2.0E1))/(pS_22*2.0E1-pS_12*pS_21*1.0+pS_11*(pS_22+2.0E1)+4.0E2);
lS_12 = (pS_12*2.0E1)/(pS_22*2.0E1-pS_12*pS_21*1.0+pS_11*(pS_22+2.0E1)+4.0E2);
lS_21 = (pS_21*2.0E1)/(pS_22*2.0E1-pS_12*pS_21*1.0+pS_11*(pS_22+2.0E1)+4.0E2);
lS_22 = (pS_22*2.0E1+pS_11*pS_22-pS_12*pS_21*1.0)/(pS_22*2.0E1-pS_12*pS_21*1.0+pS_11*(pS_22+2.0E1)+4.0E2);

*xH_1 = lS_12*(xS_2-*ym2*1.0)*-1.0-xS_1*(lS_11*1.0-1.0)+lS_11**ym1*1.0;
*xH_2 = xS_2-lS_22*(xS_2-*ym2*1.0)*1.0-lS_21*xS_1*1.0+lS_21**ym1*1.0;

pH_11 = lS_12*pS_21*-1.0-pS_11*(lS_11*1.0-1.0);
pH_12 = lS_12*pS_22*-1.0-pS_12*(lS_11-1.0)*1.0;
pH_21 = lS_21*pS_11*-1.0-pS_21*(lS_22-1.0)*1.0;
pH_22 = lS_21*pS_12*-1.0-pS_22*(lS_22-1.0)*1.0;
}

Die Funktion wird folgendermaßen aufgerufen:

kalmanRoll(&alpha, &alpha_p, &xr_1, &xr_2, &dt);

Wichtig ist, dass deine Messwerte alpha und alpha_p konsistent entweder in Radiant und Radiant/s oder in Grad und Grad/s angegeben werden.
dt ist die Dauer eines Zyklus in Sekunden.
alpha - gemessener Winkel
alpha_p - gemessene Winkelgeschwindigkeit
xr_1 - berechneter Winkel
xr_2 - berechnete Winkelgeschwindigkeit

Außerdem müssen/können deine Zustände initialisiert werden (was ich vorhin meinte). Und zwar public, außerhalb einer Funktion.

float xr_1=0;
float xr_2=0;

Wenn du z.B. mit 45° Neigung dein Kalmanfilter startest, dann solltest du xr_1=90 bzw. xr_1=1.57 wählen.
Die Werte für Q und R sind in dieser Funktion schon gesetzt und dürften eigentlich ganz gut funktionieren.

Viel Erfolg

 

[das_copter_198]

Danke für den Code. Ich hab mir meinen nochmal angeschaut und ein bisschen abgeändert und eine Funktion hinzugefügt, jetzt geht alles

 

[das_copter_198]

Hallo,

ich habe heute die PID Regelung für X und Y geschrieben, mit den Werten aus dem Kalmanfilter. Ich wollte nur kurz Fragen die Berechnungen richtig sein:

P_Part = error * P_Config;
I_Part = error_sum * I_Config;
D_Part = ((error_old - error) * D_Config) / looptime;
PID = P_Part + I_Part + D_Part;

looptime ist in Sekunden.
Stimmt das so?

 

[donvido]

Sieht soweit ganz gut aus. Wichtig ist allerdings, dass du bei error_sum nicht einfach den Fehler in jedem Zyklus aufaddierst.
Es handelt sich um das zeitliche Integral. Du musst also error_sum folgendermaßen bestimmen.
error_sum+=error*looptime;

 

[das_copter_198]

Ok, dass mit der looptime wusste ich nicht, kommt gleich in den Code. Welche Werte für die Config sind zum Testen empfelenswert? Weil ich hab jetzt noch überall 1.00. P sollte doch realtiv hoch sein im Vergleich zu I und D oder?