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 bool
 217 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)
 218 {
 219     bool confirmed = false;
 220     rw_cfg_channels_t msg;
 221
 222     msg.active.reserved = 0;
 223
 224     if (c1_active)
 225         msg.active.channel1 = 1;
 226     else
 227         msg.active.channel1 = 0;
 228
 229     if (c2_active)
 230         msg.active.channel2 = 1;
 231     else
 232         msg.active.channel2 = 0;
 233
 234     if (c3_active)
 235         msg.active.channel3 = 1;
 236     else
 237         msg.active.channel3 = 0;
 238
 239     if (c4_active)
 240         msg.active.channel4 = 1;
 241     else
 242         msg.active.channel4 = 0;
 243
 244     msg.gain_channel1 = c1_gain;
 245     msg.gain_channel2 = c2_gain;
 246     msg.gain_channel3 = c3_gain;
 247     msg.gain_channel4 = c4_gain;
 248
 249     send_ruwai_message(RW_CLASS_CFG, RW_CFG_ID_CHANNELS, &msg, sizeof(msg));
 250     confirmed = wait_for_cfg_ack(RW_CLASS_CFG, RW_CFG_ID_CHANNELS);
 251     return confirmed;
 252 }
 253
 254
 255 bool
 256 SerialParser::set_mode(uint8_t mode)
 257 {
 258     bool confirmed = false;
 259     rw_cfg_mode_t msg;
 260     msg.mode = mode;
 261
 262     send_ruwai_message(RW_CLASS_CFG, RW_CFG_ID_MODE, &msg, sizeof(msg));
 263     confirmed = wait_for_cfg_ack(RW_CLASS_CFG, RW_CFG_ID_MODE);
 264     return confirmed;
 265 }
 266
 267
 268
 269 void
 270 SerialParser::read_and_parse(void)
 271 {
 272     int bytes_read;
 273     int count = 0;
 274     const int buffer_size = 256;
 275     float timeout_limit = 1;      // The maximum timeout in seconds.
 276     bool timeout = false;
 277     int timeout_cnt = 0;
 278     float read_diff = 0;
 279     //const int parser_size = 10;
 280     std::uint8_t data_buffer[buffer_size];
 281
 282     for(int j = 0; j < buffer_size; j++)
 283     {
 284         data_buffer[j] = 0;
 285     }
 286
 287     // Flush the buffer one more time.
 288     tcflush(serial_port, TCIOFLUSH);
 289
 290     auto last_read = std::chrono::high_resolution_clock::now();
 291     while(!timeout)
 292     {
 293             //ioctl(serial_port, FIONREAD, &bytes_available);
 294             //syslog(LOG_NOTICE, "[read_and_parse] before read");
 295             bytes_read = read(serial_port, data_buffer, buffer_size);
 296             //syslog(LOG_NOTICE, "[read_and_parse] after read");
 297             //syslog(LOG_NOTICE, "[read_and_parse] bytes_read: %d.", bytes_read);
 298             //std::cout << "Bytes read: " << bytes_read << "\n";
 299
 300             /*
 301             std::cout << "serial buffer size: " << dec << serial_buffer.size() << std::endl;
 302             for(int j = 0; j < serial_buffer.size(); j++)
 303             {
 304                 std::cout << hex << int(serial_buffer[j]) << dec << " ";
 305             }
 306             std::cout << std::endl;
 307             */
 308
 309             if (bytes_read > 0)
 310             {
 311                 last_read = std::chrono::high_resolution_clock::now();
 312
 313                 for(int j = 0; j < bytes_read; j++)
 314                 {
 315                     serial_buffer.push_back(data_buffer[j]);
 316                     //std::stringstream stream;
 317                     //stream << std::hex << int(data_buffer[j]);
 318                     //std::string result( stream.str() );
 319                     //syslog(LOG_DEBUG, "[read_and_parse] byte: %s", result.c_str());
 320                     //std::cout << std::hex << int(data_buffer[j]) << std::dec << " ";
 321                 }
 322                 //std::cout << std::endl;
 323             }
 324
 325             /*
 326             std::cout << "serial buffer size: " << serial_buffer.size() << std::endl;
 327             for(int j = 0; j < serial_buffer.size(); j++)
 328             {
 329                 std::cout << hex << int(serial_buffer[j]) << dec << " ";
 330             }
 331             std::cout << std::endl;
 332             */
 333             //syslog(LOG_DEBUG, "[read_and_parse] parser_size: %d; serial_buffer.size(): %d.", parser_size, serial_buffer.size());
 334             if (serial_buffer.size() > parser_size)
 335             {
 336                 syslog(LOG_DEBUG, "[read_and_parse] parser_size: %d; serial_buffer.size(): %d.", parser_size, (int)serial_buffer.size());
 337                 //auto start = std::chrono::high_resolution_clock::now();
 338                 parse_serial_buffer();
 339                 //auto end = std::chrono::high_resolution_clock::now();
 340                 //auto diff = end - start;
 341
 342                 //std::cout.precision(10);
 343                 //std::cout << "Parsed the pakets in " << std::chrono::duration_cast<std::chrono::nanoseconds>(diff).count() << " nanoseconds.\n";
 344                 //std::cout.precision(3);
 345                 syslog(LOG_DEBUG, "[read_and_parse] after parsing ---- parser_size: %d; serial_buffer.size(): %d.", parser_size, (int)serial_buffer.size());
 346                         uint32_t max_message_size = 256;
 347                         if (serial_buffer.size() > max_message_size)
 348                         {
 349                                 serial_buffer.erase(serial_buffer.begin(), serial_buffer.end() - sizeof(rw_header_t));
 350                                 syslog(LOG_DEBUG, "[read_and_parse] cropping the buffer ---- parser_size: %d; serial_buffer.size(): %d.", parser_size, (int)serial_buffer.size());
 351                 }
 352
 353                 count++;
 354             }
 355
 356             if (record_mode)
 357             {
 358                 alarm(1);
 359             }
 360             // Check for timeout.
 361             /*
 362             if (record_mode)
 363             {
 364                 if (timeout_cnt > 10)
 365                 {
 366                      auto cur_time = std::chrono::high_resolution_clock::now();
 367                      auto diff = cur_time - last_read;
 368                      read_diff = (float)std::chrono::duration_cast<std::chrono::seconds>(diff).count();
 369                      //syslog(LOG_NOTICE, "read_diff: %f.", read_diff);
 370                      if (read_diff > timeout_limit)
 371                      {
 372                          syslog(LOG_ERR, "[read_and_parse] Timeout in record mode. No data received for %f seconds.", read_diff);
 373                          timeout = true;
 374                      }
 375                      timeout_cnt = 0;
 376                 }
 377                 timeout_cnt++;
 378            }
 379            */
 380     }
 381 }
 382
 383 void
 384 SerialParser::parse_serial_buffer(void)
 385 {
 386     if (serial_buffer.size() == 0) {
 387         //std::cout << "Serial buffer is empty. Skip parsing." << std::endl;
 388         return;
 389     }
 390
 391     std::uint8_t msg_class;
 392     std::uint8_t msg_id;
 393     std::uint16_t payload_length;
 394
 395     std::vector<std::uint8_t> sync_sequ;
 396     sync_sequ.push_back(RUWAI_SYNC_CHAR1);
 397     sync_sequ.push_back(RUWAI_SYNC_CHAR2);
 398
 399     std::vector<std::uint8_t>::iterator sync_pos = serial_buffer.begin();
 400     std::vector<std::uint8_t>::iterator last_sync_pos = sync_pos;
 401     std::vector<std::uint8_t>::iterator payload_pos = sync_pos;
 402
 403     while (sync_pos != serial_buffer.end()) {
 404         sync_pos = std::search(sync_pos, serial_buffer.end(), sync_sequ.begin(), sync_sequ.end());
 405         if (sync_pos != serial_buffer.end()) {
 406             //std::cout << "SYNC CHAR found: " << std::hex << static_cast<int>(*sync_pos) << ' ' << static_cast<int>(*(sync_pos + 1)) << std::dec << std::endl;
 407             //std::cout << "iterator diff: " << serial_buffer.end() - sync_pos << std::endl;
 408             if (serial_buffer.end() - sync_pos <= 6) {
 409                 // The remaining bytes don't represent a complete paket.
 410                 sync_pos = serial_buffer.end();
 411             }
 412             else {
 413                 msg_class = *(sync_pos + 2);
 414                 msg_id = *(sync_pos + 3);
 415                 payload_length = (*(sync_pos + 5) << 8) | *(sync_pos + 4);
 416                 //std::cout << "msg_class: " << static_cast<int>(msg_class) << std::endl;
 417                 //std::cout << "msg_id: " << static_cast<int>(msg_id) << std::endl;
 418                 //std::cout << "payload length: " << payload_length << std::endl;
 419                 payload_pos = sync_pos + 6;
 420                 if (distance(payload_pos, serial_buffer.end()) < (payload_length + 2)) {
 421                     // Not enough bytes read for this paket.
 422                     //std::cout << "not enough bytes for this paket in the buffer" << std::endl;
 423                     break;
 424                 }
 425
 426                 std::vector<uint8_t> payload;
 427                 for (int i = 0; i < payload_length; i++)
 428                 {
 429                     //payload.bytes[i] = *(payload_pos + i);
 430                     payload.push_back(*(payload_pos + i));
 431                 }
 432
 433                 if (record_mode && (msg_class == RW_CLASS_SMP)) {
 434
 435                     if (msg_id == RW_SMP_ID_24BIT)
 436                     {
 437                         parse_smp_24bit(payload);
 438                     }
 439                     else if (msg_id == RW_SMP_ID_24BIT_TSP)
 440                     {
 441                         parse_smp_24bit_tsp(payload);
 442                     }
 443                 }
 444                 else if (msg_class == RW_CLASS_ACK) {
 445                     if (msg_id == RW_ACK_ID_SYNC)
 446                     {
 447                         sync(payload);
 448                     }
 449                     else if (msg_id == RW_ACK_ID_CFG)
 450                     {
 451                         rw_ack_cfg_t paket;
 452                         assert(payload.size() == sizeof paket);
 453                         std::copy(payload.begin(), payload.end(), reinterpret_cast<uint8_t*>(&paket));
 454                         syslog(LOG_DEBUG, "Received ACK_ID_CFG for msg_class ###: %d; msg_id: %d.\n", paket.msg_class, paket.msg_id);
 455                         last_ack_msg_class = paket.msg_class;
 456                         last_ack_msg_id = paket.msg_id;
 457                     }
 458                 }
 459                 else if (msg_class == RW_CLASS_SOH) {
 460                     syslog(LOG_INFO, "Received a RW_CLASS_SOH message.");
 461                     if (msg_id == RW_SOH_ID_STATUS)
 462                     {
 463                         rw_soh_status_t paket;
 464                         std::copy(payload.begin(), payload.end(), reinterpret_cast<uint8_t*>(&paket));
 465                         syslog(LOG_NOTICE, "[as_status][type: %d] %s", paket.type, paket.status);
 466                     }
 467                     else if (msg_id == RW_SOH_ID_GPSPOS)
 468                     {
 469                         rw_soh_gpspos_t paket;
 470                         std::copy(payload.begin(), payload.end(), reinterpret_cast<uint8_t*>(&paket));
 471                         syslog(LOG_NOTICE, "[soh][gps_pos] %f, %f, %f", paket.lon * 1e-7, paket.lat * 1e-7, (float)paket.height / 1000);
 472                     }
 473                     else if (msg_id == RW_SOH_ID_ENVDATA)
 474                     {
 475                         rw_soh_envdata_t paket;
 476                         std::copy(payload.begin(), payload.end(), reinterpret_cast<uint8_t*>(&paket));
 477                         syslog(LOG_NOTICE, "[soh][temperature_gts] %f", (float) paket.temp_gts / 100);
 478                     }
 479                 }
 480                 sync_pos = payload_pos + payload_length + 2;
 481                 last_sync_pos = sync_pos;
 482             }
 483         }
 484         else {
 485             //std::cout << "SYNC_CHAR not found." << std::endl;
 486             /*
 487             for (int i = 0; i < serial_buffer.size(); i++) {
 488                 std::cout << hex << static_cast<int>(serial_buffer[i]) << " ";
 489             }
 490             std::cout << "\n\n";
 491             */
 492         }
 493     }
 494
 495
 496     //std::cout << "distance: " << distance(serial_buffer.begin(), last_sync_pos) << std::endl;
 497     if (std::distance(serial_buffer.begin(), last_sync_pos) > 0) {
 498         serial_buffer.erase(serial_buffer.begin(), last_sync_pos);
 499         //std::cout << "buffer size after erase: " << serial_buffer.size() << 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 bool
 511 SerialParser::parse_smp_24bit(std::vector<uint8_t> payload) {
 512     rw_smp_24bit_t paket;
 513     assert(payload.size() == sizeof paket);
 514     std::copy(payload.begin(), payload.end(), reinterpret_cast<uint8_t*>(&paket));
 515     recorder->add_sample(paket);
 516     return true;
 517 }
 518
 519
 520 bool
 521 SerialParser::parse_smp_24bit_tsp(std::vector<uint8_t> payload) {
 522     rw_smp_24bit_tsp_t paket;
 523     assert(payload.size() == sizeof paket);
 524     std::copy(payload.begin(), payload.end(), reinterpret_cast<uint8_t*>(&paket));
 525     recorder->add_timestamp_sample(paket);
 526     return true;
 527 }
 528