File content as of revision 73:0720e5550476:

#include "Imu.h"
#include "comFunction.h"
#include "globalFlags.h"
#define BAUD_RATE4_6AXIS   115200
#define YAW_RATE_LIMIT     0.5
#define RCV_BUF_SIZE    63
//------------------------------------------------------------------
//IMUからのUART受信データをバッファリングする
//------------------------------------------------------------------
void Imu::PushBuf(UCHAR rxChar){
    static  UCHAR   buf[RCV_BUF_SIZE]={0};
    static  int     idxBufByG=0;
    static bool flg = false;
    static int32_t old_gyroZ = 0;
    static float old_yaw = 0;

    float   tmpYaw;
    
    if(rxChar != LF){
        buf[idxBufByG] = rxChar;//バッファに詰めて
        if(idxBufByG >= RCV_BUF_SIZE-1){
            //バッファクリア
            idxBufByG = 0;
            memset(buf, 0x00, sizeof(buf));
        } else {
            ++idxBufByG;//バッファポインタを進める
        }
    }else{
        if(gf_EnbImuRead){
            //改行コードだったら
            string                 str((char*)buf);
            std::vector<string>    list = f_Split(str,HT);//水平タブで切ってベクタにする
            
            //if( (list.size() > 0) && (list.size() < 4) ){
            if( (list.size() == 2) ){
                //ヨー角
                tmpYaw= (float)atof(list.at(0).c_str());
                yaw   = wrapAroungGuard(tmpYaw);
                gyroZ = (int32_t)( strtol( list.at(1).c_str() , NULL , 16) );//Z軸角速度   
                old_gyroZ = gyroZ;
                old_yaw = yaw;
                flg = true;
            }
        }
        else
        {
            if(flg)
            {
                yaw = old_yaw;
                gyroZ = old_gyroZ;
            }
            else
            {
                yaw = 0;
                gyroZ = 0;
            }
        }
        //バッファクリア
        idxBufByG = 0;
        memset(buf, 0x00, sizeof(buf));
        
        led4 = !led4;
    }
}
//----------------------------------------------------------------
//ラップアラウンド防止 180度からマイナスへ飛ばないようにする
//----------------------------------------------------------------
float Imu::wrapAroungGuard(float iYaw){
    
    extern Serial sp;
    
    static float localYaw = 0;
    float dy = localYaw - (iYaw + yawOfset);//１週前との差
    if(dy >= 180){
        dy -= 360;
        yawOfset += 360;
    }
    if(dy <= -180){
        dy += 360;
        yawOfset -= 360;
    }
    dy = limitYawRate(dy);//変化量に制限を掛ける
    localYaw -= dy;
    return localYaw;
}

//----------------------------------------------------------------
// 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 = YAW_RATE_LIMIT * 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(){
int32_t Imu::GetGyroRef(){
    return gyro_ref;
}
//------------------------------------------------
//基準値を更新する
//------------------------------------------------
void Imu::CalYaw() {yaw_ref = yaw;}
void Imu::CalGyro(){gyro_ref = gyroZ;}