devbox
黑客灵魂
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

Mahony 互补滤波算法_mahony互补滤波算法

作者：黑客灵魂 | 2024-08-22 03:50:32

赞

踩

mahony互补滤波算法


http://blog.csdn.net/luoshi006/article/details/51513580

上接【互补滤波器】，继续学习互补滤波。。。。

参考：
Keeping a Good Attitude: A Quaternion-Based Orientation Filter for IMUs and MARGs.
PX4/Pixhawk—uORB深入理解和应用

应用场景

本文中 mahony 的应用场景为 多旋翼无人机的姿态估计。


mc

陀螺仪、加速度计、MPU6050 详述，请参考：传送门

名词解释

陀螺仪

陀螺仪，测量角速度，具有高动态特性，它是一个间接测量角度的器件。它测量的是角度的导数，即角速度，要将角速度对时间积分才能得到角度。由于噪声等误差影响，在积分作用下不断积累，最终导致陀螺仪的低频干扰和漂移。

加速度计

输出当前加速度（包含重力加速度 ）的方向【也叫重力感应器】，在悬停时，输出为 。由其测量原理导致的高频信号敏感，使得加速度计在振动环境中高频干扰较大。

磁力计

输出为当前机体与地磁场的夹角。测量原理与指南针相似。低频特性较好，易受周围磁场干扰。
磁力计的工作原理参考：WalkAnt的博客

坐标系

导航坐标系：在多旋翼中，又叫地球坐标系、地理坐标系。通常，采用北东地（NED）构成坐标系的 轴。

机体坐标系 ：固联在多旋翼飞行器上，即，坐标系原点定位于飞行器中心点（假设中心点与重心点重合）。

关于航空导航用到的坐标系，请参考 AIAA 系列丛书。在多旋翼中，因为只在低空飞行，且时间较短，只需要以上两个。

姿态表示

欧拉角 ：较直观，描述刚体在三维欧式空间中的姿态。此时，坐标系为机体坐标系，随着刚体的旋转而旋转。缺点：万向节锁。
详情参加：Wikipedia 传送门

四元数：由一组四维向量，表示刚体的三维旋转。适合用于计算机计算。
详情参加：Wikipedia 传送门

方向余弦矩阵DCM：用欧拉角余弦值或四元数，表示的坐标系的旋转。

mahony 滤波原理

互补滤波要求两个信号的干扰噪声处在不同的频率，通过设置两个滤波器的截止频率，确保融合后的信号能够覆盖需求频率。
在 IMU 的姿态估计中，互补滤波器对陀螺仪（低频噪声）使用高通滤波；对加速度/磁力计（高频噪声）使用低频滤波。
（此处尚未对传感器数据实测，噪声和有用频率未知。。。。待后期补足）

互补滤波中，两个滤波器的截止频率一致，此时就需要根据有用频率的值对其进行取舍。

机体水平时，加速度计无法测量绕 Z 轴的旋转量，即偏航角。磁力计也有同样问题，无法测得要磁轴的旋转量。故，需要加速度计和磁力计同时对陀螺仪进行校正。

mahony 滤波主要过程

式中， 表示系统姿态估计的四元数表示； 是经过 PI 调节器产生的新息； 表示实测的惯性向量 和预测的向量 之间的相对旋转（误差）。
—— pure quaternion operator（四元数实部为0），表示只有旋转。

PI 调节器中：[20160901更新]

  • 参数 用于控制加速度计和陀螺仪之间的交叉频率；
  • 参数 用于校正陀螺仪误差。

预备知识

主要是公式，不包含推导过程。。。。

欧拉角与机体角速度的关系：


旋转矩阵与机体角速度的关系：

四元数与机体角速度的关系：

参考：北航全权老师课件 第五章内容、惯性导航（秦永元）第九章内容。

预测

与卡尔曼滤波相似，mahony 滤波也分为预测-校正。
在预测环节，由三轴陀螺仪测得的角速度，通过式（1）计算出四元数姿态预测。 表示从地球坐标系到机体坐标系，或机体坐标系姿态在地球坐标系下的表示。

校正

在预测环节得到的四元数 ，通过加速度计和磁力计的值进行校正。该环节通常分为两部分：

  1. 通过加速度计得到 ，然后校正四元数中的横滚（roll）和俯仰（pitch）分量。
  2. 当磁力计可读时，通过 校正四元数中的偏航（yaw）分量。

加速度计校正

加速度计信号首先经过低通滤波器（消除高频噪声）：

然后，对得到的结果进行归一化（normalized）

计算偏差：

式中， 表示重力向量在机体坐标系的向量。

此时，由 与加速度计向量垂直分量叉乘，得到误差值。【两个物理意义相同的向量，理论上叉乘为零】

磁力计校正

数据预处理与加速度计相同，先滤波，然后归一化，得到 。

1. 无 GPS 校准时：

计算误差：

式中， 计算过程如下：

磁力计的输出（）在机体坐标系下，将其转换到导航坐标系：

导航坐标系的 轴与正北对齐，故，可以将磁力计在 平面的投影折算到 轴。

再次变换到机体坐标系：

2. 有 GPS 校准时：

在 px4 中，磁力计使用 GPS 信息 进行校准，故，公式与加速度计相同。

此时，由 与磁力计向量叉乘，得到误差值。【两个物理意义相同的向量，理论上叉乘为零】

更新四元数

由加速度计和磁力计校准得到的误差值：

由该误差值得到修正值：

修正后的角速度值：

根据一阶龙格库塔方法求解一阶微分方程：

可以求出四元数微分方程的差分形式：

四元数规范化：

源码分析

该部分源码直接截取 px4 开源飞控源码（BSD证书）。
px4 为 pixhawk 飞控的固件代码，内部涉及很多滤波及导航的算法。有较大参考价值。

源码，参考日期：20160603；

https://github.com/hsteinhaus/PX4Firmware/blob/master/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp

https://github.com/ArduPilot/PX4Firmware/blob/master/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp

参数默认值 
ATT_W_ACC0.2f加速度计权重
ATT_W_MAG0.1f磁力计权重
ATT_W_EXT_HDG0.1f外部航向权重
ATT_W_GYRO_BIAS0.1f陀螺仪偏差权重
ATT_MAG_DECL0.0f磁偏角（°）
ATT_MAG_DECL_A1启用基于GPS的自动磁偏角校正
ATT_EXT_HDG_M0外部航向模式
ATT_ACC_COMP1启用基于GPS速度的加速度补偿
ATT_BIAS_MAX0.05f陀螺仪偏差上限
ATT_VIBE_THRESH0.2f振动水平报警阈值

主程序运行流程图：

这里写图片描述

函数功能简述

  1. AttitudeEstimatorQ::AttitudeEstimatorQ();

    构造函数，初始化参数;

  2. AttitudeEstimatorQ::~AttitudeEstimatorQ();

    析构函数，杀掉所有任务;

  3. int AttitudeEstimatorQ::start();

    启动【attitude_estimator_q】进程，主函数入口： task_main_trampoline

  4. void AttitudeEstimatorQ::print();

    打印姿态信息；

  5. void AttitudeEstimatorQ::task_main_trampoline(int argc, char *argv[])

    {

    attitude_estimator_q::instance->task_main();

    }

  6. void AttitudeEstimatorQ::task_main()；

    主任务进程;

  7. void AttitudeEstimatorQ::update_parameters(bool force);

    false: 查看参数是否更新；

    true: 获取新参数, 并由磁偏角更新四元数；

  8. bool AttitudeEstimatorQ::init();

    由加速度计和磁力计初始化旋转矩阵，有GPS时，校正磁偏角。

  9. bool AttitudeEstimatorQ::update(float dt);

    调用init(); 互补滤波

  10. void AttitudeEstimatorQ::update_mag_declination(float new_declination);

    使用磁偏角更新四元数

  11. int attitude_estimator_q_main(int argc, char *argv[]);

    attitude_estimator_q { start }：实例化instance，运行instance->start()；

    attitude_estimator_q { stop }：delete instance，指针置空；

    attitude_estimator_q { status}：instance->print()，打印姿态信息。

