popufare/busunit/gps/gps_main.c

/*
 * Copyright (c) 2019 Clementine Computing LLC.
 *
 * This file is part of PopuFare.
 *
 * PopuFare is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Affero General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * PopuFare 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 Affero General Public License for more details.
 *
 * You should have received a copy of the GNU Affero General Public License
 * along with PopuFare.  If not, see <https://www.gnu.org/licenses/>.
 *
 */

#include <sys/time.h>
#include <stdio.h>
#include <stdlib.h>
#include <poll.h>
#include <unistd.h>
#include <errno.h>
#include <string.h>
#include <signal.h>
#include <time.h>

#include <ctype.h>
#include <string.h>

#include "../common/common_defs.h"
#include "../commhub/commhub.h"
#include "../commhub/client_utils.h"

#define GPS_MINDER_VERSION "2.1.1"

int gps_fd = -1;
int hub_fd = -1;

time_t last_hub_try = 0;
time_t last_diu_clock = 0;

gps_status my_gps_stat = {0};
state_info_t my_state_info = {0};

time_t mkgmtime(struct tm *tm);

//    This function takes a GPS timestamp (in gross GPS float format + gross integer date) and
// makes it into a sane UTC timestamp.  If we are more than MAX_GPS_CLOCK_DRIFT seconds off
// from GPS time, it sets the system clock to GPS time.
//
int handle_gps_time(float gtime, int date) {   
  int day,month,year;
  int hour,min,sec,frac;
  int n = 0;
  time_t utc, now;

  char buffer[32] = {0};

  struct message_record outgoing_msg;

  now = time(NULL);

  day = month = year = 0; 
  hour = min = sec = frac = 0;

  //  Just to be *ahem* clear, as per NMEA standard the date is encoded DDMMYY, and the time of day
  // is encoded HHMMSS[.frac] where there is an optional fractional second field denoted by a decimal point.
  // we must be able to decode the fractional seconds field but we discard the information since it is not
  // reliable enough to be worth the bother.
  //

  // Start out with zero parsed fields
  //
  n = 0;

  //  Construct and then re-parse the time from its icky format to discrete values (this should result
  // in at least three (possibly four) fields.
  //
  sprintf(buffer,"%010.3f", gtime);
  n += sscanf(buffer,"%02d%02d%02d.%d", &hour, &min, &sec, &frac);

  // Construct and then re-parse the date from its icky format to discrete values.  This should result in three fields.
  //
  sprintf(buffer,"%06d", date);
  n += sscanf(buffer,"%02d%02d%02d", &day, &month, &year);

  if(n >= 6)  //if we scanned at all required fields
  {   
    struct tm gpstm = {0};

    // GPS date only uses two digits for year, so we must assume the century
    //
    year += GPS_DATE_CENTURY;

    // tm.tm_year is based on the year 1900
    //
    gpstm.tm_year = year - 1900;

    // January = month 0 in struct tm.tm_mon whereas January = month 1 in an NMEA GPS date.
    //
    gpstm.tm_mon = month - 1;
    gpstm.tm_mday = day;
    gpstm.tm_hour = hour;
    gpstm.tm_min = min;
    gpstm.tm_sec = sec;

    // Go and turn the struct tm we've just populated into an utc time stamp (seconds since epoch)
    //
    utc = mkgmtime(&gpstm);

    // Most importantly... Remember what the self-reported GPS time stamp is.
    //
    my_gps_stat.gpstime = utc;

//    printf("%02d-%02d-%04d %02d:%02d:%02d\n",day,month,year,hour,min,sec);
//    printf("CALCULATED: %d\nSYSTEM  : %d\n\n",(int)utc,(int)time(NULL));

    // if we have more than MAX_GPS_CLOCK_DRIFT seconds of clock drift
    //
    if(abs(utc - now) > MAX_GPS_CLOCK_DRIFT) {   
      struct timeval ts = {0};

      // Set the timeval struct to the calculated utc timestamp from the GPS unit.
      //
      ts.tv_sec = utc;
      ts.tv_usec = 0;

      // Set the system clock from GPS time using said timeval struct.
      //
      settimeofday(&ts, NULL);

      //system("/sbin/hwclock --systohc");  //Go and push the new system clock value into the hardware realtime clock chip.

      // If we have a valid connection to the IPC hub
      //
      if( hub_fd >= 0) {   

        // Stick a message into the diagnostic log to record the fact that we've set the system clock from GPS
        //
        format_log_message(&outgoing_msg, LOGLEVEL_DEBUG, "Set syatem clock from previous value (%d) to GPS time (%d).", (int)now, (int)utc );
        send_message(hub_fd, &outgoing_msg);
      }
    }
  }

  return 0;
}


