Wie Gleichgewichtssensordaten richtig umrechnen (da Sensor um 45º gedreht)

Pram · February 16, 2014, 1:33pm

Hallo Forum.
Ich komme im Moment nicht darauf, wie ich die Daten von der MPU-6050 so umrechne, dass die Daten so gesehen um 45º gedreht sind, weil die X-Achse von dem Quadrocopter liegt nicht auf der X-Achse der MPU, sonder ist um 45º gedreht. Gleiches gilt für die Y-Achse.
Nun bleibt die Frage, wie bekomme ich nun die Kippbewegung und die entsprechende Gradzahl, die ich von der MPU bekomme so umgerechnet, dass ich weiß um wie viel Grad sich die um 45º verschobene Achse geneigt hat ?
Also wer sich jetzt noch nichts darunter vorstellen kann. Hier eine kleine Veranschaulichung:
Dies sind die Quadrocopter Achsen "X"
und dies die MPU Achsen "+"
Danke schon mal

Serenifly · February 16, 2014, 1:35pm

Wenn du nur die Koordinaten rotieren willst:

Pram · February 16, 2014, 1:38pm

Doc_Arduino · February 16, 2014, 2:18pm

Hallo,

bestimmt verstehe ich Dich auch falsch. Jedoch müßtest Du laut meiner Auffassung auch "2 Achsen mitteln" um auf Deine gewünschte zu schließen, die ja zwischen 2 Achsen liegt. Eben wegen dem 45° Versatz. Kannst Du wirklich nicht den Sensor wie gewünscht ohne 45° Versatz montieren.

Pram · February 16, 2014, 2:22pm

Leider kann ich den nicht mehr drehen, deswegen muss ich mich da jetzt durchprügeln, weil ich sonst ne neue Paltine machen muss XD

Rabenauge · February 16, 2014, 3:16pm

Also die paar Beinchen da aus-und anders wieder einlöten ist wesentlich einfacher als die Software anpassen.
Das Ding hat nunmal konstruktiv festgelegt, drei Achsen, und die zeigen in bestimmte Richtungen, da kannst auch du nix dran ändern.

Wenn du genug von verstehst: die Formeln, aus denen der Kalman-Filter menschenverständliche Werte errechnet, stehn im Quelltext-die müsstest du halt anpassen.

Doc_Arduino · February 16, 2014, 3:46pm

Hallo,

wenn ich mir das Bild so betrachte, wäre es wirklich einfacher eine neue Befestigungsplatte zurechtzusägen wo der Arduino samt Aufbau befestigt ist. Vielleicht ist es sogar ausreichend nur neue Löcher in die weiße Trägerplatte zu bohren um 45° versetzt und den Arduino samt Aufbau wieder anzuschrauben.

Pram · February 16, 2014, 4:54pm

Ok dann werde ich wohl doch lieber einfach umlöten oder bohren
Wenn das so schwer ist umzusetzen.

Doc_Arduino · February 16, 2014, 5:36pm

Hallo,

Du wolltest je Achse in Kippbewegung bestimmt nur einen Motor gezielt ansteuern.
Aktuell hast Du auf jeder Sensorachse genau 2 Motor links/rechts liegen.
Wäre es denkbar immer diese beiden Motoren gemeinsam anzusteuern?
Ich habe zwar von solchen coolen Coptern keine Ahnung, aber man muß bestimmt eh immer alle Motoren jederzeit verschieden ansteuern für eine stabile Fluglage um Schwankungen auszugleichen.
Wenn Du aktuell vorn höher willst, müssen beide Frontmotoren schneller drehen statt nur einer.
So stelle ich mir das jedenfalls vor. Für die Vorstellungskraft der Ansteuerung kann ich mir vorstellen, ist die 45° Änderung jedoch bestimmt erstmal einfacher.

volvodani · February 16, 2014, 5:55pm

Wie soll der den nun fliegen. Als x oder als +. Selbst bei großen FCs muss man die Unit drehen um in die richtige Richtung der Sensor zu haben sonst könnte man ja einfach nen Hacken setzten und dann hätte Man die richtige Konfig.
Gruß
DerDani

Rabenauge · February 16, 2014, 6:38pm

