@Joachim08
äh 6 Uhr früh ? Sag mal Rob wann schläfst Du ?
Von 0100 - 0530 war sehr knapp heute - habe mich durch den Tag geschleppt.
So jetzt Butter bei die Fische.
Die Baros (BMP & MS) haben verschiedene Modi mit verschiedenen Auflösungen. D.h wenn ich eine bessere Höhenauflösung will, braucht der Sensor mehr Bedenkzeit, d.h ich kann ihn weniger häufig/Sek abfragen, die Mittelwertberechnung (ich glaube, MWII macht einen gleitenden "moving average" Mittelwert) bringt noch mehr Zeitverzögerung in die Regelung.
1. Fehler der aktuellen Version:
In MWII Version 1.9 wurde der BMP in den Ultra high resolution Modus geschaltet, d.h. 25cm Genauigkeit, 25,5ms Latenz - dabei wurden in der Auswertroutine 25 ms angenommen. Ab Version 2.0 wurde vernünftigerweise auf den high resoltution Modus zurückgeschaltet d.h. 30cm Genauigkeit, 13,5ms Latenz, um schneller mehr Werte zu bekommen. Gute Idee, doch leider wurde die Auswertroutine bei 25ms, also 40Hz Auswertfrequenz belassen. D.h. ab Version 2.0 gibt es für den BMP nur 5 cm weniger Genauigkeit!
2.Fehler aller Versionen ab 1.9:
Die Auswertroutine holt sich alle 25ms neue Daten, egal ob es schon neue gibt oder nicht. Dazwischen liegende Werte (ca alle 14ms) gehen vollkommen an ihr vorbei.
Das sieht dann so aus (IMU):
Code:
#define UPDATE_INTERVAL 25000 // 40hz update rate (20hz LPF on acc)
#define INIT_DELAY 4000000 // 4 sec initialization delay
#define BARO_TAB_SIZE 40
void getEstimatedAltitude(){
uint8_t index;
static uint32_t deadLine = INIT_DELAY;
static int16_t BaroHistTab[BARO_TAB_SIZE];
static int8_t BaroHistIdx;
static int32_t BaroHigh,BaroLow;
int32_t temp32;
int16_t last;
if (currentTime < deadLine) return;
deadLine = currentTime + UPDATE_INTERVAL;
Das Problem lässt sich leicht umgehen, indem man im Hauptprogramm eine Variable definiert (z.B newbaroalt) die von "Sensors / Baro_update" einfach auf 1 gesetzt wird, wenn es wieder einen neuen BaroAlt Wert gibt, der dann weiter verarbeitet werden kann. Nach der Verarbeitung muss man die Variable natürlich wieder auf 0 setzten, um einen neuen Wert zu erkennen.
Die Lösung kann so aussehen: (Eine Unsauberkeit: nach ca 71 Minuten Flugzeit mit einem Akku, fällt der Baro für 4 Sec aus)
Code:
Im Hauptprogramm:
static uint8_t newbaroalt=0;
Unter IMU getEstimatedAltitude:
#define BARO_TAB_SIZE 40
#define INIT_DELAY 4000000 // 4 sec initialization delay
void getEstimatedAltitude(){
uint8_t index;
static int16_t BaroHistTab[BARO_TAB_SIZE];
static int8_t BaroHistIdx;
static int32_t BaroHigh,BaroLow;
int32_t temp32;
int16_t last;
if (newbaroalt==0 || currentTime < INIT_DELAY) return; // Possible Problem after 71 Minutes Flighttime
newbaroalt=0;
newestalt=1;
//**** Alt. Set Point stabilization PID ****
//calculate speed for D calculation
last = BaroHistTab[BaroHistIdx];
BaroHistTab[BaroHistIdx] = BaroAlt/10;
BaroHigh += BaroHistTab[BaroHistIdx];
index = (BaroHistIdx + (BARO_TAB_SIZE/2))%BARO_TAB_SIZE;
BaroHigh -= BaroHistTab[index];
BaroLow += BaroHistTab[index];
BaroLow -= last;
BaroHistIdx++;
if (BaroHistIdx == BARO_TAB_SIZE) BaroHistIdx = 0;
BaroPID = 0;
//D
temp32 = D8[PIDALT]*(BaroHigh - BaroLow) / BARO_TAB_SIZE;
BaroPID-=temp32;
EstAlt = BaroHigh*10/(BARO_TAB_SIZE/2);
temp32 = AltHold - EstAlt;
if (abs(temp32) < 10 && abs(BaroPID) < 10) BaroPID = 0; //remove small D parametr to reduce noise near zero position
//P
BaroPID += P8[PIDALT]*constrain(temp32,(-2)*P8[PIDALT],2*P8[PIDALT])/100;
BaroPID = constrain(BaroPID,-150,+150); //sum of P and D should be in range 150 Crashpilot1000
//I
errorAltitudeI += temp32*I8[PIDALT]/50;
errorAltitudeI = constrain(errorAltitudeI,-30000,30000);
temp32 = errorAltitudeI / 500; //I in range +/-60
BaroPID+=temp32;
}
Unter Sensors Baro_update()
Für den BMP:
....
case 3:
i2c_BMP085_UP_Read();
i2c_BMP085_Calculate();
BaroAlt = (1.0f - pow(pressure/101325.0f, 0.190295f)) * 4433000.0f; //centimeter
bmp085_ctx.state = 0; bmp085_ctx.deadline += 5000;
newbaroalt=1;
break;
.....
Für den MS:
......
case 3:
i2c_MS561101BA_UP_Read();
i2c_MS561101BA_Calculate();
BaroAlt = (1.0f - pow(pressure/101325.0f, 0.190295f)) * 4433000.0f; //centimeter
ms561101ba_ctx.state = 0; ms561101ba_ctx.deadline += 4000;
newbaroalt=1;
break;
......
Im Ergebnis hat das getEstimatedAltitude Unterprogramm jetzt anständigere Werte, aus denen der gleitende MW berechnet werden kann. Da die Werte jetzt konsistenter sind, kann man die Konstante BARO_TAB_SIZE 40 erniedrigen auf z.B 20. D.h. der MW wird jetzt immer nur noch über 20 Werte gebildet, d.h. ein Änderung schlägt jetzt schneller durch.
Info: Der BMP liefert so alle 14ms, der MS alle 10ms neue Werte.
Ach, ja diese Zeile:
Code:
BaroPID = constrain(BaroPID,-150,+150);
Bereitet mir auch noch Kopfzerbrechen. Der "BaroPID" wird nacher zum Gaskanal addiert. Beim Sinken/Fallen wird dem Copter schliesslich von der Gravitation geholfen, kann es daher sinnvoll sein, die Negativwerte weiter zu limitieren um Aufschwingen zu verhindern? Also z.B so:
Code:
BaroPID = constrain(BaroPID,-100,+150);
Nachteil: Die aktuellen PID Werte für den Baro kann man jetzt vergessen, die müssen neu erflogen werden. Vielleicht muss der Baro Pid Kontroller jetzt noch überarbeitet werden. Mit den aktuellen Änderungen hält mein Copter die Höhe jetzt SCHLECHTER, obwohl der Code m.E ein Schritt in die richtige Richtung ist - mal sehen, was man mit den PIDS noch machen kann.
Anbei die Datei mit den Änderungen - nur für sehr EXPERIMENTIERFREUDIGE ! WARNUNG! DANGER!
LG
ROB
P.s.:MultiWii_dev_20120528.zip gesichtet, KEINE Änderung am Baro Code!