moumde的博客

本文主要是对过去写的一些APM中姿态控制流程及函数博文的汇总。按照之前写的博文以及个人认为的学习顺序，给出APM姿态控制器的学习顺序如下：APM姿态旋转理论基础Ardupilot姿态控制器 PID控制流程(整定后的P控制器)详解APM的开方控制器sqrt_controllerArdupilot前馈及平滑函数input_euler_angle_roll_pitch_yaw解析Ardupilot倾转分离函数thrust_heading_rotation_anglesArdupilot四元数姿态控制

Ardupilot速率控制器rate_controller_run解析PID速率控制器源码解析rate_controller_run()PID运算积分限制update_i()内容补充：函数中陀螺仪数据的获取rate_controller_run()在APM中是作为PID控制器将输入的期望角速率转换计算得到实际的电机控制指令的。下面将以Copter为例进行讲解。所有无人机的PID速率控制器都是在fast_loop()这个以400Hz频率运行的函数中被实时调用的。而update_flight_mode()

Ardupilot四元数姿态控制函数attitude_controller_run_quat解析源码解析细节讲解thrust_heading_rotation_angles()update_ang_vel_target_from_att_error()角度前馈量旋转解释姿态更新部分在开始阅读本文之前，建议可以先看一下前面几篇博文，当然你基础好直接看也是没事的~详解APM的开方控制器sqrt_controllerArdupilot倾转分离函数thrust_heading_rotation_angles

Ardupilot倾转分离函数thrust_heading_rotation_angles什么是轴角分离源码分析一些细节补充效果显示及进一步修改本文主要写一下自己对于APM倾转分离（轴角分离）函数的一些学习笔记及思考。ZING已经很好地分析了APM轴角分离函数，以下是我参考的他的一些博文。[飞控]姿态误差(四)-APM如何计算姿态误差[飞控]倾转分离(一)-APM的倾转分离竟然没有效果？[飞控]倾转分离(补充)-等效旋转矢量(轴角)与旋转矩阵后续分析过程会主要引用来自上面三篇，侵删。什么是轴角

Ardupilot前馈及平滑函数input_euler_angle_roll_pitch_yaw解析源码解析这个函数做了什么部分细节euler_accel_limit()input_shaping_angle()姿态变化率与机体角速度之间的关系本文将以input_euler_angle_roll_pitch_yaw()函数为例，讲解一下APM中的前馈及平滑控制函数，当然其余的类似input_euler_angle_roll_pitch_euler_rate_yaw()等等的函数也是类似的，当学习完这个

前言前面说过，sqrt_controller是对P项进行整定用途的，目的就是让P项的控制响应“软”下来，实际上就是一个经过改进的P控制器。读懂了sqrt_controller，那么你对APM的P控制器就了解得差不多了。仅讨论P控制器，sqrt_controller的作用就是设定一个量linear_dist，以这个量的正负值为分界线划分出一个线性区域范围，在这个范围内表示误差量还很小，遵循Kp*error计算方式输出控制量。超过linear_dist后，表明误差较大，此时不能由着误差单纯按照线性方式乘以

Ardupilot姿态控制器解析（二）—— 理论基础欧拉角阅读源码的过程中发现对于一些基础理论的掌握还是不够深，因此本篇先把一些理论知识部分汇总一下。本博文会持续更新修改，所有引用均会表明出处。欧拉角借一张已经被用烂了的图简单来说一下欧拉角的一些基本概念：欧拉角是利用3次独立的绕轴旋转运动来确定三维空间中的某一刚体姿态；欧拉角可以绕着固定轴（大地坐标系） 和 运动轴（机体坐标系） 两种类型的轴进行旋转，运动过程对于前者称为外旋，后者称为内旋；对于绕轴的顺序，科学界并没有明确的规定，实

APM的控制器主要分为attitude姿态控制器和pos位置控制器。这系列文章主要来讲一下姿态控制器内部的实现。本文主要目的是为了解析APM源码然后方便自己将其实现在自己的水下航行器控制器上的，因此将会以ArduCopter和ArduSub为例进行解析，其他无人机类型也是类似的，也可用作参考。一、PID姿态控制器1.1 Copter姿态控制官方原图在真正开始解析源码之前，可以看一下Ardupilot官方网站上开发者手册对于直升机姿态控制器的描述（虽然官方的文档已经很久没更新了，4.x版本之后的..

