source: code/branches/wiimote/src/external/wiicpp/wiic/motionplus.c @ 10422

/*
 *      motionplus.c
 *
 *      Written By:
 *              Gabriele Randelli       
 *              Email: < randelli (--AT--) dis [--DOT--] uniroma1 [--DOT--] it >
 *
 *      Copyright 2010
 *
 *      This file is part of wiiC.
 *
 *      This program is free software; you can redistribute it and/or modify
 *      it under the terms of the GNU General Public License as published by
 *      the Free Software Foundation; either version 3 of the License, or
 *      (at your option) any later version.
 *
 *      This program is distributed in the hope that it will be useful,
 *      but WITHOUT ANY WARRANTY; without even the implied warranty of
 *      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *      GNU General Public License for more details.
 *
 *      You should have received a copy of the GNU General Public License
 *      along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 *      $Header$
 *
 */

/**
 *      @file
 *      @brief Motion Plus expansion device.
 */
#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#include "definitions.h"
#include "wiic_internal.h"
#include "dynamics.h"
#include "events.h"
#include "motionplus.h"

/**
 *      @brief Convert raw data in deg/sec angular rates.
 *
 *      @param mp               A pointer to a motionplus_t structure.
 *      
 *      Subtract calibration data from raw data, and convert the difference in deg/sec
 *      angular rates. The function also considers the fast/slow rotation mode
 *      and performs very simple filtering for slow rotations.
 */
void calculate_gyro_rates(struct motion_plus_t* mp, struct ang3s_t* in, struct ang3f_t* out)
{
        short int tmp_r, tmp_p, tmp_y;
        float tmp_roll, tmp_pitch, tmp_yaw;
        
        // We consider calibration data
        tmp_r = in->roll - mp->cal_gyro.roll;
        tmp_p = in->pitch - mp->cal_gyro.pitch;
        tmp_y = in->yaw - mp->cal_gyro.yaw;

        // We convert to degree/sec according to fast/slow mode
        if(mp->acc_mode & 0x04)
                tmp_roll = tmp_r / 20.0;
        else
                tmp_roll = tmp_r / 4.0;

        if(mp->acc_mode & 0x02)
                tmp_pitch = tmp_p / 20.0;
        else
                tmp_pitch = tmp_p / 4.0;
                
        if(mp->acc_mode & 0x01)
                tmp_yaw = tmp_y / 20.0;
        else
                tmp_yaw = tmp_y / 4.0;
        
        out->roll = tmp_roll;
        out->pitch = tmp_pitch;
        out->yaw = tmp_yaw;
}

/**
 *      @brief Handle Motion Plus event.
 *
 *      @param mp               A pointer to a motionplus_t structure.
 *      @param msg              The message specified in the event packet.
 */
void motion_plus_event(struct motion_plus_t* mp, byte* msg) 
{
        // Check if the gyroscope is in fast or slow mode (0 if rotating fast, 1 if slow or still)
        mp->acc_mode = ((msg[4] & 0x2) << 1) | ((msg[3] & 0x1) << 1) | ((msg[3] & 0x2) >> 1); 
        
        mp->a_raw_gyro.roll = ((msg[4] & 0xFC) << 6) | msg[1];
        mp->a_raw_gyro.pitch = ((msg[5] & 0xFC) << 6) | msg[2];
        mp->a_raw_gyro.yaw = ((msg[3] & 0xFC) << 6) | msg[0];

        // First calibration
        if((mp->a_raw_gyro.roll > 5000) && (mp->a_raw_gyro.pitch > 5000) && (mp->a_raw_gyro.yaw > 5000) && 
                !(mp->cal_gyro.roll) && !(mp->cal_gyro.pitch) && !(mp->cal_gyro.yaw)) {
                wiic_calibrate_motion_plus(mp); 
        }
        
        if(mp->smooth)
                motion_plus_apply_smoothing(mp);
        else {
                mp->raw_gyro.roll = mp->a_raw_gyro.roll;
                mp->raw_gyro.pitch = mp->a_raw_gyro.pitch;
                mp->raw_gyro.yaw = mp->a_raw_gyro.yaw;
        }
                                
        // Calculate angular rates in deg/sec and performs some simple filtering (both unsmoothed and smoothed)
        calculate_gyro_rates(mp,&(mp->a_raw_gyro),&(mp->a_gyro_rate));
        calculate_gyro_rates(mp,&(mp->raw_gyro),&(mp->gyro_rate));
}

/**
 *      @brief Apply smoothing to the Motion Plus gyroscopes.
 *
 *      @param mp               Pointer to a motion_plus_t structure.
 *
 */
void motion_plus_apply_smoothing(struct motion_plus_t *mp)
{
        float alpha = mp->smooth_alpha;
        
        mp->raw_gyro.roll = alpha*mp->raw_gyro.roll + (1.0-alpha)*mp->a_raw_gyro.roll;
        mp->raw_gyro.pitch = alpha*mp->raw_gyro.pitch + (1.0-alpha)*mp->a_raw_gyro.pitch;
        mp->raw_gyro.yaw = alpha*mp->raw_gyro.yaw + (1.0-alpha)*mp->a_raw_gyro.yaw;
}

/**
 *      @brief Handle the handshake from the Motion Plus.
 *
 *      @param wm               A pointer to a wiimote_t structure.
 *      @param data             The data that should contain the Motion Plus ID to check.
 *      @param len              The length of the data block, in bytes.
 *
 *      @return Returns 1 if handshake was successful, 0 if not.
 */
