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@67:ad35921e3cdc, 2019-03-11 (annotated)

Committer:
YutakaTakagi
Date:
Mon Mar 11 06:36:33 2019 +0000
Revision:
67:ad35921e3cdc
Parent:
62:4b873328d3a8 
add reset att value function. if set PID ON, reset att value

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