Quadrocopter (Dimensionierung)

[PICture]

Hallo!

Ich vermute eine Störung des MPU-6000 (Gyro + ACC) durch die Motoren.

Ich kann dir aus Erfahrung nur empfehlen: nicht vermuten sondern messen und prüfen. Um eventuelle EMV Störungen zu detektieren, würde ich ein Radio mit Lang- bzw. Mittelwellen nehmen, auf niedrigste Frequenz ohne Sender einstellen und zuhören.

[Che Guevara]

Hi,

ich würde da auch erstmal versuchen, soviel wie möglich zu messen & debuggen.
Wenns geht, lass dir doch die MPU-Daten (GYRO, ACC, FUS. WINKEL), die Motor-Daten (PWM-Wert) und soweiter ausgeben. Dann sollte man doch relativ schnell draufkommen, welcher Wert da in die Begrenzung geht.
Wie fusionierst du den die Werte vom MPU? Ist der P-Regler auf diesem fus. Wert aufgebaut oder aus auf was anderem?

Gruß
Chris

[crabtack]

Hi, danke für die schnellen Antworten!

Radio habe ich gerade kein funktionsfähiges in der Werkstatt, ist ironischerweise vorgestern kaputt gegangen....
Aber die Idee ist gut, werde ich definitiv probieren, wenn wieder EMV im Verdacht steht.
Mittlerweile ist aber klar, dass es nicht an EMV Störungen liegt.
Ich habe mal alle relevanten Leitungen mit dem Scope gemessen, da ist kaum was an Störungen drauf, auch wenn die Motoren laufen.

Mir mal die Werte vor der Datenfusion ausgeben zu lassen war eine gute Idee.
Das Problem ist, dass der Beschleunigungssensor durch die Vibration der Motoren gestört wird.
Das Gyroskop gibt die richtigen werte aus.
Wenn die Mototoren aus sind erhalte ich vom Beschleunigungssensor etwa den Wert 0x4000 (Entspricht 90°), also er liegt horizontal(Habe ihn ziemlich exakt ausgerichtet).
Wenn ich nun die Motoren einschalte steigt der Wert auf etwa 0x6000 (Entspricht 135°, also 45° Abweichung).
Wenn ich die Motoren Abschalte gehen die Werte wieder runter auf 0x4000.
Das Ergebnis der Fusion hängt dem Wert natürlich etwas hinterher, aber nimmt nach einer gewissen Zeit den Wert an.
Problem ist, dass der Ausgabewert des Beschleunigungssensors nicht nach unten und oben schwankt, sondern nur auf etwa 0x6000 geht und dort bleibt, sobald die Motoren an sind, dadurch wird das Ergenis recht schnell in die Höhe getrieben.

Auch wenn ich den Einfluss des Beschleunigungssensors stark reduziere bleibt das Problem bestehen, dauert dann halt noch was länger.

Der relevante Teil zur Datenfusion und zum Auslesen des MPU-6000 sieht so aus:

void MPU::Init()
{
    WriteReg(107,0x83); //Reset und PLL mit ZGyro Referenz als Takt
    _delay_ms(5);
    WriteReg(107,0x03);    //PLL mit ZGyro Referenz als Takt
    _delay_ms(1);
    WriteReg(106,0x10); //I²C Deaktivieren, SPI aktivieren
    _delay_ms(1);
    WriteReg(25,0x00);  //Sample Rate = GyroOutputRate/(1 + SMPLRT_DIV)
    _delay_ms(1);
    WriteReg(26,0x06); //Tiefpass 5Hz
    _delay_ms(1);
    WriteReg(27,0x18); //Gyro +/- 2000°/s
    _delay_ms(1);
    WriteReg(28,0x00); //ACC +/- 2g
    _delay_ms(1);
    
    TCE1.CTRLA = TC_CLKSEL_DIV1024_gc;
    TCE1.CTRLB = 0x00; //Normal Mode
    TCE1.PER = 0xFFFF; //Obergrenze
    
    Xangle.integer = 0x4000;
    Yangle.integer = 0x4000;
    
    
}


void MPU::GetSensorData()
{
    uint8_t dump;
    PORTC.OUT &= ~(1 <<PIN4);
    dump     = SpiTransfer(0xBB);
    Ax.byte2 = SpiTransfer(0x00);
    Ax.byte1 = SpiTransfer(0x00);
    Ay.byte2 = SpiTransfer(0x00);
    Ay.byte1 = SpiTransfer(0x00);
    Az.byte2 = SpiTransfer(0x00);
    Az.byte1 = SpiTransfer(0x00);
    Temperature.byte2 = SpiTransfer(0x00);
    Temperature.byte1 = SpiTransfer(0x00);
    Gx.byte2 = SpiTransfer(0x00);
    Gx.byte1 = SpiTransfer(0x00);
    Gy.byte2 = SpiTransfer(0x00);
    Gy.byte1 = SpiTransfer(0x00);
    Gz.byte2 = SpiTransfer(0x00);
    Gz.byte1 = SpiTransfer(0x00);
    
    
    
    PORTC.OUT |= (1 << PIN4);
}


