From 2de6dbd3d8c5b2a4e95337b96a5c22686d42946a Mon Sep 17 00:00:00 2001 From: Jason Short Date: Mon, 28 Oct 2013 21:25:14 -0700 Subject: [PATCH] InertialNav: added get_velocity_xy --- libraries/AP_InertialNav/AP_InertialNav.cpp | 6 ++++++ libraries/AP_InertialNav/AP_InertialNav.h | 9 ++++++++- 2 files changed, 14 insertions(+), 1 deletion(-) diff --git a/libraries/AP_InertialNav/AP_InertialNav.cpp b/libraries/AP_InertialNav/AP_InertialNav.cpp index d0470544a0..4029113a9f 100644 --- a/libraries/AP_InertialNav/AP_InertialNav.cpp +++ b/libraries/AP_InertialNav/AP_InertialNav.cpp @@ -276,6 +276,12 @@ void AP_InertialNav::set_velocity_xy(float x, float y) _velocity.y = y; } +// set_velocity_xy - set velocity in latitude & longitude directions (in cm/s) +float AP_InertialNav::get_velocity_xy() +{ + return safe_sqrt(_velocity.x + _velocity.y); +} + // // Z Axis methods // diff --git a/libraries/AP_InertialNav/AP_InertialNav.h b/libraries/AP_InertialNav/AP_InertialNav.h index 21c986442b..aaef1f9d4d 100644 --- a/libraries/AP_InertialNav/AP_InertialNav.h +++ b/libraries/AP_InertialNav/AP_InertialNav.h @@ -155,6 +155,13 @@ public: */ 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(); + /** * set_velocity_xy - overwrites the current horizontal velocity in cm/s * @@ -279,7 +286,7 @@ protected: int32_t _base_lat; // base latitude (home location) in 100 nano degrees (i.e. degree value multiplied by 10,000,000) int32_t _base_lon; // base longitude (home location) in 100 nano degrees (i.e. degree value multiplied by 10,000,000) float _lon_to_cm_scaling; // conversion of longitude to centimeters - + // Z Axis specific variables AP_Float _time_constant_z; // time constant for vertical corrections in s float _k1_z; // gain for vertical position correction