Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-07 01:04:19 +08:00
Code Issues Actions 13 Packages Projects Releases Wiki Activity

fixed geo functions

Browse Source
This commit is contained in:
Roman
2015-11-10 13:33:01 +01:00
parent e48cf53ce8
commit c989c21269
2 changed files with 34 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/geo/geo.c
+11 -4
View File
@@ -319,14 +319,21 @@ __EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, dou
}
__EXPORT void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist,
double *lat_end, double *lon_end)
double *lat_target, double *lon_target)
{
bearing = _wrap_2pi(bearing);
double radius_ratio = (double)(fabs(dist) / CONSTANTS_RADIUS_OF_EARTH);
*lat_end = asin(sin(lat_start) * cos(radius_ratio) + cos(lat_start) * sin(radius_ratio) * cos((double)bearing));
*lon_end = lon_start + atan2(sin((double)bearing) * sin(radius_ratio) * cos(lat_start),
cos(radius_ratio) - sin(lat_start) * sin(*lat_end));
double lat_start_rad = lat_start / (double)180.0 * M_PI;
double lon_start_rad = lon_start / (double)180.0 * M_PI;
*lat_target = asin(sin(lat_start_rad) * cos(radius_ratio) + cos(lat_start_rad) * sin(radius_ratio) * cos((
double)bearing));
*lon_target = lon_start_rad + atan2(sin((double)bearing) * sin(radius_ratio) * cos(lat_start_rad),
cos(radius_ratio) - sin(lat_start_rad) * sin(*lat_target));
*lat_target *= (double)180.0 / M_PI;
*lon_target *= (double)180.0 / M_PI;
}
__EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
{
src/lib/geo/geo.h
+23 -2
View File
@@ -237,12 +237,33 @@ __EXPORT int globallocalconverter_getref(double *lat_0, double *lon_0, float *al
__EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
// TODO put description for both functions and improve naming
/**
* Creates a new waypoint C on the line of two given waypoints (A, B) at certain distance
* from waypoint A
*
* @param lat_A waypoint A latitude in degrees (47.1234567°, not 471234567°)
* @param lon_A waypoint A longitude in degrees (8.1234567°, not 81234567°)
* @param lat_B waypoint B latitude in degrees (47.1234567°, not 471234567°)
* @param lon_B waypoint B longitude in degrees (8.1234567°, not 81234567°)
* @param dist distance of target waypoint from waypoint A (can be negative)
* @param lat_target latitude of target waypoint C in degrees (47.1234567°, not 471234567°)
* @param lon_target longitude of target waypoint C in degrees (47.1234567°, not 471234567°)
*/
__EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B, double lon_B, float dist,
double *lat_target, double *lon_target);
/**
* Creates a waypoint from given waypoint, distance and bearing
*
* @param lat_start latitude of starting waypoint in degrees (47.1234567°, not 471234567°)
* @param lon_start longitude of starting waypoint in degrees (8.1234567°, not 81234567°)
* @param bearing
* @param distance
* @param lat_target latitude of target waypoint in degrees (47.1234567°, not 471234567°)
* @param lon_target longitude of target waypoint in degrees (47.1234567°, not 471234567°)
*/
__EXPORT void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist,
double *end_lat, double *end_lon);
double *lat_target, double *lon_target);
/**
* Returns the bearing to the next waypoint in radians.