bug修复与优化

g122622 · May 2, 2024 · d6c876d · d6c876d
1 parent 87bceb7
commit d6c876d
Show file tree

Hide file tree

Showing 9 changed files with 111 additions and 45 deletions.
diff --git a/main/FlightController/PID/PerformPID.h b/main/FlightController/PID/PerformPID.h
@@ -4,7 +4,7 @@
  * Created Date: 2024-03-26 13:45:15
  * Author: Guoyi
  * -----
- * Last Modified: 2024-03-26 14:00:54
+ * Last Modified: 2024-05-03 00:04:10
  * Modified By: Guoyi
  * -----
  * Copyright (c) 2024 Guoyi Inc.
@@ -16,18 +16,20 @@
 #define PERFORM_PID_H
 
 #include "./config/PIDConfig.h"
+#include "utils/MathUtils.h"
+#include "globalStates/controllerState.h"
 
 float performPID(PIDConfig *configIn, float err, float dt)
 {
     float P = configIn->P_Weigh * err;
 
     configIn->errIntegral += err * dt;
-    float I = configIn->I_Weigh * configIn->errIntegral;
+    float I = LIMIT(configIn->I_Weigh * configIn->errIntegral, PID_I_Limit);
 
     float D = configIn->D_Weigh * (err - configIn->lastErr) / dt;
     configIn->lastErr = err;
 
-    return P + I + D;
+    return LIMIT(P + I + D, PID_Total_Limit);
 }
 
 #endif
diff --git a/main/FlightController/PID/clearI.h b/main/FlightController/PID/clearI.h
@@ -0,0 +1,29 @@
+/*
+ * File: \clearI.h
+ * Project: PID
+ * Created Date: 2024-05-02 23:59:14
+ * Author: Guoyi
+ * -----
+ * Last Modified: 2024-05-03 00:07:11
+ * Modified By: Guoyi
+ * -----
+ * Copyright (c) 2024 Guoyi Inc.
+ * 
+ * ------------------------------------
+ */
+
+#ifndef CLEAR_I_H
+#define CLEAR_I_H
+
+#include "./config/gyroPID.h"
+#include "./config/headingPID.h"
+#include "./config/throttlePID.h"
+
+void clearI(){
+    gyroPIDConfig.errIntegral = 0;
+    pitchPIDConfig.errIntegral = 0;
+    rollPIDConfig.errIntegral = 0;
+    throttlePIDConfig.D_Weigh = 0;
+}
+
+#endif
diff --git a/main/FlightController/controllerTick.h b/main/FlightController/controllerTick.h
@@ -4,7 +4,7 @@
  * Created Date: 2024-03-29 22:57:12
  * Author: Guoyi
  * -----
- * Last Modified: 2024-04-29 23:03:26
+ * Last Modified: 2024-05-03 00:10:13
  * Modified By: Guoyi
  * -----
  * Copyright (c) 2024 Guoyi Inc.
@@ -16,13 +16,17 @@
 #include "globalStates/motionState.h"
 #include "globalStates/PWMState.h"
 #include "utils/F3D.h"
+#include "utils/MathUtils.h"
 
 #include "./PID/PerformPID.h"
 #include "./PID/config/gyroPID.h"
 #include "./PID/config/headingPID.h"
 #include "./PID/config/throttlePID.h"
 
 #include "FlightController/motor/motor.h"
+
+#define BOUNCE_TIME_MS 1000
+
 // #define PRINT_PWM_MODE
 
 uint32_t tickCount = 0;
@@ -36,13 +40,13 @@ void controllerTick(int dt, bool shouldDriveMotors)
     float expectedRoll = 0;
     float expectedAccelMagnitude = 0.01;
 
-    // 更新全局变量
+    // 更新全局变量 1
     GyroData = getGyroData();
     AccelData = getAccelData();
     /* 读取传感器姿态数据 */
     float realAccelMagnitude = getAccelMagnitude();
     F3D realRulerAngle = calcEulerAngle(AccelData, GyroData);
