/* * 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 . * */ #include #include #include #include #include #include #include #include #include #include #include #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; }