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

AP_InertialNav: fixed EKF version for new AHRS API

This commit is contained in:
Andrew Tridgell 2014-02-09 13:57:08 +11:00
parent a7a44a9a5c
commit a88212eb7a
1 changed files with 6 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp
View File

@ -27,9 +27,13 @@ void AP_InertialNav_NavEKF::init()
void AP_InertialNav_NavEKF::update(float dt)
{
AP_InertialNav::update(dt);
_relpos_cm = _ahrs.get_relative_position_NED() * 100;
_ahrs.get_relative_position_NED(_relpos_cm);
_relpos_cm *= 100; // convert to cm
_haveabspos = _ahrs.get_position(_abspos);
_velocity_cm = _ahrs.get_velocity_NED() * 100;
_ahrs.get_velocity_NED(_velocity_cm);
_velocity_cm *= 100; // convert to cm/s
// InertialNav is NEU
_relpos_cm.z = - _relpos_cm.z;