static int urg_firstConnection(urg_t *urg, long baudrate) { long try_baudrates[] = { 115200, 19200, 38400 }; int try_size = sizeof(try_baudrates) / sizeof(try_baudrates[0]); long pre_ticks; int reply = 0; int ret; int i; /* The baud rate to be connected is replaced with the first element of the array. */ for (i = 1; i < try_size; ++i) { if (baudrate == try_baudrates[i]) { long swap_tmp = try_baudrates[i]; try_baudrates[i] = try_baudrates[0]; try_baudrates[0] = swap_tmp; break; } } /* Try to connect with the specified baudrate , and check for response */ for (i = 0; i < try_size; ++i) { /* Change host side baudrate */ ret = serial_setBaudrate(&urg->serial_, try_baudrates[i]); if (ret < 0) { return ret; } serial_clear(&urg->serial_); /* Send QT command */ ret = scip_qt(&urg->serial_, &reply, ScipWaitReply); if (ret == UrgSerialRecvFail) { /* If there is no response, consider that there is mismatch in baudrate */ continue; } if ((ret == UrgMismatchResponse) && (reply != -0xE)) { /* Process when response from MD/MS command is received */ /* Read out all data and then proceed for the next one */ /* (reply == -0xE) means SCIP1.1 error 'E' */ serial_clear(&urg->serial_); serial_skip(&urg->serial_, ScipTimeout, EachTimeout); reply = 0x00; } /* If the response is returned, consider that sensor is already in SCIP2.0 mode and no need to send SCIP2.0 */ if (reply != 0x00) { if ((ret = scip_scip20(&urg->serial_)) < 0) { /* If there is no response , continue with other baudrate */ continue; } if (ret == 12) { /* SCIP1.1 protocol */ return UrgScip10; } } /* Returns if there is no need to change baudrate */ if (baudrate == try_baudrates[i]) { return 0; } /* Change the baud rate as specified by URG */ pre_ticks = urg_ticks(); if (scip_ss(&urg->serial_, baudrate) < 0) { return UrgSsFail; } else { /* In case of serial communication, it is necessary to wait for one scan after baud rate is changed. */ long reply_msec = urg_ticks() - pre_ticks; urg_delay((reply_msec * 4 / 3) + 10); return serial_setBaudrate(&urg->serial_, baudrate); } } return UrgAdjustBaudrateFail; }
long ticks(void) { return urg_ticks(); }