From 658110dd8ca84fcbdf3f92fec03f86afc3a02877 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 3 Jan 2014 22:57:50 +1100 Subject: [PATCH] AP_InertialNav: added AP_InertialNav_NavEKF wrapper class --- libraries/AP_InertialNav/AP_InertialNav.h | 4 + .../AP_InertialNav/AP_InertialNav_NavEKF.cpp | 132 ++++++++++++++++++ .../AP_InertialNav/AP_InertialNav_NavEKF.h | 106 ++++++++++++++ 3 files changed, 242 insertions(+) create mode 100644 libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp create mode 100644 libraries/AP_InertialNav/AP_InertialNav_NavEKF.h diff --git a/libraries/AP_InertialNav/AP_InertialNav.h b/libraries/AP_InertialNav/AP_InertialNav.h index 22e1614af8..ed92510fb8 100644 --- a/libraries/AP_InertialNav/AP_InertialNav.h +++ b/libraries/AP_InertialNav/AP_InertialNav.h @@ -308,4 +308,8 @@ protected: }; +#if AP_AHRS_NAVEKF_AVAILABLE +#include "AP_InertialNav_NavEKF.h" +#endif + #endif // __AP_INERTIALNAV_H__ diff --git a/libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp b/libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp new file mode 100644 index 0000000000..b95753214f --- /dev/null +++ b/libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp @@ -0,0 +1,132 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#include "AP_InertialNav.h" + +#if AP_AHRS_NAVEKF_AVAILABLE + +/* + A wrapper around the AP_InertialNav class which uses the NavEKF + filter if available, and falls back to the AP_InertialNav filter + when EKF is not available + */ + +/** + * initializes the object. + */ +void AP_InertialNav_NavEKF::init() +{ + AP_InertialNav::init(); +} + +/** + * position_ok - true if inertial based altitude and position can be trusted + * @return + */ +bool AP_InertialNav_NavEKF::position_ok() const +{ + return AP_InertialNav::position_ok(); +} + +/** + * get_position - returns the current position relative to the home location in cm. + * + * the home location was set with AP_InertialNav::set_home_position(int32_t, int32_t) + * + * @return + */ +const Vector3f &AP_InertialNav_NavEKF::get_position(void) const +{ + return AP_InertialNav::get_position(); +} + +/** + * get_latitude - returns the latitude of the current position estimation in 100 nano degrees (i.e. degree value multiplied by 10,000,000) + */ +int32_t AP_InertialNav_NavEKF::get_latitude() const +{ + return AP_InertialNav::get_latitude(); +} + +/** + * 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 AP_InertialNav_NavEKF::get_longitude() const +{ + return AP_InertialNav::get_longitude(); +} + +/** + * 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 AP_InertialNav_NavEKF::get_latitude_diff() const +{ + return AP_InertialNav::get_latitude_diff(); +} + +/** + * 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 AP_InertialNav_NavEKF::get_longitude_diff() const +{ + return AP_InertialNav::get_longitude_diff(); +} + +/** + * get_velocity - returns the current velocity in cm/s + * + * @return velocity vector: + * .x : latitude velocity in cm/s + * .y : longitude velocity in cm/s + * .z : vertical velocity in cm/s + */ +const Vector3f &AP_InertialNav_NavEKF::get_velocity() const +{ + return AP_InertialNav::get_velocity(); +} + +/** + * get_velocity_xy - returns the current horizontal velocity in cm/s + * + * @returns the current horizontal velocity in cm/s + */ +float AP_InertialNav_NavEKF::get_velocity_xy() +{ + return AP_InertialNav::get_velocity_xy(); +} + +/** + * altitude_ok - returns true if inertial based altitude and position can be trusted + * @return + */ +bool AP_InertialNav_NavEKF::altitude_ok() const +{ + return AP_InertialNav::altitude_ok(); +} + +/** + * get_altitude - get latest altitude estimate in cm + * @return + */ +float AP_InertialNav_NavEKF::get_altitude() const +{ + return AP_InertialNav::get_altitude(); +} + +/** + * get_velocity_z - returns the current climbrate. + * + * @see get_velocity().z + * + * @return climbrate in cm/s + */ +float AP_InertialNav_NavEKF::get_velocity_z() const +{ + return AP_InertialNav::get_velocity_z(); +} + +#endif // AP_AHRS_NAVEKF_AVAILABLE diff --git a/libraries/AP_InertialNav/AP_InertialNav_NavEKF.h b/libraries/AP_InertialNav/AP_InertialNav_NavEKF.h new file mode 100644 index 0000000000..3dc2116259 --- /dev/null +++ b/libraries/AP_InertialNav/AP_InertialNav_NavEKF.h @@ -0,0 +1,106 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +/* + A wrapper around the AP_InertialNav class which uses the NavEKF + filter if available, and falls back to the AP_InertialNav filter + when EKF is not available + */ + + +#ifndef __AP_INERTIALNAV_NAVEKF_H__ +#define __AP_INERTIALNAV_NAVEKF_H__ + +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(ahrs, baro, gps, gps_glitch) + { + } + + /** + * initializes the object. + */ + void init(); + + /** + * position_ok - true if inertial based altitude and position can be trusted + * @return + */ + bool position_ok() const; + + /** + * get_position - returns the current position relative to the home location in cm. + * + * the home location was set with AP_InertialNav::set_home_position(int32_t, int32_t) + * + * @return + */ + const Vector3f& get_position() const; + + /** + * get_latitude - returns the latitude of the current position estimation in 100 nano degrees (i.e. degree value multiplied by 10,000,000) + */ + 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; + + /** + * 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; + + /** + * 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; + + /** + * get_velocity - returns the current velocity in cm/s + * + * @return velocity vector: + * .x : latitude velocity in cm/s + * .y : longitude velocity in cm/s + * .z : vertical velocity in cm/s + */ + const Vector3f& get_velocity() const; + + /** + * get_velocity_xy - returns the current horizontal velocity in cm/s + * + * @returns the current horizontal velocity in cm/s + */ + float get_velocity_xy(); + + /** + * altitude_ok - returns true if inertial based altitude and position can be trusted + * @return + */ + bool altitude_ok() const; + + /** + * get_altitude - get latest altitude estimate in cm + * @return + */ + float get_altitude() const; + + /** + * get_velocity_z - returns the current climbrate. + * + * @see get_velocity().z + * + * @return climbrate in cm/s + */ + float get_velocity_z() const; +}; + +#endif // __AP_INERTIALNAV_NAVEKF_H__