void MPU::CalcAngle()
{
    MPU::AccXangle.integer = (atan2(MPU::Az.integer,MPU::Ay.integer) * 10430); //Winkel Berechnen und auf werte von -32766 bis + 32766 hochskalieren
    MPU::AccYangle.integer = (atan2(MPU::Az.integer,MPU::Ax.integer) * 10430);
    MPU::AccZangle.integer = (atan2(MPU::Ay.integer,MPU::Ax.integer) * 10430);
    
    float deltaTime = ((float)(TCE1.CNTL + (TCE1.CNTH << 8)) / 31250.0f); //Verstrichene Zeit
    TCE1.CNTL = 0;
    TCE1.CNTH = 0;
    Xangle.integer = 0.999*(Xangle.integer +(-Gx.integer * deltaTime)) + 0.001*AccXangle.integer; //Fusion
    Yangle.integer = 0.999*(Yangle.integer +(-Gy.integer * deltaTime)) + 0.001*AccYangle.integer;
    Zangle.integer =(Zangle.integer +(-Gz.integer * deltaTime));
    //PORTH.OUTTGL = 0x01;
}


uint16_t MPU::GetXangle()
{
    return Xangle.integer - 0x4000; //Anpassen, dass der Wert bei horizontaler Lage 0 ist.
}


uint16_t MPU::GetYangle()
{
    return Yangle.integer - 0x4000;
}

Habe ich vielleicht nur einen Programmfehler oder liegt es wirklich am Beschleunigungssensor selbst?
Ich Gehe übrigens von Kreisen mit 65536° aus, um 2Byte voll auszunutzen.

Edit:
Wenn ich den Tiefpass vom MPU-600 rausnehmen, also das Register 26 auf 0x00 setze, dann wird es etwas besser, dann schwanken die werte um die 0x4000.
Problem ist, dass viel häufiger Werte deutlich über 0x4000 auftreten, als Werte unter 0x4000.
Also nach der Fusion erhalte ich immer Werte über 0x4000, meistens zwischen 0x5000 und 0x6000

mfg
Olaf

[Che Guevara]

Hi,

