在PX4 v1.9.2替换姿态控制算法方法

在PX4 v1.9.2替换姿态控制算法方法

目的是在model中写一个与mc_att_control类似的model加入自己的姿态控制算法并替换之

目录

• 姿态控制算法simlink搭建生成并简单说明

• PX4 v1.9.2姿态控制接口简单介绍

• SITL以及HITL简单调试方法

姿态控制算法simlink搭建生成并简单说明

我们设计了一个简单的串级pid控制器，如下图

输入信号：飞机的当前姿态角以及当前角速度（state）、期望姿态（reference）、命令（command包括复位和模式选择）

• 当前姿态包括pitch、roll、yaw、pitch-rate、roll-rate、yaw-rate 单位：rad、rad/s
• 期望姿态包括pitch、roll、yaw 单位为：rad

输出信号：为归一化的x、y、z的控制信号范围为[-1,1]要和PX4姿态控制输出接口吻合

中间过程就是一些外环，内环控制了，这里就不详解了。

然后我们用simlink中代码生成工具bulid并配置一些必要的参数，就可以生成我们需要的嵌入式C++代码了，接下来就是要在飞控板中实际的验证我们的算法。

PX4 v1.9.2姿态控制接口简单介绍

关于如何在PX4中添加模块，你可以参考官网的例程(hello_sky)。

根据上一节可知我们的控制器至少需要三种接口，分别为当前姿态接口，期望姿态接口以及输出接口。大致这样：

其中 当前的控制模式 需要飞机的是否解锁信息为的时判断是否启动控制器和是否reset，还有飞机当前的飞行模式，这里我们主要判断的是飞机是否处在STABILIZED模式，其他的模式控制不在我们的讨论之中。

这里我先介绍上述接口对应的TOPIC:

期望的角度：manual_control_setpoint_s

当前的角度以及角速度 ：vehicle_attitude_s

当前的控制模式 ：vehicle_control_mode_s

控制输出 ：actuator_controls_s

注意当飞机处在STABILIZED模式时，遥控器的俯仰、翻滚、油门、偏航通道分别对应manual_control_setpoint_s结构体中的x、y、z、r。 这四个量都是归一化后的数据范围为[-1,1]，其中的x、y、z可以直接送给我们的mc_att_linear_pid模型，而偏航r需要转换一下，否则当你控制yaw方向时，飞机yaw方向会随着遥控器的偏航杆回中飞机的yaw也会回到初始位置。

在mc_att_control_main.cpp中这个部分的对应的函数是MulticopterAttitudeControl::generate_attitude_setpoint()

主要的功能是把manual_control_setpoint_s结构体中的x、y、z、r，转换成我们需要的期望角度。

下图是源码mc_att_control_main.cpp中STABILIZED模式下简单的流程图（只跟踪了stabilized模式）。

SITL以及HITL简单调试方法

我们在调试代码的时候主要用到gazebo软件在环仿真，以及gazebo硬件在环仿真。

用到的函数是 **PX4_INFO()**可以在终端中打印信息，用于调试，

初期可以软件在环仿真，确保程序架构正确，能够正常运行。后期可以gazebo硬件在环仿真加入遥控器在连接PX4飞控实时的查看模型的运动情况

在运行仿真前要把自己的模块加入到sitl 中的defult.cmake如下

• SITL

  终端输入

  make px4_sitl_default gazebo
  

  然后终端中就会实时打印你在模块中写的**PX4_INFO()**内容了

  

• HITL

  HITL中，查看调试信息PX4_INFO() 需要在qgc地面站中查看，前提是你的PX4已成功连接gazbeo，qgc成功连接gazebo。具体配置可参考官网HITL Simulation.

  key:当你的model处在运行状态，但是**PX4_INFO()**信息却没有打印出来，你可已重启一下这个model试一下。