4 #include "UGear_command.hxx"
7 UGCommand::UGCommand():
14 UGCommand::~UGCommand() {}
17 // calculate the nmea check sum
18 static char calc_nmea_cksum(const char *sentence) {
19 unsigned char sum = 0;
22 // cout << sentence << endl;
24 len = std::strlen(sentence);
26 for ( i = 1; i < len; i++ ) {
27 // cout << sentence[i];
32 // printf("sum = %02x\n", sum);
37 // package and send the serial command
38 static int serial_send( SGSerialPort *serial, int sequence,
39 const string command )
41 char sequence_str[10];
42 snprintf( sequence_str, 9, "%d", sequence );
44 string package = sequence_str;
49 snprintf( pkg_sum, 3, "%02X", calc_nmea_cksum(package.c_str()) );
55 unsigned int result = serial->write_port( package.c_str(),
57 if ( result != package.length() ) {
58 printf("ERROR: wrote %u of %u bytes to serial port!\n",
59 result, (unsigned)package.length() );
67 // send current command until acknowledged
68 int UGCommand::update( SGSerialPort *serial )
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 ) {
83 // nothing to do if command queue empty
84 if ( cmd_queue.empty() ) {
90 string command = cmd_queue.front();
91 /*int result =*/ serial_send( serial, cmd_send_index, command );
93 return cmd_send_index;
97 void UGCommand::add( const string command )
99 printf("command queue: %s\n", command.c_str());
100 cmd_queue.push( command );
104 // create the global command channel manager
105 UGCommand command_mgr;