Acfly-UWB

CGC Lv4
  2023-08-24 18:33:57 2023-08-24 18:33 Created   2023-10-04 14:43:21 2023-10-04 14:43 Updated      

使用 Nooploop 的 LinkTrack 定位系统为 Acfly飞控提供定位
官方资料
本文包含本人在2023电赛中调试的辛酸泪

配置

我们使用的是4基站+1标签+1控制台的方案
参考的是官方用户手册 中第13-14页的内容

  1. 按顺序0-3的id分别配置4个anchor
  2. 配置tag标签（飞控端） 通信协议选择tag_frame0
  3. 配置console控制台

摆放及上电方向

由于飞控的数据适配性，基站摆放位置和飞机上电位置需要受到限制
（可能之后随着设备的更新会减轻这方面带来的影响，但是按我们目前的测试结果来看，还是非常建议按照已经测试好的方向进行初始化）

如下
飞控位置

飞控通信协议设置

tag端通信协议

具体通信协议可以参考官方参考通信协议
在此处对飞控段使用的协议即 Tag 端作说明 即

tag端通信协议（接收）如下
tag通信协议
可知飞控可接收到数据有位置、姿态、精度等一系列数据

官方驱动代码

以下给出acfly官方发布的驱动代码

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
#include "drv_UWB_LinkTrack.hpp"
#include "Commulink.hpp"
#include "Basic.hpp"
#include "FreeRTOS.h"
#include "task.h"
#include "SensorsBackend.hpp"
#include "MeasurementSystem.hpp"

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

typedef struct
{
	uint8_t id;
	uint8_t role;
	int pos_x:24;	int pos_y:24;	int pos_z:24;		
	int vel_x:24;	int vel_y:24;	int vel_z:24;	
	int dis_0:24;	int dis_1:24;	int dis_2:24;	int dis_3:24;	int dis_4:24;	int dis_5:24;	int dis_6:24;	int dis_7:24;
	float imuGyro[3];
	float imuAcc[3];
	uint8_t reserved1[12];
	int16_t angle[3];
	float q[4];
	uint8_t reserved2[4];
	uint32_t localTime;
	uint32_t systemTime;
	uint8_t reserved3[1];
	uint8_t eop[3]; // 估计位置的精度
	uint16_t voltage;
	uint8_t reserved4[5];
}__PACKED _Uwb;
static const unsigned char packet_ID[2] = { 0x55 , 0x01 };

static void OpticalFlow_Server(void* pvParameters)
{
	DriverInfo driver_info = *(DriverInfo*)pvParameters;
	delete (DriverInfo*)pvParameters;
	
	/*状态机*/
		_Uwb  Uwb;
		unsigned char rc_counter = 0;
		signed char sum = 0;
	/*状态机*/
	
	//等待初始化完成
	while( get_Altitude_MSStatus() != MS_Ready )
		os_delay(1);
	
	//注册传感器
	double angleOffset = 0;
	if( driver_info.param>360 )
	{
		//记录初始偏航
		Quaternion quat;
		get_Attitude_quat(&quat);
		angleOffset = quat.getYaw();
	}
	else
		angleOffset = degree2rad((double)driver_info.param);
	
	
	uint32_t sensor_key = PositionSlamSensorRegister( default_uwb_sensor_index , \
                                                    "UWB_LinkTrack" ,\
                                                    Position_Sensor_Type_RelativePositioning , \
                                                    Position_Sensor_DataType_s_xy , \
                                                    Position_Sensor_frame_SLAM , \
                                                    0.1, angleOffset, 100 );
	
	int lastP = 0;
	uint8_t pUCCounter = 0;
	while(1)
	{
		uint8_t rdata;
		if( driver_info.port.read( &rdata, 1, 2, 0.5 ) )
		{
			if( rc_counter < sizeof(packet_ID) )
			{
				//接收包头
				if( rdata != packet_ID[ rc_counter ] )
				{
					rc_counter = 0;
					sum = 0;
				}
				else
				{
					++rc_counter;
					sum += rdata;
				}
			}
			else if( rc_counter < sizeof(packet_ID) + sizeof(_Uwb) )
			{	//接收数据
				( (unsigned char*)&Uwb )[ rc_counter - sizeof(packet_ID) ] = rdata;
				sum += rdata;
				++rc_counter;
			}
			else
			{	//校验
				if( sum == rdata )
				{
					if( Uwb.eop[0]>200 || Uwb.eop[1]>200 ) // x y 精度大于2cm就失能
						PositionSensorSetInavailable(default_uwb_sensor_index,driver_info.sensor_key);
					else 
					{
						vector3<double> pos, vel;
						pos.x = Uwb.pos_x*0.1;
						pos.y = Uwb.pos_y*0.1;
						pos.z = Uwb.pos_z * 0.1;
						vel.x = Uwb.vel_x*0.01;
						vel.y = Uwb.vel_y*0.01;
						vel.z = Uwb.vel_z * 0.01;
						if( Uwb.eop[2] > 200 )
							PositionSensorChangeDataType( default_uwb_sensor_index,sensor_key, Position_Sensor_DataType_s_xy );
						else
							PositionSensorChangeDataType( default_uwb_sensor_index,sensor_key, Position_Sensor_DataType_s_xyz );
						double eop_xy = sqrtf( Uwb.eop[0]*Uwb.eop[0] + Uwb.eop[1]*Uwb.eop[1] );
						
						if( Uwb.dis_0 != lastP )
						{
							lastP = Uwb.dis_0;
							pUCCounter = 0;
						}
						else
						{
							if( pUCCounter < 255 )
								++pUCCounter;
						}
						
						if( pUCCounter>100 )
							PositionSensorSetInavailable( default_uwb_sensor_index,sensor_key );
						else
							PositionSensorUpdatePosition( default_uwb_sensor_index,sensor_key, pos, true, -1, eop_xy<100?100:eop_xy, Uwb.eop[2] );
					}
				}
				rc_counter = 0;
				sum = 0;
			}
		}
	}
}

