AP_InternationalNav类是NavEKF的封装,提供关于导航相关的信息:
状态更新函数调用嵌套关系。
FAST_TASK(read_inertia)
└──> Copter::read_inertia
└──> AP_InertialNav::update
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);
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
/**
* 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;
};
/**
* 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;
}
/**
* 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();
}
/**
* 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;
}
/**
* 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;
}
/**
* 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();
}
/**
* 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();
}
/**
* 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;
}
【1】ArduPilot开源飞控系统之简单介绍
【2】ArduPilot之开源代码Task介绍
【3】ArduPilot飞控启动&运行过程简介
【4】ArduPilot之开源代码Library&Sketches设计
【5】ArduPilot之开源代码Sensor Drivers设计