@Doc: klar kann man die Motoren so ansteuern, wie du sagst.
Das wird eh dann je nach Notwendigkeit gemacht.
Das Problem ist, dass man mit den Rohdaten des Sensors so ohne weiteres nix anfangen kann-ich spiele ja selber mit so nem Teil rum. Also nimmt man beispielsweise die kalman.h-Lib, und die gibt schön praktisch die absoluten Winkelgrade aus. Damit kann man dann weitermachen, nur: diese Winkel sind eben auf die absoluten Achsen des Moduls bezogen-wenn du das Ding drehst, kommt was anderes bei raus.
Jemand, ders richtig drauf hat, kann sicher die lib entsprechend umschreiben, aber wenn ichs recht kapiert habe, werden aus den Daten zweier Achsen die Werte für die dritte Achse errechnet- ne neue Lib zu schreiben, dürfte also auch nicht aufwendiger werden.
Da ists dann doch sinnvoller, einfach das Modul richtig einzubauen und gut- ist doch in 10 min erledigt.

Doc_Arduino · February 16, 2014, 7:07pm

Hallo,

aha Hab zwar keine Ahnung, kann mir das aber so in ca.

Pram · February 16, 2014, 7:55pm

volvodani:
Wie soll der den nun fliegen. Als x oder als +.

Also der soll als X-Konfi fliegen, wegen meiner Kamera, also ist das so doch besser, wenn der Sensor um 45º gedreht ist.

Dazu fällt mir gerade ein, dass es doch egal ist, ob man mit den Daten von einer Achse 2 Motoren oder 4 ansteuert. Es kommt auf's selbe hinaus, weil ob ich jetzt jeweils einen Motor und den gegenüberliegenden ansteuer oder 2 und deren gegenüberliegenden ist ja egal, weil ausbalancieren wird er sich trotzdem

uwefed · February 16, 2014, 8:35pm

Ich denke nicht.
Die Steuerung geht von einem mechanischen Montage der Beschleunigungssensor bezüglich der Motore aus. Wenn Du die beiden gegeneinander verdrehst dann stimmt die Steuerung nicht mehr.
Um die Beschleunigungs-werte um 45 Grad zu drehen braucht es Vektorumrechnungen mit Winkelfunktionen und wurzeln. Das belastet den Controller zu stark daß die Steuerung noch funktionieren würde.
Viele Grüße Uwe

Pram · February 16, 2014, 8:44pm

Ich muss die Daten ja nicht um 45º drehen und das kann ich auch nicht tun, weil der Quadrocopter ja sonst nicht waagerecht bleibt, weil dann die Werte verfälscht sind oder nicht ?

Pram · February 16, 2014, 9:04pm

Also das ist mein Code bislang

#include <PIDCont.h>
#include <Wire.h>
#include "Kalman.h"
#include <Servo.h>

int arm = 15;

PIDCont PIDroll;

#define ROLL_PID_KP  1
#define ROLL_PID_KI  0.1
#define ROLL_PID_KD  0
#define ROLL_PID_MIN  -50.0
#define ROLL_PID_MAX  50.0
#define Throttle 40

Servo motors[4];
#define FRONTL 0
#define FRONTR 1
#define BACKL  2
#define BACKR  3

/* IMU Data */
int16_t accX, accY, accZ;
int16_t tempRaw;
int16_t gyroX, gyroY, gyroZ;
uint32_t timer;
uint8_t i2cData[14]; // Buffer for I2C data

double accXangle, accYangle; // Angle calculate using the accelerometer
double temp; // Temperature
double gyroXangle, gyroYangle; // Angle calculate using the gyro
double kalAngleX, kalAngleY; // Calculate the angle using a Kalman filter
Kalman kalmanX; // Create the Kalman instances
Kalman kalmanY;

