File content as of revision 0:da10b9c9db7a:

/**
 *****************************************************************************
 * File Name    : GyroSensor.cpp
 *
 * Title        : Gyro sensor Class Source File
 * Revision     : 0.1
 * Notes        :
 * Target Board : mbed NXP LPC1768
 * Tool Chain   : ????
 *
 * Revision History:
 * When         Who         Description of change
 * -----------  ----------- -----------------------
 * 2012/07/10   Hiroshi M   init
 *****************************************************************************
 *
 * Copyright (C) 2012 Hiroshi M, MIT License
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
 * and associated documentation files (the "Software"), to deal in the Software without restriction,
 * including without limitation the rights to use, copy, modify, merge, publish, distribute,
 * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all copies or
 * substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
 * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 *
 **/

/* Includes ----------------------------------------------------------------- */
#include "GyroSensor.h"
#include "mbed.h"
#include "util.h"

/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
static int data[16];

/* member fanctions --------------------------------------------------------- */

// Constractor
GyroSensor::GyroSensor(PinName _pin_no=p15, GYRO_CALLBACK_FUNC _p_callback=NULL, GET_DISTANCE_FEEDBACK_FUNC _p_feedback=NULL, GET_DISTANCE_GAIN_FUNC _p_disatnce_gain=NULL)
:gyro_adc_in(_pin_no)
{
    callback = _p_callback;
    getDistanceFeedback = _p_feedback;
    getDisatnceGain = _p_disatnce_gain;
}

// Destrutctor
GyroSensor::~GyroSensor()
{
}

GyroState GyroSensor::getState(void)
{
    return state;
}

int GyroSensor::getAngle(void)
{
    return gya;            //
}

int GyroSensor::getVelocity(void)
{
    return gyv;            //
}

int GyroSensor::getOffset(void)
{
    return offset;         //
}

void GyroSensor::setReady(void)
{
    state = READY;
}

void GyroSensor::setIdle(void)
{
    state = IDLE;
}

void GyroSensor::ad_conv(void) 
{
    static int i = 0;
    static float sum = 0.0;
    float gyro_volt;

    if (i++==32) {
        gyro_volt = (3.3 * sum) / 32.0;
        adcav = (int)(1024 * gyro_volt*1.5);
        sum = 0.0;
        i = 0;
        gyro_control();
    } else {
        sum += gyro_adc_in.read();// Gyro read
    }
}



void GyroSensor::getParameter(void)
{
    if(getDisatnceGain!=NULL) {
        d_gain = getDisatnceGain();
    }
    else {
        d_gain = 1;
    }
    
    if (getDistanceFeedback!=NULL) {
        d_fb = getDistanceFeedback();
    }
    else {
        d_fb = 0; 
    }
}

void GyroSensor::gyro_control(void) {
    int i;
    static int x = 0;

    switch (state) {
        case CLEAR:
            gyaa = 0;
            gyv = 0;
            gya = 0;
            x = 0;
            offset_count = 0;
            
            d_fb = 0;
            d_gain = 0;
           
            offset_sum_old = offset_sum_old_ini;
            offset_sum = offset_sum_ini;
            state = INI;
            break;

        case INI:
            if (offset_count == AV_COUNT) {
                for (i = 0; i < 15; i++) {
                    data[i] = data[i + 1];
                }
                offset = (int)(offset_sum / AV_COUNT);
                data[15] = offset;
                offset_sum_old += offset_sum;
                offset_ini_count++;
                offset_sum = 0;
                offset_count = 0;
            }
            if (offset_ini_count == INI_COUNT) {
                offset = (int)(offset_sum_old / ((int)AV_COUNT * INI_COUNT));
                offset_sum_old = offset_sum_old / INI_COUNT;
                offset_sum_ini = offset_sum;
                offset_sum_old_ini = offset_sum_old;
                state = IDLE;
                break;
            }
            offset_sum += adcav;
            offset_count++;
            break;

        case IDLE:
            break;

        case READY:
            getParameter(); // get d_fb & d_gain.
            if (offset_count == AV_COUNT) {
                if (((offset_sum - offset_sum_old) < (int)AV_COUNT * 4) && ((offset_sum - offset_sum_old) > (int)AV_COUNT * -4)) {
                    offset = (int)((offset_sum + d_fb / d_gain) / AV_COUNT);
                    offset_sum_old = offset_sum;
                    x= 0;
                    for (i = 0; i < 15; i++) {
                        data[i] = data[i + 1];
                    }
                    data[15] = offset;
                } else {
                    offset = lsm(data, x++) + d_fb / (d_gain * AV_COUNT);
                    offset_sum_old = offset * AV_COUNT;
                }
                offset_sum = 0;
                offset_count = 0;
            }
            offset_sum += adcav;
            offset_count++;
            gyv = (adcav - offset) / 8;
            gyaa += gyv;
            gya = gyaa / 32;
            if (callback!=NULL) {
                state = callback(gyv, gyv, offset);
            }
            break;

        default:
            state = CLEAR;
            break;
    }
}

void GyroSensor::Enable(void)
{
    Pulse.attach_us(this,&GyroSensor::ad_conv,312);
}

void GyroSensor::Disable(void)
{
    Pulse.detach();
}