sdrangel/plugins/feature/satellitetracker/satellitetrackersgp4.cpp

500 wiersze
18 KiB
C++

///////////////////////////////////////////////////////////////////////////////////
// Copyright (C) 2021 Jon Beniston, M7RCE //
// Copyright (C) 2013 Daniel Warner <contact@danrw.com> //
// //
// 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 <cmath>
#include <CoordTopocentric.h>
#include <CoordGeodetic.h>
#include <Observer.h>
#include <SGP4.h>
#include "util/units.h"
#include "satellitetrackersgp4.h"
// Convert QGP4 DateTime to Qt QDataTime
static QDateTime dateTimeToQDateTime(DateTime dt)
{
QDateTime qdt(QDate(dt.Year(), dt.Month(), dt.Day()), QTime(dt.Hour(), dt.Minute(), dt.Second(), (int)(dt.Microsecond()/1000.0)), Qt::UTC);
return qdt;
}
// Convert Qt QDataTime to QGP4 DateTime
static DateTime qDateTimeToDateTime(QDateTime qdt)
{
QDateTime utc = qdt.toUTC();
QDate date = utc.date();
QTime time = utc.time();
DateTime dt(date.year(), date.month(), date.day(), time.hour(), time.minute(), time.second());
return dt;
}
// Get ground track
// Throws SatelliteException, DecayedException and TleException
void getGroundTrack(QDateTime dateTime,
const QString& tle0, const QString& tle1, const QString& tle2,
int steps, bool forward,
QList<QGeoCoordinate *>& coordinates)
{
Tle tle = Tle(tle0.toStdString(), tle1.toStdString(), tle2.toStdString());
SGP4 sgp4(tle);
OrbitalElements ele(tle);
double periodMins;
double timeStep;
// Note map doesn't support paths wrapping around Earth
DateTime currentTime = qDateTimeToDateTime(dateTime);
DateTime endTime;
if (forward)
{
periodMins = ele.Period() * 0.9;
endTime = currentTime.AddMinutes(periodMins);
timeStep = periodMins / (steps * 0.9);
}
else
{
periodMins = ele.Period() * 0.4;
endTime = currentTime.AddMinutes(-periodMins);
timeStep = -periodMins / (steps * 0.4);
}
coordinates.clear();
while ((forward && (currentTime < endTime)) || (!forward && (currentTime > endTime)))
{
// Calculate satellite position
Eci eci = sgp4.FindPosition(currentTime);
// Convert satellite position to geodetic coordinates (lat and long)
CoordGeodetic geo = eci.ToGeodetic();
QGeoCoordinate *coord = new QGeoCoordinate(Units::radiansToDegrees(geo.latitude),
Units::radiansToDegrees(geo.longitude),
geo.altitude * 1000.0);
coordinates.append(coord);
// Map is stretched at poles, so use finer steps
if (std::abs(Units::radiansToDegrees(geo.latitude)) >= 70)
currentTime = currentTime.AddMinutes(timeStep/4);
else
currentTime = currentTime.AddMinutes(timeStep);
}
}
// Find azimuth and elevation points during a pass
void getPassAzEl(QLineSeries* azimuth, QLineSeries* elevation, QLineSeries* polar,
const QString& tle0, const QString& tle1, const QString& tle2,
double latitude, double longitude, double altitude,
QDateTime& aos, QDateTime& los)
{
try
{
Tle tle = Tle(tle0.toStdString(), tle1.toStdString(), tle2.toStdString());
SGP4 sgp4(tle);
Observer obs(latitude, longitude, altitude);
DateTime aosTime = qDateTimeToDateTime(aos);
DateTime losTime = qDateTimeToDateTime(los);
DateTime currentTime(aosTime);
int steps = 20;
double timeStep = (losTime - aosTime).TotalSeconds() / steps;
while (currentTime <= losTime)
{
// Calculate satellite position
Eci eci = sgp4.FindPosition(currentTime);
// Calculate angle to satellite from antenna
CoordTopocentric topo = obs.GetLookAngle(eci);
// Save azimuth and elevation in series
QDateTime qdt = dateTimeToQDateTime(currentTime);
if (azimuth != nullptr)
azimuth->append(qdt.toMSecsSinceEpoch(), Units::radiansToDegrees(topo.azimuth));
if (elevation != nullptr)
elevation->append(qdt.toMSecsSinceEpoch(), Units::radiansToDegrees(topo.elevation));
if (polar != nullptr)
polar->append(Units::radiansToDegrees(topo.azimuth), 90.0-Units::radiansToDegrees(topo.elevation));
currentTime = currentTime.AddSeconds(timeStep);
}
}
catch (SatelliteException& se)
{
qDebug() << se.what();
}
catch (DecayedException& de)
{
qDebug() << de.what();
}
catch (TleException& tlee)
{
qDebug() << tlee.what();
}
}
// Get whether a pass passes through 0 degreees
bool getPassesThrough0Deg(const QString& tle0, const QString& tle1, const QString& tle2,
double latitude, double longitude, double altitude,
QDateTime& aos, QDateTime& los)
{
try
{
Tle tle = Tle(tle0.toStdString(), tle1.toStdString(), tle2.toStdString());
SGP4 sgp4(tle);
Observer obs(latitude, longitude, altitude);
DateTime aosTime = qDateTimeToDateTime(aos);
DateTime losTime = qDateTimeToDateTime(los);
DateTime currentTime(aosTime);
int steps = 20;
double timeStep = (losTime - aosTime).TotalSeconds() / steps;
double prevAz;
for (int i = 0; i < steps; i++)
{
// Calculate satellite position
Eci eci = sgp4.FindPosition(currentTime);
// Calculate angle to satellite from antenna
CoordTopocentric topo = obs.GetLookAngle(eci);
double az = Units::radiansToDegrees(topo.azimuth);
if (i == 0)
prevAz = az;
// Does it cross 0 degrees?
if (((prevAz > 270.0) && (az < 90.0)) || ((prevAz < 90.0) && (az >= 270.0)))
return true;
prevAz = az;
currentTime = currentTime.AddSeconds(timeStep);
}
}
catch (SatelliteException& se)
{
qDebug() << se.what();
}
catch (DecayedException& de)
{
qDebug() << de.what();
}
catch (TleException& tlee)
{
qDebug() << tlee.what();
}
return false;
}
// Find maximum elevation in a pass
static double findMaxElevation(Observer& obs1, SGP4& sgp4, const DateTime& aos, const DateTime& los)
{
Observer obs(obs1.GetLocation());
bool running;
double timeStep = (los - aos).TotalSeconds() / 9.0;
DateTime currentTime(aos);
DateTime time1(aos);
DateTime time2(los);
double maxElevation;
do
{
running = true;
maxElevation = -INFINITY;
while (running && (currentTime < time2))
{
Eci eci = sgp4.FindPosition(currentTime);
CoordTopocentric topo = obs.GetLookAngle(eci);
if (topo.elevation > maxElevation)
{
maxElevation = topo.elevation;
currentTime = currentTime.AddSeconds(timeStep);
if (currentTime > time2)
currentTime = time2;
}
else
running = false;
}
time1 = currentTime.AddSeconds(-2.0 * timeStep);
time2 = currentTime;
currentTime = time1;
timeStep = (time2 - time1).TotalSeconds() / 9.0;
}
while (timeStep > 1.0);
return Units::radiansToDegrees(maxElevation);
}
// Find the time at which the satellite crossed the minimum elevation required for AOS or LOS
static DateTime findCrossingPoint(Observer& obs, SGP4& sgp4, const DateTime& initialTime1, const DateTime& initialTime2, double minElevation, bool findingAOS)
{
bool running;
int cnt;
DateTime time1(initialTime1);
DateTime time2(initialTime2);
DateTime middleTime;
running = true;
cnt = 0;
while (running && (cnt++ < 16))
{
middleTime = time1.AddSeconds((time2 - time1).TotalSeconds() / 2.0);
Eci eci = sgp4.FindPosition(middleTime);
CoordTopocentric topo = obs.GetLookAngle(eci);
if (topo.elevation > minElevation)
{
if (findingAOS)
time2 = middleTime;
else
time1 = middleTime;
}
else
{
if (findingAOS)
time1 = middleTime;
else
time2 = middleTime;
}
if ((time2 - time1).TotalSeconds() < 1.0)
{
running = false;
int us = middleTime.Microsecond();
middleTime = middleTime.AddMicroseconds(-us);
middleTime = middleTime.AddSeconds(findingAOS ? 1 : -1);
}
}
running = true;
cnt = 0;
while (running && (cnt++ < 6))
{
Eci eci = sgp4.FindPosition(middleTime);
CoordTopocentric topo = obs.GetLookAngle(eci);
if (topo.elevation > minElevation)
middleTime = middleTime.AddSeconds(findingAOS ? -1 : 1);
else
running = false;
}
return middleTime;
}
// Find when AOS occured, by stepping backwards
static DateTime findAOSBackwards(Observer& obs, SGP4& sgp4, DateTime& startTime,
int predictionPeriod, double minElevation, bool& aosUnknown)
{
DateTime previousTime(startTime);
DateTime currentTime(startTime);
DateTime endTime(startTime.AddDays(-predictionPeriod));
while (currentTime >= endTime)
{
Eci eci = sgp4.FindPosition(currentTime);
CoordTopocentric topo = obs.GetLookAngle(eci);
if (topo.elevation < minElevation)
{
aosUnknown = false;
return findCrossingPoint(obs, sgp4, currentTime, previousTime, minElevation, true);
}
previousTime = currentTime;
currentTime = currentTime - TimeSpan(0, 0, 180);
}
aosUnknown = true;
return currentTime;
}
bool inPassWindow(DateTime dateTime, QTime passStartTime, QTime passEndTime, bool utc)
{
// Don't compare seconds as not currently settable in GUI
QDateTime qdt = dateTimeToQDateTime(dateTime);
if (!utc)
qdt = qdt.toLocalTime();
QTime qt(qdt.time().hour(), qdt.time().minute());
passStartTime = QTime(passStartTime.hour(), passStartTime.minute());
passEndTime = QTime(passEndTime.hour(), passEndTime.minute());
// If passEndTime is before passStartTime, then we allow overnight passes
if (passEndTime > passStartTime)
{
return (qt >= passStartTime) && (qt <= passEndTime);
}
else
{
return (qt <= passEndTime) || (qt >= passStartTime);
}
}
// Create a list of satellite passes, between the given start and end times, that exceed the specified minimum elevation
// We return an uninitalised QDateTime if AOS or LOS is outside of predictionPeriod
static QList<SatellitePass *> createPassList(Observer& obs, SGP4& sgp4, DateTime& startTime,
int predictionPeriod, double minAOSElevation, double minPassElevationDeg,
QTime passStartTime, QTime passEndTime, bool utc,
int noOfPasses)
{
QList<SatellitePass *> passes;
bool aos = false;
bool aosUnknown = true;
double aosAz;
double losAz;
DateTime previousTime(startTime);
DateTime currentTime(startTime);
DateTime endTime(startTime.AddDays(predictionPeriod));
DateTime aosTime;
DateTime losTime;
while (currentTime < endTime)
{
bool endOfPass = false;
Eci eci = sgp4.FindPosition(currentTime);
CoordTopocentric topo = obs.GetLookAngle(eci);
if (!aos && (topo.elevation > minAOSElevation))
{
if (startTime == currentTime)
{
// AOS is before startTime
aosTime = findAOSBackwards(obs, sgp4, startTime, predictionPeriod, minAOSElevation, aosUnknown);
}
else
{
aosTime = findCrossingPoint(obs, sgp4, previousTime, currentTime, minAOSElevation, true);
aosUnknown = false;
}
aos = true;
eci = sgp4.FindPosition(aosTime);
topo = obs.GetLookAngle(eci);
aosAz = Units::radiansToDegrees(topo.azimuth);
}
else if (aos && (topo.elevation < minAOSElevation))
{
aos = false;
endOfPass = true;
losTime = findCrossingPoint(obs, sgp4, previousTime, currentTime, minAOSElevation, false);
eci = sgp4.FindPosition(losTime);
topo = obs.GetLookAngle(eci);
losAz = Units::radiansToDegrees(topo.azimuth);
double maxElevationDeg = findMaxElevation(obs, sgp4, aosTime, losTime);
if ((maxElevationDeg >= minPassElevationDeg)
&& inPassWindow(aosTime, passStartTime, passEndTime, utc)
&& inPassWindow(losTime, passStartTime, passEndTime, utc))
{
SatellitePass *pass = new SatellitePass;
pass->m_aos = aosUnknown ? QDateTime() : dateTimeToQDateTime(aosTime);
pass->m_los = dateTimeToQDateTime(losTime);
pass->m_maxElevation = maxElevationDeg;
pass->m_aosAzimuth = aosAz;
pass->m_losAzimuth = losAz;
pass->m_northToSouth = std::min(360.0-aosAz, aosAz-0.0) < std::min(360.0-losAz, losAz-0.0);
passes.append(pass);
noOfPasses--;
if (noOfPasses <= 0)
return passes;
}
}
previousTime = currentTime;
if (endOfPass)
currentTime = currentTime + TimeSpan(0, 30, 0); // 30 minutes - no orbit likely to be that fast
else
currentTime = currentTime + TimeSpan(0, 0, 180);
if (currentTime > endTime)
currentTime = endTime;
}
if (aos)
{
// Pass still in progress at end time
Eci eci = sgp4.FindPosition(currentTime);
CoordTopocentric topo = obs.GetLookAngle(eci);
losAz = Units::radiansToDegrees(topo.azimuth);
double maxElevationDeg = findMaxElevation(obs, sgp4, aosTime, losTime);
if ((maxElevationDeg >= minPassElevationDeg)
&& inPassWindow(aosTime, passStartTime, passEndTime, utc)
&& inPassWindow(losTime, passStartTime, passEndTime, utc))
{
SatellitePass *pass = new SatellitePass;
pass->m_aos = aosUnknown ? QDateTime() : dateTimeToQDateTime(aosTime);
pass->m_los = QDateTime();
pass->m_aosAzimuth = aosAz;
pass->m_losAzimuth = losAz;
pass->m_maxElevation = maxElevationDeg;
pass->m_northToSouth = std::min(360.0-aosAz, aosAz-0.0) < std::min(360.0-losAz, losAz-0.0);
passes.append(pass);
}
}
return passes;
}
void getSatelliteState(QDateTime dateTime,
const QString& tle0, const QString& tle1, const QString& tle2,
double latitude, double longitude, double altitude,
int predictionPeriod, int minAOSElevationDeg, int minPassElevationDeg,
QTime passStartTime, QTime passFinishTime, bool utc,
int noOfPasses, int groundTrackSteps, SatelliteState *satState)
{
try {
Tle tle = Tle(tle0.toStdString(), tle1.toStdString(), tle2.toStdString());
SGP4 sgp4(tle);
Observer obs(latitude, longitude, altitude);
DateTime dt = qDateTimeToDateTime(dateTime);
// Calculate satellite position
Eci eci = sgp4.FindPosition(dt);
// Calculate angle to satellite from antenna
CoordTopocentric topo = obs.GetLookAngle(eci);
// Convert satellite position to geodetic coordinates (lat and long)
CoordGeodetic geo = eci.ToGeodetic();
satState->m_latitude = Units::radiansToDegrees(geo.latitude);
satState->m_longitude = Units::radiansToDegrees(geo.longitude);
satState->m_altitude = geo.altitude;
satState->m_azimuth = Units::radiansToDegrees(topo.azimuth);
satState->m_elevation = Units::radiansToDegrees(topo.elevation);
satState->m_range = topo.range;
satState->m_rangeRate = topo.range_rate;
OrbitalElements ele(tle);
satState->m_speed = eci.Velocity().Magnitude();
satState->m_period = ele.Period();
if (noOfPasses > 0)
{
qDeleteAll(satState->m_passes);
satState->m_passes = createPassList(obs, sgp4, dt, predictionPeriod,
Units::degreesToRadians((double)minAOSElevationDeg),
minPassElevationDeg,
passStartTime, passFinishTime, utc,
noOfPasses);
}
qDeleteAll(satState->m_groundTrack);
qDeleteAll(satState->m_predictedGroundTrack);
getGroundTrack(dateTime, tle0, tle1, tle2, groundTrackSteps, false, satState->m_groundTrack);
getGroundTrack(dateTime, tle0, tle1, tle2, groundTrackSteps, true, satState->m_predictedGroundTrack);
}
catch (SatelliteException& se)
{
qDebug() << "getSatelliteState: " << satState->m_name << ": " << se.what();
}
catch (DecayedException& de)
{
qDebug() << "getSatelliteState: " << satState->m_name << ": " << de.what();
}
catch (TleException& tlee)
{
qDebug() << "getSatelliteState: " << satState->m_name << ": " << tlee.what();
}
}