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.
25 lines
1.2 KiB
25 lines
1.2 KiB
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- |
|
|
|
#include <AP_Mount_Backend.h> |
|
|
|
// calc_angle_to_location - calculates the earth-frame roll, tilt and pan angles (and radians) to point at the given target |
|
void AP_Mount_Backend::calc_angle_to_location(const struct Location &target, Vector3f& angles_to_target_rad, bool calc_tilt, bool calc_pan) |
|
{ |
|
float GPS_vector_x = (target.lng-_frontend._current_loc.lng)*cosf(ToRad((_frontend._current_loc.lat+target.lat)*0.00000005f))*0.01113195f; |
|
float GPS_vector_y = (target.lat-_frontend._current_loc.lat)*0.01113195f; |
|
float GPS_vector_z = (target.alt-_frontend._current_loc.alt); // baro altitude(IN CM) should be adjusted to known home elevation before take off (Set altimeter). |
|
float target_distance = 100.0f*pythagorous2(GPS_vector_x, GPS_vector_y); // Careful , centimeters here locally. Baro/alt is in cm, lat/lon is in meters. |
|
|
|
// initialise all angles to zero |
|
angles_to_target_rad.zero(); |
|
|
|
// tilt calcs |
|
if (calc_tilt) { |
|
angles_to_target_rad.y = atan2f(GPS_vector_z, target_distance); |
|
} |
|
|
|
// pan calcs |
|
if (calc_pan) { |
|
angles_to_target_rad.z = atan2f(GPS_vector_x, GPS_vector_y); |
|
} |
|
}
|
|
|