path: root/app/gps.c

#include "project.h"

#define PPS (GPIO7)
#define PPS_PORT GPIOC

#define NRESET (GPIO8)
#define NRESET_PORT GPIOC

#define UBX_BUF_LEN 256

#define TIMEOUT 4000


unsigned gps_sats_searching;
unsigned gps_sats_inop;
unsigned gps_sats_locked;
unsigned gps_sats_with_e;
int gps_initting = 1;

static int chatty_gps = 1;

static EPOCH next_sec;

static int ubx_ack = 0;
static int ubx_nack = 0;
static int ubx_ack_xfer = 0;

static int current_ref_hz = 1;

static int gps_locked;
static int gps_happy;

uint64_t gps_last_happy;

static  char fix, fix2;
static int32_t freq = 0;

//static const int fish[] = { 1, 2, 3, 4, 5, 6, 7 };

static Event_ring gps_ring;

char gps_info[60];

static int  ubx_cfg_tmode (uint32_t time_mode, int32_t x, int32_t y, int32_t z,
                           uint32_t var, uint32_t min_dur, uint32_t max_var);

void exti9_5_isr (void)
{
  uint32_t now = HW_CLOCK_REG;
  int v;

  v = !!gpio_get (PPS_PORT, PPS);

  nvic_disable_irq (NVIC_EXTI9_5_IRQ);
  exti_reset_request (EXTI7);

  gps_ring.events[gps_ring.tx_ptr].when = now;
  gps_ring.events[gps_ring.tx_ptr].ptp = ptp_get();
  gps_ring.events[gps_ring.tx_ptr].value = v;
  gps_ring.tx_ptr = (gps_ring.tx_ptr + 1) & ERING_MASK;

  if (v) sysclk_event();

  nvic_enable_irq (NVIC_EXTI9_5_IRQ);

}



static inline int
ubx_recv_byte (uint8_t *v)
{
  return !ring_read_byte (&usart1_rx_ring, v);
}

static inline void
ubx_send_byte (uint8_t v)
{
  usart1_queue (v);
}

#define ALMANAC_SIZE

static int ubx_recv_almanac (uint8_t *ptr, unsigned len)
{
  uint32_t sv, week;
  uint32_t almanac[8];

  ptr += ubx_get_u32 (ptr, &sv);
  len -= 4;
  ptr += ubx_get_u32 (ptr, &week);
  len -= 4;

  if (len > sizeof (almanac))
    len = sizeof (almanac);

  bzero (almanac, sizeof (almanac));
  memcpy (almanac, ptr, len);

  printf ("    Almanac: %2d %d %06x %06x %06x %06x  %06x %06x %06x %06x\r\n",
          (int) sv, (int) week,
          (unsigned) almanac[0],
          (unsigned) almanac[1],
          (unsigned) almanac[2],
          (unsigned) almanac[3],
          (unsigned) almanac[4],
          (unsigned) almanac[5],
          (unsigned) almanac[6],
          (unsigned) almanac[7]);

  return 0;
}


static int ubx_recv_aid_eph (uint8_t *ptr, unsigned len)
{
  unsigned i;
  uint32_t sv;

  ptr += ubx_get_u32 (ptr, &sv);
  len -= 4;

  if ((sv<1) ||( sv>32)) return 0;

  i=sv-1;
  

  ptr += ubx_get_u32 (ptr, &ephemeris[i].how);
  len -= 4;

  printf("Received %d bytes of ephemeris for SV%d\r\n",len,(int) sv);

  if ((!len) ||( !ephemeris[i].how)) return 0 ;

  memcpy(&ephemeris[i].sf1,ptr,32);
  len-=32;
  ptr+=32;
  memcpy(&ephemeris[i].sf2,ptr,32);
  len-=32;
  ptr+=32;
  memcpy(&ephemeris[i].sf3,ptr,32);

  return 0;
}


double gps_lat, gps_lon;

char gps_pos[128];

static int
ubx_recv_nav_posllh (uint8_t *ptr, unsigned len)
{
  uint32_t d;
  int32_t ilat, ilon, i;

  if ((!ptr) || (len != 28))
    return -1;

  ptr += ubx_get_u32 (ptr, &d); //TOW
  ptr += ubx_get_i32 (ptr, &ilon);
  ptr += ubx_get_i32 (ptr, &ilat);
  ptr += ubx_get_i32 (ptr, &i); //geoid height
  ptr += ubx_get_i32 (ptr, &i); //msl height
  ptr += ubx_get_u32 (ptr, &d); //hacc
  ptr += ubx_get_u32 (ptr, &d); //vacc


  gps_lat = 1.e-7 * (double) ilat;
  gps_lon = 1.e-7 * (double) ilon;


  printf ("POS: %.4f %.4f\r\n", gps_lat, gps_lon);

  snprintf (gps_pos, sizeof (gps_pos) - 1, "location: %.4f %.4f", gps_lat, gps_lon);

  return 0;
}


static int
ubx_recv_nav_status (uint8_t *ptr, unsigned len)
{
  uint8_t gps_fix, flags, fix_stat, flags2;
  uint32_t d;
  static  unsigned kick_off_survey_mode = 40;


  if ((!ptr) || (len != 16))
    return -1;

  ptr += ubx_get_u32 (ptr, &d); //TOW
  ptr += ubx_get_u8 (ptr, &gps_fix); //fix type
  ptr += ubx_get_u8 (ptr, &flags); //fix type
  ptr += ubx_get_u8 (ptr, &fix_stat);
  ptr += ubx_get_u8 (ptr, &flags2);


  switch (gps_fix) {
  case 0:
  case 1:
    fix = '-';
    gps_locked = 0;
    break;

  case 2:
    fix = '2';
    gps_locked = 0;
    break;

  case 3:
    fix = 'L';
    gps_locked = 1;
    break;

  case 4:
    fix = 'R';
    gps_locked = 0;
    break;

  case 5:
    fix = 'T';
    gps_locked = 1;
    kick_off_survey_mode = 0;
    break;

  default:
    fix = '?';
    gps_locked = 0;
  }

  switch (flags & 3) {
  case 0:
  case 2:
    fix2 = '!';
    break;

  case 1:
    fix2 = '-';
    break;

  case 3:
    fix2 = 'D';

    if (gps_locked == 1)
      gps_locked = 2;

    break;

  default:
    fix2 = '?';
  }

  display_report_fix (fix, fix2);


  //  printf ("fix: %c%c happy %d Fix %02x flags %02x fix_stat %02x flags2 %02x \r\n",fix,fix2,gps_happy, gps_fix, flags,fix_stat,flags2);


  if ((gps_locked) && (gps_happy < 10000))
    gps_happy++;
  else
    gps_happy = 0;


  if (kick_off_survey_mode > 0) {
    kick_off_survey_mode--;

    if (!kick_off_survey_mode)  {
      printf ("GPS no time fix - kicking off survey mode\n");
      ubx_cfg_tmode (1, 0, 0, 0, 0, 3600, 3600); // 1 hour and 6cm
    }
  }

  return 0;
}


static int
ubx_recv_clock_stats (uint8_t *ptr, unsigned len)
{
  //char buf[40];
  int32_t drift;
  uint32_t d;

  if ((!ptr) || (len != 20))
    return -1;

  ptr += ubx_get_u32 (ptr, &d); //TOW
  ptr += ubx_get_u32 (ptr, &d); //bias
  ptr += ubx_get_i32 (ptr, &drift); //drift
  ptr += ubx_get_u32 (ptr, &d); //time acc estimate
  ptr += ubx_get_i32 (ptr, &freq); //freq acc estimate

  //  printf ("TCXO %+8dE-12\r\n", (int) freq);
#if 0
  sprintf (buf, "TCXO %+8dE-12", (int) freq);
  lcd_erase_line (18, 0);
  lcd_write (buf, 0, 0);
#endif
  return 0;

}

int gps_utc_diff;
int gps_wday;

static int
ubx_recv_utc (uint8_t *ptr, unsigned len)
{
  int32_t nano;
  uint32_t acc;
  uint32_t tow;
  int tod, utc_tod;
  uint16_t year;
  uint8_t hour, min, sec, day, month, valid;
  //  char buf[40];

  if ((!ptr) || (len != 20))
    return -1;

  ptr += ubx_get_u32 (ptr, &tow); //TOW
  ptr += ubx_get_u32 (ptr, &acc); //bias
  ptr += ubx_get_i32 (ptr, &nano);
  ptr += ubx_get_u16 (ptr, &year);
  ptr += ubx_get_u8 (ptr, &month);
  ptr += ubx_get_u8 (ptr, &day);
  ptr += ubx_get_u8 (ptr, &hour);
  ptr += ubx_get_u8 (ptr, &min);
  ptr += ubx_get_u8 (ptr, &sec);
  ptr += ubx_get_u8 (ptr, &valid);

#if 0
  printf ("GPS META-DATA %04d-%02d-%02d %02d:%02d:%02d V:%02x Fix:%c%c TXCO  %+8dE-12\r\n",
          (int) year,
          (int) month,
          (int) day,
          (int) hour,
          (int) min,
          (int) sec,
          (unsigde) valid,
          fix, fix2, (int) freq);
#endif

  sprintf (gps_info, " %04d-%02d-%02d %02d:%02d:%02d V:%02x Fix:%c%c TXCO  %+8dE-12",

           (int)year,
           (int)month,
           (int)day,
           (int)hour,
           (int)min,
           (int)sec,
           (unsigned) valid, fix, fix2, (int) freq);



  if (gps_happy > 3) {
    UTC u;
    EPOCH gps_time;
    uint64_t abs, ptp;

    u.jday = 0;
    u.year = year;
    u.month = month;
    u.mday = day;
    u.hour = hour;
    u.minute = min;
    u.second = sec;
    u.nanosecond = 0;



    gps_time = time_utc_to_epoch (u);


    next_sec = gps_time;

    abs = ref_get();
    ptp = ptp_get();

    gps_last_happy = make_happy (abs, 180);

    pll_set_offset (gps_time, abs, ptp);

  } else
    printf ("!GPS %s\r\n", gps_info);

#define SECONDS_IN_DAY  (24*60*60)

  utc_tod = (((hour * 60) + min) * 60) + sec;
  tow /= 1000;
  tod = tow % SECONDS_IN_DAY;
  gps_wday = tow / SECONDS_IN_DAY;

  utc_tod += 2 * SECONDS_IN_DAY;
  utc_tod -= tod;

  while (utc_tod >= (SECONDS_IN_DAY / 2)) utc_tod -= SECONDS_IN_DAY;

  gps_utc_diff = utc_tod;

#if 0
  sprintf (buf, "%+6dE-12 %02d%02d%02d", (int) freq,
           (int) hour, (int)min, (int) sec);
  lcd_erase_line (18, 0);
  lcd_write (buf, 0, 0);
#endif

  return 0;
}



