ACfly传感器二次开发（包含TFmini,光流驱动文件的分析解读）

我看了下ACfly传感器二次开发的视频，感觉如果要用ACfly做SLAM，首先要自己写个驱动文件，串口驱动就可以，我估计应该有现成的。

然后是传感器的注册，

关键还是我怎么把位置坐标通过串口发给STM32，这个是关键，具体波特率的设置，发送频率的设置，格式，我在树莓派上或者TX2上是要写个什么程序呢，可执行程序么，然后让它死循环跑？

接uart3口，而且好像有例程

我仔细找了下，应该是drv_SDI.c这个文件，就在ACFLY的工程里面，就是UART3的

分为传感器注册函数和传感器更新函数。更新函数是写在传感器驱动里面的。

（我的理解，你清楚了ACfly的基本逻辑之后就觉得很正常了，ACfly是自己自动进行传感器的选择，根据传感器数据的好坏，你首先注册传感器让它（那个不开源的模块）知道有哪些传感器，这样才能进行选择嘛，要对传感器进行选择还得对传感器数据做好坏判断，acfly似乎也叫异常检测？或者包含异常检测，这个基本前提就是有数据嘛，这时就是数据更新函数了，把传感器数据给那个不开源的模块它会自动检测判断这些传感器的数据，最后选择用什么传感器）

注册函数的原函数实现是在sensors.cpp里面，更新函数的原函数实现是在sensors.cpp里

注册函数和更新函数的声明在sensorsbackend.hpp这个头文件里，具体传感器驱动文件里面调用注册函数和更新函数只需要包含这个头文件就可以了，实际就是这样的，就不需要再单独声明一遍了。

他在用户手册里面也说了sensorsbackend.hpp里写的传感器注册和更新的声明。

比如这是光流驱动文件，里面只include了sensorsbackend.hpp而没有include   sensors.hpp，再次说明了这一点

注册函数的调用是写在具体的传感器驱动里面（这也可以解释为什么之前看传感器驱动文件每个文件里面都会有个注册函数的调用）。

更新函数调用是写在具体的传感器驱动里面。是好像先驱动获得传感器数据，然后用更新函数把这个数据给飞控处理。

也就是注册函数和更新函数的具体调用，也就是往里面传入具体参数，都是放在具体传感器的驱动文件里面的。

现在整个就可以理得比较清晰了。所以我们做传感器二次开发的工作其实就是写那个驱动文件，并且在驱动文件里面调用传感器注册函数和传感器更新函数。当然ACfly它是把串口驱动和传感器驱动分开写的，传感器驱动调用串口驱动的API就行，也就是串口驱动不需要我们写了，关键还是写除开串口驱动部分的传感器驱动。你至少得把串口传过来的数据整理出那个是x  哪个是y  哪个是z，对吧。

注册函数和更新函数的声明写在sensorbackend.h里面

注册函数的声音也要写在驱动文件里面（不是声明，就是调用应该，声明都是放在sensorsbackend.h头文件里面的，驱动文件只需要调用这个头文件就行了，就不需要单独声明一遍了，这本身也是头文件的作用），而且都是写在传感器初始化函数里面，就是每个传感器的驱动文件里面的init函数

下面这个再次说明我的理解是没错的，确实就是协议解析。

还有是可以普通串口通信还是必须说明mavlink协议？应该普通串口就可以了，你像超声波模块不可能带有什么mavlink协议，人家还以用在小车上，又不是只用在无人机上面。应该一个uart口就可以了。自己写个uart口的驱动。

看这是GPS的驱动文件，里面用了更新函数，更新函数的第二个输入是position，而position的值是GPS得到的数据赋的，分别赋给position.x  position.y  position.z    这个更新函数应该就是把这些值给飞控进行进一步处理，为什么要这么多此一举呢，可能是为了有统一接口，方便用户二次开发。（这里我现在可以加上一句话就是它 传感器数据好坏的判断，传感器的选择这部分是没有开源的！！！！多次一举可能是为了便于它不开源。。。可以见我这篇博文：https://blog.csdn.net/sinat_16643223/article/details/108899072）

超声波的也是先把超声波的数据赋给position.z  再用传感器更新函数。

光流传感器的驱动文件，把光流得到的X和Y方向的速度赋给vel.x       vel.y            vel再作为参数传入到传感器更新函数里面。

北醒的激光雷达的驱动文件，也是一样。

一个传感器它用的哪个串口，就会include哪个uart串口的头文件，你不是想找数据读取的函数么，我觉得应该是在uart的驱动文件里。

像GPS驱动文件有个  Read_Uart8（）   我估计就是

