位置环控制器二次开发
本教程的目的：
- 学习示例代码，对无人机位置环控制器进行二次开发
- 对外环控制器进行性能测试
- 熟悉prometheus_control模块代码，如
px4_pos_estimator.cpp及px4_pos_controllercpp - 学习Gazebo仿真基本操作
- prometheus_control模块中,有
px4_sender.cpp和px4_pos_controller.cpp两大控制方式,其中px4_sender.cpp只是给飞控发送各种期望值(实际控制算法运行于飞控中),px4_pos_controller.cpp发送期望姿态值给飞控(位置环控制算法位于Prometheus代码中),因此如果要对位置环代码进行二次开发,则需要使用px4_pos_controller.cpp. -
px4_pos_controller.cpp为位置环控制代码，其订阅无人机当前位置及期望位置，并通过位置控制器进行解算得到期望姿态角。最终，期望姿态角被发送至PX4飞控中，由PX4飞控进行姿态环控制。 - 其中，位置控制器的代码实现位于
Modules/control/include/Position_Controller文件夹中，目前共有五种位置环控制器。因此可以通过仿写的方式，新增自定义位置环控制器，并在px4_pos_controller.cpp中进行调用。 - 可通过配套的
Controller_Test类进行位置环控制器性能测试，目前提供圆形、8字形、阶跃三种形式的轨迹追踪测试。（Controller_Test类的功能已集成至代码terminal_control.cpp代码中） - 强烈推荐先使用gazebo仿真进行测试，然后再进行真机实验。
- 请仔细阅读代码
pos_controller_PID.h，学习如何编写一个位置环控制器类。 - 请仔细查阅代码
px4_pos_controller.cpp，并再该文件中声明并调用新增的位置环控制器。 - 位置控制器相关参数请查阅
Parameter_for_control.yaml
- 运行launch文件
roslaunch prometheus_gazebo sitl_pos_control.launch - 在
terminal_control终端中选择command input control,首先输入999进行解锁并切换至offboard模式 ，并起飞 - 输入4选择Move移动模式，再次输入4进入轨迹追踪子模式，并根据提示输入指令选择所需要测试的内容
- 在rviz或ground_station终端中可观察无人机的追踪情况
- 更多详细信息：演示视频
在rviz中，较粗较短的轴代表期望位姿，较长较细的为当前位姿，绿色的为期望轨迹，红色的为运行轨迹。
- 轨迹形状参数请查阅
terminal_control.yaml - 可通过修改参数
state_fromposehistory_window来修改轨迹的长短。
其他说明：
- 为了更好的区分４个电机，红色代表４号电机，黄色代表３号电机，绿色代表２号电机，蓝色代表１号电机
可使用ros_bag功能包进行数据记录，通过如下指令存储轨迹追踪相关话题
rosbag record -O subset /prometheus/drone_state /prometheus/reference_pose
- /prometheus/drone_state为无人机状态，包含了无人机位置、速度、姿态等信息
- /prometheus/reference_pose为无人机期望轨迹，包含了期望位置和期望姿态（期望姿态由控制器计算得到）
若想在Matlab中对得到的bag数据进行处理，有两种方式：（比较推荐第一种）
直接使用Matalab打开rosbag文件， 例子：
bag = rosbag('ros_turtlesim.bag');
bSel = select(bag,'Topic','/turtle1/pose');
msgStructs = readMessages(bSel,'DataFormat','struct');
现将bag文件存储为txt格式，再通过Matlab读入数据
rostopic echo -b file_name.bag -p /topic_name > Txt_name.txt
在Matlab中可绘制类似如下曲线，对所设计位置环控制器进行性能分析并可与其他控制器进行比较。
