8.24姿态拟合代码实现

arduino/kalman/i2c.ino

@@ -0,0 +1,63 @@
+/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.
+
+ This software may be distributed and modified under the terms of the GNU
+ General Public License version 2 (GPL2) as published by the Free Software
+ Foundation and appearing in the file GPL2.TXT included in the packaging of
+ this file. Please note that GPL2 Section 2[b] requires that all works based
+ on this software must also be made publicly available under the terms of
+ the GPL2 ("Copyleft").
+
+ Contact information
+ -------------------
+
+ Kristian Lauszus, TKJ Electronics
+ Web      :  http://www.tkjelectronics.com
+ e-mail   :  kristianl@tkjelectronics.com
+ */
+
+const uint8_t IMUAddress = 0x68; // AD0 is logic low on the PCB
+const uint16_t I2C_TIMEOUT = 1000; // Used to check for errors in I2C communication
+
+uint8_t i2cWrite(uint8_t registerAddress, uint8_t data, bool sendStop) {
+  return i2cWrite(registerAddress, &data, 1, sendStop); // Returns 0 on success
+}
+
+uint8_t i2cWrite(uint8_t registerAddress, uint8_t *data, uint8_t length, bool sendStop) {
+  Wire.beginTransmission(IMUAddress);
+  Wire.write(registerAddress);
+  Wire.write(data, length);
+  uint8_t rcode = Wire.endTransmission(sendStop); // Returns 0 on success
+  if (rcode) {
+    Serial.print(F("i2cWrite failed: "));
+    Serial.println(rcode);
+  }
+  return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission
+}
+
+uint8_t i2cRead(uint8_t registerAddress, uint8_t *data, uint8_t nbytes) {
+  uint32_t timeOutTimer;
+  Wire.beginTransmission(IMUAddress);
+  Wire.write(registerAddress);
+  uint8_t rcode = Wire.endTransmission(false); // Don't release the bus
+  if (rcode) {
+    Serial.print(F("i2cRead failed: "));
+    Serial.println(rcode);
+    return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission
+  }
+  Wire.requestFrom(IMUAddress, nbytes, (uint8_t)true); // Send a repeated start and then release the bus after reading
+  for (uint8_t i = 0; i < nbytes; i++) {
+    if (Wire.available())
+      data[i] = Wire.read();
+    else {
+      timeOutTimer = micros();
+      while (((micros() - timeOutTimer) < I2C_TIMEOUT) && !Wire.available());
+      if (Wire.available())
+        data[i] = Wire.read();
+      else {
+        Serial.println(F("i2cRead timeout"));
+        return 5; // This error value is not already taken by endTransmission
+      }
+    }
+  }
+  return 0; // Success
+}

arduino/kalman/kalman.ino

