Arduino Quadrotor UAV Flugsteuerung Trägheitsnavigationscode mpu9250, einschließlich Gyroskop- und Beschleunigungsmesserkopplung
#include <Wire.h>
#include <SPI.h>
#include "I2Cdev.h" // Kommunikationsbibliothek
#include "MPU6050.h" // Gyro-Beschleunigungsmodulbibliothek
// # include "IMU.h"
#include <math.h> // 数学 计算 库
#define sampleFreq 33.30f // Abtastfrequenz in Hz
#define twoKpDef (2.0f * 0.5f) // 2 * proportionale Verstärkung
#define twoKiDef (2.0f * 0.0f) // 2 * Integralverstärkung
#define betaDef 0.1f
MPU6050 accelgyro;
flüchtiger Schwimmer twoKp = twoKpDef; // 2 * Proportionalverstärkung (Kp) // KP, KI 姿态 纠正 的 快慢
flüchtiger Schwimmer twoKi = twoKiDef; // 2 *
flüchtiger Float mit integraler Verstärkung (Ki) beta = betaDef;
flüchtiger Schwimmer q0 = 1,0f, q1 = 0,0f, q2 = 0,0f, q3 = 0,0f; // Quaternion des Sensorrahmens relativ zum
flüchtigen Float-Integral des Hilfsrahmens IntegralFBx = 0,0f, IntegralFBy = 0,0f, IntegralFBz = 0,0f; // Integrale Fehlerterme, skaliert mit Ki
int16_t ax, ay, az; // Messung hinzufügen
int16_t gx, gy, gz; // Gewichtskraft
int16_t mx, my, mz; // geomagnetische Größe
int k;
// # definiere GpsSerial Serial1
#define DebugSerial Serial
Schwimmerrolle;
Float Pitch;
Float Yaw;
float yaw1;
Float Pitchoffset, Rolloffset, Yawoffset; // Zwischenvariable für die Euler-Winkelkorrektur
void setup ()
{ Wire.begin (); // Gerät initialisieren DebugSerial.println ("I2C-Geräte initialisieren ..."); accelgyro.initialize (); }}
void loop () // Hauptschleife
{ / * wenn (k <1) // Einschaltkalibrierung mpu9150 / 9250-Funktion, vorübergehend nicht öffnen { CalibrateToZero (); // mpu9150 / 9250-Kalibrierungsfunktion aufrufen k = k + 1 ; } * / accelgyro.getMotion9 (& ax, & ay, & az, & gx, & gy, & gz, & mx, & my, & mz);
gx = gx * 3,1415926f / 131,00f / 180,00f;
gy = gy * 3,1415926f / 131,00f / 180,00f;
gz = gz * 3,1415926f / 131,00f / 180,00f;
IMUupdate (float gx, float gy, float gz, float ax, float ay, float az) // Quaternionsdifferentialberechnung aufrufen, Fusion hinzufügen
DCAHRSupdateIMU (float mx, float my, float mz) // Rufe die geomagnetische Kopplungsfunktion auf
/ * Pitch + = Pitchoffset; // 校正
roll + = rolloffset;
Gieren + = Gierenversatz; * /
//DebugSerial.print (""); // Berechnungswert für den Test der Ausgabe der seriellen Schnittstelle // DebugSerial.print (
q0);
//DebugSerial.print ("");
//DebugSerial.print(q1);
// DebugSerial. print ("");
//DebugSerial.print(q2);
//DebugSerial.print ("");
//DebugSerial.print(q3);
//DebugSerial.print ("");
DebugSerial.print (roll) ; // Euler-
Winkelrolle DebugSerial.print ("");
DebugSerial.print (Pitch); // Euler-Winkel-Pitch
DebugSerial.print ("");
DebugSerial.print (Gieren); // Euler-Winkel Gieren
DebugSerial .println ( "");
}}
void IMUupdate (float gx, float gy, float gz, float ax, float ay, float az) // Quaternionendifferentialberechnung, Additionsfusion
{ float receptNorm; float halfvx, halfvy, halfvz; float halfex, halfey, halfez; float qa, qb, qc;
// normalisiere die Messungen
wenn (! ((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f)))
{ receptNorm = invSqrt (ax * ax + ay * ay + az * az ); ax * = RecipNorm; Ay * = RecipNorm; Az * = RecipNorm; // Konvertiere den hinzugefügten dreidimensionalen Vektor in einen Einheitsvektor. halfvx = q1 * q3-q0 * q2; halfvy = q0 * q1 + q2 * q3; halfvz = q0 * q0-0,5f + q3 * q3;
// halfvz = q0 * q0-q1 * q1 + q3 * q3;
// vz = q0 * q0-q1 * q1-q2 * q2 + q3 * q3;
// Ist es q0 * q0-q1 * q1-q2 * q2 + q3 * q3
// Dies ist die Umwandlung einer Quaternion in die drei Elemente in der dritten Spalte der "Directional Cosine Matrix".
// Gemäß der Definition der Kosinusmatrix und der Euler-Winkel wird der Schwerkraftvektor des geografischen Koordinatensystems auf das Körperkoordinatensystem übertragen, das zufällig diese drei Elemente sind.
// Das vx \ y \ z ist hier also tatsächlich der Schwerkraft-Einheitsvektor, der aus dem aktuellen Euler-Winkel (Quaternion) des Körperkoordinaten-Referenzsystems konvertiert wird.
// Fehler ist die Summe des Kreuzprodukts zwischen der Referenzrichtung des Feldes und der Richtung, gemessen durch den Sensor
halfex = (ay * halfvz-az * halfvy);
halfey = (az * halfvx-ax * halfvz);
halfez = (ax * halfvy-ay) * halfvx);
// axyz ist der vom Beschleunigungsmesser auf dem Körperkoordinatenreferenzsystem gemessene Schwerkraftvektor, der der tatsächlich gemessene Schwerkraftvektor ist.
// axyz ist der durch Messung erhaltene Schwerkraftvektor, und vxyz ist der Schwerkraftvektor, der aus der Haltung des Kreisels nach der Integration abgeleitet wird. Sie sind alle Schwerkraftvektoren im Körperkoordinatenreferenzsystem.
// Der Fehlervektor zwischen ihnen ist der Fehler zwischen der vom Kreisel integrierten Haltung und der durch die Addition gemessenen Haltung.
// Der Fehler zwischen Vektoren kann durch ein Vektorkreuzprodukt (auch als Vektoraußenprodukt, Kreuzprodukt bezeichnet) dargestellt werden. Exyz ist das Kreuzprodukt zweier Schwerkraftvektoren.
// Dieser Kreuzproduktvektor befindet sich immer noch im Körperkoordinatensystem, und der Kreiselintegralfehler befindet sich auch im Körperkoordinatensystem, und die Größe des Kreuzprodukts ist proportional zum Kreiselintegralfehler, der zur Korrektur des Kreisels verwendet wird . (Sie können es sich selbst vorstellen.) Da der Kreisel den Körper direkt integriert, spiegelt sich der Korrekturbetrag des Kreisels direkt in der Korrektur des Koordinatensystems des Körpers wider.
if (twoKi> 0.0f) { IntegralFBx + = zweiKi * halfex * (1.0f / sampleFreq); // Integralfehler skaliert durch Ki IntegralFBy + = zweiKi * halfey * (1.0f / sampleFreq); IntegralFBz + = zweiKi * halfez * (1.0f / sampleFreq); gx + = IntegralFBx; // Integrale Rückkopplung anwenden gy + = IntegralFBy; gz + = IntegralFBz; } else { IntegralFBx = 0.0f; // Integralaufwicklung verhindern IntegralFBy = 0.0f; IntegralFBz = 0.0f; }}
// Proportionales Feedback anwenden
gx + = zweiKp * halfex;
gy + = zweiKp * halfey;
gz + = zweiKp * halfez;
}
// Produktübergreifenden Fehler verwenden, um PI-Korrektur-Kreiselvorspannung durchzuführen
// Änderungsrate der Quaternion
gx * = integrieren (0,5f * (1,0f / sampleFreq)); // gemeinsame Faktoren
vormultiplizieren gy * = (0,5f * (1,0f / sampleFreq));
gz * = (0,5f * (1,0f / sampleFreq));
qa = q0;
qb = q1;
qc = q2;
q0 + = (-qb * gx-qc * gy-q3 * gz);
q1 + = (qa * gx + qc * gz-q3 * gy);
q2 + = ( qa * gy-qb * gz + q3 * gx);
q3 + = (qa * gz + qb * gy-qc * gx);
// Quaternionsrezept
normalisierenNorm = invSqrt (q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 * = receptNorm;
q1 * = receptNorm;
q2 * = receptNorm;
q3 * = receptNorm;
Rolle = atan2 (2 * (* q2 + q3 Q0 * q1), (- 2 * q1 * Q1-2 * q2 * q2 + 1)) * 57,3; Eulerwinkel Rolle muß von 57,3 multipliziert werden , um Winkeltest zu konvertieren
Pitch = asin (2 * (q1 * q3-q0 * q2)) * 57,3; Die Euler-Winkelsteigung sollte mit 57,3 multipliziert werden, um sie in den Winkel
// yaw = - atan2 (2 * (q1 * q2 + q0 * q3), () umzuwandeln - 2 * q2 * q2-2 * q3 * q3 + 1)) * 57,3; Euler-Winkelgier sollte mit 57,3 multipliziert werden, um in Winkel
// temporärer Test-Haltepunkt
// q0 = ax;
// q1 = ay;
// umzuwandeln q2 = az;
}
void DCAHRSupdateIMU (float mx, float my, float mz) // Benutzerdefinierte geomagnetische Kopplungsfunktion
{ float norm; float hx, hy; // doppeltes Gieren1; // Gieren, erhalten mit dem Geomagnetometer #define Ki1 0.4f; hx = mx * cos ( Tonhöhe) + my * sin (Tonhöhe) * sin (Rolle) -mz * cos (Rolle) * sin (Tonhöhe); hy = meine * cos (Rolle) + mz * sin (Rolle); yaw = -atan2 (hy, hx) * 57,3;
// yaw = (yaw1-yaw) * Ki1 + yaw;
}}
/ * void CalibrateToZero (void) // Die Funktion zum Einschalten der Kalibrierung mpu9150 / 9250 wird vorübergehend nicht geöffnet.
{ u8 t = 0; float sumpitch = 0, sumroll = 0, sumyaw = 0; for (t = 0; t <150; t ++) { IMUupdate (float gx, float gy, float gz, float ax, float ay, float az) // Quaternionsdifferentialberechnung aufrufen, Fusion hinzufügen
DCAHRSupdateIMU (float mx, float my, float mz) // Rufe die
Verzögerung der geomagnetischen Kopplungsfunktion (20) auf;
if (t> = 100)
{ sumpitch + = Pitch; sumroll + = roll; sumyaw + = yaw; } ) Pitchoffset = -sumpitch / 50,0 f; Rolloffset = -sumroll / 50,0f; Yawoffset = -sumyaw / 50,0f; } * /