static bool UWB_LinkTrack_DriverInit( Port port, uint32_t param )
{
	//波特率115200
	port.SetBaudRate( 460800, 2, 2 );
	//注册传感器
	DriverInfo* driver_info = new DriverInfo;
	driver_info->param = param;
	driver_info->port = port;
    
	xTaskCreate( OpticalFlow_Server, "OptFlowGL9306", 1024, (void*)driver_info, SysPriority_ExtSensor, NULL);
	return true;
}

void init_drv_UWB_LinkTrack()
{
	PortFunc_Register( 41, UWB_LinkTrack_DriverInit );
}

值得一提

在以上驱动代码中，值得一提的是

1
2
3
4
5
// z 精度大于2cm就不使用z轴数据
if( Uwb.eop[2] > 200 ) 
    PositionSensorChangeDataType( default_uwb_sensor_index,sensor_key, Position_Sensor_DataType_s_xy );
else
    PositionSensorChangeDataType( default_uwb_sensor_index,sensor_key, Position_Sensor_DataType_s_xyz );

及

1
2
3
4
5
6
if( Uwb.eop[0]>200 || Uwb.eop[1]>200 ) // x y 精度大于2cm就失能
    PositionSensorSetInavailable(default_uwb_sensor_index,driver_info.sensor_key);
else 
{
  // ...
}

驱动内容为 超出2cm的精度许可范围，就将传感器失能
UWB官方给出的数据是，x y 的精度在 10cm 之内 z 精度在 30cm 以内

  • 关于z轴数据
    这样一来其实可以反应我们看到的现象，首先是在只使用uwb作为外接传感器时，飞机完全定不住高（因为z精度完全无法满足2cm的要求），但是可能由于飞控中气压计等设备的存在，飞控有高度数据，室内定位可用，所以能够允许起飞，但在天上的高度数据，完全由飞控而来，所以现象是，没有定高可言。。。

注意事项及传感器使用建议

  1. 使用时发起自动标定的设备需要在线，比如我们使用控制台发起一键标定，则在使用时控制台必须要在线，否则数据会不可用
  2. 由于uwb的z轴精度较低，建议加上tfmini等作为定高传感器，通过测试，有uwb加上tfmini，定位效果较好

写在最后

  • 通过这段时间的测试，我们发现至少在电赛赛制的背景下，用uwb可能会出现某些限制，比如场地不一定能够容纳摆放下基站，并且无法在换场地后进行标定，可能受到干扰等
  • 对于电赛（赛场不允许携带笔记本及pc，上位机只支持x86，我们手上已有的板载电脑皆为arm，无法在现场进行标定）无法在换场地后进行标定的情况，可能考虑自行写接口，使用单片机或板载电脑进行标定，能够很大程度减少对位置场地的限制，我们尝试按照通信协议写了一份一键自动标定的程序 ，但还未在实际场地中进行测试。
  • Title: Acfly-UWB
  • Author: CGC
  • Created at: 2023-08-24 18:33:57
  • Updated at: 2023-10-04 14:43:21
  • Link: https://redefine.ohevan.com/2023/08/24/Acfly-UWB/
  • License: This work is licensed under CC BY-NC-SA 4.0.
On this page
Acfly-UWB
  1. 配置
  2. 摆放及上电方向
  3. 飞控通信协议设置
    1. tag端通信协议
    2. 官方驱动代码
    3. 值得一提
  4. 注意事项及传感器使用建议
  5. 写在最后