添加吊仓驱动

（1）Firmware/src/examples里面添加yuntai8.cpp

#include <stdio.h>
#include <termios.h>
#include <unistd.h>
#include <stdbool.h>
#include <errno.h>
// #include <drivers/drv_hrt.h>
#include <fcntl.h>
#include <sys/types.h>
#include <sys/stat.h>
// #include <px4_platform_common/tasks.h>
// #include <px4_platform_common/posix.h>

#include <string.h>
#include <pthread.h>
#include <stdlib.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/posix.h>
#include <uORB/topics/a_D90.h>
#include <uORB/Publication.hpp>
//#include <uORB/PublicationQueued.hpp>
#include <uORB/Subscription.hpp>



extern "C" __EXPORT int b_main(); // 主函数

a_D90_s a_D90;//实例化read_uart_sensor变量，该变量是自己定义的变量
uORB::Publication <a_D90_s>              _a_D90_pub{ORB_ID(a_D90)};//发布无人机状态；

int readSize = 0;
// int toInt(unsigned char lowIndex[8],unsigned char highIndex[8]){
// char a[16]="0";//用一个数组存储十六进制数
// for(int i=0;i<8;i++){
// 	a[i] =
// }
// for(int i=8;i<16;i++){
// 	a[i] =
// }
//  	char b[100];//储存输入的十进制数对应的十六进制数
// 	int n;
// 	int cnt;//计数

// 	scanf("%d",&n);//输入要转换的十进制数

// 	if(n==0)
// 		printf("0");
// 	while(n!=0)
// 	{
// 		b[++cnt]=a[n%16];//这里cnt从1开始
// 		n=n/16;
// 	 }

// 	for(int i=cnt;i>0;i--)/*上面cnt从1开始所以i>0，不用输出b[0]。逆序输出*/
// 		printf("%c",b[i]);
// }


// pthread_cond_t cond = PTHREAD_COND_INITIALIZER;
// pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
unsigned char send_data[] = {0xA8, 0xE5, 0x48,
				   0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
				   0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
				   0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
				   0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
				   0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
				   0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
				   0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0X8A, 0X89};

int send_frequency = 1;						   // 50Hz
int send_period_us = 1000000 / send_frequency; // 微秒级



void dealwithData(uint32_t* result_values,double* result_data,unsigned char* recv_buffer){
	// uint32_t reversed_value = 0;
	// printf("-------------------------------------\n");
	// printf("recv_buffer[20] = %02X\n", recv_buffer[20]);
	// printf("recv_buffer[21] = %02X\n", recv_buffer[21]);
	// printf("recv_buffer[22] = %02X\n", recv_buffer[22]);
	// printf("recv_buffer[23] = %02X\n", recv_buffer[23]);
	// printf("-------------------------------------\n");

	double fuyang;
	double pianhang;
	fuyang = (recv_buffer[21]*256 + recv_buffer[20])*0.01;
	if(fuyang>360){
		fuyang = fuyang -655.35;
	}
	pianhang = (recv_buffer[23]*256 + recv_buffer[22])*0.01;
	// printf("-------------------------------------\n");
	// printf("fufang = %.2f\n", fuyang);
	// printf("pianhang = %.2f\n", pianhang);
	// printf("-------------------------------------\n");


	// for(int i=0;i<72;i++){
	// 	printf("%02X ",recv_buffer[i]);
	// }
	// printf("\n");
	// result_values[0] = (recv_buffer[43] << 24) | (recv_buffer[44] << 16) | (recv_buffer[45] << 8) | recv_buffer[46];//distance
	// result_values[1] = (recv_buffer[47] << 24) | (recv_buffer[48] << 16) | (recv_buffer[49] << 8) | recv_buffer[50];//lat
	// result_values[2] = (recv_buffer[51] << 24) | (recv_buffer[52] << 16) | (recv_buffer[53] << 8) | recv_buffer[54];//lon
	// result_values[3] = (recv_buffer[55] << 24) | (recv_buffer[56] << 16) | (recv_buffer[57] << 8) | recv_buffer[58];//alt

	// // 反转每个值中的字节顺序
	// for (int i = 3; i < 7; ++i)
	// {

	// for (int j = 0; j < 4; ++j)
	// {
	// 	reversed_value |= ((result_values[i] >> (j * 8)) & 0xFF) << ((3 - j) * 8);
	// }
	// result_values[i] = reversed_value;
	// }

	// for (int i = 0; i < 3; ++i)
	// {
	// result_data[i] = result_values[i] * 0.01;
	// }
	// result_data[0] = result_values[0] ;
	// result_data[1] = result_values[1] ;
	// result_data[2] = result_values[2] ;
	// result_data[3] = result_values[3] ;
	// result_data[0] = result_values[0] * 0.1;
	// result_data[1] = result_values[1] * 0.0000001;
	// result_data[2] = result_values[2] * 0.0000001;
	// result_data[3] = result_values[3] * 0.001;

	a_D90.result_values0=fuyang;
	a_D90.result_values1=pianhang;
	// a_D90.result_values2=result_values[2];
	// a_D90.result_data0=result_data[0];
	// a_D90.result_data1=result_data[1];
	// a_D90.result_data2=result_data[2];
	// a_D90.result_data3=result_data[3];

	// // printf("distance:%3.6f\n", double(result_data[0]));
	// // printf("lat:%3.7f\n", double(result_data[1]));
	// // printf("lon:%3.7f\n", double(result_data[2]));
	// // printf("alt:%3.6f\n", double(result_data[3]));
	_a_D90_pub.publish(a_D90);

}


