Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-24 07:09:48 +08:00
Code Issues Actions 5 Packages Projects Releases Wiki Activity

geo get_bearing_to_next_waypoint() small optimization

Browse Source
This commit is contained in:
Daniel Agar
2018-06-30 11:14:41 -04:00
committed by Lorenz Meier
parent b26c2d62b8
commit 114ae4116a
1 changed files with 6 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
geo/geo.cpp
+6 -7
View File
@@ -319,17 +319,16 @@ void waypoint_from_heading_and_distance(double lat_start, double lon_start, floa
float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
{
double lat_now_rad = math::radians(lat_now);
double lon_now_rad = math::radians(lon_now);
double lat_next_rad = math::radians(lat_next);
double lon_next_rad = math::radians(lon_next);
const double lat_now_rad = math::radians(lat_now);
const double lat_next_rad = math::radians(lat_next);
double d_lon = lon_next_rad - lon_now_rad;
const double cos_lat_next = cos(lat_next_rad);
const double d_lon = math::radians(lon_next - lon_now);
/* conscious mix of double and float trig function to maximize speed and efficiency */
const float y = static_cast<float>(sin(d_lon) * cos(lat_next_rad));
const float x = static_cast<float>(cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon));
const float y = static_cast<float>(sin(d_lon) * cos_lat_next);
const float x = static_cast<float>(cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos_lat_next * cos(d_lon));
return wrap_pi(atan2f(y, x));
}