mirror of
https://github.com/meshtastic/firmware.git
synced 2025-04-26 09:59:01 +00:00
Merge branch 'master' into master
This commit is contained in:
Kevin Hester
GitHub
commit
31e833ec59
450
src/gps/GeoCoord.cpp
Normal file
450
src/gps/GeoCoord.cpp
Normal file
@ -0,0 +1,450 @@
|
||||
#include "GeoCoord.h"
|
||||
|
||||
GeoCoord::GeoCoord() {
|
||||
_dirty = true;
|
||||
}
|
||||
|
||||
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) {
|
||||
// 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) {
|
||||
// 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();
|
||||
}
|
||||
|
||||
// Initialize all the coordinate systems
|
||||
void GeoCoord::setCoords() {
|
||||
double lat = _latitude * 1e-7;
|
||||
double lon = _longitude * 1e-7;
|
||||
GeoCoord::latLongToDMS(lat, lon, _dms);
|
||||
GeoCoord::latLongToUTM(lat, lon, _utm);
|
||||
GeoCoord::latLongToMGRS(lat, lon, _mgrs);
|
||||
GeoCoord::latLongToOSGR(lat, lon, _osgr);
|
||||
GeoCoord::latLongToOLC(lat, lon, _olc);
|
||||
_dirty = false;
|
||||
}
|
||||
|
||||
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) {
|
||||
_dirty = true;
|
||||
_latitude = lat;
|
||||
_longitude = lon;
|
||||
_altitude = alt;
|
||||
setCoords();
|
||||
}
|
||||
}
|
||||
|
||||
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) {
|
||||
_dirty = true;
|
||||
_latitude = iLat;
|
||||
_longitude = iLon;
|
||||
_altitude = alt;
|
||||
setCoords();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
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) {
|
||||
_dirty = true;
|
||||
_latitude = iLat;
|
||||
_longitude = iLon;
|
||||
_altitude = alt;
|
||||
setCoords();
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* 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';
|
||||
|
||||
double latDeg = lat;
|
||||
|
||||
if (lat < 0)
|
||||
latDeg = latDeg * -1;
|
||||
|
||||
dms.latDeg = floor(latDeg);
|
||||
double latMin = (latDeg - dms.latDeg) * 60;
|
||||
dms.latMin = floor(latMin);
|
||||
dms.latSec = (latMin - dms.latMin) * 60;
|
||||
|
||||
if (lon < 0) dms.lonCP = 'W';
|
||||
else dms.lonCP = 'E';
|
||||
|
||||
double lonDeg = lon;
|
||||
|
||||
if (lon < 0)
|
||||
lonDeg = lonDeg * -1;
|
||||
|
||||
dms.lonDeg = floor(lonDeg);
|
||||
double lonMin = (lonDeg - dms.lonDeg) * 60;
|
||||
dms.lonMin = floor(lonMin);
|
||||
dms.lonSec = (lonMin - dms.lonMin) * 60;
|
||||
}
|
||||
|
||||
/**
|
||||
* 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) {
|
||||
|
||||
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
|
||||
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
|
||||
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;
|
||||
}
|
||||
|
||||
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
|
||||
}
|
||||
|
||||
// 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" };
|
||||
UTM utm;
|
||||
latLongToUTM(lat, lon, utm);
|
||||
mgrs.zone = utm.zone;
|
||||
mgrs.band = utm.band;
|
||||
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.easting = (int32_t)utm.easting % 100000;
|
||||
mgrs.northing = (int32_t)utm.northing % 100000;
|
||||
}
|
||||
|
||||
/**
|
||||
* 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) {
|
||||
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 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 osgb_Latitude;
|
||||
double osgb_Longitude;
|
||||
convertWGS84ToOSGB36(lat, lon, osgb_Latitude, osgb_Longitude);
|
||||
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);
|
||||
// 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 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 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;
|
||||
|
||||
if (easting < 0 || easting > 700000 || northing < 0 || northing > 1300000) // Check if out of boundaries
|
||||
osgr = { 'I', 'I', 0, 0 };
|
||||
else {
|
||||
uint32_t e100k = floor(easting / 100000);
|
||||
uint32_t n100k = floor(northing / 100000);
|
||||
int8_t l1 = (19 - n100k) - (19 - n100k) % 5 + floor((e100k + 10) / 5);
|
||||
int8_t l2 = (19 - n100k) * 5 % 25 + e100k % 5;
|
||||
osgr.e100k = letter[l1];
|
||||
osgr.n100k = letter[l2];
|
||||
osgr.easting = floor((int)easting % 100000);
|
||||
osgr.northing = floor((int)northing % 100000);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* 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) {
|
||||
char tempCode[] = "1234567890abc";
|
||||
const char kAlphabet[] = "23456789CFGHJMPQRVWX";
|
||||
double latitude;
|
||||
double longitude = lon;
|
||||
double latitude_degrees = std::min(90.0, std::max(-90.0, lat));
|
||||
|
||||
if (latitude_degrees < 90) // Check latitude less than lat max
|
||||
latitude = latitude_degrees;
|
||||
else {
|
||||
double precision;
|
||||
if (OLC_CODE_LEN <= 10)
|
||||
precision = pow_neg(20, floor((OLC_CODE_LEN / -2) + 2));
|
||||
else
|
||||
precision = pow_neg(20, -3) / pow(5, OLC_CODE_LEN - 10);
|
||||
latitude = latitude_degrees - precision / 2;
|
||||
}
|
||||
while (longitude < -180) // Normalize longitude
|
||||
longitude += 360;
|
||||
while (longitude >= 180)
|
||||
longitude -= 360;
|
||||
int64_t lat_val = 90 * 2.5e7;
|
||||
int64_t lng_val = 180 * 8.192e6;
|
||||
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;
|
||||
int lng_digit = lng_val % 4;
|
||||
int ndx = lat_digit * 4 + lng_digit;
|
||||
tempCode[pos--] = kAlphabet[ndx];
|
||||
lat_val /= 5;
|
||||
lng_val /= 4;
|
||||
}
|
||||
} else {
|
||||
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;
|
||||
tempCode[pos--] = kAlphabet[lng_ndx];
|
||||
tempCode[pos--] = kAlphabet[lat_ndx];
|
||||
lat_val /= 20;
|
||||
lng_val /= 20;
|
||||
|
||||
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';
|
||||
tempCode[9] = '+';
|
||||
}
|
||||
|
||||
size_t char_count = OLC_CODE_LEN;
|
||||
if (10 > char_count) {
|
||||
char_count = 10;
|
||||
}
|
||||
for (size_t i = 0; i < char_count; i++) {
|
||||
olc.code[i] = tempCode[i];
|
||||
}
|
||||
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) {
|
||||
// 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 wgsF = 1 / 298.257223563; // WGS84 datum flattening
|
||||
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;
|
||||
|
||||
// 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 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 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
|
||||
}
|
||||
|
||||
/// Ported from my old java code, returns distance in meters along the globe
|
||||
/// surface (by magic?)
|
||||
float GeoCoord::latLongToMeter(double lat_a, double lng_a, double lat_b, double lng_b)
|
||||
{
|
||||
double pk = (180 / 3.14169);
|
||||
double a1 = lat_a / pk;
|
||||
double a2 = lng_a / pk;
|
||||
double b1 = lat_b / pk;
|
||||
double b2 = lng_b / pk;
|
||||
double cos_b1 = cos(b1);
|
||||
double cos_a1 = cos(a1);
|
||||
double t1 = cos_a1 * cos(a2) * cos_b1 * cos(b2);
|
||||
double t2 = cos_a1 * sin(a2) * cos_b1 * sin(b2);
|
||||
double t3 = sin(a1) * sin(b1);
|
||||
double tt = acos(t1 + t2 + t3);
|
||||
if (std::isnan(tt))
|
||||
tt = 0.0; // Must have been the same point?
|
||||
|
||||
return (float)(6366000 * tt);
|
||||
}
|
||||
|
||||
/**
|
||||
* Computes the bearing in degrees between two points on Earth. Ported from my
|
||||
* old Gaggle android app.
|
||||
*
|
||||
* @param lat1
|
||||
* Latitude of the first point
|
||||
* @param lon1
|
||||
* Longitude of the first point
|
||||
* @param lat2
|
||||
* Latitude of the second point
|
||||
* @param lon2
|
||||
* Longitude of the second point
|
||||
* @return Bearing between the two points in radians. A value of 0 means due
|
||||
* north.
|
||||
*/
|
||||
float GeoCoord::bearing(double lat1, double lon1, double lat2, double lon2)
|
||||
{
|
||||
double lat1Rad = toRadians(lat1);
|
||||
double lat2Rad = toRadians(lat2);
|
||||
double deltaLonRad = toRadians(lon2 - lon1);
|
||||
double y = sin(deltaLonRad) * cos(lat2Rad);
|
||||
double x = cos(lat1Rad) * sin(lat2Rad) - (sin(lat1Rad) * cos(lat2Rad) * cos(deltaLonRad));
|
||||
return atan2(y, x);
|
||||
}
|
||||
|
||||
/**
|
||||
* Ported from http://www.edwilliams.org/avform147.htm#Intro
|
||||
* @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
|
||||
*/
|
||||
float GeoCoord::rangeMetersToRadians(double range_meters) {
|
||||
// 1 nm is 1852 meters
|
||||
double distance_nm = range_meters * 1852;
|
||||
return (PI / (180 * 60)) *distance_nm;
|
||||
}
|
||||
|
||||
/**
|
||||
* Ported from http://www.edwilliams.org/avform147.htm#Intro
|
||||
* @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
|
||||
*/
|
||||
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(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(GeoCoord pointB) {
|
||||
return bearing(this->getLatitude() * 1e-7, this->getLongitude() * 1e-7, pointB.getLatitude() * 1e-7, pointB.getLongitude() * 1e-7);
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a new point bassed on the passed in poin
|
||||
* Ported from http://www.edwilliams.org/avform147.htm#LL
|
||||
* @param bearing
|
||||
* The bearing in raidans
|
||||
* @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) {
|
||||
double range_radians = rangeMetersToRadians(range_meters);
|
||||
double lat1 = this->getLatitude() * 1e-7;
|
||||
double lon1 = this->getLongitude() * 1e-7;
|
||||
double lat = asin(sin(lat1) * cos(range_radians) + cos(lat1) * sin(range_radians) * cos(bearing));
|
||||
double dlon = atan2(sin(bearing) * sin(range_radians) * cos(lat1), cos(range_radians) - sin(lat1) * sin(lat));
|
||||
double lon = fmod(lon1 - dlon + PI, 2 * PI) - PI;
|
||||
|
||||
return std::make_shared<GeoCoord>(double(lat), double(lon), this->getAltitude());
|
||||
|
||||
}
|
||||
164
src/gps/GeoCoord.h
Normal file
164
src/gps/GeoCoord.h
Normal file
@ -0,0 +1,164 @@
|
||||
#pragma once
|
||||
|
||||
#include <algorithm>
|
||||
#include <string>
|
||||
#include <cstring>
|
||||
#include <cstdint>
|
||||
#include <math.h>
|
||||
#include <stdint.h>
|
||||
#include <stdexcept>
|
||||
#include <memory>
|
||||
|
||||
#define PI 3.1415926535897932384626433832795
|
||||
#define OLC_CODE_LEN 11
|
||||
|
||||
// Helper functions
|
||||
// Raises a number to an exponent, handling negative exponents.
|
||||
static double pow_neg(double base, double exponent) {
|
||||
if (exponent == 0) {
|
||||
return 1;
|
||||
} else if (exponent > 0) {
|
||||
return pow(base, exponent);
|
||||
}
|
||||
return 1 / pow(base, -exponent);
|
||||
}
|
||||
|
||||
static inline double toRadians(double deg)
|
||||
{
|
||||
return deg * PI / 180;
|
||||
}
|
||||
|
||||
static inline double toDegrees(double r)
|
||||
{
|
||||
return r * 180 / PI;
|
||||
}
|
||||
|
||||
// GeoCoord structs/classes
|
||||
// A struct to hold the data for a DMS coordinate.
|
||||
struct DMS
|
||||
{
|
||||
uint8_t latDeg;
|
||||
uint8_t latMin;
|
||||
uint32_t latSec;
|
||||
char latCP;
|
||||
uint8_t lonDeg;
|
||||
uint8_t lonMin;
|
||||
uint32_t lonSec;
|
||||
char lonCP;
|
||||
};
|
||||
|
||||
// A struct to hold the data for a UTM coordinate, this is also used when creating an MGRS coordinate.
|
||||
struct UTM
|
||||
{
|
||||
uint8_t zone;
|
||||
char band;
|
||||
uint32_t easting;
|
||||
uint32_t northing;
|
||||
};
|
||||
|
||||
// A struct to hold the data for a MGRS coordinate.
|
||||
struct MGRS
|
||||
{
|
||||
uint8_t zone;
|
||||
char band;
|
||||
char east100k;
|
||||
char north100k;
|
||||
uint32_t easting;
|
||||
uint32_t northing;
|
||||
};
|
||||
|
||||
// A struct to hold the data for a OSGR coordiante
|
||||
struct OSGR {
|
||||
char e100k;
|
||||
char n100k;
|
||||
uint32_t easting;
|
||||
uint32_t northing;
|
||||
};
|
||||
|
||||
// A struct to hold the data for a OLC coordinate
|
||||
struct OLC {
|
||||
char code[OLC_CODE_LEN + 1]; // +1 for null termination
|
||||
};
|
||||
|
||||
class GeoCoord {
|
||||
private:
|
||||
int32_t _latitude = 0;
|
||||
int32_t _longitude = 0;
|
||||
int32_t _altitude = 0;
|
||||
|
||||
DMS _dms;
|
||||
UTM _utm;
|
||||
MGRS _mgrs;
|
||||
OSGR _osgr;
|
||||
OLC _olc;
|
||||
|
||||
bool _dirty = true;
|
||||
|
||||
void setCoords();
|
||||
|
||||
public:
|
||||
GeoCoord();
|
||||
GeoCoord(int32_t lat, int32_t lon, int32_t alt);
|
||||
GeoCoord(double lat, double lon, int32_t alt);
|
||||
GeoCoord(float lat, float lon, int32_t alt);
|
||||
|
||||
void updateCoords(const int32_t lat, const int32_t lon, const int32_t alt);
|
||||
void updateCoords(const double lat, const double lon, const int32_t alt);
|
||||
void updateCoords(const float lat, const float lon, const int32_t alt);
|
||||
|
||||
// Conversions
|
||||
static void latLongToDMS(const double lat, const double lon, DMS &dms);
|
||||
static void latLongToUTM(const double lat, const double lon, UTM &utm);
|
||||
static void latLongToMGRS(const double lat, const double lon, MGRS &mgrs);
|
||||
static void latLongToOSGR(const double lat, const double lon, OSGR &osgr);
|
||||
static void latLongToOLC(const double lat, const double lon, OLC &olc);
|
||||
static void convertWGS84ToOSGB36(const double lat, const double lon, double &osgb_Latitude, double &osgb_Longitude);
|
||||
static float latLongToMeter(double lat_a, double lng_a, double lat_b, double lng_b);
|
||||
static float bearing(double lat1, double lon1, double lat2, double lon2);
|
||||
static float rangeRadiansToMeters(double range_radians);
|
||||
static float rangeMetersToRadians(double range_meters);
|
||||
|
||||
// Point to point conversions
|
||||
int32_t distanceTo(GeoCoord pointB);
|
||||
int32_t bearingTo(GeoCoord pointB);
|
||||
std::shared_ptr<GeoCoord> pointAtDistance(double bearing, double range);
|
||||
|
||||
// Lat lon alt getters
|
||||
int32_t getLatitude() const { return _latitude; }
|
||||
int32_t getLongitude() const { return _longitude; }
|
||||
int32_t getAltitude() const { return _altitude; }
|
||||
|
||||
// DMS getters
|
||||
uint8_t getDMSLatDeg() const { return _dms.latDeg; }
|
||||
uint8_t getDMSLatMin() const { return _dms.latMin; }
|
||||
uint32_t getDMSLatSec() const { return _dms.latSec; }
|
||||
char getDMSLatCP() const { return _dms.latCP; }
|
||||
uint8_t getDMSLonDeg() const { return _dms.lonDeg; }
|
||||
uint8_t getDMSLonMin() const { return _dms.lonMin; }
|
||||
uint32_t getDMSLonSec() const { return _dms.lonSec; }
|
||||
char getDMSLonCP() const { return _dms.lonCP; }
|
||||
|
||||
// UTM getters
|
||||
uint8_t getUTMZone() const { return _utm.zone; }
|
||||
char getUTMBand() const { return _utm.band; }
|
||||
uint32_t getUTMEasting() const { return _utm.easting; }
|
||||
uint32_t getUTMNorthing() const { return _utm.northing; }
|
||||
|
||||
// MGRS getters
|
||||
uint8_t getMGRSZone() const { return _mgrs.zone; }
|
||||
char getMGRSBand() const { return _mgrs.band; }
|
||||
char getMGRSEast100k() const { return _mgrs.east100k; }
|
||||
char getMGRSNorth100k() const { return _mgrs.north100k; }
|
||||
uint32_t getMGRSEasting() const { return _mgrs.easting; }
|
||||
uint32_t getMGRSNorthing() const { return _mgrs.northing; }
|
||||
|
||||
// OSGR getters
|
||||
char getOSGRE100k() const { return _osgr.e100k; }
|
||||
char getOSGRN100k() const { return _osgr.n100k; }
|
||||
uint32_t getOSGREasting() const { return _osgr.easting; }
|
||||
uint32_t getOSGRNorthing() const { return _osgr.northing; }
|
||||
|
||||
// OLC getter
|
||||
void getOLCCode(char* code) { strncpy(code, _olc.code, OLC_CODE_LEN + 1); } // +1 for null termination
|
||||
};
|
||||
|
||||
@ -35,6 +35,7 @@ along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
#include "plugins/TextMessagePlugin.h"
|
||||
#include "target_specific.h"
|
||||
#include "utils.h"
|
||||
#include "gps/GeoCoord.h"
|
||||
|
||||
#ifndef NO_ESP32
|
||||
#include "mesh/http/WiFiAPClient.h"
|
||||
@ -72,6 +73,9 @@ std::vector<MeshPlugin *> pluginFrames;
|
||||
// Stores the last 4 of our hardware ID, to make finding the device for pairing easier
|
||||
static char ourId[5];
|
||||
|
||||
// GeoCoord object for the screen
|
||||
GeoCoord geoCoord;
|
||||
|
||||
#ifdef SHOW_REDRAWS
|
||||
static bool heartbeat = false;
|
||||
#endif
|
||||
@ -379,369 +383,12 @@ static void drawGPSAltitude(OLEDDisplay *display, int16_t x, int16_t y, const GP
|
||||
// displayLine = "No GPS Lock";
|
||||
// display->drawString(x + (SCREEN_WIDTH - (display->getStringWidth(displayLine))) / 2, y, displayLine);
|
||||
} else {
|
||||
|
||||
displayLine = "Altitude: " + String(gps->getAltitude()) + "m";
|
||||
geoCoord.updateCoords(int32_t(gps->getLatitude()), int32_t(gps->getLongitude()), int32_t(gps->getAltitude()));
|
||||
displayLine = "Altitude: " + String(geoCoord.getAltitude()) + "m";
|
||||
display->drawString(x + (SCREEN_WIDTH - (display->getStringWidth(displayLine))) / 2, y, displayLine);
|
||||
}
|
||||
}
|
||||
|
||||
static inline double toRadians(double deg)
|
||||
{
|
||||
return deg * PI / 180;
|
||||
}
|
||||
|
||||
static inline double toDegrees(double r)
|
||||
{
|
||||
return r * 180 / PI;
|
||||
}
|
||||
|
||||
// A struct to hold the data for a DMS coordinate.
|
||||
struct DMS
|
||||
{
|
||||
byte latDeg;
|
||||
byte latMin;
|
||||
double latSec;
|
||||
char latCP;
|
||||
byte lonDeg;
|
||||
byte lonMin;
|
||||
double lonSec;
|
||||
char lonCP;
|
||||
};
|
||||
|
||||
// A struct to hold the data for a UTM coordinate, this is also used when creating an MGRS coordinate.
|
||||
struct UTM
|
||||
{
|
||||
byte zone;
|
||||
char band;
|
||||
double easting;
|
||||
double northing;
|
||||
};
|
||||
|
||||
// A struct to hold the data for a MGRS coordinate.
|
||||
struct MGRS
|
||||
{
|
||||
byte zone;
|
||||
char band;
|
||||
char east100k;
|
||||
char north100k;
|
||||
uint32_t easting;
|
||||
uint32_t northing;
|
||||
};
|
||||
|
||||
/**
|
||||
* Converts lat long coordinates to UTM.
|
||||
* based on this: https://github.com/walvok/LatLonToUTM/blob/master/latlon_utm.ino
|
||||
*/
|
||||
static struct UTM latLongToUTM(const double lat, const double lon)
|
||||
{
|
||||
const String latBands = "CDEFGHJKLMNPQRSTUVWXX";
|
||||
UTM utm;
|
||||
utm.zone = int((lon + 180)/6 + 1);
|
||||
utm.band = latBands.charAt(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
|
||||
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;
|
||||
}
|
||||
|
||||
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
|
||||
|
||||
return utm;
|
||||
}
|
||||
|
||||
// Converts lat long coordinates to an MGRS.
|
||||
static struct MGRS latLongToMGRS(double lat, double lon)
|
||||
{
|
||||
const String e100kLetters[3] = { "ABCDEFGH", "JKLMNPQR", "STUVWXYZ" };
|
||||
const String n100kLetters[2] = { "ABCDEFGHJKLMNPQRSTUV", "FGHJKLMNPQRSTUVABCDE" };
|
||||
UTM utm = latLongToUTM(lat, lon);
|
||||
MGRS mgrs;
|
||||
mgrs.zone = utm.zone;
|
||||
mgrs.band = utm.band;
|
||||
double col = floor(utm.easting / 100000);
|
||||
mgrs.east100k = e100kLetters[(mgrs.zone - 1) % 3].charAt(col - 1);
|
||||
double row = (int)floor(utm.northing / 100000.0) % 20;
|
||||
mgrs.north100k = n100kLetters[(mgrs.zone-1)%2].charAt(row);
|
||||
mgrs.easting = (int)utm.easting % 100000;
|
||||
mgrs.northing = (int)utm.northing % 100000;
|
||||
return mgrs;
|
||||
}
|
||||
|
||||
/**
|
||||
* Converts lat long coordinates from decimal degrees to degrees minutes seconds format.
|
||||
* DD°MM'SS"C DDD°MM'SS"C
|
||||
*/
|
||||
static struct DMS latLongToDMS(double lat, double lon)
|
||||
{
|
||||
DMS dms;
|
||||
|
||||
if (lat < 0) dms.latCP = 'S';
|
||||
else dms.latCP = 'N';
|
||||
|
||||
double latDeg = lat;
|
||||
|
||||
if (lat < 0)
|
||||
latDeg = latDeg * -1;
|
||||
|
||||
dms.latDeg = floor(latDeg);
|
||||
double latMin = (latDeg - dms.latDeg) * 60;
|
||||
dms.latMin = floor(latMin);
|
||||
dms.latSec = (latMin - dms.latMin) * 60;
|
||||
|
||||
if (lon < 0) dms.lonCP = 'W';
|
||||
else dms.lonCP = 'E';
|
||||
|
||||
double lonDeg = lon;
|
||||
|
||||
if (lon < 0)
|
||||
lonDeg = lonDeg * -1;
|
||||
|
||||
dms.lonDeg = floor(lonDeg);
|
||||
double lonMin = (lonDeg - dms.lonDeg) * 60;
|
||||
dms.lonMin = floor(lonMin);
|
||||
dms.lonSec = (lonMin - dms.lonMin) * 60;
|
||||
|
||||
return dms;
|
||||
}
|
||||
|
||||
// Raises a number to an exponent, handling negative exponents.
|
||||
static double pow_neg(double base, double exponent) {
|
||||
if (exponent == 0) {
|
||||
return 1;
|
||||
} else if (exponent > 0) {
|
||||
return pow(base, exponent);
|
||||
}
|
||||
return 1 / pow(base, -exponent);
|
||||
}
|
||||
|
||||
/**
|
||||
* Converts lat long coordinates to Open Location Code.
|
||||
* Based on: https://github.com/google/open-location-code/blob/main/c/src/olc.c
|
||||
*/
|
||||
static void latLongToOLC(double lat, double lon, char* code) {
|
||||
char tempCode[] = "1234567890abc";
|
||||
const char kAlphabet[] = "23456789CFGHJMPQRVWX";
|
||||
const byte CODE_LEN = 12;
|
||||
double latitude;
|
||||
double longitude = lon;
|
||||
double latitude_degrees = min(90.0, max(-90.0, lat));
|
||||
|
||||
if (latitude_degrees < 90) // Check latitude less than lat max
|
||||
latitude = latitude_degrees;
|
||||
else {
|
||||
double precision;
|
||||
if (CODE_LEN <= 10)
|
||||
precision = pow_neg(20, floor((CODE_LEN / -2) + 2));
|
||||
else
|
||||
precision = pow_neg(20, -3) / pow(5, CODE_LEN - 10);
|
||||
latitude = latitude_degrees - precision / 2;
|
||||
}
|
||||
|
||||
while (lon < -180) // Normalize longitude
|
||||
longitude += 360;
|
||||
while (lon >= 180)
|
||||
longitude -= 360;
|
||||
|
||||
int64_t lat_val = 90 * 2.5e7;
|
||||
int64_t lng_val = 180 * 8.192e6;
|
||||
lat_val += latitude * 2.5e7;
|
||||
lng_val += longitude * 8.192e6;
|
||||
size_t pos = CODE_LEN;
|
||||
|
||||
if (CODE_LEN > 10) { // Compute grid part of code if needed
|
||||
for (size_t i = 0; i < 5; i++) {
|
||||
int lat_digit = lat_val % 5;
|
||||
int lng_digit = lng_val % 4;
|
||||
int ndx = lat_digit * 4 + lng_digit;
|
||||
tempCode[pos--] = kAlphabet[ndx];
|
||||
lat_val /= 5;
|
||||
lng_val /= 4;
|
||||
}
|
||||
} else {
|
||||
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;
|
||||
tempCode[pos--] = kAlphabet[lng_ndx];
|
||||
tempCode[pos--] = kAlphabet[lat_ndx];
|
||||
lat_val /= 20;
|
||||
lng_val /= 20;
|
||||
|
||||
if (i == 0)
|
||||
tempCode[pos--] = '+';
|
||||
}
|
||||
|
||||
if (CODE_LEN < 9) { // Add padding if needed
|
||||
for (size_t i = CODE_LEN; i < 9; i++)
|
||||
tempCode[i] = '0';
|
||||
tempCode[9] = '+';
|
||||
}
|
||||
|
||||
size_t char_count = CODE_LEN + 1;
|
||||
if (10 > char_count) {
|
||||
char_count = 10;
|
||||
}
|
||||
for (size_t i = 0; i < char_count; i++) {
|
||||
code[i] = tempCode[i];
|
||||
}
|
||||
|
||||
code[char_count] = '\0';
|
||||
}
|
||||
|
||||
struct GeoCoord {
|
||||
double latitude;
|
||||
double longitude;
|
||||
double height;
|
||||
};
|
||||
|
||||
struct OSGR {
|
||||
char e100k;
|
||||
char n100k;
|
||||
uint32_t easting;
|
||||
uint32_t northing;
|
||||
};
|
||||
|
||||
// Converts the coordinate in WGS84 datum to the OSGB36 datum.
|
||||
static struct GeoCoord convertWGS84ToOSGB36(double lat, double lon) {
|
||||
// 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 wgsF = 1 / 298.257223563; // WGS84 datum flattening
|
||||
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;
|
||||
|
||||
// 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
|
||||
GeoCoord osgb;
|
||||
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 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 = 0;
|
||||
//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
|
||||
return osgb;
|
||||
}
|
||||
|
||||
/**
|
||||
* 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
|
||||
*/
|
||||
static struct OSGR latLongToOSGR(double lat, double lon) {
|
||||
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 lambda0 = toRadians(-2);
|
||||
double n0 = -100000;
|
||||
double e0 = 400000;
|
||||
double e2 = 1 - (b*b)/(a*a); // eccentricity squared
|
||||
double n = (a - b)/(a + b);
|
||||
|
||||
GeoCoord osgb = convertWGS84ToOSGB36(lat, lon);
|
||||
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);
|
||||
// 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 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 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;
|
||||
|
||||
OSGR osgr;
|
||||
if (easting < 0 || easting > 700000 || northing < 0 || northing > 1300000) // Check if out of boundaries
|
||||
osgr = { 'I', 'I', 0, 0 };
|
||||
else {
|
||||
uint32_t e100k = floor(easting / 100000);
|
||||
uint32_t n100k = floor(northing / 100000);
|
||||
byte l1 = (19 - n100k) - (19 - n100k) % 5 + floor((e100k + 10) / 5);
|
||||
byte l2 = (19 - n100k) * 5 % 25 + e100k % 5;
|
||||
osgr.e100k = letter[l1];
|
||||
osgr.n100k = letter[l2];
|
||||
osgr.easting = floor((int)easting % 100000);
|
||||
osgr.northing = floor((int)northing % 100000);
|
||||
}
|
||||
return osgr;
|
||||
}
|
||||
|
||||
// Draw GPS status coordinates
|
||||
static void drawGPScoordinates(OLEDDisplay *display, int16_t x, int16_t y, const GPSStatus *gps)
|
||||
{
|
||||
@ -757,85 +404,39 @@ static void drawGPScoordinates(OLEDDisplay *display, int16_t x, int16_t y, const
|
||||
} else {
|
||||
if (gpsFormat != GpsCoordinateFormat_GpsFormatDMS) {
|
||||
char coordinateLine[22];
|
||||
|
||||
geoCoord.updateCoords(int32_t(gps->getLatitude()), int32_t(gps->getLongitude()), int32_t(gps->getAltitude()));
|
||||
if (gpsFormat == GpsCoordinateFormat_GpsFormatDec) { // Decimal Degrees
|
||||
sprintf(coordinateLine, "%f %f", gps->getLatitude() * 1e-7, gps->getLongitude() * 1e-7);
|
||||
sprintf(coordinateLine, "%f %f", geoCoord.getLatitude() * 1e-7, geoCoord.getLongitude() * 1e-7);
|
||||
} else if (gpsFormat == GpsCoordinateFormat_GpsFormatUTM) { // Universal Transverse Mercator
|
||||
UTM utm = latLongToUTM(gps->getLatitude() * 1e-7, gps->getLongitude() * 1e-7);
|
||||
sprintf(coordinateLine, "%2i%1c %06.0f %07.0f", utm.zone, utm.band, utm.easting, utm.northing);
|
||||
sprintf(coordinateLine, "%2i%1c %06i %07i", geoCoord.getUTMZone(), geoCoord.getUTMBand(),
|
||||
geoCoord.getUTMEasting(), geoCoord.getUTMNorthing());
|
||||
} else if (gpsFormat == GpsCoordinateFormat_GpsFormatMGRS) { // Military Grid Reference System
|
||||
MGRS mgrs = latLongToMGRS(gps->getLatitude() * 1e-7, gps->getLongitude() * 1e-7);
|
||||
sprintf(coordinateLine, "%2i%1c %1c%1c %05i %05i", mgrs.zone, mgrs.band, mgrs.east100k, mgrs.north100k,
|
||||
mgrs.easting, mgrs.northing);
|
||||
sprintf(coordinateLine, "%2i%1c %1c%1c %05i %05i", geoCoord.getMGRSZone(), geoCoord.getMGRSBand(), geoCoord.getMGRSEast100k(),
|
||||
geoCoord.getMGRSNorth100k(), geoCoord.getMGRSEasting(), geoCoord.getMGRSNorthing());
|
||||
} else if (gpsFormat == GpsCoordinateFormat_GpsFormatOLC) { // Open Location Code
|
||||
latLongToOLC(gps->getLatitude() * 1e-7, gps->getLongitude() * 1e-7, coordinateLine);
|
||||
geoCoord.getOLCCode(coordinateLine);
|
||||
} else if (gpsFormat == GpsCoordinateFormat_GpsFormatOSGR) { // Ordnance Survey Grid Reference
|
||||
OSGR osgr = latLongToOSGR(gps->getLatitude() * 1e-7, gps->getLongitude() * 1e-7);
|
||||
if (osgr.e100k == 'I' || osgr.n100k == 'I') // OSGR is only valid around the UK region
|
||||
if (geoCoord.getOSGRE100k() == 'I' || geoCoord.getOSGRN100k() == 'I') // OSGR is only valid around the UK region
|
||||
sprintf(coordinateLine, "%s", "Out of Boundary");
|
||||
else
|
||||
sprintf(coordinateLine, "%1c%1c %05i %05i", osgr.e100k, osgr.n100k, osgr.easting, osgr.northing);
|
||||
sprintf(coordinateLine, "%1c%1c %05i %05i", geoCoord.getOSGRE100k(),geoCoord.getOSGRN100k(),
|
||||
geoCoord.getOSGREasting(), geoCoord.getOSGRNorthing());
|
||||
}
|
||||
|
||||
display->drawString(x + (SCREEN_WIDTH - (display->getStringWidth(coordinateLine))) / 2, y, coordinateLine);
|
||||
} else {
|
||||
char latLine[22];
|
||||
char lonLine[22];
|
||||
DMS dms = latLongToDMS(gps->getLatitude() * 1e-7, gps->getLongitude() * 1e-7);
|
||||
sprintf(latLine, "%2i° %2i' %2.4f\" %1c", dms.latDeg, dms.latMin, dms.latSec, dms.latCP);
|
||||
sprintf(lonLine, "%3i° %2i' %2.4f\" %1c", dms.lonDeg, dms.lonMin, dms.lonSec, dms.lonCP);
|
||||
sprintf(latLine, "%2i° %2i' %2.4f\" %1c", geoCoord.getDMSLatDeg(), geoCoord.getDMSLatMin(), geoCoord.getDMSLatSec(),
|
||||
geoCoord.getDMSLatCP());
|
||||
sprintf(lonLine, "%3i° %2i' %2.4f\" %1c", geoCoord.getDMSLonDeg(), geoCoord.getDMSLonMin(), geoCoord.getDMSLonSec(),
|
||||
geoCoord.getDMSLonCP());
|
||||
display->drawString(x + (SCREEN_WIDTH - (display->getStringWidth(latLine))) / 2, y - FONT_HEIGHT_SMALL * 1, latLine);
|
||||
display->drawString(x + (SCREEN_WIDTH - (display->getStringWidth(lonLine))) / 2, y, lonLine);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Ported from my old java code, returns distance in meters along the globe
|
||||
/// surface (by magic?)
|
||||
static float latLongToMeter(double lat_a, double lng_a, double lat_b, double lng_b)
|
||||
{
|
||||
double pk = (180 / 3.14169);
|
||||
double a1 = lat_a / pk;
|
||||
double a2 = lng_a / pk;
|
||||
double b1 = lat_b / pk;
|
||||
double b2 = lng_b / pk;
|
||||
double cos_b1 = cos(b1);
|
||||
double cos_a1 = cos(a1);
|
||||
double t1 = cos_a1 * cos(a2) * cos_b1 * cos(b2);
|
||||
double t2 = cos_a1 * sin(a2) * cos_b1 * sin(b2);
|
||||
double t3 = sin(a1) * sin(b1);
|
||||
double tt = acos(t1 + t2 + t3);
|
||||
if (isnan(tt))
|
||||
tt = 0.0; // Must have been the same point?
|
||||
|
||||
return (float)(6366000 * tt);
|
||||
}
|
||||
|
||||
/**
|
||||
* Computes the bearing in degrees between two points on Earth. Ported from my
|
||||
* old Gaggle android app.
|
||||
*
|
||||
* @param lat1
|
||||
* Latitude of the first point
|
||||
* @param lon1
|
||||
* Longitude of the first point
|
||||
* @param lat2
|
||||
* Latitude of the second point
|
||||
* @param lon2
|
||||
* Longitude of the second point
|
||||
* @return Bearing between the two points in radians. A value of 0 means due
|
||||
* north.
|
||||
*/
|
||||
static float bearing(double lat1, double lon1, double lat2, double lon2)
|
||||
{
|
||||
double lat1Rad = toRadians(lat1);
|
||||
double lat2Rad = toRadians(lat2);
|
||||
double deltaLonRad = toRadians(lon2 - lon1);
|
||||
double y = sin(deltaLonRad) * cos(lat2Rad);
|
||||
double x = cos(lat1Rad) * sin(lat2Rad) - (sin(lat1Rad) * cos(lat2Rad) * cos(deltaLonRad));
|
||||
return atan2(y, x);
|
||||
}
|
||||
|
||||
namespace
|
||||
{
|
||||
|
||||
@ -896,11 +497,11 @@ static float estimatedHeading(double lat, double lon)
|
||||
return b;
|
||||
}
|
||||
|
||||
float d = latLongToMeter(oldLat, oldLon, lat, lon);
|
||||
float d = GeoCoord::latLongToMeter(oldLat, oldLon, lat, lon);
|
||||
if (d < 10) // haven't moved enough, just keep current bearing
|
||||
return b;
|
||||
|
||||
b = bearing(oldLat, oldLon, lat, lon);
|
||||
b = GeoCoord::bearing(oldLat, oldLon, lat, lon);
|
||||
oldLat = lat;
|
||||
oldLon = lon;
|
||||
|
||||
@ -1020,7 +621,7 @@ static void drawNodeInfo(OLEDDisplay *display, OLEDDisplayUiState *state, int16_
|
||||
// display direction toward node
|
||||
hasNodeHeading = true;
|
||||
Position &p = node->position;
|
||||
float d = latLongToMeter(DegD(p.latitude_i), DegD(p.longitude_i), DegD(op.latitude_i), DegD(op.longitude_i));
|
||||
float d = GeoCoord::latLongToMeter(DegD(p.latitude_i), DegD(p.longitude_i), DegD(op.latitude_i), DegD(op.longitude_i));
|
||||
if (d < 2000)
|
||||
snprintf(distStr, sizeof(distStr), "%.0f m", d);
|
||||
else
|
||||
@ -1028,7 +629,7 @@ static void drawNodeInfo(OLEDDisplay *display, OLEDDisplayUiState *state, int16_
|
||||
|
||||
// FIXME, also keep the guess at the operators heading and add/substract
|
||||
// it. currently we don't do this and instead draw north up only.
|
||||
float bearingToOther = bearing(DegD(p.latitude_i), DegD(p.longitude_i), DegD(op.latitude_i), DegD(op.longitude_i));
|
||||
float bearingToOther = GeoCoord::bearing(DegD(p.latitude_i), DegD(p.longitude_i), DegD(op.latitude_i), DegD(op.longitude_i));
|
||||
headingRadian = bearingToOther - myHeading;
|
||||
drawNodeHeading(display, compassX, compassY, headingRadian);
|
||||
}
|
||||
|
||||
@ -66,7 +66,7 @@ MeshPacket *MeshPlugin::allocErrorResponse(Routing_Error err, const MeshPacket *
|
||||
return r;
|
||||
}
|
||||
|
||||
void MeshPlugin::callPlugins(const MeshPacket &mp)
|
||||
void MeshPlugin::callPlugins(const MeshPacket &mp, RxSource src)
|
||||
{
|
||||
// DEBUG_MSG("In call plugins\n");
|
||||
bool pluginFound = false;
|
||||
@ -79,6 +79,7 @@ void MeshPlugin::callPlugins(const MeshPacket &mp)
|
||||
// Was this message directed to us specifically? Will be false if we are sniffing someone elses packets
|
||||
auto ourNodeNum = nodeDB.getNodeNum();
|
||||
bool toUs = mp.to == NODENUM_BROADCAST || mp.to == ourNodeNum;
|
||||
|
||||
for (auto i = plugins->begin(); i != plugins->end(); ++i) {
|
||||
auto &pi = **i;
|
||||
|
||||
@ -87,6 +88,11 @@ void MeshPlugin::callPlugins(const MeshPacket &mp)
|
||||
/// We only call plugins that are interested in the packet (and the message is destined to us or we are promiscious)
|
||||
bool wantsPacket = (isDecoded || pi.encryptedOk) && (pi.isPromiscuous || toUs) && pi.wantPacket(&mp);
|
||||
|
||||
if ((src == RX_SRC_LOCAL) && !(pi.loopbackOk)) {
|
||||
// new case, monitor separately for now, then FIXME merge above
|
||||
wantsPacket = false;
|
||||
}
|
||||
|
||||
assert(!pi.myReply); // If it is !null it means we have a bug, because it should have been sent the previous time
|
||||
|
||||
if (wantsPacket) {
|
||||
@ -169,7 +175,9 @@ void MeshPlugin::callPlugins(const MeshPacket &mp)
|
||||
}
|
||||
|
||||
if (!pluginFound)
|
||||
DEBUG_MSG("No plugins interested in portnum=%d\n", mp.decoded.portnum);
|
||||
DEBUG_MSG("No plugins interested in portnum=%d, src=%s\n",
|
||||
mp.decoded.portnum,
|
||||
(src == RX_SRC_LOCAL) ? "LOCAL":"REMOTE");
|
||||
}
|
||||
|
||||
MeshPacket *MeshPlugin::allocReply()
|
||||
|
||||
@ -33,7 +33,7 @@ class MeshPlugin
|
||||
|
||||
/** For use only by MeshService
|
||||
*/
|
||||
static void callPlugins(const MeshPacket &mp);
|
||||
static void callPlugins(const MeshPacket &mp, RxSource src = RX_SRC_RADIO);
|
||||
|
||||
static std::vector<MeshPlugin *> GetMeshPluginsWithUIFrames();
|
||||
#ifndef NO_SCREEN
|
||||
@ -48,6 +48,10 @@ class MeshPlugin
|
||||
*/
|
||||
bool isPromiscuous = false;
|
||||
|
||||
/** Also receive a copy of LOCALLY GENERATED messages - most plugins should leave
|
||||
* this setting disabled - see issue #877 */
|
||||
bool loopbackOk = false;
|
||||
|
||||
/** Most plugins only understand decrypted packets. For plugins that also want to see encrypted packets, they should set this
|
||||
* flag */
|
||||
bool encryptedOk = false;
|
||||
|
||||
@ -15,6 +15,14 @@ typedef uint32_t PacketId; // A packet sequence number
|
||||
#define ERRNO_UNKNOWN 32 // pick something that doesn't conflict with RH_ROUTER_ERROR_UNABLE_TO_DELIVER
|
||||
#define ERRNO_DISABLED 34 // the itnerface is disabled
|
||||
|
||||
/*
|
||||
* Source of a received message
|
||||
*/
|
||||
enum RxSource {
|
||||
RX_SRC_LOCAL, // message was generated locally
|
||||
RX_SRC_RADIO // message was received from radio mesh
|
||||
};
|
||||
|
||||
/**
|
||||
* the max number of hops a message can pass through, used as the default max for hop_limit in MeshPacket.
|
||||
*
|
||||
|
||||
@ -161,7 +161,7 @@ ErrorCode Router::sendLocal(MeshPacket *p)
|
||||
// If we are sending a broadcast, we also treat it as if we just received it ourself
|
||||
// this allows local apps (and PCs) to see broadcasts sourced locally
|
||||
if (p->to == NODENUM_BROADCAST) {
|
||||
handleReceived(p);
|
||||
handleReceived(p, RX_SRC_LOCAL);
|
||||
}
|
||||
|
||||
return send(p);
|
||||
@ -324,7 +324,7 @@ NodeNum Router::getNodeNum()
|
||||
* Handle any packet that is received by an interface on this node.
|
||||
* Note: some packets may merely being passed through this node and will be forwarded elsewhere.
|
||||
*/
|
||||
void Router::handleReceived(MeshPacket *p)
|
||||
void Router::handleReceived(MeshPacket *p, RxSource src)
|
||||
{
|
||||
// Also, we should set the time from the ISR and it should have msec level resolution
|
||||
p->rx_time = getValidTime(RTCQualityFromNet); // store the arrival timestamp for the phone
|
||||
@ -333,13 +333,16 @@ void Router::handleReceived(MeshPacket *p)
|
||||
bool decoded = perhapsDecode(p);
|
||||
if (decoded) {
|
||||
// parsing was successful, queue for our recipient
|
||||
printPacket("handleReceived", p);
|
||||
if (src == RX_SRC_LOCAL)
|
||||
printPacket("handleReceived(local)", p);
|
||||
else
|
||||
printPacket("handleReceived(remote)", p);
|
||||
} else {
|
||||
printPacket("packet decoding failed (no PSK?)", p);
|
||||
}
|
||||
|
||||
// call plugins here
|
||||
MeshPlugin::callPlugins(*p);
|
||||
MeshPlugin::callPlugins(*p, src);
|
||||
}
|
||||
|
||||
void Router::perhapsHandleReceived(MeshPacket *p)
|
||||
|
||||
@ -122,7 +122,7 @@ class Router : protected concurrency::OSThread
|
||||
* Note: this packet will never be called for messages sent/generated by this node.
|
||||
* Note: this method will free the provided packet.
|
||||
*/
|
||||
void handleReceived(MeshPacket *p);
|
||||
void handleReceived(MeshPacket *p, RxSource src = RX_SRC_RADIO);
|
||||
|
||||
/** Frees the provided packet, and generates a NAK indicating the speicifed error while sending */
|
||||
void abortSendAndNak(Routing_Error err, MeshPacket *p);
|
||||
|
||||
@ -5,6 +5,7 @@
|
||||
#include "RTC.h"
|
||||
#include "Router.h"
|
||||
#include "configuration.h"
|
||||
#include "gps/GeoCoord.h"
|
||||
#include <Arduino.h>
|
||||
#include <SPIFFS.h>
|
||||
//#include <assert.h>
|
||||
@ -184,27 +185,6 @@ bool RangeTestPluginRadio::handleReceived(const MeshPacket &mp)
|
||||
return true; // Let others look at this message also if they want
|
||||
}
|
||||
|
||||
/// Ported from my old java code, returns distance in meters along the globe
|
||||
/// surface (by magic?)
|
||||
float RangeTestPluginRadio::latLongToMeter(double lat_a, double lng_a, double lat_b, double lng_b)
|
||||
{
|
||||
double pk = (180 / 3.14169);
|
||||
double a1 = lat_a / pk;
|
||||
double a2 = lng_a / pk;
|
||||
double b1 = lat_b / pk;
|
||||
double b2 = lng_b / pk;
|
||||
double cos_b1 = cos(b1);
|
||||
double cos_a1 = cos(a1);
|
||||
double t1 = cos_a1 * cos(a2) * cos_b1 * cos(b2);
|
||||
double t2 = cos_a1 * sin(a2) * cos_b1 * sin(b2);
|
||||
double t3 = sin(a1) * sin(b1);
|
||||
double tt = acos(t1 + t2 + t3);
|
||||
if (isnan(tt))
|
||||
tt = 0.0; // Must have been the same point?
|
||||
|
||||
return (float)(6366000 * tt);
|
||||
}
|
||||
|
||||
bool RangeTestPluginRadio::appendFile(const MeshPacket &mp)
|
||||
{
|
||||
auto &p = mp.decoded;
|
||||
@ -303,7 +283,7 @@ bool RangeTestPluginRadio::appendFile(const MeshPacket &mp)
|
||||
fileToAppend.printf("%f,", mp.rx_snr); // RX SNR
|
||||
|
||||
if (n->position.latitude_i && n->position.longitude_i && gpsStatus->getLatitude() && gpsStatus->getLongitude()) {
|
||||
float distance = latLongToMeter(n->position.latitude_i * 1e-7, n->position.longitude_i * 1e-7,
|
||||
float distance = GeoCoord::latLongToMeter(n->position.latitude_i * 1e-7, n->position.longitude_i * 1e-7,
|
||||
gpsStatus->getLatitude() * 1e-7, gpsStatus->getLongitude() * 1e-7);
|
||||
fileToAppend.printf("%f,", distance); // Distance in meters
|
||||
} else {
|
||||
|
||||
Loading…
Reference in New Issue
Block a user