You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
30 lines
979 B
30 lines
979 B
#include "Sub.h" |
|
|
|
// position_vector.pde related utility functions |
|
|
|
// position vectors are Vector3f |
|
// .x = latitude from home in cm |
|
// .y = longitude from home in cm |
|
// .z = altitude above home in cm |
|
|
|
// pv_location_to_vector - convert lat/lon coordinates to a position vector |
|
Vector3f Sub::pv_location_to_vector(const Location& loc) |
|
{ |
|
Location origin; |
|
if (!ahrs.get_origin(origin)) { |
|
origin.zero(); |
|
} |
|
float alt_above_origin = pv_alt_above_origin(loc.alt); // convert alt-relative-to-home to alt-relative-to-origin |
|
return Vector3f((loc.lat-origin.lat) * LATLON_TO_CM, (loc.lng-origin.lng) * LATLON_TO_CM * scaleLongDown, alt_above_origin); |
|
} |
|
|
|
// pv_alt_above_origin - convert altitude above home to altitude above EKF origin |
|
float Sub::pv_alt_above_origin(float alt_above_home_cm) |
|
{ |
|
Location origin; |
|
if (!ahrs.get_origin(origin)) { |
|
origin.zero(); |
|
} |
|
return alt_above_home_cm + (ahrs.get_home().alt - origin.alt); |
|
} |
|
|
|
|