Ardupilot源码分析（五）惯性导航(Inertial Nav)与姿态航向 ...

baichuanlv

上一节我们简单分析了惯性传感器(AP_InertialSensor ins)的驱动程序，主要就其初始化和前后端数据采集与处理进行了说明。惯性传感器输出的原始数据为之后的惯性导航INertial Nav和姿态航向参考系统AHRS提供输入。
惯性导航

    Inertial Nav功能比较简单，只是对AHRS的输出做一个转换，AHRS作为Inertial Nav的一个参数在其构造函数中传入。Inertial Nav原型如下。
/*A wrapper around the AP_InertialNav class which uses the NavEKF
  filter if available, and falls back to the AP_InertialNav filter
  when EKF is not available*/
#pragma once

#include <AP_NavEKF/AP_Nav_Common.h> // definitions shared by inertial and ekf nav filters

class AP_InertialNav_NavEKF : public AP_InertialNav
{
public:
    // Constructor
AP_InertialNav_NavEKF(AP_AHRS_NavEKF &ahrs) ://传入AHRS
AP_InertialNav(),
_ahrs_ekf(ahrs)
{}
//从ahrs更新当前相对位置和速度
void update() override;
nav_filter_status get_filter_status() const override;
    /**
     * get_position - returns the current position relative to the home location in cm.
     * the home location was set with AP_InertialNav::set_home_position(int32_t, int32_t)
     */
const Vector3f& get_position() const override;
    /**
     * get_velocity - 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& get_velocity() const override;
    /**
     * get_speed_xy - returns the current horizontal speed in cm/s
     *
     * @returns the current horizontal speed in cm/s
     */
float get_speed_xy() const override;
    /**
     * get_altitude - get latest altitude estimate in cm
     * @return
     */
float get_altitude() const override;
    /**
     * get_velocity_z - returns the current climbrate.
     *
     * @see get_velocity().z
     *
     * @return climbrate in cm/s
     */
float get_velocity_z() const override;

private:
Vector3f _relpos_cm;   // NEU
Vector3f _velocity_cm; // NEU
AP_AHRS_NavEKF &_ahrs_ekf;
};
姿态航向参考系统