void nread(int serial_port_fd, unsigned char* recv_buffer){

	unsigned char temp_buffer;
	// memset(&temp_buffer,0,sizeof(temp_buffer));
	while(1){
				if(readSize == 72 ){
					readSize = 0;
					return;
					// dealwithData(result_values,result_data,recv_buffer);
				}
				int ret = read(serial_port_fd, &temp_buffer, sizeof(temp_buffer));
				if(ret<0){
					printf("read error!\n");
					return;
				}
				recv_buffer[readSize++] = temp_buffer;
	}
}
// 发送数据函数
void *sendData(void *serial_port)
{
	sleep(1);
	int serial_port_fd = *(int *)serial_port;
	while(1){
	int bytes_written = write(serial_port_fd, send_data, sizeof(send_data));
	if (bytes_written < 0)
			{
				perror("写入串口失败\n");
				return (void *)-1;
			}
	usleep(send_period_us); // 控制发送频/
	}
}
void *receiveData(void *serial_port)
{

	// unsigned char *recv_buffer = (unsigned char *)malloc(sizeof(unsigned char) * 512);
	unsigned char recv_buffer[128];


	uint32_t result_values[7]; // 存储提取结果的数组
        double result_data[7];//处理后结果

	memset(recv_buffer,0,sizeof(recv_buffer));
	int serial_port_fd = *(int *)serial_port;

	// int count =0;

	// while(1)
	// 	{
	// 		int ret = read(serial_port_fd,recv_buffer,sizeof(recv_buffer));
	// 		for(int i=0;i<ret;i++){
	// 			printf("%02X ",recv_buffer[i]);
	// 			count++;
	// 			if(count % 72 == 0)
	// 				printf("\n");
	// 		}
	// 	}

	while (1)
	{
			nread(serial_port_fd, recv_buffer);
			dealwithData(result_values,result_data,recv_buffer);
	}
	return (void *)0;
}
int b_main()
{

	// 打开串口
	//	serial_port = open("/dev/ttyUSB0", O_RDWR|O_NOCTTY  |O_DIRECT );
	int serial_port = open("/dev/ttyS2", O_RDWR| O_NOCTTY | O_DIRECT );
	if (serial_port < 0)
	{
		printf("无法打开串口\n");
		return -1;
	}

	else
	{
		printf("打开串口成功\n");
	}

	// 配置串口参数
	struct termios tty;
	memset(&tty, 0, sizeof(tty));
	if (tcgetattr(serial_port, &tty) != 0)
	{
		printf("无法获取串口参数\n");
		close(serial_port);
		return -1;
	}
	else
	{
		printf("获取串口参数成功\n");
	}

	tty.c_cflag &= ~PARENB; // 无校验位
	tty.c_cflag &= ~CSTOPB; // 1位停止位
	tty.c_cflag |= CS8;		// 8位数据位

	tty.c_cflag &= ~CRTSCTS;						// 禁用硬件流控制
	tty.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); /*Input LOCAL*/

	tty.c_oflag &= ~OPOST; /*Output*/

	tty.c_iflag &= ~(IXON | IXOFF | IXANY | BRKINT | ICRNL | ISTRIP);

	tcflush(serial_port,TCIFLUSH);

	cfsetospeed(&tty, B115200); // 设置波特率为115200
	cfsetispeed(&tty, B115200);

	// 应用配置
	if (tcsetattr(serial_port, TCSANOW, &tty) != 0)
	{
		printf("无法应用串口参数\n");
		close(serial_port);
		return -1;
	}
	else
	{
		printf("应用串口参数成功\n");
	}


	// sleep(1);
	pthread_t send_pthread;
	pthread_t recev_pthread;

	pthread_create(&send_pthread, NULL, sendData, &serial_port);
	pthread_create(&recev_pthread, NULL, receiveData, &serial_port);

	printf("before join!!\n");
	pthread_join(send_pthread, NULL);
	pthread_join(recev_pthread, NULL);

	printf("after join!!\n");
	close(serial_port);
	return 0;
}

（2）修改Firmware/src/examples/CMakeLists.txt文件

SourceURL:file://文字文稿1

（3）在Firmware/boards/cuav/x7pro/default.px4board文件里面添加

        CONFIG_EXAMPLES_PX4_SIMPLE_APP=y

(4)在Firmware/ROMFS/px4fmu_common/init.d/rcs文件里面添加b start