整体介绍

通信链路总览

image-20230401175936326

image-20230401175951464

atkpTxTask：ATKP 数据包发送任务。 该任务主要是获取 stabilizerTask 中的传感器数据、姿态数据、电机 PWM 输出数据等数据以定周期发送给 radiolinkTask 和 usblinkTxTask， 由这两个任务分别发送飞遥控器和上位机。

atkpRxAnlTask：ATKP 数据包接收处理任务。 该任务主要是处理遥控器和上位机发下来的数据包， 解析到的控制指令则发送到 stabilizerTask 中去。

队列

  • uartslkDataDelivery队列
    串口2中断接收，radiolinkTask中读取的数据
  • rxQueue队列
    处理指令的同时，把radiolinkTask接收的数据从rxPacket发送至rxQueue队列中
    见函数atkpPacketDispatch(&rxPacket)
  • txQueue队列
    同样在函数atkpPacketDispatch(&rxPacket)也会处理txQueue的数据，并存到txPacket打包后，通过串口DMA发送

指令格式

image-20230401180021877

串口指令格式【Usblink】

image-20230401180551866

指令格式：帧头(AA+AF)+功能ID+数据长度+数据+校验

校验帧头：AA+AF

功能ID: 表示该数据的意义，msgID分为 上行指令ID :upmsgID_e ；下行指令ID ：downmsgID_e,这是两个枚举变量

数据长度：dataLen

数据数组：data 最大为30，即数组长度最多30，为data[0]+data[1-29]，其中data[0]用作区别是控制命令还是控制数据。

一键起飞数据包解析举例

一键起飞帧： AA AF 50 02 00 03 AE
帧头：

1
2
3
/*下行帧头*/---从哪里发来
#define DOWN_BYTE1 0xAA  
#define DOWN_BYTE2 0xAF

MsgID:

1
2
DOWN_REMOTER	= 0x50,//指定 遥控器下行给四轴 的命令
}downmsgID_e;

02 —表示有2-1 = 1个数据
data[0] —00 —遥控数据类别：控制命令

1
2
3
4
5
6
/*遥控数据类别*/ 
typedef enum 
{
	REMOTER_CMD,//控制命令0x00
	REMOTER_DATA,//控制数据0x01
}remoterType_e;

data[1] —03 —控制命令：起飞、降落

1
2
3
4
5
6
7
8
/*下行命令/----控制命令
#define  CMD_GET_MSG		0x01	/*获取四轴信息（自检）*/
#define  CMD_GET_CANFLY		0x02	/*获取四轴能否起飞*/
#define  CMD_FLIGHT_LAND	0x03	/*起飞、降落*/
#define  CMD_EMER_STOP		0x04	/*紧急停机*/
#define  CMD_FLIP			0x05	/*4D翻滚*/
#define  CMD_POWER_MODULE	0x06	/*´打开关闭拓展模块电源´*/
#define  CMD_LEDRING_EFFECT	0x07	/*设置RGB灯环效果*/

0xAE: 校验和 从帧头到数据最后一位逐字节相加，结果取最低两个字节，如AA +AF+ 50 +02 +00 +03的结果取最低两个字节等于AE。

控制数据ATKP数据帧举例

AAAF 50 1D 01 0000a040 00000000 00000000 00004842 00000000 00000000 00 00 00 00 31

帧头:

1
0xAA 0xAF // 下行帧头,说明是遥控器或者上位机发过来的

MsgID:

1
0x50： DOWN_REMOTOR //下行指令

0x1D ： 数据长度29 -1 =28 也就是结构体remoterData_t的长度，注意字节对齐。
data[0] 0x01： data[0] = 0x01，说明接下来的是控制数据。

1
2
3
4
5
6
/*遥控数据类别*/ 
typedef enum 
{
	REMOTER_CMD,//控制命令0x00
	REMOTER_DATA,//控制数据0x01
}remoterType_e;

9个数据：
0x0000A040（send.roll = 5.0f IEEE754标准32位浮点数 小端字节序）
0x40A00000 —- 0100 0000 1010 0000 0000 0000 0000 0000 —-$(-1)^02^{129-127}(1.01) = (-1)^02^2(1.01) = 101 = 5$
//关于大小端：数据存储的大端字节序还是小端字节序取决于CPU，STM32 采用小端字节序。
0x00000000（send.pitch = 0.0f）
0x00000000（send.yaw = 0.0f）
0x00004842（send. thrust = 50.0f 50%油门）
0x42480000 —- 0100 0010 0100 1000 0000 0000 0000 0000 —-$(-1)^02^{132-127}(1.1001) = (-1)^02^5(1.1001) = 110010 = 50$
0x00000000（send. trimPitch = 0.0f trim是修正系统误差，默认0）
0x00000000（send. trimRoll = 0.0f）
0x00 （u8-CtrlMode 0x00-手动模式 0x01-定高定点模式）
0x00 （bool-FlyMode true-X模式 false-无头模式）
0x00 （bool-RCLok 解锁相关，用不上）
0x00 （1byte-字节对齐）

0x31 （前面所有字节的校验和）

image-20230401183304982

代码流程图

image-20230401185827808

radiolink.c

各函数作用

  1. void radiolinkTask(void *param)
  • 该任务主要负责接收从 NRF51822 发送（串口方式）过来的数据，然后对数据进行打包和校验，打包成 ATKP 格式并校验无误后发送到 atkpRxAnlTask 的接收队列里，同时回传一帧数据给 NRF51822。
  • 接收来自NRF51822的ATKP数据c，给rxState赋值【0,1,2,3,4,5】判断，每一步判断消息是否为帧头1、帧头2、ID、数据长度、数据、校验和，若全正确，则进入atkpPacketDispatch(&rxPacket)
  • ATKP数据（基于于匿名科创地面站V4.34通信协议下位机）。

2，void radiolinkInit(void)

  • “comm.c”中调用
  • 作用：初始化串口，检测是否创建了“发送消息队列”，没有则创建，用作发消息给NRF51822等

3，static void uartSendPacket(atkp_t *p)

  • 作用：把结构体atkp_t，打包ATKPPacket数据,并通过串口DMA发送->uartslkSendDataDmaBlocking(dataSize, sendBuffer);

4，static void atkpPacketDispatch(atkp_t *rxPacket)

    1. 断言检查，将rxPacket发送至rxQueue队列中
    2. 若MsgID不是关电源，开启无线数据指示灯LED_GREEN_L；
    3. 若有需要发送的数据，读取txQueue队列的消息，并保存在txPacket中作为缓冲区
    4. 若数据大小符合要求，则开启发送指示灯，将数据通过串口DMA发送
  • 作用：把接收到的一个遥控无线数据包存到“接收队列”，并从“发送队列”中获取一个包，通过串口DMA发送
  • 其中接收包：atkpReceivePacketBlocking(rxPacket)函数原型为atkpReceivePacketBlocking(atkp_t *p)，函数里面调用xQueueSend(rxQueue, p, portMAX_DELAY)，向“接收队列”发送消息，也就是把接收到的包发送到“接收队列”
  • 其中发送包：xQueueReceive(txQueue, &txPacket, 0) == pdTRUE，从“发送队列”获取一个包，uartSendPacket(&txPacket);再通过串口发送过去

5，bool radiolinkSendPacket(const atkp_t *p)

  • 作用：发送包到“发送队列”，返回发送状态
  • return xQueueSend(txQueue, p, 0);//不等，发送失败则返回errQUEUE_FULL

6，bool radiolinkSendPacketBlocking(const atkp_t *p)

  • 作用：发送包到“发送队列”，带阻塞
  • return xQueueSend(txQueue, p, portMAX_DELAY); //一直等到

7，int radiolinkGetFreeTxQueuePackets(void)

  • 作用：获取“发送队列”剩余的容量
  • return (RADIOLINK_TX_QUEUE_SIZE - uxQueueMessagesWaiting(txQueue));//30-已被使用的=剩余的

代码

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
#include <string.h>
#include "config.h"
#include "radiolink.h"
#include "config_param.h"
#include "led.h"
#include "ledseq.h"
#include "uart_syslink.h"

/*FreeRtos includes*/
#include "FreeRTOS.h"
#include "task.h"
#include "semphr.h"
#include "queue.h"


#define RADIOLINK_TX_QUEUE_SIZE  30 /*接收队列个数*/

