Revision 1:a0ca9f62e81f, committed 2020-02-07

Comitter:

kenjiArai

Date:

Fri Feb 07 04:28:46 2020 +0000

Parent:

0:141e4576190a

Commit message:

BMX055 data by Madgwick Filter sends to PC and shows it by Python program

Changed in this revision

diff -r 141e4576190a -r a0ca9f62e81f Python_PC_program/check_ser_port.py
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Python_PC_program/check_ser_port.py	Fri Feb 07 04:28:46 2020 +0000
@@ -0,0 +1,34 @@
+"""
+Created on      Feb. 7th, 2020
+@author: Kenji Arai
+
+Refrence
+    https://qiita.com/Acqua_Alta/items/9f19afddc6db1e4d4286
+    https://debugging-code.com/2018/11/10/77/
+    https://qiita.com/usop4/items/9abfa1a4b6bf66a5f8fa
+"""
+
+import serial
+import serial.tools.list_ports
+
+def connect_dtlggr():
+    coms = serial.tools.list_ports.comports()
+    comlist=[]
+    n=0
+    for com in coms:
+        comlist.append(com.device)
+        n += 1
+    for i in range(n):
+        try:
+            s = serial.Serial(comlist[i],115200,timeout=3)
+        except:
+            continue
+
+        sData = s.readline()
+        if not sData:
+            s.close()
+        else:
+            s.close()
+            return comlist[i]
+
+    return None