源码分析

  • 此处源码逐行分析，可以使用 Ctrl+f 快速定位;
  • uORB 相关的数据结构，请参考 px4/Firmware/msg; 
  1. /*代码前的注释段（L34~L40）
  2.  * @file attitude_estimator_q_main.cpp
  3.  *
  4.  * Attitude estimator (quaternion based)
  5.  *姿态估计（基于四元数）
  6.  * @author Anton Babushkin <anton.babushkin@me.com>
  7.  */

 
 
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7

 
 
 
 
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7

头文件

  1. （l42~l76）
  2. #include <px4_config.h>
  3. #include <px4_posix.h>//add missing check;
  4. #include <unistd.h>
  5. #include <stdlib.h>
  6. #include <stdio.h>
  7. #include <stdbool.h>
  8. #include <poll.h>
  9. #include <fcntl.h>
  10. #include <float.h>
  11. #include <errno.h>
  12. #include <limits.h>
  13. #include <math.h>
  14. #include <uORB/uORB.h>
  15. #include <uORB/topics/sensor_combined.h>
  16. #include <uORB/topics/vehicle_attitude.h>
  17. #include <uORB/topics/control_state.h>
  18. #include <uORB/topics/vehicle_control_mode.h>
  19. #include <uORB/topics/vehicle_global_position.h>
  20. #include <uORB/topics/vision_position_estimate.h>//视觉位置估计, 未找到文件【待查】;
  21. #include <uORB/topics/att_pos_mocap.h>//mocap-->vicon;
  22. #include <uORB/topics/airspeed.h>
  23. #include <uORB/topics/parameter_update.h>
  24. #include <uORB/topics/estimator_status.h>
  25. #include <drivers/drv_hrt.h>
  26.  
  27. #include <mathlib/mathlib.h>
  28. #include <mathlib/math/filter/LowPassFilter2p.hpp>
  29. #include <lib/geo/geo.h>
  30. #include <lib/ecl/validation/data_validator_group.h>
  31.  
  32. #include <systemlib/systemlib.h>
  33. #include <systemlib/param/param.h>
  34. #include <systemlib/perf_counter.h>
  35. #include <systemlib/err.h>
  36. #include <systemlib/mavlink_log.h>

 
 
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36

 
 
 
 
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36

using @@@

  1. （l78~l82）
  2. extern "C" __EXPORT int attitude_estimator_q_main(int argc, char *argv[]);
  3.  
  4. using math::Vector;
  5. using math::Matrix;
  6. using math::Quaternion;

 
 
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6

 
 

  
  
 
 

 
 
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6

此处，extern “C” 表示以 C 格式编译； __EXPORT 表示 将函数名输出到链接器（Linker）； using 关键字 表示引入名称到 using 说明出现的声明区域。。

__export
This keyword aids those programming Microsoft Windows. __export causes the function name to be exported to the linker.

namespace attitude_estimator_q

  1. (l84~l89)
  2. class AttitudeEstimatorQ;
  3.  
  4. namespace attitude_estimator_q
  5. {
  6. AttitudeEstimatorQ *instance;
  7. }//定义命名空间，通过命名空间调用instance;

 
 
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7

 
 

  
  
 
 

 
 
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7

类定义: class AttitudeEstimatorQ

  1.       (l92~l210)
  2. class AttitudeEstimatorQ
  3. {//类定义; 
  4. public:
  5.     AttitudeEstimatorQ();
  6.     //构造函数
  7.     ~AttitudeEstimatorQ();
  8.     //析构函数
  9.      int        start();
  10.     //开始任务，成功--返回OK；
  11.     static void task_main_trampoline(int argc, char *argv[]);
  12.     //跳转到 task_main() ，未使用传入参数；static函数只能被本文件中的函数调用；
  13.     void        task_main();
  14.     void        print();
  15. private:
  16.     static constexpr float _dt_max = 0.02;//最大时间间隔;
  17.     //constexpr(constant expression)常数表达式，c11新关键字；
  18.     //优化语法检查和编译速度；
  19.     bool        _task_should_exit = false;
  20.     //如果为 true ，任务退出；
  21.     int     _control_task = -1;
  22.     //进程ID, 默认-1表示没有任务；
  23.     int     _sensors_sub = -1;//sensor_combined subscribe(订阅);
  24.     int     _params_sub = -1;//parameter_update subscribe;
  25.     int     _vision_sub = -1;//视觉位置估计;
  26.     int     _mocap_sub = -1;//vicon姿态位置估计;
  27.     int     _airspeed_sub = -1;//airspeed subscribe;
  28.     int     _global_pos_sub = -1;//vehicle_global_position subscribe;
  29.     orb_advert_t    _att_pub = nullptr;//vehicle_attitude publish(发布);
  30.     orb_advert_t    _ctrl_state_pub = nullptr;//发布控制状态主题control_state;
  31.     orb_advert_t    _est_state_pub = nullptr;//estimator_status
  32.  
  33.     struct {
  34.         param_t w_acc;//ATT_W_ACC
  35.         param_t w_mag;//ATT_W_MAG
  36.         param_t w_ext_hdg;//ATT_W_EXT_HDG 外部航向权重;
  37.         param_t w_gyro_bias;//ATT_W_GYRO_BIAS
  38.         param_t mag_decl;//ATT_MAG_DECL
  39.         param_t mag_decl_auto;//ATT_MAG_DECL_A 磁偏角自动校正;
  40.         param_t acc_comp;//ATT_ACC_COMP
  41.         param_t bias_max;//ATT_BIAS_MAX 陀螺仪偏差上限;
  42.         param_t vibe_thresh;//ATT_VIBE_THRESH 振动报警阈值;
  43.         param_t ext_hdg_mode;//ATT_EXT_HDG_M 外部航向模式;
  44.     }       _params_handles;
  45.     //有用参数的句柄；
  46.  
  47.     float       _w_accel = 0.0f;
  48.     //ATT_W_ACC >>> w_acc >>> _w_accel;
  49.     float       _w_mag = 0.0f;
  50.     float       _w_ext_hdg = 0.0f;
  51.     float       _w_gyro_bias = 0.0f;
  52.     float       _mag_decl = 0.0f;
  53.     bool        _mag_decl_auto = false;
  54.     bool        _acc_comp = false;
  55.     float       _bias_max = 0.0f;
  56.     float       _vibration_warning_threshold = 1.0f;//振动警告阈值;
  57.     hrt_abstime _vibration_warning_timestamp = 0;
  58.     int     _ext_hdg_mode = 0;
  59.  
  60.     Vector<3>   _gyro;//陀螺仪;
  61.     Vector<3>   _accel;//加速度计;
  62.     Vector<3>   _mag;//磁力计;
  63.  
  64.     vision_position_estimate_s _vision = {};//buffer;
  65.     Vector<3>   _vision_hdg;
  66.  
  67.     att_pos_mocap_s _mocap = {};//buffer;
  68.     Vector<3>   _mocap_hdg;
  69.  
  70.     airspeed_s _airspeed = {};//buffer;
  71.  
  72.     Quaternion  _q;//四元数;
  73.     Vector<3>   _rates;//姿态角速度;
  74.     Vector<3>   _gyro_bias;//陀螺仪偏差;
  75.  
  76.     vehicle_global_position_s _gpos = {};//buffer;
  77.     Vector<3>   _vel_prev;//前一时刻的速度：
  78.     Vector<3>   _pos_acc;//加速度（body frame??）
  79.  
  80.     DataValidatorGroup _voter_gyro;//数据验证，剔除异常值;
  81.     DataValidatorGroup _voter_accel;
  82.     DataValidatorGroup _voter_mag;
  83.  
  84.     //姿态速度的二阶低通滤波器；
  85.     math::LowPassFilter2p _lp_roll_rate;
  86.     math::LowPassFilter2p _lp_pitch_rate;
  87.     math::LowPassFilter2p _lp_yaw_rate;
  88.  
  89.     //绝对时间(ms)
  90.     hrt_abstime _vel_prev_t = 0;//前一时刻计算速度时的绝对时间;
  91.  
  92.     bool        _inited = false;//初始化标识;
  93.     bool        _data_good = false;//数据可用;
  94.     bool        _failsafe = false;//故障保护;
  95.     bool        _vibration_warning = false;//振动警告;
  96.     bool        _ext_hdg_good = false;//外部航向可用;
  97.  
  98.     orb_advert_t    _mavlink_log_pub = nullptr;//mavlink log advert;
  99.  
  100.     //performance measuring tools (counters)
  101.     perf_counter_t _update_perf;
  102.     perf_counter_t _loop_perf;//未看到使用。。。;
  103.  
  104.     void update_parameters(bool force);//更新参数;
  105.  
  106.     int update_subscriptions();//未使用【待查？？】
  107.  
  108.     bool init();
  109.  
  110.     bool update(float dt);
  111.  
  112.     // 偏航角旋转后，立即更新磁偏角；
  113.     void update_mag_declination(float new_declination);
  114. };

 
 
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63
  • 64
  • 65
  • 66
  • 67
  • 68
  • 69
  • 70
  • 71
  • 72
  • 73
  • 74
  • 75
  • 76
  • 77
  • 78
  • 79
  • 80
  • 81
  • 82
  • 83
  • 84
  • 85
  • 86
  • 87
  • 88
  • 89
  • 90
  • 91
  • 92
  • 93
  • 94
  • 95
  • 96
  • 97
  • 98
  • 99
  • 100
  • 101
  • 102
  • 103
  • 104
  • 105
  • 106
  • 107
  • 108
  • 109
  • 110
  • 111
  • 112
  • 113
  • 114

 
 
 
 
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63
  • 64
  • 65
  • 66
  • 67
  • 68
  • 69
  • 70
  • 71
  • 72
  • 73
  • 74
  • 75
  • 76
  • 77
  • 78
  • 79
  • 80
  • 81
  • 82
  • 83
  • 84
  • 85
  • 86
  • 87
  • 88
  • 89
  • 90
  • 91
  • 92
  • 93
  • 94
  • 95
  • 96
  • 97
  • 98
  • 99
  • 100
  • 101
  • 102
  • 103
  • 104
  • 105
  • 106
  • 107
  • 108
  • 109
  • 110
  • 111
  • 112
  • 113
  • 114

