Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_InertialNav: Enable flight using EKF2 

All EKF functions are accessed via the AHRS library enabling AHRS_EKF_TYPE to determine which EKF is being used by the control loops
This commit is contained in:
Paul Riseborough 2015-10-12 23:13:32 +11:00 committed by Andrew Tridgell
parent e65ae51564
commit 24c0309e85
1 changed files with 14 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp
View File

@ -16,17 +16,20 @@
*/
void AP_InertialNav_NavEKF::update(float dt)
{
_ahrs_ekf.get_NavEKF().getPosNED(_relpos_cm);
// get the position relative to the local earth frame origin
_ahrs_ekf.get_relative_position_NED(_relpos_cm);
_relpos_cm *= 100; // convert to cm
// get the absolute WGS-84 position
_haveabspos = _ahrs_ekf.get_position(_abspos);
_ahrs_ekf.get_NavEKF().getVelNED(_velocity_cm);
// get the velocity relative to the local earth frame
_ahrs_ekf.get_velocity_NED(_velocity_cm);
_velocity_cm *= 100; // convert to cm/s
// A derivative of the vertical position which is kinematically consistent with the vertical position is required by some control loops.
// Get a derivative of the vertical position which is kinematically consistent with the vertical position is required by some control loops.
// This is different to the vertical velocity from the EKF which is not always consistent with the verical position due to the various errors that are being corrected for.
_ahrs_ekf.get_NavEKF().getPosDownDerivative(_pos_z_rate);
_ahrs_ekf.get_vert_pos_rate(_pos_z_rate);
_pos_z_rate *= 100; // convert to cm/s
// InertialNav is NEU
@ -40,9 +43,9 @@ void AP_InertialNav_NavEKF::update(float dt)
*/
nav_filter_status AP_InertialNav_NavEKF::get_filter_status() const
{
nav_filter_status ret;
_ahrs_ekf.get_NavEKF().getFilterStatus(ret);
return ret;
nav_filter_status status;
_ahrs_ekf.get_filter_status(status);
return status;
}
/**
@ -51,10 +54,7 @@ nav_filter_status AP_InertialNav_NavEKF::get_filter_status() const
struct Location AP_InertialNav_NavEKF::get_origin() const
{
struct Location ret;
if (!_ahrs_ekf.get_NavEKF().getOriginLLH(ret)) {
// initialise location to all zeros if origin not yet set
memset(&ret, 0, sizeof(ret));
}
ret = _ahrs_ekf.get_origin();
return ret;
}
@ -74,7 +74,7 @@ const Vector3f &AP_InertialNav_NavEKF::get_position(void) const
*/
bool AP_InertialNav_NavEKF::get_location(struct Location &loc) const
{
return _ahrs_ekf.get_NavEKF().getLLH(loc);
return _ahrs_ekf.get_location(loc);
}
/**
@ -142,7 +142,7 @@ float AP_InertialNav_NavEKF::get_altitude() const
bool AP_InertialNav_NavEKF::get_hagl(float &height) const
{
// true when estimate is valid
bool valid = _ahrs_ekf.get_NavEKF().getHAGL(height);
bool valid = _ahrs_ekf.get_hagl(height);
// convert height from m to cm
height *= 100.0f;
return valid;
@ -157,7 +157,7 @@ bool AP_InertialNav_NavEKF::get_hagl(float &height) const
bool AP_InertialNav_NavEKF::get_hgt_ctrl_limit(float& limit) const
{
// true when estimate is valid
if (_ahrs_ekf.get_NavEKF().getHeightControlLimit(limit)) {
if (_ahrs_ekf.get_hgt_ctrl_limit(limit)) {
// convert height from m to cm
limit *= 100.0f;
return true;