From 47f541e1431c3f07d6efda89dbc6cbc159ed5edc Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 4 Jan 2014 11:16:07 +1100 Subject: [PATCH] AP_InertialNav: implement InertialNav in terms of AHRS when available --- libraries/AP_InertialNav/AP_InertialNav.h | 37 +++++++------- .../AP_InertialNav/AP_InertialNav_NavEKF.cpp | 51 +++++++++++++++++++ .../AP_InertialNav/AP_InertialNav_NavEKF.h | 13 ++++- 3 files changed, 82 insertions(+), 19 deletions(-) diff --git a/libraries/AP_InertialNav/AP_InertialNav.h b/libraries/AP_InertialNav/AP_InertialNav.h index ed92510fb8..41a19ebb65 100644 --- a/libraries/AP_InertialNav/AP_InertialNav.h +++ b/libraries/AP_InertialNav/AP_InertialNav.h @@ -34,7 +34,7 @@ class AP_InertialNav public: // Constructor - AP_InertialNav( const AP_AHRS &ahrs, AP_Baro &baro, GPS*& gps, GPS_Glitch& gps_glitch ) : + AP_InertialNav(AP_AHRS &ahrs, AP_Baro &baro, GPS*& gps, GPS_Glitch& gps_glitch ) : _ahrs(ahrs), _baro(baro), _gps(gps), @@ -52,7 +52,7 @@ public: /** * initializes the object. */ - void init(); + virtual void init(); /** * update - updates velocity and position estimates using latest info from accelerometers @@ -60,7 +60,7 @@ public: * * @param dt : time since last update in seconds */ - void update(float dt); + virtual void update(float dt); // // XY Axis specific methods @@ -80,7 +80,7 @@ public: * position_ok - true if inertial based altitude and position can be trusted * @return */ - bool position_ok() const; + virtual bool position_ok() const; /** * check_gps - checks if new gps readings have arrived and calls correct_with_gps to @@ -103,33 +103,33 @@ public: * * @return */ - const Vector3f& get_position() const { return _position; } + virtual const Vector3f& get_position() const { return _position; } /** * get_latitude - returns the latitude of the current position estimation in 100 nano degrees (i.e. degree value multiplied by 10,000,000) * @return */ - int32_t get_latitude() const; + virtual int32_t get_latitude() const; /** * get_longitude - returns the longitude of the current position estimation in 100 nano degrees (i.e. degree value multiplied by 10,000,000) * @return */ - int32_t get_longitude() const; + virtual int32_t get_longitude() const; /** * get_latitude_diff - returns the current latitude difference from the home location. * * @return difference in 100 nano degrees (i.e. degree value multiplied by 10,000,000) */ - float get_latitude_diff() const; + virtual float get_latitude_diff() const; /** * get_longitude_diff - returns the current longitude difference from the home location. * * @return difference in 100 nano degrees (i.e. degree value multiplied by 10,000,000) */ - float get_longitude_diff() const; + virtual float get_longitude_diff() const; /** * get_velocity - returns the current velocity in cm/s @@ -139,14 +139,14 @@ public: * .y : longitude velocity in cm/s * .z : vertical velocity in cm/s */ - const Vector3f& get_velocity() const { return _velocity; } + virtual const Vector3f& get_velocity() const { return _velocity; } /** * get_velocity_xy - returns the current horizontal velocity in cm/s * * @returns the current horizontal velocity in cm/s */ - float get_velocity_xy(); + virtual float get_velocity_xy(); /** * set_velocity_xy - overwrites the current horizontal velocity in cm/s @@ -179,7 +179,7 @@ public: * altitude_ok - returns true if inertial based altitude and position can be trusted * @return */ - bool altitude_ok() const { return true; } + virtual bool altitude_ok() const { return true; } /** * check_baro - checks if new baro readings have arrived and calls correct_with_baro to @@ -198,10 +198,11 @@ public: void correct_with_baro(float baro_alt, float dt); /** - * get_altitude - get latest altitude estimate in cm + * get_altitude - get latest altitude estimate in cm above the + * reference position * @return */ - float get_altitude() const { return _position.z; } + virtual float get_altitude() const { return _position.z; } /** * set_altitude - overwrites the current altitude value. @@ -215,9 +216,9 @@ public: * * @see get_velocity().z * - * @return climbrate in cm/s + * @return climbrate in cm/s (positive up) */ - float get_velocity_z() const { return _velocity.z; } + virtual float get_velocity_z() const { return _velocity.z; } /** * set_velocity_z - overwrites the current climbrate. @@ -229,7 +230,7 @@ public: /** * error_count - returns number of missed updates from GPS */ - uint8_t error_count() const { return _error_count; } + virtual uint8_t error_count() const { return _error_count; } /** * ignore_next_error - the next error (if it occurs immediately) will not be added to the error count @@ -270,7 +271,7 @@ protected: uint8_t ignore_error : 3; // the number of iterations for which we should ignore errors } _flags; - const AP_AHRS &_ahrs; // reference to ahrs object + AP_AHRS &_ahrs; // reference to ahrs object AP_Baro &_baro; // reference to barometer GPS*& _gps; // pointer to gps diff --git a/libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp b/libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp index b95753214f..ffd12a303b 100644 --- a/libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp +++ b/libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp @@ -1,7 +1,10 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include #include "AP_InertialNav.h" +extern const AP_HAL::HAL& hal; + #if AP_AHRS_NAVEKF_AVAILABLE /* @@ -18,12 +21,30 @@ void AP_InertialNav_NavEKF::init() AP_InertialNav::init(); } +/** + update internal state +*/ +void AP_InertialNav_NavEKF::update(float dt) +{ + AP_InertialNav::update(dt); + _relpos_cm = _ahrs.get_relative_position_NED() * 100; + _haveabspos = _ahrs.get_position(_abspos); + _velocity_cm = _ahrs.get_velocity_NED() * 100; + + // InertialNav is NEU + _relpos_cm.z = - _relpos_cm.z; + _velocity_cm.z = -_velocity_cm.z; +} + /** * position_ok - true if inertial based altitude and position can be trusted * @return */ bool AP_InertialNav_NavEKF::position_ok() const { + if (_ahrs.have_inertial_nav() && _haveabspos) { + return true; + } return AP_InertialNav::position_ok(); } @@ -36,6 +57,9 @@ bool AP_InertialNav_NavEKF::position_ok() const */ const Vector3f &AP_InertialNav_NavEKF::get_position(void) const { + if (_ahrs.have_inertial_nav()) { + return _relpos_cm; + } return AP_InertialNav::get_position(); } @@ -44,6 +68,9 @@ const Vector3f &AP_InertialNav_NavEKF::get_position(void) const */ int32_t AP_InertialNav_NavEKF::get_latitude() const { + if (_ahrs.have_inertial_nav() && _haveabspos) { + return _abspos.lat; + } return AP_InertialNav::get_latitude(); } @@ -53,6 +80,9 @@ int32_t AP_InertialNav_NavEKF::get_latitude() const */ int32_t AP_InertialNav_NavEKF::get_longitude() const { + if (_ahrs.have_inertial_nav() && _haveabspos) { + return _abspos.lng; + } return AP_InertialNav::get_longitude(); } @@ -63,6 +93,9 @@ int32_t AP_InertialNav_NavEKF::get_longitude() const */ float AP_InertialNav_NavEKF::get_latitude_diff() const { + if (_ahrs.have_inertial_nav()) { + return _relpos_cm.x / LATLON_TO_CM; + } return AP_InertialNav::get_latitude_diff(); } @@ -73,6 +106,9 @@ float AP_InertialNav_NavEKF::get_latitude_diff() const */ float AP_InertialNav_NavEKF::get_longitude_diff() const { + if (_ahrs.have_inertial_nav()) { + return _relpos_cm.y / _lon_to_cm_scaling; + } return AP_InertialNav::get_longitude_diff(); } @@ -86,6 +122,9 @@ float AP_InertialNav_NavEKF::get_longitude_diff() const */ const Vector3f &AP_InertialNav_NavEKF::get_velocity() const { + if (_ahrs.have_inertial_nav()) { + return _velocity_cm; + } return AP_InertialNav::get_velocity(); } @@ -96,6 +135,9 @@ const Vector3f &AP_InertialNav_NavEKF::get_velocity() const */ float AP_InertialNav_NavEKF::get_velocity_xy() { + if (_ahrs.have_inertial_nav()) { + return pythagorous2(_velocity_cm.x, _velocity_cm.y); + } return AP_InertialNav::get_velocity_xy(); } @@ -105,6 +147,9 @@ float AP_InertialNav_NavEKF::get_velocity_xy() */ bool AP_InertialNav_NavEKF::altitude_ok() const { + if (_ahrs.have_inertial_nav() && _haveabspos) { + return true; + } return AP_InertialNav::altitude_ok(); } @@ -114,6 +159,9 @@ bool AP_InertialNav_NavEKF::altitude_ok() const */ float AP_InertialNav_NavEKF::get_altitude() const { + if (_ahrs.have_inertial_nav()) { + return _relpos_cm.z; + } return AP_InertialNav::get_altitude(); } @@ -126,6 +174,9 @@ float AP_InertialNav_NavEKF::get_altitude() const */ float AP_InertialNav_NavEKF::get_velocity_z() const { + if (_ahrs.have_inertial_nav()) { + return _velocity_cm.z; + } return AP_InertialNav::get_velocity_z(); } diff --git a/libraries/AP_InertialNav/AP_InertialNav_NavEKF.h b/libraries/AP_InertialNav/AP_InertialNav_NavEKF.h index 3dc2116259..d37987c28d 100644 --- a/libraries/AP_InertialNav/AP_InertialNav_NavEKF.h +++ b/libraries/AP_InertialNav/AP_InertialNav_NavEKF.h @@ -14,7 +14,7 @@ class AP_InertialNav_NavEKF : public AP_InertialNav { public: // Constructor - AP_InertialNav_NavEKF(const AP_AHRS &ahrs, AP_Baro &baro, GPS*& gps, GPS_Glitch& gps_glitch ) : + AP_InertialNav_NavEKF(AP_AHRS &ahrs, AP_Baro &baro, GPS*& gps, GPS_Glitch& gps_glitch ) : AP_InertialNav(ahrs, baro, gps, gps_glitch) { } @@ -24,6 +24,11 @@ public: */ void init(); + /** + update internal state + */ + void update(float dt); + /** * position_ok - true if inertial based altitude and position can be trusted * @return @@ -101,6 +106,12 @@ public: * @return climbrate in cm/s */ float get_velocity_z() const; + +private: + Vector3f _relpos_cm; // NEU + Vector3f _velocity_cm; // NEU + struct Location _abspos; + bool _haveabspos; }; #endif // __AP_INERTIALNAV_NAVEKF_H__