Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialNav: use AHRS home location

Browse Source
This commit is contained in:
Andrew Tridgell 2014-01-03 22:41:03 +11:00
parent c6e25483b4
commit 99097d80a1
2 changed files with 12 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
libraries/AP_InertialNav/AP_InertialNav.cpp
View File

@ -168,8 +168,8 @@ 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_cm_scaling;
x = (float)(lat - _ahrs.get_home().lat) * LATLON_TO_CM;
y = (float)(lon - _ahrs.get_home().lng) * _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()) {
@ -213,7 +213,7 @@ int32_t AP_InertialNav::get_latitude() const
return 0;
}
return _base_lat + (int32_t)(_position.x/LATLON_TO_CM);
return _ahrs.get_home().lat + (int32_t)(_position.x/LATLON_TO_CM);
}
// get accel based longitude
@ -224,21 +224,14 @@ int32_t AP_InertialNav::get_longitude() const
return 0;
}
return _base_lon + (int32_t)(_position.y / _lon_to_cm_scaling);
return _ahrs.get_home().lng + (int32_t)(_position.y / _lon_to_cm_scaling);
}
// set_home_position - all internal calculations are recorded as the distances from this point
void AP_InertialNav::set_home_position(int32_t lon, int32_t lat)
// setup_home_position - reset state for home position change
void AP_InertialNav::setup_home_position(void)
{
// set base location
_base_lon = lon;
_base_lat = lat;
// set longitude to meters scaling to offset the shrinking longitude as we go towards the poles
Location temp_loc;
temp_loc.lat = lat;
temp_loc.lng = lon;
_lon_to_cm_scaling = longitude_scale(temp_loc) * LATLON_TO_CM;
_lon_to_cm_scaling = longitude_scale(_ahrs.get_home()) * LATLON_TO_CM;
// reset corrections to base position to zero
_position_base.x = 0;

22
libraries/AP_InertialNav/AP_InertialNav.h
View File

@ -51,9 +51,6 @@ public:
/**
* initializes the object.
*
* AP_InertialNav::set_home_position(int32_t, int32_t) should be called later,
* to enable all "horizontal related" getter-methods.
*/
void init();
@ -104,8 +101,6 @@ public:
/**
* 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 { return _position; }
@ -122,16 +117,6 @@ public:
*/
int32_t get_longitude() const;
/**
* set_home_position - sets home position
*
* all internal calculations are recorded as the distances from this point.
*
* @param lon : longitude in 100 nano degrees (i.e. degree value multiplied by 10,000,000)
* @param lat : latitude in 100 nano degrees (i.e. degree value multiplied by 10,000,000)
*/
void set_home_position(int32_t lon, int32_t lat);
/**
* get_latitude_diff - returns the current latitude difference from the home location.
*
@ -171,6 +156,11 @@ public:
*/
void set_velocity_xy(float x, float y);
/**
setup_home_position - reset on home position change
*/
void setup_home_position(void);
//
// Z Axis methods
//
@ -295,8 +285,6 @@ protected:
uint8_t _historic_xy_counter; // counter used to slow saving of position estimates for later comparison to gps
AP_BufferFloat_Size5 _hist_position_estimate_x; // buffer of historic accel based position to account for gpslag
AP_BufferFloat_Size5 _hist_position_estimate_y; // buffer of historic accel based position to account for gps lag
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