run_ctrl.c

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

#include "run_ctrl.h"
#include "tRunCtrl.h"
#include "commandCtrl.h"
#include "get_keyword.h"
#include "connect_device.h"
#include "serial_device.h"
#include "tcpip_device.h"
#include "structTables.h"
#include "nodeAccess.h"
#include <string.h>

typedef struct {
  int straight_ref_vel;
  int straight_ref_acc;
  long rotate_ref_vel;
  long rotate_ref_acc;
  int follow_r;
} runParams_t;


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

enum {
  SimulatorPort = 49763,
  SEND_COMMAND_SIZE = 256,
};

typedef struct {
  int x;
  int y;
  direction_t zt;
} position_t;
static position_t local_offset;

typedef struct {
  int send_command_size;
  char send_command[SEND_COMMAND_SIZE];
} runCommand_t;
static runCommand_t cmd;


static runCtrl_t Tbl;
static nodeInfo_t Node;
static runParams_t Params;
static int Unique_id = 0;


#ifndef RUN_CONFIG_FILE
#define RUN_CONFIG_FILE "defaultargs"
#endif


#ifndef RUN_AUTO_PORT
# ifdef LINUX
#  define RUN_AUTO_PORT "/dev/ttyS"
# else
#  define RUN_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 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(RUN_CONFIG_FILE);
  long baudrate = RUN_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], RUN_CONFIG_FILE);
    file_path[path_length + 1 + file_length] = '\0';

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


static void initialization(void) {
  Params.straight_ref_vel = StraightVel;
  Params.straight_ref_acc = StraightAcc;
  Params.rotate_ref_vel = RotateVel;
  Params.rotate_ref_acc = RotateAcc;
  Params.follow_r = FollowRadius;

  initNodeInfo(&Node);
  registerStructInfo(&Node, (unsigned char*)&Tbl, RUN_CTRL_TARGET_ID);
  Tbl.version = 0;
  //registerCrdNortifyClient();

  run_initCommandCtrl(&fd, &Tbl, &Node, &Params, &Unique_id);
}


static int checkVersion(void) {
  initialization();

  long target_version = PACKAGE_NUM_VERSION -1;
  if (recvVersion(&target_version) < 0) {
    Error_message = "Transmit fail: please check connected device.";
    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);
    exit(1);
  }
#if 0
  int ret_value = sendPositionInit();
  if (ret_value < 0) {
    error_message = "Transmit fail: in sendPositionInit()";
    return ret_value;
  }
#endif
  run_stop();

  //set_watchDogTimer(0);
  //initTicksInfo();

  // 現在自己位置で座標系を初期化

  // !!! とりあえずのコメントアウト。この処理は必要
  //run_adjustRunPosition(0, 0, deg(0));

  return 0;
}


static int connectDefault(void) {
  int ret_value = searchConfigFile(".");
  char *home;
  long baudrate = RUN_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("RUN_BAUDRATE"))) {
    baudrate = atoi(baudrate_str);
  }

  deviceName = getenv("RUN_PORT");
  if (deviceName) {
    int ret_value;
    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[]) {
  long baudrate = RUN_BAUDRATE;
  char *deviceName = NULL;
  int ret_value;
  int i;

  for (i = 1; 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])) {
      // シミュレータ(TCP/IP) への接続
      return initConnectDevice("localhost", SimulatorPort, 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, RUN_AUTO_PORT "%d", id);
      ret_value = initConnectDevice(checkDevice, baudrate, SERIAL);
      if (ret_value >= 0) {
        fd = ret_value;
        ret_value = checkVersion();
        break;
      }
    }
    if (ret_value < 0) {
      Error_device = RUN_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 = checkVersion();
  }
  return ret_value;
}


char* run_getError(void) {
  return "";
}


#include "move_ctrl.c"