int motion_plus_handshake(struct wiimote_t* wm, byte* data, unsigned short len) 
{
        WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_EXP);
        WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_EXP_FAILED);
        WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_EXP_HANDSHAKE);

        unsigned int val = (data[2] << 24) | (data[3] << 16) | (data[4] << 8) | data[5];

        if(val == EXP_ID_CODE_MOTION_PLUS) {
                /* handshake done */
                wm->event = WIIC_MOTION_PLUS_INSERTED;
                wm->exp.type = EXP_MOTION_PLUS;
                
                WIIMOTE_ENABLE_STATE(wm,WIIMOTE_STATE_EXP);
                WIIMOTE_ENABLE_STATE(wm,WIIMOTE_STATE_MOTION_PLUS);
                
                // Init gyroscopes
                wm->exp.mp.cal_gyro.roll = 0;
                wm->exp.mp.cal_gyro.pitch = 0;
                wm->exp.mp.cal_gyro.yaw = 0;
                wm->exp.mp.orient.angle.roll = 0.0;
                wm->exp.mp.orient.angle.pitch = 0.0;
                wm->exp.mp.orient.angle.yaw = 0.0;
                wm->exp.mp.raw_gyro.roll = wm->exp.mp.a_raw_gyro.roll = 0;
                wm->exp.mp.raw_gyro.pitch = wm->exp.mp.a_raw_gyro.pitch = 0;
                wm->exp.mp.raw_gyro.yaw = wm->exp.mp.a_raw_gyro.yaw = 0;
                wm->exp.mp.gyro_rate.roll = wm->exp.mp.a_gyro_rate.roll = 0.0;
                wm->exp.mp.gyro_rate.pitch = wm->exp.mp.a_gyro_rate.pitch = 0.0;
                wm->exp.mp.gyro_rate.yaw = wm->exp.mp.a_gyro_rate.yaw = 0.0;
                wm->exp.mp.raw_gyro_threshold = 15;
                wm->exp.mp.smooth = 1;
                wm->exp.mp.smooth_alpha = MP_SMOOTH_ALPHA;
                
                // Calibration (will be done as soon as we receive the first event)
                WIIC_INFO("Please, wait for gyro calibration...\n");
                sleep(1);
        }
        else {
                WIIC_ERROR("Unable to activate Motion Plus");
                wiic_set_motion_plus(wm,0);
                return 0;
        }
        
        return 1;
}

/**
 *      @brief Control the Motion Plus support.
 *
 *      @param wm               Pointer to a wiimote_t structure.
 *      @param status   Flag to control if the support should be enabled or not (1 to enable, 0 to disable).
 */
void wiic_set_motion_plus(struct wiimote_t *wm, int status)
{
        byte val;
        byte* buf = 0;
        byte* tmp = 0;

        // If we're handshaking other expansions, than skip this
        if(WIIMOTE_IS_SET(wm,WIIMOTE_STATE_EXP_HANDSHAKE))
                return;

        WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_EXP_HANDSHAKE);
        if(status) {
                // We initialize the motion plus
                val = 0x55;
                wiic_write_data(wm,WM_MOTION_PLUS_INIT,&val,1);
                usleep(10000);

                // We initialize the motion plus
                val = 0x04;
                wiic_write_data(wm,WM_MOTION_PLUS_ENABLE,&val,1);
                usleep(10000);

                // Callback to check if the init process performed right
                tmp = (byte*)malloc(sizeof(byte)*6);
                wiic_read_data_cb(wm, motion_plus_handshake, tmp, WM_MOTION_PLUS_ID_ADDR, 6);
        }
        else {
                disable_expansion(wm);
                WIIMOTE_DISABLE_STATE(wm,WIIMOTE_STATE_MOTION_PLUS);
                val = 0x55;
                wiic_write_data(wm,WM_MOTION_PLUS_DISABLE,&val,1);
                WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_EXP_HANDSHAKE);
        }
}


/**
 *      @brief Calibrate the Motion Plus gyroscopes.
 *
 *      @param mp               Pointer to a motion_plus_t structure.
 *
 *  This should be called only after receiving the first values
 *      from the Motion Plus.
 */
void wiic_calibrate_motion_plus(struct motion_plus_t *mp)
{
        mp->cal_gyro.roll = mp->raw_gyro.roll = mp->a_raw_gyro.roll ;
        mp->cal_gyro.pitch = mp->raw_gyro.pitch = mp->a_raw_gyro.pitch ;
        mp->cal_gyro.yaw = mp->raw_gyro.yaw = mp->a_raw_gyro.yaw ;
}


/**
 *      @brief Notification of Motion Plus disconnection.
 *
 *      @param mp       A pointer to a motion_plus_t structure.
 */
void motion_plus_disconnected(struct motion_plus_t* mp)
{
        WIIC_DEBUG("Motion plus disconnected");
        memset(mp, 0, sizeof(struct motion_plus_t));
}

/**
 *      @brief  Set the gyroscope event threshold.
 *
 *      @param wm                       Pointer to a wiimote_t structure.
 *      @param threshold        The decimal place that should be considered a significant change.
 */
void wiic_set_mp_threshold(struct wiimote_t* wm, int threshold) {
        if (!wm)        return;

        wm->exp.mp.raw_gyro_threshold = threshold;
}

/**
 *      @brief  Enable the gyroscope smoothing (through an exponential moving average)
 *
 *      @param wm                       Pointer to a wiimote_t structure.
 *      @param status           1 to enable - 0 to disable
 *  @param alpha                Alpha smoothness parameter (between 0.0 and 1.0)
 */
void wiic_set_mp_smooth(struct wiimote_t* wm, int status, float alpha) {
        wm->exp.mp.smooth = status;
        wm->exp.mp.smooth_alpha = alpha;
}