APM v4.x 版本PID控制器更新后的控制流程图。来源地址：Controls update

三维刚体运动、李群与李代数的一些知识点记录一、三维刚体旋转1.1 内积和外积1.2 旋转矩阵、变换矩阵和旋转向量1.2.1 旋转矩阵1.2.2 变换矩阵1.2.3 旋转向量1.3 欧拉角和四元数和姿态解算二、李群与李代数以下内容来源于《视觉SLAM十四讲》以及其他博文内容的汇总，所有出处均会表明。部分内容比较简略，因为写本文的目的是仅做自身学习记录用途，好记性不如烂笔头，方便自己回顾（之前都学过但是现在发现忘得差不多了...），当然如果对大家有帮助那再好不过。一、三维刚体旋转1.1 内积和外积

MAVLINK消息在Ardupilot中的接收和发送过程SCHED_TASKupdate_receive()update_send()由于现在网上很多的都是APM旧版本的解释，因此把自己的一些学习所得记录下来。截至写博客日期（2020/10/21），本文APM版本为4.0（针对各种车辆的）。SCHED_TASK关于MAVLINK消息在APM中的初始化过程，在之前的博文里面已经写过了：MAVLINK在Ardupilot中的初始化过程。建议先阅读一下，虽然写得不是很完整，但还是能理一下思路的，后面

QGC添加自定义组件并且发送自定义MAVLINK消息一、添加自定义组件1.1 在飞行界面添加组件1.2 实现组件事件1.3 在MOCK模拟链接中实现验证1.4 验证本文实现版本：QGC：Stable_V4.0;QT:5.12.6。下载和安装方式这边就不再赘述。侵删。一、添加自定义组件1.1 在飞行界面添加组件在QGC开发者文档中说明飞行视图的界面实现在FlightDisplayView.qml文件中在FlightDisplayView.qml文件中添加如下，可以在ToolStrip下

本次以ArduSub为例介绍如何向APM固件中添加新的飞行模式以方便后续开发，其他车辆类型操作相似，本篇也可用作参考。参考资料：Adding a New Flight ModeCreating newflight mode in Ardusub首先在ardupilot/ArduSub/文件路径下，新建一个用于添加新的飞行模式的文件control_newMode.cpp。然后，向内部添加代码如下：#include "Sub.h"bool Sub::newmode_init(){ retur

参考资料：Simulating Manual Control using MavrosArduSub S.I.T.LArdupilot SITL Simulator (Software in the Loop)本文简单来说明一下Ardusub在SITL下面配合MAVROS的仿真过程。SITL的相关基础操作和基本命令大伙就自己去看一下官方文档吧。MAVROS的安装这里就不再说了，详情可以看这篇：最新mavros详细安装教程（亲测可行）首先进入到ArduSub路径下cd ardupilot/A

MAVLINK在Ardupilot中的初始化过程前言一、_chan二、init_ardupilot()2.1 SYSID2.2 setup_console()2.3 setup_uarts()2.4 mavlink_delay_cb()本文将以ArduSub为例写一下在APM中MAVLINK是如何实现初始化的，其他车辆类型实现过程是基本一致的，也可当作参考。如有错误请及时告知。前言APM中MAVLINK的传输和控制主要实现在libraries/GCS_MAVLINK以及对应的车辆文件夹

APM源码和MAVLINK解析学习——重读stabilize和altholdstabilize modelinit()run()handle_attitude()MAVLINK消息包姿态信息传输过程之前写的模式都是基于master版本的，这次重读stable版本的Sub-4.0，想着重新把这部分写一下。这次打算把MAVLINK包接收也稍微点一下。写的过程中难免会有错，如果发现错误请务必提出。首先需要提到的一点是，在APM的源码中，具体的车辆类型文件夹如ArduCopter、Ardusub等都是作为

直接进入主题，今天被这个问题整得头皮发麻，主要网上还找不到类似的错误。这个问题在于你用的arm-none-eabi版本出错了，以我在用的Ardusub为例，目前截至我写博客为止支持的是arm-none-eabi的6.3.1，也就是下面所示的版本输入arm-none-eabi-gcc --version即可查看当前版本，正确的输出应该是而我一开始是先安装的PX 4，后面再安装的APM，在安装PX4时由于网速问题没能直接从.sh方式直接安装arm-none-eabi，后面为了图方便就自己安装了

