Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 08:13: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) void AP_InertialNav_NavEKF::update(float dt)
{ {
AP_InertialNav::update(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 _relpos_cm *= 100; // convert to cm
_haveabspos = _ahrs.get_position(_abspos); _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: public:
// Constructor // 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), AP_InertialNav(ahrs, baro, gps_glitch, baro_glitch),
_haveabspos(false) _haveabspos(false),
_ahrs_ekf(ahrs)
{ {
} }
@ -113,6 +114,7 @@ private:
Vector3f _velocity_cm; // NEU Vector3f _velocity_cm; // NEU
struct Location _abspos; struct Location _abspos;
bool _haveabspos; bool _haveabspos;
AP_AHRS_NavEKF &_ahrs_ekf;
}; };
#endif // __AP_INERTIALNAV_NAVEKF_H__ #endif // __AP_INERTIALNAV_NAVEKF_H__