teamALI / Mbed 2 deprecated HB2018

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed FreeRTOS

Imu.cpp@18:5aa48aec9cae, 2018-12-01 (annotated)

Committer:
takeru0x1103
Date:
Sat Dec 01 14:03:08 2018 +0000
Revision:
18:5aa48aec9cae
Child:
25:f3a6e7eec9c3
??????????PID????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
takeru0x1103 18:5aa48aec9cae 1 #include "Imu.h"
takeru0x1103 18:5aa48aec9cae 2 #include "comFunction.h"
takeru0x1103 18:5aa48aec9cae 3 #include "globalFlags.h"
takeru0x1103 18:5aa48aec9cae 4 #define BAUD_RATE4_6AXIS 115200
takeru0x1103 18:5aa48aec9cae 5
takeru0x1103 18:5aa48aec9cae 6 //------------------------------------------------------------------
takeru0x1103 18:5aa48aec9cae 7 //IMUからのUART受信データをバッファリングする
takeru0x1103 18:5aa48aec9cae 8 //------------------------------------------------------------------
takeru0x1103 18:5aa48aec9cae 9 void Imu::PushBuf(UCHAR rxChar){
takeru0x1103 18:5aa48aec9cae 10 static UCHAR buf[63]={0};
takeru0x1103 18:5aa48aec9cae 11 static int idxBufByG=0;
takeru0x1103 18:5aa48aec9cae 12 float tmpYaw;
takeru0x1103 18:5aa48aec9cae 13
takeru0x1103 18:5aa48aec9cae 14 if(rxChar != LF){
takeru0x1103 18:5aa48aec9cae 15 buf[idxBufByG] = rxChar;//バッファに詰めて
takeru0x1103 18:5aa48aec9cae 16 ++idxBufByG;//バッファポインタを進める
takeru0x1103 18:5aa48aec9cae 17 }else{
takeru0x1103 18:5aa48aec9cae 18 //改行コードだったら
takeru0x1103 18:5aa48aec9cae 19 string str((char*)buf);
takeru0x1103 18:5aa48aec9cae 20 std::vector<string> list = f_Split(str,HT);//水平タブで切ってベクタにする
takeru0x1103 18:5aa48aec9cae 21
takeru0x1103 18:5aa48aec9cae 22 if( (list.size() > 0) && (list.size() < 4) ){
takeru0x1103 18:5aa48aec9cae 23 //ヨー角
takeru0x1103 18:5aa48aec9cae 24 tmpYaw= (float)atof(list.at(0).c_str());
takeru0x1103 18:5aa48aec9cae 25 yaw = wrapAroungGuard(tmpYaw);
takeru0x1103 18:5aa48aec9cae 26 gyroZ = (INT16)( strtol( list.at(1).c_str() , NULL , 16) );//Z軸角速度
takeru0x1103 18:5aa48aec9cae 27 }
takeru0x1103 18:5aa48aec9cae 28 //バッファクリア
takeru0x1103 18:5aa48aec9cae 29 idxBufByG = 0;
takeru0x1103 18:5aa48aec9cae 30 memset(buf, 0x00, sizeof(buf));
takeru0x1103 18:5aa48aec9cae 31 led4 = !led4;
takeru0x1103 18:5aa48aec9cae 32 }
takeru0x1103 18:5aa48aec9cae 33 }
takeru0x1103 18:5aa48aec9cae 34 //----------------------------------------------------------------
takeru0x1103 18:5aa48aec9cae 35 //ラップアラウンド防止 180度からマイナスへ飛ばないようにする
takeru0x1103 18:5aa48aec9cae 36 //----------------------------------------------------------------
takeru0x1103 18:5aa48aec9cae 37 float Imu::wrapAroungGuard(float iYaw){
takeru0x1103 18:5aa48aec9cae 38
takeru0x1103 18:5aa48aec9cae 39 extern Serial sp;
takeru0x1103 18:5aa48aec9cae 40
takeru0x1103 18:5aa48aec9cae 41 static float oldYaw=0;
takeru0x1103 18:5aa48aec9cae 42 float dy = oldYaw - iYaw;//１週前との差
takeru0x1103 18:5aa48aec9cae 43 if(dy>=180){yawOfset+=360;}
takeru0x1103 18:5aa48aec9cae 44 if(dy<=-180){yawOfset-=360;}
takeru0x1103 18:5aa48aec9cae 45 oldYaw = iYaw;//次の週で使う過去データ更新
takeru0x1103 18:5aa48aec9cae 46 return iYaw + yawOfset;
takeru0x1103 18:5aa48aec9cae 47 }
takeru0x1103 18:5aa48aec9cae 48
takeru0x1103 18:5aa48aec9cae 49 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
takeru0x1103 18:5aa48aec9cae 50 // UART 受信割り込みハンドラ for Arduino UNO(Six Axis Sensor)
takeru0x1103 18:5aa48aec9cae 51 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
takeru0x1103 18:5aa48aec9cae 52 void Imu::uartRxIntHndler46Axis(){
takeru0x1103 18:5aa48aec9cae 53 UCHAR buf = sp46Axis.getc();//1Byte抜き出す
takeru0x1103 18:5aa48aec9cae 54 PushBuf(buf);//バッファに突っ込む
takeru0x1103 18:5aa48aec9cae 55 }
takeru0x1103 18:5aa48aec9cae 56 //======================================================
takeru0x1103 18:5aa48aec9cae 57 // コンストラクタ
takeru0x1103 18:5aa48aec9cae 58 //======================================================
takeru0x1103 18:5aa48aec9cae 59 Imu::Imu(PinName pinTx, PinName pinRx) : sp46Axis(pinTx, pinRx) {
takeru0x1103 18:5aa48aec9cae 60 //ボーレート設定
takeru0x1103 18:5aa48aec9cae 61 sp46Axis.baud(BAUD_RATE4_6AXIS);
takeru0x1103 18:5aa48aec9cae 62 //受信割り込みハンドラ登録
takeru0x1103 18:5aa48aec9cae 63 sp46Axis.attach(this,&Imu::uartRxIntHndler46Axis , Serial::RxIrq);
takeru0x1103 18:5aa48aec9cae 64 //プロパティ初期化
takeru0x1103 18:5aa48aec9cae 65 yaw = yaw_ref = 0.0f;
takeru0x1103 18:5aa48aec9cae 66 gyroZ = gyro_ref = 0;
takeru0x1103 18:5aa48aec9cae 67 yawOfset=0;
takeru0x1103 18:5aa48aec9cae 68 }
takeru0x1103 18:5aa48aec9cae 69
takeru0x1103 18:5aa48aec9cae 70 //======================================================
takeru0x1103 18:5aa48aec9cae 71 // デストラクタ
takeru0x1103 18:5aa48aec9cae 72 //======================================================
takeru0x1103 18:5aa48aec9cae 73 Imu::~Imu(){}
takeru0x1103 18:5aa48aec9cae 74
takeru0x1103 18:5aa48aec9cae 75 //------------------------------------------------
takeru0x1103 18:5aa48aec9cae 76 //ヨー角
takeru0x1103 18:5aa48aec9cae 77 //------------------------------------------------
takeru0x1103 18:5aa48aec9cae 78 float Imu::GetYaw(){
takeru0x1103 18:5aa48aec9cae 79 return yaw - yaw_ref;//角度を返す:来た値のままだと初期位置が0度じゃないので、基準値を引いて返す
takeru0x1103 18:5aa48aec9cae 80 }
takeru0x1103 18:5aa48aec9cae 81
takeru0x1103 18:5aa48aec9cae 82 //------------------------------------------------
takeru0x1103 18:5aa48aec9cae 83 //角速度
takeru0x1103 18:5aa48aec9cae 84 //------------------------------------------------
takeru0x1103 18:5aa48aec9cae 85 float Imu::GetGyroZ(){
takeru0x1103 18:5aa48aec9cae 86 INT16 tmp = gyroZ - gyro_ref;
takeru0x1103 18:5aa48aec9cae 87 float rate = (float)tmp / cGYRO_RESO;
takeru0x1103 18:5aa48aec9cae 88 return rate;
takeru0x1103 18:5aa48aec9cae 89 }
takeru0x1103 18:5aa48aec9cae 90 //------------------------------------------------
takeru0x1103 18:5aa48aec9cae 91 //基準値を更新する
takeru0x1103 18:5aa48aec9cae 92 //------------------------------------------------
takeru0x1103 18:5aa48aec9cae 93 void Imu::CalYaw() {yaw_ref = yaw;}
takeru0x1103 18:5aa48aec9cae 94 void Imu::CalGyro(){gyro_ref = gyroZ;}