Ardusub源码解析学习（五）—— althold modelalthold_init()althold_run()althold model：深度保持模式，需要配合深度计使用。stabilize mode 在系列的第一篇博文里面已经论述过了，因此这部分博文有些相同的细节将不会展开讨论。进入ardupilot\Ardusub\路径下，找到 control_althold.cpp 文件，内部头文件为 Sub.h，内部实现有3个函数：althold_init()、althold_run()和cont

Ardusub源码解析学习（五）—— 从manual model开始manual_init()manual_run()从本篇开始，将会陆续对Ardusub中各种模式进行介绍，stabilize model在系列第一篇博文已经介绍过了（如果有时间考虑再重新写一篇？），这边就直接从手动模式开始介绍吧（因为算是最简单的了...）。manual model：手动控制模式。Pixhawk烧写进Ardusub固件之后默认都是从这个模式进行启动。顾名思义，该模式下ROV的各种控制直接通过手柄实现，系统将不会自动

Ardusub源码解析学习（四）——IMU前言一、system.cpp1.1 车辆内部初始化1.2 Sub::init_ardupilot()1.3 Sub::startup_INS_ground()前言本文打算来写一下Sub下面IMU的具体驱动，当然对于其他类型的无人机实际上原理也是大同小异的，也可用作参考。在具体开始之前，想先从初始化方式开始，捋清楚了初始化顺序，到后面才不会觉得混乱。如有错误，请务必留言指出。一、system.cpp上一篇博文已经说过了，在init_ardupilot()

参考资料：Ardusb官方手册Sub: Rework joystick input and pilot input in generalFlight ModesArdusub支持多种飞行模式，但是其中一部分需要有相应的传感器支持才能运行。目前Ardusub的模式主要包括标准模式和高级模式，其中标准模式是得到了良好的支持，但是高级模式目前仍处于开发阶段，所以不建议在一般情况下使用。需要注意的是，Ardusub不同于其他空中的多旋翼无人机，不支持通过QGC控制俯仰和翻滚运动，但是可以游戏手柄加以控制

APM_Sub源码解析学习（三）——车辆类型一、前言二、class AP_HAL::HAL三、class AP_Vehicle四、class Sub4.1 .h4.2 .cpp五、总结一、前言本文就主要来讲一讲APM里面的车辆类型好了，当然是以Sub作为主要对象，不过前面的继承关系和基类基本都是一致的，也可以作为参考。Sub类定义在 Sub.h 文件内部，其向上继承自 AP_Vechile 类，这是所有无人机车辆类型（Copter、Rover…）的父类。同样，AP_Vechile向上也有相应的继承关

发现了一个好东西，国外一个支持水下机器人的开源仿真项目。本来把原文的大致内容翻译完想发到博客上的，但是考虑到IEEE的版权问题还是全删光了，这边就只是简单介绍一下这篇文章及其开源项目。原文链接如下：UUV Simulator: A Gazebo-based package for underwater intervention and multi-robot simulation由于现阶段的机器人开源仿真软件大多仅支持空中或者地面上的机器人系统，而对于水下机器人系统支持较少，因此本文的研究者研发了一套能

Ardusub源码解析学习（二）——电机库学习一、RC输入与输出1.1 RC Input1.2 RC Output二、电机库学习2.1 setup_motors()2.2 add_motor_raw_6dof()2.3 output_min()2.4 calc_thrust_to_pwm()2.5 output_to_motors()2.6 get_current_limit_max_throttle()2.7 output_armed_stabilizing()其他函数三、参考资料一、RC输入与输出

Ardusub源码解析学习（一）前言一、准备工作二、Ardusub.cpp解析2.1 scheduler table2.2 get_scheduler_tasks()2.3 fast_loop()2.3.1 flight_mode.cpp2.3.2 control_stabilize.cpp2.3.3 AC_AttitudeControl.cpp三、总结前言最近因为课题项目需要，正在学习水下机器人的控制。水下机器人实际上就是一个用于水下的无人机系统（当然有些不严谨哈，这里特指ROV），内