src/hokuyo/lib/scip_handler.c

   1 /*!
   2   \file
   3   \brief Process SKIP commands
   4
   5   \author Satofumi KAMIMURA
   6
   7   $Id: scip_handler.c 1714 2010-02-21 20:53:28Z satofumi $
   8
   9   \todo Check the checksum of acquired line
  10   \todo Add an argument to distinguish the line that contain version information.
  11 */
  12
  13 #include "scip_handler.h"
  14 #include "serial_errno.h"
  15 #include "serial_ctrl.h"
  16 #include "serial_utils.h"
  17 #include "urg_errno.h"
  18 #include <stdio.h>
  19 #include <stdlib.h>
  20 #include <string.h>
  21 #include <ctype.h>
  22
  23 #if defined(WINDOWS_OS)
  24 #define snprintf _snprintf
  25 #endif
  26
  27 extern int snprintf(char *, size_t, const char *, ...);
  28
  29
  30 /*! \todo Standardize with  urg_ctrl.c */
  31 enum {
  32   ScipTimeout = 1000,           /*!< [msec] */
  33   EachTimeout = 100,            /*!< [msec] */
  34 };
  35
  36
  37 /* Send command */
  38 int scip_send(serial_t *serial, const char *send_command)
  39 {
  40   int n = (int)strlen(send_command);
  41   return serial_send(serial, send_command, n);
  42 }
  43
  44
  45 /*!
  46   \brief Receive the response from command
  47
  48   \todo Test the checksum
  49 */
  50 int scip_recv(serial_t *serial, const char *command_first,
  51               int* return_code, int expected_ret[], int timeout)
  52 {
  53   char recv_ch = '\0';
  54   int ret_code = 0;
  55   int n;
  56   int i;
  57
  58   /* Receive the response */
  59   char buffer[ScipLineWidth];
  60
  61   /* Skip the first response */
  62   n = serial_getLine(serial, buffer, ScipLineWidth, timeout);
  63   if (n < 0) {
  64     return UrgSerialRecvFail;
  65   }
  66
  67   /* ignore 0x00 response after connection */
  68   if (! ((n == 1) && (buffer[0] == 0x00))) {
  69     if (strncmp(buffer, command_first, 2)) {
  70       /* Treat as an error,if there is mismatch with sent characters */
  71       return UrgMismatchResponse;
  72     }
  73   }
  74
  75   /* Read and pass the response characters. */
  76   n = serial_getLine(serial, buffer, ScipLineWidth, timeout);
  77
  78   /* restore last character, and use next proccessing */
  79   n = serial_recv(serial, &recv_ch, 1, timeout);
  80   if ((n == 1) && (! serial_isLF(recv_ch))) {
  81     serial_ungetc(serial, recv_ch);
  82   }
  83
  84   /* Returns 0, if received response characters are as expected */
  85   ret_code = strtol(buffer, NULL, 16);
  86   if (return_code != NULL) {
  87     *return_code = ret_code;
  88   }
  89   for (i = 0; expected_ret[i] != -1; ++i) {
  90     if (ret_code == expected_ret[i]) {
  91       return 0;
  92     }
  93   }
  94   return ret_code;
  95 }
  96
  97
  98 /* Transition to SCIP 2.0 */
  99 int scip_scip20(serial_t *serial)
 100 {
 101   int expected_ret[] = { 0x0, 0xE, -1 };
 102   int ret;
 103
 104   ret = scip_send(serial, "SCIP2.0\n");
 105   if (ret != 8) {
 106     return ret;
 107   }
 108
 109   return scip_recv(serial, "SC", NULL, expected_ret, ScipTimeout);
 110 }
 111
 112
 113 /* Send QT command */
 114 int scip_qt(serial_t *serial, int *return_code, int wait_reply)
 115 {
 116   int expected_ret[] = { 0x0, -1 };
 117   int ret;
 118
 119   ret = scip_send(serial, "QT\n");
 120   if (ret != 3) {
 121     return ret;
 122   }
 123
 124   if (wait_reply == ScipNoWaitReply) {
 125     return 0;
 126   }
 127
 128   ret = scip_recv(serial, "QT", return_code, expected_ret, ScipTimeout);
 129   if (return_code && (*return_code == 0xE)) {
 130     *return_code = -(*return_code);
 131     return UrgScip10;
 132   }
 133
 134   return ret;
 135 }
 136
 137
 138 /* Get PP information */
 139 int scip_pp(serial_t *serial, urg_parameter_t *parameters)
 140 {
 141   int send_n;
 142   int ret = 0;
 143   int expected_reply[] = { 0x0, -1 };
 144   int n;
 145   int i;
 146
 147   char buffer[ScipLineWidth];
 148
 149   send_n = scip_send(serial, "PP\n");
 150   if (send_n != 3) {
 151     return SerialSendFail;
 152   }
 153
 154   /* Receive the response */
 155   ret = scip_recv(serial, "PP", NULL, expected_reply, ScipTimeout);
 156   if (ret < 0) {
 157     return ret;
 158   }
 159
 160   /* Reception of parameter characters  */
 161   for (i = 0; i < UrgParameterLines; ++i) {
 162     n = serial_getLine(serial, buffer, ScipLineWidth, ScipTimeout);
 163     if (n <= 0) {
 164       return ret;
 165     }
 166
 167     /* !!! It is necessary to check the character string like AMIN */
 168
 169     if (i == 0) {
 170       strncpy(parameters->sensor_type,  &buffer[5], 8);
 171       parameters->sensor_type[8] = '\0';
 172
 173     } else if (i == 1) {
 174       parameters->distance_min_ = atoi(&buffer[5]);
 175
 176     } else if (i == 2) {
 177       parameters->distance_max_ = atoi(&buffer[5]);
 178
 179     } else if (i == 3) {
 180       parameters->area_total_ = atoi(&buffer[5]);
 181
 182     } else if (i == 4) {
 183       parameters->area_min_ = atoi(&buffer[5]);
 184
 185     } else if (i == 5) {
 186       parameters->area_max_ = atoi(&buffer[5]);
 187
 188     } else if (i == 6) {
 189       parameters->area_front_ = atoi(&buffer[5]);
 190
 191     } else if (i == 7) {
 192       parameters->scan_rpm_ = atoi(&buffer[5]);
 193     }
 194   }
 195
 196   return 0;
 197 }
 198
 199
 200 /* Reception of VV response*/
 201 int scip_vv(serial_t *serial, char *lines[], int lines_max)
 202 {
 203   int send_n;
 204   int ret = 0;
 205   int expected_reply[] = { 0x0, -1 };
 206   int n;
 207   int i;
 208
 209   /* Initialize by an empty message */
 210   for (i = 0; i < lines_max; ++i) {
 211     *lines[i] = '\0';
 212   }
 213
 214   /* Send VV command */
 215   send_n = scip_send(serial, "VV\n");
 216   if (send_n != 3) {
 217     return SerialSendFail;
 218   }
 219
 220   /* Receive response */
 221   ret = scip_recv(serial, "VV", NULL, expected_reply, ScipTimeout);
 222   if (ret < 0) {
 223     return ret;
 224   }
 225
 226   /* Receive version information */
 227   for (i = 0; i < lines_max; ++i) {
 228     n = serial_getLine(serial, lines[i], ScipLineWidth, ScipTimeout);
 229     if (n <= 0) {
 230       return ret;
 231     }
 232   }
 233
 234   serial_skip(serial, ScipTimeout, EachTimeout);
 235   return ret;
 236 }
 237
 238
 239 /* Change baud rate according to SS command */
 240 int scip_ss(serial_t *serial, long baudrate)
 241 {
 242   int expected_reply[] = { 0x0, 0x3, 0x4, -1 };
 243   int send_n;
 244   int ret;
 245
 246   /* !!! Should be treated as an error if baud rate is not with in range of
 247          defined range */
 248
 249   /* Send SS command */
 250   char buffer[] = "SSxxxxxx\n";
 251   snprintf(buffer, 10, "SS%06ld\n", baudrate);
 252   send_n = scip_send(serial, buffer);
 253   if (send_n != 9) {
 254     return SerialSendFail;
 255   }
 256
 257   /* Receive response */
 258   ret = scip_recv(serial, "SS", NULL, expected_reply, ScipTimeout);
 259   if (ret < 0) {
 260     return ret;
 261   }
 262
 263   return 0;
 264 }