mirror of
https://github.com/azaion/autopilot.git
synced 2026-04-22 10:16:33 +00:00
Tuomas Järvinen
ab791eb254
Added error printing and possibility to use UART as connection to the flight controller. Issue: https://denyspopov.atlassian.net/browse/AZ-16 Type: New Feature
22 lines
625 B
C++
22 lines
625 B
C++
#include "az_utils.h"
|
|
#include <math.h>
|
|
|
|
AzUtils::AzUtils() {}
|
|
|
|
#define EARTH_RADIUS 6371.0 // Earth radius in kilometers
|
|
|
|
double degrees_to_radians(double degrees)
|
|
{
|
|
return degrees * M_PI / 180.0;
|
|
}
|
|
|
|
double AzUtils::distance(double lat1, double lon1, double lat2, double lon2)
|
|
{
|
|
double dlat = degrees_to_radians(lat2 - lat1);
|
|
double dlon = degrees_to_radians(lon2 - lon1);
|
|
double a = sin(dlat / 2) * sin(dlat / 2)
|
|
+ cos(degrees_to_radians(lat1)) * cos(degrees_to_radians(lat2)) * sin(dlon / 2) * sin(dlon / 2);
|
|
double c = 2 * atan2(sqrt(a), sqrt(1 - a));
|
|
return EARTH_RADIUS * c;
|
|
}
|