static enum
{
	waitForStartByte1,//0
	waitForStartByte2,//1
	waitForMsgID,//2
	waitForDataLength,//3
	waitForData,//4
	waitForChksum1,//5
}rxState;//只能取上面的值   
//省略枚举名称，直接定义枚举变量，第一个枚举成员的默认值为整型的 0，后续枚举成员的值在前一个成员上加 1
static bool isInit;
static atkp_t txPacket;
/*通讯数据结构*/
typedef struct
{
	uint8_t msgID;
	uint8_t dataLen;
	uint8_t data[ATKP_MAX_DATA_SIZE];
}atkp_t;   
//ATKP_MAX_DATA_SIZE=30；msgID(1字节)：功能字；dataLen(1字节)：数据长度；data：有效数据。（无线数据最大传输长度为32）
static atkp_t rxPacket;   //tx是发送(transport),rx是接收(receive)
static xQueueHandle  txQueue;//声明txQueue队列

static void atkpPacketDispatch(atkp_t *rxPacket);

//radiolinkInit作用：初始化串口，检测是否创建了“发送消息队列”，没有则创建，用作发消息给NRF51822等
void radiolinkInit(void)
{
	if (isInit) return;//已初始化直接返回
	uartslkInit();   //未串口初始化则初始化 
	
	/*创建发送队列，CRTP_TX_QUEUE_SIZE个消息*/
	txQueue = xQueueCreate(RADIOLINK_TX_QUEUE_SIZE, sizeof(atkp_t));//30个消息，每个消息的大小为数据结构atkp_t的大小
	ASSERT(txQueue);
	
	isInit = true;//标记已经初始化串口
}

//radiolink接收ATKPPacket任务，把包存到rxPacket结构体中
void radiolinkTask(void *param)
{
	rxState = waitForStartByte1;	//switch从waitForStartByte1开始
	u8 c;
	u8 dataIndex = 0;
	u8 cksum = 0;//将陆续接到的变量c求和
	while(1)
	{
		if (uartslkGetDataWithTimout(&c))//传引用，双向的，从uartslkDataDelivery队列收到的值存在变量c
			/*从接受队列里读数据（带超时处理）*/
			bool uartslkGetDataWithTimout(u8 *c)
			{
				/*接受uartslkDataDelivery(1024个容量)消息*/
				if (xQueueReceive(uartslkDataDelivery, c, UARTSLK_DATA_TIMEOUT_TICKS) == pdTRUE)	
				{
					return true;
				}
				*c = 0;
				return false;
			}
		{
			switch(rxState)
			{
				case waitForStartByte1:
				/*下行帧头*/
				//#define DOWN_BYTE1 0xAA，如果成功收到DOWN_BYTE1，下次循环则进入下个case waitForStartByte2:，否则继续case waitForStartByte1:
					rxState = (c == DOWN_BYTE1) ? waitForStartByte2 : waitForStartByte1;
					cksum = c;//求和，用来校验
					break;
				case waitForStartByte2:
			    /*下行帧头*/
				//#define DOWN_BYTE2 0xAF，如果成功收到DOWN_BYTE2，下次循环则进入下个case waitForMsgID:否则进入上个case waitForStartByte1
					rxState = (c == DOWN_BYTE2) ? waitForMsgID : waitForStartByte1;
					cksum += c;//求和，用来校验
					break;
				case waitForMsgID:
					rxPacket.msgID = c;//把收到的数据c存在结构体，此时c为id
					rxState = waitForDataLength;//下次循环则进入 case waitForDataLength
					cksum += c;//求和，用来校验
					break;
				case waitForDataLength:
					if (c <= ATKP_MAX_DATA_SIZE)
					//c为数据长度，如果没超过30.#define ATKP_MAX_DATA_SIZE 30
					{
						rxPacket.dataLen = c;//把收到的数据c存在结构体，此时c为数据长度
						dataIndex = 0;
						//如果此时接收到的c > 0，下次进入case waitForData:，否则进入case waitForChksum1:
						rxState = (c > 0) ? waitForData : waitForChksum1;	
						/*c=0,数据长度为0，校验1*/
						cksum += c;//求和，用来校验
					} else //如果超过最大长度30
					{
						rxState = waitForStartByte1;//从case waitForStartByte1开始重新接收
					}
					break;
				case waitForData://由case waitForDataLength:可知dataIndex = 0;
					rxPacket.data[dataIndex] = c;//从第0个开始存储
					dataIndex++;//存到下个位置，
					cksum += c;//求和，用来校验
					//没有改变rxState，下次循环继续进入这个case
					if (dataIndex == rxPacket.dataLen)//如果存满了,改变rxState
					{
						rxState = waitForChksum1;//跳转到下个case waitForChksum1:进行校验
					}
					break;
				case waitForChksum1:
					if (cksum == c)	/*所有校验正确*/
					{
						atkpPacketDispatch(&rxPacket);//radiolink接收到ATKPPacket预处理
					} 
					else	/*校验错误*/
					{
						rxState = waitForStartByte1;	//校验错误则进入rxState = waitForStartByte1，重新接收
						IF_DEBUG_ASSERT(1);
					}
					rxState = waitForStartByte1;//校验成功，进入下一次接收
					break;
				default:
					ASSERT(0);
					break;
			}
		}
		else	/*超时处理*/
		{
			rxState = waitForStartByte1;//超时重新接收
		}
	}
}

/*radiolink接收到ATKPPacket预处理，把接收到的一个遥控无线数据包存到“接收队列”，并从“发送队列”中获取一个包，通过串口DMA发送*/
static void atkpPacketDispatch(atkp_t *rxPacket)
{
	atkpReceivePacketBlocking(rxPacket);//调用xQueueSend(rxQueue, p, portMAX_DELAY)向“接收队列”发送消息，\也就是把接收到的包发送到“接收队列”
	bool atkpReceivePacketBlocking(atkp_t *p)
	{
		ASSERT(p);
		ASSERT(p->dataLen <= ATKP_MAX_DATA_SIZE);
		return xQueueSend(rxQueue, p, portMAX_DELAY);	
	}
	if( rxPacket->msgID == DOWN_POWER)//DOWN_POWER= 0x05,//接收到的包ID=0x05，则什么都不做
	{;}/*do noting*/
	else
	{
		ledseqRun(DATA_RX_LED, seq_linkup);
		/*接收到一个遥控无线数据包则发送一个包*/
		if(xQueueReceive(txQueue, &txPacket, 0) == pdTRUE)//从“发送队列”获取一个包
		{
			ASSERT(txPacket.dataLen <= ATKP_MAX_DATA_SIZE);
			ledseqRun(DATA_TX_LED, seq_linkup);
			/*运行led的sequence序列*/
			void ledseqRun(led_e led, const ledseq_t *sequence)
			{
				int prio = getPrio(sequence);	/*获得LED优先序列*/
			
				if(prio<0) return;
			
				xSemaphoreTake(ledseqSem, portMAX_DELAY);//ledseqSem信号量
				state[led][prio] = 0; 
				updateActive(led);
				xSemaphoreGive(ledseqSem);
			
				if(activeSeq[led] == prio)	/*当前序列优先级等于活动序列优先级*/
					runLedseq(timer[led]);
			}
			uartSendPacket(&txPacket);//从“发送队列”中读取一个包，再通过串口发送过去
		}
	}
}

/*打包ATKPPacket数据通过串口DMA发送*/
static void uartSendPacket(atkp_t *p)
{
	int dataSize;
	u8 cksum = 0;
	u8 sendBuffer[36];
	
	ASSERT(p->dataLen <= ATKP_MAX_DATA_SIZE);
	//把结构体成员，打包成ATKP格式
	sendBuffer[0] = UP_BYTE1;///*上行帧头*/#define UP_BYTE1 0xAA
	sendBuffer[1] = UP_BYTE2;///*上行帧头*/#define UP_BYTE2 0xAA
	sendBuffer[2] = p->msgID;
	sendBuffer[3] = p->dataLen;
	
	memcpy(&sendBuffer[4], p->data, p->dataLen);//复制 p->data所在的内存内容的前dataLen个字节到&sendBuffer[4]内存地址上。
	//memcpy() 用来复制内存，复制 src 所指的内存内容的前 num 个字节到 dest 所指的内存地址上。\
	其原型为：void * memcpy ( void * dest, const void * src, size_t num );
	dataSize = p->dataLen + 5;//加上cksum
	/*计算校验和*/
	for (int i=0; i<dataSize-1; i++)
	{
		cksum += sendBuffer[i];//把数字所有的值加起来，作为校验
	}
	sendBuffer[dataSize-1] = cksum;
	
	/*串口DMA发送*/
	uartslkSendDataDmaBlocking(dataSize, sendBuffer);
	/*通过DMA发送原始数据*/
	void uartslkSendDataDmaBlocking(u32 size, u8* data)
	{
		if (isUartDmaInitialized)
		{
			xSemaphoreTake(uartBusy, portMAX_DELAY);
			while(DMA_GetCmdStatus(UARTSLK_DMA_STREAM) != DISABLE);	/*等待DMA空闲*/
			memcpy(dmaBuffer, data, size);		/*复制数据到DMA缓冲区*/
			DMA_InitStructure.DMA_BufferSize = size;
			initialDMACount = size;
			DMA_Init(UARTSLK_DMA_STREAM, &DMA_InitStructure);	/*重新初始化DMA数据流*/
			DMA_ITConfig(UARTSLK_DMA_STREAM, DMA_IT_TC, ENABLE);/*开启DMA传输完成中断*/		
			USART_DMACmd(UARTSLK_TYPE, USART_DMAReq_Tx, ENABLE);/* 使能USART DMA TX请求 */
			USART_ClearFlag(UARTSLK_TYPE, USART_FLAG_TC);		/* 清除传输完成中断标志位 */
			DMA_Cmd(UARTSLK_DMA_STREAM, ENABLE);	/* 使能DMA USART TX数据流 */
			xSemaphoreTake(waitUntilSendDone, portMAX_DELAY);
			xSemaphoreGive(uartBusy);
		}
	}
}

