ArduPilot开源飞控之AP_InertialNav

1. 源由

AP_InternationalNav类是NavEKF的封装，提供关于导航相关的信息：

1. 坐标位置
2. 相对位置
3. 运动速度
4. 导航状态

2. 调用关系

状态更新函数调用嵌套关系。

FAST_TASK(read_inertia)
 └──> Copter::read_inertia
     └──> AP_InertialNav::update

3. 重要例程

3.1 read_inertia

1. current_loc.lat
2. current_loc.lng
3. current_loc.alt

Copter::read_inertia
 │
 │  // inertial altitude estimates. Use barometer climb rate during high vibrations
 ├──> inertial_nav.update(vibration_check.high_vibes);
 │
 │  // pull position from ahrs
 ├──> Location loc;
 ├──> ahrs.get_location(loc);
 ├──> current_loc.lat = loc.lat;
 ├──> current_loc.lng = loc.lng;
 │
 │  // exit immediately if we do not have an altitude estimate
 ├──> 
 │   └──> return;
 │
 │  // current_loc.alt is alt-above-home, converted from inertial nav's alt-above-ekf-origin
 ├──> const int32_t alt_above_origin_cm = inertial_nav.get_position_z_up_cm();
 ├──> current_loc.set_alt_cm(alt_above_origin_cm, Location::AltFrame::ABOVE_ORIGIN);
 └──> 
     │  // if home has not been set yet we treat alt-above-origin as alt-above-home
     └──> current_loc.set_alt_cm(alt_above_origin_cm, Location::AltFrame::ABOVE_HOME);

3.2 update

1. _relpos_cm
2. _velocity_cm

AP_InertialNav::update
 │
 │  // get the NE position relative to the local earth frame origin
 ├──> Vector2f posNE;
 ├──> <_ahrs_ekf.get_relative_position_NE_origin(posNE)>
 │   ├──> _relpos_cm.x = posNE.x * 100; // convert from m to cm
 │   └──> _relpos_cm.y = posNE.y * 100; // convert from m to cm
 │
 │  // get the D position relative to the local earth frame origin
 ├──> float posD;
 ├──> <_ahrs_ekf.get_relative_position_D_origin(posD)>
 │   └──> _relpos_cm.z = - posD * 100; // convert from m in NED to cm in NEU
 │
 │  // get the velocity relative to the local earth frame
 ├──> Vector3f velNED;
 └──> <_ahrs_ekf.get_velocity_NED(velNED)>
     │  // during high vibration events use vertical position change
     ├──> 
     │   ├──> float rate_z;
     │   └──> <_ahrs_ekf.get_vert_pos_rate_D(rate_z)>
     │       └──> velNED.z = rate_z;
     ├──> _velocity_cm = velNED * 100; // convert to cm/s
     └──> _velocity_cm.z = -_velocity_cm.z; // convert from NED to NEU

4. 封装接口

4.1 get_filter_status

/**
 * get_filter_status : returns filter status as a series of flags
 */
nav_filter_status AP_InertialNav::get_filter_status() const
{
    nav_filter_status status;
    _ahrs_ekf.get_filter_status(status);
    return status;
}

union nav_filter_status {
    struct {
        bool attitude           : 1; // 0 - true if attitude estimate is valid
        bool horiz_vel          : 1; // 1 - true if horizontal velocity estimate is valid
        bool vert_vel           : 1; // 2 - true if the vertical velocity estimate is valid
        bool horiz_pos_rel      : 1; // 3 - true if the relative horizontal position estimate is valid
        bool horiz_pos_abs      : 1; // 4 - true if the absolute horizontal position estimate is valid
        bool vert_pos           : 1; // 5 - true if the vertical position estimate is valid
        bool terrain_alt        : 1; // 6 - true if the terrain height estimate is valid
        bool const_pos_mode     : 1; // 7 - true if we are in const position mode
        bool pred_horiz_pos_rel : 1; // 8 - true if filter expects it can produce a good relative horizontal position estimate - used before takeoff
        bool pred_horiz_pos_abs : 1; // 9 - true if filter expects it can produce a good absolute horizontal position estimate - used before takeoff
        bool takeoff_detected   : 1; // 10 - true if optical flow takeoff has been detected
        bool takeoff            : 1; // 11 - true if filter is compensating for baro errors during takeoff
        bool touchdown          : 1; // 12 - true if filter is compensating for baro errors during touchdown
        bool using_gps          : 1; // 13 - true if we are using GPS position
        bool gps_glitching      : 1; // 14 - true if GPS glitching is affecting navigation accuracy
        bool gps_quality_good   : 1; // 15 - true if we can use GPS for navigation
        bool initalized         : 1; // 16 - true if the EKF has ever been healthy
        bool rejecting_airspeed : 1; // 17 - true if we are rejecting airspeed data
        bool dead_reckoning     : 1; // 18 - true if we are dead reckoning (e.g. no position or velocity source)
    } flags;
    uint32_t value;
};

4.2 get_position_neu_cm

/**
 * get_position_neu_cm - returns the current position relative to the EKF origin in cm.
 *
 * @return
 */
const Vector3f &AP_InertialNav::get_position_neu_cm(void) const 
{
    return _relpos_cm;
}

4.3 get_position_xy_cm

/**
 * get_position_xy_cm - returns the current x-y position relative to the EKF origin in cm.
 *
 * @return
 */
const Vector2f &AP_InertialNav::get_position_xy_cm() const
{
    return _relpos_cm.xy();
}

4.4 get_position_z_up_cm

/**
 * get_position_z_up_cm - returns the current z position relative to the EKF origin, frame z-axis up, in cm.
 * @return
 */
float AP_InertialNav::get_position_z_up_cm() const
{
    return _relpos_cm.z;
}

4.5 get_velocity_neu_cms

/**
 * get_velocity_neu_cms - returns the current velocity in cm/s
 *
 * @return velocity vector:
 *      		.x : latitude  velocity in cm/s
 * 				.y : longitude velocity in cm/s
 * 				.z : vertical  velocity in cm/s
 */
const Vector3f &AP_InertialNav::get_velocity_neu_cms() const
{
    return _velocity_cm;
}

4.6 get_velocity_xy_cms

/**
 * get_velocity_xy_cms - returns the current x-y velocity relative to the EKF origin in cm.
 *
 * @return
 */
const Vector2f &AP_InertialNav::get_velocity_xy_cms() const
{
    return _velocity_cm.xy();
}

4.7 get_speed_xy_cms

/**
 * get_speed_xy_cms - returns the current horizontal speed in cm/s
 *
 * @returns the current horizontal speed in cm/s
 */
float AP_InertialNav::get_speed_xy_cms() const
{
    return _velocity_cm.xy().length();
}

4.8 get_velocity_z_up_cms

/**
 * get_velocity_z_up_cms - returns the current z-axis velocity, frame z-axis up, in cm/s
 *
 * @return z-axis velocity, frame z-axis up, in cm/s
 */
float AP_InertialNav::get_velocity_z_up_cms() const
{
    return _velocity_cm.z;
}

5. 参考资料

【1】ArduPilot开源飞控系统之简单介绍
【2】ArduPilot之开源代码Task介绍
【3】ArduPilot飞控启动&运行过程简介
【4】ArduPilot之开源代码Library&Sketches设计
【5】ArduPilot之开源代码Sensor Drivers设计