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

Copter: functions to convert alt-above-home vs alt-above-origin

Browse Source
This commit is contained in:
Randy Mackay 2015-02-10 21:26:53 +09:00
parent 9bd3a7249a
commit cb66bf8b98
1 changed files with 16 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
ArduCopter/position_vector.pde
View File

@ -11,7 +11,8 @@
Vector3f pv_location_to_vector(const Location& loc) Vector3f pv_location_to_vector(const Location& loc)
{ {
const struct Location &origin = inertial_nav.get_origin(); const struct Location &origin = inertial_nav.get_origin();
Vector3f tmp((loc.lat-origin.lat) * LATLON_TO_CM, (loc.lng-origin.lng) * LATLON_TO_CM * scaleLongDown, loc.alt); float alt_above_origin = loc.alt + (ahrs.get_home().alt - origin.alt); // convert alt-relative-to-home to alt-relative-to-origin
Vector3f tmp((loc.lat-origin.lat) * LATLON_TO_CM, (loc.lng-origin.lng) * LATLON_TO_CM * scaleLongDown, alt_above_origin);
return tmp; return tmp;
} }
@ -35,6 +36,20 @@ Vector3f pv_location_to_vector_with_default(const Location& loc, const Vector3f&
return pos; return pos;
} }
// pv_alt_above_origin - convert altitude above home to altitude above EKF origin
float pv_alt_above_origin(float alt_above_home_cm)
{
const struct Location &origin = inertial_nav.get_origin();
return alt_above_home_cm + (ahrs.get_home().alt - origin.alt);
}
// pv_alt_above_home - convert altitude above EKF origin to altitude above home
float pv_alt_above_home(float alt_above_origin_cm)
{
const struct Location &origin = inertial_nav.get_origin();
return alt_above_origin_cm + (origin.alt - ahrs.get_home().alt);
}
// pv_get_bearing_cd - return bearing in centi-degrees between two positions // pv_get_bearing_cd - return bearing in centi-degrees between two positions
float pv_get_bearing_cd(const Vector3f &origin, const Vector3f &destination) float pv_get_bearing_cd(const Vector3f &origin, const Vector3f &destination)
{ {