view/gyroscopic/Tds01vCtrl.cpp

/*!
  \file
  \brief HĚ TDS01V Ě§ä

  \author Satofumi KAMIMURA

  $Id$
*/

#include "Tds01vCtrl.h"
#include "SerialCtrl.h"
#include "Delay.h"
#include <cstdlib>

using namespace beego;


struct Tds01vCtrl::pImpl {
  enum {
    Baudrate = 9600,
    Timeout = 100,
    MagnetismResetTimes = 4,
  };
  std::string error_message;
  SerialCtrl con;
  int recv_times;
  double direction_offset;

  pImpl(void)
    : error_message("no error."), recv_times(0), direction_offset(0.0) {
  }

  ~pImpl(void) {
    stop();
  }

  // ACK Ě
  bool sendTag(const char* command, size_t size) {

    enum { MaxByte = 7 };
    char send_buffer[(MaxByte * 2) + 2];
    for (size_t i = 0; i < size; ++i) {
      sprintf(&send_buffer[i * 2], "%02X\r\n",
              static_cast<unsigned char>(command[i]));
    }
    size_t send_size = (size * 2) + 2;
    int n = con.send(send_buffer, send_size);
#if 0
    fprintf(stderr, "send size: %d\n", n);
    for (size_t i = 0; i < send_size-2; ++i) {
      fprintf(stderr, "%c ", send_buffer[i]);
    }
    fprintf(stderr, "\n");
#endif
    if (n < static_cast<int>(send_size)) {
      return false;
    }

    enum { RecvSize = 4 };
    char recv_buffer[RecvSize];
    n = con.recv(recv_buffer, RecvSize, Timeout);
#if 0
    fprintf(stderr, "recv size : %d\n", n);
    for (int i = 0; i < n-2; ++i) {
      fprintf(stderr, "%c ", recv_buffer[i]);
    }
    fprintf(stderr, "\n");
#endif
    recv_buffer[RecvSize -1] = '\0';
    char reply = strtol(recv_buffer, NULL, 16);

    if (reply != ~command[0]) {
      return false;
    }
    return true;
  }

  bool initializeDevice(void) {

    // \tgEFAZbgđs¤
    char reset_message[] = { 0x0f };
    if (! sendTag(reset_message, sizeof(reset_message))) {
      error_message = "fail reset command.";
      return false;
    }
    delay(250 + 50);                // ZbgÉćčĘMŞsĘÉČéÔ

    // vŞđÝč
    char init_message[] = { 0x05,
                            0x01, // Şčüú 100 [msec]
                            10000 >> 8, 10000 & 0xff, // 1000 [hpa]
                            0000 >> 8, 0000 & 0xff }; // 0.0 [deg]
    if (! sendTag(init_message, sizeof(init_message))) {
      error_message = "in measure initialize command sending...";
      return false;
    }

    // ZTîńÚÝč
    char measure_message[] = { 0x0d,
                               0x84 }; // űĘpîń
    if (! sendTag(measure_message, sizeof(measure_message))) {
      error_message = "in measure type command sending...";
      return false;
    }

    if (! initializeMagnetism()) {
      return false;
    }

    // vŞJn
    char start_message[] = { 0x21 };
    if (! sendTag(start_message, sizeof(start_message))) {
      error_message = "in start command sending...";
      return false;
    }
    delay(100);                        // ĹĚŞčŞIíéĚđŇÂ
    // !!! {ÍuóÔvvĹÎˇ×Ť

    return true;
  }

  bool initializeMagnetism(void) {
    // nĽCZTúť
    char magnetism_command[] = { 0x27 };
    if (! sendTag(magnetism_command, sizeof(magnetism_command))) {
      error_message = "magnetism initialize command.";
      return false;
    }
    return true;
  }

  void stop(void) {
    // vŞâ~
    char start_message[] = { 0x23 };
    if (! sendTag(start_message, sizeof(start_message))) {
      error_message = "start command.";
    }
  }
};


Tds01vCtrl::Tds01vCtrl(void) : pimpl(new pImpl) {
}


Tds01vCtrl::~Tds01vCtrl(void) {
}


const char* Tds01vCtrl::what(void) {
  return pimpl->error_message.c_str();
}


bool Tds01vCtrl::connect(const char* device) {

  if (! pimpl->con.connect(device, pImpl::Baudrate)) {
    pimpl->error_message = pimpl->con.what();
    return false;
  }
  pimpl->con.skip(10);
  return pimpl->initializeDevice();
}


void Tds01vCtrl::disconnect(void) {
  pimpl->con.disconnect();
}


bool Tds01vCtrl::isConnected(void) {
  return pimpl->con.isConnected();
}


double Tds01vCtrl::getDirection(void) {

  // XAnĽCZTúťvđÄŃoˇ
  ++pimpl->recv_times;
  if (pimpl->recv_times > pImpl::MagnetismResetTimes) {
    pimpl->initializeMagnetism();
    pimpl->recv_times = 0;
  }

  // óÔv
  // !!!
  // vŞf[^XVĎÝAđúŇ
  // Ćč Ś¸ČŞ

  // vŞĘĚćž
  enum { SendSize = 4 };
  char send_buffer[SendSize];
  sprintf(send_buffer, "%02X\r\n", 0x29);
  int n = pimpl->con.send(send_buffer, SendSize);
  if (n < SendSize) {
    pimpl->error_message = "require command send.";
    return -1.0;
  }

  // !!! Ćč Ś¸AűĘpÉÎ
  enum { RecvSize = 6 };
  char recv_buffer[RecvSize];
  n = pimpl->con.recv(recv_buffer, RecvSize, pImpl::Timeout);

#if 0
  fprintf(stderr, "data size : %d\n", n);
  for (int i = 0; i < n-2; ++i) {
    fprintf(stderr, "%c ", recv_buffer[i]);
  }
  fprintf(stderr, "\n");
#endif

  if (n < RecvSize) {
    pimpl->error_message = "require command recv.";
    return -1.0;
  }
  recv_buffer[RecvSize -1] = '\0';
  int degree_value = strtol(recv_buffer, NULL, 16);

  return 360.0 -degree_value / 10.0;
}


void Tds01vCtrl::setDirectionOffset(double direction_offset) {
  pimpl->direction_offset = direction_offset;
}