Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 15:53:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialNav: fixed use of _ahrs.get_relative_position_NED() with EKF disabled

Browse Source 
this prevents a floating point error caused by using an uninitialised
vector3 when switching between DCM and EKF control in AP_InertialNav

Pair-Programmed-With: Randy Mackay <rmackay9@yahoo.com>
This commit is contained in:
Andrew Tridgell 2014-10-09 18:30:20 +11:00
parent 39fadad7d6
commit 7b02d326f6
2 changed files with 5 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp
View File

@ -25,7 +25,7 @@ void AP_InertialNav_NavEKF::init()
void AP_InertialNav_NavEKF::update(float dt)
{
AP_InertialNav::update(dt);
_ahrs.get_relative_position_NED(_relpos_cm);
_ahrs_ekf.get_NavEKF().getPosNED(_relpos_cm);
_relpos_cm *= 100; // convert to cm
_haveabspos = _ahrs.get_position(_abspos);

6
libraries/AP_InertialNav/AP_InertialNav_NavEKF.h
View File

@ -14,9 +14,10 @@ class AP_InertialNav_NavEKF : public AP_InertialNav
{
public:
// Constructor
AP_InertialNav_NavEKF(AP_AHRS &ahrs, AP_Baro &baro, GPS_Glitch& gps_glitch, Baro_Glitch& baro_glitch) :
AP_InertialNav_NavEKF(AP_AHRS_NavEKF &ahrs, AP_Baro &baro, GPS_Glitch& gps_glitch, Baro_Glitch& baro_glitch) :
AP_InertialNav(ahrs, baro, gps_glitch, baro_glitch),
_haveabspos(false)
_haveabspos(false),
_ahrs_ekf(ahrs)
{
}
@ -113,6 +114,7 @@ private:
Vector3f _velocity_cm; // NEU
struct Location _abspos;
bool _haveabspos;
AP_AHRS_NavEKF &_ahrs_ekf;
};
#endif // __AP_INERTIALNAV_NAVEKF_H__