构造函数

  1.      (l213~l235)
  2. AttitudeEstimatorQ::AttitudeEstimatorQ() :
  3.     _vel_prev(0, 0, 0),
  4.     _pos_acc(0, 0, 0),
  5.     _voter_gyro(3),//数据成员3个;
  6.     _voter_accel(3),
  7.     _voter_mag(3),
  8.     _lp_roll_rate(250.0f, 30.0f),//低通滤波（采样频率,截止频率）;
  9.     _lp_pitch_rate(250.0f, 30.0f),
  10.     _lp_yaw_rate(250.0f, 20.0f)
  11. {
  12.     _voter_mag.set_timeout(200000);//磁力计超时;
  13.  
  14.     //读取Attitude_estimator_q_params.c中的参数;
  15.     _params_handles.w_acc       = param_find("ATT_W_ACC");
  16.     _params_handles.w_mag       = param_find("ATT_W_MAG");
  17.     _params_handles.w_ext_hdg   = param_find("ATT_W_EXT_HDG");//外部航向权重;
  18.     _params_handles.w_gyro_bias = param_find("ATT_W_GYRO_BIAS");
  19.     _params_handles.mag_decl    = param_find("ATT_MAG_DECL");
  20.     _params_handles.mag_decl_auto   = param_find("ATT_MAG_DECL_A");//磁偏角自动校正;
  21.     _params_handles.acc_comp    = param_find("ATT_ACC_COMP");
  22.     _params_handles.bias_max    = param_find("ATT_BIAS_MAX");//陀螺仪偏差上限;
  23.     _params_handles.vibe_thresh = param_find("ATT_VIBE_THRESH");//振动警告阈值;
  24.     _params_handles.ext_hdg_mode    = param_find("ATT_EXT_HDG_M");
  25. }

 
 
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25

 
 
 
 
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25

析构函数

  1. l240~l262
  2. AttitudeEstimatorQ::~AttitudeEstimatorQ()
  3. {//杀掉所有任务;
  4.     if (_control_task != -1) {
  5.         /* task wakes up every 100ms or so at the longest */
  6.         _task_should_exit = true;
  7.  
  8.         /* wait for a second for the task to quit at our request */
  9.         unsigned i = 0;
  10.  
  11.         do {
  12.             /* wait 20ms */
  13.             usleep(20000);
  14.  
  15.             /* if we have given up, kill it */
  16.             if (++i > 50) {
  17.                 px4_task_delete(_control_task);
  18.                 break;
  19.             }
  20.         } while (_control_task != -1);
  21.     }
  22.  
  23.     attitude_estimator_q::instance = nullptr;
  24. }

 
 
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24

 
 

  
  
 
 

 
 
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24

start();

  1. l264~l282
  2. int AttitudeEstimatorQ::start()
  3. {
  4.     ASSERT(_control_task == -1);
  5.  
  6.     /* start the task */
  7.     //启动任务，返回进程ID;
  8.     _control_task = px4_task_spawn_cmd("attitude_estimator_q",/*name*/
  9.                        SCHED_DEFAULT,/*任务调度程序*/
  10.                        SCHED_PRIORITY_MAX - 5,/*优先级*/
  11.                        2500,/*栈大小*/
  12.                        (px4_main_t)&AttitudeEstimatorQ::task_main_trampoline,/*主函数入口*/
  13.                        nullptr);
  14.  
  15.     if (_control_task < 0) {
  16.         warn("task start failed");
  17.         return -errno;
  18.     }
  19.  
  20.     return OK;
  21. }

 
 
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21

 
 
 
 
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21

print();

  1. l284~l292
  2. void AttitudeEstimatorQ::print()
  3. {//打印当前姿态信息;
  4.     warnx("gyro status:");
  5.     _voter_gyro.print();
  6.     warnx("accel status:");
  7.     _voter_accel.print();
  8.     warnx("mag status:");
  9.     _voter_mag.print();
  10. }

 
 
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10

 
 

  
  
 
 

 
 
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10

task_main_trampoline();

  1. l294~l297
  2. void AttitudeEstimatorQ::task_main_trampoline(int argc, char *argv[])
  3. {
  4.     attitude_estimator_q::instance->task_main();
  5. }

 
 
  • 1
  • 2
  • 3
  • 4
  • 5

 
 

  
  
 
 

 
 
  • 1
  • 2
  • 3
  • 4
  • 5

