ardupiolt AP_AHRS库类的分析（一）AP_AHRS

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
#pragma once

/*
 *  AHRS (Attitude Heading Reference System) interface for ArduPilot
 *
 */

#include <AP_Math/AP_Math.h>
#include <inttypes.h>
#include <AP_Compass/AP_Compass.h>
#include <AP_Airspeed/AP_Airspeed.h>
#include <AP_Beacon/AP_Beacon.h>
#include <AP_GPS/AP_GPS.h>
#include <AP_InertialSensor/AP_InertialSensor.h>
#include <AP_Baro/AP_Baro.h>
#include <AP_Param/AP_Param.h>
#include <AP_Common/Semaphore.h>

class OpticalFlow;
#define AP_AHRS_TRIM_LIMIT 10.0f        // 最大的纵倾角（trim angle）以度为单位
#define AP_AHRS_RP_P_MIN   0.05f        //  AHRS_RP_P 参数的最小值
#define AP_AHRS_YAW_P_MIN  0.05f        //  AHRS_YAW_P 参数的最小值

enum AHRS_VehicleClass : uint8_t {     //飞行器种类的选择
    AHRS_VEHICLE_UNKNOWN,
    AHRS_VEHICLE_GROUND,
    AHRS_VEHICLE_COPTER,
    AHRS_VEHICLE_FIXED_WING,
    AHRS_VEHICLE_SUBMARINE,
};


// forward declare view class          //先前声明 view 类
class AP_AHRS_View;

class AP_AHRS
{
public:
    friend class AP_AHRS_View;         //友元 AP_AHRS_View 类
    
    // Constructor                    //构造函数
    AP_AHRS() :
        _vehicle_class(AHRS_VEHICLE_UNKNOWN),
        _cos_roll(1.0f),
        _cos_pitch(1.0f),
        _cos_yaw(1.0f)
    {
        _singleton = this;

        // load default values from var_info table    从var_info表里加载默认值
        AP_Param::setup_object_defaults(this, var_info);

        // base the ki values by the sensors maximum drift
        // rate.   根据传感器的最大漂移率确定Ki值
        _gyro_drift_limit = AP::ins().get_gyro_drift_rate();

        // enable centrifugal correction by default    默认使用离心校正
        _flags.correct_centrifugal = true;

        _last_trim = _trim.get();
        _rotation_autopilot_body_to_vehicle_body.from_euler(_last_trim.x, _last_trim.y, 0.0f);
        _rotation_vehicle_body_to_autopilot_body = _rotation_autopilot_body_to_vehicle_body.transposed();
    }

    // empty virtual destructor    析构函数
    virtual ~AP_AHRS() {}

    // get singleton instance        获取单例实例
    static AP_AHRS *get_singleton() {
        return _singleton;
    }

    // init sets up INS board orientation        初始化设置INS板得方向
    virtual void init() {
        set_orientation();
    };

    // Accessors                            访问函数
    void set_fly_forward(bool b) {
        _flags.fly_forward = b;
    }

    bool get_fly_forward(void) const {
        return _flags.fly_forward;
    }

    /*
      set the "likely flying" flag. This is not guaranteed to be
      accurate, but is the vehicle codes best guess as to the whether
      the vehicle is currently flying
      设置“可能飞行”标志。 这不能保证准确无误，但这是猜测飞行器当前是否在飞行，
      代码最佳的猜测
     */
    void set_likely_flying(bool b) {
        if (b && !_flags.likely_flying) {
            _last_flying_ms = AP_HAL::millis();
        }
        _flags.likely_flying = b;
    }

    /*
      get the likely flying status. Returns true if the vehicle code
      thinks we are flying at the moment. Not guaranteed to be
      accurate
     得到可能飞行状态位，如果代码认为飞行器此刻在飞行，则返回真，不能确保一定是正确的。
     */
    bool get_likely_flying(void) const {
        return _flags.likely_flying;
    }

    /*
      return time in milliseconds since likely_flying was set
      true. Returns zero if likely_flying is currently false
      返回时间为毫秒如果可能飞行被设置为正，如果可能飞行被设置为假，则返回为0
       
    */
    uint32_t get_time_flying_ms(void) const {
        if (!_flags.likely_flying) {
            return 0;
        }
        return AP_HAL::millis() - _last_flying_ms;
    }
    
    AHRS_VehicleClass get_vehicle_class(void) const {
        return _vehicle_class;
    }

    void set_vehicle_class(AHRS_VehicleClass vclass) {
        _vehicle_class = vclass;
    }

    void set_wind_estimation(bool b) {
        _flags.wind_estimation = b;
    }

    void set_compass(Compass *compass) {
        _compass = compass;
        set_orientation();
    }

    const Compass* get_compass() const {
        return _compass;
    }

    void set_optflow(const OpticalFlow *optflow) {
        _optflow = optflow;
    }

    const OpticalFlow* get_optflow() const {
        return _optflow;
    }

    // allow for runtime change of orientation
    // this makes initial config easier
    允许在运行时更改方向，使初始配置更加容易
    void set_orientation();

    void set_airspeed(AP_Airspeed *airspeed) {
        _airspeed = airspeed;
    }

    void set_beacon(AP_Beacon *beacon) {
        _beacon = beacon;
    }

    const AP_Airspeed *get_airspeed(void) const {
        return _airspeed;
    }

    const AP_Beacon *get_beacon(void) const {
        return _beacon;
    }

    // get the index of the current primary accelerometer sensor
    获取当前主加速度传感器的索引
    virtual uint8_t get_primary_accel_index(void) const {
        return AP::ins().get_primary_accel();
    }

    // get the index of the current primary gyro sensor
    获取当前主陀螺仪传感器的索引
    virtual uint8_t get_primary_gyro_index(void) const {
        return AP::ins().get_primary_gyro();
    }
    
    // accelerometer values in the earth frame in m/s/s
    在大地坐标系下加速度计值的单位为 m/s/s
    virtual const Vector3f &get_accel_ef(uint8_t i) const {
        return _accel_ef[i];
    }
    virtual con