utils/GPSsmooth/UGear_command.cxx

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