diff -r 141e4576190a -r a0ca9f62e81f Python_PC_program/quaternion_BMX055.py
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Python_PC_program/quaternion_BMX055.py	Fri Feb 07 04:28:46 2020 +0000
@@ -0,0 +1,203 @@
+"""
+Created on      Feb. 7th, 2020
+@author: Kenji Arai
+
+Refrence
+    http://kieranwynn.github.io/pyquaternion/
+
+    https://kumadasu.com/series/quaternion-for-embedded/
+
+    https://wgld.org/d/webgl/w031.html
+    http://marupeke296.com/DXG_No10_Quaternion.html
+    http://edom18.hateblo.jp/entry/2018/06/25/084023
+"""
+
+from pyquaternion import Quaternion
+from time import sleep
+import serial
+import serial.tools.list_ports
+import tkinter as tk
+from tkinter import *
+from tkinter import messagebox
+import numpy as np
+from matplotlib import pyplot as plt
+from mpl_toolkits.mplot3d import Axes3D
+from enum import Enum
+
+from check_ser_port import connect_dtlggr
+
+D_LBL_MODE_IMU = 'Operating Mode:   Quaternion '
+D_BTN_MODE_IMU = ' IMU / Quarternion mode '
+D_LBL_MODE_ACC = 'Operating Mode:   Accelerometer '
+D_BTN_MODE_ACC = ' ACC / Accelerometer mode '
+D_SPACE = '         '
+D_LETTER = 'UTF-8'
+
+class mode(Enum):
+    ACC = 1
+    IMU = 2
+    ALL = 3
+    RST = 4
+
+class op_mode:
+    opeartion_mode = mode.IMU
+
+    def set_mode(ser,mode):
+        if mode == mode.IMU:
+            op_mode.opeartion_mode = mode.IMU
+            ser.write(bytes('2',D_LETTER))
+            sleep(0.02)
+            ser.write(bytes('2',D_LETTER))
+        elif mode == mode.ACC:
+            op_mode.opeartion_mode = mode.ACC
+            ser.write(bytes('1',D_LETTER))
+            sleep(0.02)
+            ser.write(bytes('1',D_LETTER))
+        else:
+            ser.write(bytes('R',D_LETTER))
+            sleep(0.02)
+            ser.write(bytes('R',D_LETTER))
+
+    def get_mode():
+        return op_mode.opeartion_mode
+
+def chart_initialize(root):
+    root.title("BMX055 Quarternion")
+    root.resizable(False, False)
+    frame0 = tk.Frame(root)
+    frame1 = tk.Frame(root)
+    text_widget0 = tk.Text(frame0,height=1,width=35,fg='darkblue',bg='gray94',
+                           relief='flat',font=('Arial', '12'))
+    text_widget0.grid(row=0,column=0)
+
+    def acc_action():
+        op_mode.set_mode(ser,mode.ACC)
+        text_widget0.delete('1.0','end')
+        text_widget0.insert('end',D_SPACE)
+        text_widget0.insert('end',D_LBL_MODE_ACC)
+
+    def imu_action():
+        op_mode.set_mode(ser,mode.IMU)
+        text_widget0.delete('1.0','end')
+        text_widget0.insert('end',D_SPACE)
+        text_widget0.insert('end',D_LBL_MODE_IMU)
+
+    def reset_action():
+        op_mode.set_mode(ser,mode.RST)
+        text_widget0.delete('1.0','end')
+        text_widget0.insert('end',D_SPACE)
+        text_widget0.insert('end',D_LBL_MODE_IMU)
+
+    def quit_action():
+        root.destroy()
+
+    button0 = tk.Button(frame1,text=D_BTN_MODE_ACC,command=acc_action)
+    button0.pack(anchor=tk.S,fill=tk.BOTH)
+    button1 = tk.Button(frame1,text=D_BTN_MODE_IMU,command=imu_action)
+    button1.pack(anchor=tk.S,fill=tk.BOTH)
+    button2 = tk.Button(frame1,text='RESET',command=reset_action)
+    button2.pack(anchor=tk.S,fill=tk.BOTH)
+    button3 = tk.Button(frame1,text='EXIT',command=quit_action)
+    button3.pack(anchor=tk.S,fill=tk.BOTH)
+    frame0.pack(fill='both')
+    frame1.pack(fill='both')
+
+    text_widget0.delete('1.0','end')
+    text_widget0.insert('end',D_SPACE)
+    text_widget0.insert('end',D_LBL_MODE_IMU)
+    
+
+def graph_initialize():
+    global fig, lines, startpoints, endpoints
+
+    fig = plt.figure(figsize=(8,6))
+    ax = fig.add_axes([0, 0, 1, 1], projection='3d')
+    ax.set_xlabel('X(Red)',size=14, color='red')
+    ax.set_ylabel('Y(Green)',size=14, color='green')
+    ax.set_zlabel('Z(Blue)',size=14, color='blue')
+    colors = ['red', 'green', 'blue']
+    lines = sum([ax.plot([], [], linewidth = 5, c=c) for c in colors], [])
+    startpoints = np.array([[0, 0, 0], [0, 0, 0], [0, 0, 0]])
+    endpoints = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]])
+    ax.set_xticks([-1.0, -0.5, 0.0, 0.5, 1.0])
+    ax.set_yticks([-1.0, -0.5, 0.0, 0.5, 1.0])
+    ax.set_zticks([-1.0, -0.5, 0.0, 0.5, 1.0])
+    ax.set_xlim((-1, 1))
+    ax.set_ylim((-1, 1))
+    ax.set_zlim((-1, 1))
+    ax.quiver(-1,0,0,1,0,0,length=2,arrow_length_ratio=0,
+              linestyles='dashed',color='red')
+    ax.quiver(0,-1,0,0,1,0,length=2,arrow_length_ratio=0,
+              linestyles='dashed',color='green')
+    ax.quiver(0,0,-1,0,0,1,length=2,arrow_length_ratio=0,
+              linestyles='dashed',color='blue')
+    ax.view_init(30, 10)
+
+def graph_update(serData,mode):
+    if  mode == mode.ACC:
+        n = 0
+        for line, start, end in zip(lines, startpoints, endpoints):
+            start = [0.,0.,0.]
+            end =[0., 0., 0.]
+            end[n] = float(serData[n])
+            n += 1
+            line.set_data([start[0], end[0]], [start[1], end[1]])
+            line.set_3d_properties([start[2], end[2]])
+    elif mode == mode.IMU:
+        q = Quaternion(serData)
+        for line, start, end in zip(lines, startpoints, endpoints):
+            start = q.rotate(start)
+            end = q.rotate(end)
+            line.set_data([start[0], end[0]], [start[1], end[1]])
+            line.set_3d_properties([start[2], end[2]])
+
+    fig.canvas.draw()
+
+def show_BMX055_serial():
+
+    def every_second():
+        serData = ser.readline().strip().split(b',')
+        dt = len(serData)
+        #print(serData)
+        mode = op_mode.get_mode()
+        if  mode == mode.ACC and dt != 3:
+            op_mode.set_mode(ser,mode.ACC)
+            print('ERROR @ read ser read')
+        elif  mode == mode.IMU and dt != 4:
+            op_mode.set_mode(ser,mode.IMU)
+            print('ERROR @ read ser read')
+        elif mode == mode.ACC or mode == mode.IMU:
+            graph_update(serData,mode)
+        else:
+            print(serData)
+
+        root.after(1, every_second)
+
+    root = tk.Tk()
+    com_port=connect_dtlggr()
+    if com_port == None:
+        print("Serial port is not found!")
+        root.withdraw()
+        ready = messagebox.showwarning('showwaning', 'USB is not ready!!')
+        print(ready)
+        root.destroy()
+        return
+
+    global ser
+    print('checking serial')
+    #ser = serial.Serial(com_port,921600,timeout=None)
+    ser = serial.Serial(com_port,115200,timeout=None)
+    op_mode.set_mode(ser,mode.IMU)
+    chart_initialize(root)
+    graph_initialize()
+    every_second()
+    root.mainloop()
+
+    print('Push EXIT button')
+    ser.close()
+    plt.clf()
+    plt.close()
+
+if __name__ == '__main__':
+    show_BMX055_serial()
+    print("Exit")