是的去看uart8的驱动文件，里面有专门的串口接收函数。所以传感器的驱动文件其实是调用的串口的驱动文件，所以关键还是那个串口驱动。别人嵌入式重点考察串口协议是有道理的。你会发现你如果要做无人机+SLAM，串口这块反而成了你开发的关键点，其他的都没什么，我感觉你做任何一个嵌入式系统这都是必不可少的，任何一个嵌入式系统肯定有传感器，肯定有串口通信，必不可少啊。这个是必须要掌握的。所以我现在很想自己写一个。所以啊，你弄那么多高大上的项目不如先做好这一个。

我看了下每个传感器驱动文件，一开头都是一个结构体（应该是传感器数据），最后都是一个init的传感器初始化函数，注册函数就是写在初始化函数里面。中间应该就是传感器数据的读取。

传感器初始化函数最后要在drv_Main.cpp里被调用，所以光看ACfly那两个二次开发视频是不够的，像这他就没有讲到，所以我们还是有必要把整个工程的代码逻辑理清楚再去添加自己的传感器及其驱动。

backend是后端的意思。

我感觉还是可以把ACfly整个的代码逻辑理清楚的。这跟pixhawk  ROS   MAVROS的那些代码量还有说明比起来少多了。

背后的逻辑可以看我这篇博文：https://blog.csdn.net/sinat_16643223/article/details/108899072

他这也是分为姿态控制和位置控制的。

sensors.cpp里面是二次开发用到的那些传感器说明注册函数更新函数等等的具体函数体，当然不是某一具体传感器的，是每个传感器都会调用这个函数。

ACfly  1.4版本的用户手册加入了一些代码讲解。

我仔细把TFmini的驱动文件读了下，感觉还好，就是要去把里面每个驱动文件每句话肯明白，这样你再去写自己的驱动自然就清晰了。要真正每句读懂，不要差不多就行了。

我发现它的具体传感器的驱动文件实际是创建一个传感器任务，同时在这个驱动文件里面把这个任务的执行函数也写了，你看到的驱动文件里面的大部分内容其实都是这个执行函数，驱动文件的最后往往就是一个任务创建函数。xTaskCreate( TFMini_Server, "TFMini", 1024, NULL, SysPriority_ExtSensor, NULL);

所以它的传感器初始化函数（放在传感器驱动文件里面）里面就两个东西，第一个传感器注册函数，第二个，传感器任务创建函数，就完了。

我发现下面这部分的程序我还是看懂了的， 得益于之前专门看了华清的串口课，这好像是先一位一位地数据接收，先满八位，然后还看校验位。我觉得你可以拿它练练手，写串口驱动。

我再看发现激光测高的驱动和光流的驱动写法基本一样，都是用的uart ，然后都要读取什么包头校验位什么的，是不是uart这方面的程序都是这么写的。

我现在发现需要结合TFmini的datesheet来看，不然你是不能完全看明白的，结合TFmini的datesheet来看就能彻底看明白了！！！！！！！！！而且你网上搜什么TFmini的驱动程序，实际上是搜不到什么的，TFmini官方给的就datesheet，其实它的datesheeet也不长，就几页。

https://www.dfrobot.com.cn/images/upload/File/20180228162359unvz61.pdf

看了下面这个，你就清楚，为什么

static const unsigned char packet_ID[2] = { 0x59 , 0x59 };

为什么rc_counter<8      为什么rc_counter-2

现在让我感受到写驱动的关键是要看datesheet啊，我估计光流的驱动也是的，就算是uart的驱动也是要去看STM32的手册，它相应寄存器的定义。

#include "drv_Uart7.hpp"
#include "Basic.hpp"
#include "FreeRTOS.h"
#include "task.h"
#include "SensorsBackend.hpp"
#include "MeasurementSystem.hpp"

#define SensorInd 2

//maxi：结合TFmini的datesheet来看就很多明白了！！！！！！

typedef struct
{
	int16_t Dist;	//Dist距离（30-1200cm）
	int16_t Strength;	//Strength信号强度（20-2000可信）
	uint8_t Mode;	//Mode测距档位
	uint8_t Rsv;	//预留
}__PACKED _TfMini;
static const unsigned char packet_ID[2] = { 0x59 , 0x59 };

