WhatsUp? A GPS Keychain That Shows Overhead Planets

/////////// space stuf
//////////////////////////////////////////////////////////////////////////
#include "ephemeris_tables.h"

static const double SECONDS_PER_DAY = 86400.0;
static const double JD_UNIX_EPOCH   = 2440587.5;
static const double G_AU3_DAY2      = 2.9591220828559093e-4; // AU^3 / day^2
static const double MU_SUN          = G_AU3_DAY2 * 1.0;

static const double MU_EARTH_KM = 398600.4418;
static const double AU_KM       = 149597870.700;
static const double DAY_SEC     = 86400.0;
static const double MOON_DT_SEC = 300.0;

static inline void accel_sun(const double r[3], double a[3]) {
  double d2 = r[0]*r[0] + r[1]*r[1] + r[2]*r[2];
  double d  = sqrt(d2);
  double denom = (d*d*d) + 1e-12;
  double k = -MU_SUN / denom;
  a[0] = k * r[0];
  a[1] = k * r[1];
  a[2] = k * r[2];
}

static inline void euler_step(double r[3], double v[3], double dt_days) {
  double a[3];
  accel_sun(r, a);
  r[0] += v[0] * dt_days;
  r[1] += v[1] * dt_days;
  r[2] += v[2] * dt_days;
  v[0] += a[0] * dt_days;
  v[1] += a[1] * dt_days;
  v[2] += a[2] * dt_days;
}

static inline void eci_to_ecef_target_timestamp(const double r_eci[3], double r_ecef[3], double theta) {
  double c = cos(theta);
  double s = sin(theta);
  r_ecef[0] =  c * r_eci[0] + s * r_eci[1];
  r_ecef[1] = -s * r_eci[0] + c * r_eci[1];
  r_ecef[2] =  r_eci[2];
}

double earth_rotation_angle_from_unix(int64_t t_unix) {
  double JD = JD_UNIX_EPOCH + (double)t_unix / SECONDS_PER_DAY;
  double d  = JD - 2451545.0;

  double theta = 2.0 * M_PI * (
    0.7790572732640 +
    1.00273781191135448 * d
  );
  theta = fmod(theta, 2.0 * M_PI);
  if (theta < 0.0) theta += 2.0 * M_PI;
  return theta;
}

static inline void latlon_to_unit_ecef(double lat_deg, double lon_deg, double out[3]) {
  double lat = lat_deg * (M_PI / 180.0);
  double lon = lon_deg * (M_PI / 180.0);
  double clat = cos(lat);
  out[0] = clat * cos(lon);
  out[1] = clat * sin(lon);
  out[2] = sin(lat);
}

static inline bool is_visible_est(const double planet_ecef[3], double user_lat, double user_lon) {
  double user_ecef[3];
  latlon_to_unit_ecef(user_lat, user_lon, user_ecef);
  double dot = planet_ecef[0]*user_ecef[0] + planet_ecef[1]*user_ecef[1] + planet_ecef[2]*user_ecef[2];
  return (dot > 0.0);
}

int closest_time_index(const int64_t *unix_time, int n, int64_t target) {
  if (n <= 0) return 0;
  if (target <= unix_time[0]) return 0;
  if (target >= unix_time[n - 1]) return n - 1;

  int left = 0;
  int right = n - 1;
  while (left <= right) {
    int mid = (left + right) >> 1;
    int64_t tmid = unix_time[mid];
    if (tmid == target) return mid;
    if (tmid < target) left = mid + 1;
    else right = mid - 1;
  }

  int64_t dr = target - unix_time[right];
  int64_t dl = unix_time[left] - target;
  return (dr <= dl) ? right : left;
}

