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├──> <!inertial_nav.get_filter_status().flags.vert_pos>│   └──> 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);└──> <!ahrs.home_is_set() || !current_loc.change_alt_frame(Location::AltFrame::ABOVE_HOME)>│  // 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├──> <high_vibes>│   ├──> 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 validbool horiz_vel          : 1; // 1 - true if horizontal velocity estimate is validbool vert_vel           : 1; // 2 - true if the vertical velocity estimate is validbool horiz_pos_rel      : 1; // 3 - true if the relative horizontal position estimate is validbool horiz_pos_abs      : 1; // 4 - true if the absolute horizontal position estimate is validbool vert_pos           : 1; // 5 - true if the vertical position estimate is validbool terrain_alt        : 1; // 6 - true if the terrain height estimate is validbool const_pos_mode     : 1; // 7 - true if we are in const position modebool pred_horiz_pos_rel : 1; // 8 - true if filter expects it can produce a good relative horizontal position estimate - used before takeoffbool pred_horiz_pos_abs : 1; // 9 - true if filter expects it can produce a good absolute horizontal position estimate - used before takeoffbool takeoff_detected   : 1; // 10 - true if optical flow takeoff has been detectedbool takeoff            : 1; // 11 - true if filter is compensating for baro errors during takeoffbool touchdown          : 1; // 12 - true if filter is compensating for baro errors during touchdownbool using_gps          : 1; // 13 - true if we are using GPS positionbool gps_glitching      : 1; // 14 - true if GPS glitching is affecting navigation accuracybool gps_quality_good   : 1; // 15 - true if we can use GPS for navigationbool initalized         : 1; // 16 - true if the EKF has ever been healthybool rejecting_airspeed : 1; // 17 - true if we are rejecting airspeed databool 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设计