urgTicksCtrl.cpp

/*
  URGCtrl における時間同期機能の実装
  Satofumi KAMIMURA
  $Id$
*/

#include "urgCtrl.h"
#include "timeUtils.h"


enum {
  Timeout = 150,
};


void URGCtrl::initTicksInfo(void) {
  beginTimeAdjust();
  pre_module_ticks = static_cast<unsigned short>(getURGTimestamp());

  ignore_beginTimeAdjust = true;
  adjustOwnTicks(0);
}


unsigned long URGCtrl::getHostTicks(void) {
  return VXV::GetTicks();
}


void URGCtrl::skipReply(int n) {
  char buffer[16];

  for (int i = 0; i < n; ++i) {
    //fprintf(stderr, "%d: \n", i);
    con->recv_line(buffer, 15, Timeout);
  }
}


void URGCtrl::beginTimeAdjust(void) {
  if (ignore_beginTimeAdjust) {
    ignore_beginTimeAdjust = false;
    return;
  }
  con->send("TM0\r", 4);
  skipReply(3);
}


void URGCtrl::endTimeAdjust(void) {
  con->send("TM2\r", 4);
  skipReply(3);

  //fprintf(stderr, "laser\n");
  con->laser(true);
}


int URGCtrl::updateTicksDiff(unsigned short ticks) {
  total_msec += ticks - pre_module_ticks;
  pre_module_ticks = ticks;

  return total_msec;
}


long URGCtrl::decode(const char* data, int data_byte) {
  long value = 0;
  for (int i = 0; i < data_byte; ++i) {
    value <<= 6;
    value &= ~0x3f;
    value |= data[i] - 0x30;
  }
  return value;
}


unsigned long URGCtrl::getURGTimestamp(void) {
  con->send("TM1\r", 4);
  skipReply(2);

  char ticks_buffer[8] = { 0, 0, 0, 0, 0, 0, '\0', '\0' };
  int n = con->recv_line(ticks_buffer, 6, Timeout);
  //fprintf(stderr, "n = %d\n", n);
  //fprintf(stderr, "%c%c%c%c%c\n", ticks_buffer[0],ticks_buffer[1],ticks_buffer[2],ticks_buffer[3], ticks_buffer[4]);
  //fprintf(stderr, "%02x %02x %02x %02x %02x\n", ticks_buffer[0],ticks_buffer[1],ticks_buffer[2],ticks_buffer[3], ticks_buffer[4]);
  unsigned long raw_ticks = 0;
  if (n >= 5) {
    raw_ticks = decode(ticks_buffer, 4);
    //fprintf(stderr, "raw_ticks: %d\n", raw_ticks);
  }
  skipReply(1);

  return raw_ticks;
}


unsigned long URGCtrl::getModuleRawTicks(void) {
  return updateTicksDiff(static_cast<unsigned short>(getURGTimestamp()));
}