task_main();

  1. l299~l655
  2. void AttitudeEstimatorQ::task_main()
  3. {
  4.  
  5. #ifdef __PX4_POSIX
  6. //记录事件执行所花费的时间，performance counters;
  7.     perf_counter_t _perf_accel(perf_alloc_once(PC_ELAPSED, "sim_accel_delay"));
  8.     perf_counter_t _perf_mpu(perf_alloc_once(PC_ELAPSED, "sim_mpu_delay"));
  9.     perf_counter_t _perf_mag(perf_alloc_once(PC_ELAPSED, "sim_mag_delay"));
  10. #endif
  11. //从uORB订阅主题;
  12.     _sensors_sub = orb_subscribe(ORB_ID(sensor_combined));
  13. //订阅传感器读数，包含数据参见：Firmware/msg/sensor_combined.msg
  14.     _vision_sub = orb_subscribe(ORB_ID(vision_position_estimate));//视觉;
  15.     _mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap));//vicon; 
  16.  
  17.     _airspeed_sub = orb_subscribe(ORB_ID(airspeed));//空速，见Firmware/msg/airspeed.msg;
  18.  
  19.     _params_sub = orb_subscribe(ORB_ID(parameter_update));//bool saved;
  20.     _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));//位置估计值(GPS);
  21.  
  22.     update_parameters(true);//获取新参数;
  23.  
  24.     hrt_abstime last_time = 0;
  25.  
  26.     px4_pollfd_struct_t fds[1] = {};
  27.     fds[0].fd = _sensors_sub;//文件描述符;
  28.     fds[0].events = POLLIN;//读取事件标识;
  29.     //POLLIN: data other than high-priority data may be read without blocking;
  30.     while (!_task_should_exit) {
  31.         int ret = px4_poll(fds, 1, 1000);
  32.         //timeout = 1000; fds_size = 1; 详见Linux的poll函数;
  33.         //对字符设备读写;
  34.  
  35.         if (ret < 0) {
  36.             // Poll error, sleep and try again
  37.             usleep(10000);
  38.             PX4_WARN("Q POLL ERROR");
  39.             continue;
  40.  
  41.         } else if (ret == 0) {
  42.             // Poll timeout, do nothing
  43.             PX4_WARN("Q POLL TIMEOUT");
  44.             continue;
  45.         }
  46.  
  47.         update_parameters(false);//检查orb是否更新;
  48.  
  49.         // Update sensors
  50.         sensor_combined_s sensors;
  51.  
  52.         int best_gyro = 0;
  53.         int best_accel = 0;
  54.         int best_mag = 0;
  55.  
  56.         if (!orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors)) {
  57.             // Feed validator with recent sensor data
  58.             //（put）将最近的传感器数据送入验证组（DataValidatorGroup）
  59.  
  60.             for (unsigned i = 0; i < (sizeof(sensors.gyro_timestamp) / sizeof(sensors.gyro_timestamp[0])); i++) {
  61.             //遍历每个陀螺仪数据;
  62.  
  63.                 /* ignore empty fields */
  64.                 //忽略空值;
  65.                 if (sensors.gyro_timestamp[i] > 0) {
  66.  
  67.                     float gyro[3];
  68.  
  69.                     for (unsigned j = 0; j < 3; j++) {
  70.                         if (sensors.gyro_integral_dt[i] > 0) {
  71.                         //delta time 大于零;
  72.                             gyro[j] = (double)sensors.gyro_integral_rad[i * 3 + j] / (sensors.gyro_integral_dt[i] / 1e6);
  73.                             //=角度/时间（1e6用于us->s转换）;
  74.                         } else {
  75.                             /* fall back to angular rate */
  76.                             //没有数据更新，回退;
  77.                             gyro[j] = sensors.gyro_rad_s[i * 3 + j];
  78.                         }
  79.                     }
  80.  
  81.                     _voter_gyro.put(i, sensors.gyro_timestamp[i], &gyro[0], sensors.gyro_errcount[i], sensors.gyro_priority[i]);
  82.                     //最后一个参数gyro_priority[]用于支持传感器优先级;
  83.                 }
  84.  
  85.                 /* ignore empty fields */
  86.                 if (sensors.accelerometer_timestamp[i] > 0) {
  87.                     _voter_accel.put(i, sensors.accelerometer_timestamp[i], &sensors.accelerometer_m_s2[i * 3],
  88.                              sensors.accelerometer_errcount[i], sensors.accelerometer_priority[i]);
  89.                 }
  90.                 //NED 坐标系下, 单位 m/s^2
  91.  
  92.                 /* ignore empty fields */
  93.                 if (sensors.magnetometer_timestamp[i] > 0) {
  94.                     _voter_mag.put(i, sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * 3],
  95.                                sensors.magnetometer_errcount[i], sensors.magnetometer_priority[i]);
  96.                 }
  97.                 //NED 坐标系下, 单位 Gauss
  98.             }
  99.  
  100.             // Get best measurement values
  101.             //获取最佳测量值（DataValidatorGroup）
  102.             hrt_abstime curr_time = hrt_absolute_time();
  103.             _gyro.set(_voter_gyro.get_best(curr_time, &best_gyro));
  104.             _accel.set(_voter_accel.get_best(curr_time, &best_accel));
  105.             _mag.set(_voter_mag.get_best(curr_time, &best_mag));
  106.  
  107.             if (_accel.length() < 0.01f) {
  108.                 warnx("WARNING: degenerate accel!");
  109.                 continue;
  110.             }
  111.             //退化，即非满秩，此处为奇异值（0）;
  112.  
  113.             if (_mag.length() < 0.01f) {
  114.                 warnx("WARNING: degenerate mag!");
  115.                 continue;
  116.             }//同上;
  117.  
  118.             _data_good = true;//数据可用;
  119.  
  120.             if (!_failsafe) {
  121.                 uint32_t flags = DataValidator::ERROR_FLAG_NO_ERROR;
  122.  
  123. #ifdef __PX4_POSIX
  124.                 perf_end(_perf_accel);//执行事件结束，计算均值方差等;
  125.                 perf_end(_perf_mpu);
  126.                 perf_end(_perf_mag);
  127. #endif
  128.  
  129.                 if (_voter_gyro.failover_count() > 0) {
  130.                 //陀螺仪数据故障计数大于0, 记录错误日志;
  131.                     _failsafe = true;
  132.                     flags = _voter_gyro.failover_state();
  133.                     mavlink_and_console_log_emergency(&_mavlink_log_pub, "Gyro #%i failure :%s%s%s%s%s!",
  134.                                       _voter_gyro.failover_index(),
  135.                                       ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),
  136.                                       ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""),
  137.                                       ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""),
  138.                                       ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""),
  139.                                       ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : ""));
  140.                 }
  141.  
  142.                 if (_voter_accel.failover_count() > 0) {
  143.                 //同上，故障日志;
  144.                     _failsafe = true;
  145.                     flags = _voter_accel.failover_state();
  146.                     mavlink_and_console_log_emergency(&_mavlink_log_pub, "Accel #%i failure :%s%s%s%s%s!",
  147.                                       _voter_accel.failover_index(),
  148.                                       ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),
  149.                                       ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""),
  150.                                       ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""),
  151.                                       ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""),
  152.                                       ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : ""));
  153.                 }
  154.  
  155.                 if (_voter_mag.failover_count() > 0) {
  156.                 //同上，故障日志;
  157.                     _failsafe = true;
  158.                     flags = _voter_mag.failover_state();
  159.                     mavlink_and_console_log_emergency(&_mavlink_log_pub, "Mag #%i failure :%s%s%s%s%s!",
  160.                                       _voter_mag.failover_index(),
  161.                                       ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),
  162.                                       ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""),
  163.                                       ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""),
  164.                                       ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""),
  165.                                       ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : ""));
  166.                 }
  167.  
  168.                 if (_failsafe) {
  169.                 //故障安全机制;
  170.                     mavlink_and_console_log_emergency(&_mavlink_log_pub, "SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY");
  171.                 }
  172.             }
  173.  
  174.             //若启用振动报警，且振动级别超过设定阈值，触发报警; 
  175.             //振动级别由数据的方均根（RMS）给出;
  176.             if (!_vibration_warning && (_voter_gyro.get_vibration_factor(curr_time) > _vibration_warning_threshold ||
  177.                             _voter_accel.get_vibration_factor(curr_time) > _vibration_warning_threshold ||
  178.                             _voter_mag.get_vibration_factor(curr_time) > _vibration_warning_threshold)) {
  179.  
  180.                 if (_vibration_warning_timestamp == 0) {
  181.                     _vibration_warning_timestamp = curr_time;
  182.  
  183.                 } else if (hrt_elapsed_time(&_vibration_warning_timestamp) > 10000000) {
  184.                     _vibration_warning = true;
  185.                     mavlink_and_console_log_critical(&_mavlink_log_pub, "HIGH VIBRATION! g: %d a: %d m: %d",
  186.                                      (int)(100 * _voter_gyro.get_vibration_factor(curr_time)),
  187.                                      (int)(100 * _voter_accel.get_vibration_factor(curr_time)),
  188.                                      (int)(100 * _voter_mag.get_vibration_factor(curr_time)));
  189.                 }
  190.  
  191.             } else {
  192.                 _vibration_warning_timestamp = 0;
  193.             }
  194.         }
  195.  
  196.         // Update vision and motion capture heading
  197.         //更新视觉和vicon航向
  198.         bool vision_updated = false;
  199.         orb_check(_vision_sub, &vision_updated);
  200.  
  201.         bool mocap_updated = false;
  202.         orb_check(_mocap_sub, &mocap_updated);
  203.  
  204.         if (vision_updated) {
  205.             orb_copy(ORB_ID(vision_position_estimate), _vision_sub, &_vision);//将订阅主题的内容复制到buffer(_vision)中;
  206.             math::Quaternion q(_vision.q);
  207.  
  208.             math::Matrix<3, 3> Rvis = q.to_dcm();
  209.             math::Vector<3> v(1.0f, 0.0f, 0.4f);
  210.             //没看出 v 向量具体含义，疑似磁偏校正;
  211.  
  212.             // Rvis is Rwr (robot respect to world) while v is respect to world.
  213.             // Hence Rvis must be transposed having (Rwr)' * Vw
  214.             // Rrw * Vw = vn. This way we have consistency
  215.             _vision_hdg = Rvis.transposed() * v;
  216.         }
  217.         //通过视觉得到的姿态估计q->Rvis，将v转换到机体坐标系;
  218.  
  219.         if (mocap_updated) {
  220.             orb_copy(ORB_ID(att_pos_mocap), _mocap_sub, &_mocap);
  221.             math::Quaternion q(_mocap.q);
  222.             math::Matrix<3, 3> Rmoc = q.to_dcm();
  223.  
  224.             math::Vector<3> v(1.0f, 0.0f, 0.4f);
  225.  
  226.             // Rmoc is Rwr (robot respect to world) while v is respect to world.
  227.             // Hence Rmoc must be transposed having (Rwr)' * Vw
  228.             // Rrw * Vw = vn. This way we have consistency
  229.             _mocap_hdg = Rmoc.transposed() * v;
  230.         }
  231.  
  232.         // Update airspeed
  233.         bool airspeed_updated = false;
  234.         orb_check(_airspeed_sub, &airspeed_updated);
  235.  
  236.         if (airspeed_updated) {
  237.             orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
  238.         }
  239.  
  240.         // Check for timeouts on data
  241.         if (_ext_hdg_mode == 1) {
  242.             _ext_hdg_good = _vision.timestamp_boot > 0 && (hrt_elapsed_time(&_vision.timestamp_boot) < 500000);
  243.  
  244.         } else if (_ext_hdg_mode == 2) {
  245.             _ext_hdg_good = _mocap.timestamp_boot > 0 && (hrt_elapsed_time(&_mocap.timestamp_boot) < 500000);
  246.         }
  247.  
  248.         bool gpos_updated;
  249.         orb_check(_global_pos_sub, &gpos_updated);
  250.  
  251.         if (gpos_updated) {
  252.             orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_gpos);
  253.  
  254.             if (_mag_decl_auto && _gpos.eph < 20.0f && hrt_elapsed_time(&_gpos.timestamp) < 1000000) {
  255.                 /* set magnetic declination automatically */
  256.                                 update_mag_declination(math::radians(get_mag_declination(_gpos.lat, _gpos.lon)));
  257.             }
  258.             //磁偏自动校正，且水平偏差的标准差小于20，根据位置估计值(GPS)（vehicle_global_position）校正磁偏角;
  259.             //get_mag_declination()函数查表得到地磁偏角，进行补偿。
  260.         }
  261.         if (_acc_comp && _gpos.timestamp != 0 && hrt_absolute_time() < _gpos.timestamp + 20000 && _gpos.eph < 5.0f && _inited) {
  262.             /* position data is actual */
  263.             //基于GPS的位置信息，微分得到加速度值;
  264.             if (gpos_updated) {
  265.                 Vector<3> vel(_gpos.vel_n, _gpos.vel_e, _gpos.vel_d);
  266.  
  267.                 /* velocity updated */
  268.                 if (_vel_prev_t != 0 && _gpos.timestamp != _vel_prev_t) {
  269.                     float vel_dt = (_gpos.timestamp - _vel_prev_t) / 1000000.0f;//时间间隔，单位（s）
  270.                     /* calculate acceleration in body frame */
  271.                     _pos_acc = _q.conjugate_inversed((vel - _vel_prev) / vel_dt);
  272.                 }//由ned坐标系下的速度求出机体坐标系下的加速度;
  273.  
  274.                 _vel_prev_t = _gpos.timestamp;
  275.                 _vel_prev = vel;
  276.             }
  277.  
  278.         } else {
  279.             /* position data is outdated, reset acceleration */
  280.             //位置信息已过时，重置;
  281.             _pos_acc.zero();
  282.             _vel_prev.zero();
  283.             _vel_prev_t = 0;
  284.         }
  285.  
  286.         /* time from previous iteration */
  287.         hrt_abstime now = hrt_absolute_time();
  288.         float dt = (last_time > 0) ? ((now  - last_time) / 1000000.0f) : 0.00001f;//用极小值0.00001表示零，预防除零错误;
  289.         last_time = now;
  290.  
  291.         if (dt > _dt_max) {
  292.             dt = _dt_max;
  293.         }//时间间隔上限;
  294.  
  295.         if (!update(dt)) {
  296.             continue;
  297.         }//调用update(dt)，**互补滤波**，更新四元数;
  298.         //############若不熟悉update()，请转到函数查看;
  299.  
  300.         Vector<3> euler = _q.to_euler();
  301.  
  302.         struct vehicle_attitude_s att = {};
  303.         att.timestamp = sensors.timestamp;
  304.  
  305.         att.roll = euler(0);
  306.         att.pitch = euler(1);
  307.         att.yaw = euler(2);
  308.  
  309.         att.rollspeed = _rates(0);
  310.         att.pitchspeed = _rates(1);
  311.         att.yawspeed = _rates(2);
  312.  
  313.         for (int i = 0; i < 3; i++) {
  314.             att.g_comp[i] = _accel(i) - _pos_acc(i);
  315.         }//补偿重力向量;
  316.  
  317.         /* copy offsets */
  318.         memcpy(&att.rate_offsets, _gyro_bias.data, sizeof(att.rate_offsets));
  319.         //memcpy(*dest,*src,size);
  320.  
  321.         Matrix<3, 3> R = _q.to_dcm();
  322.  
  323.         /* copy rotation matrix */
  324.         memcpy(&att.R[0], R.data, sizeof(att.R));
  325.         att.R_valid = true;
  326.         memcpy(&att.q[0], _q.data, sizeof(att.q));
  327.         att.q_valid = true;
  328.         //获取姿态振动, RMS;
  329.         att.rate_vibration = _voter_gyro.get_vibration_factor(hrt_absolute_time());
  330.         att.accel_vibration = _voter_accel.get_vibration_factor(hrt_absolute_time());
  331.         att.mag_vibration = _voter_mag.get_vibration_factor(hrt_absolute_time());
  332.  
  333.         /* the instance count is not used here */
  334.         int att_inst;
  335.         orb_publish_auto(ORB_ID(vehicle_attitude), &_att_pub, &att, &att_inst, ORB_PRIO_HIGH);
  336.         //广播姿态信息;
  337.  
  338.         {//使用当前姿态，更新control_state，并发布;
  339.             struct control_state_s ctrl_state = {};
  340.  
  341.             ctrl_state.timestamp = sensors.timestamp;
  342.  
  343.             /* attitude quaternions for control state */
  344.             ctrl_state.q[0] = _q(0);
  345.             ctrl_state.q[1] = _q(1);
  346.             ctrl_state.q[2] = _q(2);
  347.             ctrl_state.q[3] = _q(3);
  348.  
  349.             /* attitude rates for control state */
  350.             //低通滤波，输入参数为当前值;
  351.             ctrl_state.roll_rate = _lp_roll_rate.apply(_rates(0));
  352.  
  353.             ctrl_state.pitch_rate = _lp_pitch_rate.apply(_rates(1));
  354.  
  355.             ctrl_state.yaw_rate = _lp_yaw_rate.apply(_rates(2));
  356.  
  357.             /* Airspeed - take airspeed measurement directly here as no wind is estimated */
  358.             if (PX4_ISFINITE(_airspeed.indicated_airspeed_m_s) && hrt_absolute_time() - _airspeed.timestamp < 1e6
  359.                 && _airspeed.timestamp > 0) {
  360.                 ctrl_state.airspeed = _airspeed.indicated_airspeed_m_s;
  361.                 ctrl_state.airspeed_valid = true;
  362.  
  363.             } else {
  364.                 ctrl_state.airspeed_valid = false;
  365.             }
  366.  
  367.             /* the instance count is not used here */
  368.             int ctrl_inst;
  369.             /* publish to control state topic */
  370.             //发布控制状态主题,control_state.msg。
  371.             orb_publish_auto(ORB_ID(control_state), &_ctrl_state_pub, &ctrl_state, &ctrl_inst, ORB_PRIO_HIGH);
  372.         }
  373.  
  374.         {
  375.             struct estimator_status_s est = {};
  376.  
  377.             est.timestamp = sensors.timestamp;
  378.             est.vibe[0] = _voter_accel.get_vibration_offset(est.timestamp, 0);
  379.             est.vibe[1] = _voter_accel.get_vibration_offset(est.timestamp, 1);
  380.             est.vibe[2] = _voter_accel.get_vibration_offset(est.timestamp, 2);
  381.  
  382.             /* the instance count is not used here */
  383.             int est_inst;
  384.             /* publish to control state topic */
  385.             orb_publish_auto(ORB_ID(estimator_status), &_est_state_pub, &est, &est_inst, ORB_PRIO_HIGH);
  386.         }
  387.     }
  388. }

 
 
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63
  • 64
  • 65
  • 66
  • 67
  • 68
  • 69
  • 70
  • 71
  • 72
  • 73
  • 74
  • 75
  • 76
  • 77
  • 78
  • 79
  • 80
  • 81
  • 82
  • 83
  • 84
  • 85
  • 86
  • 87
  • 88
  • 89
  • 90
  • 91
  • 92
  • 93
  • 94
  • 95
  • 96
  • 97
  • 98
  • 99
  • 100
  • 101
  • 102
  • 103
  • 104
  • 105
  • 106
  • 107
  • 108
  • 109
  • 110
  • 111
  • 112
  • 113
  • 114
  • 115
  • 116
  • 117
  • 118
  • 119
  • 120
  • 121
  • 122
  • 123
  • 124
  • 125
  • 126
  • 127
  • 128
  • 129
  • 130
  • 131
  • 132
  • 133
  • 134
  • 135
  • 136
  • 137
  • 138
  • 139
  • 140
  • 141
  • 142
  • 143
  • 144
  • 145
  • 146
  • 147
  • 148
  • 149
  • 150
  • 151
  • 152
  • 153
  • 154
  • 155
  • 156
  • 157
  • 158
  • 159
  • 160
  • 161
  • 162
  • 163
  • 164
  • 165
  • 166
  • 167
  • 168
  • 169
  • 170
  • 171
  • 172
  • 173
  • 174
  • 175
  • 176
  • 177
  • 178
  • 179
  • 180
  • 181
  • 182
  • 183
  • 184
  • 185
  • 186
  • 187
  • 188
  • 189
  • 190
  • 191
  • 192
  • 193
  • 194
  • 195
  • 196
  • 197
  • 198
  • 199
  • 200
  • 201
  • 202
  • 203
  • 204
  • 205
  • 206
  • 207
  • 208
  • 209
  • 210
  • 211
  • 212
  • 213
  • 214
  • 215
  • 216
  • 217
  • 218
  • 219
  • 220
  • 221
  • 222
  • 223
  • 224
  • 225
  • 226
  • 227
  • 228
  • 229
  • 230
  • 231
  • 232
  • 233
  • 234
  • 235
  • 236
  • 237
  • 238
  • 239
  • 240
  • 241
  • 242
  • 243
  • 244
  • 245
  • 246
  • 247
  • 248
  • 249
  • 250
  • 251
  • 252
  • 253
  • 254
  • 255
  • 256
  • 257
  • 258
  • 259
  • 260
  • 261
  • 262
  • 263
  • 264
  • 265
  • 266
  • 267
  • 268
  • 269
  • 270
  • 271
  • 272
  • 273
  • 274
  • 275
  • 276
  • 277
  • 278
  • 279
  • 280
  • 281
  • 282
  • 283
  • 284
  • 285
  • 286
  • 287
  • 288
  • 289
  • 290
  • 291
  • 292
  • 293
  • 294
  • 295
  • 296
  • 297
  • 298
  • 299
  • 300
  • 301
  • 302
  • 303
  • 304
  • 305
  • 306
  • 307
  • 308
  • 309
  • 310
  • 311
  • 312
  • 313
  • 314
  • 315
  • 316
  • 317
  • 318
  • 319
  • 320
  • 321
  • 322
  • 323
  • 324
  • 325
  • 326
  • 327
  • 328
  • 329
  • 330
  • 331
  • 332
  • 333
  • 334
  • 335
  • 336
  • 337
  • 338
  • 339
  • 340
  • 341
  • 342
  • 343
  • 344
  • 345
  • 346
  • 347
  • 348
  • 349
  • 350
  • 351
  • 352
  • 353
  • 354
  • 355
  • 356
  • 357
  • 358
  • 359
  • 360
  • 361
  • 362
  • 363
  • 364
  • 365
  • 366
  • 367
  • 368
  • 369
  • 370
  • 371
  • 372
  • 373
  • 374
  • 375
  • 376
  • 377
  • 378
  • 379
  • 380
  • 381
  • 382
  • 383
  • 384
  • 385
  • 386
  • 387
  • 388

 
 
 
 
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63
  • 64
  • 65
  • 66
  • 67
  • 68
  • 69
  • 70
  • 71
  • 72
  • 73
  • 74
  • 75
  • 76
  • 77
  • 78
  • 79
  • 80
  • 81
  • 82
  • 83
  • 84
  • 85
  • 86
  • 87
  • 88
  • 89
  • 90
  • 91
  • 92
  • 93
  • 94
  • 95
  • 96
  • 97
  • 98
  • 99
  • 100
  • 101
  • 102
  • 103
  • 104
  • 105
  • 106
  • 107
  • 108
  • 109
  • 110
  • 111
  • 112
  • 113
  • 114
  • 115
  • 116
  • 117
  • 118
  • 119
  • 120
  • 121
  • 122
  • 123
  • 124
  • 125
  • 126
  • 127
  • 128
  • 129
  • 130
  • 131
  • 132
  • 133
  • 134
  • 135
  • 136
  • 137
  • 138
  • 139
  • 140
  • 141
  • 142
  • 143
  • 144
  • 145
  • 146
  • 147
  • 148
  • 149
  • 150
  • 151
  • 152
  • 153
  • 154
  • 155
  • 156
  • 157
  • 158
  • 159
  • 160
  • 161
  • 162
  • 163
  • 164
  • 165
  • 166
  • 167
  • 168
  • 169
  • 170
  • 171
  • 172
  • 173
  • 174
  • 175
  • 176
  • 177
  • 178
  • 179
  • 180
  • 181
  • 182
  • 183
  • 184
  • 185
  • 186
  • 187
  • 188
  • 189
  • 190
  • 191
  • 192
  • 193
  • 194
  • 195
  • 196
  • 197
  • 198
  • 199
  • 200
  • 201
  • 202
  • 203
  • 204
  • 205
  • 206
  • 207
  • 208
  • 209
  • 210
  • 211
  • 212
  • 213
  • 214
  • 215
  • 216
  • 217
  • 218
  • 219
  • 220
  • 221
  • 222
  • 223
  • 224
  • 225
  • 226
  • 227
  • 228
  • 229
  • 230
  • 231
  • 232
  • 233
  • 234
  • 235
  • 236
  • 237
  • 238
  • 239
  • 240
  • 241
  • 242
  • 243
  • 244
  • 245
  • 246
  • 247
  • 248
  • 249
  • 250
  • 251
  • 252
  • 253
  • 254
  • 255
  • 256
  • 257
  • 258
  • 259
  • 260
  • 261
  • 262
  • 263
  • 264
  • 265
  • 266
  • 267
  • 268
  • 269
  • 270
  • 271
  • 272
  • 273
  • 274
  • 275
  • 276
  • 277
  • 278
  • 279
  • 280
  • 281
  • 282
  • 283
  • 284
  • 285
  • 286
  • 287
  • 288
  • 289
  • 290
  • 291
  • 292
  • 293
  • 294
  • 295
  • 296
  • 297
  • 298
  • 299
  • 300
  • 301
  • 302
  • 303
  • 304
  • 305
  • 306
  • 307
  • 308
  • 309
  • 310
  • 311
  • 312
  • 313
  • 314
  • 315
  • 316
  • 317
  • 318
  • 319
  • 320
  • 321
  • 322
  • 323
  • 324
  • 325
  • 326
  • 327
  • 328
  • 329
  • 330
  • 331
  • 332
  • 333
  • 334
  • 335
  • 336
  • 337
  • 338
  • 339
  • 340
  • 341
  • 342
  • 343
  • 344
  • 345
  • 346
  • 347
  • 348
  • 349
  • 350
  • 351
  • 352
  • 353
  • 354
  • 355
  • 356
  • 357
  • 358
  • 359
  • 360
  • 361
  • 362
  • 363
  • 364
  • 365
  • 366
  • 367
  • 368
  • 369
  • 370
  • 371
  • 372
  • 373
  • 374
  • 375
  • 376
  • 377
  • 378
  • 379
  • 380
  • 381
  • 382
  • 383
  • 384
  • 385
  • 386
  • 387
  • 388