void propagate_planet_grcs(int idx, double dt_days_dbl,
                           const float planet_table[EPHEMERIS_SAMPLES][6],
                           double r_geo_out[3], double r_earth[3])
{

  double r_p[3] = {
    (double)planet_table[idx][0] + (double)sun_sv[idx][0],
    (double)planet_table[idx][1] + (double)sun_sv[idx][1],
    (double)planet_table[idx][2] + (double)sun_sv[idx][2]
  };
  double v_p[3] = {
    (double)planet_table[idx][3] + (double)sun_sv[idx][3],
    (double)planet_table[idx][4] + (double)sun_sv[idx][4],
    (double)planet_table[idx][5] + (double)sun_sv[idx][5]
  };

  euler_step(r_p, v_p, dt_days_dbl);

  r_geo_out[0] = r_p[0] - r_earth[0];
  r_geo_out[1] = r_p[1] - r_earth[1];
  r_geo_out[2] = r_p[2] - r_earth[2];
}

void propagate_moon_grcs_fine(int idx, int64_t dt_sec_i64,
                              const float moon_table[EPHEMERIS_SAMPLES][6],
                              double r_moon_out_au[3])
{
  // initial in AU / AU/day
  double r_km[3] = {
    (double)moon_table[idx][0] * AU_KM,
    (double)moon_table[idx][1] * AU_KM,
    (double)moon_table[idx][2] * AU_KM
  };
  double v_km_s[3] = {
    (double)moon_table[idx][3] * AU_KM / DAY_SEC,
    (double)moon_table[idx][4] * AU_KM / DAY_SEC,
    (double)moon_table[idx][5] * AU_KM / DAY_SEC
  };

  double dt_remaining = (double)dt_sec_i64;
  double sign = (dt_remaining >= 0.0) ? 1.0 : -1.0;
  dt_remaining = fabs(dt_remaining);

  while (dt_remaining > 0.0) {
    double h = MOON_DT_SEC;
    if (h > dt_remaining) h = dt_remaining;
    h *= sign;
    rk4_step_moon(r_km, v_km_s, h);
    dt_remaining -= fabs(h);
  }

  r_moon_out_au[0] = r_km[0] / AU_KM;
  r_moon_out_au[1] = r_km[1] / AU_KM;
  r_moon_out_au[2] = r_km[2] / AU_KM;
}

#include <math.h>
#include <stdint.h>

static inline double dot3(const double a[3], const double b[3]) {
  return a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
}
static inline double norm3(const double a[3]) {
  return sqrt(dot3(a,a));
}

// Stumpff
static inline double stumpff_C(double z) {
  if (z > 0.0) {
    double s = sqrt(z);
    return (1.0 - cos(s)) / z;
  } else if (z < 0.0) {
    double s = sqrt(-z);
    return (cosh(s) - 1.0) / (-z);
  } else {
    return 0.5;
  }
}

// Stumpff z
static inline double stumpff_S(double z) {
  if (z > 0.0) {
    double s = sqrt(z);
    double s3 = s*s*s;
    return (s - sin(s)) / s3;
  } else if (z < 0.0) {
    double s = sqrt(-z);
    double s3 = s*s*s;
    return (sinh(s) - s) / s3;
  } else {
    return 1.0 / 6.0;
  }
}


//// This method isnt discussed in the Instructables. I tested the RK4 approach on a more powerful chip when i wrote the article.
//// I also accidentally uploaded the fast, inaccurate Euler based Moon version in my demo but it just happened to be right when i was testing...
//// On the ATtiny3216 I switched to the rk4 and it slowed things down noticeably. This Kepler method (Im not 100% sure how all of it works) 
//// achieves about 98 to 99% accuracy for the Moon and runs much faster.


