File content as of revision 58:fb7876549148:

#include "Imu.h"
#include "comFunction.h"
#include "globalFlags.h"
#define BAUD_RATE4_6AXIS   115200

//------------------------------------------------------------------
//IMUからのUART受信データをバッファリングする
//------------------------------------------------------------------
void Imu::PushBuf(UCHAR rxChar){
    static  UCHAR   buf[63]={0};
    static  int     idxBufByG=0;
    float   tmpYaw;
    
    if(rxChar != LF){
        buf[idxBufByG] = rxChar;//バッファに詰めて
        ++idxBufByG;//バッファポインタを進める
    }else{
        //改行コードだったら
        string                 str((char*)buf);
        std::vector<string>    list = f_Split(str,HT);//水平タブで切ってベクタにする
        
        if( (list.size() > 0) && (list.size() < 4) ){
            //ヨー角
            tmpYaw= (float)atof(list.at(0).c_str());
            yaw   = wrapAroungGuard(tmpYaw);
            gyroZ = (INT16)( strtol( list.at(1).c_str() , NULL , 16) );//Z軸角速度   
        }
        //バッファクリア
        idxBufByG = 0;
        memset(buf, 0x00, sizeof(buf));
        led4 = !led4;
    }
}
//----------------------------------------------------------------
//ラップアラウンド防止 180度からマイナスへ飛ばないようにする
//----------------------------------------------------------------
float Imu::wrapAroungGuard(float iYaw){
    
    extern Serial sp;
    
    static float oldYaw=0;
    float dy = oldYaw - iYaw;//１週前との差
    dy = limitYawRate(dy);
    if(dy>=180){yawOfset+=360;}
    if(dy<=-180){yawOfset-=360;}
    oldYaw -= dy;//次の週で使う過去データ更新
    return iYaw + yawOfset;
}

//----------------------------------------------------------------
// Yaw角の変化量に制限をかける 値飛び対策
//----------------------------------------------------------------
float Imu::limitYawRate(float yawDiff){
    float tmpDiff = yawDiff;
    if (yawRateLimit < fabs(yawDiff * UPDATE_RATE)){
        const float limitedDiff = yawRateLimit / static_cast<float>(UPDATE_RATE); 
        tmpDiff = 0 < yawDiff ? limitedDiff : -limitedDiff;
    }
    return tmpDiff;
}

//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// UART 受信割り込みハンドラ for Arduino UNO(Six Axis Sensor)
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
void Imu::uartRxIntHndler46Axis(){
    UCHAR buf = sp46Axis.getc();//1Byte抜き出す
    PushBuf(buf);//バッファに突っ込む
}
//======================================================
// コンストラクタ
//======================================================
Imu::Imu(PinName pinTx, PinName pinRx) : sp46Axis(pinTx, pinRx) {
    //ボーレート設定
    sp46Axis.baud(BAUD_RATE4_6AXIS);
    //受信割り込みハンドラ登録
    sp46Axis.attach(this,&Imu::uartRxIntHndler46Axis , Serial::RxIrq);
    //プロパティ初期化
    yaw = yaw_ref = 0.0f;
    gyroZ = gyro_ref = 0;
    yawOfset=0;
    yawRateLimit = 100.0 * UPDATE_RATE;//センサ実測値をベースにリミット値決定
}

//======================================================
// デストラクタ
//======================================================
Imu::~Imu(){}

//------------------------------------------------
//ヨー角
//------------------------------------------------
float Imu::GetYaw(){
    return yaw - yaw_ref;//角度を返す:来た値のままだと初期位置が0度じゃないので、基準値を引いて返す
}

//------------------------------------------------
//角速度
//------------------------------------------------
float Imu::GetGyroZ(){
    INT16 tmp = gyroZ - gyro_ref;
    float rate = (float)tmp / cGYRO_RESO;
    return rate;
}
float Imu::GetYawRef(){
    return yaw_ref;
}
INT16 Imu::GetGyroRef(){
    return gyro_ref;
}
//------------------------------------------------
//基準値を更新する
//------------------------------------------------
void Imu::CalYaw() {yaw_ref = yaw;}
void Imu::CalGyro(){gyro_ref = gyroZ;}