BMX055 data by Madgwick Filter sends to PC and shows it by Python program
Dependencies: MadgwickFilter BMX055
Diff: main.cpp
- Revision:
- 1:a0ca9f62e81f
- Parent:
- 0:141e4576190a
--- a/main.cpp Mon Mar 04 04:15:19 2019 +0000 +++ b/main.cpp Fri Feb 07 04:28:46 2020 +0000 @@ -1,25 +1,21 @@ /* - * Mbed Application program / / Study 2018 day12 -> BMX055(Gyro+Acc+Mag) + * Mbed Application program / BMX055(Gyro+Acc+Mag) + Madgwick Filter + * tested on Nucleo-F446RE * - * Copyright (c) 2019 Kenji Arai / JH1PJL + * Copyright (c) 2020 Kenji Arai / JH1PJL * http://www.page.sannet.ne.jp/kenjia/index.html * http://mbed.org/users/kenjiArai/ - * Created: February 26th, 2019 - * Revised: March 3rd, 2019 + * Created: February 7th, 2020 + * Revised: February 7th, 2020 */ // Include -------------------------------------------------------------------- -// Mbedでプログラムを作成する時にはこの一行は必須です #include "mbed.h" -// Bosch製BMX055 9軸(Gyro-3軸+Acc-3軸+Mag-3軸)を使用するための定義ファイル #include "BMX055.h" -// 慣性測定装置(IMU)として、クオータニオンという座標系を使用する #include "Quaternion.hpp" -// 慣性測定装置(IMU)として、Madgwickフィルターを使用する #include "MadgwickFilter.hpp" // Definition ----------------------------------------------------------------- -// 列挙型と呼ばれる値の定義を行うACC_DATAが1で、IMU_DATAが2となる(define文で定義も可) enum { ALL_DATA = 0, ACC_DATA, @@ -28,43 +24,31 @@ IMU_DATA }; -// whileループの周期(LOOP_TIME)を設定(mSec)し、全部を出す周期、加速度を出す周期、IMUデータ -// 周期を定義している(初期設定では、測定周期5mS、IMUモードでは100mS、ACCモードで200mS毎) #define LOOP_TIME 5 #define OUT_ALL_CYC (1000 / LOOP_TIME) #define OUT_ACC_CYC (200 / LOOP_TIME) #define OUT_IMU_CYC (200 / LOOP_TIME) // Object --------------------------------------------------------------------- -// 2本線でデータのやり取りをするI2Cのクラスを使用して、i2clcdというインスタンスを生成 +DigitalIn mode_sw(PC_8, PullUp); I2C i2c(I2C_SDA, I2C_SCL); -// i2cのラインを使用して、BMX055を動作させる BMX055 imu(i2c); -// シリアルという通信形式のクラスを宣言し、pcという実態を作り出している -// これは、Mbedでは定番の設定で、この宣言でPCとUSBケーブルを通じて仮想COMラインでの -// 通信が可能になる(送信及び受信の双方向通信 -// 今回は「Serial」でなく「RawSerial」であることに注意 -// !!!!! 注意→通信速度が変更になっているので、Tera Termなどの受信側のボーレート変更要 !!!!! -RawSerial pc(USBTX, USBRX, 921600); -// 時間計測用にタイマーを使用する(ストップウォッチとして) -Timer t; -// 地磁気センサーデータを使用するか否かを選択するスイッチ -DigitalIn mode_sw(PC_8, PullUp); +RawSerial pc(USBTX, USBRX, 115200); +Timer t; // RAM ------------------------------------------------------------------------ // ROM / Constant data -------------------------------------------------------- -// 角度をラジアン単位に変換する際の定数をROM内に固定値として保存 const double DEG_TO_RAD = 0.01745329251994329576923690768489f;//PI / 180.0; // Function prototypes -------------------------------------------------------- +void print_revision(void); //------------------------------------------------------------------------------ // Control Program //------------------------------------------------------------------------------ int main() { - // BMX055をどんな条件で使用するかのパラメータ設定 const BMX055_TypeDef bmx055_my_parameters = { // ACC ACC_2G, @@ -76,70 +60,46 @@ MAG_ODR20Hz }; - // BMX055からの加速度3軸データを保存する器 BMX055_ACCEL_TypeDef acc; - // BMX055からのヨーレイト3軸データを保存する器 BMX055_GYRO_TypeDef gyr; - // BMX055からの地磁気3軸データを保存する器 BMX055_MAGNET_TypeDef mag; - - // BMX055を呼び出してハードウェア的に通信が出来て準備完了しているかをチェック + bool state = imu.chip_ready(); - // 準備が完了であればstate=trueとなり、下記でreadyとなる if (state){ pc.printf("ACC+GYR+MAG are ready!\r\n"); - } else { // state=falseではこちらに来る(そのまま制御が継続するが止めてもよい) + } else { pc.printf("ACC+GYR+MAG are NOT ready!\r\n"); } - // パラメータをBMX055に書き込んで、動作条件を決定する imu.set_parameter(&bmx055_my_parameters); - // Madgwickフィルターのコンストラクタを呼び出し、attitudeというインスタンスを作る - //MadgwickFilter attitude(0.05); MadgwickFilter attitude(0.1); - // クオータニオンのインスタンスを呼び出し、myQというインスタンスを作る Quaternion myQ; - // データ出力モードを、全データ出力とする(Tera Termで確認する際に全てのデータが見える) uint32_t output_mode = ALL_DATA; - // 一周ごとにカウントアップする器を用意し、初期化する uint32_t n = 0; - // 電源を切られるまで、一定周期でデータを測定し、出力を繰り返す while(true) { - // ストップウォッチの準備 t.reset(); - // 周期測定開始 t.start(); - // 加速度データをBMX055からaccの器に読み出し imu.get_accel(&acc); - // ヨーレイトデータをBMX055からgyrの器に読み出し imu.get_gyro(&gyr); - // 地磁気データをBMX055からmagの器に読み出し imu.get_magnet(&mag); if (mode_sw == 1) { - // 地磁気データは使用しないように設定 - // 9軸データをフィルターに入力して、フィルター処理を実行(出力は別の場所で) attitude.MadgwickAHRSupdate( gyr.x*DEG_TO_RAD, gyr.y*DEG_TO_RAD, gyr.z*DEG_TO_RAD, acc.x, acc.y, acc.z, 0.0f, 0.0f, 0.0f ); } else { - // 9軸データをフィルターに入力して、フィルター処理を実行(出力は別の場所で) attitude.MadgwickAHRSupdate( gyr.x*DEG_TO_RAD, gyr.y*DEG_TO_RAD, gyr.z*DEG_TO_RAD, acc.x, acc.y, acc.z, -mag.x, -mag.y, -mag.z ); } - // PCからのコマンド分析(Pythonプログラムで、加速度表示かIMU表示かの指示があるはず) - if (pc.readable()){ // PCからコマンドが来ているかチェック - // 文字を受け取る + if (pc.readable()){ char c = pc.getc(); - // 受信した文字が'1'ならば、出力は加速度データを送り出すモードに設定 if (c == '1'){ output_mode = ACC_DATA; - // 文字が'2'ならば、出力はIMU(クオータニオン)データを送り出すモードに設定 } else if (c == '2'){ output_mode = IMU_DATA; } else if (c == '3'){ @@ -148,78 +108,58 @@ output_mode = MAG_DATA; } else if (c == 'R'){ NVIC_SystemReset(); // Reset - } else { // それ以外の文字は無視して、全データを出力するモードに設定 + } else { output_mode = ALL_DATA; } } - // Madgwickフィルターの演算結果をmyQに連動させて取り込む - // (myQのインスタンス内に結果が格納されるので、後から読み出す) attitude.getAttitude(&myQ); - // PCへの出力すべき内容をモード別に処理する switch (output_mode){ - // 加速度データを出力するモード case ACC_DATA: if ((n % OUT_ACC_CYC) == 0){ - // カウンター値の余りがなければ、所定の時間になったので出力 - // 加速度3軸の値を出力 pc.printf("%+4.3f,%+4.3f,%+4.3f\r\n", acc.x, acc.y, acc.z); } - // breakを入れないと下記の処理も行われてしまうので必須 break; - // IMU(クオータニオン)を出力するモード case IMU_DATA: if ((n % OUT_IMU_CYC) == 0){ - // カウンター値の余りがなければ、所定の時間になったので出力 - // クオータニオンの4値を出力 pc.printf("%+4.3f,%+4.3f,%+4.3f,%+4.3f\r\n", myQ.w, myQ.x, myQ.y, myQ.z); } break; - // ヨーレイトを出力するモード case GYR_DATA: if ((n % OUT_ACC_CYC) == 0){ - // ACCと同じ周期出力 - // ヨーレイト3軸の値を出力 pc.printf("%+4.3f,%+4.3f,%+4.3f\r\n", gyr.x, gyr.y, gyr.z); } break; - // 地磁気を出力するモード case MAG_DATA: if ((n % OUT_ACC_CYC) == 0){ - // ACCと同じ周期出力 - // 地磁気3軸の値を出力 pc.printf("%+4.3f,%+4.3f,%+4.3f\r\n", mag.x, mag.y, mag.z); } break; - // 上記二つのモード以外では、全データを出力する default: case ALL_DATA: if ((n % OUT_ALL_CYC) == 0){ - // 加速度 pc.printf("//ACC: x=%+4.3f y=%+4.3f z=%+4.3f //", acc.x, acc.y, acc.z); - // ヨーレイト pc.printf("GYR: x=%+3.2f y=%+3.2f z=%+3.2f //", gyr.x, gyr.y, gyr.z); - // 地磁気 pc.printf("MAG: x=%+3.2f y=%+3.2f z=%+3.2f ,", mag.x, mag.y, mag.z); - // カウンター値 pc.printf(" passed %.1f sec\r\n", float(n * LOOP_TIME) / 1000.0f); } break; } - // カウンターをひとつアップする ++n; - // ここまでの処理時間をmS単位で計測する uint32_t passed_time = t.read_ms(); - // もしも、実処理時間がLOOP_TIMEより長ければ(期待値)、必要な時間だけ待機 if (passed_time < (LOOP_TIME -1)){ +#if (MBED_MAJOR_VERSION == 2) wait_ms(LOOP_TIME - t.read_ms()); +#elif (MBED_MAJOR_VERSION == 5) + ThisThread::sleep_for(LOOP_TIME - t.read_ms()); +#endif } } }