SatclockRS41/satpredict.c

/* This program is derived from the publicly licensed
 * PREDICT: A satellite tracking/orbital prediction program
 * Copyright John A. Magliacane, KD2BD 1991-2000
 *
 * [John's original licensing terms]
 * 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; either version 2 of the License or any later
 * version.
 *
 * This program is distributed in the hope that it will useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
 * for more details.
 */

/* SATCLOCK: A satellite tracking/orbital prediction program
 * intended for Amateur Radio Satellites. Originally predict1
 * that was hacked together in 2000, and again in 2018 by
 * Chris Baird VK2FJDA/VK2CJB <cjb@brushtail.apana.org.au>.
 * ...and again in 2020 for use on Vailsa RS41 hardware.
 */

/* Oh, STM32F1 doesn't support double...
 * Thankfully Andrew Thall's "float2" work saved the day!
 * http://andrewthall.org/papers/df64_qf128.pdf
 */

#include <math.h>
#include "config.h"
#include "ublox.h"		/* struct GPSEntry */
#include "satpredict.h"
#include "satdata.h"

extern GPSEntry gpsData;
extern volatile char flaga;
struct SatStatus satstatus[MAXSATS];

/* ---------------------------------------------------------------------- */

void split (float2* r, float a)
{
  const float SPLIT = ((1<<12)+1);
  float t = a * SPLIT;
  float a_hi = t - (t-a);
  float a_lo = a - a_hi;
  r->x = a_hi;
  r->y = a_lo;
}


void quickTwoSum (float2 *r, float a, float b)
{
  /* iff |a| >= |b| */
  float s = a + b;
  float e = b - (s-a);
  r->x = s;
  r->y = e;
}


void twoSum (float2 *r, float a, float b)
{
  float s = a + b;
  float v = s - a ;
  float e = (a-(s-v)) + (b-v);
  r->x = s;
  r->y = e;
}


void df64add (float2 *s, float2 *a, float2 *b)
{
  float2 t;
  twoSum(s, a->x, b->x);
  twoSum(&t, a->y, b->y);
  s->y += t.x;
  quickTwoSum(s, s->x, s->y);
  s->y += t.y;
  quickTwoSum(s, s->x, s->y);
}


int df64lt (float2 *a, float2 *b)
{
  return ((b->x - a->x) + (b->y - a->y) > 0.0) ? 1 : 0;
  /* return (a->x < b->x || ((a->x == b->x) && (a->y < b->y))); */
}


/* ---------------------------------------------------------------------- */

struct {
  float stnlat;
  float stnlon;			/* in opposite orientation? */
  int   stnalt;			/* in metres */
} qth;


#define FF (3.35289186924e-03)
#define KM (1.609344)		/* lol america */
#define R0 (6378.135)		/* radius of earth */
#define TP (6.283185307179586)
#define deg2rad (1.74532925199e-02)

float2 daynum;
float age, ak, apogee, azimuth, c0, c1, c2, c3, c7, c8, c9, c[3][2],
  df, e1, e2, e, elevation, g7, k2, l8, m1, m5, m, n0, o,
  perigee, q0, q, r3, r9, r, range, rk, s0, s1, s2, s3, s7, seight, s9,
  se, sma, t1, w, x0, x1, x5, x8, x9, x, y5, y8,
  y9, y, yone, yr, yzero, z1, z5, z8, z9, z;


/* ---------------------------------------------------------------------- */

/* reference: http://nghiaho.com/?p=997 .. claims 0.09 degree accuracy */

float myatanf (float x)
{
#if 1 /* slow_and_uses_up_flash_but_accurate */
  return atanf(x) / deg2rad;
#endif

#if fast_but_slightly_dodgy
#define M_PI_6 (0.5235987755982988)
#define M_PI_12 (0.2617993877991494)
  float y;
  int complement = 0;
  int region = 0;
  int sign = 0;

  if (x < 0) { x = -x; sign = 1; }
  if (x > 1.0) { x = 1.0/x; complement = 1; }
  if (x > M_PI_12) { x = (x - M_PI_6) / (1 + M_PI_6 * x); region = 1; }

#if 1 /*this_approximation_is_about_?2deg_off_in_the_final_results*/
  float ax = fabs(x);
  y = M_PI_4*x - x*(ax-1) * (0.2447 + 0.0663*ax);
#endif
#if this_is_about_2deg_off_as_well
  float x2 = x*x;
  y = x*(1.6867629106 + x2*0.4378497304)/(1.6867633134 + x2);
#endif
  if (region) y += M_PI_6;
  if (complement) y = M_PI_2 - y;
  if (sign) y = -y;
  return y / deg2rad;
#endif
}


/* ---------------------------------------------------------------------- */

