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@62:4b873328d3a8, 2019-03-11 (annotated)

Committer:
YutakaTakagi
Date:
Mon Mar 11 04:07:43 2019 +0000
Revision:
62:4b873328d3a8
Parent:
58:fb7876549148
Child:
67:ad35921e3cdc
Child:
70:a7b1e3eec51b
update IMU limit and TargetAngle setting

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;//１週前との差
YutakaTakagi 58:fb7876549148 43 dy = limitYawRate(dy);
takeru0x1103 18:5aa48aec9cae 44 if(dy>=180){yawOfset+=360;}
takeru0x1103 18:5aa48aec9cae 45 if(dy<=-180){yawOfset-=360;}
YutakaTakagi 58:fb7876549148 46 oldYaw -= dy;//次の週で使う過去データ更新
YutakaTakagi 62:4b873328d3a8 47 return oldYaw + yawOfset;
takeru0x1103 18:5aa48aec9cae 48 }
takeru0x1103 18:5aa48aec9cae 49
YutakaTakagi 58:fb7876549148 50 //----------------------------------------------------------------
YutakaTakagi 58:fb7876549148 51 // Yaw角の変化量に制限をかける 値飛び対策
YutakaTakagi 58:fb7876549148 52 //----------------------------------------------------------------
YutakaTakagi 58:fb7876549148 53 float Imu::limitYawRate(float yawDiff){
YutakaTakagi 58:fb7876549148 54 float tmpDiff = yawDiff;
YutakaTakagi 58:fb7876549148 55 if (yawRateLimit < fabs(yawDiff * UPDATE_RATE)){
YutakaTakagi 58:fb7876549148 56 const float limitedDiff = yawRateLimit / static_cast<float>(UPDATE_RATE);
YutakaTakagi 58:fb7876549148 57 tmpDiff = 0 < yawDiff ? limitedDiff : -limitedDiff;
YutakaTakagi 58:fb7876549148 58 }
YutakaTakagi 58:fb7876549148 59 return tmpDiff;
YutakaTakagi 58:fb7876549148 60 }
YutakaTakagi 58:fb7876549148 61
takeru0x1103 18:5aa48aec9cae 62 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
takeru0x1103 18:5aa48aec9cae 63 // UART 受信割り込みハンドラ for Arduino UNO(Six Axis Sensor)
takeru0x1103 18:5aa48aec9cae 64 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
takeru0x1103 18:5aa48aec9cae 65 void Imu::uartRxIntHndler46Axis(){
takeru0x1103 18:5aa48aec9cae 66 UCHAR buf = sp46Axis.getc();//1Byte抜き出す
takeru0x1103 18:5aa48aec9cae 67 PushBuf(buf);//バッファに突っ込む
takeru0x1103 18:5aa48aec9cae 68 }
takeru0x1103 18:5aa48aec9cae 69 //======================================================
takeru0x1103 18:5aa48aec9cae 70 // コンストラクタ
takeru0x1103 18:5aa48aec9cae 71 //======================================================
takeru0x1103 18:5aa48aec9cae 72 Imu::Imu(PinName pinTx, PinName pinRx) : sp46Axis(pinTx, pinRx) {
takeru0x1103 18:5aa48aec9cae 73 //ボーレート設定
takeru0x1103 18:5aa48aec9cae 74 sp46Axis.baud(BAUD_RATE4_6AXIS);
takeru0x1103 18:5aa48aec9cae 75 //受信割り込みハンドラ登録
takeru0x1103 18:5aa48aec9cae 76 sp46Axis.attach(this,&Imu::uartRxIntHndler46Axis , Serial::RxIrq);
takeru0x1103 18:5aa48aec9cae 77 //プロパティ初期化
takeru0x1103 18:5aa48aec9cae 78 yaw = yaw_ref = 0.0f;
takeru0x1103 18:5aa48aec9cae 79 gyroZ = gyro_ref = 0;
takeru0x1103 18:5aa48aec9cae 80 yawOfset=0;
YutakaTakagi 62:4b873328d3a8 81 yawRateLimit = 1.3 * UPDATE_RATE;//センサ実測値をベースにリミット値決定
takeru0x1103 18:5aa48aec9cae 82 }
takeru0x1103 18:5aa48aec9cae 83
takeru0x1103 18:5aa48aec9cae 84 //======================================================
takeru0x1103 18:5aa48aec9cae 85 // デストラクタ
takeru0x1103 18:5aa48aec9cae 86 //======================================================
takeru0x1103 18:5aa48aec9cae 87 Imu::~Imu(){}
takeru0x1103 18:5aa48aec9cae 88
takeru0x1103 18:5aa48aec9cae 89 //------------------------------------------------
takeru0x1103 18:5aa48aec9cae 90 //ヨー角
takeru0x1103 18:5aa48aec9cae 91 //------------------------------------------------
takeru0x1103 18:5aa48aec9cae 92 float Imu::GetYaw(){
takeru0x1103 18:5aa48aec9cae 93 return yaw - yaw_ref;//角度を返す:来た値のままだと初期位置が0度じゃないので、基準値を引いて返す
takeru0x1103 18:5aa48aec9cae 94 }
takeru0x1103 18:5aa48aec9cae 95
takeru0x1103 18:5aa48aec9cae 96 //------------------------------------------------
takeru0x1103 18:5aa48aec9cae 97 //角速度
takeru0x1103 18:5aa48aec9cae 98 //------------------------------------------------
takeru0x1103 18:5aa48aec9cae 99 float Imu::GetGyroZ(){
takeru0x1103 18:5aa48aec9cae 100 INT16 tmp = gyroZ - gyro_ref;
takeru0x1103 18:5aa48aec9cae 101 float rate = (float)tmp / cGYRO_RESO;
takeru0x1103 18:5aa48aec9cae 102 return rate;
takeru0x1103 18:5aa48aec9cae 103 }
MasashiNomura 25:f3a6e7eec9c3 104 float Imu::GetYawRef(){
MasashiNomura 25:f3a6e7eec9c3 105 return yaw_ref;
MasashiNomura 25:f3a6e7eec9c3 106 }
MasashiNomura 25:f3a6e7eec9c3 107 INT16 Imu::GetGyroRef(){
MasashiNomura 25:f3a6e7eec9c3 108 return gyro_ref;
MasashiNomura 25:f3a6e7eec9c3 109 }
takeru0x1103 18:5aa48aec9cae 110 //------------------------------------------------
takeru0x1103 18:5aa48aec9cae 111 //基準値を更新する
takeru0x1103 18:5aa48aec9cae 112 //------------------------------------------------
takeru0x1103 18:5aa48aec9cae 113 void Imu::CalYaw() {yaw_ref = yaw;}
takeru0x1103 18:5aa48aec9cae 114 void Imu::CalGyro(){gyro_ref = gyroZ;}