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

AP_IntertialNav: ensure we work on valid updated data

This commit is contained in:
Tom Pittenger 2016-03-01 10:05:59 -08:00 committed by Randy Mackay
parent 38b3625ed8
commit c0a6a94936
1 changed files with 12 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp
View File

@ -17,25 +17,26 @@
void AP_InertialNav_NavEKF::update(float dt)
{
// get the position relative to the local earth frame origin
_ahrs_ekf.get_relative_position_NED(_relpos_cm);
_relpos_cm *= 100; // convert to cm
if (_ahrs_ekf.get_relative_position_NED(_relpos_cm)) {
_relpos_cm *= 100; // convert to cm
_relpos_cm.z = - _relpos_cm.z; // InertialNav is NEU
}
// get the absolute WGS-84 position
_haveabspos = _ahrs_ekf.get_position(_abspos);
// get the velocity relative to the local earth frame
_ahrs_ekf.get_velocity_NED(_velocity_cm);
_velocity_cm *= 100; // convert to cm/s
if (_ahrs_ekf.get_velocity_NED(_velocity_cm)) {
_velocity_cm *= 100; // convert to cm/s
_velocity_cm.z = -_velocity_cm.z; // InertialNav is NEU
}
// 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_vert_pos_rate(_pos_z_rate);
_pos_z_rate *= 100; // convert to cm/s
// InertialNav is NEU
_relpos_cm.z = - _relpos_cm.z;
_velocity_cm.z = -_velocity_cm.z;
_pos_z_rate = - _pos_z_rate;
if (_ahrs_ekf.get_vert_pos_rate(_pos_z_rate)) {
_pos_z_rate *= 100; // convert to cm/s
_pos_z_rate = - _pos_z_rate; // InertialNav is NEU
}
}
/**