-    // 更新全局变量
+    // 更新全局变量 2
     EulerAngleData = realRulerAngle;
 
     /* 期望值和实际值作差 */
@@ -57,10 +61,14 @@ void controllerTick(int dt, bool shouldDriveMotors)
     float pitchPID = performPID(&pitchPIDConfig, pitchErr, dt);
 
     /* 将PID输出值转为电机PWM百分比 */
-    float PWM1 = PWM_Mult * (-rollPID - pitchPID) + PWM_Basic;
-    float PWM2 = PWM_Mult * (+rollPID - pitchPID) + PWM_Basic;
-    float PWM3 = PWM_Mult * (+rollPID + pitchPID) + PWM_Basic;
-    float PWM4 = PWM_Mult * (-rollPID + pitchPID) + PWM_Basic;
+    float PWM1 = PID_Mult * (-rollPID - pitchPID) + MIN(PWM_Basic, PWM_Basic / BOUNCE_TIME_MS * (tickCount * dt));
+    float PWM2 = PID_Mult * (+rollPID - pitchPID) + MIN(PWM_Basic, PWM_Basic / BOUNCE_TIME_MS * (tickCount * dt));
+    float PWM3 = PID_Mult * (+rollPID + pitchPID) + MIN(PWM_Basic, PWM_Basic / BOUNCE_TIME_MS * (tickCount * dt));
+    float PWM4 = PID_Mult * (-rollPID + pitchPID) + MIN(PWM_Basic, PWM_Basic / BOUNCE_TIME_MS * (tickCount * dt));
+    PWM1 *= PWM1_Mult;
+    PWM2 *= PWM2_Mult;
+    PWM3 *= PWM3_Mult;
+    PWM4 *= PWM4_Mult;
 #ifdef PRINT_PWM_MODE
     if ((tickCount % 100) == 0)
     {
@@ -86,7 +94,6 @@ void controllerTick(int dt, bool shouldDriveMotors)
     {
         stopAllMotors();
     }
-
 #endif
     tickCount++;
 }
diff --git a/main/bluetooth/services/remoteControll.h b/main/bluetooth/services/remoteControll.h
@@ -4,7 +4,7 @@
  * Created Date: 2024-03-03 23:03:17
  * Author: Guoyi
  * -----
- * Last Modified: 2024-04-28 19:34:46
+ * Last Modified: 2024-05-03 00:32:54
  * Modified By: Guoyi
  * -----
  * Copyright (c) 2024 Guoyi Inc.
@@ -29,14 +29,17 @@
 #include "FlightController/PID/config/headingPID.h"
 #include "FlightController/PID/config/throttlePID.h"
 
+#include "FlightController/PID/clearI.h"
+
 // ===========================遥控器服务 0xffe0================================== //
 
 /* 服务的uuid */
 static const ble_uuid16_t gatt_remoteControll_svc_uuid = BLE_UUID16_INIT(0xffe0);
 
 /* 服务内的characteristic */
 // 1.PID配置
-static uint32_t gatt_remoteControll_chr_PID_val[4 * 3]; // 4个PID配置，每个配置含3个float数据
+// 4个PID配置，每个配置含3个float数据，再加一个积分上限和PID总输出上限共2个float
+static uint32_t gatt_remoteControll_chr_PID_val[4 * 3 + 2];
 static uint16_t gatt_remoteControll_chr_PID_val_handle;
 static const ble_uuid16_t gatt_remoteControll_chr_PID_uuid = BLE_UUID16_INIT(0xffe1);
 // 2.飞行状态
