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:
Code:
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
Lesezeichen