ACFly A9飞控使用T265作为位置传感器

使用一块Jetson nano读取T265数据发送给飞控代替光流实现室内定点飞行。

1.Jetson nano数据发送

sudo apt-get install ros-melodic-serial
cd ros_ws/src
catkin_create_pkg fcu_serial std_msgs roscpp serial
cd fcu_serial/src
gedit t265_serial_node.cpp

输入如下代码：

#include 
#include 
#include 


void twist_call_back(const nav_msgs::Odometry::ConstPtr& odom_msg, int* vel_x, int* vel_y, bool* rc_flag)
{
	*vel_x = odom_msg->twist.twist.linear.x * 100;
	*vel_y = odom_msg->twist.twist.linear.y * 100;
	*rc_flag = true;
}

int main (int argc, char** argv)
{
	ros::init(argc, argv, "t265_serial_node");
	ros::NodeHandle nh;
	ros::Rate loop_rate(30);

	serial::Serial fcu_serial;
	int vel_x,vel_y;
	bool rc_flag = false;

	ros::Subscriber t265_sub = nh.subscribe
				("/camera/odom/sample",10,boost::bind(&twist_call_back,_1,&vel_x,&vel_y,&rc_flag));

	fcu_serial.setPort("/dev/ttyUSB0");
	fcu_serial.setBaudrate(115200);
	serial::Timeout to = serial::Timeout::simpleTimeout(1000);
	fcu_serial.setTimeout(to);

	try
	{
		//sudo chmod 777 /dev/ttyUSB0
		fcu_serial.open();
	}
	catch(const serial::IOException& e)
	{
		ROS_INFO_STREAM("Failed to open serial");
		return -1;
	}

	if(fcu_serial.isOpen())
		ROS_INFO_STREAM("serial opened");
	else
		return -1;
	

	while(ros::ok())
	{
		char str[20];
		sprintf(str,"a+000+000b");

		if(rc_flag)
		{
			vel_x >= 0 ? str[1] = '+' : (str[1] = '-') && (vel_x *= -1);
			vel_y >= 0 ? str[5] = '+' : (str[5] = '-') && (vel_y *= -1);

			str[2] = vel_x / 100 + 48;
			str[3] = (vel_x % 100) / 10 + 48;
			str[4] = (vel_x % 100) % 10 + 48;

			str[6] = vel_y / 100 + 48;
			str[7] = (vel_y % 100) / 10 + 48;
			str[8] = (vel_y % 100) % 10 + 48;

			ROS_INFO_STREAM(str);
			fcu_serial.write(str);
			rc_flag = false;
		}
			

		ros::spinOnce();
		loop_rate.sleep();
	}
}

CMakeList.txt中添加：

add_executable( t265_serial_node src/t265_serial_node.cpp)

编译工作区后控制台输入：

roslaunch realsense2_camera rs_t265.launch
rosrun fcu_serial t265_serial_node

串口便开始下发T265速度数据，如果串口打开失败控制台输入：

sudo chmod 777 /dev/ttyUSB0

我使用的是USB串口，还可以使用nano的8号、10号管脚发送，把程序中的ttyUSB0替换为ttyTHS1即可。

2.飞控数据接收

首先要注册一个位置传感器，在飞控工程中Sensors.cpp第925行左右添加：

const uint8_t default_T265_sensor_index = 10;

Sensors.hpp第80行左右添加

extern const uint8_t default_T265_sensor_index;

Drivers文件夹下新建drv_T265.hpp，内容如下：

#pragma once

void init_drv_T265();

新建drv_T265.cpp，内容如下：

#include "Commulink.hpp"
#include "Basic.hpp"
#include "FreeRTOS.h"
#include "task.h"
#include "SensorsBackend.hpp"
#include "MeasurementSystem.hpp"
#include "Modes.hpp"
#include "drv_T265.hpp"
#include 

struct DriverInfo
{
	uint32_t param;
	Port port;
	uint32_t sensor_key;
};

static void T265_Server(void* pvParameters)
{
	DriverInfo driver_info = *(DriverInfo*)pvParameters;
	delete (DriverInfo*)pvParameters;
	
	static uint32_t T265_RX_STA = 0x00;
	ALIGN4 static uint8_t T265_MSG_PACK[24];
	
	while(1)
	{
		uint8_t Res;
		if( driver_info.port.read( &Res, 1, 2, 0.5 ) )
		{
			//如果没有接受完成（最高位为0
      if((T265_RX_STA & 0x8000) == 0x00 )	
			{
				//如果字符串长度大于10清零
				if((T265_RX_STA & 0x3fff) > 10)
				{		
					for(uint8_t i=0;i<24;i++)	
						T265_MSG_PACK[i]=0;
					T265_RX_STA = 0;							
				}
				//如果之前接收过0x61（第二位为1	
				if((T265_RX_STA & 0x4000) == 0x4000)
				{
					if(Res==0x62)//此次接收到0x62,接收到一次完整数据包
          {
						T265_MSG_PACK[T265_RX_STA&0x3fff]=Res;
						T265_RX_STA++;
            T265_RX_STA |= 0x8000;//第一位置1
          }
          else
          {
						T265_MSG_PACK[T265_RX_STA&0x3fff]=Res;
            T265_RX_STA++;
          }							
				}
				else//之前没有接收过0x61
				{
					if(Res==0x61)//此次接收到0x61
          {
						T265_RX_STA |= 0x4000;//第二位置1
            T265_MSG_PACK[T265_RX_STA&0x3fff]=Res;
            T265_RX_STA++;
					}
				}
			}		
		}
		if(T265_RX_STA & 0x8000)
		{
			uint8_t len = T265_RX_STA & 0x3fff;
			uint8_t i;
			//如果头帧尾帧正确
			if( T265_MSG_PACK[0] == 'a' && T265_MSG_PACK[len-1]== 'b' && len==10 )
			{
				vector3 vel;
				
					//字符串转数字
					#define FirstData  ((T265_MSG_PACK[2]-0x30)*100 + (T265_MSG_PACK[3]-0x30)* 10 + T265_MSG_PACK[4]-0x30)
					#define SecondData ((T265_MSG_PACK[6]-0x30)*100 + (T265_MSG_PACK[7]-0x30)* 10 + T265_MSG_PACK[8]-0x30)
				
				//读取T265数据
				T265_MSG_PACK[1] == '+' ? vel.x = FirstData : vel.x = (-1) * FirstData;
				T265_MSG_PACK[5] == '+' ? vel.y = SecondData : vel.y = (-1) * SecondData;
				
				//传感器数据更新
				PositionSensorUpdateVel( default_T265_sensor_index,driver_info.sensor_key, vel , true );
				
				//清零准备下一次接收
				for(i=0;iparam = param;
	driver_info->port = port;
	driver_info->sensor_key = sensor_key;
	xTaskCreate( T265_Server, "T265", 1024, (void*)driver_info, SysPriority_ExtSensor, NULL);
	return true;
}

void init_drv_T265()
{
	PortFunc_Register( 10, T265_DriverInit );
}

在drv_main.cpp中添加：

#include "drv_T265.hpp"
init_drv_T265();

烧录程序之后在地面站中设置想要用于通信的串口，串口功能设置为10，设置完毕飞控重新上电，可以发现飞控屏幕上Pos传感器显示中第十个位置由×变为 · ，Nano开始下发T265数据后 · 变为钩。