@@ -73,7 +76,7 @@ void convertPIDtoBle()
 
 void convertBletoPID()
 {
-    union F1D gyroP, gyroI, gyroD, pitchP, pitchI, pitchD, rollP, rollI, rollD;
+    union F1D gyroP, gyroI, gyroD, pitchP, pitchI, pitchD, rollP, rollI, rollD, iLimit, totLimit;
 
     gyroP.i = gatt_remoteControll_chr_PID_val[0];
     gyroI.i = gatt_remoteControll_chr_PID_val[1];
@@ -84,6 +87,9 @@ void convertBletoPID()
     rollP.i = gatt_remoteControll_chr_PID_val[6];
     rollI.i = gatt_remoteControll_chr_PID_val[7];
     rollD.i = gatt_remoteControll_chr_PID_val[8];
+    // TODO index等于9,10,11的情况
+    iLimit.i = gatt_remoteControll_chr_PID_val[12];
+    totLimit.i = gatt_remoteControll_chr_PID_val[13];
 
     gyroPIDConfig.P_Weigh = gyroP.f;
     gyroPIDConfig.I_Weigh = gyroI.f;
@@ -94,6 +100,8 @@ void convertBletoPID()
     rollPIDConfig.P_Weigh = rollP.f;
     rollPIDConfig.I_Weigh = rollI.f;
     rollPIDConfig.D_Weigh = rollD.f;
+    PID_I_Limit = iLimit.f;
+    PID_Total_Limit = totLimit.f;
 }
 
 // 读写回调函数