static void TFMini_Server(void* pvParameters)
{
	/*状态机*/
		_TfMini  SensorD;     //maxi：注意SensorD在这里被定义，就是上面的结构体
		unsigned char rc_counter = 0;
		unsigned char sum = 0;
	/*状态机*/
	
	while(1)
	{
		uint8_t rdata;
		if( Read_Uart7( &rdata, 1, 2, 0.5 ) )  //mx这里用到了uart函数
		{
			if( rc_counter == 0 )
				sum = 0;
			if( rc_counter < 2 )
			{
				//接收包头
				if( rdata != packet_ID[ rc_counter ] )
					rc_counter = 0;
				else
				{
					sum += rdata;
					++rc_counter;
				}
			}
			else if( rc_counter < 8 )
			{	//接收数据
				( (unsigned char*)&SensorD )[ rc_counter - 2 ] = rdata;    //maxi:这里可能就是实际在接收数据，因为SensorD就是激光测高的数据结构体，下面赋值也是用它。
				sum += (unsigned char)rdata;
				++rc_counter;
			}
			else
			{	//校验
				if( sum == rdata )
				{	//校验成功
					if( SensorD.Strength>20 && SensorD.Strength<3000 && SensorD.Dist>30 && SensorD.Dist<1200 )  //maxi:这里是判断信号强度和高度是不是在区间范围内。
					{
						vector3<double> position;
						position.z = SensorD.Dist;   //maxi:SensorD也是最上面定义的激光测高的结构体
						//获取倾角
						Quaternion quat;
						get_Airframe_quat( &quat );
						double lean_cosin = quat.get_lean_angle_cosin();
						//更新
						position.z *= lean_cosin;
						PositionSensorUpdatePosition( SensorInd, position, true );
					}
					else
						PositionSensorSetInavailable( SensorInd );
				}
				rc_counter = 0;
			}
		}
	}
}

void init_drv_TFMini()
{
	//波特率115200
	SetBaudRate_Uart7( 115200, 2, 2 );
	//注册传感器
	bool res = PositionSensorRegister( SensorInd , 
																			Position_Sensor_Type_RangePositioning , 
																			Position_Sensor_DataType_s_z , 
																			Position_Sensor_frame_ENU , 
																			0.05 , //延时
																			0 ,	//xy信任度
																			0 //z信任度
																			);
	if(res)
		xTaskCreate( TFMini_Server, "TFMini", 1024, NULL, SysPriority_ExtSensor, NULL);
}

这是优象光流给的，根据这个应该也是可以写优象光流的驱动了。

我感觉这么一来你也有了一些驱动的开发经验，这种感觉倒是挺棒的，不光是串口驱动，还有一些基本传感器的驱动。

也怪不得ACfly对写光流驱动那么自信。

下面是ACfly的优象光流的驱动代码

#include "drv_Uart5.hpp"
#include "Basic.hpp"
#include "FreeRTOS.h"
#include "task.h"
#include "SensorsBackend.hpp"
#include "MeasurementSystem.hpp"