static int ubx_recv_nav_sbas (uint8_t *ptr, unsigned len)
{
  uint32_t d4;
  uint8_t prn, mode, service;
  int8_t sys;
  uint8_t  n;

  if (!chatty_gps) return 0 ;

  ptr += ubx_get_u32 (ptr, &d4);
  ptr += ubx_get_u8 (ptr, &prn);
  ptr += ubx_get_u8 (ptr, &mode);
  ptr += ubx_get_i8 (ptr, &sys);
  ptr += ubx_get_u8 (ptr, &service);

  ptr += ubx_get_u8 (ptr, &n);
  ptr += 3;

  printf ("GPS SBAS PRN:%d M:%d S:%d SVC:%02x SVS:", prn, mode, sys, service);

  while (n--) {
    ptr += ubx_get_u8 (ptr, &prn);
    ptr += 11;
    printf (" %d", prn);
  }

  printf ("\r\n");
  return 0;
}

char gps_svinfo[4096];

static int ubx_recv_nav_svinfo (uint8_t *ptr, unsigned len)
{
  uint32_t d4;
  uint16_t d2;
  uint8_t ch, quality, cno, svid;
  uint8_t  n, flags, cflags;
  const char *st;

  PTB_INIT (gps_svinfo);


  ptr += ubx_get_u32 (ptr, &d4);
  ptr += ubx_get_u8 (ptr, &n);
  ptr += ubx_get_u8 (ptr, &flags);
  ptr += ubx_get_u16 (ptr, &d2);

  gps_sats_searching = gps_sats_inop = gps_sats_locked = gps_sats_with_e = 0;

  while (n--) {
    ptr += ubx_get_u8 (ptr, &ch);
    ptr += ubx_get_u8 (ptr, &svid);
    ptr += ubx_get_u8 (ptr, &cflags);
    ptr += ubx_get_u8 (ptr, &quality);
    ptr += ubx_get_u8 (ptr, &cno);
    ptr += 7;


    switch (quality) {
    case 0:
      st = "idle";
      break;

    case 1:
      st = "search";
      gps_sats_searching++;
      break;

    case 2:
      st = "acq";
      gps_sats_searching++;
      break;

    case 3:
      st = "inop";
      gps_sats_inop++;
      break;

    case 4:
      st = "lock";
      gps_sats_locked++;

      if (cflags & 0x04)
        gps_sats_with_e++;

      break;

    case 5:
    case 6:
    case 7:
      st = "good";
      gps_sats_locked++;

      if (cflags & 0x04)
        gps_sats_with_e++;

      break;

    default:
      st = "?";
    }


    if (chatty_gps)
      printf ("GPS    SVID:%3d C/N:%3d %c%c%c %s\r\n",
              svid, cno,
              (cflags & 0x01) ? 'U' : '-',
              (cflags & 0x02) ? 'D' : '-',
              (cflags & 0x04) ? 'E' : '-',
              st);


    PTB_ADD ("GPS    SVID:%3d C/N:%3d %c%c%c %s\n",
             svid, cno,
             (cflags & 0x01) ? 'U' : '-',
             (cflags & 0x02) ? 'D' : '-',
             (cflags & 0x04) ? 'E' : '-',
             st);

  }


  return 0;
}


static int ubx_recv_rinex (uint8_t *payload, unsigned len)
{

  printf ("Rinex\r\n");
  return 0;
}

char gps_svin_info[128];

static int ubx_recv_tim_svin (uint8_t *ptr, unsigned len)
{
  uint8_t valid, active;
  uint32_t dur, obs, var;
  int32_t x, y, z;

  ptr += ubx_get_u32 (ptr, &dur);
  ptr += ubx_get_i32 (ptr, &x);
  ptr += ubx_get_i32 (ptr, &y);
  ptr += ubx_get_i32 (ptr, &z);
  ptr += ubx_get_u32 (ptr, &var);
  ptr += ubx_get_u32 (ptr, &obs);
  ptr += ubx_get_u8 (ptr, &valid);
  ptr += ubx_get_u8 (ptr, &active);

  printf ("TIM-SVIN dur %u var %u obs %u valid %02x active %02x\r\n",
          (unsigned) dur, (unsigned) var, (unsigned) obs, valid, active);

  snprintf (gps_svin_info, sizeof (gps_svin_info) - 1, "TIM-SVIN dur %u var %u obs %u valid %02x active %02x",
            (unsigned) dur, (unsigned) var, (unsigned) obs, valid, active);

  display_report_svin (valid, active);

  return 0;
}



