Quadrocopter (Dimensionierung)

[crabtack]

Hi, Danke für die Antworten!

Gut, dann kommen Kompass und Luftdrucksensor noch rein.
Ja, ich habe versucht nach den Abkürzungen zu googlen, aber einfach nichts brauchbares gefunden.
Zu autonomen Funktionen habe ich mir noch nichts durchgelesen, da ich die Probleme selbst lösen möchte.
Den Kompass wollte ich dadurch ersetzen, dass durch die GPS Daten (in Bewegung) die Richtung bestimmt wird und bei Drehungen auf der Stelle soll das Gyro übernehmen.
Wenn der Copter dann nah genufgam Homepoint ist soll die Kamera den Landeplatz erkennen und den Copter landen (Er soll nie ohne PC gesteuert werden).
Aber wenn Kompassmodule so gut funktionieren werde ich noch eins einplanen.

Sorry, dass ich gerade etwas unmotiviert bin, aber es gibt einfach nichts, was ich mehr hasse, als Layouten.
Ich möchte einfach so schnell wie möglich damit fertig werden, damit ich mit dem Programmieren anfangen kann, woran ich deutlich mehr Spaß habe.
Ab nächster Woche darf ich endlich wieder arbeiten, dann wird das wieder besser.

mfg
Olaf

[crabtack]

Hi!

So, jetzt passiert endlich mal was.
Letzte Woche sind die Bauteile für die Fernbedienung angekommen, habe dann gleich eine Platine geätzt und bestückt.
Nach 3 Stunden probieren wusste ich dann auch, dass der Kondensator an Reset zum Programmieren weg muss
Dann lief erstmal alles gut, Ansteuerung für Dualshock war schnell geschrieben, Kommunikation zum PC läuft auch ohne Probleme und das Funkmodul scheint auch zu funktionieren.
Xmegas sind echt toll.

Jedenfalls habe ich jetzt doch ein Problem, dass ich mir einfach nicht erklären kann.
In meinem Programm sende ich dem Xbee 3 Plus Zeichen um es in den Command Mode zu versetzen, danach soll es ein "OK<CR>" zur Bestätigung senden.
Das kommt auch definitiv an.
Jedenfalls möchte ich die Zeichen, die das Xbee sendet in einer Interrupt Routine in ein Char Array schreiben, das später durch die Debug() Funktion an den PC übertragen werden soll.
In der Funktion gebe ich auch die Zählvariable ISRCounter aus, die in der Interrupt Routine hochgezählt werden soll.
Außerdem Toggle ich in der ISR das untere Nibble von PORTB, dadurch sehe ich an den LEDs, dass die ISR auch ausgeführt wird.
Das Problem ist, dass der ISRCounter immer seinen Ursprungswert behält, in dem Fall 0, ich habe es aber auch mal oben mit 1 definiert, auch dabei bleibt der Wert 1.
Ich kann mir das einfach nicht erklären, der Wert ist auch volatil und global, aber er ändert sich einfach nicht.
Hier das Hauptprogramm:

#include "Dualshock.h"


volatile char USARTdata[20];
volatile uint8_t ISRCounter = 0;


void DebugOut()
{
    while(!(USARTD0.STATUS & USART_DREIF_bm));
    USARTD0.DATA = ISRCounter;
    for(uint8_t i = 0; ((USARTdata[i - 1] != 13) && (i < 20));i++)
    {
        if(USARTdata[i] != 0x00)
        {
            while(!(USARTD0.STATUS & USART_DREIF_bm));
            USARTD0.DATA = i;
            
            while(!(USARTD0.STATUS & USART_DREIF_bm));
            USARTD0.DATA = USARTdata[i];
        }
        
    }


}