diff -r 141e4576190a -r a0ca9f62e81f check_revision.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/check_revision.cpp	Fri Feb 07 04:28:46 2020 +0000
@@ -0,0 +1,30 @@
+/*
+ * Check Mbed revision
+ *
+ * Copyright (c) 2020 Kenji Arai / JH1PJL
+ *  http://www7b.biglobe.ne.jp/~kenjia/
+ *  https://os.mbed.com/users/kenjiArai/
+ *      Revised:    FFeburary  7th, 2020
+ *      Revised:    FFeburary  7th, 2020
+ */
+
+#include "mbed.h"
+
+//    RUN ONLY ON mbed-os6.0.0-alpha1 (430e64fce8)
+//      https://github.com/ARMmbed/mbed-os/releases/tag/mbed-os-6.0.0-alpha1
+//    ?????
+//      not 6.0.0 but 5.14.2 -> is mbed-os6 a fake revision?
+#if (MBED_MAJOR_VERSION == 5) &&\
+    (MBED_MINOR_VERSION == 14) &&\
+    (MBED_PATCH_VERSION == 2)
+#else
+//#warning "Please use mbed-os6.0.0-alpha1"
+#   error "Please use mbed-os6.0.0-alpha1"
+#endif
+
+void print_revision(void)
+{
+    printf("MBED_MAJOR_VERSION = %d\r\n", MBED_MAJOR_VERSION);
+    printf("MBED_MAJOR_VERSION = %d\r\n", MBED_MINOR_VERSION);
+    printf("MBED_MAJOR_VERSION = %d\r\n", MBED_PATCH_VERSION);
+} 
\ No newline at end of file

diff -r 141e4576190a -r a0ca9f62e81f main.cpp
--- 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
         }
     }
 }

diff -r 141e4576190a -r a0ca9f62e81f mbed-os.lib
--- a/mbed-os.lib	Mon Mar 04 04:15:19 2019 +0000
+++ b/mbed-os.lib	Fri Feb 07 04:28:46 2020 +0000
@@ -1,1 +1,1 @@
-https://github.com/ARMmbed/mbed-os/#51d55508e8400b60af467005646c4e2164738d48
+https://github.com/ARMmbed/mbed-os/#430e64fce8098fd5e54f6de32f3f029f35dc705f