//发送包到“发送队列”
bool radiolinkSendPacket(const atkp_t *p)
{
	ASSERT(p);
	ASSERT(p->dataLen <= ATKP_MAX_DATA_SIZE);
	return xQueueSend(txQueue, p, 0);//不等，发送失败则返回errQUEUE_FULL
}

//发送包到“发送队列”，带阻塞
bool radiolinkSendPacketBlocking(const atkp_t *p)
{
	ASSERT(p);
	ASSERT(p->dataLen <= ATKP_MAX_DATA_SIZE);
	return xQueueSend(txQueue, p, portMAX_DELAY);	//一直等到
}

//获取“发送队列”剩余的容量
int radiolinkGetFreeTxQueuePackets(void)	
{
	return (RADIOLINK_TX_QUEUE_SIZE - uxQueueMessagesWaiting(txQueue));//30-已被使用的=剩余的
}

通讯协议

atkp.h

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
#ifndef __ATKP_H
#define __ATKP_H
#include <stdint.h>
#include <stdbool.h>

/*上行帧头*/
#define UP_BYTE1 0xAA
#define UP_BYTE2 0xAA

/*下行帧头*/
#define DOWN_BYTE1 0xAA 
#define DOWN_BYTE2 0xAF  

#define ATKP_MAX_DATA_SIZE 30

/*通讯数据结构*/
typedef struct
{
	uint8_t msgID;
	uint8_t dataLen;
	uint8_t data[ATKP_MAX_DATA_SIZE];
}atkp_t;

/*上行指令ID*/
typedef enum 
{
	UP_VERSION		= 0x00,
	UP_STATUS		= 0x01,
	UP_SENSER		= 0x02,
	UP_RCDATA		= 0x03,
	UP_GPSDATA		= 0x04,
	UP_POWER		= 0x05,
	UP_MOTOR		= 0x06,
	UP_SENSER2		= 0x07,
	UP_FLYMODE		= 0x0A,
	UP_SPEED 		= 0x0B,
	UP_PID1		= 0x10,
	UP_PID2		= 0x11,
	UP_PID3		= 0x12,
	UP_PID4		= 0x13,
	UP_PID5		= 0x14,
	UP_PID6		= 0x15,
	UP_RADIO		= 0x40,
	UP_MSG		= 0xEE,
	UP_CHECK		= 0xEF,
	
	UP_REMOTER		= 0x50,
	UP_PRINTF		= 0x51,
	
	UP_USER_DATA1	= 0xF1,
	UP_USER_DATA2	= 0xF2,
	UP_USER_DATA3	= 0xF3,
	UP_USER_DATA4	= 0xF4,
	UP_USER_DATA5	= 0xF5,
	UP_USER_DATA6	= 0xF6,
	UP_USER_DATA7	= 0xF7,
	UP_USER_DATA8	= 0xF8,
	UP_USER_DATA9	= 0xF9,
	UP_USER_DATA10	= 0xFA,
}upmsgID_e;


/*下行指令*/
#define  D_COMMAND_ACC_CALIB		0x01
#define  D_COMMAND_GYRO_CALIB		0x02
#define  D_COMMAND_MAG_CALIB		0x04
#define  D_COMMAND_BARO_CALIB		0x05
#define  D_COMMAND_ACC_CALIB_EXIT	0x20
#define  D_COMMAND_ACC_CALIB_STEP1	0x21
#define  D_COMMAND_ACC_CALIB_STEP2	0x22
#define  D_COMMAND_ACC_CALIB_STEP3	0x23
#define  D_COMMAND_ACC_CALIB_STEP4	0x24
#define  D_COMMAND_ACC_CALIB_STEP5	0x25
#define  D_COMMAND_ACC_CALIB_STEP6	0x26
#define  D_COMMAND_FLIGHT_LOCK		0xA0
#define  D_COMMAND_FLIGHT_ULOCK		0xA1

#define  D_ACK_READ_PID				0x01
#define  D_ACK_READ_VERSION			0xA0
#define  D_ACK_RESET_PARAM			0xA1

/*下行指令ID*/
typedef enum 
{
	DOWN_COMMAND	= 0x01,
	DOWN_ACK		= 0x02,
	DOWN_RCDATA	= 0x03,
	DOWN_POWER		= 0x05,
	DOWN_FLYMODE	= 0x0A,
	DOWN_PID1		= 0x10,
	DOWN_PID2		= 0x11,
	DOWN_PID3		= 0x12,
	DOWN_PID4		= 0x13,
	DOWN_PID5		= 0x14,
	DOWN_PID6		= 0x15,
	DOWN_RADIO		= 0x40,
	
	DOWN_REMOTER	= 0x50,//指定 遥控器下行给四轴 的命令
}downmsgID_e;

void atkpTxTask(void *param);
void atkpRxAnlTask(void *param);
void atkpInit(void);
bool atkpReceivePacketBlocking(atkp_t *p);

#endif /*ATKP_H*/

remoter_ctrl.h

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
/*遥控数据类别*/ 
typedef enum 
{
	REMOTER_CMD,//控制命令0x00
	REMOTER_DATA,//控制数据0x01
}remoterType_e;

/*下行命令/----控制命令
#define  CMD_GET_MSG		0x01	/*获取四轴信息（自检）*/
#define  CMD_GET_CANFLY		0x02	/*获取四轴能否起飞*/
#define  CMD_FLIGHT_LAND	0x03	/*起飞、降落*/
#define  CMD_EMER_STOP		0x04	/*紧急停机*/
#define  CMD_FLIP			0x05	/*4D翻滚*/
#define  CMD_POWER_MODULE	0x06	/*´打开关闭拓展模块电源´*/
#define  CMD_LEDRING_EFFECT	0x07	/*设置RGB灯环效果*/
#define  CMD_POWER_VL53LXX	0x08	/*´打开关闭激光*/

/*遥控数据结构*/-----控制数据
typedef __packed struct
{
	float roll;      
	float pitch;  
	float yaw;      
	float thrust;
	float trimPitch;
	float trimRoll;
	u8	ctrlMode;
	bool flightMode;
	bool RCLock;
} remoterData_t;

/*遥控数据结构*/-----控制数据
typedef __packed struct
{
	float roll;      
	float pitch;  
	float yaw;      
	float thrust;
	float trimPitch;
	float trimRoll;
	u8	ctrlMode;
	bool flightMode;
	bool RCLock;
} remoterData_t;

atpk.c

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
#include <string.h>
#include <stdint.h>
#include <stdio.h>
#include <stdbool.h>
#include "atkp.h"
#include "radiolink.h"
#include "usblink.h"
#include "usbd_usr.h"
#include "stabilizer.h"
#include "motors.h"
#include "commander.h"
#include "flip.h"
#include "pm.h"
#include "pid.h"
#include "attitude_pid.h"
#include "sensors.h"
#include "position_pid.h"
#include "config_param.h"
#include "power_control.h"
#include "remoter_ctrl.h"

/*FreeRTOS相关头文件*/
#include "FreeRTOS.h"
#include "task.h"
#include "semphr.h"
#include "queue.h"


//数据拆分宏定义
#define  BYTE0(dwTemp)       ( *( (u8 *)(&dwTemp)	)  )
#define  BYTE1(dwTemp)       ( *( (u8 *)(&dwTemp) + 1) )
#define  BYTE2(dwTemp)       ( *( (u8 *)(&dwTemp) + 2) )
#define  BYTE3(dwTemp)       ( *( (u8 *)(&dwTemp) + 3) )

