libs/controller/WiiJoystick_lin.cpp

Go to the documentation of this file.

#include "WiiJoystick.h"
#include "MovingAverage.h"
#include "Angle.h"
#include "MathUtils.h"
#include <cwiid.h>
#include <string>
#include <map>


using namespace qrk;
using namespace std;


namespace
{
  bdaddr_t initializeBdaddr(void)
  {
    bdaddr_t addr;
    memset(&addr.b[0], 0, 6);

    return addr;
  }
}


struct WiiJoystick::pImpl
{
  class CallbackHandler
  {
  public:
    class WiiInformation {
      enum { Buttons = 7, };

    public:
      bool quit_;
      int battery_percent_;

      size_t buttons_timestamp_;
      short axis_[2];
      bool pressed_[Buttons];
      int pressed_times_[Buttons];

      struct acc_cal calibration_;
      Point3d<double> acceleration_;
      Point3d<long> rotation_calibration_;
      Point3d<double> rotation_;
      size_t acceleration_timestamp_;

      MovingAverage<double>* average_x_;
      MovingAverage<double>* average_y_;
      MovingAverage<double>* average_z_;

      size_t acceleration_capture_times_;
      size_t rotation_capture_times_;

      vector<ir_position> ir_positions_;


      WiiInformation(void)
        : quit_(false), battery_percent_(0),
          buttons_timestamp_(0), acceleration_timestamp_(0),
          average_x_(new MovingAverage<double>(1)),
          average_y_(new MovingAverage<double>(1)),
          average_z_(new MovingAverage<double>(1)),
          acceleration_capture_times_(0), rotation_capture_times_(0)
      {
        // ゼロ初期化
        axis_[AxisX] = 0;
        axis_[AxisY] = 0;

        for (int i = 0; i < Buttons; ++i) {
          pressed_[i] = false;
          pressed_times_[i] = 0;
        }

        // !!! １回が約 10 msec で 1000 分なので、100 回以上の resize()
        //accelerations_.resize(200);

        // !!! calibration_ は未初期化
      }
    };
    typedef map<const cwiid_wiimote_t*, WiiInformation*> WiiHash;
    static WiiHash wii_hash_;


    CallbackHandler(void)
    {
    }


    ~CallbackHandler(void) {
    }


    static CallbackHandler* getObject(void)
    {
      static CallbackHandler singleton;
      return &singleton;
    }


    void registerObject(const cwiid_wiimote_t* wii)
    {
      WiiInformation* wii_information = new WiiInformation;
      wii_hash_.insert(WiiHash::value_type(wii, wii_information));
    }


    void removeObject(const cwiid_wiimote_t* wii)
    {
      delete wii_hash_[wii];
      wii_hash_.erase(wii);
    }


    void setCalibrationValue(const cwiid_wiimote_t* wii,
                             struct acc_cal* calibration)
    {
      wii_hash_[wii]->calibration_ = *calibration;
    }


    bool haveQuitEvent(const cwiid_wiimote_t* wii)
    {
      return wii_hash_[wii];
    }


    static void recordPressed(cwiid_btn_mesg* mesg, WiiInformation* state,
                              const struct timespec* timestamp)
    {
      (void)timestamp;

      uint16_t buttons = mesg->buttons;

      // 軸の入力を取得
      state->axis_[AxisX] =
        ((buttons & CWIID_BTN_LEFT) ? -32767 : 0) +
        ((buttons & CWIID_BTN_RIGHT) ? +32767 : 0);

      state->axis_[AxisY] =
        ((buttons & CWIID_BTN_UP) ? +32767 : 0) +
        ((buttons & CWIID_BTN_DOWN) ? -32767 : 0);

      // ボタンの押下情報を取得
      state->pressed_[BUTTON_A] = (buttons & CWIID_BTN_A) ? true : false;
      state->pressed_[BUTTON_B] = (buttons & CWIID_BTN_B) ? true : false;
      state->pressed_[BUTTON_MINUS] =
        (buttons & CWIID_BTN_MINUS) ? true : false;
      state->pressed_[BUTTON_PLUS] = (buttons & CWIID_BTN_PLUS) ? true : false;
      state->pressed_[BUTTON_HOME] = (buttons & CWIID_BTN_HOME) ? true : false;
      state->pressed_[BUTTON_1] = (buttons & CWIID_BTN_1) ? true : false;
      state->pressed_[BUTTON_2] = (buttons & CWIID_BTN_2) ? true : false;

      // ボタンが押された回数を更新
      // !!!
    }


