Imu.cpp

00001 #include "Imu.h"
00002 #include "comFunction.h"
00003 #include "globalFlags.h"
00004 #define BAUD_RATE4_6AXIS   115200
00005 #define YAW_RATE_LIMIT     0.5
00006 #define RCV_BUF_SIZE    63
00007 //------------------------------------------------------------------
00008 //IMUからのUART受信データをバッファリングする
00009 //------------------------------------------------------------------
00010 void Imu::PushBuf(UCHAR rxChar){
00011     static  UCHAR   buf[RCV_BUF_SIZE]={0};
00012     static  int     idxBufByG=0;
00013     static bool flg = false;
00014     static UINT32   buf_over_count = 0;
00015     static UINT32   unmatch_count = 0;
00016     static UINT32   ang_over_range_count = 0;
00017 
00018     if(rxChar != LF){
00019         if(idxBufByG >= RCV_BUF_SIZE - 1){
00020             flg = true;//オーバー時のデータは使わない
00021             buf_over_count++;
00022         }
00023         else{
00024             buf[idxBufByG] = rxChar;//バッファに詰めて
00025             ++idxBufByG;//バッファポインタを進める
00026         }
00027     }
00028     else{
00029         if(gf_EnbImuRead && !flg){
00030             //改行コードだったら
00031             string                 str((char*)buf);
00032             std::vector<string>    list = f_Split(str,HT);//水平タブで切ってベクタにする
00033             
00034             if(list.size() == 4){
00035                 float   tmpYaw1,tmpYaw2;
00036                 int32_t tmpGyroZ1,tmpGyroZ2;
00037                 //ヨー角,Z軸角速度
00038                 tmpYaw1 = (float)atof(list.at(0).c_str());
00039                 tmpGyroZ1 = (int32_t)( strtol( list.at(1).c_str() , NULL , 16) );
00040                 tmpYaw2 = (float)atof(list.at(2).c_str());
00041                 tmpGyroZ2 = (int32_t)( strtol( list.at(3).c_str() , NULL , 16) );
00042                 //通信時の化け防止 2つのデータがそれぞれ同じであることを確認する
00043                 if (tmpYaw1 == tmpYaw2 && tmpGyroZ1 == tmpGyroZ2){
00044                     //Nanチェックと範囲外の値が入ってないかチェック
00045                     if (!isnan(tmpYaw1) && (-180.0 < tmpYaw1 && 180.0 > tmpYaw1)){
00046                         yaw   = wrapAroungGuard(tmpYaw1);
00047                     }
00048                     else{ang_over_range_count++;}
00049                     if (!isnan(tmpGyroZ1)){
00050                         gyroZ = tmpGyroZ1;
00051                     }
00052                 }
00053                 else {unmatch_count++;}
00054             }
00055         }
00056         //バッファクリア
00057         idxBufByG = 0;
00058         memset(buf, 0x00, sizeof(buf));
00059         
00060         flg = false;
00061         led4 = !led4;
00062     }
00063     if (gf_Print.d1.bf.err){
00064         sp.printf("IMU NG COUNT buf: %u, unmatch: %u, angNG: %u\n",buf_over_count, unmatch_count, ang_over_range_count);
00065         gf_Print.d1.bf.err=false;
00066     }
00067 }
00068 //----------------------------------------------------------------
00069 //ラップアラウンド防止 180度からマイナスへ飛ばないようにする
00070 //----------------------------------------------------------------
00071 float Imu::wrapAroungGuard(float iYaw){
00072     
00073     extern Serial sp;
00074     
00075     static float localYaw = 0;
00076     float dy = localYaw - (iYaw + yawOfset);//１週前との差
00077     if(dy >= 180){
00078         dy -= 360;
00079         yawOfset += 360;
00080     }
00081     if(dy <= -180){
00082         dy += 360;
00083         yawOfset -= 360;
00084     }
00085     dy = limitYawRate(dy);//変化量に制限を掛ける
00086     localYaw -= dy;
00087     return localYaw;
00088 }
00089 
00090 //----------------------------------------------------------------
00091 // Yaw角の変化量に制限をかける 値飛び対策
00092 //----------------------------------------------------------------
00093 float Imu::limitYawRate(float yawDiff){
00094     float tmpDiff = yawDiff;
00095     if (yawRateLimit < fabs(yawDiff * UPDATE_RATE)){
00096         const float limitedDiff = yawRateLimit / static_cast<float>(UPDATE_RATE); 
00097         tmpDiff = 0 < yawDiff ? limitedDiff : -limitedDiff;
00098     }
00099     return tmpDiff;
00100 }
00101 
00102 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00103 // UART 受信割り込みハンドラ for Arduino UNO(Six Axis Sensor)
00104 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00105 void Imu::uartRxIntHndler46Axis(){
00106     UCHAR buf = sp46Axis.getc();//1Byte抜き出す
00107     PushBuf(buf);//バッファに突っ込む
00108 }
00109 //======================================================
00110 // コンストラクタ
00111 //======================================================
00112 Imu::Imu(PinName pinTx, PinName pinRx) : sp46Axis(pinTx, pinRx) {
00113     //ボーレート設定
00114     sp46Axis.baud(BAUD_RATE4_6AXIS);
00115     //受信割り込みハンドラ登録
00116     sp46Axis.attach(this,&Imu::uartRxIntHndler46Axis , Serial::RxIrq);
00117     //プロパティ初期化
00118     yaw = yaw_ref = 0.0f;
00119     gyroZ = gyro_ref = 0;
00120     yawOfset = 0;
00121     yawRateLimit = YAW_RATE_LIMIT * UPDATE_RATE;//センサ実測値をベースにリミット値決定
00122 }
00123 
00124 //======================================================
00125 // デストラクタ
00126 //======================================================
00127 Imu::~Imu(){}
00128 
00129 //------------------------------------------------
00130 //ヨー角
00131 //------------------------------------------------
00132 float Imu::GetYaw(){
00133     return yaw - yaw_ref;//角度を返す:来た値のままだと初期位置が0度じゃないので、基準値を引いて返す
00134 }
00135 
00136 //------------------------------------------------
00137 //角速度
00138 //------------------------------------------------
00139 float Imu::GetGyroZ(){
00140     INT16 tmp = gyroZ - gyro_ref;
00141     float rate = (float)tmp / cGYRO_RESO;
00142     return rate;
00143 }
00144 float Imu::GetYawRef(){
00145     return yaw_ref;
00146 }
00147 //INT16 Imu::GetGyroRef(){
00148 int32_t Imu::GetGyroRef(){
00149     return gyro_ref;
00150 }
00151 //------------------------------------------------
00152 //基準値を更新する
00153 //------------------------------------------------
00154 void Imu::CalYaw() {yaw_ref = yaw;}
00155 void Imu::CalGyro(){gyro_ref = gyroZ;}