long DayNum (int m, int d, int y)
{
  /* calculate the day number from m/d/y */
  long dn;
  float mm, yy;

  if (m < 3)
    {
      y--;
      m += 12;
    }

  if (y <= 50) y += 100;        /* Correct for Y2K... */

  yy = (float)y;
  mm = (float)m;
  dn = (long)(floorf(365.25 * (yy - 80.0)) - floorf(19.0 + yy / 100.0) +
	      floorf(4.75 + yy / 400.0) - 16.0);
  dn += d + 30*m + (long)floorf(0.6 * mm - 0.3);
  return dn;
}


void CurrentDaynum (void)
{
  float day, d;
  float2 temp1, temp2;

  day = DayNum (gpsData.month, gpsData.day, gpsData.year);
  d = (float)gpsData.seconds/(float)86400.0;
  d += (float)gpsData.minutes/(float)1440.0;
  d += (float)gpsData.hours/(float)24.0;
  split (&temp1, day);
  split (&temp2, d);
  df64add (&daynum, &temp1, &temp2);
}


int AosHappens (int satnum)
{
  /* This function returns a 1 if the satellite can ever
   * rise above the horizon of the ground station */
  float lin, sma, apogee;

  lin = satdata[satnum].incl;
  if (lin >= 90.0) lin = 180.0 - lin;

  sma = 331.25 * powf(1440.0/satdata[satnum].meanmo, 2.0/3.0);
  apogee = sma * (1.0+satdata[satnum].eccn) - R0;

  if ((acosf(R0 / (apogee + R0)) + (lin*deg2rad))
      > fabs((qth.stnlat+HORIZON) * deg2rad))
    return 1;
  else
    return 0;
}


int Decayed (int satnum)
{
  /* This function returns a 1 if it appears that the current satellite has
   * decayed at the time of 'daynum' */

  float2 nsatepoch;
  float2 r;
  float t;

  t = (16.666666 - satdata[satnum].meanmo) / (10.0 * fabs(satdata[satnum].drag));
  t += satdata[satnum].refepoch;
  twoSum(&nsatepoch, -DayNum(1, 0, satdata[satnum].year), -t);
  df64add(&r, &daynum, &nsatepoch);

  if (r.x > 0)
    return 1;
  else
    return 0;
}


int Geostationary (int satnum)
{
  /* This function returns a 1 if the current satellite appears to be in a
   * geostationary orbit */

  if (fabs(satdata[satnum].meanmo - 1.0027) < 0.0002)
    return 1;
  else
    return 0;
}