#ifdef CLEAR_GPS_ON_STALE
static inline void clear_stale_gps_data() {   
  my_gps_stat.lat = my_gps_stat.lon = my_gps_stat.heading = my_gps_stat.velocity = 0;
  my_gps_stat.num_sats = 0;
}
#else
static inline void clear_stale_gps_data() { }
#endif

static int handle_stale_gps_condition() {
  //   This return code will be > 0 if this function generates a status change that
  // will then need to be communicated to other modules in the system via a message to
  // MAILBOX_GPS_STATUS.
  //
  int return_code = 0;

  //   This will hold the gps_good flag as it stood at the beginning of this subroutine
  // previous to any adjustments we make.
  //
  int previous_good_flag = my_gps_stat.gps_good;

  int stale_time = 0;

  time_t now = time(NULL);

  stale_time = (now - my_gps_stat.stamp);

  // If we have entered this function with the impression that we have a valid GPS fix...
  //
  if(previous_good_flag > 0) {   

    // If it has been at least GPS_STALE_THRESHOLD seconds since the last fix
    //
    if( stale_time >= GPS_STALE_THRESHOLD ) {   
      clear_stale_gps_data();
      my_gps_stat.gps_good = 0;
      return_code |= 1;
    }

  }

  //   If we have determined that we need to declare the GPS data stale and invalid and
  // we have a valid connection to the IPC hub we should use that IPC hub connection to
  // add a note to the diagnostic log indicating that we've declared the GPS data stale.
  //
  if( return_code && (hub_fd >= 0) ) {   
    struct message_record outgoing_msg;

    format_log_message(&outgoing_msg, LOGLEVEL_DEBUG, "GPS fix has been stale for %d seconds, setting GPS = NO.", stale_time);
    send_message(hub_fd, &outgoing_msg);
  }

  return return_code;
}

//------------

// Standard c parsing and tokenization functions are having trouble with blank fields,
// so these custom field parsing routines were developed.
//
// Each takes in a string and parses the first field, where appropriate.
// It then advances to the next token by searching for the field delimiter (',')
// and pushing the pointer one past it, prepping it for next use.
//
// A default value of '0' is assigned if no conversion has taken place.
//
// Return NULL on error or end of string.
//
static char *_convert_advance_nop(char *in) {
  if (!in) { return NULL; }
  if ((*in) == ',') { return in+1; }
  in = strchr(in, ',');
  if (in) { return in+1; }
  return in;
}

static char *_convert_advance_float(char *in, float *f) {

  *f = 0.0;

  if (!in) { return NULL; }
  if ((*in) != ',') { *f = strtof(in, NULL); }
  if ((*in) == ',') { return in+1; }
  in = strchr(in, ',');
  if (in) { return in+1; }
  return in;
}

static char *_convert_advance_char(char *in, char *ch) {

  *ch = '\0';

  if (!in) { return NULL; }
  if ((*in) != ',') { *ch = *in; }
  if ((*in) == ',') { return in+1; }
  in = strchr(in, ',');
  if (in) { return in+1; }
  return in;
}

//----

int update_gps(char *in) {
  // This will hold the number of matched variables populated by sscanf().
  //
  int num = 0;

  //   This will allow us to know if we have made a transition from an invalid
  // GPS fix to a valid one so that we can log this information.
  //
  int previous_good_flag = my_gps_stat.gps_good;

  //   This return code will be > 0 if this function generates a status change that
  // will then need to be communicated to other modules in the system via a message to
  // MAILBOX_GPS_STATUS.
  //
  int return_code = 0;


  if(!strncmp(in,"$GPRMC",6)) {   
    float f1=0;
    char f2=0;
    float f3=0;
    char f4=0;
    float f5=0;
    char f6=0;
    float f7=0;
    float f8=0;
    float f9=0; 
    float f10=0;
    char f11=0;

    char *chp;

    chp = in;
    num=0;
    do {

      // ignore first token
      //
      chp = _convert_advance_nop(in);
      if (!chp) { break; }

      chp = _convert_advance_float(chp, &f1);
      if (!chp) { break; }
      num++;

      chp = _convert_advance_char(chp, &f2);
      if (!chp) { break; }
      num++;

      chp = _convert_advance_float(chp, &f3);
      if (!chp) { break; }
      num++;

      chp = _convert_advance_char(chp, &f4);
      if (!chp) { break; }
      num++;

      chp = _convert_advance_float(chp, &f5);
      if (!chp) { break; }
      num++;

      chp = _convert_advance_char(chp, &f6);
      if (!chp) { break; }
      num++;

      chp = _convert_advance_float(chp, &f7);
      if (!chp) { break; }
      num++;

      chp = _convert_advance_float(chp, &f8);
      if (!chp) { break; }
      num++;

      chp = _convert_advance_float(chp, &f9);
      if (!chp) { break; }
      num++;

      chp = _convert_advance_float(chp, &f10);
      if (!chp) { break; }
      num++;

      chp = _convert_advance_char(chp, &f11);
      if (!chp) { break; }
      num++;

    } while (0);

    // If we have a full GPRMC sentence we can consider setting the time
    //
    if(num == 11) {

      //   Require at least MIN_SATS_FOR_TIME satellites to accept a new system clock value from the GPS unit.
      // This is to keep a crummy GPS fix from generating a bogus or unstable system time.
      //

      if(my_gps_stat.num_sats >= MIN_SATS_FOR_TIME) {

        //   Pass the time field (f1) and the date field (f9) in to the routine that sets the system clock if needed.
        // (this routine also stores the utc timestamp derived from the GPS date and time fields so it can be passed to
        // other modules that may have a need for this information).
        //
        //handle_gps_time(f1,(int)f9);

      }
    }

    if(num > 0) {

      // update snapshot with latitude
      //
      my_gps_stat.lat = f3 * ((f4 == 'N')?(1):(-1));

      // longitude
      //
      my_gps_stat.lon = f5 * ((f6 == 'E')?(1):(-1));

      // meters per second (converted from knots)
      //
      my_gps_stat.velocity = f7 / 1.94384449f;

      // course
      //
      my_gps_stat.heading = f8;

      // update snapshot's staledate
      //
      my_gps_stat.stamp = time(NULL);

      return_code |= 1;

    }
  }
  else if(!strncmp(in,"$GPGGA",6)) {   
    float f1=0;
    float f2=0;
    char f3=0;
    float f4=0;
    char f5=0;
    int f6=0;
    int f7=0;
    float f8=0;
    float f9=0;
    char f10=0;
    float f11=0;
    char f12=0;

    //num=sscanf(in,"$GPGGA,%d,%f,%c,%f,%c,%d,%d,%f,%f,%c,%f,%c",&f1,&f2,&f3,&f4,&f5,&f6,&f7,&f8,&f9,&f10,&f11,&f12);
    num=sscanf(in,"$GPGGA,%f,%f,%c,%f,%c,%d,%d,%f,%f,%c,%f,%c",&f1,&f2,&f3,&f4,&f5,&f6,&f7,&f8,&f9,&f10,&f11,&f12);

    if(num == 12) {   

      // require 3 satellites minimum
      //
      if ( f7 >= 3 ) {   

        // store GPS valid flag in snapshot
        //
        my_gps_stat.gps_good = f6;

        // store number of satellites in view in snapshot
        //
        my_gps_stat.num_sats = f7;

        //   Do NOT store the timestamp since we only want to remember timestamps of position
        // fixes and the GPGGA message is all metadata (number of satellites, fix quality, etc...).
        // It is worth noting that GPGGA does report altitude, but that is not a piece of information
        // we track because the road is where the road is, and if a bus becomes airborn we have much
        // bigger problems than figuring out how far off the ground it is...
        //
        return_code |= 1;
      }
    }

  }

  return_code |= handle_stale_gps_condition();

  //   If we have a connection to the IPC hub and we had previously not had a valid
  // GPS fix but we now do, make a note of it in the diagnostic log.
  //
  if( (previous_good_flag == 0) && (my_gps_stat.gps_good != 0) && (hub_fd >= 0) ) {   
    struct message_record outgoing_msg;

    format_log_message(&outgoing_msg, LOGLEVEL_DEBUG, "GPS fix is now valid with %d satellites. Setting GPS = YES.", my_gps_stat.num_sats);
    send_message(hub_fd, &outgoing_msg);
  }

  return return_code;
}