void propagate_moon_grcs_fine_kepler(int idx, int64_t dt_sec_i64,
                              const float moon_table[EPHEMERIS_SAMPLES][6],
                              double r_moon_out_au[3])
{
  const double dt = (double)dt_sec_i64;

  if (fabs(dt) < 1e-6) {
    r_moon_out_au[0] = (double)moon_table[idx][0];
    r_moon_out_au[1] = (double)moon_table[idx][1];
    r_moon_out_au[2] = (double)moon_table[idx][2];
    return;
  }

  double r0[3] = {
    (double)moon_table[idx][0] * AU_KM,
    (double)moon_table[idx][1] * AU_KM,
    (double)moon_table[idx][2] * AU_KM
  };
  double v0[3] = {
    (double)moon_table[idx][3] * AU_KM / DAY_SEC,
    (double)moon_table[idx][4] * AU_KM / DAY_SEC,
    (double)moon_table[idx][5] * AU_KM / DAY_SEC
  };

  const double mu = MU_EARTH_KM;

  const double r0_norm = norm3(r0);
  const double v0_norm = norm3(v0);
  const double vr0 = dot3(r0, v0) / r0_norm; // radial

  // 1/semimajor axis
  const double alpha = 2.0 / r0_norm - (v0_norm * v0_norm) / mu;

  // init guess
  const double sqrt_mu = sqrt(mu);
  double chi;
  if (alpha != 0.0) chi = sqrt_mu * fabs(alpha) * dt;
  else              chi = sqrt_mu * dt / r0_norm;

  for (int it = 0; it < 6; ++it) {
    const double z = alpha * chi * chi;
    const double C = stumpff_C(z);
    const double S = stumpff_S(z);

    const double chi2 = chi * chi;
    const double chi3 = chi2 * chi;

    const double F =
      (r0_norm * vr0 / sqrt_mu) * chi2 * C +
      (1.0 - alpha * r0_norm)   * chi3 * S +
      r0_norm * chi -
      sqrt_mu * dt;

    const double dF =
      (r0_norm * vr0 / sqrt_mu) * chi * (1.0 - z * S) +
      (1.0 - alpha * r0_norm)   * chi2 * C +
      r0_norm;

    if (fabs(dF) < 1e-14) break;
    chi -= F / dF;
  }

  const double z = alpha * chi * chi;
  const double C = stumpff_C(z);
  const double S = stumpff_S(z);

  const double f = 1.0 - (chi * chi / r0_norm) * C;
  const double g = dt  - (chi * chi * chi / sqrt_mu) * S;

  double r[3] = {
    f * r0[0] + g * v0[0],
    f * r0[1] + g * v0[1],
    f * r0[2] + g * v0[2]
  };

  r_moon_out_au[0] = r[0] / AU_KM;
  r_moon_out_au[1] = r[1] / AU_KM;
  r_moon_out_au[2] = r[2] / AU_KM;
}


