utils/GPSsmooth/UGear_command.cxx

   1 #include <cstring>
   2 #include <cstdio>
   3
   4 #include "UGear_command.hxx"
   5
   6
   7 UGCommand::UGCommand():
   8     cmd_send_index(0),
   9     cmd_recv_index(0),
  10     prime_state(true)
  11 {}
  12
  13
  14 UGCommand::~UGCommand() {}
  15
  16
  17 // calculate the nmea check sum
  18 static char calc_nmea_cksum(const char *sentence) {
  19     unsigned char sum = 0;
  20     int i, len;
  21
  22     // cout << sentence << endl;
  23
  24     len = std::strlen(sentence);
  25     sum = sentence[0];
  26     for ( i = 1; i < len; i++ ) {
  27         // cout << sentence[i];
  28         sum ^= sentence[i];
  29     }
  30     // cout << endl;
  31
  32     // printf("sum = %02x\n", sum);
  33     return sum;
  34 }
  35
  36
  37 // package and send the serial command
  38 static int serial_send( SGSerialPort *serial, int sequence,
  39                         const string command )
  40 {
  41     char sequence_str[10];
  42     snprintf( sequence_str, 9, "%d", sequence );
  43
  44     string package = sequence_str;
  45     package += ",";
  46     package += command;
  47
  48     char pkg_sum[10];
  49     snprintf( pkg_sum, 3, "%02X", calc_nmea_cksum(package.c_str()) );
  50
  51     package += "*";
  52     package += pkg_sum;
  53     package += "\n";
  54
  55     unsigned int result = serial->write_port( package.c_str(),
  56                                               package.length() );
  57     if ( result != package.length() ) {
  58         printf("ERROR: wrote %u of %u bytes to serial port!\n",
  59                result, (unsigned)package.length() );
  60         return 0;
  61     }
  62
  63     return 1;
  64 }
  65
  66
  67 // send current command until acknowledged
  68 int UGCommand::update( SGSerialPort *serial )
  69 {
  70     // if current command has been received, advance to next command
  71     printf("sent = %d  recv = %d\n", cmd_send_index, cmd_recv_index);
  72     if ( cmd_recv_index >= cmd_send_index ) {
  73         if ( ! cmd_queue.empty() ) {
  74             if ( ! prime_state ) {
  75                 cmd_queue.pop();
  76                 cmd_send_index++;
  77             } else {
  78                 prime_state = false;
  79             }
  80         }
  81     }
  82
  83     // nothing to do if command queue empty
  84     if ( cmd_queue.empty() ) {
  85         prime_state = true;
  86         return 0;
  87     }
  88
  89     // send the command
  90     string command = cmd_queue.front();
  91     /*int result =*/ serial_send( serial, cmd_send_index, command );
  92
  93     return cmd_send_index;
  94 }
  95
  96
  97 void UGCommand::add( const string command )
  98 {
  99     printf("command queue: %s\n", command.c_str());
 100     cmd_queue.push( command );
 101 }
 102
 103
 104 // create the global command channel manager
 105 UGCommand command_mgr;
 106