Simlink与PX4硬件在环仿真(HIL)实现

Bingo · 发表于 2019-9-18 00:37:08

本帖最后由 Bingo 于 2019-9-18 21:13 编辑

Simlink与PX4硬件在环仿真(HIL)实现

介于涉及的知识比较多，这里只是简单的介绍一下，更多内容可以参考阿木实验室《基于matlab模型开发》课程。

硬件在环HIL介绍
simlink与PX4通信实现

硬件在环HIL介绍

为来贯彻万物都可以用数学公式表示，我们以simlink的角度来看我们的无人机，那么UAV可以分为以下的几个模块：

传感器模型
滤波器模块
控制器模块
飞机的物理模型

一般我们都会在simlink中搭建如上所有的模块，那么我们可以进行纯软件仿真。如下图当我们只仿真飞行器的物理模型，把物理模型的姿态、位置信息打包发给PX4飞控中处理，飞控的中控制器在根据这些物理模型的状态然后输出的控制量在来控制simlink中的物理模型。这样就形成了一闭环的HIL。

simlink与PX4通信实现

硬件在环原理不难，关键是两者的通信如何实现。因为PX4通过usb口向外发送的是打包好的mavlink信息，所以我们的simlink模块能不能直接解析出串口中的mavlink信号呢，遗憾的是不可以。目前有两种方法可以实现通信：

用s-function链接c程序可以参考这个连接
```
https://gitee.com/bingobinlw/simulink_mavlink 
```
用s-function连接m程序，我们的工程用到的是此方法，不过需要下图的工具箱

版本要是v19.1.1及以上才行。因为在这个工具箱里集成了malink解析打包的函数。

大概会用到以下几个函数：

deserializemsg()，解析来自缓冲区的mavlink包，比如来自PX4的控制量通过com的消息
serializemsg()，打包成mavlink包，然后可以通过com口发给PX4

然后再看下用到的几个重要的simlink与PX4通信的mavlink包

mavlink_msg_hil_actuator_controls ID=93

typedef struct __mavlink_hil_actuator_controls_t {
 uint64_t time_usec; /*< [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number.*/
 uint64_t flags; /*<  Flags as bitfield, reserved for future use.*/
 float controls[16]; /*<  Control outputs -1 .. 1. Channel assignment depends on the simulated hardware.*/
 uint8_t mode; /*<  System mode. Includes arming state.*/
}) mavlink_hil_actuator_controls_t;

mavlink_msg_hil_state_quaternion ID = 115

typedef struct __mavlink_hil_state_quaternion_t {
 uint64_t time_usec; /*< [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number.*/
 float attitude_quaternion[4]; /*<  Vehicle attitude expressed as normalized quaternion in w, x, y, z order (with 1 0 0 0 being the null-rotation)*/
 float rollspeed; /*< [rad/s] Body frame roll / phi angular speed*/
 float pitchspeed; /*< [rad/s] Body frame pitch / theta angular speed*/
 float yawspeed; /*< [rad/s] Body frame yaw / psi angular speed*/
 int32_t lat; /*< [degE7] Latitude*/
 int32_t lon; /*< [degE7] Longitude*/
 int32_t alt; /*< [mm] Altitude*/
 int16_t vx; /*< [cm/s] Ground X Speed (Latitude)*/
 int16_t vy; /*< [cm/s] Ground Y Speed (Longitude)*/
 int16_t vz; /*< [cm/s] Ground Z Speed (Altitude)*/
 uint16_t ind_airspeed; /*< [cm/s] Indicated airspeed*/
 uint16_t true_airspeed; /*< [cm/s] True airspeed*/
 int16_t xacc; /*< [mG] X acceleration*/
 int16_t yacc; /*< [mG] Y acceleration*/
 int16_t zacc; /*< [mG] Z acceleration*/
}) mavlink_hil_state_quaternion_t;

如下图右边需要的mavlink包是mavlink_msg_hil_actuator_controls，左边需要的mavlink包是mavlink_msg_hil_state_quaternion