Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 15:53:56 -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 // calculate distance from base location
x = (float)(lat - _base_lat) * LATLON_TO_CM; x = (float)(lat - _ahrs.get_home().lat) * LATLON_TO_CM;
y = (float)(lon - _base_lon) * _lon_to_cm_scaling; 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 // 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()) {
@ -213,7 +213,7 @@ int32_t AP_InertialNav::get_latitude() const
return 0; 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 // get accel based longitude
@ -224,21 +224,14 @@ int32_t AP_InertialNav::get_longitude() const
return 0; 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 // setup_home_position - reset state for home position change
void AP_InertialNav::set_home_position(int32_t lon, int32_t lat) 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 // set longitude to meters scaling to offset the shrinking longitude as we go towards the poles
Location temp_loc; _lon_to_cm_scaling = longitude_scale(_ahrs.get_home()) * LATLON_TO_CM;
temp_loc.lat = lat;
temp_loc.lng = lon;
_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;

22
libraries/AP_InertialNav/AP_InertialNav.h
View File

@ -51,9 +51,6 @@ public:
/** /**
* initializes the object. * 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(); void init();
@ -104,8 +101,6 @@ public:
/** /**
* get_position - returns the current position relative to the home location in cm. * 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 * @return
*/ */
const Vector3f& get_position() const { return _position; } const Vector3f& get_position() const { return _position; }
@ -122,16 +117,6 @@ public:
*/ */
int32_t get_longitude() const; 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. * 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); void set_velocity_xy(float x, float y);
/**
setup_home_position - reset on home position change
*/
void setup_home_position(void);
// //
// Z Axis methods // 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 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_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 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 float _lon_to_cm_scaling; // conversion of longitude to centimeters
// Z Axis specific variables // Z Axis specific variables