//数据返回周期时间（单位ms）
#define  PERIOD_STATUS	30
#define  PERIOD_SENSOR 	10
#define  PERIOD_RCDATA 	40
#define  PERIOD_POWER 	100
#define  PERIOD_MOTOR	40
#define  PERIOD_SENSOR2 	40
#define  PERIOD_SPEED   	50

#define ATKP_RX_QUEUE_SIZE 	10 /*ATKP包接收队列消息个数*/

typedef struct  
{
	u16 roll;
	u16 pitch;
	u16 yaw;
	u16 thrust;
}joystickFlyui16_t;

bool isConnect = false;
bool isInit = false;
bool flyable = false;
static joystickFlyui16_t rcdata;
static xQueueHandle rxQueue;

extern PidObject pidAngleRoll;
extern PidObject pidAnglePitch;
extern PidObject pidAngleYaw;
extern PidObject pidRateRoll;
extern PidObject pidRatePitch;
extern PidObject pidRateYaw;

//把ATKP包发送到radiolink和usblink的“发送队列txQueue”
static void atkpSendPacket(atkp_t *p)
{
	radiolinkSendPacket(p);//发送到radiolink的“发送队列txQueue”
	bool radiolinkSendPacket(const atkp_t *p)
	{
		ASSERT(p);
		ASSERT(p->dataLen <= ATKP_MAX_DATA_SIZE);
		return xQueueSend(txQueue, p, 0);//不等，发送失败则返回errQUEUE_FULL
	}
	
	if(getusbConnectState())
	{
		usblinkSendPacket(p);//发送到usblink的“发送队列txQueue”
		/*usb连接发送atkpPacket*/
		bool usblinkSendPacket(const atkp_t *p)
		{
			ASSERT(p);
			ASSERT(p->dataLen <= ATKP_MAX_DATA_SIZE);
			return xQueueSend(txQueue, p, 0);	
		}
		
	}	
}

/***************************发送至匿名上位机指令******************************/
static void sendStatus(float roll, float pitch, float yaw, s32 alt, u8 fly_model, u8 armed)//飞机姿态等基本信息
{
	u8 _cnt=0;
	atkp_t p;
	vs16 _temp;
	vs32 _temp2 = alt;
	
	p.msgID = UP_STATUS;--1
	
	_temp = (int)(roll*100);
	p.data[_cnt++]=BYTE1(_temp);
	p.data[_cnt++]=BYTE0(_temp);
	_temp = (int)(pitch*100);
	p.data[_cnt++]=BYTE1(_temp);
	p.data[_cnt++]=BYTE0(_temp);
	_temp = (int)(yaw*100);
	p.data[_cnt++]=BYTE1(_temp);
	p.data[_cnt++]=BYTE0(_temp);
	
	p.data[_cnt++]=BYTE3(_temp2);
	p.data[_cnt++]=BYTE2(_temp2);
	p.data[_cnt++]=BYTE1(_temp2);
	p.data[_cnt++]=BYTE0(_temp2);
	
	p.data[_cnt++] = fly_model;
	p.data[_cnt++] = armed;
	
	p.dataLen = _cnt;
	atkpSendPacket(&p);//把ATKP包发送到radiolink和usblink的“发送队列txQueue”
}

static void sendSenser(s16 a_x,s16 a_y,s16 a_z,s16 g_x,s16 g_y,s16 g_z,s16 m_x,s16 m_y,s16 m_z)//九轴传感器数据
{
	u8 _cnt=0;
	atkp_t p;
	vs16 _temp;
	
	p.msgID = UP_SENSER;--2

	_temp = a_x;
	p.data[_cnt++]=BYTE1(_temp);
	p.data[_cnt++]=BYTE0(_temp);
	_temp = a_y;
	p.data[_cnt++]=BYTE1(_temp);
	p.data[_cnt++]=BYTE0(_temp);
	_temp = a_z;	
	p.data[_cnt++]=BYTE1(_temp);
	p.data[_cnt++]=BYTE0(_temp);
	
	_temp = g_x;	
	p.data[_cnt++]=BYTE1(_temp);
	p.data[_cnt++]=BYTE0(_temp);
	_temp = g_y;	
	p.data[_cnt++]=BYTE1(_temp);
	p.data[_cnt++]=BYTE0(_temp);
	_temp = g_z;	
	p.data[_cnt++]=BYTE1(_temp);
	p.data[_cnt++]=BYTE0(_temp);
	
	_temp = m_x;	
	p.data[_cnt++]=BYTE1(_temp);
	p.data[_cnt++]=BYTE0(_temp);
	_temp = m_y;	
	p.data[_cnt++]=BYTE1(_temp);
	p.data[_cnt++]=BYTE0(_temp);
	_temp = m_z;	
	p.data[_cnt++]=BYTE1(_temp);
	p.data[_cnt++]=BYTE0(_temp);
	_temp = 0;	
	p.data[_cnt++]=BYTE1(_temp);
	p.data[_cnt++]=BYTE0(_temp);	
	
	p.dataLen = _cnt;
	atkpSendPacket(&p);//把ATKP包发送到radiolink和usblink的“发送队列txQueue”
}
static void sendRCData(u16 thrust,u16 yaw,u16 roll,u16 pitch,u16 aux1,u16 aux2,u16 aux3,u16 aux4,u16 aux5,u16 aux6)
//飞机收到的控制数据
{
	u8 _cnt=0;
	atkp_t p;
	
	p.msgID = UP_RCDATA;--3
	p.data[_cnt++]=BYTE1(thrust);
	p.data[_cnt++]=BYTE0(thrust);
	p.data[_cnt++]=BYTE1(yaw);
	p.data[_cnt++]=BYTE0(yaw);
	p.data[_cnt++]=BYTE1(roll);
	p.data[_cnt++]=BYTE0(roll);
	p.data[_cnt++]=BYTE1(pitch);
	p.data[_cnt++]=BYTE0(pitch);
	p.data[_cnt++]=BYTE1(aux1);
	p.data[_cnt++]=BYTE0(aux1);
	p.data[_cnt++]=BYTE1(aux2);
	p.data[_cnt++]=BYTE0(aux2);
	p.data[_cnt++]=BYTE1(aux3);
	p.data[_cnt++]=BYTE0(aux3);
	p.data[_cnt++]=BYTE1(aux4);
	p.data[_cnt++]=BYTE0(aux4);
	p.data[_cnt++]=BYTE1(aux5);
	p.data[_cnt++]=BYTE0(aux5);
	p.data[_cnt++]=BYTE1(aux6);
	p.data[_cnt++]=BYTE0(aux6);

	p.dataLen = _cnt;
	atkpSendPacket(&p);//把ATKP包发送到radiolink和usblink的“发送队列txQueue”
}

static void sendPower(u16 votage, u16 current)
{
	u8 _cnt=0;
	atkp_t p;
	
	p.msgID = UP_POWER;--5
	
	p.data[_cnt++]=BYTE1(votage);
	p.data[_cnt++]=BYTE0(votage);
	p.data[_cnt++]=BYTE1(current);
	p.data[_cnt++]=BYTE0(current);
	
	p.dataLen = _cnt;
	atkpSendPacket(&p);//把ATKP包发送到radiolink和usblink的“发送队列txQueue”
}

static void sendMotorPWM(u16 m_1,u16 m_2,u16 m_3,u16 m_4,u16 m_5,u16 m_6,u16 m_7,u16 m_8)//电机PWM值
{
	u8 _cnt=0;
	atkp_t p;
	
	p.msgID = UP_MOTOR;--6
	
	p.data[_cnt++]=BYTE1(m_1);
	p.data[_cnt++]=BYTE0(m_1);
	p.data[_cnt++]=BYTE1(m_2);
	p.data[_cnt++]=BYTE0(m_2);
	p.data[_cnt++]=BYTE1(m_3);
	p.data[_cnt++]=BYTE0(m_3);
	p.data[_cnt++]=BYTE1(m_4);
	p.data[_cnt++]=BYTE0(m_4);
	p.data[_cnt++]=BYTE1(m_5);
	p.data[_cnt++]=BYTE0(m_5);
	p.data[_cnt++]=BYTE1(m_6);
	p.data[_cnt++]=BYTE0(m_6);
	p.data[_cnt++]=BYTE1(m_7);
	p.data[_cnt++]=BYTE0(m_7);
	p.data[_cnt++]=BYTE1(m_8);
	p.data[_cnt++]=BYTE0(m_8);
	
	p.dataLen = _cnt;
	atkpSendPacket(&p);//把ATKP包发送到radiolink和usblink的“发送队列txQueue”
}

