urg_ctrl.c

/*
  北陽電機製 測域センサ URG-X002S の基本操作クラス

  Satofumi KAMIMURA
  $Id$
*/

#include "urg_ctrl.h"
#include "connect_device.h"
#include "serial_device.h"
#include "tcpip_device.h"
#include <math.h>
#include "math_util.h"
#include <stdio.h>
#include <string.h>
#include <ctype.h>
#include "get_keyword.h"


enum {
  FALSE = 0,
  TRUE = 1,
};

#ifndef URG_CONFIG_FILE
#define URG_CONFIG_FILE "defaultargs"
#endif


#ifndef URG_AUTO_PORT
# ifdef LINUX
#  define URG_AUTO_PORT "/dev/ttyS"
# else
#  define URG_AUTO_PORT "COM"
# endif
#endif


static char *Error_device = "";
static long Error_baudrate = 0;
static char *Error_message = "connection device is not specified";
static int fd = -1;


static int isLF(char ch) {
  if ((ch == '\r') || (ch == '\n')) {
    return TRUE;
  }
  return FALSE;
}


static int configBaudrate(int id, long baudrate) {
  long expected_baudrate[] = { 19200, 115200, 57600 };
  char message[15];
  char pre_ch;
  int i;

  sprintf(message, "S%06ldXXXXXXX\r", baudrate);
  for (i = 0; i < 3; ++i) {
    char recv_buffer[18];
    int product_check;
    int lf_count = 0;
    char ch;
    int ret;
    int was_timeout;

    device_set_baudrate(id, expected_baudrate[i]);
    device_flush(id);

    // 'V' コマンドによるバージョンの確認
    device_send(id, "V\r", 2);
    device_send(id, message, 15);

    // タイムアウトを監視しつつ１文字ずつ受信し、改行を 3 つ読み飛ばす
    while ((ret = device_recv(id, &ch, 1, SCI_TIMEOUT)) > 0) {
      if (isLF(ch)) {
        if (++lf_count >= 3) {
          break;
        }
      }
    }
    was_timeout = (ret == 0) ? TRUE : FALSE;
    product_check = FALSE;
    if (lf_count >= 3) {
      // PROD行のメッセージを読み込む ("PROD:SOKUIKI Sensor URG")
      char expected_message[] = "PROD:SOKUIKI Sensor URG";
      char message_buffer[sizeof(expected_message)-1];
      if (device_recv(id, message_buffer, sizeof(expected_message)-1,
                      SCI_TIMEOUT) >= (int)sizeof(expected_message)-1) {
        int j;
        for (j = 0; j < (int)sizeof(expected_message)-1; ++j) {
          if (expected_message[j] != message_buffer[j]) {
            break;
          }
        }
        if (j >= (int)sizeof(expected_message)-1) {
          product_check = TRUE;
        }
      }
    }

    pre_ch = '\0';
    while ((!was_timeout) && (device_recv(id, &ch, 1, SCI_TIMEOUT)) > 0) {
      if (isLF(ch) && isLF(pre_ch)) {
        break;
      }
      pre_ch = ch;
    }

    if (product_check == FALSE) {
      continue;
    }

    if (device_recv(id, recv_buffer, 18, SCI_TIMEOUT) >= 15) {
      if (recv_buffer[15] == '0') {
        device_set_baudrate(id, baudrate);
        device_flush(id);
        if (!product_check) {
          return URG_PRODUCT_MISMATCH_ERROR;
        }
        return 0;
      }
    }
  }

  closeDevice(id);
  return URG_BAUDRATE_ADJUST_ERROR;
}


