Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_InertialNav: add comments, rename incorrectly named member, 

initialize member, remove redundant assignment
adjustments to original commit by randy
This commit is contained in:
Tobias 2013-08-07 16:00:09 +02:00 committed by Randy Mackay
parent 5af51140a9
commit cbff58e2ed
2 changed files with 8 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_InertialNav/AP_InertialNav.cpp
View File

@ -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

7
libraries/AP_InertialNav/AP_InertialNav.h
View File

@ -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