static void sendSenser2(s32 bar_alt,u16 csb_alt)//发送气压计信息
{
	u8 _cnt=0;
	atkp_t p;
	
	p.msgID = UP_SENSER2;--7
	
	p.data[_cnt++]=BYTE3(bar_alt);
	p.data[_cnt++]=BYTE2(bar_alt);
	p.data[_cnt++]=BYTE1(bar_alt);
	p.data[_cnt++]=BYTE0(bar_alt);
	
	p.data[_cnt++]=BYTE1(csb_alt);
	p.data[_cnt++]=BYTE0(csb_alt);
	
	p.dataLen = _cnt;
	atkpSendPacket(&p);//把ATKP包发送到radiolink和usblink的“发送队列txQueue”
}

static void sendPid(u8 group,float p1_p,float p1_i,float p1_d,float p2_p,float p2_i,float p2_d,float p3_p,float p3_i,float p3_d)
{
	u8 _cnt=0;
	atkp_t p;
	vs16 _temp;
	
	p.msgID = 0x10+group-1;--10~15

	_temp = p1_p * 10 ;
	p.data[_cnt++]=BYTE1(_temp);
	p.data[_cnt++]=BYTE0(_temp);
	_temp = p1_i * 10 ;
	p.data[_cnt++]=BYTE1(_temp);
	p.data[_cnt++]=BYTE0(_temp);
	_temp = p1_d * 10 ;
	p.data[_cnt++]=BYTE1(_temp);
	p.data[_cnt++]=BYTE0(_temp);
	_temp = p2_p * 10 ;
	p.data[_cnt++]=BYTE1(_temp);
	p.data[_cnt++]=BYTE0(_temp);
	_temp = p2_i * 10 ;
	p.data[_cnt++]=BYTE1(_temp);
	p.data[_cnt++]=BYTE0(_temp);
	_temp = p2_d * 10 ;
	p.data[_cnt++]=BYTE1(_temp);
	p.data[_cnt++]=BYTE0(_temp);
	_temp = p3_p * 10 ;
	p.data[_cnt++]=BYTE1(_temp);
	p.data[_cnt++]=BYTE0(_temp);
	_temp = p3_i * 10 ;
	p.data[_cnt++]=BYTE1(_temp);
	p.data[_cnt++]=BYTE0(_temp);
	_temp = p3_d * 10 ;
	p.data[_cnt++]=BYTE1(_temp);
	p.data[_cnt++]=BYTE0(_temp);
	
	p.dataLen = _cnt;
	atkpSendPacket(&p);//把ATKP包发送到radiolink和usblink的“发送队列txQueue”
}

static void sendCheck(u8 head, u8 check_sum)
{
	atkp_t p;
	
	p.msgID = UP_CHECK;--EF
	p.dataLen = 2;
	p.data[0] = head;
	p.data[1] = check_sum;
	atkpSendPacket(&p);//把ATKP包发送到radiolink和usblink的“发送队列txQueue”
}
/****************************************************************************/

/*数据周期性发送给上位机，每1ms调用一次*/
static void atkpSendPeriod(void)
{
	static u16 count_ms = 1;
	//if（！（）） ，只有当里面的为0时，！0就是真。
	if(!(count_ms % PERIOD_STATUS))//count_ms/30ms,每30ms发送一次
	{
		attitude_t attitude;//结构体=时间戳+欧拉角
		getAttitudeData(&attitude);
		int positionZ = getPositionZ() * 100.f;
		sendStatus(attitude.roll, attitude.pitch, attitude.yaw, positionZ, 0, flyable);	//发送状态			
	}
	if(!(count_ms % PERIOD_SENSOR))//count_ms/10ms，每10ms发送一次MPU数据
	{
		Axis3i16 acc;
		Axis3i16 gyro;
		Axis3i16 mag;
		getSensorRawData(&acc, &gyro, &mag);
		sendSenser(acc.x, acc.y, acc.z, gyro.x, gyro.y, gyro.z, mag.x, mag.y, mag.z);//发送MPU九轴传感器数据	
		//3 轴加速度传感器、3 轴陀螺仪传感器和 3 轴磁力计		
	}
	if(!(count_ms % PERIOD_RCDATA))//count_ms/40ms，每40ms发送一次遥控数据（油门，偏航，翻滚，俯仰）
	{
		sendRCData(rcdata.thrust, rcdata.yaw, rcdata.roll,
					rcdata.pitch, 0, 0, 0, 0, 0, 0);//发送遥控器的数据
	}
	if(!(count_ms % PERIOD_POWER))//count_ms/100ms，每100ms发送一次电压数据
	{
		float bat = pmGetBatteryVoltage();
		sendPower(bat*100,500);//发送电量信息
	}
	if(!(count_ms % PERIOD_MOTOR))//count_ms/40ms，每40ms发送一次电机PWMxxx数据
	{
		u16 m1,m2,m3,m4;
		motorPWM_t motorPWM;
		getMotorPWM(&motorPWM);
		m1 = (float)motorPWM.m1/65535*1000;
		m2 = (float)motorPWM.m2/65535*1000;
		m3 = (float)motorPWM.m3/65535*1000;
		m4 = (float)motorPWM.m4/65535*1000;
		sendMotorPWM(m1,m2,m3,m4,0,0,0,0);
	}
	if(!(count_ms % PERIOD_SENSOR2))//count_ms/40ms，每40ms发送一次气压计数据
	{
		int baro = getBaroData() * 100.f;
		sendSenser2(baro,0);//发送气压计信息
	}
	if(++count_ms>=65535) 
		count_ms = 1;	
}
//将帧头，地址，帧数据部分的长度，每个数据求和作为校验
static u8 atkpCheckSum(atkp_t *packet)//分析atkp_t结构体（包），得出校验值
{
	u8 sum;
	sum = DOWN_BYTE1;/*下行帧头*/
	sum += DOWN_BYTE2;
	sum += packet->msgID;//帧地址
	sum += packet->dataLen;//帧数据部分的长度
	for(int i=0; i<packet->dataLen; i++)
	{
		sum += packet->data[i];
	}
	return sum;
}
//解包，根据msgID==下行指令ID（枚举类型）和data[0]做出处理
static void atkpReceiveAnl(atkp_t *anlPacket)
{
	if(anlPacket->msgID	== DOWN_COMMAND)//DOWN_COMMAND	= 0x01，下行指令
	{
		/*下行指令*/
		#define  D_COMMAND_ACC_CALIB		0x01
		#define  D_COMMAND_GYRO_CALIB		0x02
		#define  D_COMMAND_MAG_CALIB		0x04
		#define  D_COMMAND_BARO_CALIB		0x05
		#define  D_COMMAND_ACC_CALIB_EXIT	0x20
		#define  D_COMMAND_ACC_CALIB_STEP1	0x21
		#define  D_COMMAND_ACC_CALIB_STEP2	0x22
		#define  D_COMMAND_ACC_CALIB_STEP3	0x23
		#define  D_COMMAND_ACC_CALIB_STEP4	0x24
		#define  D_COMMAND_ACC_CALIB_STEP5	0x25
		#define  D_COMMAND_ACC_CALIB_STEP6	0x26
		#define  D_COMMAND_FLIGHT_LOCK		0xA0
		#define  D_COMMAND_FLIGHT_ULOCK		0xA1
		
		#define  D_ACK_READ_PID			0x01
		#define  D_ACK_READ_VERSION		0xA0
		#define  D_ACK_RESET_PARAM			0xA1
	
		switch(anlPacket->data[0])
		{
			case D_COMMAND_ACC_CALIB:
				break;
			
			case D_COMMAND_GYRO_CALIB:
				break;
			
			case D_COMMAND_MAG_CALIB:				
				break;
			
			case D_COMMAND_BARO_CALIB:
				break;
			
			case D_COMMAND_ACC_CALIB_STEP1:
				break;
			case D_COMMAND_ACC_CALIB_STEP2:
				break;
			case D_COMMAND_ACC_CALIB_STEP3:
				break;
			case D_COMMAND_ACC_CALIB_STEP4:
				break;
			case D_COMMAND_ACC_CALIB_STEP5:
				break;
			case D_COMMAND_ACC_CALIB_STEP6:
				break;
			
			case D_COMMAND_FLIGHT_LOCK:
				flyable = false;
				break;
			
			case D_COMMAND_FLIGHT_ULOCK:
				flyable = true;
		}
	}			
	else if(anlPacket->msgID == DOWN_ACK)//DOWN_ACK	= 0x02,
	{
		if(anlPacket->data[0] == D_ACK_READ_PID)/*读取PID参数*/
		{
			sendPid(1, pidRateRoll.kp, pidRateRoll.ki, pidRateRoll.kd,
					   pidRatePitch.kp, pidRatePitch.ki, pidRatePitch.kd,
					   pidRateYaw.kp, pidRateYaw.ki, pidRateYaw.kd 
				   );
			sendPid(2, pidAngleRoll.kp, pidAngleRoll.ki, pidAngleRoll.kd,
					   pidAnglePitch.kp, pidAnglePitch.ki, pidAnglePitch.kd,
					   pidAngleYaw.kp, pidAngleYaw.ki, pidAngleYaw.kd 
				   );
			sendPid(3, pidVZ.kp, pidVZ.ki, pidVZ.kd,
					   pidZ.kp, pidZ.ki, pidZ.kd,
					   pidVX.kp, pidVX.ki, pidVX.kd
				   );
			sendPid(4, pidX.kp, pidX.ki, pidX.kd,
					   0, 0, 0,
					   0, 0, 0
				   );
		}
		if(anlPacket->data[0] == D_ACK_RESET_PARAM)/*恢复默认参数*/
		{
			resetConfigParamPID();
			
			attitudeControlInit(RATE_PID_DT, ANGEL_PID_DT); /*初始化姿态PID*/	
			positionControlInit(VELOCITY_PID_DT, POSITION_PID_DT); /*初始化位置PID*/
			
			sendPid(1, pidRateRoll.kp, pidRateRoll.ki, pidRateRoll.kd,
					   pidRatePitch.kp, pidRatePitch.ki, pidRatePitch.kd,
					   pidRateYaw.kp, pidRateYaw.ki, pidRateYaw.kd 
				   );
			sendPid(2, pidAngleRoll.kp, pidAngleRoll.ki, pidAngleRoll.kd,
					   pidAnglePitch.kp, pidAnglePitch.ki, pidAnglePitch.kd,
					   pidAngleYaw.kp, pidAngleYaw.ki, pidAngleYaw.kd 
				   );
			sendPid(3, pidVZ.kp, pidVZ.ki, pidVZ.kd,
					   pidZ.kp, pidZ.ki, pidZ.kd,
					   pidVX.kp, pidVX.ki, pidVX.kd
				   );
			sendPid(4, pidX.kp, pidX.ki, pidX.kd,
					   0, 0, 0,
					   0, 0, 0
				   );
		}
		
	}
	else if(anlPacket->msgID == DOWN_RCDATA)/*指令值*/
	{
		rcdata = *((joystickFlyui16_t*)anlPacket->data);
	}
	else if(anlPacket->msgID == DOWN_POWER)/*nrf51822*/
	{
		pmSyslinkUpdate(anlPacket);
	}
	else if(anlPacket->msgID == DOWN_REMOTER)/*遥控器*/	
	{
		remoterCtrlProcess(anlPacket);
	}
	else if(anlPacket->msgID == DOWN_PID1)//在线PID调试
	{
		pidRateRoll.kp  = 0.1*((s16)(*(anlPacket->data+0)<<8)|*(anlPacket->data+1));
		pidRateRoll.ki  = 0.1*((s16)(*(anlPacket->data+2)<<8)|*(anlPacket->data+3));
		pidRateRoll.kd  = 0.1*((s16)(*(anlPacket->data+4)<<8)|*(anlPacket->data+5));
		pidRatePitch.kp = 0.1*((s16)(*(anlPacket->data+6)<<8)|*(anlPacket->data+7));
		pidRatePitch.ki = 0.1*((s16)(*(anlPacket->data+8)<<8)|*(anlPacket->data+9));
		pidRatePitch.kd = 0.1*((s16)(*(anlPacket->data+10)<<8)|*(anlPacket->data+11));
		pidRateYaw.kp   = 0.1*((s16)(*(anlPacket->data+12)<<8)|*(anlPacket->data+13));
		pidRateYaw.ki   = 0.1*((s16)(*(anlPacket->data+14)<<8)|*(anlPacket->data+15));
		pidRateYaw.kd   = 0.1*((s16)(*(anlPacket->data+16)<<8)|*(anlPacket->data+17));
		u8 cksum = atkpCheckSum(anlPacket);
		sendCheck(anlPacket->msgID,cksum);
	}
	else if(anlPacket->msgID == DOWN_PID2)
	{
		pidAngleRoll.kp  = 0.1*((s16)(*(anlPacket->data+0)<<8)|*(anlPacket->data+1));
		pidAngleRoll.ki  = 0.1*((s16)(*(anlPacket->data+2)<<8)|*(anlPacket->data+3));
		pidAngleRoll.kd  = 0.1*((s16)(*(anlPacket->data+4)<<8)|*(anlPacket->data+5));
		pidAnglePitch.kp = 0.1*((s16)(*(anlPacket->data+6)<<8)|*(anlPacket->data+7));
		pidAnglePitch.ki = 0.1*((s16)(*(anlPacket->data+8)<<8)|*(anlPacket->data+9));
		pidAnglePitch.kd = 0.1*((s16)(*(anlPacket->data+10)<<8)|*(anlPacket->data+11));
		pidAngleYaw.kp   = 0.1*((s16)(*(anlPacket->data+12)<<8)|*(anlPacket->data+13));
		pidAngleYaw.ki   = 0.1*((s16)(*(anlPacket->data+14)<<8)|*(anlPacket->data+15));
		pidAngleYaw.kd   = 0.1*((s16)(*(anlPacket->data+16)<<8)|*(anlPacket->data+17));
		u8 cksum = atkpCheckSum(anlPacket);
		sendCheck(anlPacket->msgID,cksum);
	}		
	else if(anlPacket->msgID == DOWN_PID3)
	{
		float kp  = 0.1*((s16)(*(anlPacket->data+0)<<8)|*(anlPacket->data+1));
		float ki  = 0.1*((s16)(*(anlPacket->data+2)<<8)|*(anlPacket->data+3));
		float kd  = 0.1*((s16)(*(anlPacket->data+4)<<8)|*(anlPacket->data+5));
		setPositionPIDZ(kp, ki, kd);
		u8 cksum = atkpCheckSum(anlPacket);
		sendCheck(anlPacket->msgID,cksum);
	}
	else if(anlPacket->msgID == DOWN_PID4)
	{
		u8 cksum = atkpCheckSum(anlPacket);
		sendCheck(anlPacket->msgID,cksum);
	}
	else if(anlPacket->msgID == DOWN_PID5)
	{
		u8 cksum = atkpCheckSum(anlPacket);
		sendCheck(anlPacket->msgID,cksum);
	}
	-
	{
//		s16 temp1  = ((s16)(*(anlPacket->data+0)<<8)|*(anlPacket->data+1));
//		s16 temp2  = ((s16)(*(anlPacket->data+2)<<8)|*(anlPacket->data+3));
//		s16 temp3  = ((s16)(*(anlPacket->data+4)<<8)|*(anlPacket->data+5));
		s16 enable = ((s16)(*(anlPacket->data+6)<<8)|*(anlPacket->data+7));
		s16 m1_set = ((s16)(*(anlPacket->data+8)<<8)|*(anlPacket->data+9));
		s16 m2_set = ((s16)(*(anlPacket->data+10)<<8)|*(anlPacket->data+11));
		s16 m3_set = ((s16)(*(anlPacket->data+12)<<8)|*(anlPacket->data+13));
		s16 m4_set = ((s16)(*(anlPacket->data+14)<<8)|*(anlPacket->data+15));
		setMotorPWM(enable,m1_set,m2_set,m3_set,m4_set);
		attitudePIDwriteToConfigParam();
		positionPIDwriteToConfigParam();
		u8 cksum = atkpCheckSum(anlPacket);
		sendCheck(anlPacket->msgID,cksum);
	}
} 

void atkpTxTask(void *param)
{
	sendMsgACK();//返回四轴信息
	while(1)
	{
		atkpSendPeriod();//数据周期性发送给上位机，每1ms调用一次
		vTaskDelay(1);
	}
}
//ATKP 数据包接收处理任务，主要是处理遥控器和上位机发下来的数据包，解析到的控制指令则发送到 stabilizerTask 中去
void atkpRxAnlTask(void *param)
{
	atkp_t p;
	while(1)
	{
		xQueueReceive(rxQueue, &p, portMAX_DELAY);//从“接收队列rxQueue”提取一个包，存入缓冲区p
		atkpReceiveAnl(&p);//双向值传递，分析处理包
	}
}

void atkpInit(void)//检测是否创建了“接收队列”，没有则创建
{
	if(isInit) return;
	rxQueue = xQueueCreate(ATKP_RX_QUEUE_SIZE, sizeof(atkp_t));
	ASSERT(rxQueue);
	isInit = true;
}