static int searchConfigFile(const char *path) {
  enum {
    CONFIG_PATH_LENGTH = 256,
  };
  char file_path[CONFIG_PATH_LENGTH +1];
  int path_length = strlen(path);
  int file_length = strlen(URG_CONFIG_FILE);
  long baudrate = URG_BAUDRATE;
  char *baudrate_str;
  char *deviceName;

  if ((path_length + 1 + file_length + 1) < CONFIG_PATH_LENGTH) {
    strcpy(file_path, path);
    strcpy(&file_path[path_length], "/");
    strcpy(&file_path[path_length +1], URG_CONFIG_FILE);
    file_path[path_length + 1 + file_length] = '\0';

    if ((baudrate_str = get_keyword(file_path, "urg_baudrate"))) {
      baudrate = atoi(baudrate_str);
      free(baudrate_str);
    }
    deviceName = get_keyword(file_path, "urg_port");
  }
  if (deviceName) {
    int ret_value = initConnectDevice(deviceName, baudrate, SERIAL);
    free(deviceName);
    if (ret_value >= 0) {
      fd = ret_value;
      ret_value = configBaudrate(fd, baudrate);
    }
    return ret_value;
  }
  return NO_FILE_ERROR;
}


static int connectDefault(void) {
  int ret_value = searchConfigFile(".");
  char *home;
  long baudrate = URG_BAUDRATE;
  char *deviceName = NULL;
  char *baudrate_str = NULL;

  if ((ret_value >= 0) || (ret_value != NO_FILE_ERROR)) {
    return ret_value;
  }

  home = getenv("HOME");
  if (home) {
    ret_value = searchConfigFile(home);
    // !!! free する必要はあるのか？
    if ((ret_value >= 0) || (ret_value != NO_FILE_ERROR)) {
      return ret_value;
    }
  }

  if ((baudrate_str = getenv("URG_BAUDRATE"))) {
    baudrate = atoi(baudrate_str);
  }

  deviceName = getenv("URG_PORT");
  if (deviceName) {
    int ret_value;
    ret_value = initConnectDevice(deviceName, baudrate, SERIAL);
    if (ret_value >= 0) {
      fd = ret_value;
      ret_value = configBaudrate(fd, baudrate);
    }
    return ret_value;
  }
  return -1;
}


int initURGCtrl(int argc, char *argv[]) {
  long baudrate = URG_BAUDRATE;
  char *deviceName = NULL;
  int ret_value;
  int i;

  for (i = 1; i < argc; ++i) {
    if (!strncmp("--urg_port=", argv[i], 11) && (strlen(argv[i]) > 11)) {
      deviceName = &argv[i][11];

    } else if (!strncmp("--urg_baudrate=", argv[i], 15) &&
               (strlen(argv[i]) > 15)) {
      baudrate = atoi(&argv[i][15]);

    } else if (!strcmp("-s", argv[i]) || !strcmp("--simulator", argv[i])) {
      // シミュレータ(TCP/IP) への接続
      return initConnectDevice("localhost", 49763, TCP_IP);
    }
  }
  if (deviceName &&
      (!strcmp("auto", deviceName) || !strcmp("AUTO", deviceName))) {
    int id;
    for (id = 20; id >= 0; --id) {
      char checkDevice[16] = { '\0','\0','\0','\0','\0','\0','\0','\0',
                               '\0','\0','\0','\0','\0','\0','\0','\0' };
      sprintf(checkDevice, URG_AUTO_PORT "%d", id);
      ret_value = initConnectDevice(checkDevice, baudrate, SERIAL);
      if (ret_value >= 0) {
        fd = ret_value;
        ret_value = configBaudrate(fd, baudrate);
        break;
      }
    }
    if (ret_value < 0) {
      Error_device = URG_AUTO_PORT;
      Error_baudrate = baudrate;
      Error_message = "auto dvice detection is fail";
    } else {
      Error_message = "success";
    }
    return ret_value;
  }

  if (!deviceName) {
    return connectDefault();
  }
  ret_value = initConnectDevice(deviceName, baudrate, SERIAL);
  if (ret_value >= 0) {
    fd = ret_value;
    ret_value = configBaudrate(fd, baudrate);
  }
  return ret_value;
}


char* urg_getError(void) {

  // 保持しているエラーメッセージバッファへのポインタを返す

  return Error_message;
}


static void swap(int *a, int *b) {
  int tmp = *a;
  *a = *b;
  *b = tmp;
}


