From cbff58e2ed50bc425d5d22ca53991412396eb1a6 Mon Sep 17 00:00:00 2001 From: Tobias Date: Wed, 7 Aug 2013 16:00:09 +0200 Subject: [PATCH] AP_InertialNav: add comments, rename incorrectly named member, initialize member, remove redundant assignment adjustments to original commit by randy --- libraries/AP_InertialNav/AP_InertialNav.cpp | 8 ++++---- libraries/AP_InertialNav/AP_InertialNav.h | 7 ++++--- 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/libraries/AP_InertialNav/AP_InertialNav.cpp b/libraries/AP_InertialNav/AP_InertialNav.cpp index 3cf1b9dac0..0b92c0e64a 100644 --- a/libraries/AP_InertialNav/AP_InertialNav.cpp +++ b/libraries/AP_InertialNav/AP_InertialNav.cpp @@ -160,7 +160,7 @@ void AP_InertialNav::correct_with_gps(uint32_t now, int32_t lon, int32_t lat) // calculate distance from base location x = (float)(lat - _base_lat) * LATLON_TO_CM; - y = (float)(lon - _base_lon) * _lon_to_m_scaling; + y = (float)(lon - _base_lon) * _lon_to_cm_scaling; // sanity check the gps position. Relies on the main code calling GPS_Glitch::check_position() immediatley after a GPS update if (_glitch_detector.glitching()) { @@ -215,7 +215,7 @@ int32_t AP_InertialNav::get_longitude() const return 0; } - return _base_lon + (int32_t)((_position_base.y+_position_correction.y) / _lon_to_m_scaling); + return _base_lon + (int32_t)((_position_base.y+_position_correction.y) / _lon_to_cm_scaling); } // set_home_position - all internal calculations are recorded as the distances from this point @@ -229,7 +229,7 @@ void AP_InertialNav::set_home_position(int32_t lon, int32_t lat) Location temp_loc; temp_loc.lat = lat; temp_loc.lng = lon; - _lon_to_m_scaling = longitude_scale(temp_loc) * LATLON_TO_CM; + _lon_to_cm_scaling = longitude_scale(temp_loc) * LATLON_TO_CM; // reset corrections to base position to zero _position_base.x = 0; @@ -264,7 +264,7 @@ float AP_InertialNav::get_longitude_diff() const return 0; } - return (_position_base.y+_position_correction.y) / _lon_to_m_scaling; + return (_position_base.y+_position_correction.y) / _lon_to_cm_scaling; } // get velocity in latitude & longitude directions diff --git a/libraries/AP_InertialNav/AP_InertialNav.h b/libraries/AP_InertialNav/AP_InertialNav.h index 4dbb4fdff6..f3f7e5499c 100644 --- a/libraries/AP_InertialNav/AP_InertialNav.h +++ b/libraries/AP_InertialNav/AP_InertialNav.h @@ -34,6 +34,7 @@ public: _xy_enabled(false), _gps_last_update(0), _gps_last_time(0), + _historic_xy_counter(0), _baro_last_update(0), _glitch_detector(gps_glitch) { @@ -65,7 +66,7 @@ public: // get_position - returns current position from home in cm Vector3f get_position() const { return _position_base + _position_correction; } - // get latitude & longitude positions + // get latitude & longitude positions in degrees * 10,000,000 int32_t get_latitude() const; int32_t get_longitude() const; @@ -146,14 +147,14 @@ protected: AP_BufferFloat_Size5 _hist_position_estimate_y; // buffer of historic accel based position to account for lag int32_t _base_lat; // base latitude int32_t _base_lon; // base longitude - float _lon_to_m_scaling; // conversion of longitude to meters + float _lon_to_cm_scaling; // conversion of longitude to centimeters // Z Axis specific variables AP_Float _time_constant_z; // time constant for vertical corrections float _k1_z; // gain for vertical position correction float _k2_z; // gain for vertical velocity correction float _k3_z; // gain for vertical accelerometer offset correction - uint32_t _baro_last_update; // time of last barometer update + uint32_t _baro_last_update; // time of last barometer update AP_BufferFloat_Size15 _hist_position_estimate_z; // buffer of historic accel based altitudes to account for lag // general variables