bool atkpReceivePacketBlocking(atkp_t *p)//接收ATKP包，带阻塞（死等），只被usblinkRxTask和atkpPacketDispatch调用
{
	ASSERT(p);
	ASSERT(p->dataLen <= ATKP_MAX_DATA_SIZE);
	return xQueueSend(rxQueue, p, portMAX_DELAY);	
}

usblink.c

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
#include <stdbool.h>
#include <string.h>
#include "config.h"
#include "usblink.h"
#include "atkp.h"
#include "config_param.h"
#include "ledseq.h"
#include "pm.h"
#include "usbd_cdc_vcp.h"
#include "usbd_usr.h"

/*FreeRTOS相关头文件*/
#include "FreeRTOS.h"
#include "task.h"
#include "queue.h"
#include "semphr.h"


#define USBLINK_TX_QUEUE_SIZE 	30 /*接收队列个数*/

static enum
{
	waitForStartByte1,
	waitForStartByte2,
	waitForMsgID,
	waitForDataLength,
	waitForData,
	waitForChksum1,
}rxState;

static bool isInit;
static atkp_t rxPacket;
static xQueueHandle  txQueue;

/*usb连接初始化*/
void usblinkInit()//检测是否创建了“发送消息队列”，没有则创建，
{
	if(isInit) return;
	
	usbd_cdc_vcp_Init();// 初始化usb，并创建“usbDataDelivery队列”
	/*创建发送队列，USBLINK_TX_QUEUE_SIZE个消息*/
	txQueue = xQueueCreate(USBLINK_TX_QUEUE_SIZE, sizeof(atkp_t));//创建了“发送消息队列”
	ASSERT(txQueue);
	isInit = true;
}

/*usb连接发送atkpPacket*/
bool usblinkSendPacket(const atkp_t *p)
{
	ASSERT(p);
	ASSERT(p->dataLen <= ATKP_MAX_DATA_SIZE);
	return xQueueSend(txQueue, p, 0);	//向队列发送消息，其他任务调用它，把自己的atkpPacket存到txQueue，\  usblinkTxTask任务又从txQueue取出发给上位机等。
}

//获取剩余可用txQueue个数
int usblinkGetFreeTxQueuePackets(void)	
{
	return (USBLINK_TX_QUEUE_SIZE - uxQueueMessagesWaiting(txQueue));
}

//USB发送ATKPPacket任务
void usblinkTxTask(void *param)
{
	atkp_t p;
	u8 sendBuffer[64];
	u8 cksum;
	u8 dataLen;
	while(1)
	{
		xQueueReceive(txQueue, &p, portMAX_DELAY);
		
		sendBuffer[0] = UP_BYTE1;/*上行帧头*/
		sendBuffer[1] = UP_BYTE2;
		sendBuffer[2] = p.msgID;
		sendBuffer[3] = p.dataLen;
		memcpy(&sendBuffer[4], p.data, p.dataLen);
		cksum = 0;
		for (int i = 0; i < p.dataLen+4; i++)
		{
			cksum += sendBuffer[i];
		}
		dataLen = p.dataLen + 5;
		sendBuffer[dataLen - 1] = cksum;
		usbsendData(sendBuffer, dataLen);
		void usbsendData(u8* data, u16 length)
		{
			for(int i=0; i<length; i++)
			{
				VCP_DataTx(data[i]);
			}
		}
		/**
		  * @brief  VCP_DataTx
		  *         CDC received data to be send over USB IN endpoint are managed in 
		  *         this function.
		  * @param  Buf: Buffer of data to be sent
		  * @param  Len: Number of data to be sent (in bytes)
		  * @retval Result of the opeartion: USBD_OK if all operations are OK else VCP_FAIL
		  */
		static uint16_t VCP_DataTx (uint8_t data)
		{
		  if (linecoding.datatype == 7)
		  {
		    APP_Rx_Buffer[APP_Rx_ptr_in] = data & 0x7F;
		  }
		  else if (linecoding.datatype == 8)
		  {
		    APP_Rx_Buffer[APP_Rx_ptr_in] = data;
		  }
		  
		  APP_Rx_ptr_in++;
		  
		  /* To avoid buffer overflow */
		  if(APP_Rx_ptr_in == APP_RX_DATA_SIZE)
		  {
		    APP_Rx_ptr_in = 0;
		  }  
		  
		  return USBD_OK;
		}
		
		ledseqRun(DATA_TX_LED, seq_linkup);
	}
}

//USB虚拟串口接收ATKPPacket任务
void usblinkRxTask(void *param)
{
	u8 c;
	u8 dataIndex = 0;
	u8 cksum = 0;
	rxState = waitForStartByte1;
	while(1)
	{
		if (usbGetDataWithTimout(&c))
		{
			switch(rxState)
			{
				case waitForStartByte1:
					rxState = (c == DOWN_BYTE1) ? waitForStartByte2 : waitForStartByte1;
					cksum = c;
					break;
				case waitForStartByte2:
					rxState = (c == DOWN_BYTE2) ? waitForMsgID : waitForStartByte1;
					cksum += c;
					break;
				case waitForMsgID:
					rxPacket.msgID = c;
					rxState = waitForDataLength;
					cksum += c;
					break;
				case waitForDataLength:
					if (c <= ATKP_MAX_DATA_SIZE)
					{
						rxPacket.dataLen = c;
						dataIndex = 0;
						rxState = (c > 0) ? waitForData : waitForChksum1;	
						cksum += c;
					} else 
					{
						rxState = waitForStartByte1;
					}
					break;
				case waitForData:
					rxPacket.data[dataIndex] = c;
					dataIndex++;
					cksum += c;
					if (dataIndex == rxPacket.dataLen)
					{
						rxState = waitForChksum1;
					}
					break;
				case waitForChksum1:
					if (cksum == c)	/*所有校验正确*/
					{
						ledseqRun(DATA_RX_LED, seq_linkup);
						atkpReceivePacketBlocking(&rxPacket);
						bool atkpReceivePacketBlocking(atkp_t *p)
						{
							ASSERT(p);
							ASSERT(p->dataLen <= ATKP_MAX_DATA_SIZE);
							return xQueueSend(rxQueue, p, portMAX_DELAY);	
						}
					} 
					else	/*校验错误*/
					{
						rxState = waitForStartByte1;	
						IF_DEBUG_ASSERT(1);
					}
					rxState = waitForStartByte1;
					break;
				default:
					ASSERT(0);
					break;
			}
		}
		else	/*超时处理*/
		{
			rxState = waitForStartByte1;
		}
	}
}

附录：匿名地面站协议

