PX4姿态解算源码原理理解

PX4源码原理理解一．主要参考资料链接：1.1 取PX4源码一小部分姿态解算来进行讲解姿态解算源码中文注释：https://blog.csdn.net/zouxu634866/article/details/79806948(但是并不全面，下面有我添加注释的较为完整的版本1.2 姿态解算文件层级 Cmakelists是编译脚本为之后使用命令刷入固件提供方便 attitude_estimator_q_params是默认参数文件即在地面站可以看到的参数，调参调的就是这些attitude_estimator_q_main是功能代码所在的文件（源码最后） 二．涉及到的部分传感器以及数学知识2.1传感器介绍（imu单元，陀螺仪，加速度计，磁力计）数据来源2.1.1加速度计三轴加速度计是用来推出角度的（主要是横滚角和俯仰角）2.1.2陀螺仪（测量角速度）（主要是横滚角和俯仰角和偏航角）加速度计不会影响w的测量2.1.3磁力计（主要是偏航角）磁场的变化会导致磁阻传感器电阻值发生变化，很容易受到干扰到，得到的是数据当前磁场 三个传感器获取的数据进行整合即可得到准确姿态信息 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维平面上旋转一个角度 拓展到三维： 2.3.3四元数（现在的位置到期待的位置，找一个旋转足一次旋转到位）（图中的θ为旋转角度，u为旋转轴） 2.4.1三种方法的对比： 三．姿态解算原理3.1姿态解算简化流程：Subscribe→姿态解算→publishing（sensor_conbine) (算法) (vehicle_attitude)（556- 709） （437-457） 3.2完整流程3.2.1 q0（四元数）初始化获取（498-555行）3.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 //高精度的定时器。 在控制过程中多数环节都是使用经典的PID控制算法为 2. //了获取较为实时的响应最重要的时间变量，这就涉及如何获取高精度的时间问题。pixhawk中就有高精度的RTC（实时时钟），这个 3. //头文件就做了介绍 4. 5. 6. 7. #include 8. #include /滤波器/ 9. #include 10. #include 11. #include 12. #include 13. #include /包含一些错误警告的功能/ 14. #include 15. #include /性能评估/ 16. #include 17. #include 18. #include /通过uorb获取传感器数据，进行下一步姿态解算19. #include /我们在这个程序中解算的姿态就要发布进这个主要是/ 20. 21. #include /去看看有什么，后面讲位置估计时要用到/ 22. 23. extern “C” __EXPORT int attitude_estimator_q_main(int argc, char *argv[]); 24. 25. using math::Vector; /使用向量/ 26. using math::Matrix; /使用矩阵/ 27. using math::Quaternion; /使用四元数/ 28. 29. class AttitudeEstimatorQ; 30. 31. namespace attitude_estimator_q 32. { 33. AttitudeEstimatorQ instance; 34. } // namespace attitude_estimator_q attitude_estimator_q 35. 36. class AttitudeEstimatorQ 37. { 38. public: 39. / 40. * Constructor 41. / 42. AttitudeEstimatorQ(); 43. 44. / 45. * Destructor, also kills task. 46. / 47. ~AttitudeEstimatorQ(); 48. 49. / 50. * Start task. 51. * 52. * @return OK on success. 53. */ 54. int start(); //创建一个新的任务 55. 56. static void task_main_trampoline(int argc, char *argv[]); 57. 58. void task_main(); 59. 60. private: 61. const float _dt_min = 0.00001f; /dt是指更新数据的时间间隔，也就是离散pid公式中的T/ 62. const float _dt_max = 0.02f; 63. 64. bool _task_should_exit = false; /< if true, task should exit / 65. int _control_task = -1; /< task handle for task */ 66. 67. int _params_sub = -1; /与parameter_update有关/ 68. int _sensors_sub = -1; /这个变量是订阅传感器数据时用的句柄/ 69. int _global_pos_sub = -1; /这个变量是订阅地理位置数据时用的句柄/ 70. int _vision_sub = -1; /这个变量是视觉信息时用的句柄/ 71. int _mocap_sub = -1; /mocap=motion capture，动作捕捉/ 72. 73. orb_advert_t _att_pub = nullptr; /nullptr是c++11标准引入的更严谨的空指针，这里的att_pub是用于发布姿态信息的handle/ 74. 75. struct { 76. 77. /前面带w都是权重的意思/ 78. param_t w_acc; /这里的acc为加速度计的权重，用于后面互补滤波/ 79. param_t w_mag; /mag为磁力计/ 80. param_t w_ext_hdg; 81. param_t w_gyro_bias; /陀螺仪偏差/ 82. param_t mag_decl; /磁方位角/ 83. param_t mag_decl_auto; /利用gps自动获得磁方位角/ 84. param_t acc_comp; /加速度的补偿/ 85. param_t bias_max; /最大偏差/ 86. param_t ext_hdg_mode; /外部的航向使用模式，=1表示来自于视觉，=2表示来自mocap/ 87. } _params_handles{}; /< handles for interesting parameters 这个结构体里面全是一些系统参数*/ 88. /在px4的程序中获取系统参数的方法是用param_get和param_find去获取，而不是通过uorb/ 89. 90. 91. 92. /下面这些初始化的数据是用来存放从系统参数get过来的数据*/ 93. float _w_accel = 0.0f; 94. float _w_mag = 0.0f; 95. float _w_ext_hdg = 0.0f; 96. float _w_gyro_bias = 0.0f; 97. float _mag_decl = 0.0f; 98. bool _mag_decl_auto = false; 99. bool _acc_comp = false; 100. float _bias_max = 0.0f; 101. int32_t _ext_hdg_mode = 0; 102. 103. Vector<3> _gyro; /通过传感器获取的三轴的角速度/ 104. Vector<3> _accel; /通过加速度计获取的三轴的加速度/ 105. Vector<3> _mag; /通过磁力计获取的磁航向/ 106. 107. Vector<3> _vision_hdg; /通过视觉获取的handing/ 108. Vector<3> _mocap_hdg; /通过mocap获取的handing/ 109. 110. Quaternion _q; 111. Vector<3> _rates; /与上面_gyro不同的是这个用于存放修正后的绕三轴角速度/ 112. Vector<3> _gyro_bias; 113. 114. Vector<3> _vel_prev; /前一刻的速度/ 115. hrt_abstime _vel_prev_t = 0; /uint64_t的typedef，abstime意思为绝对时间，这里为记录前一时刻速度时的绝对时间/ 116. /用于后面根据速度差除以时间差求加速度/ 117. 118. Vector<3> _pos_acc; //运动加速度，注意：这个加速度不包括垂直方向的 119. 120. bool _inited = false; //初始化标识 121. bool _data_good = false;//数据可用 122. bool _ext_hdg_good = false;//外部航向可用 123. 124. void update_parameters(bool force); 125. 126. int update_subscriptions(); /只有声明，无定义？？？字面的意思是更新订阅信息？？/ 127. 128. bool init();//初始化函数，初始化旋转矩阵和四元数 129. 130. bool update(float dt);//dt是更新周期，这个函数是整个程序的核心 131. 132. //Update magnetic declination (in rads) immediately changing yaw rotation 133. //偏航角改变立刻更新磁方位角 134. void update_mag_declination(float new_declination); 135. }; 136. 137. 138. AttitudeEstimatorQ::AttitudeEstimatorQ() 139. { 140. //先用find匹配系统参数，然后用get拷贝系统的参数，之所以这样不会直接影响到系统参数，但是又可以正常使用系统参数。 141. 142. _params_handles.w_acc = param_find(“ATT_W_ACC”); //加速度计143. _params_handles.w_mag = param_find(“ATT_W_MAG”); //磁力计144. _params_handles.w_ext_hdg = param_find(“ATT_W_EXT_HDG”); 145. _params_handles.w_gyro_bias = param_find(“ATT_W_GYRO_BIAS”); 146. _params_handles.mag_decl = param_find(“ATT_MAG_DECL”); 147. _params_handles.mag_decl_auto = param_find(“ATT_MAG_DECL_A”); 148. _params_handles.acc_comp = param_find(“ATT_ACC_COMP”); 149. _params_handles.bias_max = param_find(“ATT_BIAS_MAX”); 150. _params_handles.ext_hdg_mode = param_find(“ATT_EXT_HDG_M”); 151. 152. _vel_prev.zero();/清零/ 153. _pos_acc.zero(); 154. 155. _gyro.zero(); 156. _accel.zero(); 157. _mag.zero(); 158. 159. _vision_hdg.zero();/通过视觉得到的航向清零/ 160. _mocap_hdg.zero();/通过mocap得到的航向清零/ 161. 162. _q.zero(); 163. _rates.zero(); 164. _gyro_bias.zero(); 165. 166. _vel_prev.zero(); 167. _pos_acc.zero(); 168. } 169. 170. /** 171. * Destructor, also kills task. 172. * 析构函数，也用于结束任务 173. / 174. AttitudeEstimatorQ::~AttitudeEstimatorQ() 175. { 176. if (_control_task != -1) { 177. / task wakes up every 100ms or so at the longest / 178. _task_should_exit = true; 179. 180. / wait for a second for the task to quit at our request / 181. unsigned i = 0; 182. 183. do { 184. / wait 20ms / 185. usleep(20000); 186. 187. / if we have given up, kill it / 188. if (++i > 50) { 189. px4_task_delete(_control_task); 190. break; 191. } 192. } while (_control_task != -1); 193. } 194. 195. attitude_estimator_q::instance = nullptr; 196. } 197. 198. int AttitudeEstimatorQ::start() 199. { 200. ASSERT(_control_task == -1); //AEESRT是assert函数的define了，作用是参数值为0时终止程序 201. 202. / start the task / 203. /这里的px4_task_spawn_cmd函数的作用是创建一个新的任务，属于nuttx系统中的，最为一个独立的进程，通过uorb与别的进程通信/ 204. _control_task = px4_task_spawn_cmd(“attitude_estimator_q”, //进程入口函数205. SCHED_DEFAULT, //进程默认调度206. SCHED_PRIORITY_ESTIMATOR, //进程的优先级207. 2000, //进程私有栈的大小208. (px4_main_t)&AttitudeEstimatorQ::task_main_trampoline, //进程入口函数209. Nullptr//进程参数列表); 210. 211. if (_control_task < 0) 212. { 213. warn(“task start failed”); 214. return -errno; 215. } 216. 217. return OK; 218. } 219. 220. void AttitudeEstimatorQ::task_main_trampoline(int argc, char argv[]) /运行task_main/ 221. { 222. attitude_estimator_q::instance->task_main(); 223. } 224. 225. 226. 227. 真正姿态结算代码所在228. void AttitudeEstimatorQ::task_main() 229. { 230. 231. #ifdef __PX4_POSIX 232. /加速度计的表现性能。。第一个参数是counter的类型，第二个是counter的名字/ 233. perf_counter_t _perf_accel(perf_alloc_once(PC_ELAPSED, “sim_accel_delay”)); 234. /mpu的表现性能/ 235. perf_counter_t _perf_mpu(perf_alloc_once(PC_ELAPSED, “sim_mpu_delay”)); 236. /mag的表现性能/ 237. perf_counter_t _perf_mag(perf_alloc_once(PC_ELAPSED, “sim_mag_delay”)); 238. #endif 239. 240. _sensors_sub = orb_subscribe(ORB_ID(sensor_combined));/订阅传感器信息/ 241. _vision_sub = orb_subscribe(ORB_ID(vehicle_vision_attitude));/订阅视觉信息/ 242. _mocap_sub = orb_supdbscribe(ORB_ID(att_pos_mocap));/订阅mocap动作捕捉信息/ 243. _params_sub = orb_subscribe(ORB_ID(parameter_update));/订阅parameter信息，用于之后的parameter_update的copy/ 244. _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));/订阅位置信息/ 245. 246. 247. /上面是订阅，copy部分在下面这个函数中/ 248. update_parameters(true); //进行参数更新，将参数文件里面的参数拷贝到当前进程里面来使用249. 250. hrt_abstime last_time = 0; 251. 252. 253. 254. /poll*/ 255. /nuttx任务使能**/ 256. px4_pollfd_struct_t fds[1] = {}; //阻塞等待某个数据257. fds[0].fd = _sensors_sub; //阻塞等待的句柄258. fds[0].events = POLLIN; //阻塞的方式（一种为pollin，一种为check轮询的方式检查更新，前者为必须拿到数据，不让不罢休。后者隔一段时间检查一下数据，这种数据相应来说没有polling要拿到的数据重要。259. 260. while (!_task_should_exit) 261. { 262. int ret = px4_poll(fds, 1, 1000); //以1000毫秒进行阻塞等待sensor_combine263. 264. if (ret < 0) { 265. // Poll error, sleep and try again ，出错了266. usleep(10000); 267. PX4_WARN(“POLL ERROR”); 268. continue; 269. 270. } else if (ret == 0) { 271. // Poll timeout, do nothing ,超时272. PX4_WARN(“POLL TIMEOUT”); 273. continue; 274. } 275. 276. update_parameters(false); //498行 参数为ture为强制更新，false为检查277. ，有更新才更新。对比313行278. // Update sensors 279. sensor_combined_s sensors; /新建的sensor结构体用于装复制的传感器信息/ 280. //从sensor_combine这个topic中取出数据放在sensors中281. if (orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors) == PX4_OK) { 282. // Feed validator with recent sensor data 283. 284. if (sensors.timestamp > 0) { 285. _gyro(0) = sensors.gyro_rad[0]; 286. _gyro(1) = sensors.gyro_rad[1]; 287. _gyro(2) = sensors.gyro_rad[2]; 288. } 289. 290. if (sensors.accelerometer_timestamp_relative != sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) { 291. _accel(0) = sensors.accelerometer_m_s2[0]; 292. _accel(1) = sensors.accelerometer_m_s2[1]; 293. _accel(2) = sensors.accelerometer_m_s2[2]; 294. 295. if (_accel.length() < 0.01f) { 296. PX4_ERR(“WARNING: degenerate accel!”); 297. continue; 298. } 299. } 300. 301. if (sensors.magnetometer_timestamp_relative != sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) { 302. _mag(0) = sensors.magnetometer_ga[0]; 303. _mag(1) = sensors.magnetometer_ga[1]; 304. _mag(2) = sensors.magnetometer_ga[2]; 305. 306. if (_mag.length() < 0.01f) { 307. PX4_ERR(“WARNING: degenerate mag!”); 308. continue; 309. } 310. } 311. 312. _data_good = true; 313. } 314. 315. // Update vision and motion capture heading 316. // 通过uORB模型获取vision和mocap的数据（视觉和mocap） 317. bool vision_updated = false; 318. orb_check(_vision_sub, &vision_updated); 319. 320. if (vision_updated) { 321. vehicle_attitude_s vision; /新建vision结构体/ 322. 323. if (orb_copy(ORB_ID(vehicle_vision_attitude), _vision_sub, &vision) == PX4_OK) { 324. math::Quaternion q(vision.q);//将基于视觉获得的四元数赋给新建的math：：q 325. 326. math::Matrix<3, 3> Rvis = q.to_dcm(); /基于视觉得到的转换矩阵/ 327. math::Vector<3> v(1.0f, 0.0f, 0.4f); /从他给定的v可知这里的v是一个指北的向量/ 328. /至于为什么初始给的是0.4,还有疑惑/ 329. 330. 331. 332. 333. // Rvis is Rwr (robot respect to world) while v is respect to world. 334. // Hence Rvis must be transposed having (Rwr)’ * Vw v是在地系下的，而dcm是b—>e,所以要转置 335. // Rrw * Vw = vn. This way we have consistency 336. _vision_hdg = Rvis.transposed() * v;/转置后与v乘，就反映了视觉上的指北的向量/ 337. 338. // vision external heading usage (ATT_EXT_HDG_M 1) 飞机偏航计算用视觉339. if (_ext_hdg_mode == 1) { 340. // Check for timeouts on data 检查数据超时 341. _ext_hdg_good = vision.timestamp > 0 && (hrt_elapsed_time(&vision.timestamp) < 500000); 342. } 343. } 344. } 345. 346. 347. /基于mocap获取handing同理/ 348. bool mocap_updated = false; 349. orb_check(_mocap_sub, &mocap_updated); 350. 351. if (mocap_updated) { 352. att_pos_mocap_s mocap; 353. 354. if (orb_copy(ORB_ID(att_pos_mocap), _mocap_sub, &mocap) == PX4_OK) { 355. math::Quaternion q(mocap.q); 356. math::Matrix<3, 3> Rmoc = q.to_dcm(); 357. 358. math::Vector<3> v(1.0f, 0.0f, 0.4f); 359. 360. // Rmoc is Rwr (robot respect to world) while v is respect to world. 361. // Hence Rmoc must be transposed having (Rwr)’ * Vw 362. // Rrw * Vw = vn. This way we have consistency 363. _mocap_hdg = Rmoc.transposed() * v; 364. 365. // Motion Capture external heading usage (ATT_EXT_HDG_M 2) 飞机偏航用动作捕捉366. if (_ext_hdg_mode == 2) { 367. // Check for timeouts on data 368. _ext_hdg_good = mocap.timestamp > 0 && (hrt_elapsed_time(&mocap.timestamp) < 500000); 369. } 370. } 371. } 372. 373. 374. //下面的是自动获取磁偏角 375. bool gpos_updated = false; 376. orb_check(_global_pos_sub, &gpos_updated); 377. 378. if (gpos_updated) {//如果位置已经更新 就取出位置数据 379. vehicle_global_position_s gpos; 380. 381. if (orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &gpos) == PX4_OK) 382. { 383. //首先检测是否配置了自动磁方位角获取，且gps数据精度很好即用下面的if，mag_decl_auto变量是是否根据当前gps获取当前磁偏角 384. if (_mag_decl_auto && gpos.eph < 20.0f && hrt_elapsed_time(&gpos.timestamp) < 1000000) { 385. /* set magnetic declination automatically / 386. //自动获取磁方位角，如果配置了则用当前的经纬度（longitude and latitude）通过get_mag_declination(_gpos.lat,_gpos.lon)函数获取当前位置的磁偏角 根据经纬度查表获取当前磁偏角387. update_mag_declination(math::radians(get_mag_declination(gpos.lat, gpos.lon))); 388. } 389. 390. //获取机体的速度，通过速度计算机体的加速度 ，gps速度算出加速度（地理坐标系）再旋转回机体坐标系acc_comp是否进行运动加速度测量 （加速度计=重力加速度+运动加速度，运动加速度由gps进行计算，gps得到速度）391. //先判断位置信息是否有效 392. if (_acc_comp && gpos.timestamp != 0 && hrt_absolute_time() < gpos.timestamp + 20000 && gpos.eph < 5.0f && _inited) 393. { 394. / position data is actual / 395. Vector<3> vel(gpos.vel_n, gpos.vel_e, gpos.vel_d); //北东地系 396. 397. / velocity updated / 398. if (_vel_prev_t != 0 && gpos.timestamp != _vel_prev_t) { //本次速度减去上次速度除以时间，后者算出的是地理坐标系上的，而前者得到的是机体坐标系上的，故得旋转399. float vel_dt = (gpos.timestamp - _vel_prev_t) / 1e6f; //us 400. / calculate acceleration in body frame / 401. //计算e系下的运动加速度，这里的pos_acc很重要，要用于后面加速度计的校正 402. _pos_acc = _q.conjugate_inversed((vel - _vel_prev) / vel_dt); 403. } 404. 405. _vel_prev_t = gpos.timestamp; //为迭代做准备 406. _vel_prev = vel; 407. 408. } 409. else {/否则位置信息过时，将加速度信息清零/ 410. / position data is outdated, reset acceleration / 411. _pos_acc.zero(); 412. _vel_prev.zero(); 413. _vel_prev_t = 0; 414. } 415. } 416. } 417. 418. / time from previous iteration （高精度定时器获取系统当前时间）/ 419. hrt_abstime now = hrt_absolute_time(); 420. 421. //下面的constrain函数用于限定作用，限定规则：return (val < min_val) ? min_val : ((val > max_val) ? max_val : val); 积分时间dt 422. const float dt = math::constrain((now - last_time) / 1e6f, _dt_min, _dt_max); 423. last_time = now; 424. 425. if (update(dt)) { 426. vehicle_attitude_s att = { 427. .timestamp = sensors.timestamp, 428. .rollspeed = _rates(0), 429. .pitchspeed = _rates(1), 430. .yawspeed = _rates(2), 431. 432. .q = {_q(0), _q(1), _q(2), _q(3)}, 433. .delta_q_reset = {}, 434. .quat_reset_counter = 0, 435. }; 436. 437. / the instance count is not used here / 438. //最后发布给了vehicle_attitude,这也对应了最开始我们说的我们不能直接把陀螺仪的数据当成姿态信息，而应该 439. //经过我们处理后才能使用。即vehicle_attitude的信息流为：订阅的是sensor combined，发布的是vehicle attitude 440. int att_inst; 441. orb_publish_auto(ORB_ID(vehicle_attitude), &_att_pub, &att, &att_inst, ORB_PRIO_HIGH); 442. } 443. } 444. 445. #ifdef __PX4_POSIX 446. perf_end(_perf_accel); 447. perf_end(_perf_mpu); 448. perf_end(_perf_mag); 449. #endif 450. 451. orb_unsubscribe(_params_sub); 452. orb_unsubscribe(_sensors_sub); 453. orb_unsubscribe(_global_pos_sub); 454. orb_unsubscribe(_vision_sub); 455. orb_unsubscribe(_mocap_sub); 456. } 457. 458. void AttitudeEstimatorQ::update_parameters(bool force) 459. { 460. bool updated = force; 461. 462. if (!updated) //如果更新了 463. { 464. orb_check(_params_sub, &updated); /***检查一个主题在发布者最后更新后，有没parameter_update有人调用过orb_copy来接收如果主题在被公告前就有人订阅，那么这个API将返回“not-updated”直到主题被公告。/ 465. } 466. 467. if (updated) //如果没更新 468. { 469. parameter_update_s param_update; /建立建构体param_update，里面有更新的时间、是否更新、填充信息/ 470. orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_update);/根据头文件可知parameter_update是自带的主题/ 471. /疑问：上面的copy之前为什么没有订阅，是不是后面参数更新后还要发布出去？其实不是，订阅发生在前面的task_main函数中/ 472. param_get(_params_handles.w_acc, &_w_accel);/把获得的参数值赋值给后面那个参数，前面那些参数的值都在构造函数中定义了/ 473. param_get(_params_handles.w_mag, &_w_mag); 474. param_get(_params_handles.w_ext_hdg, &_w_ext_hdg); 475. param_get(_params_handles.w_gyro_bias, &_w_gyro_bias); 476. 477. float mag_decl_deg = 0.0f; 478. param_get(_params_handles.mag_decl, &mag_decl_deg); 479. update_mag_declination(math::radians(mag_decl_deg));/radians函数为取弧度，外面的函数为消除偏差的积累/ 480. 481. int32_t mag_decl_auto_int; 482. param_get(_params_handles.mag_decl_auto, &mag_decl_auto_int); 483. _mag_decl_auto = (mag_decl_auto_int != 0); 484. 485. int32_t acc_comp_int; 486. param_get(_params_handles.acc_comp, &acc_comp_int); 487. _acc_comp = (acc_comp_int != 0); 488. 489. param_get(_params_handles.bias_max, &_bias_max); 490. param_get(_params_handles.ext_hdg_mode, &_ext_hdg_mode); 491. } 492. } 493. 494. 495. 496. /init（）函数作用：由加速度计和磁力计初始化旋转矩阵和四元数*/ 497. 498. bool AttitudeEstimatorQ::init() 499. { 500. // Rotation matrix can be easily constructed from acceleration and mag field vectors 501. // ‘k’ is Earth Z axis (Down) unit vector in body frame 502. //，加速度计为z轴所以取反向 503. Vector<3> k = -_accel; //accel已经在task_main中订阅 504. //k为加速度传感器测量到加速度方向向量，由于第一次测量数据时无人机一般为平稳状态无运动状态， 505. //所以可以直接将测到的加速度视为重力加速度g，以此作为dcm旋转矩阵的第三行k 506. 507. k.normalize(); //向量归一化，也就是向量除以他的长度 508. 509. 510. 511. // ‘i’ is Earth X axis (North) unit vector in body frame, orthogonal with ‘k’ 512. // i是体坐标系下指向正北的单位向量，与k正交 ，磁力计为x轴，i513. Vector<3> i = (_mag - k * (_mag * k));//施密特正交化,强制使i与k垂直;因向量k已归一化，故分母等于1; 514. //这里的_mag是指北的，所以下面求得的R是默认飞机机头也是指向北 515. i.normalize(); //向量归一化，也就是向量除以他的长度 516. 517. 518. // ‘j’ is Earth Y axis (East) unit vector in body frame, orthogonal with ‘k’ and ‘i’ 519. //j是e坐标系下指向正东的单位向量在b系下的向量，与k、i正交 ，通过叉乘算出y轴520. Vector<3> j = k % i; 521. 522. // 填充旋转矩阵，旋转矩阵代表两个坐标系的转换，几何意义代表一个在坐标系在另一个坐标系中的投影523. Matrix<3, 3> R; //新建一个33的矩阵R 524. 525. R.set_row(0, i); //set_row函数的第一个参数为第几行，将i向量填入矩阵第一行 526. R.set_row(1, j); //将j向量填入矩阵第二行 527. R.set_row(2, k); //将k向量填入矩阵第三行，注意：从这里可知该旋转矩阵为b系到n系 528. 529. // 将R矩阵转换为四元数 530. _q.from_dcm®; 531. 532. // Compensate for magnetic declination 补偿飞机的磁方位角，因为前面求得q是默认飞机指向北，但起飞时飞机不一定指向北 533. Quaternion decl_rotation; 534. decl_rotation.from_yaw(_mag_decl);/将旋转矩阵仅仅通过yaw偏航的四元数表示/ 535. _q = decl_rotation * _q; 536. 537. _q.normalize(); /归一化/ /此时的q才是真正的初始的q/ 538. 539. if (PX4_ISFINITE(_q(0)) && PX4_ISFINITE(_q(1)) && /判断是否发散/ 540. PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)) && 541. _q.length() > 0.95f && _q.length() < 1.05f) 542. { 543. _inited = true; /初始化完成/ 544. } 545. else 546. { 547. _inited = false; /初始化失败/ 548. } 549. 550. 551. return _inited; 552. } 553. 554. 555. 556. bool AttitudeEstimatorQ::update(float dt) 557. { 558. if (!_inited) 559. { 560. 561. if (!_data_good) /在前面task_main函数中如果传感器数据订阅正常，data_good就为true/ 562. { 563. return false; 564. } 565. 566. return init(); /没有初始化（前提：传感器数据订阅正常）就初始化/ 567. //并且注意：init在一次飞行trampoline中只执行一次，因为以后的四元数和转换矩阵由输出的角速度经过龙格库塔法求，而飞机在初始飞行时就需要通过init求转换矩阵 568. } 569. 570. 571. 572. 573. /前面的init等都完成就继续往下执行/ 574. Quaternion q_last = _q; /注意：此时四元数_q已经有内容了，此处的q_last的作用为给q弄一个备份，可从最后一个if看出/ 575. 576. // Angular rate of correction 修正角速率,下面的是重点 577. Vector<3> corr; 578. float spinRate = _gyro.length(); /定义一个变量：旋转速率，length函数为平方和开方/ 579. 580. 581. //首先判断是使用什么mode做修正的，比如vision、mocap、acc和mag，这里我们着重分析用acc和mag做修正，另外两个同理 582. if (_ext_hdg_mode > 0 && _ext_hdg_good) 583. { 584. if (_ext_hdg_mode == 1) 585. { 586. // Vision heading correction 对基于视觉的航向模式进行修正 587. // Project heading to global frame and extract XY component 588. Vector<3> vision_hdg_earth = _q.conjugate(_vision_hdg);/将b系转为e系/ 589. float vision_hdg_err = _wrap_pi(atan2f(vision_hdg_earth(1), vision_hdg_earth(0))); 590. // Project correction to body frame 591. corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -vision_hdg_err)) * _w_ext_hdg; 592. } 593. 594. if (_ext_hdg_mode == 2) { 595. // Mocap heading correction 596. // Project heading to global frame and extract XY component 597. Vector<3> mocap_hdg_earth = _q.conjugate(_mocap_hdg); 598. float mocap_hdg_err = _wrap_pi(atan2f(mocap_hdg_earth(1), mocap_hdg_earth(0))); 599. // Project correction to body frame 600. corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mocap_hdg_err)) * _w_ext_hdg; 601. } 602. } 603. 604. if (_ext_hdg_mode == 0 || !_ext_hdg_good) { 605. // Magnetometer correction 606. // Project mag field vector to global frame and extract XY component 607. Vector<3> mag_earth = _q.conjugate(_mag); /将b系转为机体系/ 608. 609. //只考虑Vector<3> mag_earth中的前两维的数据mag_earth(1)和mag_earth(0)（即x、y，忽略z轴上的偏移），通 610. //过arctan（mag_earth(1),mag_earth(0)）得到的角度和前面根据经纬度获取的磁方位角做差值得到误差角度mag_err 611. //_wrap_pi函数是用于限定结果-pi到pi的函数，大于pi则与2pi做差取补，小于-pi则与2pi做和取补 612. //注意1：这里在修正磁力计时用到的是有gps校准时的修正办法，即下面的减去自动获取的磁偏角。没有gps的校准方法见ppt 613. //注意2：这里校正的方法是用磁力计计算的磁偏角与gps得出来的做差，然后转换到b系。 614. float mag_err = _wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl);//与自动获取的磁方位角做差，即通过经纬度查表得到磁偏角，wrap-pi是进行归一化615. float gainMult = 1.0f; 616. const float fifty_dps = 0.873f; 617. 618. if (spinRate > fifty_dps) { 619. gainMult = math::min(spinRate / fifty_dps, 10.0f); 620. } 621. 622. // Project magnetometer correction to body frame 623. //下面Vector<3>(0.0f, 0.0f, -mag_err))是因为raw本质上应该由磁力计产生的水平磁向量与gps产生的水平磁向量叉乘得到，而叉乘得到的正好向下，磁力计的误差累计到了一起624. corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mag_err)) * _w_mag * gainMult; 625. } 626. 627. _q.normalize(); 628. 629. 630. // Accelerometer correction 631. // Project ‘k’ unit vector of earth frame to body frame 632. // Vector<3> k = _q.conjuq.conjugate_inversed(Vector<3>(0.0f, 0.0f, 1.0f)); 633. // Optimized version with dropped zeros 634. //下面的k是n系中重力的单位向量转换到b系中，即左乘旋转矩阵得到的 635. Vector<3> k( 636. 2.0f * (_q(1) * _q(3) - _q(0) * _q(2)), 637. 2.0f * (_q(2) * _q(3) + _q(0) * _q(1)), 638. (_q(0) * _q(0) - _q(1) * _q(1) - _q(2) * _q(2) + _q(3) * _q(3)) 639. ); 640. 641. //总的受到合力的方向（_accel）减去机体加速度方向（_pos_acc）得到g的方向，即总加速度（加速度获取）减去机体运动加速度获取重力加速度 642. //重力加速度的方向是导航坐标系下的z轴，加上运动加速度之后，总加速度的方向就不是与导航坐标系的天或地平行了，所以要消除这个误差，即“_accel-_pos_acc” 643. //这里与k差乘得到的是与重力方向的夹角sinx，约等于x，即roll和pitch 644. corr += (k % (_accel - _pos_acc).normalized()) * _w_accel; 645. 646. // Gyro bias estimation 647. if (spinRate < 0.175f) { 648. _gyro_bias += corr * (_w_gyro_bias * dt);//对应积分控制 649. 650. //对_gyro_bias做约束处理。 651. for (int i = 0; i < 3; i++) { 652. _gyro_bias(i) = math::constrain(_gyro_bias(i), -_bias_max, _bias_max); 653. } 654. 655. } 656. 657. _rates = _gyro + _gyro_bias;//角速度 = 陀螺仪的测量值 + 误差校准 658. 659. // pi控制器的体现，对应比例部分 660. corr += _rates;//corr为update函数中定义的变量，所以每次进入update函数中时会刷新corr变量的数据 661. 662. // Apply correction to state 663. //最后就是使用修正的数据更新四元数，并把_rates和_gyro_bias置零便于下次调用时使用。 664. _q += _q.derivative(corr) * dt;//用龙格库塔法更新四元数 665. 666. // Normalize quaternion 667. _q.normalize(); 668. 669. if (!(PX4_ISFINITE(_q(0)) && PX4_ISFINITE(_q(1)) && 670. PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)))) { 671. // 如果数据不合适就恢复备份的q，即q_last 672. _q = q_last; 673. _rates.zero(); 674. _gyro_bias.zero(); 675. return false; 676. } 677. 678. return true; 679. } //更新完四元数后，我们跳到update函数被调用的地方，即444行，发现后面将更新后的姿态信息发布出去了，到此整个过程结束 680. 681. 682. /偏航角改变立刻更新磁方位角/ 683. void AttitudeEstimatorQ::update_mag_declination(float new_declination) 684. { 685. // Apply initial declination or trivial rotations without changing estimation 686. // 忽略微小旋转（比如机体的微小震动），继续沿用之前的磁方位角 687. if (!_inited || fabsf(new_declination - _mag_decl) < 0.0001f) 688. { 689. _mag_decl = new_declination; 690. } 691. 692. else //转动较大时，修正姿态（q）和更新磁方位角 693. { 694. // Immediately rotate current estimation to avoid gyro bias growth 695. Quaternion decl_rotation; 696. decl_rotation.from_yaw(new_declination - _mag_decl);//偏航角生成的四元数，生成方法是令另外2个角度为0,使用欧拉角转四元数公式 697. _q = decl_rotation * _q;//此处两个四元数相乘表示角度相加，因为在数学上q1*q2表示q1q2这个合成动作，所以此处修正了四元数 698. _mag_decl = new_declination;//使用新的磁偏角 699. } 700. } 701. 702. 703. 704. //下面的总结起来：attitude_estimator_q { start }：实例化instance，运行instance->start()； 705. 706. //attitude_estimator_q { stop }：delete instance，指针置空； 707. 708. //attitude_estimator_q { status}：instance->print()，打印是否在running 709. 710. 711. 文件的入口函数：712. int attitude_estimator_q_main(int argc, char *argv[]) 713. { 714. if (argc < 2) /命令行总的参数个数<2,由后面可知有三个参数/ 715. { 716. warnx(“usage: attitude_estimator_q {start|stop|status}”); /**打印出错误用法 **/717. return 1; 718. } 719. 720. if (!strcmp(argv[1], “start”)) /用户输入的第一个参数是否为start，是的话即继续执行/ 721. { 722. 723. if (attitude_estimator_q::instance != nullptr) /不为空即代表已生成了内容，所以已经启动/ 724. { 725. warnx(“already running”); 726. return 1; 727. } 728. 729. attitude_estimator_q::instance = new AttitudeEstimatorQ;/instanc为空就申请空间，申请完成后不再为空指针/ 730. 731. if (attitude_estimator_q::instance == nullptr) /再一次判断是否为空/ 732. { 733. warnx(“alloc failed”); /是的话即分配空间失败/ 734. return 1; 735. } 736. 737. if (OK != attitude_estimator_q::instance->start()) /到这一步说明已经申请成功，start函数返回ok代表系统已经新建了一个进程任务，这里代表没有启动/ 738. { 739. delete attitude_estimator_q::instance; /没有启动就释放空间，重新变为空指针/ 740. attitude_estimator_q::instance = nullptr; 741. warnx(“start failed”); 742. return 1; 743. } 744. 745. return 0; 746. } 747. 748. if (!strcmp(argv[1], “stop”)) { 749. if (attitude_estimator_q::instance == nullptr) 750. { 751. warnx(“not running”); 752. return 1; 753. } 754. 755. delete attitude_estimator_q::instance; /能执行这一步说明instance不为空指针，任务已经执行，然后进行删除/ 756. attitude_estimator_q::instance = nullptr; /恢复原样/ 757. return 0; 758. } 759. 760. if (!strcmp(argv[1], “status”)) 761. { 762. if (attitude_estimator_q::instance) /这里代码补全应为if (attitude_estimator_q::instance != nullptr)/ 763. { 764. warnx(“running”); 765. return 0; 766. } 767. else 768. { 769. warnx(“not running”); 770. return 1; 771. } 772. } 773. 774. warnx(“unrecognized command”); /出去start、stop、status外其他的为unrecognized command/ 775. return 1; 776. }