Skip to content
Snippets Groups Projects
Commit cbff58e2 authored by Tobias's avatar Tobias Committed by Randy Mackay
Browse files

AP_InertialNav: add comments, rename incorrectly named member,

initialize member, remove redundant assignment
adjustments to original commit by randy
parent 5af51140
No related branches found
No related tags found
No related merge requests found
...@@ -160,7 +160,7 @@ void AP_InertialNav::correct_with_gps(uint32_t now, int32_t lon, int32_t lat) ...@@ -160,7 +160,7 @@ void AP_InertialNav::correct_with_gps(uint32_t now, int32_t lon, int32_t lat)
// calculate distance from base location // calculate distance from base location
x = (float)(lat - _base_lat) * LATLON_TO_CM; 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 // sanity check the gps position. Relies on the main code calling GPS_Glitch::check_position() immediatley after a GPS update
if (_glitch_detector.glitching()) { if (_glitch_detector.glitching()) {
...@@ -215,7 +215,7 @@ int32_t AP_InertialNav::get_longitude() const ...@@ -215,7 +215,7 @@ int32_t AP_InertialNav::get_longitude() const
return 0; 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 // 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) ...@@ -229,7 +229,7 @@ void AP_InertialNav::set_home_position(int32_t lon, int32_t lat)
Location temp_loc; Location temp_loc;
temp_loc.lat = lat; temp_loc.lat = lat;
temp_loc.lng = lon; 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 // reset corrections to base position to zero
_position_base.x = 0; _position_base.x = 0;
...@@ -264,7 +264,7 @@ float AP_InertialNav::get_longitude_diff() const ...@@ -264,7 +264,7 @@ float AP_InertialNav::get_longitude_diff() const
return 0; 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 // get velocity in latitude & longitude directions
......
...@@ -34,6 +34,7 @@ public: ...@@ -34,6 +34,7 @@ public:
_xy_enabled(false), _xy_enabled(false),
_gps_last_update(0), _gps_last_update(0),
_gps_last_time(0), _gps_last_time(0),
_historic_xy_counter(0),
_baro_last_update(0), _baro_last_update(0),
_glitch_detector(gps_glitch) _glitch_detector(gps_glitch)
{ {
...@@ -65,7 +66,7 @@ public: ...@@ -65,7 +66,7 @@ public:
// get_position - returns current position from home in cm // get_position - returns current position from home in cm
Vector3f get_position() const { return _position_base + _position_correction; } 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_latitude() const;
int32_t get_longitude() const; int32_t get_longitude() const;
...@@ -146,14 +147,14 @@ protected: ...@@ -146,14 +147,14 @@ protected:
AP_BufferFloat_Size5 _hist_position_estimate_y; // buffer of historic accel based position to account for lag 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_lat; // base latitude
int32_t _base_lon; // base longitude 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 // Z Axis specific variables
AP_Float _time_constant_z; // time constant for vertical corrections AP_Float _time_constant_z; // time constant for vertical corrections
float _k1_z; // gain for vertical position correction float _k1_z; // gain for vertical position correction
float _k2_z; // gain for vertical velocity correction float _k2_z; // gain for vertical velocity correction
float _k3_z; // gain for vertical accelerometer offset 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 AP_BufferFloat_Size15 _hist_position_estimate_z; // buffer of historic accel based altitudes to account for lag
// general variables // general variables
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment