src/lidars/hokuyo/lib/urg_ctrl.c

   1 /*!
   2   \file
   3   \brief URG control
   4
   5   \author Satofumi KAMIMURA
   6
   7   $Id: urg_ctrl.c 1714 2010-02-21 20:53:28Z satofumi $
   8 */
   9
  10 #include "math_utils.h"
  11 #include "urg_ctrl.h"
  12 #include "scip_handler.h"
  13 #include "urg_errno.h"
  14 #include "serial_ctrl.h"
  15 #include "serial_utils.h"
  16 #include "serial_errno.h"
  17 #include "ticks.h"
  18 #include "delay.h"
  19 #include <string.h>
  20 #include <stdlib.h>
  21 #include <stdio.h>
  22
  23 #if defined(WINDOWS_OS)
  24 #define snprintf _snprintf
  25 #endif
  26
  27
  28 enum {
  29   ScipTimeout = 1000,      /*!< [msec] */
  30   EachTimeout = 100 * 2,   /*!< URG series timeout x 2 [msec] */
  31
  32   InvalidRange = -1,
  33 };
  34
  35
  36 void urg_initialize(urg_t *urg)
  37 {
  38   serial_initialize(&urg->serial_);
  39   urg->errno_ = UrgNoError;
  40   urg->last_timestamp_ = 0;
  41 }
  42
  43
  44 static int urg_firstConnection(urg_t *urg, long baudrate)
  45 {
  46   long try_baudrates[] = { 115200, 19200, 38400 };
  47   int try_size = sizeof(try_baudrates) / sizeof(try_baudrates[0]);
  48   int pre_ticks;
  49   int reply = 0;
  50   int ret;
  51   int i;
  52
  53   /* The baud rate to be connected is replaced with the first element
  54      of the array. */
  55   for (i = 1; i < try_size; ++i) {
  56     if (baudrate == try_baudrates[i]) {
  57       long swap_tmp = try_baudrates[i];
  58       try_baudrates[i] = try_baudrates[0];
  59       try_baudrates[0] = swap_tmp;
  60       break;
  61     }
  62   }
  63
  64   /* Try to connect with the specified baudrate , and check for response */
  65   for (i = 0; i < try_size; ++i) {
  66
  67     /* Change host side baudrate  */
  68     ret = serial_setBaudrate(&urg->serial_, try_baudrates[i]);
  69     if (ret < 0) {
  70       return ret;
  71     }
  72
  73     serial_clear(&urg->serial_);
  74
  75     /* Send QT command */
  76     ret = scip_qt(&urg->serial_, &reply, ScipWaitReply);
  77     if (ret == UrgSerialRecvFail) {
  78       /* If there is no response, consider that there is mismatch in baudrate */
  79       continue;
  80     }
  81
  82     if ((ret == UrgMismatchResponse) && (reply != -0xE)) {
  83       /* Process when response from MD/MS command is received  */
  84       /* Read out all data and then proceed for the next one */
  85       /* (reply == -0xE) means SCIP1.1 error 'E' */
  86       serial_clear(&urg->serial_);
  87       serial_skip(&urg->serial_, ScipTimeout, EachTimeout);
  88       reply = 0x00;
  89     }
  90
  91     /* If the response is returned, consider that sensor is already in SCIP2.0
  92        mode and no need to send SCIP2.0 */
  93     if (reply != 0x00) {
  94       if ((ret = scip_scip20(&urg->serial_)) < 0) {
  95         /* If there is no response , continue with other baudrate */
  96         continue;
  97       }
  98       if (ret == 12) {
  99         /* SCIP1.1 protocol */
 100         return UrgScip10;
 101       }
 102     }
 103
 104     /* Returns if there is no need to change baudrate */
 105     if (baudrate == try_baudrates[i]) {
 106       return 0;
 107     }
 108
 109     /* Change the baud rate as specified by URG */
 110     pre_ticks = ticks();
 111     if (scip_ss(&urg->serial_, baudrate) < 0) {
 112       return UrgSsFail;
 113
 114     } else {
 115       /* In case of serial communication, it is necessary to wait for
 116          one scan after baud rate is changed. */
 117       int reply_msec = ticks() - pre_ticks;
 118       delay((reply_msec * 4 / 3) + 10);
 119
 120       return serial_setBaudrate(&urg->serial_, baudrate);
 121     }
 122   }
 123
 124   return UrgAdjustBaudrateFail;
 125 }
 126
 127
 128 static void urg_t_initialize(urg_t *urg)
 129 {
 130   urg->parameters_.area_max_ = 0;
 131   urg->parameters_.scan_rpm_ = 0;
 132   urg->parameters_.sensor_type[0] = '\0';
 133   urg->remain_byte_ = 0;
 134 }
 135
 136
 137 /* Open serial device and initialize URG */
 138 int urg_connect(urg_t *urg, const char *device, long baudrate)
 139 {
 140   int ret;
 141   urg_t_initialize(urg);
 142
 143   /* Open serial communication */
 144   ret = serial_connect(&urg->serial_, device, baudrate);
 145   if (ret != 0) {
 146     urg->errno_ = UrgSerialConnectionFail;
 147     return ret;
 148   }
 149   // change timestamp resolution in Windows OS
 150   delay(0);
 151
 152   /* URG connection */
 153   ret = urg_firstConnection(urg, baudrate);
 154   if (ret < 0) {
 155     urg->errno_ = ret;
 156     serial_disconnect(&urg->serial_);
 157     return ret;
 158   }
 159
 160   /* Update parameter information, nothing but an initialization */
 161   ret = scip_pp(&urg->serial_, &urg->parameters_);
 162   if (ret < 0) {
 163     urg->errno_ = ret;
 164     serial_disconnect(&urg->serial_);
 165     return ret;
 166   }
 167   urg->skip_lines_ = 1;
 168   urg->skip_frames_ = 0;
 169   urg->capture_times_ = 0;
 170   urg->is_laser_on_ = UrgLaserUnknown;
 171   urg->remain_times_ = 0;
 172
 173   urg->errno_ = UrgNoError;
 174   return 0;
 175 }
 176
 177
 178 void urg_disconnect(urg_t *urg)
 179 {
 180   /* To stop MD/MS command */
 181   urg_laserOff(urg);
 182   serial_skip(&urg->serial_, ScipTimeout, EachTimeout);
 183
 184   /* Disconnect serial connection */
 185   serial_disconnect(&urg->serial_);
 186 }
 187
 188
 189 int urg_isConnected(const urg_t *urg)
 190 {
 191   /* Return 0, if serial connectionis valid */
 192   return serial_isConnected(&urg->serial_);
 193 }
 194
 195
 196 const char *urg_error(const urg_t *urg)
 197 {
 198   return urg_strerror(urg->errno_);
 199 }
 200
 201
 202 int urg_versionLines(urg_t *urg, char* lines[], int lines_max)
 203 {
 204   if (! urg_isConnected(urg)) {
 205     return -1;
 206   }
 207   return scip_vv(&urg->serial_, lines, lines_max);
 208 }
 209
 210
 211 /* Send PP command. Analyse and store the response */
 212 int urg_parameters(urg_t *urg, urg_parameter_t* parameters)
 213 {
 214   int ret = 0;
 215
 216   if (urg_isConnected(urg)) {
 217     *parameters = urg->parameters_;
 218   } else {
 219     ret = scip_pp(&urg->serial_, &urg->parameters_);
 220     if (parameters) {
 221       *parameters = urg->parameters_;
 222     }
 223   }
 224
 225   urg->errno_ = UrgNoError;
 226   return 0;
 227 }
 228
 229
 230 const char* urg_model(const urg_t *urg)
 231 {
 232   return urg->parameters_.sensor_type;
 233 }
 234
 235
 236 int urg_dataMax(const urg_t *urg)
 237 {
 238   return urg->parameters_.area_max_ + 1;
 239 }
 240
 241
 242 int urg_scanMsec(const urg_t *urg)
 243 {
 244   int scan_rpm = urg->parameters_.scan_rpm_;
 245   return (scan_rpm <= 0) ? 1 : (1000 * 60 / scan_rpm);
 246 }
 247
 248
 249 long urg_maxDistance(const urg_t *urg)
 250 {
 251   return urg->parameters_.distance_max_;
 252 }
 253
 254
 255 long urg_minDistance(const urg_t *urg)
 256 {
 257   return urg->parameters_.distance_min_;
 258 }
 259
 260
 261 int urg_setSkipLines(urg_t *urg, int lines)
 262 {
 263   /* Register number of lines to be skipped */
 264   if (lines == 0) {
 265     lines = 1;
 266   }
 267   if ((lines < 0) || (lines > 99)) {
 268     return -1;
 269   }
 270
 271   urg->skip_lines_ = lines;
 272   return 0;
 273 }
 274
 275
 276 int urg_setSkipFrames(urg_t *urg, int frames)
 277 {
 278   /* Register number of frames to be skipped */
 279   if ((frames < 0) || (frames > 9)) {
 280     return -1;
 281   }
 282
 283   urg->skip_frames_ = frames;
 284   return 0;
 285 }
 286
 287
 288 int urg_setCaptureTimes(urg_t *urg, int times)
 289 {
 290   /* Register frequency at which MD/MS data is received */
 291   if ((times < 0) || (times >= 100)) {
 292     urg->capture_times_ = 0;
 293   } else {
 294     urg->capture_times_ = times;
 295   }
 296
 297   return 0;
 298 }
 299
 300
 301 int urg_remainCaptureTimes(const urg_t *urg)
 302 {
 303   if (urg->capture_times_ == 0) {
 304     /* Get data infinitely */
 305     return 100;
 306
 307   } else {
 308     return urg->remain_times_;
 309   }
 310 }
 311
 312
 313 int urg_requestData(urg_t *urg,
 314                     urg_request_type request_type,
 315                     int first_index,
 316                     int last_index)
 317 {
 318   char buffer[] = "MDsssseeeellstt\n";
 319
 320   if (first_index == URG_FIRST) {
 321     first_index = urg->parameters_.area_min_;
 322   }
 323   if (last_index == URG_LAST) {
 324     last_index = urg->parameters_.area_max_;
 325   }
 326
 327   if ((request_type == URG_GD) || (request_type == URG_GS) ||
 328       (request_type == URG_GD_INTENSITY)) {
 329
 330     /* In case of GD/GS */
 331     snprintf(buffer, 14, "G%c%04d%04d%02d\n",
 332              (((request_type == URG_GD) ||
 333                (request_type == URG_GD_INTENSITY)) ? 'D' : 'S'),
 334              first_index, last_index,
 335              urg->skip_lines_);
 336
 337     /* Switch on the laser if laser is in off state */
 338     if (urg->is_laser_on_ != UrgLaserOn) {
 339       int ret = urg_laserOn(urg);
 340       if (ret < 0) {
 341         return ret;
 342       }
 343     }
 344
 345   } else if ((request_type == URG_MD) || (request_type == URG_MS) ||
 346              (request_type == URG_MD_INTENSITY)) {
 347     char type = (request_type == URG_MS) ? 'S' : 'D';
 348
 349     /* In case of MD/MS */
 350     snprintf(buffer, 17, "M%c%04d%04d%02d%d%02d\n",
 351              type,
 352              first_index, last_index,
 353              urg->skip_lines_,
 354              urg->skip_frames_,
 355              urg->capture_times_);
 356     urg->remain_times_ = urg->capture_times_;
 357
 358   } else {
 359     urg->errno_ = UrgInvalidArgs;;
 360     return urg->errno_;
 361   }
 362
 363   if ((request_type == URG_GD_INTENSITY) ||
 364       (request_type == URG_MD_INTENSITY)) {
 365     if (! strcmp("UTM-30LX", urg->parameters_.sensor_type)) {
 366       if (request_type == URG_GD_INTENSITY) {
 367         urg->errno_ = UtmNoGDIntensity;
 368         return urg->errno_;
 369       }
 370       /* use ME command in TOF series */
 371       buffer[0] = 'M';
 372       buffer[1] = 'E';
 373
 374       /* cluster value */
 375       buffer[10] = '0';
 376       buffer[11] = '2';
 377
 378     } else {
 379       /* AM series */
 380       buffer[10] = 'F';
 381       buffer[11] = 'F';
 382     }
 383   }
 384
 385   return scip_send(&urg->serial_, buffer);
 386 }
 387
 388
 389 /* Decode 6bit data of URG */
 390 static long decode(const char* data, int data_byte)
 391 {
 392   const char* p = data;
 393   const char* last_p = p + data_byte;
 394
 395   int value = 0;
 396   while (p < last_p) {
 397     value <<= 6;
 398     value &= ~0x3f;
 399     value |= *p++ - 0x30;
 400   }
 401   return value;
 402 }
 403
 404
 405 static int convertRawData(long data[], int data_max,
 406                           const char* buffer, int buffer_size, int filled,
 407                           int is_me_type, int data_bytes, int skip_lines,
 408                           int store_last, urg_t* urg)
 409 {
 410   int n;
 411   int i;
 412   int j;
 413   int remain_byte = urg->remain_byte_;
 414   long length;
 415   long *data_p = data + filled;
 416
 417   (void)is_me_type;
 418
 419   if (data_max > store_last) {
 420     data_max = store_last;
 421   }
 422
 423   if (filled == 0) {
 424     /* Initialize the number of data, which are remained when the first time
 425        is called  */
 426     remain_byte = 0;
 427   }
 428
 429   if (buffer_size <= 0) {
 430     return filled;
 431   }
 432
 433   /* If there is any data left, then process that data */
 434   if (remain_byte > 0) {
 435     memcpy(&urg->remain_data_[remain_byte], buffer, data_bytes - remain_byte);
 436     n = skip_lines;
 437     if ((filled + n) > data_max) {
 438       n = data_max - filled;
 439     }
 440     length = decode(urg->remain_data_, data_bytes);
 441     for (j = 0; j < n; ++j) {
 442       *data_p++ = length;
 443     }
 444     filled += n;
 445 #if 0
 446     if (is_me_type) {
 447       for (j = 0; j < skip_lines; ++j) {
 448         /* !!! */
 449       }
 450     }
 451 #endif
 452   }
 453
 454   /* Process one line of data */
 455   n = buffer_size - data_bytes;
 456   for (i = (data_bytes - remain_byte) % data_bytes; i <= n; i += data_bytes) {
 457     length = decode(&buffer[i], data_bytes);
 458     for (j = 0; j < skip_lines; ++j) {
 459       if (filled >= data_max) {
 460         return data_max;
 461       }
 462       *data_p++ = length;
 463       ++filled;
 464     }
 465
 466 #if 0
 467     if (is_me_type) {
 468       for (j = 0; j < skip_lines; ++j) {
 469         /* !!! */
 470       }
 471     }
 472 #endif
 473   }
 474
 475   /* Save the remaining data */
 476   urg->remain_byte_ = buffer_size - i;
 477   memcpy(urg->remain_data_, &buffer[i], urg->remain_byte_);
 478
 479   return filled;
 480 }
 481
 482
 483 static int checkSum(const char buffer[], int size, char actual_sum)
 484 {
 485   const char *p = buffer;
 486   const char *last_p = p + size;
 487   char expected_sum = 0x00;
 488
 489   while (p < last_p) {
 490     expected_sum += *p++;
 491   }
 492   expected_sum = (expected_sum & 0x3f) + 0x30;
 493
 494   return (expected_sum == actual_sum) ? 0 : -1;
 495 }
 496
 497
 498 static int atoi_substr(const char *str, size_t len)
 499 {
 500   char buffer[13];
 501
 502   strncpy(buffer, str, len);
 503   buffer[len] = '\0';
 504
 505   return atoi(buffer);
 506 }
 507
 508
 509 static int internal_receiveData(urg_t *urg, long data[], int data_max,
 510                                 int store_first, int store_last,
 511                                 int skip_lines)
 512 {
 513   enum {
 514     EchoBack = 0,
 515     ReplyCode,
 516     Timestamp,
 517
 518     False = 0,
 519     True = 1,
 520
 521     MD_MS_Length = 15,          /* Length of MD, MS */
 522     GD_GS_Length = 12,          /* Length of GD, GS */
 523   };
 524
 525   int lines = 0;
 526   char buffer[UrgLineWidth];
 527   int filled = 0;
 528   int is_echoback = False;
 529   int n;
 530
 531   int is_me_type = 0;
 532   char current_type[] = "xx";
 533   int current_first = -1;
 534   int current_last = -1;
 535   int current_skip_lines = -1;
 536   int current_skip_frames = -1;
 537   int current_capture_times = -1;
 538   int current_data_bytes = 3;
 539   int dummy_last;
 540   int timeout = ScipTimeout;
 541
 542   /* Initialization of time stamp */
 543   urg->last_timestamp_ = UrgInvalidTimestamp;
 544
 545   urg->errno_ = UrgNoResponse;
 546
 547   while (1) {
 548     n = serial_getLine(&urg->serial_, buffer, ScipLineWidth, timeout);
 549     if (n < 0)
 550             return -1;
 551     //fprintf(stderr, "%d: %s\n", ticks(), buffer);
 552     if (n <= 0) {
 553       if (is_echoback) {
 554         is_echoback = False;
 555         lines = 0;
 556         continue;
 557       }
 558       break;
 559     }
 560
 561     if (lines > 0) {
 562       /* ignore echoback */
 563       if (checkSum(buffer, n - 1, buffer[n - 1]) < 0) {
 564         urg->errno_ = UrgInvalidResponse;
 565         lines = 0;
 566         filled = 0;
 567         is_echoback = False;
 568         continue;
 569       }
 570     }
 571
 572     if (lines > Timestamp) {
 573       /* convert data */
 574       filled = convertRawData(data, data_max, buffer, n - 1, filled,
 575                               is_me_type, current_data_bytes, skip_lines,
 576                               store_last, urg);
 577
 578     } else if (lines == EchoBack) {
 579
 580       if ((n != GD_GS_Length) && (n != MD_MS_Length)) {
 581         /* Return if response is not GD/GS, MD/MS */
 582         urg->errno_ = UrgInvalidResponse;
 583         lines = 0;
 584         filled = 0;
 585         is_echoback = False;
 586         continue;
 587       }
 588       /* Response command */
 589       current_type[0] = buffer[0];
 590       current_type[1] = buffer[1];
 591       if (! strncmp("ME", current_type, 2)) {
 592         is_me_type = 1;
 593       }
 594
 595       /* Initialisation of receiving settings */
 596       current_first = atoi_substr(&buffer[2], 4);
 597       current_last = atoi_substr(&buffer[6], 4);
 598       current_skip_lines = atoi_substr(&buffer[10], 2);
 599
 600       if ((current_first - store_first) >= data_max) {
 601         /* no data */
 602         return 0;
 603       }
 604
 605       /* Arrangement of dummy data */
 606       dummy_last = current_first - store_first;
 607       for (filled = 0; filled < dummy_last; ++filled) {
 608         data[filled] = InvalidRange;
 609       }
 610
 611       if (n == GD_GS_Length) {
 612         /* Ignore receive frame settings and number of frames settings for
 613            GD/GS command */
 614         urg->remain_times_ = 0;
 615
 616       } else {
 617         current_skip_frames = atoi_substr(&buffer[12], 1);
 618         current_capture_times = atoi_substr(&buffer[13], 2);
 619
 620         /* In case of MD/MS, store the remaining number of scans. */
 621         urg->remain_times_ = atoi(&buffer[13]);
 622       }
 623       current_data_bytes = (current_type[1] == 'S') ? 2 : 3;
 624
 625     } else if (lines == ReplyCode) {
 626       if (! strncmp(buffer, "10", 2)) {
 627         urg->is_laser_on_ = UrgLaserOff;
 628       }
 629
 630       /* If response is "0B", ignore all response. Because there is a
 631          possibility that the correspondence of the response shifts. */
 632       if (! strncmp(buffer, "0B", 2)) {
 633         serial_skip(&urg->serial_, ScipTimeout, timeout);
 634       }
 635
 636       /* In case of MD/MS, response = "00" means transition request and hence
 637          readout one more line, and then reset the process */
 638       if (current_type[0] == 'M' && (! strncmp(buffer, "00", 2))) {
 639         is_echoback = True;
 640       }
 641
 642     } else if (lines == Timestamp) {
 643       urg->last_timestamp_ = decode(buffer, 4);
 644     }
 645
 646     ++lines;
 647     timeout = EachTimeout;
 648   }
 649
 650   if (filled <= 0) {
 651     return urg->errno_;
 652   } else {
 653 #if 0
 654     // fill to urg->parameters_.area_max_ or data_max
 655     int last_index = data_max;
 656     if (urg->parameters_.area_max_ < last_index) {
 657       last_index = urg->parameters_.area_max_;
 658     }
 659     for (; filled <= last_index; ++filled) {
 660       data[filled] = InvalidRagne;
 661     }
 662 #endif
 663
 664     return filled;
 665   }
 666 }
 667
 668
 669 int urg_receiveData(urg_t *urg, long data[], int data_max)
 670 {
 671   if (! urg_isConnected(urg)) {
 672     return -1;
 673   }
 674   return internal_receiveData(urg, data, data_max,
 675                               0, data_max, urg->skip_lines_);
 676 }
 677
 678
 679 int urg_receiveDataWithIntensity(urg_t *urg, long data[], int data_max,
 680                                  long intensity[])
 681 {
 682   int i;
 683   int n;
 684
 685   n = internal_receiveData(urg, data, data_max,
 686                            0, data_max, urg->skip_lines_);
 687
 688   for (i = 0; i < n; i += 2) {
 689     long length = data[i];
 690
 691     if ((i + 1) < data_max) {
 692       long intensity_value = data[i + 1];
 693       intensity[i] = intensity_value;
 694       intensity[i + 1] = intensity_value;
 695       data[i + 1] = length;
 696     }
 697   }
 698   return n;
 699 }
 700
 701
 702 int urg_receivePartialData(urg_t *urg, long data[], int data_max,
 703                            int first_index, int last_index)
 704 {
 705   return internal_receiveData(urg, data, data_max, first_index, last_index, 1);
 706 }
 707
 708
 709 long urg_recentTimestamp(const urg_t *urg)
 710 {
 711   /* Return latest time stamp */
 712   return urg->last_timestamp_;
 713 }
 714
 715
 716 double urg_index2rad(const urg_t *urg, int index)
 717 {
 718   double radian = (2.0 * M_PI) *
 719     (index - urg->parameters_.area_front_) / urg->parameters_.area_total_;
 720
 721   return radian;
 722 }
 723
 724
 725 int urg_index2deg(const urg_t *urg, int index)
 726 {
 727   int degree = (int)floor((urg_index2rad(urg, index) * 180 / M_PI) + 0.5);
 728
 729   return degree;
 730 }
 731
 732
 733 int urg_rad2index(const urg_t *urg, double radian)
 734 {
 735   int index =
 736     (int)floor((((radian * urg->parameters_.area_total_) / (2.0*M_PI))
 737                 + urg->parameters_.area_front_) + 0.5);
 738
 739   if (index < 0) {
 740     index = 0;
 741   } else if (index > urg->parameters_.area_max_) {
 742     index = urg->parameters_.area_max_;
 743   }
 744   return index;
 745 }
 746
 747
 748 int urg_deg2index(const urg_t *urg, int degree)
 749 {
 750   return urg_rad2index(urg, M_PI * degree / 180.0);
 751 }
 752
 753
 754 int urg_laserOn(urg_t *urg)
 755 {
 756   /* send BM command */
 757   int expected_ret[] = { 0, 2, -1 };
 758   int send_n = scip_send(&urg->serial_, "BM\n");
 759   if (send_n != 3) {
 760     /* !!! urg->errno = UrgSendFail; */
 761     return SerialSendFail;
 762   }
 763   if (scip_recv(&urg->serial_, "BM", NULL, expected_ret, ScipTimeout) == 0) {
 764     urg->is_laser_on_ = UrgLaserOn;
 765   }
 766
 767   return 0;
 768 }
 769
 770
 771 int urg_laserOff(urg_t *urg)
 772 {
 773   return scip_qt(&urg->serial_, NULL, ScipWaitReply);
 774 }
 775
 776
 777 int urg_reboot(urg_t *urg)
 778 {
 779   int expected_ret[][2] = {
 780     { 1, -1 },
 781     { 0, -1 },
 782   };
 783   int send_n;
 784   int recv_n;
 785   int i;
 786
 787   urg_laserOff(urg);
 788
 789   /* send RB twice */
 790   for (i = 0; i < 2; ++i) {
 791     send_n = scip_send(&urg->serial_, "RB\n");
 792     if (send_n != 3) {
 793       return SerialSendFail;
 794     }
 795
 796     recv_n = scip_recv(&urg->serial_, "RB", NULL,
 797                        expected_ret[i], ScipTimeout);
 798     if (recv_n < 0) {
 799       return recv_n;
 800     }
 801   }
 802
 803   /* disconnect immediately */
 804   urg_disconnect(urg);
 805
 806   return 0;
 807 }
 808
 809
 810 int urg_reset(urg_t *urg)
 811 {
 812   return urg_reboot(urg);
 813 }
 814
 815
 816 int urg_enableTimestampMode(urg_t *urg)
 817 {
 818   /* Send TM0 */
 819   int expected_ret[] = { 0, 2, -1 };
 820   int send_n = scip_send(&urg->serial_, "TM0\n");
 821   if (send_n != 4) {
 822     return SerialSendFail;
 823   }
 824   return scip_recv(&urg->serial_, "TM", NULL, expected_ret, ScipTimeout);
 825 }
 826
 827
 828 int urg_disableTimestampMode(urg_t *urg)
 829 {
 830   /* Send TM2 */
 831   int expected_ret[] = { 0, 3, -1 };
 832   int send_n = scip_send(&urg->serial_, "TM2\n");
 833   if (send_n != 4) {
 834     return SerialSendFail;
 835   }
 836   return scip_recv(&urg->serial_, "TM", NULL, expected_ret, ScipTimeout);
 837 }
 838
 839
 840 long urg_currentTimestamp(urg_t *urg)
 841 {
 842   char buffer[ScipLineWidth];
 843   long timestamp = -1;
 844   int ret = 0;
 845   int n;
 846
 847   /* Send TM1 */
 848   int expected_ret[] = { 0, -1 };
 849   int send_n = scip_send(&urg->serial_, "TM1\n");
 850   if (send_n != 4) {
 851     return SerialSendFail;
 852   }
 853   ret = scip_recv(&urg->serial_, "TM", NULL, expected_ret, ScipTimeout);
 854   if (ret != 0) {
 855     return ret;
 856   }
 857
 858   /* Decode the timestamp and return */
 859   n = serial_getLine(&urg->serial_, buffer, ScipLineWidth, ScipTimeout);
 860   if (n == 5) {
 861     timestamp = decode(buffer, 4);
 862   }
 863
 864   /* Read and throw the last response */
 865   n = serial_recv(&urg->serial_, buffer, 1, ScipTimeout);
 866   if (! serial_isLF(buffer[0])) {
 867     serial_ungetc(&urg->serial_, buffer[0]);
 868   }
 869
 870   return timestamp;
 871 }