run_ctrl.c

/*
  走行制御ライブラリ
  Satofumi KAMIMURA
  $Id$
*/
#define C_RUNCTRL_SOURCE

#include "run_ctrl.h"
#include "tRunCtrl.h"
#include "connect_device.h"
#include "commandCtrl.h"
#include "get_keyword.h"
#include "nodeAccess.h"
#include "parameter_ctrl.h"
#include <stdio.h>
#include <string.h>
#include <stdlib.h>

extern void run_initCommandCtrl(int *Con, runCtrl_t *Run, nodeInfo_t *Node,
                                runParams_t *Params, int *Unique_id);


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

enum { NO_FILE_ERROR = -2 };

#ifndef RUN_CONFIG_FILE
#define RUN_CONFIG_FILE "b5conf.txt"
#endif


static int checkVersion(void) {
  int ret_value;
#if 0
  long target_version = 0;
  if (recvVersion(&target_version) < 0) {
    Error_message = "Connection fail: recvVersion";
    return -1;
  }

  if (target_version/10 != PACKAGE_NUM_VERSION/10) {
    fprintf(stderr,
            "warnning: RunCtrl version mismatch between PC(%d) and SH2(%ld)\n",
            PACKAGE_NUM_VERSION, target_version);
    Error_message = "RunCtrl version mismatch between host and target";
    closeDevice(fd);
    return -1;
  }
#endif

#if 1
  ret_value = sendPositionInit();
  if (ret_value < 0) {
    Error_message = "Transmit fail: in sendPositionInit()";
    return ret_value;
  }
#endif

  // サーボをかける
  if (sendServoCtrl(1) < 0) {
    Error_message = "Connection fail: sendServoCtrl";
    closeDevice(fd);
    return -1;
  }

  return 0;
}


#include "get_keyword.c"

static int searchConfigFile(const char *path) {
  int path_length = strlen(path);
  int file_length = strlen(RUN_CONFIG_FILE);
  char *file_path = (char *)malloc(path_length + 1 + file_length + 1);
  long baudrate;
  char *baudrate_str;
  char *deviceName;

  strcpy(file_path, path);
  strcpy(&file_path[path_length], "/");
  strcpy(&file_path[path_length +1], RUN_CONFIG_FILE);
  file_path[path_length + 1 + file_length] = '\0';

  baudrate = RUN_CTRL_BAUDRATE;
  if ((baudrate_str = get_keyword(file_path, "run_baudrate"))) {
    baudrate = atoi(baudrate_str);
    free(baudrate_str);
  }
  deviceName = get_keyword(file_path, "run_port");
  free(file_path);
  if (deviceName) {
    int ret_value = initConnectDevice(deviceName, baudrate, SERIAL);
    if (ret_value >= 0) {
      fd = ret_value;
      ret_value = checkVersion();
    }
    free(deviceName);
    return ret_value;
  }
  return NO_FILE_ERROR;
}


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

  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;
    }
  }

  baudrate = RUN_CTRL_BAUDRATE;
  deviceName = NULL;
  baudrate_str = NULL;
  if ((baudrate_str = getenv("RUN_BAUDRATE"))) {
    baudrate = atoi(baudrate_str);
  }

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


int initRunCtrl(int argc, char *argv[]) {
  enum { COORDINATE_NUM_MAX = 256 };
  static runCtrl_t Run;
  static nodeInfo_t Node;
  static runParams_t RunParam;
  static int Unique_id = 0;
  static offsetInfo_t CrdInfo[COORDINATE_NUM_MAX];

  long baudrate = RUN_CTRL_BAUDRATE;
  char *deviceName = NULL;
  int i;
  int ret_value = -1;

  // 走行制御テーブルの初期化
  initNodeInfo(&Node);
  run_initRunParam(&RunParam);
  run_initCommandCtrl(&fd, &Run, &Node, &RunParam, &Unique_id);
  _initCoordinateCtrl(CrdInfo, COORDINATE_NUM_MAX);

  // 通信資源の初期化
  for (i = 0; i < argc; ++i) {
    if (!strncmp("--run_port=", argv[i], 11) && (strlen(argv[i]) > 11)) {
      deviceName = &argv[i][11];

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

    } else if (!strcmp("-s", argv[i]) || !strcmp("--simulator", argv[i])) {
      int ret_value =
        initConnectDevice("localhost", SIMULATOR_T_RUN_CTRL_PORT, TCP_IP);
      if (ret_value < 0) {
        Error_device = "TCP/IP";
        Error_baudrate = SIMULATOR_T_RUN_CTRL_PORT;
        Error_message = "connection fail to simulator";
      } else {
        Error_message = "success";
        fd = ret_value;
        ret_value = checkVersion();
      }
      return ret_value;
    }
  }

  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, RUN_AUTO_PORT "%d", id);
      ret_value = initConnectDevice(checkDevice, baudrate, SERIAL);
      if (ret_value >= 0) {
        fd = ret_value;
        break;
      }
    }
    if (ret_value < 0) {
      Error_device = RUN_AUTO_PORT;
      Error_baudrate = baudrate;
      Error_message = "auto dvice detection is fail";
    } else {
      Error_message = "success";
      ret_value = checkVersion();
    }
    return ret_value;
  }

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


char* run_getError(void) {
  return Error_message;
}