static void
ubx_recv (uint8_t class, uint8_t id, uint8_t *payload, unsigned len)
{
  //printf ("RX> %02x.%02x (%d bytes)\r\n", class, id, len);

  switch (class) {
  case 1:
    switch (id) {
    case 0x2:
      ubx_recv_nav_posllh (payload, len);
      break;

    case 0x3:
      ubx_recv_nav_status (payload, len);
      break;

    case 0x21:
      ubx_recv_utc (payload, len);
      break;

    case 0x22:
      ubx_recv_clock_stats (payload, len);
      break;

    case 0x30:
      ubx_recv_nav_svinfo (payload, len);
      break;

    case 0x32:
      ubx_recv_nav_sbas (payload, len);
      break;


    default:
      printf ("RX> %02x.%02x (%d bytes)\r\n", class, id, len);
      hexdump (payload, len);
    }

    break;

  case 2:
    switch (id) {
    case 0x10:
      ubx_recv_rinex (payload, len);

      break;
    }

    break;

  case 4:
    switch (id) {
    case 0x4:
      break;

    default:
      printf ("RX> %02x.%02x (%d bytes)\r\n", class, id, len);
      hexdump (payload, len);
    }

    break;
  case 0xd:
    switch (id) {
    case 0x4:
      ubx_recv_tim_svin (payload, len);

      break;

    default:
      printf ("RX> %02x.%02x (%d bytes)\r\n", class, id, len);
      hexdump (payload, len);
    }

    break;





  case 5:
    ubx_ack++;

    if (!id) ubx_nack++;

    printf (" %s for %02x.%02x\r\n", id ? "ACK" : "NAK", payload[0],
            payload[1]);

    break;

  case 0x0b:
    switch (id) {
    case 0x30:
      ubx_recv_almanac (payload, len);
      break;
	case 0x31:
 	ubx_recv_aid_eph(payload,len);
	break;

    case 0x50:
      printf ("xfer ack\r\n");
      ubx_ack_xfer++;
      break;

    default:
      printf ("RX> %02x.%02x (%d bytes)\r\n", class, id, len);
      hexdump (payload, len);
    }

    break;

  default:
    printf ("RX> %02x.%02x (%d bytes)\r\n", class, id, len);

    if (class != 0x03)
      hexdump (payload, len);
  }


}


typedef enum {
  UBX_SM_LOST = 0,
  UBX_SM_S2,
  UBX_SM_C,
  UBX_SM_I,
  UBX_SM_L1,
  UBX_SM_L2,
  UBX_SM_DATA,
  UBX_SM_CKA,
  UBX_SM_CKB
} ubx_sm_t;




static uint8_t *
ubx_dispatch_search (int s_class, int s_id, unsigned *len_ptr)
{
  static uint8_t buf[UBX_BUF_LEN];
  static ubx_sm_t sm = UBX_SM_LOST;
  static uint8_t class, id;
  static unsigned ptr;
  static unsigned len;
  static uint8_t ck_a, ck_b;


  void ubx_ck (uint8_t v) {
    ck_a += v;
    ck_b = ck_b + ck_a;
  }


  uint8_t c;

  while (ubx_recv_byte (&c)) {

    switch (sm) {
    case UBX_SM_LOST:
      if (c == 0xb5)
        sm = UBX_SM_S2;

      break;

    case UBX_SM_S2:
      if (c == 0x62)
        sm = UBX_SM_C;
      else
        sm = UBX_SM_LOST;

      break;

    case UBX_SM_C:
      ck_a = ck_b = class = c;
      sm = UBX_SM_I;
      break;

    case UBX_SM_I:
      ubx_ck (c);
      id = c;
      sm = UBX_SM_L1;
      break;

    case UBX_SM_L1:
      ubx_ck (c);
      len = c;
      sm = UBX_SM_L2;
      break;

    case UBX_SM_L2:
      ubx_ck (c);
      len |= c << 8;

      ptr = 0;

      if (len)
        sm = UBX_SM_DATA;
      else
        sm = UBX_SM_CKA;

      break;

    case UBX_SM_DATA:
      ubx_ck (c);

      if (ptr < UBX_BUF_LEN)
        buf[ptr] = c;

      ptr++;

      if (ptr == len)
        sm = UBX_SM_CKA;

      break;

    case UBX_SM_CKA:
      if (c == ck_a)
        sm = UBX_SM_CKB;
      else
        sm = UBX_SM_LOST;

      break;

    case UBX_SM_CKB:
      sm = UBX_SM_LOST;

      if (c != ck_b)
        break;

      ubx_recv (class, id, buf, len);

      if ((class == s_class) && (id == s_id)) {
        if (len_ptr)
          *len_ptr = len;

        return buf;
      }

      break;
    }

  }

  return NULL;
}
static void gps_pps_dispatch (void)
{
  //char buf[80];
  uint32_t now;
  uint64_t abs;
  uint64_t ptp;
  int v;
  //EPOCH e;
  //UTC u;

  if (gps_ring.rx_ptr == gps_ring.tx_ptr) return;

  v = gps_ring.events[gps_ring.rx_ptr].value;
  now = gps_ring.events[gps_ring.rx_ptr].when;
  ptp = gps_ring.events[gps_ring.rx_ptr].ptp;

  if (gps_happy)
    led3_set (v);


  gps_ring.rx_ptr = (gps_ring.rx_ptr + 1) & ERING_MASK;


  if (!v) return;

  abs = ref_extend (now);



  if (gps_happy > 30) {
    //chatty_gps = 0;
    pll_dispatch (gps_last_happy, abs, ptp, "GPS");
  }

  //e = ref_decompose (abs);


  //u = time_epoch_to_utc (e);
  //time_print_epoch ("GPS  : ", e, gps_info);

  report_time ("GPS", next_sec, abs, gps_info);

}


