runCtrlHub.cpp

00001 /*
00002   走行制御の通信を媒介するクラス
00003   Satofumi KAMIMURA
00004   $Id$
00005 */
00006 
00007 #include "runCtrlHub.h"
00008 #include "serialDevice.h"
00009 #include "nodeAccess.h"
00010 #include "timeUtils.h"
00011 
00012 
00013 RunCtrlHub::RunCtrlHub(int port, const char* device, long baudrate)
00014   : ConnectionHub(port, device, baudrate),
00015     mode(WaitCommandHeader), require_size(0), cmd_filled(0), reply_filled(0),
00016     retry_max(RetryMax), retry_times(0), recv_timeout(DefaultTimeout) {
00017 }
00018 
00019 
00020 RunCtrlHub::~RunCtrlHub(void) {
00021 }
00022 
00023 
00024 bool RunCtrlHub::update(void) {
00025   bool ret_value = false;
00026   if (preUpdate(ret_value)) {
00027     return ret_value;
00028   }
00029 
00030   if (mode == WaitCommandHeader) {
00031     // 送信コマンドのヘッダ部が届くのを待つ
00032     if (port_a->size() >= NODE_ACCESS_PACKET_HEADER_SIZE - cmd_filled) {
00033       cmd_filled = port_a->recv((char*)&command_recv[cmd_filled],
00034                                 NODE_ACCESS_PACKET_HEADER_SIZE - cmd_filled);
00035       mode = WaitCommandPacket;
00036       require_size = getPacketLength((unsigned char*)command_recv) -
00037         NODE_ACCESS_PACKET_HEADER_SIZE;
00038     }
00039   }
00040   if (mode == WaitCommandPacket) {
00041     // 送信コマンドのデータ部が届くのを待つ
00042     if (port_a->size() >= require_size) {
00043       cmd_filled += port_a->recv(&command_recv[cmd_filled], require_size);
00044       port_b->send(command_recv, cmd_filled);
00045       mode = WaitReplyHeader;
00046 
00047       // 応答パケットサイズの計算
00048       int type = getPacketType((unsigned char*)command_recv);
00049       require_size = (type == PACKET_READ_REQUEST) ?
00050         cmd_filled : NODE_ACCESS_PACKET_HEADER_SIZE + 1;
00051       wait_begin = VXV::GetTicks();
00052     }
00053   }
00054   if (mode == WaitReplyHeader) {
00055     // コマンド応答が届くのを待つ
00056     if (port_b->size() >= require_size) {
00057       reply_filled = port_b->recv(reply_recv, require_size);
00058       if (checkPacketFormat((unsigned char*)reply_recv, require_size) < 0) {
00059         sendRetryPacket();
00060         return true;
00061       }
00062       port_a->send(reply_recv, require_size);
00063       retry_times = 0;
00064       mode = WaitCommandHeader;
00065       cmd_filled = 0;
00066 
00067     } else {
00068       if ((int)(VXV::GetTicks() - wait_begin) > recv_timeout) {
00069         if (++retry_times > retry_max) {
00070           return false;
00071         }
00072         sendRetryPacket();
00073       }
00074     }
00075   }
00076   return true;
00077 }
00078 
00079 
00080 void RunCtrlHub::sendRetryPacket(void) {
00081   mode = WaitCommandHeader;
00082   port_a->send(command_recv, reply_filled);
00083   cmd_filled = removeInvalidPacketHeader((unsigned char*)reply_recv,
00084                                          require_size);
00085 }
00086 

Generated on Mon Apr 13 22:52:04 2009 by  doxygen 1.5.7.1