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

AP_Common: have get_vector_xy_from_origin_NE take a Vector2f rather than 3f 

Also, rename it from get_vector_xy_from_origin_NED
This commit is contained in:
Peter Barker 2017-12-14 13:09:59 +11:00 committed by Francisco Ferreira
parent bf5f51a616
commit 302e5c6871
2 changed files with 10 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_Common/Location.cpp
View File

@ -190,24 +190,26 @@ bool Location_Class::get_alt_cm(ALT_FRAME desired_frame, int32_t &ret_alt_cm) co
}
}
bool Location_Class::get_vector_xy_from_origin_NEU(Vector3f &vec_neu) const
bool Location_Class::get_vector_xy_from_origin_NE(Vector2f &vec_ne) const
{
// convert to neu
Location ekf_origin;
if (!_ahrs->get_origin(ekf_origin)) {
return false;
}
vec_neu.x = (lat-ekf_origin.lat) * LATLON_TO_CM;
vec_neu.y = (lng-ekf_origin.lng) * LATLON_TO_CM * longitude_scale(ekf_origin);
vec_ne.x = (lat-ekf_origin.lat) * LATLON_TO_CM;
vec_ne.y = (lng-ekf_origin.lng) * LATLON_TO_CM * longitude_scale(ekf_origin);
return true;
}
bool Location_Class::get_vector_from_origin_NEU(Vector3f &vec_neu) const
{
// convert lat, lon
if (!get_vector_xy_from_origin_NEU(vec_neu)) {
Vector2f vec_ne;
if (!get_vector_xy_from_origin_NE(vec_ne)) {
return false;
}
vec_neu.x = vec_ne.x;
vec_neu.y = vec_ne.y;
// convert altitude
int32_t alt_above_origin_cm = 0;

5
libraries/AP_Common/Location.h
View File

@ -55,10 +55,11 @@ public:
// the original frame or desired frame is above-terrain
bool change_alt_frame(ALT_FRAME desired_frame);
// get position as a vector from home (x,y only or x,y,z)
// get position as a vector from origin (x,y only or x,y,z)
// return false on failure to get the vector which can only
// happen if the EKF origin has not been set yet
bool get_vector_xy_from_origin_NEU(Vector3f &vec_neu) const;
// x, y and z are in centimetres
bool get_vector_xy_from_origin_NE(Vector2f &vec_ne) const;
bool get_vector_from_origin_NEU(Vector3f &vec_neu) const;
// return distance in meters between two locations