int main(void)
{
    Init::Clock_Init();
    Init::USART_Init();
    PMIC.CTRL |= PMIC_HILVLEN_bm; //High Level Interrupts freigeben
    sei();
    
    PORTB.DIR = 0xFF;
    PORTB.OUT = 0x00;
    Dualshock::init();
    while (!(Dualshock::SetAnalogMode() == 0x73)); //Versuchen in Analog Mode zu wechseln, bis es funktioniert        
    
    _delay_ms(1000); //Funkmodul in Command Mode
    while(!(USARTC0.STATUS & USART_DREIF_bm));
    USARTC0.DATA = '+';
    while(!(USARTC0.STATUS & USART_DREIF_bm));
    USARTC0.DATA = '+';
    while(!(USARTC0.STATUS & USART_DREIF_bm));
    USARTC0.DATA = '+';
    _delay_ms(1000);
    
    DebugOut();
    while(1)
    {
    Dualshock::getBytes();
    //while (!( USARTD0.STATUS & USART_DREIF_bm));
    //USARTD0.DATA = Dualshock::RightY;
    //while (!( USARTD0.STATUS & USART_DREIF_bm));
    //USARTD0.DATA = Dualshock::RightX;
    //PORTB.OUT = (Dualshock::RightX & 0x0F);
    
    }
}


ISR (USARTC0_RXC_vect)
{
    USARTdata[ISRCounter] = USARTC0.DATA;
    PORTB.OUTTGL = 0x0F;
    //if(USARTdata[ISRCounter] == 13)
    //ISRCounter = 0;
    //else
    ISRCounter ++;
}

Kann sich das jemand erklären?

Ich habe mal darauf verzichtet, die Klassen zu Posten, die Funktionen sollten soweit selbsterklärend sein.
Falls jemand Interesse hat kann ich natürlich die Dualshock Klasse hochladen.

Edit:
Habe das Problem jetzt gelöst bekommen.
Problem war einfach nur, dass das Xbee etwas mehr als eine Sekunde braucht um nach dem empfangen der "+++" Zeichenfolge das OK zurückzuschicken.
Deshalb wurde zu dem Zeitpunkt, wo die Debug Funktion aufgerufen wird noch kein einziges mal die ISR ausgeführt.
Echt mies, dass ich erst jetzt drauf gekommen bin, naja egal, jetzt läuft alles bestens.
Hoffe, dass der Kram aus China bald ankommt, langsam wird es langweilig mit der Fernbedienung.

mfg
Olaf

[crabtack]

Morgen!

Ich habe wieder ein Problem.
Der Copter ist jetzt Hardwaremäßig fertig, Funk funktioniert auch fehlerfrei, genauso wie die Datenfusion von Gyroskop und Beschleunigungssensor.
Ansteuerung der Motoren läuft auch.
Minimalgas bei 780µS und Maximum bei 1780µS.

Ich habe erstmal eine einfache P-Reglung geschrieben.
Jetzt habe ich das Problem, dass wenn ich die Motoren Ausgeschaltet habe und das Signal an einem der Motoren messe, verhält es sich richtig.
Ist der Copter zu weit nach hinten geneigt wird das Signal länger, ist er nach vorne geneigt kürzer.

Problem ist jetzt, wenn ich die Motoren einschalte und sie Laufen, dann funktioniert es kurz, aber irgendwann bleibt das Signal beim Maximalwert (1780µS) hängen und schwankt nur noch leicht.
Woran kann das liegen?
Ich vermute eine Störung des MPU-6000 (Gyro + ACC) durch die Motoren.
Der MPU-6000 befindet sich unter der Hauptplatine und ist mit einer etwa 10cm langen Flachbandleitung angeschlossen. (Oben Mittig im Bild zu sehen, zwischen Xbee und Spannungsregler)
Könnte es sinn machen, eine Abgeschirmte Leitung zu verwenden?
Ich hätte nur 2 Adrige Leitung da (1Ader + Abschirmung), würde es funktionieren, den Sensor mit insgesamt 5 Leitungen anzuschließen? (SCK, MISO, MOSI, CS, VCC und Masse als Abschirmung)?

Oder Kann es vielleicht noch an etwas ganz anderem liegen?

mfg
Olaf

[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