also wenn ich mich recht erinnere, hatte Harald ( https://www.roboternetz.de/community...uette-NanoQuad ) damals fast das selbe Problem, da half glaube ich nur die Suche nach anderen Motoren, die runder laufen.
Was mir spontan auffällt:
- Beim MPU-6000 ist der DLPF auf 5Hz gestellt, das ist zu stark --> ich habe ihn auf 44Hz, etwas mehr ginge wohl auch noch (experimentiere gerade mit nem FIR-Filter)
- Beim Komplementärfilter beziehst du die Regelfrequenz (dt) mit ein. Da du aber in Fixed-Point rechnest und deswegen sowieso skalieren musst, würde ich (auch im Bezug aufs Ergebnis) den Filter mit einem festen Zeitintervall (z.b. 2ms oder 1ms oder..) aufrufen, so ist dt konstant und kann entfallen. Du musst dann nur den ACC-Winkel richtig skallieren.
- ATAN2 ist relativ teuer, es ginge auch mit arcsin. Wenn du aber bei ATAN2 bleiben willst, würde ich folgende Gleichung vorschlagen:

TotalForce = sqrtf((AccX*AccX)+(AccY*AccY)+(AccZ*AccZ));
RollAngleAcc = atan2(AccY, TotalForce);
NickAngleAcc = atan2(AccX, TotalForce);

Damit beziehst du die aktuelle Gesamtbeschleunigung mit ein und der Fehler durch "Querbeschleunigungen" wird geringer.
- Beim Komplementärfilter solltest du statt "0.999" & "0.001" lieber zwei Variablen nehmen, z.b. GyroInfluence & AccInfuence. Dann beschreibst du eine Variable und berechnest (!!!) aus dieser die andere Variable. So verhinderst du "Rundungsfehler" durch die begrenzte Nachkommastellenzahl bei Floats. Ansonsten könnte es passieren, dass 0.001f + 0.999f != 1.0f .
- Den Komplementärfilter würde ich als Float berechnen, ansonsten bekommst du ein ähnliches Problem wie beim Punkt vorher (Rundungsfehler).
- Hin und wieder ein Type-cast würde nicht schaden, schützt vor üblen Überraschungen
- Wenn du den Timer TE1 verwendest, addressierst du immer CNTL & CNTH. Wieso nimmst du nicht einfach CNT (u16)?

Hoffe, das sind jetzt nicht zuviele Punkte auf einmal, aber ich hab mit den meisten davon selbst schon zu kämpfen gehabt ...

Gruß
Chris

[crabtack]

Hi!

Zu viele Punkte gibts nicht, ich bin dankbar für jeden einzelnen.
Die 5Hz waren nur zum testen, ob es das Verhalten des Beschleunigungssensors verbessert, ursprünglich hatte ich auch die 44Hz genommen (Wie in deinem Programm vom Anfang).

Die Idee, die Berechnung in einem festen Zeitintervall erfolgen zu lassen ist gut, lasse ich dann einfach per Interrupt steuern.

Ich musste schon feststellen, dass die Berechnung etwas Langsam ist, das Programm schafft nur etwa 1000 Durchläufe pro Sekunde, da sollte noch mehr möglich sein.
Sobald die groben Fehler beseitigt sind werde ich mal darauf umstellen, danke!

Zu den Rundungsfehlern.
Ich denke, dass ein 16bit int genau genug ist, dafür habe ich ja alles auf Werte von 0 bis 2^16 skaliert, damit ich nicht den höheren Rechenaufwand von float in kauf nehmen muss.
Oder irre ich mich?

- Hin und wieder ein Type-cast würde nicht schaden, schützt vor üblen Überraschungen

Du meinst, dass ich jeweils explizit auf signed oder unsigned casten soll?

- Wenn du den Timer TE1 verwendest, addressierst du immer CNTL & CNTH. Wieso nimmst du nicht einfach CNT (u16)?

Ich wusste bisher einfach nicht, dass diese Möglichkeit besteht und war es so gewohnt, aber wenn das geht werde ich es ab jetzt so machen.

Ich bin dem Problem jetzt etwas näher gekommen.
Ich habe mir mal die Werte, die das Gyroskop ausgibt über UART ausgeben lassen und grafisch dargestellt.
Dabei haben sich die Y und Z Achse so verhalten, wie ich es erwartet habe.
Der Wert bleibt bei Ausgeschalteten Motoren stabil, reagiert auf Lageänderungen und Erschütterungen, wie er soll.
Wenn ich die Motoren Einschalte schwingt der Wert um den eigentlich richtigen Wert (z.B. 0).
Wenn ich das Gleiche auf der X Achse mache, dann verhält es sich ohne Motoren normal und reagiert auf Lageänderungen und Erschütterungen.
Wenn ich aber die Motoren einschalte ist der Ausgangswert ungewöhnlich stark gestört.
Und vor allem Schwingt er nicht um 0, sondern darunter.
Ich habe die Bilder in den Anhang gesteckt, damit man sieht, was ich meine.

Ich verstehe nicht, wie das sein kann.
Die Y Beschleunigung wird auf die gleiche Weise ausgelesen, wie der X und die Z Beschleunigung.
Woher kann es kommen, dass nur die X Beschleunigung fehlerhaft ist?

@Chris: Gibt es eigentlich Videos oder sonstige Doku zu deinem Copter?
Würde mich mal interessieren, wie der sich so verhält.

mfg
Olaf

[Che Guevara]

Hi,

also zu den Rundungsfehlern. Nehmen wir mal dieses Stückchen Code:

Xangle.integer = 0.999*(Xangle.integer + (-Gx.integer * deltaTime)) + 0.001*AccXangle.integer;

So sähe es schonmal mit Typecase aus (ich verwende lieber zuviel als zuwenig wie man merkt):

Xangle.integer = (int16_t)((0.999f*(float32_t)(Xangle.integer+((float32_t)-Gx.integer * deltaTime)) + (0.001f*(float32_t)AccXangle.integer));

So wird auch korrekt gerechnet. Hast du mal überprüft, was dabei rauskommt, wenn man einen int16_t (vor allem sehr kleine Werte) mit einem float (0.999 oder 0.001) multipliziert?
Diese Kurve vom AccX sieht wirklich etwas komisch aus, da fällt mir momentan auch keine Erklärung ein (außer evtl. Vibrationen von den Motoren).
Ein Video bzw. Homepage steht schon lange auf der Liste, aber dafür hab ich irgendwie nie so wirklich Zeit ...

Gruß
Chris

[crabtack]

Hi, danke für die Antwort!

Bisher hatte ich noch nicht mit float auf Mikrocontrollern gearbeitet, sondern nur auf dem PC, dort hatte ich damit nie Probleme gehabt.
Ich werde es mal auf dem Controller ausprobieren, sobald ich wieder in der Werkstatt bin.

Also es liegt definitiv an den Vibrationen der Motoren.
Das "Schwingen" der werte auf den Anderen beiden Achsen liegt ja auch daran.
Problem ist nur, dass die AccX Kurve nicht um die Nullinie Schwingt und außerdem viel zu stark.
Vibrationen können ja eigentlich nicht auf eine Achse viel Stärker wirken als auf die andere, zumindest in diesem Fall nicht.
Ich finde einfach keinen Grund dafür, bin jetzt alles nochmal mehrfach durchgegangen.

mfg
Olaf