Fork me on GitHub

Pixhawk连接摇杆进行仿真 12月 02, 2017

1. 问题

最近有这样一个需求: 对Pixhawk软件中的控制算法进行更改后,需要对其控制算法、各模式间的切换等进行全面地仿真测试,避免实际飞行时炸机的损失。 按照官方文档中的方法, 可利用Gazebo, QGroundControl等配合完成大部分的大部分的测试,还可连接控制摇杆/手柄(Joystick/Gamepad), 将其作为手动输入。同事付老师帮忙给了一个Made in China的Saitek Cyborg evo摇杆,按照官网方法进行配置,发现在仿真中无法通过其设置手动、增稳、自动等飞行模式,一查,原来仅有高端的Sony的摇杆才能支持模式切换。 只好想办法省钱,通过更改软件解决无法通过摇杆切换模式的问题。

2. 问题分析

PX4原有的仿真环境交联关系如下图:

从图中可以看出摇杆的输入通过QGroundControl地面站读取后,转发给控制软件。若能通过控制软件直接读取摇杆输入,如下图:

在控制软件中读取摇杆输入后,对其解析,将按键消息打包为软件能识别的模式切换指令消息,并按照PX4软件中的消息发布订阅机制将其发送至负责模式切换的模块,即能解决前述问题。因此须分以下两步达到目标:

  1. 读取摇杆输入,正确解析其消息;
  2. 将摇杆消息打包为软件可识别的Topic, 将其发布;

事后想了一下官网中通过QGroundControl转发摇杆数据,而非通过控制软件直接读取的原因可能如下:

  1. 方便对不同摇杆进行校准操作. 因最终软件需要的控制量数据均需要进行归一化处理, 可通过QGroundControl对不同类型摇杆进行统一校准操作;
  2. 可保持软件的通用性,在SITL, HITL, 或实际飞行时均可通过Joystick控制飞行。

3. 解决方案

3.1. 读取摇杆输入

首选需要弄清如何读取摇杆输入. 以谷歌找到的一个linux官方文档代码工程为参考,写了如下一个简单测试程序读取摇杆输入,测试环境为Ubuntu 16.04LTS.

#include <iostream>
#include <sys/stat.h>
#include <unistd.h>
#include <stdio.h>
#include <sys/types.h>
#include <sys/fcntl.h>
#include <time.h>

using namespace std;

#define JS_EVENT_BUTTON 0x01 // button pressed/released
#define JS_EVENT_AXIS   0x02 // joystick moved
#define JS_EVENT_INIT   0x80 // initial state of device

typedef struct{
    unsigned int  time;     /* event timestamp in milliseconds */
    short         value;    /* value: for buttons: 1/0 = down/up; axes: -32767~32767*/
    unsigned char type;     /* event type */
    unsigned char number;   /* axis/button number */
}JS_EVENT;

int main() {
    //open the joystick device, the name might be different
    int fd = open("/dev/input/js0", O_RDONLY); 

    if(fd<0) {
        printf("can not open joystick!\n");
        return 1;
    }

    struct stat sbuf;

    stat("/dev/input/js0", &sbuf);
    printf("/etc/hosts file size = %ld\n", sbuf.st_rdev);

    JS_EVENT event = {0};

    int bytes = 0;

    while (true) {
        // Restrict rate
        usleep(1000);

        // Attempt to sample an event from the joystick
        bytes = read(fd, &event, sizeof(JS_EVENT));

        if (bytes == sizeof(JS_EVENT)) {
            if (event.type & JS_EVENT_BUTTON) {
                printf("Button %u  is %s\n", event.number, event.value == 0 ? "up" : "down");
            }

            if (event.type & JS_EVENT_AXIS) {
                printf("Axis %u is at position %d\n", event.number, event.value);
            }
        }
    }
    return 0;
}

使用如下命令编译

g++ -g -o readJoy main.cpp

通过USB连接Saitek Cyborg evo摇杆, 执行程序,得到如下结果:

代码中通过JS_EVENT结构体读取数据,该数据结构中的type表示了摇杆事件类型(轴移动或按键), 而number和value则分别表示了哪一个轴/按键的事件值。 拟通过摇杆的各轴作为roll, pitch, yaw, throttle控制,以按键控制模式切换。为了进行下一步工作,须测试得出各轴的极限位置下软件采到的事件值:

Axis Meaning Value Direction
axis0 Roll -32767~32767 left~right
axis1 Pitch 32767~-32767 backward~forward
axis2 Throttle 32767~-32767 min~max
axis3 Yaw -32767~32767 left~right

3.2. 修改Pixhawk源码

读取解析到摇杆的动作事件数据仅是第一步,为将其集成至Pixhawk中的软件模块,还需注意以下问题:

  1. 上述示例代码中的read()函数为阻塞调用,若直接将其放入某个线程中,没有事件发生时,会导致该线程阻塞而无法执行其他语句,须使用linux提供的poll()接口,仅当查询到有事件发生才调用read()读设备文件上的消息;
  2. 须按照Pixhawk的消息订阅/发布(publish/subscribe)机制将摇杆消息打包成软件可识别的消息,在此我们将其打包为manual_control_setpoint消息。因为事实上软件中sensors模块中rc_update.cpp将原始的遥控器消息也解析为manual_control_setpoint消息,并将其发布。位置控制模块订阅该消息,获取其中的各轴控制量信息。特别需要注意的是Commander模块也订阅该消息,在Commander.cpp中的set_main_state_rc()函数中,根据该消息中的模式设置变量设置飞机的模式。需要根据该函数逆推出发生各模式按键事件后,应该给manual_control_setpoint消息中所需填充的正确值,以期得到目标模式。
  3. 按照上述思路更改代码后,发现按压摇杆上对应按键后,有时无法切换模式的问题。排查发现发布消息的频率过快(6ms),很可能导致消息队列满,使Commander模块无法接收到新消息,从而无法切换模式的问题。将发布消息的周期改为50ms, 该问题再无复现。

最终通过摇杆上的按键0,1,2分别控制手动、增稳、自动三种模式,以其四个轴(axis)控制滚转、俯仰、偏航及油门。在PX4/Firmware的v1.6.5稳定版代码基础上,对simulator_mavlink.cpp文件更改,插入部分代码,均在#ifdef ENABLE_JOYSTICK ... #endif 预处理块内,详见https://github.com/oneWayOut/Firmware上的cai_readJS分支。

在代码目录的终端中输入以下命令,可对其进行测试:

make posix gazebo_plane

连接QGroundControl后,即可模拟飞行操作。