CWiidMotionReceiver.cpp

Go to the documentation of this file.

#include "CWiidMotionReceiver.h"
#include <bluetooth/bluetooth.h>
#include <iostream>
#include <cstdlib>

CWiidMotionReceiver::CWiidMotionReceiver() :
    m_wiimoteMacAdress("00:00:00:00:00:00")
{
}

CWiidMotionReceiver::~CWiidMotionReceiver()
{
    stop();
}

void CWiidMotionReceiver::start() throw(Error)
{
    bdaddr_t addr;
    if (str2ba(m_wiimoteMacAdress.c_str(), &addr) != 0) {
        std::string err = "CWiidMotionReceiver::start(): cannot convert MAC-adress '";
        err += m_wiimoteMacAdress + "' to btaddr_t struct";
        throw Error(err.c_str());
    }

    // connect wiimote
    m_wiimote = cwiid_open(&addr, CWIID_FLAG_MESG_IFC | CWIID_FLAG_CONTINUOUS | CWIID_FLAG_NONBLOCK);
    if (m_wiimote == NULL) {
        throw Error("CWiidMotionReceiver::start(): cannot connect to wiimote");
    }

    // calibrate wiimote
    if (cwiid_get_acc_cal(m_wiimote, CWIID_EXT_NONE, &m_calibration)) {
        throw Error("CWiidMotionReceiver::start(): unable to retreive calibration data");
    }

    cwiid_set_led(m_wiimote, CWIID_LED1_ON);
}

void CWiidMotionReceiver::stop()
{
    // uninit CWiid
    setRunning(false);
    if (m_wiimote != NULL) {
        cwiid_close(m_wiimote);
        m_wiimote = NULL;
    }
}

void CWiidMotionReceiver::run()
{
    setRunning(true);
    cwiid_enable(m_wiimote, CWIID_FLAG_MESG_IFC | CWIID_FLAG_CONTINUOUS | CWIID_FLAG_NONBLOCK);

    int msgCount = 0, status;
    union cwiid_mesg *msgs;
    struct timespec timestamp;

    while (isRunning()) {
        status = cwiid_get_mesg(m_wiimote, &msgCount, &msgs, &timestamp);
        if (status == 0) {
            std::cout << "Got " << msgCount << " messages!" << std::endl;
            for (int i = 0; i < msgCount; ++i) {
                handleMessages(&msgs[i]);
            }
            // msgs is malloced in libcwiid/interface.c, so we need to free it.
            if (msgs != NULL) {
                free(msgs);  
                msgs = NULL;
            }
        } else {
            std::string err = "CWiidMotionReceiver::run(): Something went wront in cwiid_get_mesg()!";
            throw Error(err.c_str());
        }
    }
}

// TODO: Make this work!
void CWiidMotionReceiver::handleMessages(union cwiid_mesg *msg) 
{
    switch (msg->type) {
        case CWIID_MESG_BTN:
            handleButtons(&msg->btn_mesg);
            break;
        case CWIID_MESG_ACC:
            handleAccData(&msg->acc_mesg);
            break;
        case CWIID_MESG_ERROR:
            {
                std::string err = "CWiidMotionReceiver::handleMessages(): received error message #";
                err += msg->error_mesg.error;
                throw Error(err.c_str());
            }
            break;
        default:
            break;
    }
}

void CWiidMotionReceiver::handleButtons(struct cwiid_btn_mesg *msg)
{
    if (msg->buttons & CWIID_BTN_A) {
        setRunning(false);
        cwiid_disable(m_wiimote, CWIID_FLAG_MESG_IFC | CWIID_FLAG_CONTINUOUS | CWIID_FLAG_NONBLOCK);
    }
}

void CWiidMotionReceiver::handleAccData(struct cwiid_acc_mesg *msg)
{
    Vector3d v;

    // see http://www.wiili.org/index.php/Motion_analysis
    v.set(0, 
            (static_cast<double>( msg->acc[CWIID_X]) - m_calibration.zero[CWIID_X])
            / (m_calibration.one[CWIID_X] - m_calibration.zero[CWIID_X])
         );
    v.set(1, 
            (static_cast<double>(msg->acc[CWIID_Y]) - m_calibration.zero[CWIID_Y])
            / (m_calibration.one[CWIID_Y] - m_calibration.zero[CWIID_Y])
         );
    v.set(2, 
            (static_cast<double>(msg->acc[CWIID_Z]) - m_calibration.zero[CWIID_Z])
            / (m_calibration.one[CWIID_Z] - m_calibration.zero[CWIID_Z])
         );

    notify(v);
}

---