static inline void rk4_step_moon(double r_km[3], double v_km_s[3], double dt_sec) {
  // k1
  double a1[3]; accel_earth_moon(r_km, a1);
  double k1_r[3] = { v_km_s[0], v_km_s[1], v_km_s[2] };
  double k1_v[3] = { a1[0], a1[1], a1[2] };

  // k2
  double r2[3] = { r_km[0] + 0.5*dt_sec*k1_r[0], r_km[1] + 0.5*dt_sec*k1_r[1], r_km[2] + 0.5*dt_sec*k1_r[2] };
  double v2[3] = { v_km_s[0] + 0.5*dt_sec*k1_v[0], v_km_s[1] + 0.5*dt_sec*k1_v[1], v_km_s[2] + 0.5*dt_sec*k1_v[2] };
  double a2[3]; accel_earth_moon(r2, a2);
  double k2_r[3] = { v2[0], v2[1], v2[2] };
  double k2_v[3] = { a2[0], a2[1], a2[2] };

  // k3
  double r3[3] = { r_km[0] + 0.5*dt_sec*k2_r[0], r_km[1] + 0.5*dt_sec*k2_r[1], r_km[2] + 0.5*dt_sec*k2_r[2] };
  double v3[3] = { v_km_s[0] + 0.5*dt_sec*k2_v[0], v_km_s[1] + 0.5*dt_sec*k2_v[1], v_km_s[2] + 0.5*dt_sec*k2_v[2] };
  double a3[3]; accel_earth_moon(r3, a3);
  double k3_r[3] = { v3[0], v3[1], v3[2] };
  double k3_v[3] = { a3[0], a3[1], a3[2] };

  // k4
  double r4[3] = { r_km[0] + dt_sec*k3_r[0], r_km[1] + dt_sec*k3_r[1], r_km[2] + dt_sec*k3_r[2] };
  double v4[3] = { v_km_s[0] + dt_sec*k3_v[0], v_km_s[1] + dt_sec*k3_v[1], v_km_s[2] + dt_sec*k3_v[2] };
  double a4[3]; accel_earth_moon(r4, a4);
  double k4_r[3] = { v4[0], v4[1], v4[2] };
  double k4_v[3] = { a4[0], a4[1], a4[2] };

  // combine
  r_km[0] += (dt_sec / 6.0) * (k1_r[0] + 2.0*k2_r[0] + 2.0*k3_r[0] + k4_r[0]);
  r_km[1] += (dt_sec / 6.0) * (k1_r[1] + 2.0*k2_r[1] + 2.0*k3_r[1] + k4_r[1]);
  r_km[2] += (dt_sec / 6.0) * (k1_r[2] + 2.0*k2_r[2] + 2.0*k3_r[2] + k4_r[2]);

  v_km_s[0] += (dt_sec / 6.0) * (k1_v[0] + 2.0*k2_v[0] + 2.0*k3_v[0] + k4_v[0]);
  v_km_s[1] += (dt_sec / 6.0) * (k1_v[1] + 2.0*k2_v[1] + 2.0*k3_v[1] + k4_v[1]);
  v_km_s[2] += (dt_sec / 6.0) * (k1_v[2] + 2.0*k2_v[2] + 2.0*k3_v[2] + k4_v[2]);
}

static inline void accel_earth_moon(const double r_km[3], double a_km[3]) {
  double d2 = r_km[0]*r_km[0] + r_km[1]*r_km[1] + r_km[2]*r_km[2];
  double d  = sqrt(d2);
  double denom = (d*d*d) + 1e-12;
  double k = -MU_EARTH_KM / denom;
  a_km[0] = k * r_km[0];
  a_km[1] = k * r_km[1];
  a_km[2] = k * r_km[2];
}

static inline bool normalize3(double v[3]) {
  double n2 = v[0]*v[0] + v[1]*v[1] + v[2]*v[2];
  if (n2 < 1e-18) return false;
  double inv = 1.0 / sqrt(n2);
  v[0] *= inv;
  v[1] *= inv;
  v[2] *= inv;
  return true;
}

/////////// board stuf
//////////////////////////////////////////////////////////////////////////

#include <Wire.h>
#include <tinyNeoPixel.h>
#include "SAMM10Q.h"
GPSData_t gpsData; 

#define LED_PIN     16
#define LED_COUNT   9
#define LATCH_PIN   14
#define OFF_BUTTON  7

#define RED_R   10
#define RED_G   0
#define RED_B   0


tinyNeoPixel strip = tinyNeoPixel(LED_COUNT, LED_PIN, NEO_GRB + NEO_KHZ800);

// flash
bool flashState   = false;
unsigned long lastFlash = 0;
const unsigned long FLASH_INTERVAL = 500;   // ms

/////////// led stuf
//////////////////////////////////////////////////////////////////////////

void clearLEDs() {for (int i = 0; i < LED_COUNT; i++) {strip.setPixelColor(i, 0, 0, 0);}}
void setLEDOverhead(int i, bool overhead, int r, int g, int b) {
  if (overhead) strip.setPixelColor(i, r, g, b);
  else          strip.setPixelColor(i, 0,   0,   0);
}