    static void recordAcceleration(struct cwiid_acc_mesg* mesg,
                                   WiiInformation* state,
                                   const struct timespec* timestamp)
    {
      (void)timestamp;

      ++state->acceleration_capture_times_;
      if (state->acceleration_capture_times_ < 4) {
        // 最初の数回分は、不正なデータが返されるため、捨てる
        return;
      }

      uint8_t* acc = mesg->acc;
      struct acc_cal& calibration = state->calibration_;

      double x = ((double)acc[CWIID_X] - calibration.zero[CWIID_X]) /
        (calibration.one[CWIID_X] - calibration.zero[CWIID_X]);
      double y = ((double)acc[CWIID_Y] - calibration.zero[CWIID_Y]) /
        (calibration.one[CWIID_Y] - calibration.zero[CWIID_Y]);
      double z = ((double)acc[CWIID_Z] - calibration.zero[CWIID_Z]) /
        (calibration.one[CWIID_Z] - calibration.zero[CWIID_Z]);

      state->acceleration_.x = state->average_x_->push_back(x);
      state->acceleration_.y = state->average_y_->push_back(y);
      state->acceleration_.z = state->average_z_->push_back(z);
    }


    static void recordRotation(const uint16_t angle_rate[],
                               WiiInformation* state,
                               const struct timespec* timestamp)
    {
      (void)timestamp;

      // 最初の数回分のデータを基準値として用いる
      enum {
        IgnoreTimes = 8,
        AverageTimes = 16
      };

      ++state->rotation_capture_times_;
      if (state->rotation_capture_times_ < IgnoreTimes) {
        // 最初のデータは無効なため捨てる
        return;

      } else if (state->rotation_capture_times_ < (AverageTimes +
                                                   IgnoreTimes)) {
        state->rotation_calibration_.x += angle_rate[0];
        state->rotation_calibration_.y += angle_rate[1];
        state->rotation_calibration_.z += angle_rate[2];
        return;

      } else if (state->rotation_capture_times_ == (AverageTimes +
                                                    IgnoreTimes)) {
        state->rotation_calibration_.x /= AverageTimes;
        state->rotation_calibration_.y /= AverageTimes;
        state->rotation_calibration_.z /= AverageTimes;
      }

      state->rotation_.x += angle_rate[0] - state->rotation_calibration_.x;
      state->rotation_.y += angle_rate[1] - state->rotation_calibration_.y;
      state->rotation_.z += angle_rate[2] - state->rotation_calibration_.z;
    }


    static void recordIrPosition(struct cwiid_ir_mesg* ir_data,
                                 WiiInformation* state,
                                 const struct timespec* timestamp)
    {
      (void)timestamp;

      state->ir_positions_.clear();
      for (int i = 0; i < CWIID_IR_SRC_COUNT; ++i) {
        ir_position each_position;

        if (ir_data->src[i].valid) {
          if (ir_data->src[i].size == -1) {
            each_position.size = 3;
          } else {
            each_position.size = ir_data->src[i].size + 1;
          }

          // 位置情報の格納
          each_position.position =
            Point<double>(1.0 * ir_data->src[i].pos[CWIID_X] / CWIID_IR_X_MAX,
                          1.0 * ir_data->src[i].pos[CWIID_Y] / CWIID_IR_Y_MAX);
          state->ir_positions_.push_back(each_position);
        }
      }
    }


    static void callback(cwiid_wiimote_t* wii, int mesg_count,
                         union cwiid_mesg mesg_array[],
                         struct timespec *timestamp)
    {
      WiiInformation* state = wii_hash_[wii];

      for (int i = 0; i < mesg_count; ++i) {

        switch (mesg_array[i].type) {
        case CWIID_MESG_STATUS:
          wii_hash_[wii]->battery_percent_ =
            static_cast<int>((100.0 * mesg_array[i].status_mesg.battery
                              / CWIID_BATTERY_MAX));
          // 元のソースでは、ここで NUNCHUK とかの処理もしていたが、省略
          break;

        case CWIID_MESG_BTN:
          // 押下の記録
          recordPressed(&mesg_array[i].btn_mesg, state, timestamp);
          break;

        case CWIID_MESG_ACC:
          // 加速度情報の取得
          recordAcceleration(&mesg_array[i].acc_mesg, state, timestamp);
          break;

        case CWIID_MESG_IR:
          // IR 情報の取得
          recordIrPosition(&mesg_array[i].ir_mesg, state, timestamp);
          break;

#if defined(CWIID_RPT_MOTIONPLUS)
        case CWIID_MESG_MOTIONPLUS:
          // Motion Plus 情報の取得
          recordRotation(mesg_array[i].motionplus_mesg.angle_rate,
                         state, timestamp);
          break;
#endif

        case CWIID_MESG_ERROR:
          wii_hash_[wii]->quit_ = true;
          break;

        default:
          break;
        }
      }
    }


    static int getAxisValue(size_t index, const cwiid_wiimote_t* wii)
    {
      return wii_hash_[wii]->axis_[index];
    }


    static bool isButtonPressed(size_t index, const cwiid_wiimote_t* wii)
    {
      return wii_hash_[wii]->pressed_[index];
    }


    static int getBatteryPercent(const cwiid_wiimote_t* wii)
    {
      return wii_hash_[wii]->battery_percent_;
    }


    static void getAcceleration(Point3d<double>& acceleration,
                                size_t* timestamp,
                                const cwiid_wiimote_t* wii)
    {
      // !!! timestamp を反映させる
      (void)timestamp;

      acceleration = wii_hash_[wii]->acceleration_;
    }


    static void getRotation(Angle& x_axis, Angle& y_axis, Angle& z_axis,
                            size_t* timestamp, const cwiid_wiimote_t* wii)
    {
      // !!! timstamp を反映させる
      (void)timestamp;

      x_axis = rad(2.0 * M_PI * wii_hash_[wii]->rotation_.x / 8192.0 / 100.0);
      y_axis = rad(2.0 * M_PI * wii_hash_[wii]->rotation_.y / 8192.0 / 100.0);
      z_axis = rad(2.0 * M_PI * wii_hash_[wii]->rotation_.z / 8192.0 / 100.0);
    }


    bool setLed(unsigned char led_value, cwiid_wiimote_t* wii)
    {
      uint8_t led_state =
        ((led_value & 0x08) ? CWIID_LED1_ON : 0) |
        ((led_value & 0x04) ? CWIID_LED2_ON : 0) |
        ((led_value & 0x02) ? CWIID_LED3_ON : 0) |
        ((led_value & 0x01) ? CWIID_LED4_ON : 0);

      return (cwiid_set_led(wii, led_state)) ? false : true;
    }


    // 振動
    bool setRumble(bool rumble, cwiid_wiimote_t* wii)
    {
      return (cwiid_set_rumble(wii, rumble ? 1 : 0)) ? false : true;
    }

    // 加速度の移動平均数
    void setAccelerationAverageSize(size_t num, const cwiid_wiimote_t* wii)
    {
      if (num <= 0) {
        // ゼロは設定させない
        return;
      }

      // オブジェクトの再生成
      delete wii_hash_[wii]->average_x_;
      delete wii_hash_[wii]->average_y_;
      delete wii_hash_[wii]->average_z_;

      wii_hash_[wii]->average_x_ = new MovingAverage<double>(num);
      wii_hash_[wii]->average_y_ = new MovingAverage<double>(num);
      wii_hash_[wii]->average_z_ = new MovingAverage<double>(num);
    }
  };


  static bdaddr_t bdaddr_any_;

  string error_message_;
  cwiid_wiimote_t* wii_;
  bdaddr_t bdaddr_;
  CallbackHandler* callback_;
  struct acc_cal calibration_;


  pImpl(void)
    : error_message_("no error."), wii_(NULL), bdaddr_(bdaddr_any_),
      callback_(CallbackHandler::getObject())
  {
  }


  ~pImpl(void)
  {
    disconnect();
  }


  bool isError(cwiid_wiimote_t* wii)
  {
    return (callback_->haveQuitEvent(wii)) ? true : false;
  }


  bool connect(void)
  {
    if (isConnected()) {
      return true;
    }
    disconnect();

    // 接続
    wii_ = cwiid_open(&bdaddr_, CWIID_FLAG_MESG_IFC);
    if (wii_ == NULL) {
      error_message_ = "unable to connect.";
      return false;
    }
    setRepeatMode();

    // コールバックを登録
    callback_->registerObject(wii_);
    if (cwiid_set_mesg_callback(wii_, &CallbackHandler::callback)) {
      disconnect();
      error_message_ = "error setting callback";
      return false;
    }

    // キャリブレーション値の取得
    if (cwiid_get_acc_cal(wii_, CWIID_EXT_NONE, &calibration_)) {
      error_message_ = "unable to retrieve accelerometer calibration";
      return false;
    }
    callback_->setCalibrationValue(wii_, &calibration_);

    return true;
  }