void setup(){
  Serial.begin(9600);
  Wire.begin();
  TWBR = ((F_CPU / 400000L) - 16) / 2; // Set I2C frequency to 400kHz

  i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
  i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
  i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
  i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
  while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once
  while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode

  while (i2cRead(0x75, i2cData, 1));
  if (i2cData[0] != 0x68) { // Read "WHO_AM_I" register
    Serial.print(F("Error reading sensor"));
    while (1);
  }

  delay(100); // Wait for sensor to stabilize

  /* Set kalman and gyro starting angle */
  while (i2cRead(0x3B, i2cData, 6));
  accX = ((i2cData[0] << 8) | i2cData[1]);
  accY = ((i2cData[2] << 8) | i2cData[3]);
  accZ = ((i2cData[4] << 8) | i2cData[5]);
  // atan2 outputs the value of -? to ? (radians) - see http://en.wikipedia.org/wiki/Atan2
  // We then convert it to 0 to 2? and then from radians to degrees
  accYangle = (atan2(accX, accZ) + PI) * RAD_TO_DEG;
  accXangle = (atan2(accY, accZ) + PI) * RAD_TO_DEG;

  kalmanX.setAngle(accXangle); // Set starting angle
  kalmanY.setAngle(accYangle);
  gyroXangle = accXangle;
  gyroYangle = accYangle;

  timer = micros();
  
  
  
  
  
    //                          Kp,        Ki,         Kd           Lval         Hval
  PIDroll.ChangeParameters(ROLL_PID_KP,ROLL_PID_KI,ROLL_PID_KD,ROLL_PID_MIN,ROLL_PID_MAX);
  
  
  motors[FRONTL].attach(2);
  motors[FRONTR].attach(3);
  motors[BACKL].attach(4);
  motors[BACKR].attach(5);
  setSpeed(FRONTL, arm);
  setSpeed(FRONTR, arm);
  setSpeed(BACKL, arm);
  setSpeed(BACKR, arm);
  delay(3000);
}

void loop(){
  
  while (i2cRead(0x3B, i2cData, 14));
  accX = ((i2cData[0] << 8) | i2cData[1]);
  accY = ((i2cData[2] << 8) | i2cData[3]);
  accZ = ((i2cData[4] << 8) | i2cData[5]);
  tempRaw = ((i2cData[6] << 8) | i2cData[7]);
  gyroX = ((i2cData[8] << 8) | i2cData[9]);
  gyroY = ((i2cData[10] << 8) | i2cData[11]);
  gyroZ = ((i2cData[12] << 8) | i2cData[13]);
  
  accXangle = (atan2(accY, accZ) + PI) * RAD_TO_DEG;
  accYangle = (atan2(accX, accZ) + PI) * RAD_TO_DEG;
  
  double gyroXrate = (double)gyroX / 131.0;
  double gyroYrate = -((double)gyroY / 131.0);
  
  
  kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros() - timer) / 1000000);
  timer = micros();
  
  
  int PIDroll_val= (int)PIDroll.Compute((float)179.5-kalAngleY);
  
  
  
  int m0_val=(Throttle+PIDroll_val);
  int m1_val=(Throttle-PIDroll_val);
  
  setSpeed(FRONTL, m0_val);
  setSpeed(BACKR, m1_val);
  
  Serial.print(m0_val);Serial.print("     ");Serial.println(m1_val);
}


void setSpeed(int motor, int speed)
{
  int fakeAngle = map(speed, 0, 100, 0, 180);
  motors[motor].write(fakeAngle);
}

Aber den könnte ich doch einfach zu dem ändern

#include <PIDCont.h>
#include <Wire.h>
#include "Kalman.h"
#include <Servo.h>

int arm = 15;

PIDCont PIDroll, PIDpitch;


#define ROLL_PID_KP  1
#define ROLL_PID_KI  0.1
#define ROLL_PID_KD  0
#define ROLL_PID_MIN  -50.0
#define ROLL_PID_MAX  50.0
#define Throttle 40

Servo motors[4];
#define FRONTL 0
#define FRONTR 1
#define BACKL  2
#define BACKR  3

/* IMU Data */
int16_t accX, accY, accZ;
int16_t tempRaw;
int16_t gyroX, gyroY, gyroZ;
uint32_t timer;
uint8_t i2cData[14]; // Buffer for I2C data

double accXangle, accYangle; // Angle calculate using the accelerometer
double temp; // Temperature
double gyroXangle, gyroYangle; // Angle calculate using the gyro
double kalAngleX, kalAngleY; // Calculate the angle using a Kalman filter
Kalman kalmanX; // Create the Kalman instances
Kalman kalmanY;

