File content as of revision 75:215dd63032d6:

#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 UINT32   buf_over_count = 0;
    static UINT32   unmatch_count = 0;
    static UINT32   ang_over_range_count = 0;

    if(rxChar != LF){
        if(idxBufByG >= RCV_BUF_SIZE - 1){
            flg = true;//オーバー時のデータは使わない
            buf_over_count++;
        }
        else{
            buf[idxBufByG] = rxChar;//バッファに詰めて
            ++idxBufByG;//バッファポインタを進める
        }
    }
    else{
        if(gf_EnbImuRead && !flg){
            //改行コードだったら
            string                 str((char*)buf);
            std::vector<string>    list = f_Split(str,HT);//水平タブで切ってベクタにする
            
            if(list.size() == 4){
                float   tmpYaw1,tmpYaw2;
                int32_t tmpGyroZ1,tmpGyroZ2;
                //ヨー角,Z軸角速度
                tmpYaw1 = (float)atof(list.at(0).c_str());
                tmpGyroZ1 = (int32_t)( strtol( list.at(1).c_str() , NULL , 16) );
                tmpYaw2 = (float)atof(list.at(2).c_str());
                tmpGyroZ2 = (int32_t)( strtol( list.at(3).c_str() , NULL , 16) );
                //通信時の化け防止 2つのデータがそれぞれ同じであることを確認する
                if (tmpYaw1 == tmpYaw2 && tmpGyroZ1 == tmpGyroZ2){
                    //Nanチェックと範囲外の値が入ってないかチェック
                    if (!isnan(tmpYaw1) && (-180.0 < tmpYaw1 && 180.0 > tmpYaw1)){
                        yaw   = wrapAroungGuard(tmpYaw1);
                    }
                    else{ang_over_range_count++;}
                    if (!isnan(tmpGyroZ1)){
                        gyroZ = tmpGyroZ1;
                    }
                }
                else {unmatch_count++;}
            }
        }
        //バッファクリア
        idxBufByG = 0;
        memset(buf, 0x00, sizeof(buf));
        
        flg = false;
        led4 = !led4;
    }
    if (gf_Print.d1.bf.err){
        sp.printf("IMU NG COUNT buf: %u, unmatch: %u, angNG: %u\n",buf_over_count, unmatch_count, ang_over_range_count);
        gf_Print.d1.bf.err=false;
    }
}
//----------------------------------------------------------------
//ラップアラウンド防止 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;}