  void disconnect(void)
  {
    if (! wii_) {
      // 接続されていなければ、戻る
      return;
    }

    // 切断処理
    callback_->removeObject(wii_);
    cwiid_close(wii_);
    wii_ = NULL;
  }


  bool isConnected(void)
  {
    return (wii_) ? true : false;
  }


  void setRepeatMode(void)
  {
    uint8_t rpt_mode =
      CWIID_RPT_STATUS | CWIID_RPT_BTN | CWIID_RPT_IR | CWIID_RPT_ACC;
#if defined(CWIID_RPT_MOTIONPLUS)
    rpt_mode |= CWIID_RPT_MOTIONPLUS;
#endif
    if (cwiid_set_rpt_mode(wii_, rpt_mode)) {
      error_message_ = "error setting report mode";
    }
#if defined(CWIID_RPT_MOTIONPLUS)
    cwiid_enable(wii_, CWIID_FLAG_MOTIONPLUS);
#endif
  }
};
bdaddr_t WiiJoystick::pImpl::bdaddr_any_ = initializeBdaddr();
WiiJoystick::pImpl::CallbackHandler::WiiHash
WiiJoystick::pImpl::CallbackHandler::wii_hash_;


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


WiiJoystick::~WiiJoystick(void)
{
}


bool WiiJoystick::findController(int timeout)
{
  bool exist = (cwiid_find_wiimote(&pimpl->bdaddr_, timeout)) ? false : true;
  return exist;
}


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


bool WiiJoystick::connect(int id)
{
  (void)id;

  return pimpl->connect();
}


void WiiJoystick::disconnect(void)
{
  pimpl->disconnect();
}


bool WiiJoystick::isConnected(void) const
{
  return pimpl->isConnected();
}


size_t WiiJoystick::axisNum(void) const
{
  return 2;
}


int WiiJoystick::axisValue(size_t index)
{
  // !!! 排他制御

  return pimpl->callback_->getAxisValue(index, pimpl->wii_);
}


size_t WiiJoystick::buttonsNum(void) const
{
  return 7;
}


bool WiiJoystick::isButtonPressed(size_t index)
{
  if (! pimpl->wii_) {
    return false;
  }

  // !!! 排他制御

  return pimpl->callback_->isButtonPressed(index, pimpl->wii_);
}


int WiiJoystick::buttonPressedTimes(size_t index)
{
  // !!! 実装すること
  static_cast<void>(index);

  // !!!
  return 0;
}


void WiiJoystick::acceleration(Point3d<double>& acceleration,
                               size_t* timestamp)
{
  if (! pimpl->wii_) {
    return;
  }

  size_t timestamp_value = 0;
  pimpl->callback_->getAcceleration(acceleration,
                                    &timestamp_value, pimpl->wii_);

  if (timestamp) {
    *timestamp = timestamp_value;
  }
}


void WiiJoystick::rotation(Angle& x_axis, Angle& y_axis, Angle& z_axis,
                           size_t* timestamp)
{
  if (! pimpl->wii_) {
    return;
  }

  size_t timestamp_value = 0;
  pimpl->callback_->getRotation(x_axis, y_axis, z_axis,
                                &timestamp_value, pimpl->wii_);

  if (timestamp) {
    *timestamp = timestamp_value;
  }
}


size_t WiiJoystick::batteryPercent(void)
{
  // !!! 排他制御

  return pimpl->callback_->getBatteryPercent(pimpl->wii_);
}


bool WiiJoystick::irPosition(vector<ir_position>& positions)
{
  // !!! 排他制御

  positions = pimpl->callback_->wii_hash_[pimpl->wii_]->ir_positions_;

  return true;
}


// LED 設定
bool WiiJoystick::setLed(unsigned char led_value)
{
  return pimpl->callback_->setLed(led_value, pimpl->wii_);
}


// 振動
bool WiiJoystick::setRumble(bool rumble)
{
  return pimpl->callback_->setRumble(rumble, pimpl->wii_);
}


// 加速度の移動平均数
void WiiJoystick::setAccelerationAverageSize(size_t size)
{
  if (! pimpl->wii_) {
    return;
  }

  pimpl->callback_->setAccelerationAverageSize(size, pimpl->wii_);
}