@@ -0,0 +1,111 @@
+#include <Wire.h>
+#include <Kalman.h> // Source: https://github.com/TKJElectronics/KalmanFilter
+#define gyroZ_OFF -0.72
+Kalman kalmanZ;
+/* ----IMU Data---- */
+double accX, accY, accZ;
+double gyroX, gyroY, gyroZ;
+int16_t tempRaw;
+
+double gyroZangle; // Angle calculate using the gyro only
+double compAngleZ; // Calculated angle using a complementary filter
+double kalAngleZ;  // Calculated angle using a Kalman filter
+
+uint32_t timer;
+uint8_t i2cData[14]; // Buffer for I2C data
+
+/* ----FOC Data---- */
+
+// driver instance
+double acc2rotation(double x, double y);
+void setup() {
+  Serial.begin(115200);
+  /* ----IMU init---- */
+  Wire.begin(19, 18,400000);// 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 = (int16_t)((i2cData[0] << 8) | i2cData[1]);
+  accY = (int16_t)((i2cData[2] << 8) | i2cData[3]);
+  accZ = (int16_t)((i2cData[4] << 8) | i2cData[5]);
+  double pitch = acc2rotation(accX, accY);
+
+  kalmanZ.setAngle(pitch);
+  gyroZangle = pitch;
+
+  timer = micros();
+  /* ----FOC init---- */
+}
+
+void loop() {
+  /*----IMU loop----*/
+  while (i2cRead(0x3B, i2cData, 14));
+    accX = (int16_t)((i2cData[0] << 8) | i2cData[1]);
+    accY = (int16_t)((i2cData[2] << 8) | i2cData[3]);
+    accZ = (int16_t)((i2cData[4] << 8) | i2cData[5]);
+    tempRaw = (int16_t)((i2cData[6] << 8) | i2cData[7]);
+    gyroX = (int16_t)((i2cData[8] << 8) | i2cData[9]);
+    gyroY = (int16_t)((i2cData[10] << 8) | i2cData[11]);
+    gyroZ = (int16_t)((i2cData[12] << 8) | i2cData[13]);
+    
+    double dt = (double)(micros() - timer) / 1000000; // Calculate delta time
+    timer = micros();
+    
+    double pitch = acc2rotation(accX, accY);
+    double gyroZrate = gyroZ / 131.0; // Convert to deg/s
+
+    kalAngleZ = kalmanZ.getAngle(pitch, gyroZrate + gyroZ_OFF, dt);
+    gyroZangle += (gyroZrate + gyroZ_OFF) * dt;
+    compAngleZ = 0.93 * (compAngleZ + (gyroZrate + gyroZ_OFF) * dt) + 0.07 * pitch;
+
+    // Reset the gyro angle when it has drifted too much
+    if (gyroZangle < -180 || gyroZangle > 180)
+      gyroZangle = kalAngleZ;
+
+#if 1 // Set to 1 to activate
+  Serial.print(pitch); Serial.print("\t");
+  Serial.print(gyroZangle); Serial.print("\t");
+  Serial.print(compAngleZ); Serial.print("\t");
+  Serial.print(kalAngleZ); Serial.print("\t");
+#endif
+Serial.print("\r\n");
+delay(2);
+}
+/* mpu6050加速度转换为角度
+            acc2rotation(ax, ay)
+            acc2rotation(az, ay) */
+double acc2rotation(double x, double y)
+{
+  if (y < 0)
+  {
+    return atan(x / y) / 1.570796 * 90 + 180;
+  }
+  else if (x < 0)
+  {
+    return (atan(x / y) / 1.570796 * 90 + 360);
+  }
+  else
+  {
+    return (atan(x / y) / 1.570796 * 90);
+  }
+}

arduino/udp_test1/udp_test1/udp_test1.ino

@@ -83,7 +83,7 @@ void loop()
 //  Serial.print(" | GyX = "); Serial.print(GyX);
 //  Serial.print(" | GyY = "); Serial.print(GyY);
 //  Serial.print(" | GyZ = "); Serial.println(GyZ);
-  delay(100);
+  delay(10);
 
 //  udp.broadcastTo(broadcastData, broadcastPort); //可以使用该方法广播信息
 

python_gui/gui/main.py

@@ -34,14 +34,14 @@ class MyWindow(QMainWindow, Ui_MainWindow):
         self.timer.start(50)
         # wifi udp
         self.udp = udp()
-        self.wifi_IP_lineEdit.setText(self.udp.user_ip)
+        # self.wifi_IP_lineEdit.setText(self.udp.user_ip)
     def variable_init(self):
         # 图表数据变量
         self.wifi_recv_flag = 0
         self.udp_data = 0
         self.target_velocity = 0
         self.now_velocity = 0
-
+        self.close_flag = 1
     def plot_init(self):
         # 图表可视化数组
         self.v_list = []
@@ -60,6 +60,7 @@ class MyWindow(QMainWindow, Ui_MainWindow):
 
     def wifi_config_pushButton_clicked(self):
         try:
+            print(self.wifi_IP_lineEdit.text(),type(self.wifi_IP_lineEdit.text()))
             self.udp.udpClientSocket.bind((self.wifi_IP_lineEdit.text(), 2333))
             t1 = threading.Thread(target=self.udp_recv)
             t1.start()
@@ -75,7 +76,7 @@ class MyWindow(QMainWindow, Ui_MainWindow):
             self.wifi_recv_flag = 0
             self.wifi_recv_open_pushButton.setText('打开')
     def udp_recv(self):
-        while self.wifi_recv_flag:
+        while self.close_flag:
             recv_data = self.udp.udpClientSocket.recv(1024)
             recv_data = recv_data.decode('utf-8')
             self.udp_data = recv_data
@@ -84,6 +85,9 @@ class MyWindow(QMainWindow, Ui_MainWindow):
             self.v_list[0] = np.roll(self.v_list[0], -1)
             self.v_list[0][-1] = self.udp_data
             self.curve.setData(self.timeArray, self.v_list[0])  # 在绘图部件中绘制折线图
+
+    def closeEvent(self, a0: QtGui.QCloseEvent) -> None:
+        self.close_flag = 0
 if __name__ == '__main__':
     app = QApplication(sys.argv)
     myWin = MyWindow()