static unsigned char encode6bit(char ch) {
  return 0x3f & (ch - 0x30);
}


int urg_capture(long *data, int from ,int to, int group) {
  char message[10];
  char reply[10];
  char ret;
  char recv_buffer[3072];
  char lf[2];
  int range[2] = { from, to };
  int numdata;
  int index;
  int total;
  int i;
  int n;

  for (i = 0; i < 2; ++i) {
    if (range[i] < 0) {
      range[i] = 0;
    } else if (range[i] >= URG_DATA_SIZE) {
      range[i] = URG_DATA_SIZE -1;
    }
  }
  if (range[0] > range[1]) {
    swap(&range[0], &range[1]);
  }
  if (group <= 0) {
    group = 1;
  } else if (group > 99) {
    group = 99;
  }

  // データ要求の送信
  sprintf(message, "G%03d%03d%02d\r", range[0], range[1], group);
  device_flush(fd);
  device_send(fd, message, 10);

  // データの受信
  device_recv(fd, reply, 9, SCI_TIMEOUT);
  if (strncmp(message, reply, 9)) {
    return -5;
  }

  device_recv(fd, lf, 1, SCI_TIMEOUT);
  if (!isLF(lf[0])) {
    return -6;
  }
  device_recv(fd, &ret, 1, SCI_TIMEOUT);
  if (ret != '0') {
    return -7;
  }
  device_recv(fd, lf, 1, SCI_TIMEOUT);
  if (!isLF(lf[0])) {
    return -8;
  }

  numdata = range[1] - range[0] +1;
  numdata = (numdata / group) + ((numdata % group == 0) ? 0 : 1);
  total = (numdata << 1) + (numdata >> 5);
  n = device_recv(fd, recv_buffer, total, SCI_TIMEOUT);
  if (n != total) {
    //printf("total: %d, n: %d\n", total, n);
    return -9;
  }
  if ((2 != device_recv(fd, lf, 2, SCI_TIMEOUT))
      || !isLF(lf[0]) || !isLF(lf[1])) {
    return -10;
  }

  // 距離データを配列に格納
  for (i = 0; i < range[0]; ++i) {
    data[i] = -1;
  }
  index = range[0];
  for (i = 0; (index <= range[1]) && (i < numdata); ++i) {
    int j;
    int actual_index = (i << 1) + (i >> 5);
    if ((i > 0) && ((i & 0x3f) == 0) && !isLF(recv_buffer[actual_index -1])) {
      printf("i: %d\n", i);
#if 0
      for (j = 0; j < 70; ++j) {
        printf("%d: %c\n", j, recv_buffer[j]);
      }
#endif
      return -11;
    }
    for (j = 0; (index <= range[1]) && (j < group); ++j) {
      data[index++] =
        (encode6bit(recv_buffer[actual_index]) << 6) |
        encode6bit(recv_buffer[actual_index +1]);
    }
  }
  for (i = range[1] +1; i < URG_DATA_SIZE; ++i) {
    data[i] = -1;
  }

  return URG_DATA_SIZE;
}


int deg2index(const int degree) {
  int index;
  int sense_deg = ((degree % 360) + 360) % 360;
  if (sense_deg > 180) {
    sense_deg = sense_deg - 360;
  }
  index = (int)((135 - sense_deg) / (360.0/1024.0));
  if (index < 0) {
    index = 0;
  } else if (index > URG_DATA_SIZE-1) {
    index = URG_DATA_SIZE-1;
  }
  
  return index;
}


int index2deg(const int index) {
  return (int)(360 * index2rad(index) / (2.0 * M_PI));
}


double index2rad(const int index) {
  static const double rad135 = 2.0 * M_PI * 135.0/360.0;

  double rad = rad135 - (index * 2.0 * M_PI / 1024.0);
  if (rad < -rad135) {
    rad = -rad135;
  } else if (rad > rad135) {
    rad = rad135;
  }

  return rad;
}


double deg2rad(const int degree) {
  return (2.0 * M_PI * degree / 360.0);
}