飞控算法完善

g122622 · Apr 10, 2024 · fe861f8 · fe861f8
1 parent 423d2da
commit fe861f8
Show file tree

Hide file tree

Showing 8 changed files with 74 additions and 25 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -2,7 +2,7 @@
 # CMakeLists in this exact order for cmake to work correctly
 cmake_minimum_required(VERSION 3.16)
 
-list(APPEND EXTRA_COMPONENT_DIRS "$ENV{IDF_PATH}/examples/storage/sd_card/sdmmc/components/sd_card")
+# list(APPEND EXTRA_COMPONENT_DIRS "$ENV{IDF_PATH}/examples/storage/sd_card/sdmmc/components/sd_card")
 
 include($ENV{IDF_PATH}/tools/cmake/project.cmake)
 project(bleprph)
diff --git a/main/FlightController/PID/config/headingPID.h b/main/FlightController/PID/config/headingPID.h
@@ -4,7 +4,7 @@
  * Created Date: 2024-03-26 20:22:24
  * Author: Guoyi
  * -----
- * Last Modified: 2024-04-04 19:26:16
+ * Last Modified: 2024-04-05 21:16:59
  * Modified By: Guoyi
  * -----
  * Copyright (c) 2024 Guoyi Inc.
@@ -17,15 +17,15 @@
 PIDConfig pitchPIDConfig = {
     .P_Weigh = 1,
     .I_Weigh = 0,
-    .D_Weigh = 5,
+    .D_Weigh = 2.5,
     .lastErr = 0,
     .errIntegral = 0,
 };
 
 PIDConfig rollPIDConfig = {
-    .P_Weigh = 2,
+    .P_Weigh = 1,
     .I_Weigh = 0,
-    .D_Weigh = 10,
+    .D_Weigh = 2.5,
     .lastErr = 0,
     .errIntegral = 0,
 };
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-04 19:29:17
+ * Last Modified: 2024-04-07 22:27:48
  * Modified By: Guoyi
  * -----
  * Copyright (c) 2024 Guoyi Inc.
@@ -23,6 +23,8 @@
 
 #include "FlightController/motor/motor.h"
 
+uint32_t tickCount = 0;
+
 void controllerTick()
 {
     /* 从全局变量读取用户指令 */
@@ -51,10 +53,25 @@ void controllerTick()
     float pitchPID = performPID(&pitchPIDConfig, pitchErr, 5);
 
     /* 将PID输出值转为电机PWM百分比 */
-    float mult = 1.5;
-    float basic = 30;
-    setMotorPWMPercentage(0, mult * (-rollPID + pitchPID) + basic);
-    setMotorPWMPercentage(1, mult * (-rollPID - pitchPID) + basic);
-    setMotorPWMPercentage(2, mult * (+rollPID - pitchPID) + basic);
-    setMotorPWMPercentage(3, mult * (+rollPID + pitchPID) + basic);
+    float mult = 0.5;
+    float basic = 70;
+    if ((tickCount % 100) == 0)
+    {
+        printf("m%d, PWM: %f \t", 1, mult * (-rollPID + pitchPID) + basic);
+        printf("m%d, PWM: %f \t", 2, mult * (-rollPID - pitchPID) + basic);
+        printf("m%d, PWM: %f \t", 3, mult * (+rollPID - pitchPID) + basic);
+        printf("m%d, PWM: %f \n", 4, mult * (+rollPID + pitchPID) + basic);
+    }
+    // if (tickCount == 600)
+    // {
+    //     stopAllMotors();
+    //     esp_restart();
+    // }
+
+    tickCount++;
+    // setMotorPWMPercentage(0, mult * (-rollPID + pitchPID) + basic);
+    // setMotorPWMPercentage(1, mult * (-rollPID - pitchPID) + basic);
+    // setMotorPWMPercentage(2, mult * (+rollPID - pitchPID) + basic);
+    // setMotorPWMPercentage(3, mult * (+rollPID + pitchPID) + basic);
+
 }
diff --git a/main/FlightController/motor/motor.h b/main/FlightController/motor/motor.h
@@ -4,7 +4,7 @@
  * Created Date: 2024-03-27 16:27:48
  * Author: Guoyi
  * -----
- * Last Modified: 2024-04-04 17:30:02
+ * Last Modified: 2024-04-05 21:58:04
  * Modified By: Guoyi
  * -----
  * Copyright (c) 2024 Guoyi Inc.
@@ -18,7 +18,7 @@
 #include "../../PWMDriver/motorPWM.h"
 #include <globalStates/PWMState.h>
 
-#define DUTY_RANGE (4096 / 2)
+#define DUTY_RANGE (4096.0 / 1.5)
 
 void setMotorPWMPercentage(int motorNum, float percentage)
 {
@@ -35,4 +35,12 @@ void changeMotorPWMPercentage(int motorNum, float delta)
     setMotorPWMPercentage(motorNum, currentPWMPercentage[motorNum] + delta);
 }
 
