1.mult和basic可配置

2.提高校准时长，优化校准时机，校准时亮灯

main/FlightController/controllerTick.h

@@ -4,7 +4,7 @@
  * Created Date: 2024-03-29 22:57:12
  * Author: Guoyi
  * -----
- * Last Modified: 2024-04-27 22:26:37
+ * Last Modified: 2024-04-28 15:22:56
  * Modified By: Guoyi
  * -----
  * Copyright (c) 2024 Guoyi Inc.
@@ -14,6 +14,7 @@
 
 #include "mpu6050/motionData.h"
 #include "globalStates/motionState.h"
+#include "globalStates/PWMState.h"
 #include "utils/F3D.h"
 
 #include "./PID/PerformPID.h"
@@ -22,6 +23,7 @@
 #include "./PID/config/throttlePID.h"
 
 #include "FlightController/motor/motor.h"
+// #define PRINT_PWM_MODE
 
 uint32_t tickCount = 0;
 
@@ -55,25 +57,30 @@ void controllerTick(int dt)
     float pitchPID = performPID(&pitchPIDConfig, pitchErr, dt);
 
     /* 将PID输出值转为电机PWM百分比 */
-    float mult = 0.5;
-    float basic = 30;
+    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;
+#ifdef PRINT_PWM_MODE
     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);
+
+        printf("m%d, PWM: %f \t", 1, PWM1);
+        printf("m%d, PWM: %f \t", 2, PWM2);
+        printf("m%d, PWM: %f \t", 3, PWM3);
+        printf("m%d, PWM: %f \n", 4, PWM4);
     }
-    // if (tickCount == 300)
+#endif
+#ifndef PRINT_PWM_MODE
+    // if (tickCount * dt >= 3000)
     // {
     //     stopAllMotors();
     //     esp_restart();
     // }
-
-    setMotorPWMPercentage(0, mult * (-rollPID + pitchPID) + basic);
-    setMotorPWMPercentage(1, mult * (-rollPID - pitchPID) + basic);
-    setMotorPWMPercentage(2, mult * (+rollPID - pitchPID) + basic);
-    setMotorPWMPercentage(3, mult * (+rollPID + pitchPID) + basic);
-
+    setMotorPWMPercentage(0, PWM1);
+    setMotorPWMPercentage(1, PWM2);
+    setMotorPWMPercentage(2, PWM3);
+    setMotorPWMPercentage(3, PWM4);
+#endif
     tickCount++;
 }

main/LED/StatusLED.h

@@ -0,0 +1,40 @@
+/*
+ * File: \flashStatusLED.h
+ * Project: LED
+ * Created Date: 2024-04-28 15:07:32
+ * Author: Guoyi
+ * -----
+ * Last Modified: 2024-04-28 15:19:25
+ * Modified By: Guoyi
+ * -----
+ * Copyright (c) 2024 Guoyi Inc.
+ *
+ * ------------------------------------
+ */
+
+#ifndef STATUS_LED_H
+#define STATUS_LED_H
+
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+#include "driver/gpio.h"
+
+static gpio_num_t gpio_led_num = GPIO_NUM_32; // 连接LED的GPIO
+
+void StatusLED_Init()
+{
+    // 设置控制LED的GPIO为输出模式
+    gpio_set_direction(gpio_led_num, GPIO_MODE_OUTPUT);
+}
+
+void enableStatusLED()
+{
+    gpio_set_level(gpio_led_num, 1);
+}
+
+void disableStatusLED()
+{
+    gpio_set_level(gpio_led_num, 0);
+}
+
+#endif

main/bluetooth/gatt_server.c

@@ -79,6 +79,18 @@ static const struct ble_gatt_svc_def gatt_svr_svcs[] = {
                 .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_WRITE | BLE_GATT_CHR_F_NOTIFY | BLE_GATT_CHR_F_WRITE_NO_RSP,
                 .val_handle = &gatt_remoteControll_chr_PID_val_handle,
             },
+            {
+                .uuid = &gatt_remoteControll_chr_flight_state_uuid.u,
+                .access_cb = gatt_remoteControll_svc_access,
+                .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_WRITE | BLE_GATT_CHR_F_NOTIFY | BLE_GATT_CHR_F_WRITE_NO_RSP,
+                .val_handle = &gatt_remoteControll_chr_flight_state_val_handle,
+            },
+            {
+                .uuid = &gatt_remoteControll_chr_PWM_config_uuid.u,
+                .access_cb = gatt_remoteControll_svc_access,
+                .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_WRITE | BLE_GATT_CHR_F_NOTIFY | BLE_GATT_CHR_F_WRITE_NO_RSP,
+                .val_handle = &gatt_remoteControll_chr_PWM_config_val_handle,
+            },
             {
                 0, /* No more characteristics in this service. */
             }},

main/bluetooth/services/remoteControll.h

@@ -4,7 +4,7 @@
  * Created Date: 2024-03-03 23:03:17
  * Author: Guoyi
  * -----
- * Last Modified: 2024-04-12 11:04:45
+ * Last Modified: 2024-04-28 19:34:46
  * Modified By: Guoyi
  * -----
  * Copyright (c) 2024 Guoyi Inc.