update_parameters();

  1. l657~l686
  2. void AttitudeEstimatorQ::update_parameters(bool force)
  3. {
  4.     bool updated = force;
  5.  
  6.     if (!updated) {
  7.         orb_check(_params_sub, &updated);//查看参数是否更新;
  8.     }
  9.  
  10.     if (updated) {//获取新参数;
  11.         parameter_update_s param_update;
  12.         orb_copy(ORB_ID(parameter_update), _params_sub, &param_update);
  13.  
  14.         param_get(_params_handles.w_acc, &_w_accel);
  15.         param_get(_params_handles.w_mag, &_w_mag);
  16.         param_get(_params_handles.w_ext_hdg, &_w_ext_hdg);
  17.         param_get(_params_handles.w_gyro_bias, &_w_gyro_bias);
  18.         float mag_decl_deg = 0.0f;
  19.         param_get(_params_handles.mag_decl, &mag_decl_deg);
  20.         update_mag_declination(math::radians(mag_decl_deg));
  21.         int32_t mag_decl_auto_int;
  22.         param_get(_params_handles.mag_decl_auto, &mag_decl_auto_int);
  23.         _mag_decl_auto = mag_decl_auto_int != 0;//自动磁偏角校正;
  24.         int32_t acc_comp_int;
  25.         param_get(_params_handles.acc_comp, &acc_comp_int);
  26.         _acc_comp = acc_comp_int != 0;
  27.         param_get(_params_handles.bias_max, &_bias_max);//陀螺仪偏差上限;
  28.         param_get(_params_handles.vibe_thresh, &_vibration_warning_threshold);//振动警告阈值;
  29.         param_get(_params_handles.ext_hdg_mode, &_ext_hdg_mode);
  30.     }
  31. }

 
 
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31

 
 
 
 
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31