void CalcSat (int satnum)
{
  float xx, xxt;

  age = ((daynum.x - DayNum(1, 0, satdata[satnum].year)) - satdata[satnum].refepoch) + daynum.y;

  yr = (float)satdata[satnum].year;
  if (yr <= 50.0) yr += 100.0;  /* Y2K... */

  t1 = yr - 1.0;
  df = 366.0 + floorf(365.25 * (t1 - 80.0)) -
    floorf(t1 / 100.0) + floorf(t1 / 400.0 + 0.75);
  t1 = (df + 29218.5) / 36525.0;
  t1 = 6.6460656 + t1*(2400.051262 + t1*2.581e-5);
  se = t1 / 24.0 - yr;
  n0 = satdata[satnum].meanmo + (age * satdata[satnum].drag);
  sma = 331.25 * powf((1440.0 / n0), (2.0 / 3.0));
  e2 = 1.0 - (satdata[satnum].eccn * satdata[satnum].eccn);
  e1 = sqrtf(e2);
  k2 = 9.95 * powf(R0 / sma, 3.5) / e2 * e2;
  s1 = sinf(satdata[satnum].incl * deg2rad);
  c1 = cosf(satdata[satnum].incl * deg2rad);
  l8 = qth.stnlat * deg2rad;
  s9 = sinf(l8);
  c9 = cosf(l8);
  seight = sinf(-qth.stnlon * deg2rad);
  c8 = cosf(qth.stnlon * deg2rad);
  r9 = R0 * (1.0 - (FF/2.0) * (cosf(2.0*l8) - 1.0)) - (qth.stnalt/1000.0);
  xx = (1.0-FF)*(1.0-FF) * s9 / c9;
  xxt = r9 / sqrtf(1 + xx*xx);	/* sin(atan(x)) = x/((1+x^2)^-2) */
  z9 = xxt * xx;
  x9 = xxt * c8;
  y9 = xxt * seight;
  apogee = sma * (1.0 + satdata[satnum].eccn) - R0;
  perigee = sma * (1.0 - satdata[satnum].eccn) - R0;

  age = ((daynum.x - DayNum(1, 0, satdata[satnum].year)) - satdata[satnum].refepoch) + daynum.y;
  o = deg2rad * (satdata[satnum].raan - age * k2 * c1);
  s0 = sinf(o);
  c0 = cosf(o);
  w = deg2rad * (satdata[satnum].argper + age * k2 * (2.5 * c1 * c1 - 0.5));
  s2 = sinf(w);
  c2 = cosf(w);
  c[0][0] = (c2*c0) - (s2*s0*c1);
  c[1][0] = (c2*s0) + (s2*c0*c1);
  c[0][1] = (-s2*c0) - (c2*s0*c1);
  c[1][1] = (-s2*s0) + (c2*c0*c1);
  c[2][0] = s2*s1;
  c[2][1] = c2*s1;
  q0 = (satdata[satnum].meanan / 360.0) + satdata[satnum].orbitnum;
  q = n0 * age + q0;
  q = q - floorf(q);
  m = q * TP;
  e = m + satdata[satnum].eccn * (sinf(m) + 0.5*satdata[satnum].eccn * sinf(m*2.0));

  do /* Kepler's Equation */
    {
      s3 = sinf(e);
      c3 = cosf(e);
      r3 = 1.0 - (satdata[satnum].eccn * c3);
      m1 = e - (satdata[satnum].eccn * s3);
      m5 = m1 - m;
      e = e - m5 / r3;
    }
  while (fabs(m5) >= 1.0e-6);

  x0 = sma * (c3 - satdata[satnum].eccn);
  yzero = sma * e1 * s3;
  r = sma * r3;
  x1 = (x0 * c[0][0]) + (yzero * c[0][1]);
  yone = (x0 * c[1][0]) + (yzero * c[1][1]);
  z1 = (x0 * c[2][0]) + (yzero * c[2][1]);
  g7 = ((daynum.x - df) + daynum.y) * 1.0027379093 + se;
  g7 = TP * (g7 - floorf(g7));
  s7 = -sinf(g7);
  c7 = cosf(g7);
  x = (x1 * c7) - (yone * s7);
  y = (x1 * s7) + (yone * c7);
  z = z1;
  x5 = x - x9;
  y5 = y - y9;
  z5 = z - z9;
  range = (x5*x5) + (y5*y5) + (z5*z5);
  z8 = (x5*c8*c9) + (y5*seight*c9) + (z5*s9);
  x8 = (-x5*c8*s9) - (y5*seight*s9) + (z5*c9);
  y8 = (y5*c8) - (x5*seight);
  ak = r - R0;
  rk = sqrtf(range);
  elevation = myatanf(z8 / sqrtf(range - z8*z8));
  azimuth = myatanf(y8 / x8);
  if (x8 < 0.0) azimuth += 180.0;
  if (azimuth < 0.0) azimuth += 360.0;

  struct SatStatus *s = &satstatus[satnum];

  s->lastrange = s->range;
  s->lastdaynum.x = s->daynum.x;
  s->lastdaynum.y = s->daynum.y;

  s->daynum.x = daynum.x;
  s->daynum.y = daynum.y;
  s->range = rk;
  s->elevation = elevation;
  s->azimuth = azimuth;

  // Thall did provide a diff and division procedure, but I'm cutting corners..

  float dt = 86400.0 * (s->daynum.x - s->lastdaynum.x);
  dt += (86400.0 * s->daynum.y) - (86400.0 * s->lastdaynum.y);
  float dr = (rk - s->lastrange) * 1000.0;

  if (s->lastrange && (dt > 0.0))
    s->dopplerhz = dr * satdata[satnum].uplink * 1000000.0 / (-299792458.0 * dt);
}


/* ---------------------------------------------------------------------- */

void sat_update (void)
{
  if (!(flaga & 0x80))
    {
      for (int i = 0; i < numsats; i++)
	satstatus[i].state = -8;
      return;
    }

  qth.stnlat = gpsData.lat_raw / 10000000.0;
  qth.stnlon = -gpsData.lon_raw / 10000000.0;
  qth.stnalt = gpsData.alt_raw / 1000.0;

  CurrentDaynum();

  /* something to do when time & location details are known */

  for (int i = 0; i < numsats; i++)
    {
      satstatus[i].state = 0;
      if (Geostationary(i)) satstatus[i].state -= 1;
      if (Decayed(i)) satstatus[i].state -= 2;
      if (!AosHappens(i)) satstatus[i].state -= 4;
    }

  /* scan sats for visibility */

  for (int i = 0; i < numsats; i++)
    {
      if (satstatus[i].state < 0) /* eliminated sats */
	continue;

      satstatus[i].state = 1;
      CalcSat(i);
    }
}

/* ---------------------------------------------------------------------- */