@@ -21,7 +21,9 @@
 #include "services/gatt/ble_svc_gatt.h"
 #include "services/ans/ble_svc_ans.h"
 #include "../utils/gatt_svr_write.h"
-#include "../../utils/F1D.h"
+#include "utils/F1D.h"
+#include "globalStates/controllerState.h"
+#include "globalStates/PWMState.h"
 
 #include "FlightController/PID/config/gyroPID.h"
 #include "FlightController/PID/config/headingPID.h"
@@ -37,6 +39,12 @@ static const ble_uuid16_t gatt_remoteControll_svc_uuid = BLE_UUID16_INIT(0xffe0)
 static uint32_t gatt_remoteControll_chr_PID_val[4 * 3]; // 4个PID配置，每个配置含3个float数据
 static uint16_t gatt_remoteControll_chr_PID_val_handle;
 static const ble_uuid16_t gatt_remoteControll_chr_PID_uuid = BLE_UUID16_INIT(0xffe1);
+// 2.飞行状态
+static uint16_t gatt_remoteControll_chr_flight_state_val_handle;
+static const ble_uuid16_t gatt_remoteControll_chr_flight_state_uuid = BLE_UUID16_INIT(0xffe2);
+// 3.PWM配置
+static uint16_t gatt_remoteControll_chr_PWM_config_val_handle;
+static const ble_uuid16_t gatt_remoteControll_chr_PWM_config_uuid = BLE_UUID16_INIT(0xffe3);
 
 void convertPIDtoBle()
 {
@@ -106,6 +114,26 @@ static int gatt_remoteControll_svc_access(uint16_t conn_handle, uint16_t attr_ha
                                 sizeof(gatt_remoteControll_chr_PID_val));
             return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
         }
+        else if (attr_handle == gatt_remoteControll_chr_flight_state_val_handle)
+        {
+            rc = os_mbuf_append(ctxt->om,
+                                &flightState,
+                                sizeof(flightState));
+            return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
+        }
+        else if (attr_handle == gatt_remoteControll_chr_PWM_config_val_handle)
+        {
+            uint32_t data[2]; // 2 * float
+            union F1D basic, mult;
+            basic.f = PWM_Basic;
+            mult.f = PWM_Mult;
+            data[0] = basic.i;
+            data[1] = mult.i;
+            rc = os_mbuf_append(ctxt->om,
+                                data,
+                                sizeof(data));
+            return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
+        }
         goto unknown;
 
     // 写操作 Write ctxt->om to the value if the operation is WRITE
@@ -123,6 +151,37 @@ static int gatt_remoteControll_svc_access(uint16_t conn_handle, uint16_t attr_ha
             // MODLOG_DFLT(INFO, "Notification/Indication scheduled for all subscribed peers.\n");
             return rc;
         }
+        else if (attr_handle == gatt_remoteControll_chr_flight_state_val_handle)
+        {
+            rc = gatt_svr_write(ctxt->om,
+                                sizeof(flightState),
+                                sizeof(flightState),
+                                &flightState, NULL);
+            // Send notification (or indication) to any connected devices that
+            // have subscribed for notification (or indication) for specified characteristic.
+            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
+            rc = gatt_svr_write(ctxt->om,
+                                sizeof(data),
+                                sizeof(data),
+                                data, NULL);
+            union F1D basic, mult;
+            basic.i = data[0];
+            mult.i = data[1];
+            PWM_Basic = basic.f;
+            PWM_Mult = mult.f;
+
+            // Send notification (or indication) to any connected devices that
+            // have subscribed for notification (or indication) for specified characteristic.
+            ble_gatts_chr_updated(attr_handle);
+            // MODLOG_DFLT(INFO, "Notification/Indication scheduled for all subscribed peers.\n");
+            return rc;
+        }
         goto unknown;
 
     case BLE_GATT_ACCESS_OP_READ_DSC:

main/globalStates/PWMState.h

@@ -4,7 +4,7 @@
  * Created Date: 2024-03-31 16:10:19
  * Author: Guoyi
  * -----
- * Last Modified: 2024-03-31 16:18:26
+ * Last Modified: 2024-04-28 15:22:00
  * Modified By: Guoyi
  * -----
  * Copyright (c) 2024 Guoyi Inc.
@@ -15,6 +15,8 @@
 #ifndef PWM_STATE_H
 #define PWM_STATE_H
 
-float currentPWMPercentage[4] = {0.0f, 0.0f, 0.0f, 0.0f};
+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;
 
 #endif

main/globalStates/controllerState.h

@@ -4,7 +4,7 @@
  * Created Date: 2024-04-05 21:25:11
  * Author: Guoyi
  * -----
- * Last Modified: 2024-04-10 22:58:27
+ * Last Modified: 2024-04-28 00:38:58
  * Modified By: Guoyi
  * -----
  * Copyright (c) 2024 Guoyi Inc.
@@ -15,7 +15,6 @@
 #ifndef CONTROLLER_STATE_H
 #define CONTROLLER_STATE_H
 