static void
ubx_send (uint8_t class, uint8_t id, const void *_payload, unsigned len)
{
  uint8_t ck_a = 0, ck_b = 0;
  uint8_t *payload = (uint8_t *) _payload;

  void ubx_send_byte_ck (uint8_t v) {
    ubx_send_byte (v);
    ck_a += v;
    ck_b = ck_b + ck_a;
  }


  ubx_send_byte (0xb5);
  ubx_send_byte (0x62);

  ubx_send_byte_ck (class);
  ubx_send_byte_ck (id);
  ubx_send_byte_ck (len & 0xff);
  ubx_send_byte_ck (len >> 8);

  while (len--)
    ubx_send_byte_ck (* (payload++));

  ubx_send_byte (ck_a);
  ubx_send_byte (ck_b);

  printf ("TX> %02x.%02x\r\n", class, id);

}


void
gps_dispatch (void)
{
static uint32_t last_eph;
uint32_t t ;

  ubx_dispatch_search (-1, -1, NULL);
  gps_pps_dispatch();



t=ticks-last_eph;
if (t>60000)  {
	printf("Requesting ephemeris data\r\n");
        gps_dump_eph();
	last_eph=ticks;
}

}


static int
ubx_handshake (uint8_t class, uint8_t id, const void *payload, unsigned len)
{
  uint32_t timeout = 4000;
  unsigned tries = 4;

  ubx_ack = 0;
  ubx_nack = 0;
  ubx_send (class, id, payload, len);

  while (!ubx_ack) {

    if (!timeout) {
      printf ("GPS timeout resending packet\r\n");

      if (!tries) {
        printf ("GPS 3 timesout - resetting system\r\n");
        usart2_drain();
        scb_reset_system();
      }

      usart1_drain();
      ubx_send (class, id, payload, len);
      timeout = 4000;
      tries--;
    }

    timeout--;


    gps_dispatch();
    delay_ms (1);
  }

  return !!ubx_nack;
}

static int
ubx_handshake_xfer (uint8_t class, uint8_t id, const void *payload,
                    unsigned len)
{
  uint32_t timeout;

  ubx_ack_xfer = 0;
  //  usart1_drain();
  ubx_send (class, id, payload, len);



  timeout = ticks + TIMEOUT;

  while (!ubx_ack_xfer) {

    if (ticks > timeout) {
      printf ("GPS timeout resending packet\r\n");
      //      usart1_drain();
      ubx_send (class, id, payload, len);
      timeout = ticks + TIMEOUT;
    }




    gps_dispatch();
  }

  return 0;
}




static uint8_t *
ubx_fetch (uint8_t class, uint8_t id, void *payload, unsigned len,
           unsigned *len_ptr)
{
  uint8_t *ret;

  while (!ring_empty (&usart1_tx_ring) || !ring_empty (&usart1_rx_ring))
    gps_dispatch();

  ubx_send (class, id, payload, len);

  while (! (ret = ubx_dispatch_search (class, id, len_ptr)));


  return ret;
}

static int ubx_set_nav_rate (uint16_t ms)
{
  uint8_t buf[6], *ptr;

  ptr = buf;

  ptr += ubx_put_u16 (ptr, ms);
  ptr += ubx_put_u16 (ptr, 1); /*1:1*/
  ptr += ubx_put_u16 (ptr, 0); /*UTC*/

  return ubx_handshake (0x06, 0x08, buf, (unsigned) (ptr - buf));
}

static int
ubx_set_message_rate_port1 (uint8_t class, uint8_t id, uint8_t rate)
{
  uint8_t buf[8], *ptr;
  int ret;

  ptr = buf;

  ptr += ubx_put_u8 (ptr, class);
  ptr += ubx_put_u8 (ptr, id);  //reserved
  ptr += ubx_put_u8 (ptr, 0);   //nothing on port i2c
  ptr += ubx_put_u8 (ptr, rate); //rate on uart
  ptr += ubx_put_u8 (ptr, 0);   //nothing on port 2
  ptr += ubx_put_u8 (ptr, 0);   //nothing on usb
  ptr += ubx_put_u8 (ptr, 0);   //nothing on spi
  ptr += ubx_put_u8 (ptr, 0);   //nothing on port 5

  ret = ubx_handshake (0x06, 0x01, buf, (unsigned) (ptr - buf));

  printf ("GPS set message rate for %02x.%02x %s\r\n", class, id, ret ? "Failed" : "Succeeded");
  return ret;
}


static int
ubx_cfg_rst (uint16_t flags)
{
  uint8_t buf[8], *ptr;

  ptr = buf;

  ptr += ubx_put_u16 (ptr, flags); //Flags
  ptr += ubx_put_u8 (ptr, 0x4); //Hardware reset after shutdown
  ptr += ubx_put_u8 (ptr, 0);   //reserved

  ubx_send (0x06, 0x04, buf, (unsigned) (ptr - buf));
  return 0;
}