void updateLEDs() {

  clearLEDs();
  if (!gpsData.dataReceived || gpsData.numSV < 3) {
    if (flashState) {for (int i = 0; i < LED_COUNT; i++) {strip.setPixelColor(i, RED_R, RED_G, RED_B);}}    
    return;
  }

  // uint64_t unix_time = (uint64_t)gpsData.unixTime;
  int64_t unix_time = (int64_t)gpsData.unixTime;

  // for testing with python
  // const uint64_t SECONDS_212_DAYS = 60ULL * 60ULL * 24ULL * 212ULL;
  // unix_time += SECONDS_212_DAYS;

  double currentLat = gpsData.lat / 10000000.0;
  double currentLon = gpsData.lon / 10000000.0;

  int idx = closest_time_index(time_table, 8, unix_time);

  int64_t t0 = time_table[idx];
  int64_t dt_sec_i64 = (int64_t)unix_time - t0;
  double dt_days = (double)dt_sec_i64 / SECONDS_PER_DAY;


  double earth_theta = earth_rotation_angle_from_unix(unix_time);

  // Propagate earth
  double r_e[3] = { (double)sun_sv[idx][0], (double)sun_sv[idx][1], (double)sun_sv[idx][2] };
  double v_e[3] = { (double)sun_sv[idx][3], (double)sun_sv[idx][4], (double)sun_sv[idx][5] };
  euler_step(r_e, v_e, dt_days);

  double r_geo[3], r_ecef[3];


  propagate_planet_grcs(idx, dt_days, mercury_sv, r_geo, r_e);
  normalize3(r_geo); // normalize r_geo, its in units of AU it's small, this was a hard bug to find....
  eci_to_ecef_target_timestamp(r_geo, r_ecef, earth_theta);
  setLEDOverhead(0, flashState * is_visible_est(r_ecef, currentLat, currentLon),
                 MERCURY_R, MERCURY_G, MERCURY_B);

  propagate_planet_grcs(idx, dt_days, venus_sv, r_geo, r_e);
  normalize3(r_geo);
  eci_to_ecef_target_timestamp(r_geo, r_ecef, earth_theta);
  setLEDOverhead(1, flashState * is_visible_est(r_ecef, currentLat, currentLon),
                 VENUS_R, VENUS_G, VENUS_B);

  propagate_planet_grcs(idx, dt_days, mars_sv, r_geo, r_e);
  normalize3(r_geo);
  eci_to_ecef_target_timestamp(r_geo, r_ecef, earth_theta);
  setLEDOverhead(2, flashState * is_visible_est(r_ecef, currentLat, currentLon),
                 MARS_R, MARS_G, MARS_B);

  propagate_planet_grcs(idx, dt_days, jupiter_barycenter_sv, r_geo, r_e);
  normalize3(r_geo);
  eci_to_ecef_target_timestamp(r_geo, r_ecef, earth_theta);
  setLEDOverhead(3, flashState * is_visible_est(r_ecef, currentLat, currentLon),
                 JUPITER_R, JUPITER_G, JUPITER_B);

  propagate_planet_grcs(idx, dt_days, saturn_barycenter_sv, r_geo, r_e);
  normalize3(r_geo);
  eci_to_ecef_target_timestamp(r_geo, r_ecef, earth_theta);
  setLEDOverhead(4, flashState * is_visible_est(r_ecef, currentLat, currentLon),
                 SATURN_R, SATURN_G, SATURN_B);

  propagate_planet_grcs(idx, dt_days, uranus_barycenter_sv, r_geo, r_e);
  normalize3(r_geo);
  eci_to_ecef_target_timestamp(r_geo, r_ecef, earth_theta);
  setLEDOverhead(5, flashState * is_visible_est(r_ecef, currentLat, currentLon),
                 URANUS_R, URANUS_G, URANUS_B);

  propagate_planet_grcs(idx, dt_days, neptune_barycenter_sv, r_geo, r_e);
  normalize3(r_geo);
  eci_to_ecef_target_timestamp(r_geo, r_ecef, earth_theta);
  setLEDOverhead(6, flashState * is_visible_est(r_ecef, currentLat, currentLon),
                 NEPTUNE_R, NEPTUNE_G, NEPTUNE_B);

  propagate_planet_grcs(idx, dt_days, pluto_barycenter_sv, r_geo, r_e);
  normalize3(r_geo);
  eci_to_ecef_target_timestamp(r_geo, r_ecef, earth_theta);
  setLEDOverhead(7, flashState * is_visible_est(r_ecef, currentLat, currentLon),
                 PLUTO_R, PLUTO_G, PLUTO_B);

  propagate_moon_grcs_fine_kepler(idx, dt_sec_i64, moon_sv, r_geo);
  normalize3(r_geo);
  eci_to_ecef_target_timestamp(r_geo, r_ecef, earth_theta);
  setLEDOverhead(8, flashState * is_visible_est(r_ecef, currentLat, currentLon),
                 MOON_R, MOON_G, MOON_B);


  // propagate_moon_grcs_fine(idx, dt_sec_i64, moon_sv, r_geo);
  // normalize3(r_geo);
  // eci_to_ecef_target_timestamp(r_geo, r_ecef, earth_theta);
  // setLEDOverhead(8, flashState * is_visible_est(r_ecef, currentLat, currentLon),
  //                MOON_R, MOON_G, MOON_B);

  strip.show();
}

