8.24姿态拟合代码实现

master
zrg 2021-08-24 23:35:12 +08:00
parent 153a57649c
commit c3b32954a5
6 changed files with 182 additions and 4 deletions

63
arduino/kalman/i2c.ino Normal file
View File

@ -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
}

111
arduino/kalman/kalman.ino Normal file
View File

@ -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);
}
}

View File

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

View File

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