static int
ubx_cfg_poll (uint8_t class, uint8_t id)
{
  return ubx_handshake (class, id, NULL, 0);
}

static int  ubx_cfg_sbas (uint8_t mode, uint8_t usage, uint8_t channels)
{
  uint8_t buf[8], *ptr;

  ptr = buf;

  ptr += ubx_put_u8 (ptr, mode);
  ptr += ubx_put_u8 (ptr, usage);
  ptr += ubx_put_u8 (ptr, channels);
#if 0
  ptr += ubx_put_u8 (ptr, 0x40 /*PRN158*/);
  ptr += ubx_put_u32 (ptr, /* 0x1 PRN 120 |  */ 0x8 /*PRN 123*/ | /* 0x40  PRN 126  | */   0x10000 /*PRN136*/);
#else
#if 1
  ptr += ubx_put_u8 (ptr, 0x7f);
  ptr += ubx_put_u32 (ptr, 0xffffffffU);
#else
  ptr += ubx_put_u8 (ptr, 0x40 /*PRN158*/);
  ptr += ubx_put_u32 (ptr, /* 0x1 PRN 120 |  */ 0x8 /*PRN 123*/ | /* 0x40  PRN 126  | */   0x10000 /*PRN136*/);
#endif
#endif

  return ubx_handshake (0x06, 0x16, buf, (unsigned) (ptr - buf));
}


static int  ubx_cfg_tmode (uint32_t time_mode, int32_t x, int32_t y, int32_t z,
                           uint32_t var, uint32_t min_dur, uint32_t max_var)
{
  uint8_t buf[28], *ptr;

  ptr = buf;

  ptr += ubx_put_u32 (ptr, time_mode);
  ptr += ubx_put_i32 (ptr, x);
  ptr += ubx_put_i32 (ptr, y);
  ptr += ubx_put_i32 (ptr, z);
  ptr += ubx_put_u32 (ptr, var);
  ptr += ubx_put_u32 (ptr, min_dur);
  ptr += ubx_put_u32 (ptr, max_var);

  return ubx_handshake (0x06, 0x1d, buf, (unsigned) (ptr - buf));
}

static int  ubx_aid_ini (int32_t lat, int32_t lon, int32_t alt, uint32_t acc)
{
  uint8_t buf[48], *ptr;

  ptr = buf;

  ptr += ubx_put_i32 (ptr, lat);
  ptr += ubx_put_i32 (ptr, lon);
  ptr += ubx_put_i32 (ptr, alt);
  ptr += ubx_put_u32 (ptr, acc);
  ptr += ubx_put_u16 (ptr, 0); //time mark flags
  ptr += ubx_put_u16 (ptr, 0); // week number
  ptr += ubx_put_u32 (ptr, 0); // tow s
  ptr += ubx_put_i32 (ptr, 0); // tow ns
  ptr += ubx_put_u32 (ptr, 0); // tow accuracy ms
  ptr += ubx_put_u32 (ptr, 0); // tow accuracy ns
  ptr += ubx_put_i32 (ptr, 0); // clock drift or freq
  ptr += ubx_put_u32 (ptr, 0); // clock drift or freq accuracy
  ptr += ubx_put_u32 (ptr, 0x21); //flags LLA no time

  return ubx_handshake (0x0b, 0x01, buf, (unsigned) (ptr - buf));
  //return ubx_send (0x0b, 0x01, NULL, 0);

}

static int
gps_set_ref (int ref_hz)
{
  uint8_t buf[80], *ptr;
  int ret;

  printf ("setting gps ref to %d hz\r\n", ref_hz);

  /*Try ublox 5 version first */

  current_ref_hz = ref_hz;

  ptr = buf;

  ptr += ubx_put_u32 (ptr, 1000000 / ref_hz); //period /us
  ptr += ubx_put_u32 (ptr, 1000); //duration /us
  ptr += ubx_put_i8 (ptr, 1); //+ve sense
  ptr += ubx_put_u8 (ptr, 1); //lock to utc
  ptr += ubx_put_u8 (ptr, 0); //pulse only when locked
  ptr += ubx_put_u8 (ptr, 0); //res

  /* 50.52 ns from 10m of RG58 */
  /* 15.15 ns from 3m of RG174 */
  /* total 66ns*/

  ptr += ubx_put_i16 (ptr, 66); //antenna cable delay /ns
  ptr += ubx_put_i16 (ptr, 0); //rf group delay /ns
  ptr += ubx_put_i32 (ptr, 0); //user delay /ns

  ret = ubx_handshake (0x06, 0x07, buf, (unsigned) (ptr - buf));

  if (!ret) return ret;

  ptr = buf;

  ptr += ubx_put_u8 (ptr, 0);   //timepluse 1
  ptr += ubx_put_u8 (ptr, 0);   //version 0
  ptr += ubx_put_u16 (ptr, 0);  //reserved

  ptr += ubx_put_u16 (ptr, 0); //ant cable delay ns
  ptr += ubx_put_u16 (ptr, 0);  //rf group delay ns
  ptr += ubx_put_u32 (ptr, 0); //period unlocked/us
  ptr += ubx_put_u32 (ptr, 1000000 / ref_hz); //period locked/us

  //  ptr += ubx_put_u32 (ptr, 0);  //pulse width unlocked/us
  ptr += ubx_put_u32 (ptr, 0); //pulse width unlocked/us
  ptr += ubx_put_u32 (ptr, 1000 / ref_hz); //pulse width locked/us

  ptr += ubx_put_i32 (ptr, 0);  // ? delay
  ptr += ubx_put_u32 (ptr, 0xf7);

  ret = ubx_handshake (0x06, 0x31, buf, (unsigned) (ptr - buf));

  return ret;
}