void maintain_ipc_hub_connect(char *progname) {
  struct message_record outgoing_msg;

  // if we have no connection to the IPC hub
  //
  if(hub_fd < 0) {

    // if we haven't tried the hub in a few seconds
    //
    if( (time(NULL) - last_hub_try) > HUB_RETRY_TIME ) {


      // retry it
      //
      last_hub_try = time(NULL);

      // try and get one
      //
      hub_fd = connect_to_message_server(progname);

      if(hub_fd >= 0) {

        //Subscribe to the default status messages
        //
        subscribe_to_default_messages(hub_fd);

        // Ask for a status update
        //
        prepare_message(&outgoing_msg, MAILBOX_STATUS_REQUEST, "", 0);
        send_message(hub_fd,&outgoing_msg);
      }
      else {
        fprintf(stderr, "Cannot connect to IPC hub!\n");
      }
    }
  }
}

// This might be better done at a higher level but kept here
// in case we need it.
//
int reset_gps() {
  char msg_gps_off[] = "AT+CGPS=0\n",
       msg_gps_on[] = "AT+CGPS=1\n",
       msg_gps_poll_off[] = "AT+CGPSINFOCFG=0\n",
       msg_gps_poll_on[] = "AT+CGPSINFOCFG=3,31\n";
  size_t n;

  if (gps_fd < 0) { return -1; }

  n = strlen(msg_gps_off)+1;
  write(gps_fd, msg_gps_off, n);

  n = strlen(msg_gps_poll_off)+1;
  write(gps_fd, msg_gps_poll_off, n);

  n = strlen(msg_gps_on)+1;
  write(gps_fd, msg_gps_on, n);

  n = strlen(msg_gps_poll_on)+1;
  write(gps_fd, msg_gps_poll_on, n);

  return 0;
}

//----------------------------------

