software/c++/ruwaicom/src/serial_parser.cpp

   1 /*--------------------------------------------------------------------------*/
   2 // LICENSE
   3 //
   4 // This file is part of ruwai.
   5 //
   6 // If you use ruwai_parser in any program or publication, please inform and
   7 // acknowledge its author Stefan Mertl (stefan@mertl-research.at).
   8 //
   9 // ruwai is free software: you can redistribute it and/or modify
  10 // it under the terms of the GNU General Public License as published by
  11 // the Free Software Foundation, either version 3 of the License, or
  12 // (at your option) any later version.
  13 //
  14 // This program is distributed in the hope that it will be useful,
  15 // but WITHOUT ANY WARRANTY; without even the implied warranty of
  16 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  17 // GNU General Public License for more details.
  18 //
  19 // You should have received a copy of the GNU General Public License
  20 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
  21 /*--------------------------------------------------------------------------*/
  22
  23 #include "serial_parser.h"
  24 #include <signal.h>
  25
  26
  27 void
  28 serial_watchdog(int signum)
  29 {
  30     if (signum == SIGALRM)
  31     {
  32         syslog(LOG_ERR, "[serial_watchdog] Timeout in record mode. No serial data received for some time.");
  33         std::exit(EXIT_FAILURE);
  34     }
  35 }
  36
  37
  38 SerialParser::SerialParser(int serial_port, Recorder* recorder)
  39 {
  40     this->serial_port = serial_port;
  41     this->recorder = recorder;
  42
  43     synced = false;
  44     seq_num = 1;
  45     last_ack_msg_id = -1;
  46     last_ack_msg_class = -1;
  47
  48     control_mode = true;
  49     record_mode = false;
  50     parser_size = sizeof(rw_header_t);
  51 }
  52
  53
  54 bool
  55 SerialParser::set_control_mode(void)
  56 {
  57     bool confirmed;
  58     confirmed = set_mode(0);
  59     control_mode = true;
  60     record_mode = false;
  61     parser_size = sizeof(rw_header_t);
  62     signal(SIGALRM, SIG_DFL);
  63     return confirmed;
  64 }
  65
  66
  67 bool
  68 SerialParser::set_record_mode(void)
  69 {
  70     bool confirmed;
  71     struct termios port_options; // struct to hold the port settings
  72
  73     confirmed = set_mode(1);
  74     control_mode = false;
  75     record_mode = true;
  76     parser_size = 1000;
  77
  78     //close(serial_port);
  79
  80     tcgetattr(serial_port, &port_options);
  81     port_options.c_cc[VMIN] = 200;     // Minimum number of bytes read.
  82     port_options.c_cc[VTIME] = 0;   // timeout in deci-seconds
  83
  84     tcsetattr(serial_port, TCSANOW, &port_options);
  85     tcflush(serial_port, TCIOFLUSH);
  86     //fcntl(serial_port, F_SETFL, 0);
  87
  88     signal(SIGALRM, serial_watchdog);
  89     alarm(5);
  90
  91     return confirmed;
  92 }
  93
  94
  95 bool
  96 SerialParser::request_version(void)
  97 {
  98     rw_soh_request_t msg;
  99     msg.msg_id = RW_SOH_ID_VERSION;
 100
 101     send_ruwai_message(RW_CLASS_SOH, RW_SOH_ID_REQUEST, &msg, sizeof(msg));
 102
 103     return true;
 104 }
 105
 106
 107 bool
 108 SerialParser::is_synced(void)
 109 {
 110     return synced;
 111 }
 112
 113 void
 114 SerialParser::start_handshake(void)
 115 {
 116     syslog(LOG_NOTICE, "Starting the handshake.");
 117     send_ack_sync(1, 0, seq_num, 0);
 118 }
 119
 120 void
 121 SerialParser::sync(std::vector<uint8_t> payload)
 122 {
 123     syslog(LOG_DEBUG, "in sync");
 124     rw_ack_sync_t paket;
 125     assert(payload.size() == sizeof paket);
 126     std::copy(payload.begin(), payload.end(), reinterpret_cast<uint8_t*>(&paket));
 127     syslog(LOG_DEBUG, "sync: %d, ack: %d, seq_num: %d, ack_num: %d", paket.sync, paket.ack, paket.seq_num, paket.ack_num);
 128     if ((paket.sync == 1) && (paket.ack == 1))
 129     {
 130         // Handle the sync request.
 131         syslog(LOG_NOTICE, "Got a handshake response from the Arduino stack.");
 132         //send_ack_sync(0, 1, seq_num++, payload.rw_ack_sync.seq_num + 1);
 133         send_ack_sync(0, 1, seq_num, 0);
 134         synced = true;
 135     }
 136 }
 137
 138
 139 void
 140 SerialParser::send_ack_sync(uint8_t sync, uint8_t ack, uint8_t seq_num, uint8_t ack_num)
 141 {
 142     rw_ack_sync_t msg;
 143     msg.sync = sync;
 144     msg.ack = ack;
 145     msg.seq_num = seq_num;
 146     msg.ack_num = ack_num;
 147
 148     send_ruwai_message(RW_CLASS_ACK, RW_ACK_ID_SYNC, &msg, sizeof(msg));
 149 }
 150
 151
 152 void
 153 SerialParser::send_ruwai_message(uint8_t msg_class, uint8_t msg_id, void* msg, uint8_t length)
 154 {
 155     rw_header_t header;
 156     uint8_t ck0 = 0, ck1 = 0;
 157     header.sync_char1 = RUWAI_SYNC_CHAR1;
 158     header.sync_char2 = RUWAI_SYNC_CHAR2;
 159     header.msg_class = msg_class;
 160     header.msg_id = msg_id;
 161     header.payload_length = length;
 162
 163     // Update the checksum using the relevant header part.
 164     update_fletcher16_checksum((uint8_t *)&header.msg_class, sizeof(header) - 2, ck0, ck1);
 165
 166     // Update the checksum using the payload.
 167     update_fletcher16_checksum((uint8_t *)msg, length, ck0, ck1);
 168
 169     // Send the paket to the serial port.
 170     write(serial_port, (const uint8_t *)&header, sizeof(header));
 171     write(serial_port, (const uint8_t *)msg, length);
 172     write(serial_port, &ck0, 1);
 173     write(serial_port, &ck1, 1);
 174 }
 175
 176 bool
 177 SerialParser::wait_for_cfg_ack(uint8_t msg_class, uint8_t msg_id)
 178 {
 179     float timeout = 5;         // The timeout [s].
 180     auto start = std::chrono::high_resolution_clock::now();
 181
 182     float diff_sec = 0;
 183
 184     while ((last_ack_msg_class != msg_class) && (last_ack_msg_id != msg_id))
 185     {
 186         //std::cout << "last_ack_msg_id: " << last_ack_msg_id << std::endl;
 187         //std::cout << "msg_id: " << msg_id << std::endl;
 188         auto end = std::chrono::high_resolution_clock::now();
 189         auto diff = end - start;
 190         diff_sec = (float)std::chrono::duration_cast<std::chrono::seconds>(diff).count();
 191
 192         if (diff_sec > timeout)
 193         {
 194             last_ack_msg_class = -1;
 195             last_ack_msg_id = -1;
 196             return false;
 197         }
 198         usleep(100000);
 199     }
 200     last_ack_msg_class = -1;
 201     last_ack_msg_id = -1;
 202     return true;
 203 }
 204
 205 bool
 206 SerialParser::set_sps(uint16_t sps)
 207 {
 208     bool confirmed = false;
 209     rw_cfg_sps_t msg;
 210     msg.sps = sps;
 211     send_ruwai_message(RW_CLASS_CFG, RW_CFG_ID_SPS, &msg, sizeof(msg));
 212     confirmed = wait_for_cfg_ack(RW_CLASS_CFG, RW_CFG_ID_SPS);
 213     return confirmed;
 214 }
 215
 216
 217 bool
 218 SerialParser::set_gps_ports(bool usb_active)
 219 {
 220     bool confirmed = false;
 221     rw_cfg_gps_t msg;
 222
 223     if (usb_active)
 224         msg.ports.usb = 1;
 225
 226     send_ruwai_message(RW_CLASS_CFG, RW_CFG_ID_GPS, &msg, sizeof(msg));
 227     confirmed = wait_for_cfg_ack(RW_CLASS_CFG, RW_CFG_ID_GPS);
 228     return confirmed;
 229 }
 230
 231
 232 bool
 233 SerialParser::set_channels(bool c1_active, bool c2_active, bool c3_active, bool c4_active, pga_gain_t c1_gain, pga_gain_t c2_gain, pga_gain_t c3_gain, pga_gain_t c4_gain)
 234 {
 235     bool confirmed = false;
 236     rw_cfg_channels_t msg;
 237
 238     msg.active.reserved = 0;
 239
 240     if (c1_active)
 241         msg.active.channel1 = 1;
 242     else
 243         msg.active.channel1 = 0;
 244
 245     if (c2_active)
 246         msg.active.channel2 = 1;
 247     else
 248         msg.active.channel2 = 0;
 249
 250     if (c3_active)
 251         msg.active.channel3 = 1;
 252     else
 253         msg.active.channel3 = 0;
 254
 255     if (c4_active)
 256         msg.active.channel4 = 1;
 257     else
 258         msg.active.channel4 = 0;
 259
 260     msg.gain_channel1 = c1_gain;
 261     msg.gain_channel2 = c2_gain;
 262     msg.gain_channel3 = c3_gain;
 263     msg.gain_channel4 = c4_gain;
 264
 265     send_ruwai_message(RW_CLASS_CFG, RW_CFG_ID_CHANNELS, &msg, sizeof(msg));
 266     confirmed = wait_for_cfg_ack(RW_CLASS_CFG, RW_CFG_ID_CHANNELS);
 267     return confirmed;
 268 }
 269
 270
 271 bool
 272 SerialParser::set_mode(uint8_t mode)
 273 {
 274     bool confirmed = false;
 275     rw_cfg_mode_t msg;
 276     msg.mode = mode;
 277
 278     send_ruwai_message(RW_CLASS_CFG, RW_CFG_ID_MODE, &msg, sizeof(msg));
 279     confirmed = wait_for_cfg_ack(RW_CLASS_CFG, RW_CFG_ID_MODE);
 280     return confirmed;
 281 }
 282
 283
 284
 285 void
 286 SerialParser::read_and_parse(void)
 287 {
 288     int bytes_read;
 289     int count = 0;
 290     const int buffer_size = 256;
 291     float timeout_limit = 1;      // The maximum timeout in seconds.
 292     bool timeout = false;
 293     int timeout_cnt = 0;
 294     float read_diff = 0;
 295     //const int parser_size = 10;
 296     std::uint8_t data_buffer[buffer_size];
 297
 298     for(int j = 0; j < buffer_size; j++)
 299     {
 300         data_buffer[j] = 0;
 301     }
 302
 303     // Flush the buffer one more time.
 304     tcflush(serial_port, TCIOFLUSH);
 305
 306     auto last_read = std::chrono::high_resolution_clock::now();
 307     while(!timeout)
 308     {
 309             //ioctl(serial_port, FIONREAD, &bytes_available);
 310             //syslog(LOG_NOTICE, "[read_and_parse] before read");
 311             bytes_read = read(serial_port, data_buffer, buffer_size);
 312             //syslog(LOG_NOTICE, "[read_and_parse] after read");
 313             //syslog(LOG_NOTICE, "[read_and_parse] bytes_read: %d.", bytes_read);
 314             //std::cout << "Bytes read: " << bytes_read << "\n";
 315
 316             /*
 317             std::cout << "serial buffer size: " << dec << serial_buffer.size() << std::endl;
 318             for(int j = 0; j < serial_buffer.size(); j++)
 319             {
 320                 std::cout << hex << int(serial_buffer[j]) << dec << " ";
 321             }
 322             std::cout << std::endl;
 323             */
 324
 325             if (bytes_read > 0)
 326             {
 327                 last_read = std::chrono::high_resolution_clock::now();
 328
 329                 for(int j = 0; j < bytes_read; j++)
 330                 {
 331                     serial_buffer.push_back(data_buffer[j]);
 332                     //std::stringstream stream;
 333                     //stream << std::hex << int(data_buffer[j]);
 334                     //std::string result( stream.str() );
 335                     //syslog(LOG_DEBUG, "[read_and_parse] byte: %s", result.c_str());
 336                     //std::cout << std::hex << int(data_buffer[j]) << std::dec << " ";
 337                 }
 338                 //std::cout << std::endl;
 339             }
 340
 341             /*
 342             std::cout << "serial buffer size: " << serial_buffer.size() << std::endl;
 343             for(int j = 0; j < serial_buffer.size(); j++)
 344             {
 345                 std::cout << hex << int(serial_buffer[j]) << dec << " ";
 346             }
 347             std::cout << std::endl;
 348             */
 349             //syslog(LOG_DEBUG, "[read_and_parse] parser_size: %d; serial_buffer.size(): %d.", parser_size, serial_buffer.size());
 350             if (serial_buffer.size() > parser_size)
 351             {
 352                 syslog(LOG_DEBUG, "[read_and_parse] parser_size: %d; serial_buffer.size(): %d.", parser_size, (int)serial_buffer.size());
 353                 //auto start = std::chrono::high_resolution_clock::now();
 354                 parse_serial_buffer();
 355                 //auto end = std::chrono::high_resolution_clock::now();
 356                 //auto diff = end - start;
 357
 358                 //std::cout.precision(10);
 359                 //std::cout << "Parsed the pakets in " << std::chrono::duration_cast<std::chrono::nanoseconds>(diff).count() << " nanoseconds.\n";
 360                 //std::cout.precision(3);
 361                 syslog(LOG_DEBUG, "[read_and_parse] after parsing ---- parser_size: %d; serial_buffer.size(): %d.", parser_size, (int)serial_buffer.size());
 362                         uint32_t max_message_size = 256;
 363                         if (serial_buffer.size() > max_message_size)
 364                         {
 365                                 serial_buffer.erase(serial_buffer.begin(), serial_buffer.end() - sizeof(rw_header_t));
 366                                 syslog(LOG_DEBUG, "[read_and_parse] cropping the buffer ---- parser_size: %d; serial_buffer.size(): %d.", parser_size, (int)serial_buffer.size());
 367                 }
 368
 369                 count++;
 370             }
 371
 372             // Check for timeout.
 373             /*
 374             if (record_mode)
 375             {
 376                 if (timeout_cnt > 10)
 377                 {
 378                      auto cur_time = std::chrono::high_resolution_clock::now();
 379                      auto diff = cur_time - last_read;
 380                      read_diff = (float)std::chrono::duration_cast<std::chrono::seconds>(diff).count();
 381                      //syslog(LOG_NOTICE, "read_diff: %f.", read_diff);
 382                      if (read_diff > timeout_limit)
 383                      {
 384                          syslog(LOG_ERR, "[read_and_parse] Timeout in record mode. No data received for %f seconds.", read_diff);
 385                          timeout = true;
 386                      }
 387                      timeout_cnt = 0;
 388                 }
 389                 timeout_cnt++;
 390            }
 391            */
 392     }
 393 }
 394
 395 void
 396 SerialParser::parse_serial_buffer(void)
 397 {
 398     if (serial_buffer.size() == 0) {
 399         //std::cout << "Serial buffer is empty. Skip parsing." << std::endl;
 400         return;
 401     }
 402
 403     std::uint8_t msg_class;
 404     std::uint8_t msg_id;
 405     std::uint16_t payload_length;
 406
 407     std::vector<std::uint8_t> sync_sequ;
 408     sync_sequ.push_back(RUWAI_SYNC_CHAR1);
 409     sync_sequ.push_back(RUWAI_SYNC_CHAR2);
 410
 411     std::vector<std::uint8_t>::iterator sync_pos = serial_buffer.begin();
 412     std::vector<std::uint8_t>::iterator last_sync_pos = sync_pos;
 413     std::vector<std::uint8_t>::iterator payload_pos = sync_pos;
 414
 415     while (sync_pos != serial_buffer.end()) {
 416         sync_pos = std::search(sync_pos, serial_buffer.end(), sync_sequ.begin(), sync_sequ.end());
 417         if (sync_pos != serial_buffer.end()) {
 418             //std::cout << "SYNC CHAR found: " << std::hex << static_cast<int>(*sync_pos) << ' ' << static_cast<int>(*(sync_pos + 1)) << std::dec << std::endl;
 419             //std::cout << "iterator diff: " << serial_buffer.end() - sync_pos << std::endl;
 420             if (serial_buffer.end() - sync_pos <= 6) {
 421                 // The remaining bytes don't represent a complete paket.
 422                 sync_pos = serial_buffer.end();
 423             }
 424             else {
 425                 msg_class = *(sync_pos + 2);
 426                 msg_id = *(sync_pos + 3);
 427                 payload_length = (*(sync_pos + 5) << 8) | *(sync_pos + 4);
 428                 //std::cout << "msg_class: " << static_cast<int>(msg_class) << std::endl;
 429                 //std::cout << "msg_id: " << static_cast<int>(msg_id) << std::endl;
 430                 //std::cout << "payload length: " << payload_length << std::endl;
 431                 payload_pos = sync_pos + 6;
 432                 if (distance(payload_pos, serial_buffer.end()) < (payload_length + 2)) {
 433                     // Not enough bytes read for this paket.
 434                     //std::cout << "not enough bytes for this paket in the buffer" << std::endl;
 435                     break;
 436                 }
 437
 438                 std::vector<uint8_t> payload;
 439                 for (int i = 0; i < payload_length; i++)
 440                 {
 441                     //payload.bytes[i] = *(payload_pos + i);
 442                     payload.push_back(*(payload_pos + i));
 443                 }
 444
 445                 if (record_mode && (msg_class == RW_CLASS_SMP)) {
 446
 447                     if (msg_id == RW_SMP_ID_24BIT)
 448                     {
 449                         parse_smp_24bit(payload);
 450                         alarm(1);
 451                     }
 452                     else if (msg_id == RW_SMP_ID_24BIT_TSP)
 453                     {
 454                         parse_smp_24bit_tsp(payload);
 455                         alarm(1);
 456                     }
 457                 }
 458                 else if (msg_class == RW_CLASS_ACK) {
 459                     if (msg_id == RW_ACK_ID_SYNC)
 460                     {
 461                         sync(payload);
 462                     }
 463                     else if (msg_id == RW_ACK_ID_CFG)
 464                     {
 465                         rw_ack_cfg_t paket;
 466                         assert(payload.size() == sizeof paket);
 467                         std::copy(payload.begin(), payload.end(), reinterpret_cast<uint8_t*>(&paket));
 468                         syslog(LOG_DEBUG, "Received ACK_ID_CFG for msg_class ###: %d; msg_id: %d.\n", paket.msg_class, paket.msg_id);
 469                         last_ack_msg_class = paket.msg_class;
 470                         last_ack_msg_id = paket.msg_id;
 471                     }
 472                 }
 473                 else if (msg_class == RW_CLASS_SOH) {
 474                     syslog(LOG_INFO, "Received a RW_CLASS_SOH message.");
 475                     if (msg_id == RW_SOH_ID_STATUS)
 476                     {
 477                         rw_soh_status_t paket;
 478                         std::copy(payload.begin(), payload.end(), reinterpret_cast<uint8_t*>(&paket));
 479                         syslog(LOG_NOTICE, "[as_status][type: %d] %s", paket.type, paket.status);
 480                     }
 481                     else if (msg_id == RW_SOH_ID_GPSPOS)
 482                     {
 483                         rw_soh_gpspos_t paket;
 484                         std::copy(payload.begin(), payload.end(), reinterpret_cast<uint8_t*>(&paket));
 485                         syslog(LOG_NOTICE, "[soh][gps_pos] %f, %f, %f", paket.lon * 1e-7, paket.lat * 1e-7, (float)paket.height / 1000);
 486                     }
 487                     else if (msg_id == RW_SOH_ID_ENVDATA)
 488                     {
 489                         rw_soh_envdata_t paket;
 490                         std::copy(payload.begin(), payload.end(), reinterpret_cast<uint8_t*>(&paket));
 491                         syslog(LOG_NOTICE, "[soh][temperature_gts] %f", (float) paket.temp_gts / 100);
 492                     }
 493                 }
 494                 sync_pos = payload_pos + payload_length + 2;
 495                 last_sync_pos = sync_pos;
 496             }
 497         }
 498         else {
 499             //std::cout << "SYNC_CHAR not found." << std::endl;
 500             /*
 501             for (int i = 0; i < serial_buffer.size(); i++) {
 502                 std::cout << hex << static_cast<int>(serial_buffer[i]) << " ";
 503             }
 504             std::cout << "\n\n";
 505             */
 506         }
 507     }
 508
 509
 510     //std::cout << "distance: " << distance(serial_buffer.begin(), last_sync_pos) << std::endl;
 511     if (std::distance(serial_buffer.begin(), last_sync_pos) > 0) {
 512         serial_buffer.erase(serial_buffer.begin(), last_sync_pos);
 513         //std::cout << "buffer size after erase: " << serial_buffer.size() << std::endl;
 514         /*
 515         for (int i = 0; i < serial_buffer.size(); i++) {
 516             std::cout << hex << static_cast<int>(serial_buffer[i]) << " ";
 517         }
 518         std::cout << "\n\n";
 519         */
 520     }
 521 }
 522
 523
 524 bool
 525 SerialParser::parse_smp_24bit(std::vector<uint8_t> payload) {
 526     rw_smp_24bit_t paket;
 527     assert(payload.size() == sizeof paket);
 528     std::copy(payload.begin(), payload.end(), reinterpret_cast<uint8_t*>(&paket));
 529     recorder->add_sample(paket);
 530     return true;
 531 }
 532
 533
 534 bool
 535 SerialParser::parse_smp_24bit_tsp(std::vector<uint8_t> payload) {
 536     rw_smp_24bit_tsp_t paket;
 537     assert(payload.size() == sizeof paket);
 538     std::copy(payload.begin(), payload.end(), reinterpret_cast<uint8_t*>(&paket));
 539     recorder->add_timestamp_sample(paket);
 540     return true;
 541 }
 542