void hackerStartScreen() {
  const int loops = 20;
  const int frameDelay = 70;

  strip.clear();
  strip.show();

  for (int t = 0; t < loops; t++) {
    for (int i = 0; i < LED_COUNT; i++) {
      int g = random(5, 15);
      strip.setPixelColor(i, 0, g, 0);
    }

    int head = t % LED_COUNT;
    strip.setPixelColor(head, 0, 24, 0);

    strip.show();
    delay(frameDelay);

    if (t % 5 == 0) {
      for (int i = 0; i < LED_COUNT; i++) {
        strip.setPixelColor(
          i,
          random(5, 20),
          random(5, 20),
          random(5, 20)
        );
      }
      strip.show();
      delay(50);
    }

    int pulse = (t % 6) * 4;
    strip.setPixelColor(4, pulse, 0, pulse);
    strip.show();
    delay(frameDelay);
  }

  for (int k = 0; k < 2; k++) {
    for (int i = 0; i < LED_COUNT; i++) {
      strip.setPixelColor(i, 0, 20, 5);
    }
    strip.show();
    delay(150);

    strip.clear();
    strip.show();
    delay(150);
  }
  strip.clear();
  strip.show();
}



void setup() {


  pinMode(LATCH_PIN, OUTPUT);
  digitalWrite(LATCH_PIN, HIGH); 

  strip.begin();
  strip.show();

  Wire.begin();
  delay(1000);

  gpsData.dataReceived = false;
  gpsData.numSV = 0;

  // wit for GPS
  int attempts = 0;
  while (!gpsFound() && attempts < 10) {
    delay(300);
    attempts++;
  }


  if (attempts >= 10) {
    for (int i = 0; i < LED_COUNT; i++) {
      strip.setPixelColor(i, RED_R, RED_G, RED_B);
    }
    strip.show();
    while (1);
  }

  configureNAVPVT();
  delay(2000);

  hackerStartScreen();


}

void loop() {


  if (digitalRead(OFF_BUTTON) == LOW) {
    delay(50); 
    if (digitalRead(OFF_BUTTON) == LOW) {
      digitalWrite(LATCH_PIN, LOW);
      while (true) {
        // wait for power to cut
      }
    }
  }


  uint16_t avail = getBytesAvailable();

  if (avail > 8) {
    readUBXMessage();
  }

  if (millis() - lastFlash >= FLASH_INTERVAL) {
    lastFlash = millis();
    flashState = !flashState;
    updateLEDs();
    strip.show();
  }


  delay(50);
}