init();

  1. 为导航坐标系（NED）的 轴（D）在机体坐标系中的表示;

    导航系中，D正方向朝下;

  2. 为导航坐标系（NED）的 轴（N）在机体坐标系;

    i = (_mag - k * (_mag * k)); //施密特正交化;

    //因 向量 k 已归一化，故分母等于1;

  3. 为导航坐标系（NED）的 轴（E）在机体坐标系;

    j = k % i //叉乘求正交向量;

  4. 构造旋转矩阵

    R.set_row(0, i); R.set_row(1, j); R.set_row(2, k);

  5. 转换为四元数 ，根据设定校正磁偏，归一化;

  1. l688~l728
  2. bool AttitudeEstimatorQ::init()
  3. {
  4.     // Rotation matrix can be easily constructed from acceleration and mag field vectors
  5.     // 'k' is Earth Z axis (Down) unit vector in body frame
  6.     Vector<3> k = -_accel;
  7.     k.normalize();
  8.  
  9.     // 'i' is Earth X axis (North) unit vector in body frame, orthogonal with 'k'
  10.     Vector<3> i = (_mag - k * (_mag * k));
  11.     i.normalize();
  12.  
  13.     // 'j' is Earth Y axis (East) unit vector in body frame, orthogonal with 'k' and 'i'
  14.     Vector<3> j = k % i;
  15.  
  16.     // Fill rotation matrix
  17.     Matrix<3, 3> R;
  18.     R.set_row(0, i);
  19.     R.set_row(1, j);
  20.     R.set_row(2, k);
  21.  
  22.     // Convert to quaternion
  23.     _q.from_dcm(R);
  24.  
  25.     // Compensate for magnetic declination
  26.     Quaternion decl_rotation;
  27.     decl_rotation.from_yaw(_mag_decl);
  28.     _q = decl_rotation * _q;
  29.  
  30.     _q.normalize();
  31.  
  32.     if (PX4_ISFINITE(_q(0)) && PX4_ISFINITE(_q(1)) &&
  33.         PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)) &&
  34.         _q.length() > 0.95f && _q.length() < 1.05f) {
  35.         _inited = true;
  36.  
  37.     } else {
  38.         _inited = false;
  39.     }
  40.  
  41.     return _inited;
  42. }

 
 
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42

 
 

  
  
 
 

 
 
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42

