Kenji Arai
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が１で、IMU_DATAが２となる(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){
- // カウンター値の余りがなければ、所定の時間になったので出力
- // 加速度３軸の値を出力
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){
- // カウンター値の余りがなければ、所定の時間になったので出力
- // クオータニオンの４値を出力
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と同じ周期出力
- // ヨーレイト３軸の値を出力
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と同じ周期出力
- // 地磁気３軸の値を出力
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
}
}
}