@@ -123,10 +131,11 @@ static int gatt_remoteControll_svc_access(uint16_t conn_handle, uint16_t attr_ha
         }
         else if (attr_handle == gatt_remoteControll_chr_PWM_config_val_handle)
         {
+            // TODO 扩充pwm1~4的读写
             uint32_t data[2]; // 2 * float
             union F1D basic, mult;
             basic.f = PWM_Basic;
-            mult.f = PWM_Mult;
+            mult.f = PID_Mult;
             data[0] = basic.i;
             data[1] = mult.i;
             rc = os_mbuf_append(ctxt->om,
@@ -157,24 +166,37 @@ static int gatt_remoteControll_svc_access(uint16_t conn_handle, uint16_t attr_ha
                                 sizeof(flightState),
                                 sizeof(flightState),
                                 &flightState, NULL);
+            if (flightState == 1)
+            {
+                clearI();
+            }
             // Send notification (or indication) to any connected devices that
             // have subscribed for notification (or indication) for specified characteristic.
-            ble_gatts_chr_updated(attr_handle);
+            ble_gatts_chr_updated(attr_handle);    
             // MODLOG_DFLT(INFO, "Notification/Indication scheduled for all subscribed peers.\n");
             return rc;
         }
         else if (attr_handle == gatt_remoteControll_chr_PWM_config_val_handle)
         {
-            uint32_t data[2]; // 2 * float
+            uint32_t data[2 + 4]; // (2 + 4) * float
             rc = gatt_svr_write(ctxt->om,
                                 sizeof(data),
                                 sizeof(data),
                                 data, NULL);
-            union F1D basic, mult;
+            union F1D basic, mult, p1m, p2m, p3m, p4m;
             basic.i = data[0];
             mult.i = data[1];
+            p1m.i = data[2];
+            p2m.i = data[3];
+            p3m.i = data[4];
+            p4m.i = data[5];
+
             PWM_Basic = basic.f;
-            PWM_Mult = mult.f;
+            PID_Mult = mult.f;
+            PWM1_Mult = p1m.f;
+            PWM2_Mult = p2m.f;
+            PWM3_Mult = p3m.f;
+            PWM4_Mult = p4m.f;
 
             // Send notification (or indication) to any connected devices that
             // have subscribed for notification (or indication) for specified characteristic.

diff --git a/main/globalStates/PWMState.h b/main/globalStates/PWMState.h
@@ -4,19 +4,24 @@
  * Created Date: 2024-03-31 16:10:19
  * Author: Guoyi
  * -----
- * Last Modified: 2024-04-28 15:22:00
+ * Last Modified: 2024-05-02 16:53:30
  * Modified By: Guoyi
  * -----
  * Copyright (c) 2024 Guoyi Inc.
- * 
+ *
  * ------------------------------------
  */
 
 #ifndef PWM_STATE_H
 #define PWM_STATE_H
 
 float __attribute__((weak)) currentPWMPercentage[4] = {0.0f, 0.0f, 0.0f, 0.0f};
-float __attribute__((weak)) PWM_Mult = 0.5;
-float __attribute__((weak)) PWM_Basic = 70;
+float __attribute__((weak)) PID_Mult = 0.5;
+float __attribute__((weak)) PWM_Basic = 30;
+
+float __attribute__((weak)) PWM1_Mult = 1;
+float __attribute__((weak)) PWM2_Mult = 1;
+float __attribute__((weak)) PWM3_Mult = 1;
+float __attribute__((weak)) PWM4_Mult = 1;
 
 #endif
diff --git a/main/globalStates/controllerState.h b/main/globalStates/controllerState.h
@@ -4,7 +4,7 @@
  * Created Date: 2024-04-05 21:25:11
  * Author: Guoyi
  * -----
- * Last Modified: 2024-04-28 23:08:52
+ * Last Modified: 2024-05-03 00:03:06
  * Modified By: Guoyi
  * -----
  * Copyright (c) 2024 Guoyi Inc.
@@ -16,5 +16,7 @@
 #define CONTROLLER_STATE_H
 
 int __attribute__((weak)) flightState = 0;
+float __attribute__((weak)) PID_I_Limit = 1;
+float __attribute__((weak)) PID_Total_Limit = 5;
 
 #endif
diff --git a/main/mpu6050/algorithm/slidingFilter.h b/main/mpu6050/algorithm/slidingFilter.h
@@ -4,7 +4,7 @@
  * Created Date: 2024-04-29 21:10:08
  * Author: Guoyi
  * -----
- * Last Modified: 2024-04-29 22:54:36
+ * Last Modified: 2024-05-02 19:40:40
  * Modified By: Guoyi
  * -----
  * Copyright (c) 2024 Guoyi Inc.

diff --git a/main/mpu6050/motionData.h b/main/mpu6050/motionData.h
@@ -4,7 +4,7 @@
  * Created Date: 2024-03-07 22:51:03
  * Author: Guoyi
  * -----
- * Last Modified: 2024-04-30 00:31:36
+ * Last Modified: 2024-05-02 21:48:37
  * Modified By: Guoyi
  * -----
  * Copyright (c) 2024 Guoyi Inc.
@@ -22,10 +22,7 @@
 #include "freertos/task.h"
 #include "LED/StatusLED.h"
 #include "./algorithm/slidingFilter.h"
-
-#define MIN(X, Y) ((X) < (Y) ? (X) : (Y))
-#define MAX(X, Y) ((X) > (Y) ? (X) : (Y))
-#define LIMIT(X, Y) (MAX(MIN(X, Y), (-1) * Y)) // 将X的绝对值限制在Y以内
+#include "utils/MathUtils.h"
 
 #define ACCEL_RANGE 2
 #define GYRO_RANGE 250
@@ -42,12 +39,12 @@ static float q0 = 1, q1 = 0, q2 = 0, q3 = 0;  // 四元数的元素，代表估
 static float exInt = 0, eyInt = 0, ezInt = 0; // 按比例缩小积分误差
 struct
 {
-    int16_t ax;
-    int16_t ay;
-    int16_t az;
-    int16_t gx;
-    int16_t gy;
-    int16_t gz;
+    float ax;
+    float ay;
+    float az;
+    float gx;
+    float gy;
+    float gz;
 } CalibrationOffset;
 
 F3D getAccelData();
@@ -105,9 +102,9 @@ void MotionData_Calibrate()
 F3D getAccelData()
 {
     F3D originalVal = {
-        .x = MPU6050_Get_16bit_Data(ACCEL_XOUT_H) / (double)(65536 / 2 / ACCEL_RANGE) - CalibrationOffset.ax,
-        .y = MPU6050_Get_16bit_Data(ACCEL_YOUT_H) / (double)(65536 / 2 / ACCEL_RANGE) - CalibrationOffset.ay,
-        .z = MPU6050_Get_16bit_Data(ACCEL_ZOUT_H) / (double)(65536 / 2 / ACCEL_RANGE) - CalibrationOffset.az,
+        .x = MPU6050_Get_16bit_Data(ACCEL_XOUT_H) / (float)(65536 / 2 / ACCEL_RANGE) - CalibrationOffset.ax,
+        .y = MPU6050_Get_16bit_Data(ACCEL_YOUT_H) / (float)(65536 / 2 / ACCEL_RANGE) - CalibrationOffset.ay,
+        .z = MPU6050_Get_16bit_Data(ACCEL_ZOUT_H) / (float)(65536 / 2 / ACCEL_RANGE) - CalibrationOffset.az,
     };
     F3D ret = performSlidingFilter(originalVal);
     return ret;
@@ -126,9 +123,9 @@ F3D getGyroData()
 {
     // 限制角速度感知范围
     F3D ret = {
-        .x = LIMIT(MPU6050_Get_16bit_Data(GYRO_XOUT_H) / (double)(65536 / 2 / GYRO_RANGE) - CalibrationOffset.gx, 120),
-        .y = LIMIT(MPU6050_Get_16bit_Data(GYRO_YOUT_H) / (double)(65536 / 2 / GYRO_RANGE) - CalibrationOffset.gy, 120),
-        .z = LIMIT(MPU6050_Get_16bit_Data(GYRO_ZOUT_H) / (double)(65536 / 2 / GYRO_RANGE) - CalibrationOffset.gz, 120),
+        .x = LIMIT(MPU6050_Get_16bit_Data(GYRO_XOUT_H) / (float)(65536 / 2 / GYRO_RANGE) - CalibrationOffset.gx, 120),
+        .y = LIMIT(MPU6050_Get_16bit_Data(GYRO_YOUT_H) / (float)(65536 / 2 / GYRO_RANGE) - CalibrationOffset.gy, 120),
+        .z = LIMIT(MPU6050_Get_16bit_Data(GYRO_ZOUT_H) / (float)(65536 / 2 / GYRO_RANGE) - CalibrationOffset.gz, 120),
     };
     return ret;
 }
@@ -180,7 +177,7 @@ F3D calcEulerAngle(F3D accel, F3D gyro)
     q2 = q2 / norm;
     q3 = q3 / norm;
 
-    float Pitch = asin(-2 * q1 * q3 + 2 * q0 * q2) * 57.3;                                // pitch ,转换为度数
+    float Pitch = asin(-2 * q1 * q3 + 2 * q0 * q2) * 57.3;                                // pitch, 转换为度数
     float Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 57.3; // rollv
     // float Yaw = atan2(2 * (q1 * q2 + q0 * q3), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * 57.3;
     float Yaw = atan2(2 * (q1 * q2 + q0 * q3), 1 - 2 * (q2 * q2 + q3 * q3)) * 57.3;

diff --git a/main/utils/MathUtils.h b/main/utils/MathUtils.h
@@ -4,7 +4,7 @@
  * Created Date: 2024-03-16 14:24:32
  * Author: Guoyi
  * -----
- * Last Modified: 2024-03-16 14:28:08
+ * Last Modified: 2024-05-01 19:03:07
  * Modified By: Guoyi
  * -----
  * Copyright (c) 2024 Guoyi Inc.
@@ -15,6 +15,8 @@
 #ifndef MATHUTILS_H
 #define MATHUTILS_H
 
-#define min(a, b) ((a < b) ? a : b)
+#define MIN(X, Y) ((X) < (Y) ? (X) : (Y))
+#define MAX(X, Y) ((X) > (Y) ? (X) : (Y))
+#define LIMIT(X, Y) (MAX(MIN(X, Y), (-1) * Y)) // 将X的绝对值限制在Y以内
 
 #endif