多旋翼无人机
一、系统设计
在之前的所有文章中推荐使用的自驾仪是Pixhawk2。在2018年秋季，Pixhawk4上市了，其处理器由STM32F427升级到了STM32F765，可用内存也从256KB上升到512KB。对于嵌入式程序来说512KB的内存已经算是比较大的了，但是在很多时候我们还是希望飞控系统可以完成更加复杂的飞行任务，实现更加强大的系统功能，因此如果将所有的功能都交给Pixhawk来完成的话无论是Pixhaw2还是Pixhawk4都很难完成。因此在本文中我们提出一个改良方案——外部控制系统，并充分利用外部控制系统完成一系列无人机的趣味航线的规划与飞行功能。
为解决Pixhawk处理器性能和内存较小的问题，我们提出一种改良方案，采用树莓派3B（后续简称为树莓派）作为一个外部控制器与Pixhawk自驾仪进行合理的组装到一起，形成一个带有外部控制器的新型飞控系统。我们先来对树莓派做一些简单的介绍：
我们可以看到相比Pixhawk来说树莓派的处理器性能较高和内存也大的多。实际上PX4飞控程序也支持编译成可以在树莓派运行上的版本（树莓派上运行的是Linux操作系统），编译命令如下：
make posix_rpi_cross
但是，如果将PX4飞控程序直接编译成在树莓派上运行的程序，就需要为树莓派接入所有飞行所需要的传感器，例如MPU6000或MPU9250、MS5611、GPS等等，这需要使用者手动接线，焊接，固定等等，最终的效果并不理想。因此我们并不推荐这种方案。
另一种方案就是我们所提到的带有外部控制器的Pixhawk自驾仪，也就是将Pixhawk和树莓派结合起来，将Pixhawk的一个串口4与树莓派的GPIO串口通过导线连接，如下图所示：
除了解决Pixhawk处理器性能和内存较小的问题之外，这种结构还有另外一个好处，就是我们可以将飞控程序中的驱动、状态估计、控制等部分与导航和其他业务分离。Pixhawk只负责控制无人机的稳定飞行，而树莓派负责航线规划与其他更为复杂的功能放在树莓派中运行。
实际上，在对无人机飞控程序开发的过程当中，控制无人机稳定飞行的驱动、状态估计、控制系统等修改频率较小，而对导航、业务等修改和追加的功能频率较高。甚至大多数情况下都是在修改导航和其他业务功能，因此我们也有必要将这两部分内容与无人机的控制系统分离。而在高频修改的程序中出现错误的几率大大增加。将控制系统与业务系统分离也会保证在业务程序出现意外错误时不会影响到控制系统对无人机的稳定控制。这也保证在业务系统出现错误时，不至于导致无人机坠毁，而可以利用稳定的控制系统使无人机返航。
在实际飞行过程中，Pixhawk通过串口4将当前无人机的主状态、导航状态、姿态、速度、位置等相关信息发送给树莓派；树莓派根据无人机的这些相关数据进行合理的规划并通过串口Pixhawk发送无人机的实时期望。
实时期望包括对无人机进行水平速度期望、垂直速度期望、水平位置期望和垂直高度期望。其中不对无人机的姿态进行实时控制。因为无人机的姿态控制相对敏感，自动控制无人机的姿态会无法很好的对无人机当前的速度和位置很好的控制，可能导致不确定性的危险。
对于PX4飞控程序来讲，我们需要完成3个部分的功能：
1）实现外部控制通信模块（extctl），用于通过串口设备与树莓派进行通信，通过订阅并接收相关uORB数据并向树莓派发送无人机的当前的实时状态，同时接收树莓派对飞控程序的指令和控制期望，公告并发布外部控制的期望uORB。
2）实现新的外部控制模式，同时加入到主状态（MAIN_STATE_EXTCTL）、导航状态（NAVIGATION_STATE_EXTCTL）和控制状态（flag_control_extctl_enabled），用于无人机可根据需要通过地面站程序或遥控器控制杆来切换到外部控制模式。
3）实现外部控制模式的控制功能，在位置控制模块中进行判断，如果当前控制模式为外部控制，则通过订阅外部控制的uORB数据并根据此数据执行相关的位置期望或速度期望。
二、功能实现
首先实现在PX4飞控程序中与树莓派通信用的外部控制模块。在src/modules文件夹下创建extctl文件夹，并在其中新建一下文件：
extctl
├── CMakeLists.txt
├── extctl_main.c
├── extctl_main.h
├── extctl_pos.c
├── extctl_pos.h
├── extctl_sp.c
├── extctl_sp.h
├── extctl_status.c
├── extctl_status.h
└── extctl_typedef.h
在CMakeList.txt中加入如下内容：
px4_add_module( #添加新模块
MODULE modules__extctl #模块名称
MAIN extctl #入口函数
STACK_MAIN 1200 #函数栈内存
SRCS #源代码文件
extctl_main.c #主函数
extctl_pos.c #速度位置发送相关
extctl_sp.c #速度位置期望相关
extctl_status.c #状态相关
DEPENDS #依赖模块
platforms__common
)
我们需要在extctl_pos.c中完成接收当前无人机速度和位置的uORB数据，并将其中有效的数据通过串口发送给树莓派具体实现功能代码如下：
int extctl_pos_send(void)
{
int pos_sub_local = orb_subscribe(ORB_ID(vehicle_local_position));
int pos_sub_global = orb_subscribe(ORB_ID(vehicle_global_position));
struct vehicle_local_position_s pos_local;
struct vehicle_global_position_s pos_global;
struct ext_vehicle_pos_s pos = { 0 };
while (!_extctl_should_exit) {
uint8_t status_pos = 0;
bool updated = false;
orb_check(pos_sub_local, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_local_position), pos_sub_local, &pos_local);
pos.x = pos_local.x;
pos.y = pos_local.y;
pos.z = pos_local.z;
pos.vx = pos_local.vx;
pos.vy = pos_local.vy;
pos.vz = pos_local.vz;
pos.yaw = pos_local.yaw;
status_pos |= (1 << 0);
}
orb_check(pos_sub_global, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_global_position), pos_sub_global,
&pos_global);
pos.lat = pos_global.lat;
pos.lon = pos_global.lon;
pos.alt = pos_global.alt;
pos.vel_n = pos_global.vel_n;
pos.vel_e = pos_global.vel_e;
pos.vel_d = pos_global.vel_d;
status_pos |= (1 << 1);
}
if (status_pos) {
extctl_protocal_write(&pos, DATA_TYPE_POS, sizeof(pos));
}
usleep (DEV_RATE_POS);
}
return 0;
}
此外，我们还需要编写程序，将无人机的当前状态信息发送给外部控制器，例如：主状态、导航状态、解锁/锁定状态、着陆状态、起飞点信息等相关内容。这部分程序我们在extctl_status.c文件中实现，其代码与extctl_pos.c类似，内容如下：
int extctl_status_send(void)
{
int cmd_state_sub = orb_subscribe(ORB_ID(commander_state));
int vec_state_sub = orb_subscribe(ORB_ID(vehicle_status));
int armed_state_sub = orb_subscribe(ORB_ID(actuator_armed));
int land_state_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
int home_pos_sub = orb_subscribe(ORB_ID(home_position));
struct commander_state_s cmd_state;
struct vehicle_status_s vec_state;
struct actuator_armed_s arm_state;
struct vehicle_land_detected_s land_state;
struct home_position_s home_pos;
struct ext_sys_status_s sys_status = { 0 };
while (!_extctl_should_exit) {
uint8_t status = 0;
bool updated = false;
orb_check(cmd_state_sub, &updated);
if (updated) {
orb_copy(ORB_ID(commander_state), cmd_state_sub, &cmd_state);
sys_status.main_state = cmd_state.main_state;
status |= (1 << 0);
}
orb_check(vec_state_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_status), vec_state_sub, &vec_state);
sys_status.nav_state = vec_state.nav_state;
sys_status.arming_state = vec_state.arming_state;
status |= (1 << 1);
}
orb_check(armed_state_sub, &updated);
if (updated) {
orb_copy(ORB_ID(actuator_armed), armed_state_sub, &arm_state);
sys_status.armed = arm_state.armed;
status |= (1 << 2);
}
orb_check(land_state_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_land_detected), land_state_sub, &land_state);
sys_status.landed = land_state.landed;
status |= (1 << 3);
}
orb_check(home_pos_sub, &updated);
if (updated || !sys_status.homed) {
orb_copy(ORB_ID(home_position), home_pos_sub, &home_pos);
if (fabs(home_pos.lat + home_pos.lon) > DBL_EPSILON) {
sys_status.home_lat = home_pos.lat;
sys_status.home_lon = home_pos.lon;
sys_status.home_alt = home_pos.alt;
sys_status.homed = true;
status |= (1 << 4);
}
}
if (status) {
extctl_protocal_write(&sys_status, DATA_TYPE_STATUS,
sizeof(struct ext_sys_status_s));
}
usleep (DEV_RATE_STATUS);
}
return 0;
}
除了发送无人机数据到外部控制器当中之外，我们还需要编写接收外部控制器对无人机的控制数据，也就是对无人机的速度和位置期望。代码在extctl_sp.c中实现，具体内容如下：
int extctl_sp_handle(void *data)
{
struct ext_vehicle_sp_s *sp = data;
if (sp == NULL) {
return -1;
}
_orb_sp.run_pos_control = sp->run_pos_control;
_orb_sp.run_alt_control = sp->run_alt_control;
_orb_sp.run_yaw_control = sp->run_yaw_control;
_orb_sp.sp_yaw = sp->sp_yaw;
_orb_sp.sp_x = sp->sp_x;
_orb_sp.sp_y = sp->sp_y;
_orb_sp.sp_z = sp->sp_z;
_orb_sp.vel_sp_x = sp->vel_sp_x;
_orb_sp.vel_sp_y = sp->vel_sp_y;
_orb_sp.vel_sp_z = sp->vel_sp_z;
if (_orb_sp_topic < 0) {
_orb_sp_topic = orb_advertise_multi(ORB_ID(extctl_sp), &_orb_sp,
&_orb_sp_instance, ORB_PRIO_DEFAULT);
}
else {
orb_publish(ORB_ID(extctl_sp), _orb_sp_topic, &_orb_sp);
}
return 0;
}
此外，还需要在extctl_main.c中实现基本的多线程机制启动上述3个线程，并实时地读取串口设备当中的数据，并进行解析和处理，这部分代码略。
之后，我们需要在主状态、导航状态、控制状态中，分别加入外部控制模式，如下：
#commander_state.msg
uint8 MAIN_STATE_EXTCTL = 13
#vehicle_status.msg
uint8 NAVIGATION_STATE_EXTCTL = 20
vehicle_control_mode.msg
bool flag_control_extctl_enabled
然后在Commander模块中加入对外部控制模式的切换和超时保护功能。首先接收地面站或遥控器的外部控制切换命令，然后在设置导航模式和控制模式，具体代码如下：
bool Commander::handle_command(...)
{
if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_EXTCTL) {
main_ret = main_state_transition(status_local,
commander_state_s::MAIN_STATE_EXTCTL, ...);
mavlink_log_info(&mavlink_log_pub, "cmd handle extmode");
}
//其他代码略
}
bool Commander::set_nav_state(...)
{
switch (internal_state->main_state) {
case commander_state_s::MAIN_STATE_EXTCTL:
status->nav_state = vehicle_status_s::NAVIGATION_STATE_EXTCTL;
break;
}
//其他代码略
}
void Commander::set_control_mode()
{
switch (status.nav_state) {
case vehicle_status_s::NAVIGATION_STATE_EXTCTL:
control_mode.flag_control_extctl_enabled = true;
break;
}
//其他代码略
}
void Commander::run()
{
while (1) {
orb_check(extctl_sp_sub, &updated);
if (updated) {
orb_copy(ORB_ID(extctl_sp), extctl_sp_sub, &extctl_sp_s);
time_at_extctl = hrt_absolute_time();
}
else {
if (internal_state.main_state == commander_state_s::MAIN_STATE_EXTCTL
&& hrt_absolute_time() - time_at_extctl > TIMEOUT_EXTCTL_SP) {
main_state_transition(&status,
commander_state_s::MAIN_STATE_AUTO_RTL);
}
}
//其他代码略
}
}
之后我们还需要在位置控制系统中对外部控制模式进行处理，在启动了外部控制模式之后，执行外部控制系统对无人机的所有期望内容，具体代码如下：
void MulticopterPositionControl::poll_subscriptions()
{
//其他代码略
orb_check(_extctl_sp_sub, &updated);
if (updated) {
orb_copy(ORB_ID(extctl_sp), _extctl_sp_sub, &_extctl_sp);
_last_extctl = hrt_absolute_time();
}
}
void MulticopterPositionControl::calculate_velocity_setpoint(float dt)
{
if (_control_mode.flag_control_extctl_enabled) {
_run_alt_control = _extctl_sp.run_alt_control;
_run_pos_control = _extctl_sp.run_pos_control;
if (_extctl_sp.run_yaw_control) {
_att_sp.yaw_body = _extctl_sp.sp_yaw;
}
_pos_sp(0) = _extctl_sp.sp_x;
_pos_sp(1) = _extctl_sp.sp_y;
_pos_sp(2) = _extctl_sp.sp_z;
_vel_sp(0) = _extctl_sp.vel_sp_x;
_vel_sp(1) = _extctl_sp.vel_sp_y;
_vel_sp(2) = _extctl_sp.vel_sp_z;
}
//其他代码略
}
最后，我们还需要在树莓派上编写一个可以与Pixhawk进行通信，并处理相关功能的外部控制系统程序。此部分程序的功能与PX4中我们所编写的extctl模块功能类似，只不过在数据收发上是相反的，外部控制程序需要从树莓派的串口上读取无人机当前的状态信息、速度信息和位置信息，然后根据自己的行新规划和业务需要通过串口向Pixhawk中的PX4飞控程序发送速度期望和位置期望等。这部分代码略，请有兴趣的读者自行完成。
Copyright © 2015-2023 问渠网 辽ICP备15013245号