int
gps_init (void)
{
  uint8_t buf[80], *ptr;
  //unsigned len;
  //  uint16_t u2;

  usart1_drain();

  while (!ring_empty (&usart1_tx_ring) || !ring_empty (&usart1_rx_ring))
    gps_dispatch();


  printf ("Testing GNSS...\r\n");
  ubx_handshake (0x06, 0x00, NULL, 0);
  printf ("GNSS there\r\n");

  // Set up port
  ptr = buf;
  ptr += ubx_put_u8 (ptr, 1);   //uart1
  ptr += ubx_put_u8 (ptr, 0);   //reserved
  ptr += ubx_put_u16 (ptr, 0x0); //flow control off
  ptr += ubx_put_u32 (ptr, 0x8c0); //no parity, 8 bits
  ptr += ubx_put_u32 (ptr, 9600); // baudrate
  ptr += ubx_put_u16 (ptr, 0x3); // receive NMEA, UBX
  ptr += ubx_put_u16 (ptr, 0x1); // transmit UBX
  ptr += ubx_put_u16 (ptr, 0x0); // no txtimeout
  ptr += ubx_put_u16 (ptr, 0x0); // reserved
  ubx_handshake (0x06, 0x00, buf, (unsigned) (ptr - buf));

  printf ("configured GNSS protocol\r\n");

#if 0
  ptr = buf;
  ptr += ubx_put_u16 (ptr, 0x14);
  ptr += ubx_put_u16 (ptr, 0x00);
  ubx_handshake (0x06, 0x13, buf, (unsigned) (ptr - buf));

  printf ("configured antenna pins\r\n");
#endif



#if 0
  // Check we're in WGS84
  ptr = ubx_fetch (0x06, 0x06, NULL, 0, &len);
  ptr += ubx_get_u16 (ptr, &u2);

  if (u2)
    return -1;

  printf ("configured GNSS datum\r\n");
#endif

  ptr = buf;

  ptr += ubx_put_u8 (ptr, 0);   //UBX
  ptr += ubx_put_u8 (ptr, 0);   //reserved
  ptr += ubx_put_u16 (ptr, 0);  //reserved
  ptr += ubx_put_u8 (ptr, 0);   //nothing on port i2c
  ptr += ubx_put_u8 (ptr, 0x18); //everything on uart
  ptr += ubx_put_u8 (ptr, 0);   //nothing on port 2
  ptr += ubx_put_u8 (ptr, 0);   //nothing on usb
  ptr += ubx_put_u8 (ptr, 0);   //nothing on spi
  ptr += ubx_put_u8 (ptr, 0);   //nothing on port 5

  ptr += ubx_put_u8 (ptr, 1);   //NMEA
  ptr += ubx_put_u8 (ptr, 0);   //reserved
  ptr += ubx_put_u16 (ptr, 0);  //reserved
  ptr += ubx_put_u8 (ptr, 0);   //nothing on port i2c
  ptr += ubx_put_u8 (ptr, 0);   //nothing on uart
  ptr += ubx_put_u8 (ptr, 0);   //nothing on port 2
  ptr += ubx_put_u8 (ptr, 0);   //nothing on usb
  ptr += ubx_put_u8 (ptr, 0);   //nothing on spi
  ptr += ubx_put_u8 (ptr, 0);   //nothing on port 5

  ubx_handshake (0x06, 0x02, buf, (unsigned) (ptr - buf));

  printf ("configured GNSS data\r\n");

  gps_set_ref (1);

#if 0
  ubx_fetch (0x06, 0x31, NULL, 0, &len);
  printf ("configured GNSS 6.31\r\n");
#endif


  ubx_set_nav_rate (1000);

#if 1
  ubx_set_message_rate_port1 (0x01, 0x02, 10);
  ubx_set_message_rate_port1 (0x01, 0x03, 1);
  ubx_set_message_rate_port1 (0x01, 0x06, 0);
  ubx_set_message_rate_port1 (0x01, 0x21, 1);
  ubx_set_message_rate_port1 (0x01, 0x22, 1);

  ubx_set_message_rate_port1 (0x01, 0x30, 10); /*SV in view*/
  ubx_set_message_rate_port1 (0x01, 0x32, 10); /*SBAS*/

  //  ubx_set_message_rate_port1 (0x03, 0x0a, 0);
  //  ubx_set_message_rate_port1 (0x03, 0x10, 0);
  // ubx_set_message_rate_port1 (0x03, 0x21, 0); //?
  //  ubx_set_message_rate_port1 (0x04, 0x04, 0);

  ubx_set_message_rate_port1 (0x0D, 0x04, 10);

  ubx_cfg_sbas (1, 0x7, 3);

  ubx_cfg_poll (0x6, 0x16);

  //  ubx_aid_ini (522202400, 1279080, 2900, 20000);

  printf ("GNSS ready\r\n");
  gps_initting = 0;
#else
  ubx_set_message_rate_port1 (0x01, 0x03, 0);
  ubx_set_message_rate_port1 (0x01, 0x21, 0);
  ubx_set_message_rate_port1 (0x01, 0x22, 0);
  ubx_set_message_rate_port1 (0x01, 0x32, 0);
  ubx_set_message_rate_port1 (0x03, 0x0a, 0);
  //ubx_set_message_rate_port1 (0x03, 0x10, 0);
  printf ("GNSS ready\r\n");
#endif



  //  printf ("GPS ready\r\n");
  //  ubx_get_clock_stats();
  //


  SET (NRESET);

  MAP_INPUT (PPS);
  MAP_OUTPUT_PP (NRESET);

  exti_select_source (EXTI7, PPS_PORT);
  exti_set_trigger (EXTI7, EXTI_TRIGGER_BOTH);
  exti_enable_request (EXTI7);
  nvic_enable_irq (NVIC_EXTI9_5_IRQ);

  return 0;
}


