code cleanup, documentation, gps test bug fix

* gpsmath.c - cosmetic code changes
* paddlemgr.c - cosmetic code changes
* README.md - short readme on paddlemgr
* gps-sim.py - bug in displaying lat/lon. No neg. numbers, appropriately
  set n/s, e/w depending on whether input is negative or positive

busunit/common/gpsmath.c

@@ -28,8 +28,9 @@
 
 double trunc(double x);
 
-float GPS_Dist(float lat1, float lon1, float lat2, float lon2)  //calculated GPS distance in meters between two points
-{
+// calculated GPS distance in meters between two points
+//
+float GPS_Dist(float lat1, float lon1, float lat2, float lon2) {
   float dr_lat1,dr_lat2,delta_lon;
   float sinlat1,coslat1,sinlat2,coslat2,sindl,cosdl;
   float y,x;
@@ -49,7 +50,6 @@ float GPS_Dist(float lat1, float lon1, float lat2, float lon2)  //calculated GPS
   y=sqrt( pow(coslat2 * sindl , 2) + pow((coslat1 * sinlat2) - (sinlat1 * coslat2 * cosdl), 2));
   x=(sinlat1 * sinlat2) + (coslat2 * coslat2 * cosdl);
   return EARTH_RAD * atan2(y,x);
-
 }
 
 /* int main() */

busunit/paddlemgr/README.md

@@ -0,0 +1,63 @@
+`paddlemgr`
+===
+
+This is a program to manage "paddles", the name for bus trips and bus routes.
+
+This program is in charge of taking "advance bus stop" requests as well as monitoring
+GPS messages and system time to advance the route appropriately.
+
+Building
+---
+
+```
+$ ./buildit_native.sh
+```
+
+Program Overview
+---
+
+The `paddlemgr` program manages "paddles", including bus stops and advances due to driver
+input or from GPS updates.
+
+On driver login, the driver will select a paddle which will send a message to the `paddlemgr` through
+the IPC to load a paddle.
+On successful paddle load, the `paddlemgr` will store the paddle in memory for use.
+
+The GPS updates come in through the IPC hub and is processed to determine if a stop rollover should occur.
+If the stop time, as recorded in the `paddle` file, is within some time window (currently set to 10mins)
+and the current position is within a distance window of the stop (currently set to 60m), the stop is advanced.
+
+The drive can manually advance stops by pressing the 'NXT' or 'PRV' buttons from the driver UI which
+will send messages through the IPC to the `paddlemgr` to advance the stop.
+Regardless of whether a driver signal for 'NXT', 'PRV' or an automatic stop rollover has occured,
+the internal state of `paddlemgr` is updated to reflect the new stop and a message sent through the IPC
+to signal what the current stop is.
+
+Inter Process Communication (IPC)
+---
+
+| IPC Mailbox | Function |
+|---|---|
+| `MAILBOX_GPS_STATUS` | `handle_gps_update` |
+| `MAILBOX_STATUS_REQUEST` | `handle_status_req` |
+| `MAILBOX_SET_PADDLE` | `handle_set_paddle_req` |
+| `MAILBOX_NEXT_STOP` | `handle_next_req` |
+| `MAILBOX_PREV_STOP` | `handle_prev_req` |
+
+| IPC Messages | Description |
+|---|---|
+| `MAILBOX_PADDLE_ACK` | Sent on receipt of paddle and successful paddle load (through `load_paddle`) |
+| `MAILBOX_STOP_STATUS` | Sent on receipt of a "status request" messages or on stop change (through `send_status_update`) |
+
+Quick Reference
+---
+
+| Value | Description |
+|---|---|
+| `CONFIG_FILE_PATH` | Default location of where to look for config files (paddles etc.). Default to `/home/bus/config`. |
+| `ROLLOVER_TIME_WINDOW` | Window to consider stop if it's within +/- the time window. Default to `600` seconds (10min). |
+| `ROLLOVER_DISTANCE` | Distance window to consider a stop rollover if it's within +/- the range. Default to `60.0f` (in DMM format). |
+
+See the [Paddle Format](...) documentation for the format of the paddle file.
+
+

busunit/paddlemgr/paddlemgr.c

@@ -40,34 +40,51 @@
 #include "../commhub/client_utils.h"
 #include "../common/gpsmath.h"
 
-typedef struct stop_struct	//structure for loading stop definitions
-{
-  int hour;		//scheduled arrival time
+// structure for loading stop definitions
+//
+typedef struct stop_struct {
+
+  // scheduled arrival time
+  //
+  int hour;
   int min;
 
-  double lat;		//coordinates
-  double lon;	
+  // coordinates
+  //
+  double lat;
+  double lon;
 
-  int route;		//route	
-  int trip;		//trip number
-  int stop;		//stop
+  int route;
+  int trip;
+  int stop;
 
-  char name[STOP_NAME_LEN];	//human readable stop name
+  // human readable stop name
+  //
+  char name[STOP_NAME_LEN];
 
 } stop;
 
 
-int	commhub_fd = -1;
+int  commhub_fd = -1;
+
+// Currently selected paddle
+//
+int   current_paddle_num = 0;
 
-int 	current_paddle_num = 0;		//Currently selected paddle
-int 	current_paddle_len = 0;		//Number of stops on said paddle
-int 	current_paddle_idx = 0;		//Index of the active stop on this paddle
+// Number of stops on said paddle
+//
+int   current_paddle_len = 0;
 
-stop 	current_paddle[MAX_PADDLE_SIZE] = {{0}};	//Data block to hold loaded paddle
+// Index of the active stop on this paddle
+//
+int   current_paddle_idx = 0;
 
+// Data block to hold loaded paddle
+//
+stop   current_paddle[MAX_PADDLE_SIZE] = {{0}};
 
-int load_paddle(int paddlenum)
-{
+
+int load_paddle(int paddlenum) {
   char buffer[LINE_BUFFER_SIZE];
   char buffer2[LINE_BUFFER_SIZE];
   FILE *f;
@@ -79,8 +96,7 @@ int load_paddle(int paddlenum)
 
   f = fopen(buffer, "rb");
 
-  if(!f)
-  {
+  if (!f) {
     printf("Paddle not found: %s\n", buffer);
     return -1;
   }
@@ -90,16 +106,16 @@ int load_paddle(int paddlenum)
   current_paddle_len = 0;
   n = 0;
 
-  //For each line in the input file
-  while( fgets(buffer, LINE_BUFFER_SIZE, f) )
-  {
-    if(current_paddle_idx >= MAX_PADDLE_SIZE)
-    {
+  // For each line in the input file
+  //
+  while( fgets(buffer, LINE_BUFFER_SIZE, f) ) {
+
+    if(current_paddle_idx >= MAX_PADDLE_SIZE) {
       fprintf(stderr, "Paddle %d has overflowed its maximum size of %d stops and has been truncated!\n", current_paddle_num, MAX_PADDLE_SIZE);
       break;
     }
 
-    strip_crlf(buffer);	//get rid of any trailing CR/LF characters
+    strip_crlf(buffer);  //get rid of any trailing CR/LF characters
 
     n++;
 
@@ -107,10 +123,12 @@ int load_paddle(int paddlenum)
 
     i += get_field(buffer2, buffer + i, LINE_BUFFER_SIZE, &eol);
 
-    //Skip any blank or comment lines
+    // Skip any blank or comment lines
+    //
     if(eol || buffer2[0] == '#') { continue; }
 
-    //Clear this row
+    // Clear this row
+    //
     memset(current_paddle + current_paddle_len, 0, sizeof(stop));
 
     current_paddle[current_paddle_len].hour = strtol(buffer2, NULL, 10);
@@ -149,8 +167,8 @@ int load_paddle(int paddlenum)
   return paddlenum;
 }
 
-int clear_paddle()
-{
+int clear_paddle() {
+
   current_paddle_num = 0;
   current_paddle_idx = 0;
   current_paddle_len = 0;
@@ -158,10 +176,12 @@ int clear_paddle()
   return 0;
 }
 
-int where_the_hell_are_we()
-{
+int where_the_hell_are_we() {
   int i;
-  int found_stop = current_paddle_idx;	//by default, we will report that we are still at our last known stop
+
+  // by default, we will report that we are still at our last known stop
+  //
+  int found_stop = current_paddle_idx;
 
   struct tm temp;
   time_t now;
@@ -169,61 +189,73 @@ int where_the_hell_are_we()
 
   now = time(NULL);
 
-  if(current_paddle_num == 0)
-  {
+  if (current_paddle_num == 0) {
     return -1;
   }
 
   #ifdef ROLLOVER_FORWARD_ONLY
-    //If we are in rollover-forward mode, we will not actively re-select any previous stops, nor the current one.
-    //This allows two stops to have the same location but different route numbers for places where the route changes at a stop.
+    // If we are in rollover-forward mode, we will not actively re-select any previous stops, nor the current one.
+    // This allows two stops to have the same location but different route numbers for places where the route changes at a stop.
+    //
     i = current_paddle_idx;
   #else
     i = 0;
   #endif
 
+  for( ; i < current_paddle_len; i++) {
+
+    // populate our time structure based on now
+    // so the date will be correct
+    //
+    localtime_r(&now,&temp);
 
-  for( /*see ifdef block above*/; i < current_paddle_len; i++)
-  {
-    localtime_r(&now,&temp);		//populate our time structure based on now
-                        //so the date will be correct
+    // Set the expected arrival time
+    //
+    temp.tm_hour = current_paddle[i].hour;
 
-    temp.tm_hour = current_paddle[i].hour;	//Set the expected arrival time
-    temp.tm_min = current_paddle[i].min;	//Set the expected arrival time
+    // Set the expected arrival time
+    //
+    temp.tm_min = current_paddle[i].min;
 
-    sched = mktime( &temp );		//and convert it back to a scheduled arrival time in UTC unix timestamp format
+    // and convert it back to a scheduled arrival time in UTC unix timestamp format
+    //
+    sched = mktime( &temp );
 
-    if( (abs(now - sched) <= ROLLOVER_TIME_WINDOW) &&	//First we do the time check, because that's cheap integer math
-      (GPS_Dist(gps_stat.lat, gps_stat.lon, current_paddle[i].lat, current_paddle[i].lon) <= ROLLOVER_DISTANCE)	//The GPS distance last because that's expensive trig
-      )
-    {
-      found_stop = i;			//update our found_stop index to the matching stop
+    // First we do the time check, because that's cheap integer math
+    // then GPS distance last because that's expensive trig
+    //
+    if( (abs(now - sched) <= ROLLOVER_TIME_WINDOW) &&
+        (GPS_Dist(gps_stat.lat, gps_stat.lon, current_paddle[i].lat, current_paddle[i].lon) <= ROLLOVER_DISTANCE)) {
+
+      // update our found_stop index to the matching stop
+      //
+      found_stop = i;
 
       #ifndef ROLLOVER_TO_FURTHEST_STOP
 
-        break;				//if ROLLOVER_TO_FURTHEST_STOP is NOT defined, we break as soon as
-                        //we've found ANY matching stop (even the one we started at)
+        // if ROLLOVER_TO_FURTHEST_STOP is NOT defined, we break as soon as
+        // we've found ANY matching stop (even the one we started at)
+        //
+        break;
+
       #endif
     }
   }
 
-  return found_stop;				//Return found_stop
+  return found_stop;
 }
 
-int send_status_update()
-{
+int send_status_update() {
   struct message_record outgoing_msg;
   stop_status current = {0};
 
-  if(commhub_fd < 0)
-  {
+  if(commhub_fd < 0) {
     return -1;
   }
 
   current.paddle = current_paddle_num;
 
-  if(current_paddle_num)
-  {
+  if(current_paddle_num) {
     current.route = current_paddle[current_paddle_idx].route;
     current.trip = current_paddle[current_paddle_idx].trip;
     current.stop = current_paddle[current_paddle_idx].stop;
@@ -238,70 +270,67 @@ int send_status_update()
   return send_message(commhub_fd, &outgoing_msg);
 }
 
-//This function sends an update to the driver and to the diagnostic log saying we're doing a stop rollover
-int send_driver_update()
-{
+// This function sends an update to the driver and to the diagnostic log saying we're doing a stop rollover
+//
+int send_driver_update() {
   struct message_record outgoing_msg;
 
-  if(commhub_fd < 0)
-  {
-    return -1;
-  }
+  if(commhub_fd < 0) { return -1; }
 
-  if(current_paddle_num)
-  {
-    format_log_message(&outgoing_msg, LOGLEVEL_DEBUG, "%02d:%02d %s", current_paddle[current_paddle_idx].hour, current_paddle[current_paddle_idx].min, current_paddle[current_paddle_idx].name);
+  if(current_paddle_num) {
+    format_log_message(&outgoing_msg,
+                       LOGLEVEL_DEBUG,
+                       "%02d:%02d %s",
+                       current_paddle[current_paddle_idx].hour,
+                       current_paddle[current_paddle_idx].min,
+                       current_paddle[current_paddle_idx].name);
     send_message(commhub_fd, &outgoing_msg);
 
-    format_driver_message(&outgoing_msg, LOGLEVEL_EVENT, "%02d:%02d %s", current_paddle[current_paddle_idx].hour, current_paddle[current_paddle_idx].min, current_paddle[current_paddle_idx].name);
+    format_driver_message(&outgoing_msg,
+                          LOGLEVEL_EVENT,
+                          "%02d:%02d %s",
+                          current_paddle[current_paddle_idx].hour,
+                          current_paddle[current_paddle_idx].min,
+                          current_paddle[current_paddle_idx].name);
     return send_message(commhub_fd, &outgoing_msg);
   }
 
   return -1;
 }
 
-int send_vault_drop()
-{
+int send_vault_drop() {
   struct message_record outgoing_msg;
 
-  if(commhub_fd < 0)
-  {
-    return -1;
-  }
+  if(commhub_fd < 0) { return -1; }
 
   prepare_message(&outgoing_msg, MAILBOX_VAULT_DROP, "", 0);
   return send_message(commhub_fd, &outgoing_msg);
 }
 
-
-
-message_callback_return handle_status_req(struct message_record *msg, void *param)
-{
+message_callback_return handle_status_req(struct message_record *msg, void *param) {
   send_status_update();
   return MESSAGE_HANDLED_CONT;
 }
 
-message_callback_return handle_gps_update(struct message_record *msg, void *param)
-{
+message_callback_return handle_gps_update(struct message_record *msg, void *param) {
   int tempidx;
 
   //The system callback will have already handled this message and put it into the correct data structure
+  //
 
   //If either we have no current paddle, or no real GPS data, ignore this message
-  if( (!current_paddle_num) || (!gps_stat.gps_good) )
-  {
+  //
+  if( (!current_paddle_num) || (!gps_stat.gps_good) ) {
     return MESSAGE_HANDLED_CONT;
   }
 
   tempidx = where_the_hell_are_we();
 
-  if(tempidx < 0)
-  {
+  if(tempidx < 0) {
     return MESSAGE_HANDLED_CONT;
   }
 
-  if(tempidx != current_paddle_idx)
-  {
+  if(tempidx != current_paddle_idx) {
     current_paddle_idx = tempidx;
     send_status_update();
     send_driver_update();
@@ -311,10 +340,9 @@ message_callback_return handle_gps_update(struct message_record *msg, void *para
   return MESSAGE_HANDLED_CONT;
 }
 
-message_callback_return handle_next_req(struct message_record *msg, void *param)
-{
-  if(current_paddle_idx < (current_paddle_len - 1))
-  {
+message_callback_return handle_next_req(struct message_record *msg, void *param) {
+
+  if(current_paddle_idx < (current_paddle_len - 1)) {
     current_paddle_idx++;
     send_driver_update();
   }
@@ -323,10 +351,9 @@ message_callback_return handle_next_req(struct message_record *msg, void *param)
   return MESSAGE_HANDLED_CONT;
 }
 
-message_callback_return handle_prev_req(struct message_record *msg, void *param)
-{
-  if(current_paddle_idx > 0)
-  {
+message_callback_return handle_prev_req(struct message_record *msg, void *param) {
+
+  if(current_paddle_idx > 0) {
     current_paddle_idx--;
     send_driver_update();
   }
@@ -335,16 +362,15 @@ message_callback_return handle_prev_req(struct message_record *msg, void *param)
   return MESSAGE_HANDLED_CONT;
 }
 
-message_callback_return handle_set_paddle_req(struct message_record *msg, void *param)
-{
+message_callback_return handle_set_paddle_req(struct message_record *msg, void *param) {
+
   struct message_record outgoing_msg;
   set_paddle_req *req = (set_paddle_req *)msg->payload;
 
   clear_paddle();
   req->result = load_paddle(req->request);
 
-  if(req->result > 0)
-  {
+  if(req->result > 0) {
     send_driver_update();
   }
 
@@ -356,19 +382,23 @@ message_callback_return handle_set_paddle_req(struct message_record *msg, void *
 }
 
 
-void maintain_ipc_hub_connect(char *progname)
-{
+void maintain_ipc_hub_connect(char *progname) {
   struct message_record outgoing_msg;
 
-  if(commhub_fd < 0)	//if we have no connection to the communication hub
-  {
-    commhub_fd = connect_to_message_server(progname);	//try and get one
+  // if we have no connection to the communication hub
+  //
+  if(commhub_fd < 0) {
+
+    // try and get one
+    //
+    commhub_fd = connect_to_message_server(progname);
 
-//    printf("commhub_fd = %d\n", commhub_fd);
+    // if it worked
+    //
+    if(commhub_fd >= 0) {
 
-    if(commhub_fd >= 0)	//if it worked
-    {
-      //Subscribe to the command mailboxes we act on
+      // Subscribe to the command mailboxes we act on
+      //
       prepare_message(&outgoing_msg, MAILBOX_SUBSCRIBE, MAILBOX_SET_PADDLE, strlen(MAILBOX_SET_PADDLE));
       send_message(commhub_fd,&outgoing_msg);
 
@@ -378,19 +408,20 @@ void maintain_ipc_hub_connect(char *progname)
       prepare_message(&outgoing_msg, MAILBOX_SUBSCRIBE, MAILBOX_PREV_STOP, strlen(MAILBOX_PREV_STOP));
       send_message(commhub_fd,&outgoing_msg);
 
-      //Subscribe to the relevant status management mailboxes
+      // Subscribe to the relevant status management mailboxes
+      //
       subscribe_to_default_messages(commhub_fd);
 
-      //Request updated status information...
-      prepare_message(&outgoing_msg, MAILBOX_STATUS_REQUEST, "", 0);		
+      // Request updated status information...
+      //
+      prepare_message(&outgoing_msg, MAILBOX_STATUS_REQUEST, "", 0);    
       send_message(commhub_fd,&outgoing_msg);
     }
   }
 }
 
 
-int main(int argc, char **argv)
-{
+int main(int argc, char **argv) {
   struct pollfd fds[2];
   int nfds = 0;
   int poll_return = 0;
@@ -408,10 +439,12 @@ int main(int argc, char **argv)
   configure_signal_handlers(argv[0]);
   maintain_ipc_hub_connect(argv[0]);
 
-  //Register our default keep-up-with-system status callbacks
+  // Register our default keep-up-with-system status callbacks
+  //
   register_system_status_callbacks();
 
-  //Add our module-specific callbacks
+  // Add our module-specific callbacks
+  //
   register_dispatch_callback(MAILBOX_GPS_STATUS, CALLBACK_USER(1), handle_gps_update, NULL);
   register_dispatch_callback(MAILBOX_STATUS_REQUEST, CALLBACK_USER(2), handle_status_req, NULL);
   register_dispatch_callback(MAILBOX_SET_PADDLE, CALLBACK_USER(3), handle_set_paddle_req, NULL);
@@ -426,63 +459,60 @@ int main(int argc, char **argv)
     _usec_now = get_usec_time();
     if ((_usec_now - _usec_prv) > _usec_del) {
       printf("[%lli] paddlemgr: heartbeat\n", get_usec_time());
+      fflush(stdout);
       _usec_prv = _usec_now;
     }
     //DEBUG
 
-
     maintain_ipc_hub_connect(argv[0]);
 
     nfds=0;
 
-    if(commhub_fd >= 0)
-    {
+    if(commhub_fd >= 0) {
       fds[nfds].fd = commhub_fd;
       fds[nfds].events = POLLIN;
       nfds++;
     }
 
-    if(nfds > 0)
-    {
+    if(nfds > 0) {
       poll_return = poll(fds, nfds, POLL_TIMEOUT);
     }
-    else
-    {
+    else {
       usleep(POLL_TIMEOUT * 1000);
       poll_return = 0;
     }
 
-    if(poll_return <= 0)
-    {
+    if(poll_return <= 0) {
       continue;
     }
 
     //----  If we got a message
 
-    for(i=0; i < nfds; i++)
-    {
-      if( fds[i].fd == commhub_fd )
-      {	
+    for(i=0; i < nfds; i++) {
+
+      if( fds[i].fd == commhub_fd ) {  
+
         //If we've lost connection, break this loop and poll all over again
-        if(fds[i].revents & (POLLERR | POLLHUP | POLLNVAL))	
-        {
+        //
+        if(fds[i].revents & (POLLERR | POLLHUP | POLLNVAL))  {
           close(commhub_fd);
           commhub_fd = -1;
           break;
         }
 
-        if(fds[i].revents & POLLIN)
-        {
+        if(fds[i].revents & POLLIN) {
+
           read_return = get_message(commhub_fd, &incoming_msg);
 
-          if( read_return < 0 )
-          {
+          if( read_return < 0 ) {
             close(commhub_fd);
             commhub_fd = -1;
             break;
           }
 
-          process_message(&incoming_msg);	//This passes the received message through the callback list
+          // This passes the received message through the callback list
+          //
+          process_message(&incoming_msg);
 
         }
 
@@ -492,8 +522,7 @@ int main(int argc, char **argv)
 
   }
 
-  if(commhub_fd >= 0)
-  {
+  if(commhub_fd >= 0) {
     close(commhub_fd);
   }
 
@@ -501,5 +530,3 @@ int main(int argc, char **argv)
 
   return 0;
 }
-
-

busunit/testing/gps/gps-sim.py

@@ -116,12 +116,17 @@ def print_gps_nmea_messages(ofp, lat, lon, utc_str = ""):
   ofp.write("$GPGSV,4,3,13,13,08,275,23,03,09,127,20,30,47,210,20,18,38,053,*7E\n")
   ofp.write("$GPGSV,4,4,13,04,,,*7F\n")
 
-  msg = "GPGGA," + utc_str + "," + str(lat) + ",N," + str(lon) + ",W,1,08,0.9,116.9,M,0,M,,"
+  latdir = 'N'
+  if lat < 0.0: latdir = 'S'
+  londir = 'E'
+  if lon < 0.0: londir = 'W'
+
+  msg = "GPGGA," + utc_str + "," + str(abs(lat)) + "," + latdir + "," + str(abs(lon)) + "," + londir + ",1,08,0.9,116.9,M,0,M,,"
   ofp.write("$" + msg + "*" + nmea_checksum(msg) + "\n")
 
   ofp.write("$GPVTG,NaN,T,,M,0.0,N,0.0,K,A*42\n")
 
-  msg = "GPRMC," + utc_str + ",A," + str(lat) + ",N," + str(lon) + ",W,0.0,0.0,050500,,,A"
+  msg = "GPRMC," + utc_str + ",A," + str(abs(lat)) + "," + latdir + "," + str(abs(lon)) + "," + londir + ",0.0,0.0,050500,,,A"
   ofp.write("$" + msg + "*" + nmea_checksum(msg) + "\n")
 
   ofp.write("$GPGSA,A,3,01,07,08,11,17,19,22,28,,,,,1.5,0.9,1.1*35\n")