1
0
mirror of https://github.com/f4exb/sdrangel.git synced 2024-11-07 17:16:02 -05:00
sdrangel/sdrbase/util/azel.cpp
Jon Beniston 1a9b6d1763 Add ADS-B Demodulator plugin.
Add GS232 Rotator Controller feature plugin.
Add altitude and station name to My Positon.
Use QtDeploy to ensure all required Qt libraries are included (Should
fix OpenGL issues)
2020-10-27 16:22:10 +00:00

152 lines
5.8 KiB
C++

///////////////////////////////////////////////////////////////////////////////////
// Copyright (C) 2020 Jon Beniston, M7RCE //
// //
// This program is free software; you can redistribute it and/or modify //
// it under the terms of the GNU General Public License as published by //
// the Free Software Foundation as version 3 of the License, or //
// (at your option) any later version. //
// //
// This program is distributed in the hope that it will be useful, //
// but WITHOUT ANY WARRANTY; without even the implied warranty of //
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the //
// GNU General Public License V3 for more details. //
// //
// You should have received a copy of the GNU General Public License //
// along with this program. If not, see <http://www.gnu.org/licenses/>. //
///////////////////////////////////////////////////////////////////////////////////
#include "azel.h"
#define _USE_MATH_DEFINES
#include <cmath>
// Calculate cartesian distance between two points
double AzEl::cartDistance(const AzElPoint& a, const AzElPoint& b)
{
double dx = b.m_cart.m_x - a.m_cart.m_x;
double dy = b.m_cart.m_y - a.m_cart.m_y;
double dz = b.m_cart.m_z - a.m_cart.m_z;
return std::sqrt(dx*dx + dy*dy + dz*dz);
}
// Calculate vector difference then normalise the result
bool AzEl::normVectorDiff(const AzElCartesian& a, const AzElCartesian& b, AzElCartesian& n)
{
n.m_x = b.m_x - a.m_x;
n.m_y = b.m_y - a.m_y;
n.m_z = b.m_z - a.m_z;
double distance = std::sqrt(n.m_x*n.m_x + n.m_y*n.m_y + n.m_z*n.m_z);
if (distance > 0.0f)
{
n.m_x = n.m_x / distance;
n.m_y = n.m_y / distance;
n.m_z = n.m_z / distance;
return true;
}
else
{
return false;
}
}
// Convert geodetic latitude (as given by GPS) to geocentric latitude (angle from centre of Earth between the point and equator)
// Both in radians.
// https://en.wikipedia.org/wiki/Latitude#Geocentric_latitude
double AzEl::geocentricLatitude(double latRad) const
{
double e2 = 0.00669437999014;
return std::atan((1.0 - e2) * std::tan(latRad));
}
// Earth radius for a given latitude, as it's not quite spherical
// http://en.wikipedia.org/wiki/Earth_radius
double AzEl::earthRadiusInMetres(double geodeticLatRad) const
{
double equatorialRadius = 6378137.0;
double polarRadius = 6356752.3;
double cosLat = std::cos(geodeticLatRad);
double sinLat = std::sin(geodeticLatRad);
double t1 = equatorialRadius * equatorialRadius * cosLat;
double t2 = polarRadius * polarRadius * sinLat;
double t3 = equatorialRadius * cosLat;
double t4 = polarRadius * sinLat;
return std::sqrt((t1*t1 + t2*t2)/(t3*t3 + t4*t4));
}
// Convert spherical coordinate to cartesian. Also calculates radius and a normal vector
void AzEl::sphericalToCartesian(AzElPoint& point)
{
// First calculate cartesian coords for point on Earth's surface
double latRad = point.m_spherical.m_latitude * M_PI/180.0;
double longRad = point.m_spherical.m_longitude * M_PI/180.0;
point.m_radius = earthRadiusInMetres(latRad);
double clat = geocentricLatitude(latRad);
double cosLong = cos(longRad);
double sinLong = sin(longRad);
double cosLat = cos(clat);
double sinLat = sin(clat);
point.m_cart.m_x = point.m_radius * cosLong * cosLat;
point.m_cart.m_y = point.m_radius * sinLong * cosLat;
point.m_cart.m_z = point.m_radius * sinLat;
// Calculate normal vector at surface
double cosGLat = std::cos(latRad);
double sinGLat = std::sin(latRad);
point.m_norm.m_x = cosGLat * cosLong;
point.m_norm.m_y = cosGLat * sinLong;
point.m_norm.m_z = sinGLat;
// Add altitude along normal vector
point.m_cart.m_x += point.m_spherical.m_altitude * point.m_norm.m_x;
point.m_cart.m_y += point.m_spherical.m_altitude * point.m_norm.m_y;
point.m_cart.m_z += point.m_spherical.m_altitude * point.m_norm.m_z;
}
// Calculate azimuth of target from location
void AzEl::calcAzimuth()
{
AzElPoint bRot;
// Rotate so location is at lat=0, long=0
bRot.m_spherical.m_latitude = m_target.m_spherical.m_latitude;
bRot.m_spherical.m_longitude = m_target.m_spherical.m_longitude - m_location.m_spherical.m_longitude;
bRot.m_spherical.m_altitude = m_target.m_spherical.m_altitude;
sphericalToCartesian(bRot);
double aLat = geocentricLatitude(-m_location.m_spherical.m_latitude * M_PI / 180.0);
double aCos = std::cos(aLat);
double aSin = std::sin(aLat);
double bx = (bRot.m_cart.m_x * aCos) - (bRot.m_cart.m_z * aSin);
double by = bRot.m_cart.m_y;
double bz = (bRot.m_cart.m_x * aSin) + (bRot.m_cart.m_z * aCos);
if (bz*bz + by*by > 1e-6)
{
double theta = std::atan2(bz, by) * 180.0 / M_PI;
m_azimuth = 90.0 - theta;
if (m_azimuth < 0.0)
m_azimuth += 360.0;
else if (m_azimuth > 360.0)
m_azimuth -= 360.0;
}
else
m_azimuth = 0.0;
}
// Calculate elevation of target from location
void AzEl::calcElevation()
{
AzElCartesian bma;
if (normVectorDiff(m_location.m_cart, m_target.m_cart, bma))
{
m_elevation = 90.0 - (180.0/M_PI) * std::acos(bma.m_x * m_location.m_norm.m_x
+ bma.m_y * m_location.m_norm.m_y
+ bma.m_z * m_location.m_norm.m_z);
}
else
m_elevation = 0.0;
}