runCtrlHub.cpp

/*
  走行制御の通信を媒介するクラス
  Satofumi KAMIMURA
  $Id$
*/

#include "runCtrlHub.h"
#include "serialDevice.h"
#include "nodeAccess.h"
#include "timeUtils.h"


RunCtrlHub::RunCtrlHub(int port, const char* device, long baudrate)
  : ConnectionHub(port, device, baudrate),
    mode(WaitCommandHeader), require_size(0), cmd_filled(0), reply_filled(0),
    retry_max(RetryMax), retry_times(0), recv_timeout(DefaultTimeout) {
}


RunCtrlHub::~RunCtrlHub(void) {
}


bool RunCtrlHub::update(void) {
  bool ret_value = false;
  if (preUpdate(ret_value)) {
    return ret_value;
  }

  if (mode == WaitCommandHeader) {
    // 送信コマンドのヘッダ部が届くのを待つ
    if (port_a->size() >= NODE_ACCESS_PACKET_HEADER_SIZE - cmd_filled) {
      cmd_filled = port_a->recv((char*)&command_recv[cmd_filled],
                                NODE_ACCESS_PACKET_HEADER_SIZE - cmd_filled);
      mode = WaitCommandPacket;
      require_size = getPacketLength((unsigned char*)command_recv) -
        NODE_ACCESS_PACKET_HEADER_SIZE;
    }
  }
  if (mode == WaitCommandPacket) {
    // 送信コマンドのデータ部が届くのを待つ
    if (port_a->size() >= require_size) {
      cmd_filled += port_a->recv(&command_recv[cmd_filled], require_size);
      port_b->send(command_recv, cmd_filled);
      mode = WaitReplyHeader;

      // 応答パケットサイズの計算
      int type = getPacketType((unsigned char*)command_recv);
      require_size = (type == PACKET_READ_REQUEST) ?
        cmd_filled : NODE_ACCESS_PACKET_HEADER_SIZE + 1;
      wait_begin = VXV::GetTicks();
    }
  }
  if (mode == WaitReplyHeader) {
    // コマンド応答が届くのを待つ
    if (port_b->size() >= require_size) {
      reply_filled = port_b->recv(reply_recv, require_size);
      if (checkPacketFormat((unsigned char*)reply_recv, require_size) < 0) {
        sendRetryPacket();
        return true;
      }
      port_a->send(reply_recv, require_size);
      retry_times = 0;
      mode = WaitCommandHeader;
      cmd_filled = 0;

    } else {
      if ((int)(VXV::GetTicks() - wait_begin) > recv_timeout) {
        if (++retry_times > retry_max) {
          return false;
        }
        sendRetryPacket();
      }
    }
  }
  return true;
}


void RunCtrlHub::sendRetryPacket(void) {
  mode = WaitCommandHeader;
  port_a->send(command_recv, reply_filled);
  cmd_filled = removeInvalidPacketHeader((unsigned char*)reply_recv,
                                         require_size);
}