void setup(){
  Serial.begin(9600);
  Wire.begin();
  TWBR = ((F_CPU / 400000L) - 16) / 2; // Set I2C frequency to 400kHz

  i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
  i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
  i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
  i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
  while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once
  while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode

  while (i2cRead(0x75, i2cData, 1));
  if (i2cData[0] != 0x68) { // Read "WHO_AM_I" register
    Serial.print(F("Error reading sensor"));
    while (1);
  }

  delay(100); // Wait for sensor to stabilize

  /* Set kalman and gyro starting angle */
  while (i2cRead(0x3B, i2cData, 6));
  accX = ((i2cData[0] << 8) | i2cData[1]);
  accY = ((i2cData[2] << 8) | i2cData[3]);
  accZ = ((i2cData[4] << 8) | i2cData[5]);
  // atan2 outputs the value of -? to ? (radians) - see http://en.wikipedia.org/wiki/Atan2
  // We then convert it to 0 to 2? and then from radians to degrees
  accYangle = (atan2(accX, accZ) + PI) * RAD_TO_DEG;
  accXangle = (atan2(accY, accZ) + PI) * RAD_TO_DEG;

  kalmanX.setAngle(accXangle); // Set starting angle
  kalmanY.setAngle(accYangle);
  gyroXangle = accXangle;
  gyroYangle = accYangle;

  timer = micros();
  
  
  
  
  
    //                          Kp,        Ki,         Kd           Lval         Hval
  PIDroll.ChangeParameters(ROLL_PID_KP,ROLL_PID_KI,ROLL_PID_KD,ROLL_PID_MIN,ROLL_PID_MAX);
  
  
  motors[FRONTL].attach(2);
  motors[FRONTR].attach(3);
  motors[BACKL].attach(4);
  motors[BACKR].attach(5);
  setSpeed(FRONTL, arm);
  setSpeed(FRONTR, arm);
  setSpeed(BACKL, arm);
  setSpeed(BACKR, arm);
  delay(3000);
}

void loop(){
  
  while (i2cRead(0x3B, i2cData, 14));
  accX = ((i2cData[0] << 8) | i2cData[1]);
  accY = ((i2cData[2] << 8) | i2cData[3]);
  accZ = ((i2cData[4] << 8) | i2cData[5]);
  tempRaw = ((i2cData[6] << 8) | i2cData[7]);
  gyroX = ((i2cData[8] << 8) | i2cData[9]);
  gyroY = ((i2cData[10] << 8) | i2cData[11]);
  gyroZ = ((i2cData[12] << 8) | i2cData[13]);
  
  accXangle = (atan2(accY, accZ) + PI) * RAD_TO_DEG;
  accYangle = (atan2(accX, accZ) + PI) * RAD_TO_DEG;
  
  double gyroXrate = (double)gyroX / 131.0;
  double gyroYrate = -((double)gyroY / 131.0);
  
  
  kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros() - timer) / 1000000);
  kalAngleX = kalmanX.getAngle(accXangle, gyroXrate, (double)(micros() - timer) / 1000000);
  timer = micros();
  
  
  int PIDroll_val= (int)PIDroll.Compute((float)179.5-kalAngleY);
  int PIDpitch_val= (int)PIDpitch.Compute((float)179.5-kalAngleX);
  
  
  int m0_val=(Throttle+(PIDroll_val+PIDpitch_val));
  int m1_val=(Throttle-(PIDroll_val+PIDpitch_val));
  
  setSpeed(FRONTL, m0_val);
  setSpeed(BACKR, m1_val);
  
  Serial.print(m0_val);Serial.print("     ");Serial.println(m1_val);
}


void setSpeed(int motor, int speed)
{
  int fakeAngle = map(speed, 0, 100, 0, 180);
  motors[motor].write(fakeAngle);
}

Topic		Replies	Views
MPU6050-aus dem Beschleunigungssensor die Geschwindigkeit berechnen Deutsch	3	1994	May 6, 2021
Arduino MPU 6050 Deutsch	1	663	May 6, 2021
MPU6050 - Schräglage Deutsch	11	2405	May 6, 2021
Schwingung mit MPU-6050 darstellen Deutsch	5	2189	May 6, 2021
MPU6050 (G-Y521) gibt vertauschte Werte aus Deutsch	24	3933	May 6, 2021

Wie Gleichgewichtssensordaten richtig umrechnen (da Sensor um 45º gedreht)

Related Topics