#define SENSOR_IIC_ADDR 0xdc
const static uint8_t tab_focus[4] = {0x96,0x26,0xbc,0x50};
const static uint8_t Sensor_cfg[]={
//地址, 数据
0x12, 0x80, 
0x11, 0x30, 
0x1b, 0x06, 
0x6b, 0x43, 
0x12, 0x20, 
0x3a, 0x00, 
0x15, 0x02, 
0x62, 0x81, 
0x08, 0xa0, 
0x06, 0x68, 
0x2b, 0x20, 
0x92, 0x25, 
0x27, 0x97, 
0x17, 0x01, 
0x18, 0x79, 
0x19, 0x00, 
0x1a, 0xa0, 
0x03, 0x00, 
0x13, 0x00, 
0x01, 0x13, 
0x02, 0x20, 
0x87, 0x16, 
0x8c, 0x01, 
0x8d, 0xcc, 
0x13, 0x07, 
0x33, 0x10, 
0x34, 0x1d, 
0x35, 0x46, 
0x36, 0x40, 
0x37, 0xa4, 
0x38, 0x7c, 
0x65, 0x46, 
0x66, 0x46, 
0x6e, 0x20, 
0x9b, 0xa4, 
0x9c, 0x7c, 
0xbc, 0x0c, 
0xbd, 0xa4, 
0xbe, 0x7c, 
0x20, 0x09, 
0x09, 0x03, 
0x72, 0x2f, 
0x73, 0x2f, 
0x74, 0xa7, 
0x75, 0x12, 
0x79, 0x8d, 
0x7a, 0x00, 
0x7e, 0xfa, 
0x70, 0x0f, 
0x7c, 0x84, 
0x7d, 0xba, 
0x5b, 0xc2, 
0x76, 0x90, 
0x7b, 0x55, 
0x71, 0x46, 
0x77, 0xdd, 
0x13, 0x0f, 
0x8a, 0x10, 
0x8b, 0x20, 
0x8e, 0x21, 
0x8f, 0x40, 
0x94, 0x41, 
0x95, 0x7e, 
0x96, 0x7f, 
0x97, 0xf3, 
0x13, 0x07, 
0x24, 0x58, 
0x97, 0x48, 
0x25, 0x08, 
0x94, 0xb5, 
0x95, 0xc0, 
0x80, 0xf4, 
0x81, 0xe0, 
0x82, 0x1b, 
0x83, 0x37, 
0x84, 0x39, 
0x85, 0x58, 
0x86, 0xff, 
0x89, 0x15, 
0x8a, 0xb8, 
0x8b, 0x99, 
0x39, 0x98, 
0x3f, 0x98, 
0x90, 0xa0, 
0x91, 0xe0, 
0x40, 0x20, 
0x41, 0x28, 
0x42, 0x26, 
0x43, 0x25, 
0x44, 0x1f, 
0x45, 0x1a, 
0x46, 0x16, 
0x47, 0x12, 
0x48, 0x0f, 
0x49, 0x0d, 
0x4b, 0x0b, 
0x4c, 0x0a, 
0x4e, 0x08, 
0x4f, 0x06, 
0x50, 0x06, 
0x5a, 0x56, 
0x51, 0x1b, 
0x52, 0x04, 
0x53, 0x4a, 
0x54, 0x26, 
0x57, 0x75, 
0x58, 0x2b, 
0x5a, 0xd6, 
0x51, 0x28, 
0x52, 0x1e, 
0x53, 0x9e, 
0x54, 0x70, 
0x57, 0x50, 
0x58, 0x07, 
0x5c, 0x28, 
0xb0, 0xe0, 
0xb1, 0xc0, 
0xb2, 0xb0, 
0xb3, 0x4f, 
0xb4, 0x63, 
0xb4, 0xe3, 
0xb1, 0xf0, 
0xb2, 0xa0, 
0x55, 0x00, 
0x56, 0x40, 
0x96, 0x50, 
0x9a, 0x30, 
0x6a, 0x81, 
0x23, 0x33, 
0xa0, 0xd0, 
0xa1, 0x31, 
0xa6, 0x04, 
0xa2, 0x0f, 
0xa3, 0x2b, 
0xa4, 0x0f, 
0xa5, 0x2b, 
0xa7, 0x9a, 
0xa8, 0x1c, 
0xa9, 0x11, 
0xaa, 0x16, 
0xab, 0x16, 
0xac, 0x3c, 
0xad, 0xf0, 
0xae, 0x57, 
0xc6, 0xaa, 
0xd2, 0x78, 
0xd0, 0xb4, 
0xd1, 0x00, 
0xc8, 0x10, 
0xc9, 0x12, 
0xd3, 0x09, 
0xd4, 0x2a, 
0xee, 0x4c, 
0x7e, 0xfa, 
0x74, 0xa7, 
0x78, 0x4e, 
0x60, 0xe7, 
0x61, 0xc8, 
0x6d, 0x70, 
0x1e, 0x39, 
0x98, 0x1a
};

typedef struct
{
	int16_t flow_x_integral;	// X 像素点累计时间内的累加位移(radians*10000)
														// [除以 10000 乘以高度(mm)后为实际位移(mm)]
	int16_t flow_y_integral;	// Y 像素点累计时间内的累加位移(radians*10000)
														// [除以 10000 乘以高度(mm)后为实际位移(mm)]
	uint16_t integration_timespan;	// 上一次发送光流数据到本次发送光流数据的累计时间（us）
	uint16_t ground_distance; // 预留。 默认为 999（0x03E7）
	uint8_t valid;	// 状态值:0(0x00)为光流数据不可用
									//245(0xF5)为光流数据可用
	uint8_t version; //版本号
}__PACKED _Flow;
static const unsigned char packet_ID[2] = { 0xfe , 0x0a };

