PX1.主要参考资料链接：1.1 取PX4.解释一小部分姿态来解释中文注释：https://blog.csdn.net/zouxu634866/article/details/79806948(但不全面，以下是我添加注释的完整版本1.2 解决文件级别的姿态 Cmakelists是编译脚本为之后使用命令刷入固件提供方便 attitude_estimator_q_params是地面站可以看到的默认参数文件，这就是这些attitude_estimator_q_main是功能代码所在的文件(源码最后) 2.涉及的部分传感器和数学知识2.1传感器介绍（imu加速度计，单元，陀螺仪，磁计)数据源2.1.三轴加速度计用于推出角度(主要是横滚角和俯仰角)2.1.2陀螺仪(测量角速度)(主要是横滚角、俯仰角和偏航角)加速度计不会影响w的测量.1.磁场的变化(主要是偏航角)会导致磁阻传感器电阻值的变化，容易受到干扰，得到的是当前磁场的数据 集成三个传感器获得的数据以获得准确的态度信息 2.2姿态的三种表达方式以及两种坐标系2.2.1坐标系 PX4机体坐标系(前x轴，右y轴，下z轴):飞机上收集的数据基于机体坐标系的地理坐标系（ned坐标系为x轴，东为y轴，向下为z轴)在地面上观察飞机数据2.3.1欧拉角：Yaw偏航角(绕z轴)Pitch俯仰角(绕y轴)Roll横滚(绕x轴)实现一个坐标系向另一个坐标系的转换(航空航天领域规定zyx为欧拉角旋转顺序) 2.3.旋转矩阵2维平面旋转角度 拓展到三维： 2.3.34元(现在的位置到预期的位置，找一个旋转脚一次旋转到位)(图中θ旋转角度，u为旋转轴） 2.4.比较13种方法： 三、姿态解算原理3.简化姿态解算流程：Subscribe→姿态解算→publishing（sensor_conbine) (算法) (vehicle_attitude)（556- 709） （437-457） 3.2完整流程3.2.1 q初始化获取(498-555行)0.2.2 误差来源(635-645行)重力通过旋转矩阵到机体坐标系(635-640)取结果和加速度计（ax，ay，az）相比之下，叉乘的结果是否等于0，因为前提加速计只测量重力vx，vy，vz这是重力在身体上的分解。只有当加速度计测量重力在身体上的分解时，它实际上可能包括重力加速度和运动加速度，我们不需要运动加速度，因此减去运动加速度（运动加速度的值是由gps给出。）重力误差。）重力误差。 另一个误差-磁力计的误差(581磁力计获得的数据通过旋转矩阵返回机体，与gps比较工作良好时获得的经纬度查表获得的磁偏角数据，积累误差。3.2.3 将误差弥补在陀螺仪上，用陀螺仪数据(即w）重新解决四元数(代码是_q = _q.derivative(corr) * dt;也就是说，第一阶龙格-库塔法求解，更新后的四元数)得到这个四元数，姿态解算就完成了，最后数据填充发布。到目前为止，姿态解算已经完成 四、姿态解决完整源码注释分析：1. #include <drivers/drv_hrt.h> //高精度定时器。 在控制过程中，大多数环节使用经典PID控制算法为 2. //获得最重要的实时响应时间变量，这涉及到如何获得高精度时间。pixhawk有高精度RTC(实时时钟)，这个 3. ///头文件介绍 4. 5. 6. 7. #include <lib/geo/geo.h> 8. #include <mathlib/math/filter/LowPassFilter2p.hpp> // 9. #include <mathlib/mathlib.h> 10. #include <px4_config.h> 11. #include <px4_posix.h> 12. #include <px4_tasks.h> 13. #include <systemlib/err.h> // 14. #include <systemlib/param/param.h> 15. #include <systemlib/perf_counter.h> // 16. #include <uORB/topics/att_pos_mocap.h> 17. #include <uORB/topics/parameter_update.h> 18. #include <uORB/topics/sensor_combined.h> /< if true, task should exit / 65. int _control_task = -1; /