飞控->上位机
帧 帧头 功能字 长度 数据 校验 备注
VER AAAA 00 LEN uint8 HardwareType硬件种类
uint16 HardwareVER100硬件版本
uint16 SoftwareVER100软件版本
uint16 ProtocolVER100协议版本
uint16 BootloaderVER100		 SUM 版本信息
STATUS AAAA 01 LEN int16 ROL100
int16 PIT100
int16 YAW*100
int32 ALT_USE(高度cm)
uint8 FLY_MODEL(飞行模式）
u8 ARMED : 0加锁 1解锁		 SUM 飞机姿态等基本信息
SENSER AAAA 02 18 int16 ACC_X
int16 ACC_Y
int16 ACC_Z
int16 GYRO_X
int16 GYRO_Y
int16 GYRO_Z
int16 MAG_X
int16 MAG_Y
int16 MAG_Z		 SUM 飞机传感器数据
RCDATA AAAA 03 20 int16 THR
int16 YAW
int16 ROL
int16 PIT
int16 AUX1
int16 AUX2
int16 AUX3
int16 AUX4
int16 AUX5
int16 AUX6		 SUM 飞机收到的控制数据
GPSDATA AAAA 04 LEN 参见数据定义->当前位置信息 SUM 机载GPS 信息
POWER AAAA 05 LEN uint16 Votage100
uint16 Current100		 SUM
MOTO 06 LEN uint16 PWM_MOTO12345678 SUM 马达PWM(范围0- 1000)
SENSER2 AAAA 07 LEN int32 ALT_BAR(推荐为cm单位)
uint16 ALT_CSB(超声波高度cm)		 SUM
AAAA 08 LEN SUM
AAAA 09 LEN SUM
FLY MODEL AAAA 0A LEN int8 AUX1_LOW_MODEL
int8 AUX1_MID_MODEL
int8 AUX1_HIG_MODEL
int8 AUX2_LOW_MODEL
int8 AUX2_MID_MODEL
int8 AUX2_HIG_MODEL
int8 AUX3_LOW_MODEL
int8 AUX3_MID_MODEL
int8 AUX3_HIG_MODEL		 SUM 飞行模式
AAAA 0B LEN int16 SPEED_ROL
int16 SPEED_PIT
int16 SPEED_Z（单位cm）		 SUM
AAAA 0C LEN SUM
AAAA 0D LEN SUM
AAAA 0E LEN SUM
AAAA 0F LEN SUM
PID1 AAAA 10 18 int16 PID1_P
int16 PID1_I
int16 PID1_D
int16 PID2_P
int16 PID2_I
int16 PID2_D
int16 PID3_P
int16 PID3_I
int16 PID3_D		 SUM PID数据帧1
PID2 AAAA 11 18 int16 PID4_P
int16 PID4_I
int16 PID4_D
int16 PID5_P
int16 PID5_I
int16 PID5_D
int16 PID6_P
int16 PID6_I
int16 PID6_D		 SUM PID数据帧2
PID3 AAAA 12 18 int16 PID7_P
int16 PID7_I
int16 PID7_D
int16 PID8_P
int16 PID8_I
int16 PID8_D
int16 PID9_P
int16 PID9_I
int16 PID9_D		 SUM PID数据帧3
PID4 AAAA 13 18 int16 PID10_P
int16 PID10_I
int16 PID10_D
int16 PID11_P
int16 PID11_I
int16 PID11_D
int16 PID12_P
int16 PID12_I
int16 PID12_D		 SUM PID数据帧4
PID5 AAAA 14 18 int16 PID13_P
int16 PID13_I
int16 PID13_D
int16 PID14_P
int16 PID14_I
int16 PID14_D
int16 PID15_P
int16 PID15_I
int16 PID15_D		 SUM PID数据帧5
PID6 AAAA 15 18 int16 PID16_P
int16 PID16_I
int16 PID16_D
int16 PID17_P
int16 PID17_I
int16 PID17_D
int16 PID18_P
int16 PID18_I
int16 PID18_D		 SUM PID数据帧6
AAAA 16 LEN SUM
AAAA 17 LEN SUM
AAAA LEN SUM
AAAA LEN SUM
FP_NUM AAAA 20 LEN uing16 FP_NUM SUM 飞控已有航点数量
FP AAAA 21 LEN 参见数据定义->航点信息 SUM 航点信息
AAAA LEN SUM
AAAA LEN SUM
AAAA LEN SUM
AAAA LEN SUM
AAAA LEN SUM
AAAA LEN SUM
MSG AAAA EE 7 uint8_t MSG_ID
uint8_t MSG_DATA		 SUM MSG_ID: 01:加速度 02:陀螺仪 03:罗盘 MSG_DATA: 01:校准成功 E1:校准失败
CHECK AAAA EF 7 uint8 FREAM_HEAD 帧头
uint8 CHECK_SUM 和校验		 SUM 比如要返回PID1数据帧的校验数据，FREAM_HEAD=0x10，CHECK_SUM=接收到PID1数据帧计算出的SUM
AAAA F0 LEN SUM
USER_DATA AAAA F1 LEN 自定义 SUM 用户数据
USER_DATA AAAA F2 LEN 自定义 SUM 用户数据
USER_DATA AAAA F3 LEN 自定义 SUM 用户数据
USER_DATA AAAA F4 LEN 自定义 SUM 用户数据
USER_DATA AAAA F5 LEN 自定义 SUM 用户数据
USER_DATA AAAA F6 LEN 自定义 SUM 用户数据
USER_DATA AAAA F7 LEN 自定义 SUM 用户数据
USER_DATA AAAA F8 LEN 自定义 SUM 用户数据
USER_DATA AAAA F9 LEN 自定义 SUM 用户数据
USER_DATA AAAA FA LEN 自定义 SUM 用户数据
上位机（遥控）->飞控
帧 帧头 功能字 长度 数据 校验 返回校验 备注
CONMAND AAAF 01 1 uint8 CMD1 SUM YES 命令集合1 01：ACC校准 02：GYRO校准 04：MAG校准 05：BARO校准 20：退出6面校准 21：6面校准第1步 22：6面校准第2步 23：6面校准第3步 24：6面校准第4步 25：6面校准第5步 26：6面校准第6步 A0：飞控锁定（仅用于手机蓝牙控制） A1：飞控解锁（仅用于手机蓝牙控制）
ACK AAAF 02 1 uint8 CMD2 SUM YES 命令集合2 01：读取PID请求（返回AAAA 10\11\12\13\14\15数据帧） 02：读取飞行模式设置请求（返回AAAA 0A数据帧） 21：读取飞控内航点数量（返回AAAA 20数据帧） A0：读取下位机版本信息（返回AAAA 00数据帧） A1：恢复默认参数
RCDATA AAAF 03 20 int16 THR
int16 YAW
int16 ROL
int16 PIT
int16 AUX1
int16 AUX2
int16 AUX3
int16 AUX4
int16 AUX5
int16 AUX6		 SUM NO 飞行控制数据(仅用于微型飞机，无刷飞机请用航模遥控控制）
AAAF 04 LEN SUM NO
AAAF 05 LEN SUM NO
AAAF 06 LEN SUM NO
AAAF 07 LEN SUM NO
AAAF 08 LEN SUM NO
AAAF 09 LEN SUM NO
FLY MODEL AAAF 0A LEN int8 AUX1_LOW_MODEL
int8 AUX1_MID_MODEL
int8 AUX1_HIG_MODEL
int8 AUX2_LOW_MODEL
int8 AUX2_MID_MODEL
int8 AUX2_HIG_MODEL
int8 AUX3_LOW_MODEL
int8 AUX3_MID_MODEL
int8 AUX3_HIG_MODEL		 SUM YES 飞行模式
AAAF 0B LEN SUM
AAAF 0C LEN SUM
AAAF 0D LEN SUM
AAAF 0E LEN SUM
AAAF 0F LEN SUM
PID1 AAAF 10 18 int16 PID1_P
int16 PID1_I
int16 PID1_D
int16 PID2_P
int16 PID2_I
int16 PID2_D
int16 PID3_P
int16 PID3_I
int16 PID3_D		 SUM YES PID数据帧1到6 共有18组PID，每3组PID组成一个PID数据帧，所以共6个PID数据帧 上位机会对每个PID数据帧内的数据进行判断，如果都是65535的默认值，则会跳过发送该PID数据帧
PID2 AAAF 11 18 int16 PID4_P
int16 PID4_I
int16 PID4_D
int16 PID5_P
int16 PID5_I
int16 PID5_D
int16 PID6_P
int16 PID6_I
int16 PID6_D		 SUM YES
PID3 AAAF 12 18 int16 PID7_P
int16 PID7_I
int16 PID7_D
int16 PID8_P
int16 PID8_I
int16 PID8_D
int16 PID9_P
int16 PID9_I
int16 PID9_D		 SUM YES
PID4 AAAF 13 18 int16 PID10_P
int16 PID10_I
int16 PID10_D
int16 PID11_P
int16 PID11_I
int16 PID11_D
int16 PID12_P
int16 PID12_I
int16 PID12_D		 SUM YES
PID5 AAAF 14 18 int16 PID13_P
int16 PID13_I
int16 PID13_D
int16 PID14_P
int16 PID14_I
int16 PID14_D
int16 PID15_P
int16 PID15_I
int16 PID15_D		 SUM YES
PID6 AAAF 15 18 int16 PID16_P
int16 PID16_I
int16 PID16_D
int16 PID17_P
int16 PID17_I
int16 PID17_D
int16 PID18_P
int16 PID18_I
int16 PID18_D		 SUM YES
AAAF 16 LEN SUM
AAAF 17 LEN SUM
AAAF LEN SUM
AAAF LEN SUM
READ_FP AAAF 20 LEN uint16 FP_CNT SUM 读取第CNT个航点信息
FP AAAF 21 LEN 参见数据定义->航点信息 SUM YES 写入航点信息
AAAF LEN SUM
AAAF LEN SUM
AAAF LEN SUM
AAAF LEN SUM
AAAF LEN SUM
CAM CONTROL AAAF 80 13 uint8 OBJ1_OK =1可用=0不可用 uint16 OBJ1_X uint16 OBJ1_Y uint8 OBJ2_OK uint16 OBJ2_X uint16 OBJ2_Y uint8 ALT_OK uint16 ALT SUM NO 摄像头控制数据 OBJ1用于定位 OBJ2用于计算航向 ALT为高度数据
AAAF LEN SUM
AAAF LEN SUM
IAP AAAF F0 LEN uint16 BOOT_STA SUM NO 立即进入IAP功能： BOOT_STA=0xFFFA
AAAF LEN SUM
AAAF LEN SUM