Skip to content

Commit

Permalink
蓝牙协议栈-可以通过遥控改变PID参数，极大方便调试飞控算法
Browse files Browse the repository at this point in the history
  • Loading branch information
@g122622
g122622 committed Apr 10, 2024
1 parent fe861f8 commit bc57b63
Show file tree
Hide file tree
Showing 3 changed files with 77 additions and 16 deletions.
4 changes: 2 additions & 2 deletions main/bluetooth/gatt_server.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -74,10 +74,10 @@ static const struct ble_gatt_svc_def gatt_svr_svcs[] = {
.characteristics = (struct ble_gatt_chr_def[]){

{
.uuid = &gatt_remoteControll_chr_uuid.u,
.uuid = &gatt_remoteControll_chr_PID_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_val_handle,
.val_handle = &gatt_remoteControll_chr_PID_val_handle,
},
{
0, /* No more characteristics in this service. */
Expand Down
84 changes: 72 additions & 12 deletions main/bluetooth/services/remoteControll.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@
* Created Date: 2024-03-03 23:03:17
* Author: Guoyi
* -----
* Last Modified: 2024-03-03 23:31:21
* Last Modified: 2024-04-10 23:56:09
* Modified By: Guoyi
* -----
* Copyright (c) 2024 Guoyi Inc.
Expand All @@ -22,15 +22,72 @@
#include "services/ans/ble_svc_ans.h"
#include "../utils/gatt_svr_write.h"

#include "FlightController/PID/config/gyroPID.h"
#include "FlightController/PID/config/headingPID.h"
#include "FlightController/PID/config/throttlePID.h"

// ===========================遥控器服务 0xffe0================================== //

/* 服务的uuid */
static const ble_uuid16_t gatt_remoteControll_svc_uuid = BLE_UUID16_INIT(0xffe0);

/* 服务内的characteristic */
static uint8_t gatt_remoteControll_chr_val;
static uint16_t gatt_remoteControll_chr_val_handle;
static const ble_uuid16_t gatt_remoteControll_chr_uuid = BLE_UUID16_INIT(0xffe1);
// 1.PID配置
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);

void convertPIDtoBle()
{
union F1D gyroP, gyroI, gyroD, pitchP, pitchI, pitchD, rollP, rollI, rollD;

gyroP.f = gyroPIDConfig.P_Weigh;
gyroI.f = gyroPIDConfig.I_Weigh;
gyroD.f = gyroPIDConfig.D_Weigh;
pitchP.f = pitchPIDConfig.P_Weigh;
pitchI.f = pitchPIDConfig.I_Weigh;
pitchD.f = pitchPIDConfig.D_Weigh;
rollP.f = rollPIDConfig.P_Weigh;
rollI.f = rollPIDConfig.I_Weigh;
rollD.f = rollPIDConfig.D_Weigh;

gatt_remoteInfo_chr_basic_motion_val[0] = gyroP.i;
gatt_remoteInfo_chr_basic_motion_val[1] = gyroI.i;
gatt_remoteInfo_chr_basic_motion_val[2] = gyroD.i;
gatt_remoteInfo_chr_basic_motion_val[3] = pitchP.i;
gatt_remoteInfo_chr_basic_motion_val[4] = pitchI.i;
gatt_remoteInfo_chr_basic_motion_val[5] = pitchD.i;
gatt_remoteInfo_chr_basic_motion_val[6] = rollP.i;
gatt_remoteInfo_chr_basic_motion_val[7] = rollI.i;
gatt_remoteInfo_chr_basic_motion_val[8] = rollD.i;
}

void convertBletoPID()
{
union F1D gyroP, gyroI, gyroD, pitchP, pitchI, pitchD, rollP, rollI, rollD;

gyroP.i = gatt_remoteInfo_chr_basic_motion_val[0];
gyroI.i = gatt_remoteInfo_chr_basic_motion_val[1];
gyroD.i = gatt_remoteInfo_chr_basic_motion_val[2];
pitchP.i = gatt_remoteInfo_chr_basic_motion_val[3];
pitchI.i = gatt_remoteInfo_chr_basic_motion_val[4];
pitchD.i = gatt_remoteInfo_chr_basic_motion_val[5];
rollP.i = gatt_remoteInfo_chr_basic_motion_val[6];
rollI.i = gatt_remoteInfo_chr_basic_motion_val[7];
rollD.i = gatt_remoteInfo_chr_basic_motion_val[8];

gyroPIDConfig.P_Weigh = gyroP.f;
gyroPIDConfig.I_Weigh = gyroI.f;
gyroPIDConfig.D_Weigh = gyroD.f;
pitchPIDConfig.P_Weigh = pitchP.f;
pitchPIDConfig.I_Weigh = pitchI.f;
pitchPIDConfig.D_Weigh = pitchD.f;
rollPIDConfig.P_Weigh = rollP.f;
rollPIDConfig.I_Weigh = rollI.f;
rollPIDConfig.D_Weigh = rollD.f;
}

// 读写回调函数
static int gatt_remoteControll_svc_access(uint16_t conn_handle, uint16_t attr_handle, struct ble_gatt_access_ctxt *ctxt, void *arg);
static int gatt_remoteControll_svc_access(uint16_t conn_handle, uint16_t attr_handle, struct ble_gatt_access_ctxt *ctxt, void *arg)
{
Expand All @@ -40,26 +97,29 @@ static int gatt_remoteControll_svc_access(uint16_t conn_handle, uint16_t attr_ha
{
// 读操作 Append the value to ctxt->om if the operation is READ
case BLE_GATT_ACCESS_OP_READ_CHR:
if (attr_handle == gatt_remoteControll_chr_val_handle)
if (attr_handle == gatt_remoteControll_chr_PID_val_handle)
{
convertPIDtoBle();
rc = os_mbuf_append(ctxt->om,
&gatt_remoteControll_chr_val,
sizeof(gatt_remoteControll_chr_val));
gatt_remoteControll_chr_PID_val,
sizeof(gatt_remoteControll_chr_PID_val));
return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
}
goto unknown;

// 写操作 Write ctxt->om to the value if the operation is WRITE
case BLE_GATT_ACCESS_OP_WRITE_CHR:
if (attr_handle == gatt_remoteControll_chr_val_handle)
if (attr_handle == gatt_remoteControll_chr_PID_val_handle)
{
rc = gatt_svr_write(ctxt->om,
sizeof(gatt_remoteControll_chr_val),
sizeof(gatt_remoteControll_chr_val),
&gatt_remoteControll_chr_val, NULL);
sizeof(gatt_remoteControll_chr_PID_val),
sizeof(gatt_remoteControll_chr_PID_val),
gatt_remoteControll_chr_PID_val, 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);
convertBletoPID();
// MODLOG_DFLT(INFO, "Notification/Indication scheduled for all subscribed peers.\n");
printf("%d \n", gatt_remoteControll_chr_val);
return rc;
}
goto unknown;
Expand Down
5 changes: 3 additions & 2 deletions main/bluetooth/services/remoteInfo.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@
* Created Date: 2024-03-11 22:55:56
* Author: Guoyi
* -----
* Last Modified: 2024-03-12 13:37:40
* Last Modified: 2024-04-10 23:08:29
* Modified By: Guoyi
* -----
* Copyright (c) 2024 Guoyi Inc.
Expand Down Expand Up @@ -44,7 +44,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 uint16_t gatt_remoteInfo_chr_basic_motion_val_handle;
static const ble_uuid16_t gatt_remoteInfo_chr_basic_motion_uuid = BLE_UUID16_INIT(0x1023);
Expand All @@ -66,6 +66,7 @@ void calcBasicMotionVal()
gatt_remoteInfo_chr_basic_motion_val[5] = gyrz.i;
}

// 访问回调函数
static int gatt_remoteInfo_svc_access(uint16_t conn_handle, uint16_t attr_handle, struct ble_gatt_access_ctxt *ctxt, void *arg);
static int gatt_remoteInfo_svc_access(uint16_t conn_handle, uint16_t attr_handle, struct ble_gatt_access_ctxt *ctxt, void *arg)
{
Expand Down

0 comments on commit bc57b63

Please sign in to comment.