int main(int argc, char **argv) {
  int i;

  char line[LINE_BUFFER_SIZE] = {0};

  struct message_record incoming_msg;
  struct message_record outgoing_msg;

  struct pollfd fds[2];
  int nfd;

  int poll_return;
  int read_return;

  time_t now;
  time_t last_stale_gps_check = 0;

#ifdef DEBUG_PRINT
  long long int _usec_now, _usec_prv, _usec_del;
  _usec_now = get_usec_time();
  _usec_prv = _usec_now;
  _usec_del = 60000000;
#endif

  long long int _usec_ratelimit_now, _usec_ratelimit_prv, _usec_ratelimit_del;
  _usec_ratelimit_now = get_usec_time();
  _usec_ratelimit_del = 1000000;
  _usec_ratelimit_prv = _usec_ratelimit_now - _usec_ratelimit_del;

  if ( (argc>1) && (
        (strncmp(argv[1], "-h", 3)==0) ||
        (strncmp(argv[1], "-v", 3)==0) ) ) {
    printf("gps_minder version: %s\n", GPS_MINDER_VERSION);
    exit(0);
  }

  // Restore GPS state in case `gps_minder` has been restarted
  //
  get_state_info(&my_state_info);
  my_gps_stat.lat = my_state_info.lat;
  my_gps_stat.lon = my_state_info.lon;
  my_gps_stat.heading = my_state_info.heading;
  my_gps_stat.velocity = my_state_info.velocity;
  my_gps_stat.num_sats = my_state_info.num_sats;
  my_gps_stat.gps_good = my_state_info.gps_good;
  my_gps_stat.stamp = my_state_info.stamp;
  my_gps_stat.gpstime = my_state_info.gpstime;


  // Configure our signal handlers to deal with SIGINT, SIGTERM, etc... 
  // and make graceful exits while logging
  //
  configure_signal_handlers(argv[0]);

  // Make an initial attempt to get in touch with the
  // interprocess communication hub (it may not be up yet depending on the start order)
  //
  maintain_ipc_hub_connect(argv[0]);

  // Register our defualt system message processing callbacks
  //
  register_system_status_callbacks();

  // loop until we get asked to exit...
  //
  while( exit_request_status == EXIT_REQUEST_NONE ) {

    RESET_WATCHDOG();

#ifdef DEBUG_PRINT
    _usec_now = get_usec_time();
    if ((_usec_now - _usec_prv) > _usec_del) {
      printf("[%lli] gps_minder: heartbeat\n", get_usec_time());
      _usec_prv = _usec_now;
    }
#endif

    maintain_ipc_hub_connect(argv[0]);

    if(gps_fd < 0) {
      gps_fd = open_rs232_device(GPS_PORT, GPS_DEFAULT_BAUD, RS232_LINE);

      if(gps_fd < 0) {
        _usec_ratelimit_now = get_usec_time();
        if ((_usec_ratelimit_now - _usec_ratelimit_prv) > _usec_ratelimit_del) {
          fprintf(stderr, "Cannot open serial port %s for GPS!\n", GPS_PORT);
          _usec_ratelimit_prv = _usec_ratelimit_now;
        }
      }

    }

    //---

    now = time(NULL);

    // Every second we want to check to make sure that our GPS data have note gone stale...
    //
    if((now - last_stale_gps_check) > 0) {

      //   If the stale check results in an update to my_gps_stat, we must update any other
      // modules which may be tracking GPS status via the IPC hub.
      //
      if( handle_stale_gps_condition() > 0 ) {

        // If we have a connection to the IPC hub
        //
        if(hub_fd >= 0) {

          // Go and toss the data to any other modules who happen to care about GPS
          //
          prepare_message(&outgoing_msg, MAILBOX_GPS_STATUS, &my_gps_stat, sizeof(my_gps_stat));
          send_message(hub_fd, &outgoing_msg);

        }

      }

      // Either way, remember that we did this stale check.
      //
      last_stale_gps_check = now;
    }

    //---

    if (hup_request_status) {
      fprintf(stderr, "gps_minder: hup request\n");
      hup_request_status = 0;
    }

    //---

    nfd = 0;

    if(hub_fd >= 0) {
      fds[nfd].fd = hub_fd;
      fds[nfd].events = POLLIN;
      fds[nfd].revents = 0;
      nfd++;
    }

    if(gps_fd >= 0) {
      fds[nfd].fd = gps_fd;
      fds[nfd].events = POLLIN;
      fds[nfd].revents = 0;
      nfd++;
    }

    // if we have any file descriptors, poll them
    //
    if(nfd > 0) {
      poll_return = poll(fds, nfd, POLL_TIMEOUT);
    }

    // otherwise, whistle and look busy
    //
    else {
      poll_return = 0;  //(this keeps us from buringing 100% cpu cycles if we don't have contact with either
      sleep(1);    //the IPC hub or the DIU hardware).
    }

    //--------------------------------------------------------------------------------------------------

    // if poll didn't net us any work to do,
    //
    if (poll_return < 1) {
      // lets try again
      //
      continue;
    }

    // Loop through all polled file descriptors
    //
    for (i = 0; i < nfd; i++) {

      // If we're looking at the DIU...
      //
      if ( fds[i].fd == gps_fd ) {

        // if poll says our serial port has become bogus...
        //
        if (fds[i].revents & (POLLHUP | POLLERR | POLLNVAL)) {
          fprintf(stderr, "This is very odd... Poll returned flags %d on our serial port...\n", fds[i].revents);

          // close it, flag as invalid, 
          // and break out of the for loop to allow the while to cycle
          //
          close(gps_fd);
          gps_fd = -1;
          break;    
        }

        if (fds[i].revents & POLLIN) {
          read_return = read(fds[i].fd, line, sizeof(line));

          if (read_return > 0) {
            char *trav = line;

            line[read_return] = '\0';
            strip_crlf(line);

            // advance until EOL or we hit our start sentinel
            //
            while (*trav && (*trav != '$') ) {
              trav++;
            }

            // Check to see that our address header is intact...
            //
            if (trav[0] == '$') {
              switch (trav[1]) {

                case 'G':  //-----------------------------------"/G:" means it is a new GPS input

                  // If this GPS update constitutes a meaningful piece of data
                  //
                  if(update_gps(trav) > 0) {

                    //and we have a connection to the IPC hub
                    //
                    if(hub_fd >= 0) {

                      // Go and toss the data to any other modules who happen to care about GPS
                      //
                      prepare_message(&outgoing_msg, MAILBOX_GPS_STATUS, &my_gps_stat, sizeof(my_gps_stat));
                      send_message(hub_fd, &outgoing_msg);

                    }

                    // Remember that we did a stale GPS check as part of our update.
                    //
                    last_stale_gps_check = now;
                  }
                  break;

                // ignore any message addresses that we don't know what to do with
                //
                default:
                  printf("Ignoring command \"%s\"\n", trav);
                  break;
              }
            }
            else { }
          }
          else {

            fprintf(stderr, "Read from %s returned %d!\n", DRIVER_UI_PORT, read_return);

            // close it, flag it invalid and break out of the
            // for loop to allow for the while to cycle.
            //
            close(gps_fd);
            gps_fd = -1;
            break;

          }
        }

      }

      // If we're looking at the IPC hub...
      //
      else if ( fds[i].fd == hub_fd ) {

        // if poll says our connection to the IPC hub has died...
        //
        if(fds[i].revents & (POLLHUP | POLLERR | POLLNVAL)) {
          fprintf(stderr, "The connection to the IPC hub has gone away...\n");  //complain

          // close it, flag it invalid and break out of the
          // for loop to allow for the while to cycle.
          //
          close(hub_fd);
          hub_fd = -1;
          break;
        }

        // if we have mail in any of our mailboxes...
        //
        if (fds[i].revents & POLLIN) {

          read_return = get_message(hub_fd, &incoming_msg);

          if (read_return < 0) {
            fprintf(stderr, "The connection to the IPC hub has gone away...\n");  //complain

            // close it, flag it invalid and break out of the
            // for loop to allow for the while to cycle.
            //
            close(hub_fd);
            hub_fd = -1;
            break;
          }
          else {
            message_callback_return msg_status;
            msg_status = process_message(&incoming_msg);
            if (msg_status) { }
          }

        }

      }
    }
  }

  if (hub_fd >= 0) { close(hub_fd); }
  if (gps_fd >= 0) { close(gps_fd); }

  return 0;
}