3 #include "UGear_command.hxx"
6 UGCommand::UGCommand():
13 UGCommand::~UGCommand() {}
16 // calculate the nmea check sum
17 static char calc_nmea_cksum(const char *sentence) {
18 unsigned char sum = 0;
21 // cout << sentence << endl;
23 len = std::strlen(sentence);
25 for ( i = 1; i < len; i++ ) {
26 // cout << sentence[i];
31 // printf("sum = %02x\n", sum);
36 // package and send the serial command
37 static int serial_send( SGSerialPort *serial, int sequence,
38 const string command )
40 char sequence_str[10];
41 snprintf( sequence_str, 9, "%d", sequence );
43 string package = sequence_str;
48 snprintf( pkg_sum, 3, "%02X", calc_nmea_cksum(package.c_str()) );
54 unsigned int result = serial->write_port( package.c_str(),
56 if ( result != package.length() ) {
57 printf("ERROR: wrote %d of %d bytes to serial port!\n",
58 result, package.length() );
66 // send current command until acknowledged
67 int UGCommand::update( SGSerialPort *serial )
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 ) {
82 // nothing to do if command queue empty
83 if ( cmd_queue.empty() ) {
89 string command = cmd_queue.front();
90 int result = serial_send( serial, cmd_send_index, command );
92 return cmd_send_index;
96 void UGCommand::add( const string command )
98 printf("command queue: %s\n", command.c_str());
99 cmd_queue.push( command );
103 // create the global command channel manager
104 UGCommand command_mgr;