fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 21:43:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Completed main implementation and debugging

Browse Source
This commit is contained in:
Simon Wilks
2013-06-04 00:10:58 +02:00
parent 6571629dca
commit f435025d26
6 changed files with 133 additions and 93 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/systemlib/geo/geo.c
+4 -4
View File
@@ -46,7 +46,7 @@
#include <systemlib/geo/geo.h>
#include <nuttx/config.h>
#include <unistd.h>
//#include <unistd.h>
#include <pthread.h>
#include <stdio.h>
#include <math.h>
@@ -185,12 +185,12 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou
double d_lat = lat_next_rad - lat_now_rad;
double d_lon = lon_next_rad - lon_now_rad;
double a = sin(d_lat / 2.0d) * sin(d_lat / 2.0) + sin(d_lon / 2.0d) * sin(d_lon / 2.0d) * cos(lat_now_rad) * cos(lat_next_rad);
double a = sin(d_lat / 2.0d) * sin(d_lat / 2.0d) + sin(d_lon / 2.0d) * sin(d_lon / 2.0d) * cos(lat_now_rad) * cos(lat_next_rad);
double c = 2.0d * atan2(sqrt(a), sqrt(1.0d - a));
const double radius_earth = 6371000.0d;
return radius_earth * c;
printf("DIST: %.4f\n", radius_earth * c);
return radius_earth * c;
}
__EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
src/modules/systemlib/geo/geo.h
+16
View File
@@ -82,8 +82,24 @@ __EXPORT void map_projection_project(double lat, double lon, float *x, float *y)
*/
__EXPORT void map_projection_reproject(float x, float y, double *lat, double *lon);
/**
* Returns the distance to the next waypoint in meters.
*
* @param lat_now current position in degrees (47.1234567°, not 471234567°)
* @param lon_now current position in degrees (8.1234567°, not 81234567°)
* @param lat_next next waypoint position in degrees (47.1234567°, not 471234567°)
* @param lon_next next waypoint position in degrees (8.1234567°, not 81234567°)
*/
__EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
/**
* Returns the bearing to the next waypoint in radians.
*
* @param lat_now current position in degrees (47.1234567°, not 471234567°)
* @param lon_now current position in degrees (8.1234567°, not 81234567°)
* @param lat_next next waypoint position in degrees (47.1234567°, not 471234567°)
* @param lon_next next waypoint position in degrees (8.1234567°, not 81234567°)
*/
__EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
__EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end);