+void stopAllMotors()
+{
+    setMotorPWMPercentage(0, 0);
+    setMotorPWMPercentage(1, 0);
+    setMotorPWMPercentage(2, 0);
+    setMotorPWMPercentage(3, 0);
+}
+
 #endif
diff --git a/main/globalStates/controllerState.h b/main/globalStates/controllerState.h
@@ -0,0 +1,21 @@
+/*
+ * File: \controllerState.h
+ * Project: globalStates
+ * Created Date: 2024-04-05 21:25:11
+ * Author: Guoyi
+ * -----
+ * Last Modified: 2024-04-10 22:58:27
+ * Modified By: Guoyi
+ * -----
+ * Copyright (c) 2024 Guoyi Inc.
+ * 
+ * ------------------------------------
+ */
+
+#ifndef CONTROLLER_STATE_H
+#define CONTROLLER_STATE_H
+
+int shouldControllerLoopRun = 0;
+
+
+#endif
diff --git a/main/main.c b/main/main.c
@@ -32,10 +32,11 @@
 
 #define tskHIGH_PRIORITY 10
 
+TaskHandle_t controllerTickLoopHandle = NULL;
+
 /* 启动任务 */
 void Tasks_Init()
 {
-    TaskHandle_t controllerTickLoopHandle = NULL;
     xTaskCreatePinnedToCore(controllerTickLoop, "controllerTickLoop",
                             4096, NULL, tskHIGH_PRIORITY, controllerTickLoopHandle, 1);
 }
@@ -51,6 +52,8 @@ void app_main(void)
     vTaskDelay(7000 / portTICK_PERIOD_MS);
     /* 启动所有任务 */
     Tasks_Init();
-    vTaskDelay(3000 / portTICK_PERIOD_MS);
-    esp_restart();
+
+    vTaskDelay(1000 / portTICK_PERIOD_MS);
+    // stopAllMotors();
+    // esp_restart();
 }
diff --git a/sdkconfig b/sdkconfig
@@ -1047,7 +1047,9 @@ CONFIG_ESP_CONSOLE_UART_DEFAULT=y
 CONFIG_ESP_CONSOLE_UART=y
 CONFIG_ESP_CONSOLE_UART_NUM=0
 CONFIG_ESP_CONSOLE_UART_BAUDRATE=115200
-# CONFIG_ESP_INT_WDT is not set
+CONFIG_ESP_INT_WDT=y
+CONFIG_ESP_INT_WDT_TIMEOUT_MS=1000
+CONFIG_ESP_INT_WDT_CHECK_CPU1=y
 CONFIG_ESP_TASK_WDT_EN=y
 CONFIG_ESP_TASK_WDT_INIT=y
 # CONFIG_ESP_TASK_WDT_PANIC is not set
@@ -2025,7 +2027,9 @@ CONFIG_CONSOLE_UART_DEFAULT=y
 CONFIG_CONSOLE_UART=y
 CONFIG_CONSOLE_UART_NUM=0
 CONFIG_CONSOLE_UART_BAUDRATE=115200
-# CONFIG_INT_WDT is not set
+CONFIG_INT_WDT=y
+CONFIG_INT_WDT_TIMEOUT_MS=1000
+CONFIG_INT_WDT_CHECK_CPU1=y
 CONFIG_TASK_WDT=y
 CONFIG_ESP_TASK_WDT=y
 # CONFIG_TASK_WDT_PANIC is not set

diff --git a/sdkconfig.old b/sdkconfig.old
@@ -1047,9 +1047,7 @@ CONFIG_ESP_CONSOLE_UART_DEFAULT=y
 CONFIG_ESP_CONSOLE_UART=y
 CONFIG_ESP_CONSOLE_UART_NUM=0
 CONFIG_ESP_CONSOLE_UART_BAUDRATE=115200
-CONFIG_ESP_INT_WDT=y
-CONFIG_ESP_INT_WDT_TIMEOUT_MS=2999
-CONFIG_ESP_INT_WDT_CHECK_CPU1=y
+# CONFIG_ESP_INT_WDT is not set
 CONFIG_ESP_TASK_WDT_EN=y
 CONFIG_ESP_TASK_WDT_INIT=y
 # CONFIG_ESP_TASK_WDT_PANIC is not set
@@ -2027,9 +2025,7 @@ CONFIG_CONSOLE_UART_DEFAULT=y
 CONFIG_CONSOLE_UART=y
 CONFIG_CONSOLE_UART_NUM=0
 CONFIG_CONSOLE_UART_BAUDRATE=115200
-CONFIG_INT_WDT=y
-CONFIG_INT_WDT_TIMEOUT_MS=2999
-CONFIG_INT_WDT_CHECK_CPU1=y
+# CONFIG_INT_WDT is not set
 CONFIG_TASK_WDT=y
 CONFIG_ESP_TASK_WDT=y
 # CONFIG_TASK_WDT_PANIC is not set