-int shouldControllerLoopRun = 0;
-
+int __attribute__((weak)) flightState = 0;
 
 #endif

main/main.c

@@ -20,6 +20,8 @@
 #include "freertos/FreeRTOS.h"
 #include "freertos/task.h"
 
+/* 状态指示LED */
+#include "LED/StatusLED.h"
 /* 蓝牙协议栈 */
 #include "bluetooth/Bleprph_Init.h"
 /* mpu6050 */
@@ -50,13 +52,10 @@ void Tasks_Init()
 void app_main(void)
 {
     /* 初始化各组件 */
+    StatusLED_Init();
     Bleprph_Init();
     MotorPWMDriver_Init();
 
     /* 启动所有任务 */
     Tasks_Init();
-
-    vTaskDelay(18000 / portTICK_PERIOD_MS);
-    stopAllMotors();
-    esp_restart();
 }

main/mpu6050/motionData.h

@@ -4,7 +4,7 @@
  * Created Date: 2024-03-07 22:51:03
  * Author: Guoyi
  * -----
- * Last Modified: 2024-04-01 20:03:12
+ * Last Modified: 2024-04-28 15:19:41
  * Modified By: Guoyi
  * -----
  * Copyright (c) 2024 Guoyi Inc.
@@ -16,10 +16,11 @@
 #define MOTION_DATA_H
 
 #include "mpu6050.h"
-#include "../utils/F3D.h"
+#include "utils/F3D.h"
 #include <math.h>
 #include "freertos/FreeRTOS.h"
 #include "freertos/task.h"
+#include "LED/StatusLED.h"
 
 #define ACCEL_RANGE 2
 #define GYRO_RANGE 250
@@ -69,16 +70,18 @@ void MotionData_Calibrate()
     // 等一段时间，等系统稳定后进行校准
     vTaskDelay(50 / portTICK_PERIOD_MS);
 
-    // 校准
+    // 开始校准
+    enableStatusLED();
     float gyroZ_SUM = 0;
     for (int i = 0; i < 10; i++)
     {
-        vTaskDelay(10 / portTICK_PERIOD_MS);
+        vTaskDelay(30 / portTICK_PERIOD_MS);
         // F3D accel = getAccelData();
         F3D gyro = getGyroData();
         gyroZ_SUM += gyro.z;
     }
     CalibrationOffset.gz = gyroZ_SUM / 10.0f;
+    disableStatusLED();
 }
 
 /**

main/mpu6050/mpu6050.h

@@ -4,7 +4,7 @@
  * Created Date: 2024-03-07 19:55:56
  * Author: Guoyi
  * -----
- * Last Modified: 2024-04-01 12:20:31
+ * Last Modified: 2024-04-27 23:12:23
  * Modified By: Guoyi
  * -----
  * Copyright (c) 2024 Guoyi Inc.
@@ -58,7 +58,7 @@ void MPU6050_Init()
     uint8_t flag = ESP_OK;
     flag = MPU6050_register_write_byte(PWR_MGMT_1, 0x00);   // 解除休眠状态
     flag = MPU6050_register_write_byte(SMPLRT_DIV, 0x00);   // 陀螺仪采样率。1KHz
-    flag = MPU6050_register_write_byte(CONFIG, 0x01);       // 启用低通滤波。等级为0x01
+    flag = MPU6050_register_write_byte(CONFIG, 0x06);       // 启用低通滤波。等级为0x05
     flag = MPU6050_register_write_byte(GYRO_CONFIG, 0x0);   // 陀螺仪自检及测量范围。不自检，250deg/s
     flag = MPU6050_register_write_byte(ACCEL_CONFIG, 0x01); // 加速计自检、测量范围及高通滤波频率。不自检，2G，5Hz
     if (flag != ESP_OK)

main/tasks/controllerTickLoop.h

@@ -4,7 +4,7 @@
  * Created Date: 2024-03-11 13:27:21
  * Author: Guoyi
  * -----
- * Last Modified: 2024-04-27 21:12:02
+ * Last Modified: 2024-04-28 14:58:33
  * Modified By: Guoyi
  * -----
  * Copyright (c) 2024 Guoyi Inc.
@@ -18,18 +18,31 @@
 #include "freertos/FreeRTOS.h"
 #include "freertos/task.h"
 #include "../FlightController/controllerTick.h"
+#include "../globalStates/controllerState.h"
 
 #include "mpu6050/motionData.h"
 
 void controllerTickLoop(void *argument)
 {
+    vTaskDelay(5000 / portTICK_PERIOD_MS); // 等待5000秒之后，开始初始化mpu6050，并进行校准
     MotionData_Init();
     vTaskDelay(200 / portTICK_PERIOD_MS);
-    vTaskDelay(15000 / portTICK_PERIOD_MS);
     while (1)
     {
         vTaskDelay(10 / portTICK_PERIOD_MS);
-        controllerTick(10);
+        switch (flightState)
+        {
+        case 0:
+            stopAllMotors();
+            break;
+
+        case 1:
+            controllerTick(10);
+            break;
+
+        default:
+            break;
+        }
     }
 }