#define ALMANAC_LUMP 64

int
gps_almanac (void)
{
  uint32_t len = almanac_alp_len;
  const uint8_t *ptr = almanac_alp;
  uint32_t lumpsz;
  uint8_t dummy;

  printf ("Downloading GPS almanac %x %x %x\r\n", (int) ptr[0], (int) ptr[1],
          (int) ptr[2]);


  while (len) {
    printf ("%d bytes to go\r\n", (int) len);

    lumpsz = len > ALMANAC_LUMP ? ALMANAC_LUMP : len;

    ubx_handshake_xfer (0x0b, 0x50, ptr, lumpsz);

    ptr += lumpsz;
    len -= lumpsz;

  }

  ubx_handshake_xfer (0x0b, 0x50, &dummy, 1);

  return 0;
}

void gps_dump_almanac (void)
{
  ubx_send (0xb, 0x30, NULL, 0);
}


void gps_dump_eph (void)
{
  ubx_send (0xb, 0x31, NULL, 0);
}


#if 1
int gps_bs (void)
{
  uint32_t now = HW_CLOCK_REG;
  uint64_t abs = ref_extend (now);
  EPOCH e = ref_decompose (abs);
  UTC u = time_epoch_to_utc (e);

  uint8_t buf[80], *ptr;
  int ret;

  ptr = buf;

  ptr += ubx_put_u32 (ptr, 391706007); /*19 sleaford st*/
  ptr += ubx_put_u32 (ptr, 955140);
  ptr += ubx_put_u32 (ptr, 501672842);
  ptr += ubx_put_u32 (ptr, 100000); /*1km std dev*/

  ptr += ubx_put_u16 (ptr, 0); /*no time mark*/

  ptr += ubx_put_u16 (ptr, (u.year - 2000) * 100 + u.month);
  ptr += ubx_put_u32 (ptr, (u.mday * 1000000) + (u.hour * 10000) + (u.minute * 100) + u.second);
  ptr += ubx_put_u32 (ptr, u.nanosecond);

  ptr += ubx_put_u32 (ptr, 2000); /* time good to 2s */
  ptr += ubx_put_u32 (ptr, 0);

  ptr += ubx_put_u32 (ptr, 0);
  ptr += ubx_put_u32 (ptr, 0);

  ptr += ubx_put_u32 (ptr, 0x403);

  printf ("Bootstrapping GPS\r\n");

  hexdump (buf, (unsigned) (ptr - buf));

  ubx_send (0x0b, 0x01, buf, (unsigned) (ptr - buf));


  ret = 0;


  /*
   00000: 97 f5 58 17
          04 93 0e 00
          8a eb e6 1d
          a0 86 01 00
   00010: 00 00
          6f 07     19/03
          03 cb 06 01   /17 22:24:03
          65 ca af 02   45075045ns

    d0 07 00 00   2000ms
   00020: 00 00 00 00     0ns
          00 00 00 00
          00 00 00 00
          03 04 00 00
  */

  return ret;
}

#endif


void
gps_reset (void)
{
  printf ("Restting gps..\r\n");

#define REPHEMERIDIES (1UL << 0)
#define RALMANAC  (1UL << 1)
#define RPOS    (1UL << 4)
#define RRTC    (1UL << 8)

  ubx_cfg_rst (REPHEMERIDIES | RALMANAC | RPOS | RRTC);

  delay_ms (1000);
  usart1_drain();


  printf ("Testing GNSS...\r\n");
  ubx_handshake (0x06, 0x00, NULL, 0);
  printf ("GNSS there\r\n");

  gps_init();

  gps_set_ref (current_ref_hz);

}

#if 0
static int
ubx_get_clock_stats (void)
{
  uint8_t *ptr;

  unsigned len;

  ptr = ubx_fetch (0x01, 0x22, NULL, 0, &len);

  //return ubx_recv_clock_stats(ptr,len);
  return ptr ? 0 : -1;
}
#endif