update();

  1. init();//执行一次;

    由加速度计和磁力计初始化旋转矩阵和四元数;

  2. mag_earth = _q.conjugate(_mag);

    不使用外部航向，或外部航向异常时，采用磁力计校准;
    将磁力计读数从机体坐标系转换到导航坐标系;

  3. mag_err = _wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl);

    将磁力计的读数由向量换算到角度; 该角度减去磁偏，得到磁场偏差;
    _mag_decl 由GPS得到;
    **atan2f: 2 表示两个输入参数; 支持四象限内角度换算; 输出弧度值;
    **_wrap_pi: 将(0~2pi)的角度映射到(-pi~pi);

  4. corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mag_err)) * _w_mag;

    将磁场偏差转换到机体坐标系，并乘以其对应权重;

  5. _q.normalize();

    四元数归一化;
    这里的归一化用于校正update_mag_declination后的偏差。

  6. corr += (k % (_accel - _pos_acc).normalized()) * _w_accel;

    向量 k 是重力加速度向量(0,0,1)转换到机体坐标系;
    _accel 是加速度计的读数;
    _pos_acc 是由位置估计(GPS) 微分得到的加速度;
    _accel - _pos_acc 表示飞行器加速度向量减去其水平分量;
    k 与 (_accel - _pos_acc)叉乘，得到偏差;

  7. _gyro_bias += corr * (_w_gyro_bias * dt);

    得到陀螺仪修正值
    此处修正值为 mahony 滤波的 pi 控制器的 积分部分;
    因为 _gyro_bias 不归零，故不断累积;

  8. _rates = _gyro + _gyro_bias;

    _rates 表示角速度;

  9. corr += _rates;

    这里的 corr = (Ea+Em) + (Ea+Em)*dt + gyro;
    个人认为这里的 corr 才是更新后的角速度;

  10. _q += _q.derivative(corr) * dt;

    更新四元数，derivative为求导函数，使用一阶龙格库塔求导。 

  1. l730~l817
  2. bool AttitudeEstimatorQ::update(float dt)
  3. {
  4.     if (!_inited) {
  5.  
  6.         if (!_data_good) {
  7.             return false;
  8.         }
  9.  
  10.         return init();
  11.     }
  12.  
  13.     Quaternion q_last = _q;//保存上一状态，以便恢复;
  14.  
  15.     // Angular rate of correction
  16.     Vector<3> corr;//初始化元素为0;
  17.  
  18.     //_ext_hdg_good表示外部航向数据可用;
  19.     if (_ext_hdg_mode > 0 && _ext_hdg_good) {
  20.         if (_ext_hdg_mode == 1) {
  21.             // Vision heading correction
  22.             //视觉航向校正;
  23.             // Project heading to global frame and extract XY component
  24.             //将航向投影到导航坐标系，提取XY分量;
  25.             Vector<3> vision_hdg_earth = _q.conjugate(_vision_hdg);
  26.             float vision_hdg_err = _wrap_pi(atan2f(vision_hdg_earth(1), vision_hdg_earth(0)));
  27.             // Project correction to body frame
  28.             corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -vision_hdg_err)) * _w_ext_hdg;
  29.         }
  30.  
  31.         if (_ext_hdg_mode == 2) {
  32.             // Mocap heading correction
  33.             // Project heading to global frame and extract XY component
  34.             Vector<3> mocap_hdg_earth = _q.conjugate(_mocap_hdg);
  35.             float mocap_hdg_err = _wrap_pi(atan2f(mocap_hdg_earth(1), mocap_hdg_earth(0)));
  36.             // Project correction to body frame
  37.             corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mocap_hdg_err)) * _w_ext_hdg;
  38.         }
  39.     }
  40.  
  41.     if (_ext_hdg_mode == 0  || !_ext_hdg_good) {
  42.         // Magnetometer correction
  43.         // Project mag field vector to global frame and extract XY component
  44.         Vector<3> mag_earth = _q.conjugate(_mag);
  45.         float mag_err = _wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl);
  46.         // Project magnetometer correction to body frame
  47.         corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mag_err)) * _w_mag;
  48.     }
  49.  
  50.     _q.normalize();
  51.  
  52.     // Accelerometer correction
  53.     // Project 'k' unit vector of earth frame to body frame
  54.     // Vector<3> k = _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, 1.0f));
  55.     // Optimized version with dropped zeros
  56.     Vector<3> k(
  57.         2.0f * (_q(1) * _q(3) - _q(0) * _q(2)),
  58.         2.0f * (_q(2) * _q(3) + _q(0) * _q(1)),
  59.         (_q(0) * _q(0) - _q(1) * _q(1) - _q(2) * _q(2) + _q(3) * _q(3))
  60.     );
  61.  
  62.     corr += (k % (_accel - _pos_acc).normalized()) * _w_accel;
  63.  
  64.     // Gyro bias estimation
  65.     _gyro_bias += corr * (_w_gyro_bias * dt);
  66.  
  67.     for (int i = 0; i < 3; i++) {//陀螺仪最大偏差上限;
  68.         _gyro_bias(i) = math::constrain(_gyro_bias(i), -_bias_max, _bias_max);
  69.     }
  70.  
  71.     _rates = _gyro + _gyro_bias;
  72.  
  73.     // Feed forward gyro
  74.     corr += _rates;
  75.  
  76.     // Apply correction to state
  77.     _q += _q.derivative(corr) * dt;
  78.  
  79.     // Normalize quaternion
  80.     _q.normalize();
  81.     //判断四元数是否发散，若发散，则沿用之前的四元数;
  82.     if (!(PX4_ISFINITE(_q(0)) && PX4_ISFINITE(_q(1)) &&
  83.           PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)))) {
  84.         // Reset quaternion to last good state
  85.         _q = q_last;
  86.         _rates.zero();
  87.         _gyro_bias.zero();
  88.         return false;
  89.     }
  90.  
  91.     return true;
  92. }

 
 
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63
  • 64
  • 65
  • 66
  • 67
  • 68
  • 69
  • 70
  • 71
  • 72
  • 73
  • 74
  • 75
  • 76
  • 77
  • 78
  • 79
  • 80
  • 81
  • 82
  • 83
  • 84
  • 85
  • 86
  • 87
  • 88
  • 89
  • 90
  • 91
  • 92

 
 

  
  
 
 

 
 
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63
  • 64
  • 65
  • 66
  • 67
  • 68
  • 69
  • 70
  • 71
  • 72
  • 73
  • 74
  • 75
  • 76
  • 77
  • 78
  • 79
  • 80
  • 81
  • 82
  • 83
  • 84
  • 85
  • 86
  • 87
  • 88
  • 89
  • 90
  • 91
  • 92