static void OpticalFlow_Server(void* pvParameters)
{
	/*状态机*/
		_Flow  Flow;
		unsigned char rc_counter = 0;
		signed char sum = 0;
	/*状态机*/
	
	while(1)
	{
		uint8_t rdata;
		if( Read_Uart5( &rdata, 1, 2, 0.5 ) )
		{
			if( rc_counter < 2 )
			{
				//接收包头
				if( rdata != packet_ID[ rc_counter ] )
					rc_counter = 0;
				else
				{
					++rc_counter;
					sum = 0;
				}
			}
			else if( rc_counter < 12 )
			{	//接收数据
				( (unsigned char*)&Flow )[ rc_counter - 2 ] = rdata;
				sum ^= (signed char)rdata;
				++rc_counter;
			}
			else if( rc_counter == 12 )
			{	//校验
				if( sum != rdata )
					rc_counter = 0;
				else
					++rc_counter;
			}
			else
			{	//接收包尾
				if( rdata == 0x55 )
				{
					PosSensorHealthInf1 ZRange_inf;
					if( get_OptimalRange_Z( &ZRange_inf ) )
					{	//测距传感器可用
						if( ZRange_inf.last_healthy_TIME.is_valid() && ZRange_inf.last_healthy_TIME.get_pass_time() < 50 )
						{	//测距50秒内健康
							//获取高度
							double height = ZRange_inf.HOffset + ZRange_inf.PositionENU.z;
							//获取角速度
							vector3<double> AngularRate;
							get_AngularRate_Ctrl( &AngularRate );
							//补偿光流
							vector3<double> vel;
							double rotation_compensation_x = -constrain( AngularRate.y * 10000 , 4500000000.0 );
							double rotation_compensation_y = constrain(  AngularRate.x * 10000 , 4500000000.0 );
							double integral_time = (Flow.integration_timespan * 1e-6f);
							if( integral_time > 1e-3 )
							{
								integral_time = 1.0 / integral_time;
								vel.x = ( (double)Flow.flow_x_integral*integral_time - rotation_compensation_x ) * 1e-4f * ( 1 + height );     //maxi:注意这里面的Flow就是最上面定义的光流数据的结构体
								vel.y = ( -(double)Flow.flow_y_integral*integral_time - rotation_compensation_y ) * 1e-4f * ( 1 + height ) ;
								PositionSensorUpdateVel( default_optical_flow_index , vel , true );
							}
							else
								PositionSensorSetInavailable( default_optical_flow_index );
						}
						else
							PositionSensorSetInavailable( default_optical_flow_index );
					}
					else
						PositionSensorSetInavailable( default_optical_flow_index );
				}
				rc_counter = 0;
			}
			
		}
	}
}

void init_drv_OpticalFlow()
{
	//波特率19200
	SetBaudRate_Uart5( 19200, 2, 2 );
	
//	uint8_t buf[30];
//	uint16_t len = sizeof(Sensor_cfg);
//	
//	//0xAA指令
//	buf[0] = 0xAA;
//	Write_Uart5( buf, 1, 1, 1 );

//	//0xAB指令				
//	buf[0] = 0xAB;
//	Write_Uart5( buf, 1, 1, 1 );	
//	Write_Uart5( tab_focus, 4, 1, 1 );
//	buf[0] = tab_focus[0]^tab_focus[1]^tab_focus[2]^tab_focus[3];
//	Write_Uart5( buf, 1, 1, 1 );			 
//	os_delay(0.01);
//			
//	//0xBB指令							
//	for(uint16_t i=0; i<len;i+=2 )
//	{
//		buf[0] = 0xBB;
//		buf[1] = SENSOR_IIC_ADDR;
//		buf[2] = Sensor_cfg[i];
//		buf[3] = Sensor_cfg[i+1];
//		buf[4] = SENSOR_IIC_ADDR^Sensor_cfg[i]^Sensor_cfg[i+1];
//		Write_Uart5( buf, 5, 1, 1 );	
//		 
//		os_delay(0.01);						 
//	}
//	 
//	//0xDD			 
//	buf[0] = 0xDD;
//	Write_Uart5( buf, 1, 1, 1 );	
	
	//注册传感器
	PositionSensorRegister( default_optical_flow_index , 
													Position_Sensor_Type_RelativePositioning , 
													Position_Sensor_DataType_v_xy , 
													Position_Sensor_frame_BodyHeading , 
													0.1 );
	xTaskCreate( OpticalFlow_Server, "OpticalFlow", 1024, NULL, SysPriority_ExtSensor, NULL);
}

无名就是用的LC306，他的光流驱动文件里面就有这一列地址和数据

继续写一些发现，那几个UART的驱动文件似乎都是一样，除了名字后面多个数字之外

所以我做二次开发，UART部分的程序应该是不需要我写的，我应该只需要把串口名字改改就可以了，如果有现成的串口可能就直接用了。

位置更新函数的具体实现在sensors.cpp里，你看懂就OK了，知道最后把位置的值给谁了。

最后

以上就是大方保温杯为你收集整理的ACfly传感器二次开发（包含TFmini,光流驱动文件的分析解读）的全部内容，希望文章能够帮你解决ACfly传感器二次开发（包含TFmini,光流驱动文件的分析解读）所遇到的程序开发问题。

如果觉得靠谱客网站的内容还不错，欢迎将靠谱客网站推荐给程序员好友。