Diff: Imu.cpp

Revision:

18:5aa48aec9cae

Child:

25:f3a6e7eec9c3

diff -r f9610f3cfa1b -r 5aa48aec9cae Imu.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Imu.cpp	Sat Dec 01 14:03:08 2018 +0000
@@ -0,0 +1,94 @@
+#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;//１週前との差
+    if(dy>=180){yawOfset+=360;}
+    if(dy<=-180){yawOfset-=360;}
+    oldYaw = iYaw;//次の週で使う過去データ更新
+    return iYaw + yawOfset;
+}
+
+//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+// 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;
+}
+
+//======================================================
+// デストラクタ
+//======================================================
+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;
+}
+//------------------------------------------------
+//基準値を更新する
+//------------------------------------------------
+void Imu::CalYaw() {yaw_ref = yaw;}
+void Imu::CalGyro(){gyro_ref = gyroZ;}
\ No newline at end of file