update_mag_declination();

  1. l819~l832
  2. void AttitudeEstimatorQ::update_mag_declination(float new_declination)
  3. {
  4.     // Apply initial declination or trivial rotations without changing estimation
  5.     //忽略微小旋转;
  6.     if (!_inited || fabsf(new_declination - _mag_decl) < 0.0001f) {
  7.         _mag_decl = new_declination;
  8.  
  9.     } else {
  10.         // Immediately rotate current estimation to avoid gyro bias growth
  11.         //磁偏超过一定值后，校正姿态;
  12.         Quaternion decl_rotation;
  13.         decl_rotation.from_yaw(new_declination - _mag_decl);
  14.         //由磁偏角度转化为四元数;
  15.         _q = decl_rotation * _q;
  16.         //四元数相乘表示角度相加;
  17.         _mag_decl = new_declination;
  18.     }
  19. }

 
 
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19

 
 

  
  
 
 

 
 
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19

attitude_estimator_q_main();

  1. l834~l890
  2. int attitude_estimator_q_main(int argc, char *argv[])
  3. {//外部调用接口;
  4.     if (argc < 2) {
  5.         warnx("usage: attitude_estimator_q {start|stop|status}");
  6.         return 1;
  7.     }
  8.  
  9.     if (!strcmp(argv[1], "start")) {
  10.  
  11.         if (attitude_estimator_q::instance != nullptr) {
  12.             warnx("already running");
  13.             return 1;
  14.         }
  15.         //实例化，instance;
  16.         attitude_estimator_q::instance = new AttitudeEstimatorQ;
  17.  
  18.         if (attitude_estimator_q::instance == nullptr) {
  19.             warnx("alloc failed");
  20.             return 1;
  21.         }
  22.  
  23.         if (OK != attitude_estimator_q::instance->start()) {
  24.             delete attitude_estimator_q::instance;
  25.             attitude_estimator_q::instance = nullptr;
  26.             warnx("start failed");
  27.             return 1;
  28.         }
  29.  
  30.         return 0;
  31.     }
  32.  
  33.     if (!strcmp(argv[1], "stop")) {
  34.         if (attitude_estimator_q::instance == nullptr) {
  35.             warnx("not running");
  36.             return 1;
  37.         }
  38.         //删除实例化对象，指针置空;
  39.         delete attitude_estimator_q::instance;
  40.         attitude_estimator_q::instance = nullptr;
  41.         return 0;
  42.     }
  43.     //打印当前姿态信息;
  44.     if (!strcmp(argv[1], "status")) {
  45.         if (attitude_estimator_q::instance) {
  46.             attitude_estimator_q::instance->print();
  47.             warnx("running");
  48.             return 0;
  49.  
  50.         } else {
  51.             warnx("not running");
  52.             return 1;
  53.         }
  54.     }
  55.  
  56.     warnx("unrecognized command");
  57.     return 1;
  58. }

 
 
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58

 
 

  
  
 
 

 
 
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/黑客灵魂/article/detail/1014595
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号