支持蓝牙读取欧拉角

g122622 · Apr 27, 2024 · f31e319 · f31e319
1 parent 3f12f4a
commit f31e319
Show file tree

Hide file tree

Showing 5 changed files with 29 additions and 34 deletions.
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-27 17:31:24
+ * Last Modified: 2024-04-27 22:26:37
  * Modified By: Guoyi
  * -----
  * Copyright (c) 2024 Guoyi Inc.
@@ -34,12 +34,14 @@ void controllerTick(int dt)
     float expectedRoll = 0;
     float expectedAccelMagnitude = 0.01;
 
-    // 全局变量
+    // 更新全局变量
     GyroData = getGyroData();
     AccelData = getAccelData();
     /* 读取传感器姿态数据 */
     float realAccelMagnitude = getAccelMagnitude();
     F3D realRulerAngle = calcEulerAngle(AccelData, GyroData);
+    // 更新全局变量
+    EulerAngleData = realRulerAngle;
 
     /* 期望值和实际值作差 */
     float pitchErr = expectedPitch - realRulerAngle.x;
@@ -54,24 +56,24 @@ void controllerTick(int dt)
 
     /* 将PID输出值转为电机PWM百分比 */
     float mult = 0.5;
-    float basic = 70;
+    float basic = 30;
     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)
+    // if (tickCount == 300)
     // {
     //     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);
 
+    setMotorPWMPercentage(0, mult * (-rollPID + pitchPID) + basic);
+    setMotorPWMPercentage(1, mult * (-rollPID - pitchPID) + basic);
+    setMotorPWMPercentage(2, mult * (+rollPID - pitchPID) + basic);
+    setMotorPWMPercentage(3, mult * (+rollPID + pitchPID) + basic);
+
+    tickCount++;
 }
diff --git a/main/bluetooth/services/remoteInfo.h b/main/bluetooth/services/remoteInfo.h
@@ -4,21 +4,7 @@
  * Created Date: 2024-03-11 22:55:56
  * Author: Guoyi
  * -----
- * Last Modified: 2024-04-27 13:19:04
- * Modified By: Guoyi
- * -----
- * Copyright (c) 2024 Guoyi Inc.
- *
- * ------------------------------------
- */
-
-/*
- * File: \remoteInfo.h
- * Project: services
- * Created Date: 2024-03-03 23:03:17
- * Author: Guoyi
- * -----
- * Last Modified: 2024-03-03 23:31:21
+ * Last Modified: 2024-04-27 22:09:55
  * Modified By: Guoyi
  * -----
  * Copyright (c) 2024 Guoyi Inc.
@@ -46,7 +32,7 @@ static const ble_uuid16_t gatt_remoteInfo_svc_uuid = BLE_UUID16_INIT(0x1022);
 
 /* 服务内的characteristic */
 // 1.基础运动参数
-static uint32_t gatt_remoteInfo_chr_basic_motion_val[6];
+static uint32_t gatt_remoteInfo_chr_basic_motion_val[3 + 3 + 2]; // 3组加速度+3组角速度+2组欧拉角
 static uint16_t gatt_remoteInfo_chr_basic_motion_val_handle;
 static const ble_uuid16_t gatt_remoteInfo_chr_basic_motion_uuid = BLE_UUID16_INIT(0x1023);
 // 2.电池电压
@@ -55,19 +41,23 @@ static const ble_uuid16_t gatt_remoteInfo_chr_battery_voltage_uuid = BLE_UUID16_
 
 void calcBasicMotionVal()
 {
-    union F1D accx, accy, accz, gyrx, gyry, gyrz;
+    union F1D accx, accy, accz, gyrx, gyry, gyrz, pitch, roll;
     accx.f = AccelData.x;
     accy.f = AccelData.y;
     accz.f = AccelData.z;
     gyrx.f = GyroData.x;
     gyry.f = GyroData.y;
     gyrz.f = GyroData.z;
+    pitch.f = EulerAngleData.x;
+    roll.f = EulerAngleData.y;
     gatt_remoteInfo_chr_basic_motion_val[0] = accx.i;
     gatt_remoteInfo_chr_basic_motion_val[1] = accy.i;
     gatt_remoteInfo_chr_basic_motion_val[2] = accz.i;
     gatt_remoteInfo_chr_basic_motion_val[3] = gyrx.i;
     gatt_remoteInfo_chr_basic_motion_val[4] = gyry.i;
     gatt_remoteInfo_chr_basic_motion_val[5] = gyrz.i;
+    gatt_remoteInfo_chr_basic_motion_val[6] = pitch.i;
+    gatt_remoteInfo_chr_basic_motion_val[7] = roll.i;
 }
 
 // 访问回调函数
@@ -87,7 +77,9 @@ static int gatt_remoteInfo_svc_access(uint16_t conn_handle, uint16_t attr_handle
                                 gatt_remoteInfo_chr_basic_motion_val,
                                 sizeof(gatt_remoteInfo_chr_basic_motion_val));
             return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
-        } else if(attr_handle == gatt_remoteInfo_chr_battery_voltage_val_handle){
+        }
+        else if (attr_handle == gatt_remoteInfo_chr_battery_voltage_val_handle)
+        {
             rc = os_mbuf_append(ctxt->om,
                                 &batteryVoltage,
                                 sizeof(batteryVoltage));

diff --git a/main/globalStates/motionState.h b/main/globalStates/motionState.h
@@ -4,7 +4,7 @@
  * Created Date: 2024-03-11 13:39:33
  * Author: Guoyi
  * -----
- * Last Modified: 2024-03-12 13:35:41
+ * Last Modified: 2024-04-27 22:04:06
  * Modified By: Guoyi
  * -----
  * Copyright (c) 2024 Guoyi Inc.
@@ -19,5 +19,6 @@
 
 F3D __attribute__((weak)) AccelData;
 F3D __attribute__((weak)) GyroData;
+F3D __attribute__((weak)) EulerAngleData;
 
 #endif
diff --git a/main/main.c b/main/main.c
@@ -53,11 +53,10 @@ void app_main(void)
     Bleprph_Init();
     MotorPWMDriver_Init();
 
-    // vTaskDelay(7000 / portTICK_PERIOD_MS);
     /* 启动所有任务 */
     Tasks_Init();
 
-    vTaskDelay(1000 / portTICK_PERIOD_MS);
-    // stopAllMotors();
-    // esp_restart();
+    vTaskDelay(18000 / portTICK_PERIOD_MS);
+    stopAllMotors();
+    esp_restart();
 }
diff --git a/main/tasks/controllerTickLoop.h b/main/tasks/controllerTickLoop.h
@@ -4,7 +4,7 @@
  * Created Date: 2024-03-11 13:27:21
  * Author: Guoyi
  * -----
- * Last Modified: 2024-04-27 17:06:58
+ * Last Modified: 2024-04-27 21:12:02
  * Modified By: Guoyi
  * -----
  * Copyright (c) 2024 Guoyi Inc.
@@ -25,6 +25,7 @@ void controllerTickLoop(void *argument)
 {
     MotionData_Init();
     vTaskDelay(200 / portTICK_PERIOD_MS);
+    vTaskDelay(15000 / portTICK_PERIOD_MS);
     while (1)
     {
         vTaskDelay(10 / portTICK_PERIOD_MS);