rbxii3/firmware
1
0
Fork 0
mirror of https://github.com/meshtastic/firmware.git synced 2026-01-06 18:08:00 +00:00
Code Issues Packages Projects Releases Wiki Activity

trunk roundhouse kick

Browse Source
This commit is contained in:
Thomas Göttgens
2023-01-21 14:34:29 +01:00
parent 6cf18b7d07
commit 51b2c431d9
234 changed files with 4989 additions and 5101 deletions
Download Patch File Download Diff File Expand all files Collapse all files

285
src/gps/GeoCoord.cpp
View File

@@ -1,21 +1,25 @@
#include "GeoCoord.h"
GeoCoord::GeoCoord() {
GeoCoord::GeoCoord()
{
_dirty = true;
}
GeoCoord::GeoCoord (int32_t lat, int32_t lon, int32_t alt) : _latitude(lat), _longitude(lon), _altitude(alt) {
GeoCoord::GeoCoord(int32_t lat, int32_t lon, int32_t alt) : _latitude(lat), _longitude(lon), _altitude(alt)
{
GeoCoord::setCoords();
}
GeoCoord::GeoCoord (float lat, float lon, int32_t alt) : _altitude(alt) {
GeoCoord::GeoCoord(float lat, float lon, int32_t alt) : _altitude(alt)
{
// Change decimial reprsentation to int32_t. I.e., 12.345 becomes 123450000
_latitude = int32_t(lat * 1e+7);
_longitude = int32_t(lon * 1e+7);
GeoCoord::setCoords();
}
GeoCoord::GeoCoord(double lat, double lon, int32_t alt): _altitude(alt) {
GeoCoord::GeoCoord(double lat, double lon, int32_t alt) : _altitude(alt)
{
// Change decimial reprsentation to int32_t. I.e., 12.345 becomes 123450000
_latitude = int32_t(lat * 1e+7);
_longitude = int32_t(lon * 1e+7);
@@ -23,7 +27,8 @@ GeoCoord::GeoCoord(double lat, double lon, int32_t alt): _altitude(alt) {
}
// Initialize all the coordinate systems
void GeoCoord::setCoords() {
void GeoCoord::setCoords()
{
double lat = _latitude * 1e-7;
double lon = _longitude * 1e-7;
GeoCoord::latLongToDMS(lat, lon, _dms);
@@ -34,9 +39,10 @@ void GeoCoord::setCoords() {
_dirty = false;
}
void GeoCoord::updateCoords(int32_t lat, int32_t lon, int32_t alt) {
void GeoCoord::updateCoords(int32_t lat, int32_t lon, int32_t alt)
{
// If marked dirty or new coordiantes
if(_dirty || _latitude != lat || _longitude != lon || _altitude != alt) {
if (_dirty || _latitude != lat || _longitude != lon || _altitude != alt) {
_dirty = true;
_latitude = lat;
_longitude = lon;
@@ -45,25 +51,26 @@ void GeoCoord::updateCoords(int32_t lat, int32_t lon, int32_t alt) {
}
}
void GeoCoord::updateCoords(const double lat, const double lon, const int32_t alt) {
void GeoCoord::updateCoords(const double lat, const double lon, const int32_t alt)
{
int32_t iLat = lat * 1e+7;
int32_t iLon = lon * 1e+7;
// If marked dirty or new coordiantes
if(_dirty || _latitude != iLat || _longitude != iLon || _altitude != alt) {
if (_dirty || _latitude != iLat || _longitude != iLon || _altitude != alt) {
_dirty = true;
_latitude = iLat;
_longitude = iLon;
_altitude = alt;
setCoords();
}
}
void GeoCoord::updateCoords(const float lat, const float lon, const int32_t alt) {
void GeoCoord::updateCoords(const float lat, const float lon, const int32_t alt)
{
int32_t iLat = lat * 1e+7;
int32_t iLon = lon * 1e+7;
// If marked dirty or new coordiantes
if(_dirty || _latitude != iLat || _longitude != iLon || _altitude != alt) {
if (_dirty || _latitude != iLat || _longitude != iLon || _altitude != alt) {
_dirty = true;
_latitude = iLat;
_longitude = iLon;
@@ -73,12 +80,15 @@ void GeoCoord::updateCoords(const float lat, const float lon, const int32_t alt)
}
/**
* Converts lat long coordinates from decimal degrees to degrees minutes seconds format.
* Converts lat long coordinates from decimal degrees to degrees minutes seconds format.
* DD°MM'SS"C DDD°MM'SS"C
*/
void GeoCoord::latLongToDMS(const double lat, const double lon, DMS &dms) {
if (lat < 0) dms.latCP = 'S';
else dms.latCP = 'N';
void GeoCoord::latLongToDMS(const double lat, const double lon, DMS &dms)
{
if (lat < 0)
dms.latCP = 'S';
else
dms.latCP = 'N';
double latDeg = lat;
@@ -90,8 +100,10 @@ void GeoCoord::latLongToDMS(const double lat, const double lon, DMS &dms) {
dms.latMin = floor(latMin);
dms.latSec = (latMin - dms.latMin) * 60;
if (lon < 0) dms.lonCP = 'W';
else dms.lonCP = 'E';
if (lon < 0)
dms.lonCP = 'W';
else
dms.lonCP = 'E';
double lonDeg = lon;
@@ -108,52 +120,64 @@ void GeoCoord::latLongToDMS(const double lat, const double lon, DMS &dms) {
* Converts lat long coordinates to UTM.
* based on this: https://github.com/walvok/LatLonToUTM/blob/master/latlon_utm.ino
*/
void GeoCoord::latLongToUTM(const double lat, const double lon, UTM &utm) {
void GeoCoord::latLongToUTM(const double lat, const double lon, UTM &utm)
{
const std::string latBands = "CDEFGHJKLMNPQRSTUVWXX";
utm.zone = int((lon + 180)/6 + 1);
utm.band = latBands[int(lat/8 + 10)];
double a = 6378137; // WGS84 - equatorial radius
double k0 = 0.9996; // UTM point scale on the central meridian
double eccSquared = 0.00669438; // eccentricity squared
double lonTemp = (lon + 180) - int((lon + 180)/360) * 360 - 180; //Make sure the longitude is between -180.00 .. 179.9
utm.zone = int((lon + 180) / 6 + 1);
utm.band = latBands[int(lat / 8 + 10)];
double a = 6378137; // WGS84 - equatorial radius
double k0 = 0.9996; // UTM point scale on the central meridian
double eccSquared = 0.00669438; // eccentricity squared
double lonTemp = (lon + 180) - int((lon + 180) / 360) * 360 - 180; // Make sure the longitude is between -180.00 .. 179.9
double latRad = toRadians(lat);
double lonRad = toRadians(lonTemp);
// Special Zones for Norway and Svalbard
if( lat >= 56.0 && lat < 64.0 && lonTemp >= 3.0 && lonTemp < 12.0 ) // Norway
if (lat >= 56.0 && lat < 64.0 && lonTemp >= 3.0 && lonTemp < 12.0) // Norway
utm.zone = 32;
if( lat >= 72.0 && lat < 84.0 ) { // Svalbard
if ( lonTemp >= 0.0 && lonTemp < 9.0 ) utm.zone = 31;
else if( lonTemp >= 9.0 && lonTemp < 21.0 ) utm.zone = 33;
else if( lonTemp >= 21.0 && lonTemp < 33.0 ) utm.zone = 35;
else if( lonTemp >= 33.0 && lonTemp < 42.0 ) utm.zone = 37;
if (lat >= 72.0 && lat < 84.0) { // Svalbard
if (lonTemp >= 0.0 && lonTemp < 9.0)
utm.zone = 31;
else if (lonTemp >= 9.0 && lonTemp < 21.0)
utm.zone = 33;
else if (lonTemp >= 21.0 && lonTemp < 33.0)
utm.zone = 35;
else if (lonTemp >= 33.0 && lonTemp < 42.0)
utm.zone = 37;
}
double lonOrigin = (utm.zone - 1)*6 - 180 + 3; // puts origin in middle of zone
double lonOrigin = (utm.zone - 1) * 6 - 180 + 3; // puts origin in middle of zone
double lonOriginRad = toRadians(lonOrigin);
double eccPrimeSquared = (eccSquared)/(1 - eccSquared);
double N = a/sqrt(1 - eccSquared*sin(latRad)*sin(latRad));
double T = tan(latRad)*tan(latRad);
double C = eccPrimeSquared*cos(latRad)*cos(latRad);
double A = cos(latRad)*(lonRad - lonOriginRad);
double M = a*((1 - eccSquared/4 - 3*eccSquared*eccSquared/64 - 5*eccSquared*eccSquared*eccSquared/256)*latRad
- (3*eccSquared/8 + 3*eccSquared*eccSquared/32 + 45*eccSquared*eccSquared*eccSquared/1024)*sin(2*latRad)
+ (15*eccSquared*eccSquared/256 + 45*eccSquared*eccSquared*eccSquared/1024)*sin(4*latRad)
- (35*eccSquared*eccSquared*eccSquared/3072)*sin(6*latRad));
utm.easting = (double)(k0*N*(A+(1-T+C)*pow(A, 3)/6 + (5-18*T+T*T+72*C-58*eccPrimeSquared)*A*A*A*A*A/120)
+ 500000.0);
utm.northing = (double)(k0*(M+N*tan(latRad)*(A*A/2+(5-T+9*C+4*C*C)*A*A*A*A/24
+ (61-58*T+T*T+600*C-330*eccPrimeSquared)*A*A*A*A*A*A/720)));
if(lat < 0)
utm.northing += 10000000.0; //10000000 meter offset for southern hemisphere
double eccPrimeSquared = (eccSquared) / (1 - eccSquared);
double N = a / sqrt(1 - eccSquared * sin(latRad) * sin(latRad));
double T = tan(latRad) * tan(latRad);
double C = eccPrimeSquared * cos(latRad) * cos(latRad);
double A = cos(latRad) * (lonRad - lonOriginRad);
double M =
a * ((1 - eccSquared / 4 - 3 * eccSquared * eccSquared / 64 - 5 * eccSquared * eccSquared * eccSquared / 256) * latRad -
(3 * eccSquared / 8 + 3 * eccSquared * eccSquared / 32 + 45 * eccSquared * eccSquared * eccSquared / 1024) *
sin(2 * latRad) +
(15 * eccSquared * eccSquared / 256 + 45 * eccSquared * eccSquared * eccSquared / 1024) * sin(4 * latRad) -
(35 * eccSquared * eccSquared * eccSquared / 3072) * sin(6 * latRad));
utm.easting = (double)(k0 * N *
(A + (1 - T + C) * pow(A, 3) / 6 +
(5 - 18 * T + T * T + 72 * C - 58 * eccPrimeSquared) * A * A * A * A * A / 120) +
500000.0);
utm.northing =
(double)(k0 * (M + N * tan(latRad) *
(A * A / 2 + (5 - T + 9 * C + 4 * C * C) * A * A * A * A / 24 +
(61 - 58 * T + T * T + 600 * C - 330 * eccPrimeSquared) * A * A * A * A * A * A / 720)));
if (lat < 0)
utm.northing += 10000000.0; // 10000000 meter offset for southern hemisphere
}
// Converts lat long coordinates to an MGRS.
void GeoCoord::latLongToMGRS(const double lat, const double lon, MGRS &mgrs) {
const std::string e100kLetters[3] = { "ABCDEFGH", "JKLMNPQR", "STUVWXYZ" };
const std::string n100kLetters[2] = { "ABCDEFGHJKLMNPQRSTUV", "FGHJKLMNPQRSTUVABCDE" };
void GeoCoord::latLongToMGRS(const double lat, const double lon, MGRS &mgrs)
{
const std::string e100kLetters[3] = {"ABCDEFGH", "JKLMNPQR", "STUVWXYZ"};
const std::string n100kLetters[2] = {"ABCDEFGHJKLMNPQRSTUV", "FGHJKLMNPQRSTUVABCDE"};
UTM utm;
latLongToUTM(lat, lon, utm);
mgrs.zone = utm.zone;
@@ -161,7 +185,7 @@ void GeoCoord::latLongToMGRS(const double lat, const double lon, MGRS &mgrs) {
double col = floor(utm.easting / 100000);
mgrs.east100k = e100kLetters[(mgrs.zone - 1) % 3][col - 1];
double row = (int32_t)floor(utm.northing / 100000.0) % 20;
mgrs.north100k = n100kLetters[(mgrs.zone-1)%2][row];
mgrs.north100k = n100kLetters[(mgrs.zone - 1) % 2][row];
mgrs.easting = (int32_t)utm.easting % 100000;
mgrs.northing = (int32_t)utm.northing % 100000;
}
@@ -170,52 +194,54 @@ void GeoCoord::latLongToMGRS(const double lat, const double lon, MGRS &mgrs) {
* Converts lat long coordinates to Ordnance Survey Grid Reference (UK National Grid Ref).
* Based on: https://www.movable-type.co.uk/scripts/latlong-os-gridref.html
*/
void GeoCoord::latLongToOSGR(const double lat, const double lon, OSGR &osgr) {
void GeoCoord::latLongToOSGR(const double lat, const double lon, OSGR &osgr)
{
const char letter[] = "ABCDEFGHJKLMNOPQRSTUVWXYZ"; // No 'I' in OSGR
double a = 6377563.396; // Airy 1830 semi-major axis
double b = 6356256.909; // Airy 1830 semi-minor axis
double f0 = 0.9996012717; // National Grid point scale factor on the central meridian
double phi0 = toRadians(49);
double a = 6377563.396; // Airy 1830 semi-major axis
double b = 6356256.909; // Airy 1830 semi-minor axis
double f0 = 0.9996012717; // National Grid point scale factor on the central meridian
double phi0 = toRadians(49);
double lambda0 = toRadians(-2);
double n0 = -100000;
double e0 = 400000;
double e2 = 1 - (b*b)/(a*a); // eccentricity squared
double n = (a - b)/(a + b);
double e2 = 1 - (b * b) / (a * a); // eccentricity squared
double n = (a - b) / (a + b);
double osgb_Latitude;
double osgb_Longitude;
convertWGS84ToOSGB36(lat, lon, osgb_Latitude, osgb_Longitude);
double phi = osgb_Latitude; // already in radians
double phi = osgb_Latitude; // already in radians
double lambda = osgb_Longitude; // already in radians
double v = a * f0 / sqrt(1 - e2 * sin(phi) * sin(phi));
double rho = a * f0 * (1 - e2) / pow(1 - e2 * sin(phi) * sin(phi), 1.5);
double eta2 = v / rho - 1;
double mA = (1 + n + (5/4)*n*n + (5/4)*n*n*n) * (phi - phi0);
double mB = (3*n + 3*n*n + (21/8)*n*n*n) * sin(phi - phi0) * cos(phi + phi0);
double mA = (1 + n + (5 / 4) * n * n + (5 / 4) * n * n * n) * (phi - phi0);
double mB = (3 * n + 3 * n * n + (21 / 8) * n * n * n) * sin(phi - phi0) * cos(phi + phi0);
// loss of precision in mC & mD due to floating point rounding can cause innaccuracy of northing by a few meters
double mC = (15/8*n*n + 15/8*n*n*n) * sin(2*(phi - phi0)) * cos(2*(phi + phi0));
double mD = (35/24)*n*n*n * sin(3*(phi - phi0)) * cos(3*(phi + phi0));
double m = b*f0*(mA - mB + mC - mD);
double mC = (15 / 8 * n * n + 15 / 8 * n * n * n) * sin(2 * (phi - phi0)) * cos(2 * (phi + phi0));
double mD = (35 / 24) * n * n * n * sin(3 * (phi - phi0)) * cos(3 * (phi + phi0));
double m = b * f0 * (mA - mB + mC - mD);
double cos3Phi = cos(phi)*cos(phi)*cos(phi);
double cos5Phi = cos3Phi*cos(phi)*cos(phi);
double tan2Phi = tan(phi)*tan(phi);
double tan4Phi = tan2Phi*tan2Phi;
double cos3Phi = cos(phi) * cos(phi) * cos(phi);
double cos5Phi = cos3Phi * cos(phi) * cos(phi);
double tan2Phi = tan(phi) * tan(phi);
double tan4Phi = tan2Phi * tan2Phi;
double I = m + n0;
double II = (v/2)*sin(phi)*cos(phi);
double III = (v/24)*sin(phi)*cos3Phi*(5 - tan2Phi + 9*eta2);
double IIIA = (v/720)*sin(phi)*cos5Phi*(61 - 58*tan2Phi + tan4Phi);
double IV = v*cos(phi);
double V = (v/6)*cos3Phi*(v/rho - tan2Phi);
double VI = (v/120)*cos5Phi*(5 - 18*tan2Phi + tan4Phi + 14*eta2 - 58*tan2Phi*eta2);
double II = (v / 2) * sin(phi) * cos(phi);
double III = (v / 24) * sin(phi) * cos3Phi * (5 - tan2Phi + 9 * eta2);
double IIIA = (v / 720) * sin(phi) * cos5Phi * (61 - 58 * tan2Phi + tan4Phi);
double IV = v * cos(phi);
double V = (v / 6) * cos3Phi * (v / rho - tan2Phi);
double VI = (v / 120) * cos5Phi * (5 - 18 * tan2Phi + tan4Phi + 14 * eta2 - 58 * tan2Phi * eta2);
double deltaLambda = lambda - lambda0;
double deltaLambda2 = deltaLambda*deltaLambda;
double northing = I + II*deltaLambda2 + III*deltaLambda2*deltaLambda2 + IIIA*deltaLambda2*deltaLambda2*deltaLambda2;
double easting = e0 + IV*deltaLambda + V*deltaLambda2*deltaLambda + VI*deltaLambda2*deltaLambda2*deltaLambda;
double deltaLambda2 = deltaLambda * deltaLambda;
double northing =
I + II * deltaLambda2 + III * deltaLambda2 * deltaLambda2 + IIIA * deltaLambda2 * deltaLambda2 * deltaLambda2;
double easting = e0 + IV * deltaLambda + V * deltaLambda2 * deltaLambda + VI * deltaLambda2 * deltaLambda2 * deltaLambda;
if (easting < 0 || easting > 700000 || northing < 0 || northing > 1300000) // Check if out of boundaries
osgr = { 'I', 'I', 0, 0 };
osgr = {'I', 'I', 0, 0};
else {
uint32_t e100k = floor(easting / 100000);
uint32_t n100k = floor(northing / 100000);
@@ -232,7 +258,8 @@ void GeoCoord::latLongToOSGR(const double lat, const double lon, OSGR &osgr) {
* Converts lat long coordinates to Open Location Code.
* Based on: https://github.com/google/open-location-code/blob/main/c/src/olc.c
*/
void GeoCoord::latLongToOLC(double lat, double lon, OLC &olc) {
void GeoCoord::latLongToOLC(double lat, double lon, OLC &olc)
{
char tempCode[] = "1234567890abc";
const char kAlphabet[] = "23456789CFGHJMPQRVWX";
double latitude;
@@ -258,7 +285,7 @@ void GeoCoord::latLongToOLC(double lat, double lon, OLC &olc) {
lat_val += latitude * 2.5e7;
lng_val += longitude * 8.192e6;
size_t pos = OLC_CODE_LEN;
if (OLC_CODE_LEN > 10) { // Compute grid part of code if needed
for (size_t i = 0; i < 5; i++) {
int lat_digit = lat_val % 5;
@@ -272,9 +299,9 @@ void GeoCoord::latLongToOLC(double lat, double lon, OLC &olc) {
lat_val /= pow(5, 5);
lng_val /= pow(4, 5);
}
pos = 10;
for (size_t i = 0; i < 5; i++) { // Compute pair section of code
int lat_ndx = lat_val % 20;
int lng_ndx = lng_val % 20;
@@ -286,7 +313,7 @@ void GeoCoord::latLongToOLC(double lat, double lon, OLC &olc) {
if (i == 0)
tempCode[pos--] = '+';
}
if (OLC_CODE_LEN < 9) { // Add padding if needed
for (size_t i = OLC_CODE_LEN; i < 9; i++)
tempCode[i] = '0';
@@ -300,50 +327,52 @@ void GeoCoord::latLongToOLC(double lat, double lon, OLC &olc) {
for (size_t i = 0; i < char_count; i++) {
olc.code[i] = tempCode[i];
}
olc.code[char_count] = '\0';
olc.code[char_count] = '\0';
}
// Converts the coordinate in WGS84 datum to the OSGB36 datum.
void GeoCoord::convertWGS84ToOSGB36(const double lat, const double lon, double &osgb_Latitude, double &osgb_Longitude) {
void GeoCoord::convertWGS84ToOSGB36(const double lat, const double lon, double &osgb_Latitude, double &osgb_Longitude)
{
// Convert lat long to cartesian
double phi = toRadians(lat);
double lambda = toRadians(lon);
double h = 0.0; // No OSTN height data used, some loss of accuracy (up to 5m)
double wgsA = 6378137; // WGS84 datum semi major axis
double h = 0.0; // No OSTN height data used, some loss of accuracy (up to 5m)
double wgsA = 6378137; // WGS84 datum semi major axis
double wgsF = 1 / 298.257223563; // WGS84 datum flattening
double ecc = 2*wgsF - wgsF*wgsF;
double ecc = 2 * wgsF - wgsF * wgsF;
double vee = wgsA / sqrt(1 - ecc * pow(sin(phi), 2));
double wgsX = (vee + h) * cos(phi) * cos(lambda);
double wgsY = (vee + h) * cos(phi) * sin(lambda);
double wgsZ = ((1 - ecc) * vee + h) * sin(phi);
// 7-parameter Helmert transform
double tx = -446.448; // x shift in meters
double ty = 125.157; // y shift in meters
double tz = -542.060; // z shift in meters
double s = 20.4894/1e6 + 1; // scale normalized parts per million to (s + 1)
double rx = toRadians(-0.1502/3600); // x rotation normalize arcseconds to radians
double ry = toRadians(-0.2470/3600); // y rotation normalize arcseconds to radians
double rz = toRadians(-0.8421/3600); // z rotation normalize arcseconds to radians
double osgbX = tx + wgsX*s - wgsY*rz + wgsZ*ry;
double osgbY = ty + wgsX*rz + wgsY*s - wgsZ*rx;
double osgbZ = tz - wgsX*ry + wgsY*rx + wgsZ*s;
double tx = -446.448; // x shift in meters
double ty = 125.157; // y shift in meters
double tz = -542.060; // z shift in meters
double s = 20.4894 / 1e6 + 1; // scale normalized parts per million to (s + 1)
double rx = toRadians(-0.1502 / 3600); // x rotation normalize arcseconds to radians
double ry = toRadians(-0.2470 / 3600); // y rotation normalize arcseconds to radians
double rz = toRadians(-0.8421 / 3600); // z rotation normalize arcseconds to radians
double osgbX = tx + wgsX * s - wgsY * rz + wgsZ * ry;
double osgbY = ty + wgsX * rz + wgsY * s - wgsZ * rx;
double osgbZ = tz - wgsX * ry + wgsY * rx + wgsZ * s;
// Convert cartesian to lat long
double airyA = 6377563.396; // Airy1830 datum semi major axis
double airyB = 6356256.909; // Airy1830 datum semi minor axis
double airyF = 1/ 299.3249646; // Airy1830 datum flattening
double airyEcc = 2*airyF - airyF*airyF;
double airyA = 6377563.396; // Airy1830 datum semi major axis
double airyB = 6356256.909; // Airy1830 datum semi minor axis
double airyF = 1 / 299.3249646; // Airy1830 datum flattening
double airyEcc = 2 * airyF - airyF * airyF;
double airyEcc2 = airyEcc / (1 - airyEcc);
double p = sqrt(osgbX*osgbX + osgbY*osgbY);
double R = sqrt(p*p + osgbZ*osgbZ);
double tanBeta = (airyB*osgbZ) / (airyA*p) * (1 + airyEcc2*airyB/R);
double sinBeta = tanBeta / sqrt(1 + tanBeta*tanBeta);
double p = sqrt(osgbX * osgbX + osgbY * osgbY);
double R = sqrt(p * p + osgbZ * osgbZ);
double tanBeta = (airyB * osgbZ) / (airyA * p) * (1 + airyEcc2 * airyB / R);
double sinBeta = tanBeta / sqrt(1 + tanBeta * tanBeta);
double cosBeta = sinBeta / tanBeta;
osgb_Latitude = atan2(osgbZ + airyEcc2*airyB*sinBeta*sinBeta*sinBeta, p - airyEcc*airyA*cosBeta*cosBeta*cosBeta); // leave in radians
osgb_Longitude = atan2(osgbY, osgbX); // leave in radians
//osgb height = p*cos(osgb.latitude) + osgbZ*sin(osgb.latitude) -
//(airyA*airyA/(airyA / sqrt(1 - airyEcc*sin(osgb.latitude)*sin(osgb.latitude)))); // Not used, no OSTN data
osgb_Latitude = atan2(osgbZ + airyEcc2 * airyB * sinBeta * sinBeta * sinBeta,
p - airyEcc * airyA * cosBeta * cosBeta * cosBeta); // leave in radians
osgb_Longitude = atan2(osgbY, osgbX); // leave in radians
// osgb height = p*cos(osgb.latitude) + osgbZ*sin(osgb.latitude) -
//(airyA*airyA/(airyA / sqrt(1 - airyEcc*sin(osgb.latitude)*sin(osgb.latitude)))); // Not used, no OSTN data
}
/// Ported from my old java code, returns distance in meters along the globe
@@ -397,12 +426,13 @@ float GeoCoord::bearing(double lat1, double lon1, double lat2, double lon2)
* @brief Convert from meters to range in radians on a great circle
* @param range_meters
* The range in meters
* @return range in radians on a great circle
* @return range in radians on a great circle
*/
float GeoCoord::rangeMetersToRadians(double range_meters) {
float GeoCoord::rangeMetersToRadians(double range_meters)
{
// 1 nm is 1852 meters
double distance_nm = range_meters * 1852;
return (PI / (180 * 60)) *distance_nm;
return (PI / (180 * 60)) * distance_nm;
}
/**
@@ -410,22 +440,27 @@ float GeoCoord::rangeMetersToRadians(double range_meters) {
* @brief Convert from radians to range in meters on a great circle
* @param range_radians
* The range in radians
* @return Range in meters on a great circle
* @return Range in meters on a great circle
*/
float GeoCoord::rangeRadiansToMeters(double range_radians) {
float GeoCoord::rangeRadiansToMeters(double range_radians)
{
double distance_nm = ((180 * 60) / PI) * range_radians;
// 1 meter is 0.000539957 nm
return distance_nm * 0.000539957;
}
// Find distance from point to passed in point
int32_t GeoCoord::distanceTo(const GeoCoord& pointB) {
return latLongToMeter(this->getLatitude() * 1e-7, this->getLongitude() * 1e-7, pointB.getLatitude() * 1e-7, pointB.getLongitude() * 1e-7);
int32_t GeoCoord::distanceTo(const GeoCoord &pointB)
{
return latLongToMeter(this->getLatitude() * 1e-7, this->getLongitude() * 1e-7, pointB.getLatitude() * 1e-7,
pointB.getLongitude() * 1e-7);
}
// Find bearing from point to passed in point
int32_t GeoCoord::bearingTo(const GeoCoord& pointB) {
return bearing(this->getLatitude() * 1e-7, this->getLongitude() * 1e-7, pointB.getLatitude() * 1e-7, pointB.getLongitude() * 1e-7);
int32_t GeoCoord::bearingTo(const GeoCoord &pointB)
{
return bearing(this->getLatitude() * 1e-7, this->getLongitude() * 1e-7, pointB.getLatitude() * 1e-7,
pointB.getLongitude() * 1e-7);
}
/**
@@ -436,8 +471,9 @@ int32_t GeoCoord::bearingTo(const GeoCoord& pointB) {
* @param range_meters
* range in meters
* @return GeoCoord object of point at bearing and range from initial point
*/
std::shared_ptr<GeoCoord> GeoCoord::pointAtDistance(double bearing, double range_meters) {
*/
std::shared_ptr<GeoCoord> GeoCoord::pointAtDistance(double bearing, double range_meters)
{
double range_radians = rangeMetersToRadians(range_meters);
double lat1 = this->getLatitude() * 1e-7;
double lon1 = this->getLongitude() * 1e-7;
@@ -446,5 +482,4 @@ std::shared_ptr<GeoCoord> GeoCoord::pointAtDistance(double bearing, double range
double lon = fmod(lon1 - dlon + PI, 2 * PI) - PI;
return std::make_shared<GeoCoord>(double(lat), double(lon), this->getAltitude());
}