XIAO ESP32C3の消費電力低減まとめ

電子工作

スリープ動作まとめ

ESP32C3にはmodem sleep ,light sleep ,deep sleep のスリープ動作が存在します。いずれもWi-Fiやbluetoothといった無線通信を切断し、大幅な消費電力削減を可能にしています。それらの特徴に関してまとめるとともにCPUの駆動周波数の設定、無線電波強度の設定等を備忘録的に説明します。

動作モードに関して

ESP32C3には4つの動作モードがあります。active、modem sleep、light sleep、deep sleepとなっています。

deep sleep mode

deep sleep modeにおけるデータシート上の消費電力は5uAと非常に低消費電力になっています。deep sleep中でもRTCメモリ（8KB）の電源はオン状態のため、変数等を保存することができます。以下にarduinoIDE上でプログラムを書いた際の一例を載せます。

RTC_DATA_ATTR int i = 0;//RTC memory（8KB）

esp_sleep_enable_timer_wakeup(1000*1000*1);// 1 s sleep
esp_deep_sleep_start();//deep sleep start

light sleep mode

CPUを停止/部分的動作、外部割り込みやRTC、MACで起動することも可能なモードです。無線回路も動作させることが可能です。ペリフェラル通信は選択したチャネルで停止することができます。

esp_sleep_enable_timer_wakeup(1000*1000*1);// 1 s sleep
esp_light_sleep_start();//light sleep start

modem sleep mode

cpuは動作していますが、駆動周波数が低くなっているモードです。無線回路は必要な時にオンにするといった設定が可能です。

active mode

全ての機能がオン状態のモードです。

CPUの駆動周波数の設定

CPUの駆動周波数は10〜160MHzで調整することが可能となっています。無線を利用する場合は40MHz以上で動作させる必要があります。複雑な計算や無線を利用しない場合は10MHzでも十分な速度で動作することが可能です。省電力動作を目指す場合は10MHzにすると良いです。

複雑な計算をする場合は動作時間が延びる可能性があります。また、無線通信においても短時間に抑えた方が省電力になることがあるため最高動作周波数である160MHzがいいと考えられます。

setCpuFrequencyMhz(10);// set cpu freq 10MHz

PlatformIOを利用している場合はplatformio.iniファイルに以下の行を加えることでも設定可能です。

[env:seeed_xiao_esp32c3]
platform = espressif32
board = seeed_xiao_esp32c3
framework = arduino
monitor_speed = 115200
board_build.f_cpu = 10000000L //set cpu freq 10MHz

wifi,bluetoothのオンオフ

modem sleep modeでない場合にWi-Fi、Bluetoothをオンオフする設定が可能です。以下にそのコード例をいくつか示します。

#include "esp_bt_main.h"
#include "esp_bt.h"
#include "esp_wifi.h"

esp_bluedroid_disable();//bluetooth stop
esp_bt_controller_disable();
esp_wifi_stop();//wifi stop
#include "esp_bt_main.h"
#include "esp_bt.h"
btStop();//bluetooth stop
#include "WiFi.h"
WiFi.disconnect(true);//wifi stop

wifiの電波強度の最適化

Wi-Fiの電波強度は初期状態で19.5dBmに設定されており、電波強度は強めに設定されています。利用する場所の伝搬係数や通信距離に応じて最適化することで省電力化することが可能となっています。

#include "WiFi.h"
WiFi.setTxPower(WIFI_POWER_19_5dBm);//初期設定
WiFi.setTxPower(WIFI_POWER_15dBm);//set 15 dBm

※一般的なノートPCの電波強度は15dBmと言われています。

Serial通信

シリアル通信も消費電力が増加する要因となるため切断しておいた方がいいとされています。

Serial.end();

動作テスト

各種項目の動作テストを実施してみました。

実験条件

評価ボード：xiao esp32c3
供給電圧：3.4V
供給ピン：5Vピン
開発環境：PlatformIO

1.deep sleep

void setup() {
}
void loop() {
  delay(10000);
  esp_sleep_enable_timer_wakeup(1000*1000*10);// 10 s sleep
  esp_deep_sleep_start();//deep sleep start
}

※cpu周波数：160MHz
active : 17 mA
deep sleep : 290 uA

2.cpu freq

[env:seeed_xiao_esp32c3]
platform = espressif32
board = seeed_xiao_esp32c3
framework = arduino
monitor_speed = 115200
board_build.f_cpu = 10000000L

※platformio.iniにて設定、main.cppは１のまま。
active : 7 mA
deep sleep : 290 uA

3.wifi/bluetoothオフ

#include <WiFi.h>
#include "esp_bt_main.h"
#include "esp_bt.h"
#include "esp_wifi.h"
void setup() {
}
void loop() {
  delay(10000);
  esp_bluedroid_disable();//bluetooth stop
  esp_bt_controller_disable();
  esp_wifi_stop();//wifi stop
  esp_sleep_enable_timer_wakeup(1000*1000*10);// 10 s sleep
  esp_deep_sleep_start();//deep sleep start
}

※cpu周波数：160MHz
active : 17 mA
deep sleep : 290 uA

4.シリアル通信オフ

void setup() {
  Serial.end();
}
void loop() {
  delay(10000);
  esp_sleep_enable_timer_wakeup(1000*1000*10);// 10 s sleep
  esp_deep_sleep_start();//deep sleep start
}

※cpu周波数：160MHz
active : 16.8 mA
deep sleep : 290 uA

5.全のせ

#include <WiFi.h>
#include "esp_bt_main.h"
#include "esp_bt.h"
#include "esp_wifi.h"
void setup() {
  Serial.end();
}
void loop() {
  delay(10000);
  esp_bluedroid_disable();//bluetooth stop
  esp_bt_controller_disable();
  esp_wifi_stop();//wifi stop
  esp_sleep_enable_timer_wakeup(1000*1000*10);// 10 s sleep
  esp_deep_sleep_start();//deep sleep start
}

※cpu周波数：10MHz
active : 6.4 mA
deep sleep : 290 uA

6.全のせ+回路見直し

評価ボード：xiao esp32c3
供給電圧：3.35V
供給ピン：3.3Vピン
開発環境：PlatformIO
active : 6.4 mA
deep sleep : 50 uA

nuitkaの使い方：軽量なexeファイルの作成手順

プログラミング

初めに

pythonをexeファイルにしたいと思ったので 軽量なexeファイルを作れる「nuitka」の使い方を説明します。あくまで、私がうまくいったやり方を備忘録的にまとめています。

仮想環境の構築

pythonの仮想環境を作ってない方は「pipenv」をコマンドプロンプトなりpowershellなりでインストールしてください。

pip install pipenv

cdコマンドで任意のディレクトリに移動した後に仮想環境を構築します。
pythonのバージョンを指定しています。

pipenv --python 3.8

次に必要なモジュールをインストールします。

pipenv install numpy

モジュールのバージョンを指定する場合は以下のようにしてください。

pipenv install numpy == ○.○.○○

MEMO

  • 仮想環境のアドレス
pipenv --venv
  • 仮想環境の削除
pipenv --rm

「nuitka」の導入

「nuitka」「zstandard」モジュールをインストールします。複数のモジュールを同時にインストール可能です。

pipenv install nuitka zstandard

仮想環境の有効化

以下のコマンドで「nuitka」を実行する前に仮想環境を有効化します。

pipenv shell

仮想環境を終了する場合は以下のコマンドを利用します。

exit

「nuitka」の実行

「nuitka」を以下のコマンドで実行します。

nuitka --mingw64 --follow-imports --onefile ○○.py

  • 「--mingw64」はpythonが64bitである必要があります。

  • 「--onefile」は単一の実行ファイル化するオプションです。zstandardモジュールによってコードを圧縮します。

C言語コンパイラがない場合は実行中にインストールするか聞かれます。 そのままインストールしてください。

Nuitka-Scons:INFO: Backend linking program ( no progress information available)

実行中に上のような表示で止まります。辛抱強く待ってください。
「Pyinstaller」で60MB程度のプログラムだと20min程度かかります。

MEMO

  • GUIなどを使っている場合にコンソールの表示を消すオプション
--windows-disable-console
--plugin-enable=○○○

EXEファイルの実行

無事に起動出来ればいいですが、うまくいかないことが多いです。エラー文の確認をした後に、問題がありそうなモジュールのバージョンを下げたり、プラグインを有効にしましょう。ちなみにnumpyはプラグインを有効にしたらうまくいきました。実行できないモジュールがある場合にはそれだけ省く事も可能です。

  • モジュールを省くオプション
--nofollow-import-to=○○○

参考にしたサイト

blog.tsukumijima.net

vucavucalife.com

qiita.com

qiita.com

Pythonによるデータ操作と実行ファイルの作製

プログラミング

初めに

卒業研究でよく散布図を用いるのですが、散布図の積分って面倒なんです。プロット数が多いので尚更そうなんですが骨が折れるので、今回は散布図で台形積分するプログラムをpythonを使って書きました。

調べても意外と載ってないのでここにメモしておきます。 プログラム全体の説明はせず、部分的に説明していきます。 ついでにpythonでデータ整理をしたいときに便利な機能も掻い摘んで説明していきます。

※今回ご紹介する以外にも今までにpythonを用いたプログラムはいくつか書いてきましたが、体系的に勉強したことがない筆者は蛇足なプログラムが多々あると思うのでご理解ください。アドバイスを頂けるとありがたいです。

積分するプロットについて

今回、積分しようとしているのはヒステリシスループで囲まれた面積です。ヒステリシス損失と言われるもので、変圧器などで使われる軟磁性体でよく議論されているような気がします。

積分の方法は省略して、以降プログラムの各種機能・使い方を説明します。

テキストファイルを読み込む

with open(name,'r') as f :
    source = f.read()

  • open関数内にてファイル名を「name」で指定してmodeは読み取り専用モードの「r」としている。

  • 「with」を使うことでファイルを「close」する必要がない。また、「as f」とすれば「f」で指定したファイルをプログラム内で呼び出せる。

  • 「f.read()」はstr型で読み込んでsourceに格納している。

文字・数字を成形する

source = f.read().split()
  • 先ほど格納したsourceを「.split()」を使うことでスペース・改行・タブで区切ってlist型にできる。
source = f.read().split(",",2)
  • 第一引数に指定した文字で区切ったり第二引数で最大分割回数を設定できたりする。

２つ以上の指定文字で区切りたい場合

「reライブラリ」を使用する方法。詳しくは省略するがこれを利用すると特定の文字で自由自在に区切ったり省いたりできる。下に「+-。」で区切る例を載せている。

re.split('[+-。]',source)

リストを結合して文字列に戻す方法

「join」を利用。下にlist型の「source」を各要素の間に「|」(縦棒)を入れて結合した後、文字列を出力する例を載せている。

'|'.join(source)

リスト型の簡単な成形方法

データの取り出しと結合

source_y = []
source_y.extend(source[10::3])
  • 空のlistであるsource_yに「.extend」でlistの後ろから要素を追加していく。演算子「+」でも同じことができる。
  • 追加する際に「source[10::3]」とすることでlist型の「source」の10個目の要素から3つ飛ばしで最後まで追加するように指定している。
source = [0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22]
source_y.extend(source[9:21:3]) 

>>> source_y = [9,12,15,18]
  • 「source[始まるindex:終わるindex:飛ばす要素数]」となっている。ちなみに上のプログラムでもわかるように「9以上21未満」となるので注意。

データの演算

source_y_2= [0]+source_y                  
source_y = source_y + [0]
  • 「.extend」と同様に使える。今回は積分をするので「[0]」をlistに加えることで意図的にindexをずらして後々の計算が簡単にできるようにしている。
  • 「[0]」の演算を「source_y」の前に加えるとそれに続く全ての要素のindexが+1ずれる。後に加えるとlistの最後に加えられることになるためindexは変化せず、要素の数を増やすことができる。
  • ２つのlist「source_y」と「source_y_2」の要素数が合わないと後々の計算ができないため両方に「[0]」を加えている。

numpyを利用する

numpyはpythonの行列計算ライブラリ。今回は積分する際にlistの要素ごとに計算が必要になるため利用した。numpyを利用することで要素ごとの四則演算を簡単に行うことができる。

listからndarrayへの変換方法

import numpy as np
arr_y1= np.array(source_y,dtype='float32') 
arr_y2= np.array(source_y_2,dtype='float32')
  • 「np.array」を用いてlist型「source_y」をnumpyで利用できる「ndarray」型へ変換している。
  • 「dtype='float32'」でfloat型のデータとして変換するように指定している。
  • 整数のint型や桁数を増やしたfloat64なども指定できる。

ndarray内で最大値・最小値のindexを得る

index_y1 = np.argmax(arr_y1)
index_y2 = np.argmin(arr_y1)
  • 「index_y1」にndarray内の最大値のindex、「index_y2」にndarray内の最小値のindexを代入している。

ndarray内で連続データの符号が反転するindexを得る

mult = np.sign(np.multiply(arr_y1,arr_y2))
hanten =np.where(mult<0)
print(hanten)

>>> array([132, 225], dtype=int64)

先に用意しておいた積分用にindexを１つずらしたlistと元のlistを掛け合わせることで符号が反転するindexを特定する。

  • 「np.multiply(arr_y1,arr_y2)」で２つのndarrayの要素ごとの積をとっている。
  • 「np.sign」で符号のみに着目してマイナス値を「-1」、0を「0」、プラス値を「1」としたndarray「mult」を生成する。
  • 「np.where」でmultが0より小さくなるindexをすべて得る。

出力結果を見るとわかるがこのやり方だと計算用にindexの数字だけを取り出すのが難しい。そこでこの値はプロットに異常がないかの確認用とすることにした。

index_y =np.argmin(mult)

先ほど紹介した最小値のindexを得るプログラムを利用した。
しかし、このようにすると最初の「-1」のindexしか得られない。

index_y =np.argmin(mult)
mult = mult[index_y+1:]
index_y2 = np.argmin(mult)+index_y+1

そこで、確認済みのindexまでをすべて削ったndarrayを新たに作ることで次の「-1」のindexを無理やり得られるようにした。
（これを応用してfor文でndarrayを削っていって全ての符号反転indexを得ることができるプログラムが書けそうですが今回は２つだけで十分なので書いてません。）

numpyで絶対値と四則演算

arr_y1 = np.abs(arr_y1)

「np.abs」でarr_y1の絶対値をとる。

com_y = np.add(arr_y2,arr_y1)
dis_y = np.subtract(arr_y2,arr_y1) 
mul_yy = np.multiply(com_y,dis_y)
div_yy = np.divide(com_y,dis_y)
delta = np.abs(mul_yy/2)

今回の積分は台形積分のため割り算を除いて要素ごとに四則演算が必要となる。

  • 「np.add」足し算
  • 「np.subtract」引き算
  • 「np.multiply」掛け算
  • 「np.divide」割り算

最後の「mul_yy/2」は全ての要素を2で割っている。

ndarrayの一部データ削除と合計

delta = np.delete(delta,0,0)
delta = np.delete(delta,delta.size-1,0)

行列である微小面積「delta」は最初と最後だけ「0」を足した影響で値が大きくなるので省く。

  • 「np.delete」を利用する。
  • 「np.delete(行列名,行番号,列番号)」で指定。行番号と列番号は0スタートなので注意。
  • 「delta.size」でdeltaの要素の数を取得し0スタートを考慮して「delta.size-1」すると最後の要素を消せる。
summ = delta[0:100].sum()
  • 「.sum()」で行列の合計をy1に代入している。
  • 「delta[0:100]」とすることで0から100番目の要素の合計を出せる。

テキストファイルを出力する

 with open("面積.txt",'a') as f_out :
    f_out.writelines("\n"+"面積：" +str(summ) +"\n")
    f_out.close()

open関数内にてファイル名を「面積.txt」としてmodeは書き込みモードの「a」としている。ここで書き込みモードに関して説明する。

  • 「a」：ファイルがない場合は新規作成。ある場合は続きに書き足される。

  • 「w」：ファイルがない場合は新規作成。ある場合は上書きされる。

  • 「x」：ファイルがない場合は新規作成。ある場合はエラー。

最後に「f_out.close()」でファイルを閉じている。必要ないかもしれない・・・。

ループ処理・待機・例外処理

try:
    while True:

        print("ENTERキーを押すと繰り返し計算できます。")
        key = input("半角SPACEを入力すると終了します。")
        if key == ' ':
            break

except:
    print("error")

  • 「try」と「except」で例外処理を行う。例外の場合は「error」と表示される。

  • 「while True:」でループ処理が可能。

  • 「input」を利用することで待機ができる。

  • 「input」で特定の入力を読み込み、プログラムを終了させることができる。

実行ファイル「.exe」に変換

pip install pyinstaller

「pyinstaller」を利用する。ライブラリを入れていない人はpipでインストール。

pyinstaller hello.py --onefile

コマンドプロンプトなどで階層を合わせて上記のコマンドを実行すると実行ファイルがdistフォルダの中にできる。 ファイルが多数生成されるため、実行ファイルを作る前にプログラム本体をフォルダの中に入れておくのがおすすめ。

型の確認方法

source = []
print(type(source))

>>> list

型のエラーはよく出るのでこれで確認するのがおすすめ。

終わりに

ここまで読んでいただきありがとうございます。メモ書きということで、最初から最後まで分かりにくく見にくい内容になっています。掻い摘んで色々と説明しているためまとまりもないのですがご了承ください。また、今後も修正を重ねていく予定です。

９軸センサ（BMX055）の基礎とMadgwickフィルターの使い方

電子工作

9軸センサとは

9軸センサとは加速度・角速度・地磁気を計測するセンサです。
それぞれの値を計測する独立したセンサも存在しています。

  • 加速度センサ
  • ジャイロセンサ
  • 地磁気センサ

各々のセンサでXYZ３軸分の数値が出力されます。つまり、９軸センサでは「３つのセンサ」×「３つの軸」で９つの数値が出力されるということになります。

ちなみに9軸は「9DoF」(9 degree of freedom)と言われることもあります。

９軸センサの特徴的な機能は姿勢推定になります。ロボットや人間などの姿勢推定を行うことができ、フィードバック制御やモーションキャプチャに利用されています。 続いて、それぞれのセンサについて説明していきます。

加速度センサ

加速度センサは加速度を読み取るセンサになります。 このセンサは、センサ上の座標系で重力加速度のXYZ軸それぞれの成分が分かります。

よって、重力加速度の方向が測定できます。

POINT!
センサのプログラムがうまくできていれば、センサを地面に置いたときに、地面に対して垂直な方向の軸の値は9.8付近になると思います。これは高校物理でも出てくる重力加速度の値です。

重力以外にもセンサ搭載物の動きに対して加速度を得ることができます。
しかし、このとき重力加速度の方向が一時的にブレてしまうことになります。

つまり、この加速度センサだけだと静止した物体の姿勢推定しかできません。

ジャイロセンサ

ジャイロセンサは物体の角速度を読み取れます。

先ほど紹介した加速度センサとあわせて６軸センサとして利用されています。
この６軸センサのことをIMU*1といいます。（9軸はAHRS*2といいます）

理論上は加速度センサとジャイロセンサを合わせたIMUで姿勢推定が可能です。 しかし、センサにはドリフトとオフセットという誤差が生じます。
また、ヨー方向の姿勢推定はどこかを基準とした相対姿勢しか分かりません。

いかにこの誤差を取り除くか、ヨー方向の姿勢を推定するかが問題です。
そこで、次に説明する地磁気センサの出番になります。

地磁気センサ

地磁気だけでなく磁気があるものを計測できるセンサです。地磁気という常に一定方向に生じているベクトルを計測できるのでIMUに補うことでより精度のいい値をとれます。

弱点としては周囲に磁気を帯びたものがあると正確な値が取れません。

座標系

ここで座標系について捕捉の説明をします。 XYZ軸のそれぞれの方向に沿って右ねじの向きで Roll(ロール)Pitch(ピッチ)Yaw(ヨー)になります。角速度やフィルタを扱う時に使います。

座標系の説明

オフセットとドリフト

オフセット

オフセットとは零点誤差とも言われます。 例えばジャイロセンサの計測対象である物体が全く回転していない場合を考えます。 この場合、計測値は(0,0,0)をとるのが理想ですが、実際は(1,1,0)など少し誤差がでることがあります。 回転してない、つまり零点での誤差がオフセットと言われます。

ドリフト

ドリフトとは温度変化などによって変化する誤差のことです。 センサによってその特徴が異なるので考慮するのが難しい誤差になります。

オフセットやドリフトはセンサの種類やセンサの使用環境で異なってきます。 センサのデータシートに許容誤差が記載されている場合が多いです。

ここまで９軸センサの特徴について紹介してきました。ここからは９軸センサの使い方を紹介していきます。先に紹介した通り、オフセットとドリフトによる誤差を考慮しなければ使い物になりません。そのため９軸センサの誤差を取り除く必要があります。

そこで、プログラムで実装するフィルタを使って誤差を取り除きます。

フィルタ

９軸センサに使われる主なフィルタは私が知っている限りでは以下の通りです。

  • カルマンフィルター
  • 相補フィルター
  • Madgwickフィルター（相補フィルターの1種）

この3つが主に使われているようです。
本記事ではこの中でも高速処理が可能だと言われている「Madgwickフィルター」を紹介します。

Madgwickフィルター

このフィルターは前述したように高速処理が可能なためマイコンでも精度が良い補正ができます。クォータニオンという演算子を用いて計算しているようです。

ArduinoIDEにてMadgwickライブラリがあるのでそちらの使い方を紹介します。
まずは、ライブラリの入れ方です。

ライブラリ導入

ArduinoIDEのツールからライブラリを管理を選択しましょう。

ライブラリを管理を選択

すると、このような画面が現れると思います。

ライブラリ検索画面

この画面の右上の検索窓に「madgwick」と入力してください。

おそらく一番上に「Madgwickライブラリ」があるのでインストールしましょう。 これでライブラリのインスト―ルは終了です。 続いて、このライブラリの使い方を紹介していきます。

madgwickフィルタの使い方

今回、利用している９軸センサはBMX055です。一部のプログラムは秋月電子のサイトから引用しています。まずはライブラリの読み込みとBMX055のI2Cアドレスの設定です。

#include<MadgwickAHRS.h>
#include<Wire.h>

// BMX055　加速度センサのI2Cアドレス
#define Addr_Accl 0x19  // (JP1,JP2,JP3 = Openの時)
// BMX055　ジャイロセンサのI2Cアドレス
#define Addr_Gyro 0x69  // (JP1,JP2,JP3 = Openの時)
// BMX055　磁気センサのI2Cアドレス
#define Addr_Mag 0x13   // (JP1,JP2,JP3 = Openの時)

次に、フィルタをインスタンス化します。

Madgwick MadgwickFilter;

そして、setup関数内でBMX055とI2C通信を行う設定とフィルタのサンプリング周波数とゲインの設定をします。

void setup()
{
  Wire.begin(I2C接続するピン番号, I2C接続するピン番号);
  BMX055_Init();
  MadgwickFilter.begin(100);
  MadgwickFilter.setGain(0.1);
  delay(100);
}

今回は、サンプリング周波数が100[Hz]になっています。フィルタのゲインはデフォルトが「0.1」になっており、「1.0」に近づけるとノイズが大きくなる代わりに収束が早くなります。次に、loop関数内のプログラムの説明です。

void loop() {

  BMX055_Gyro();
  BMX055_Accl();
  BMX055_Mag();

  MadgwickFilter.update(xGyro,yGyro,zGyro,xAccl,yAccl,zAccl,xMag,yMag,zMag);

  ROLL = MadgwickFilter.getRoll();
  PITCH = MadgwickFilter.getPitch();
  YAW = MadgwickFilter.getYaw();

  delay(1000);
}

loop関数内ではセンサの計測値を更新してフィルタを通した後に推定されるROLL、PITCH、YAWの値を計算しています。

MadgwickFilter.update(xGyro,yGyro,zGyro,xAccl,yAccl,zAccl,xMag,yMag,zMag);

これはセンサの計測値をフィルタに通す部分です。 Gyroは「ジャイロ」、Acclは「加速度」、Magは「地磁気」センサから得られた計測値です。

MadgwickFilter.updateIMU(xGyro,  yGyro,  zGyro,  xAccl,  yAccl,  zAccl);

６軸で利用する場合は上記のようになります。

  ROLL = MadgwickFilter.getRoll();
  PITCH = MadgwickFilter.getPitch();
  YAW = MadgwickFilter.getYaw();

上記のプログラムでROLL、PITCH、YAW角を得ることができます。 なお、出力される値はdegreeになります。

以上で９軸センサの使い方の紹介になります。 以下にフィルタで得られた値を表示するプログラムを載せておきます。（BMX055の関数は秋月より引用したプログラムになります。）

（2024/10更新）

参考

#include <Arduino.h>
#include <Wire.h>
#include<MadgwickAHRS.h>

// BMX055　加速度センサのI2Cアドレス
#define Addr_Accl 0x19  // (JP1,JP2,JP3 = Openの時)
// BMX055　ジャイロセンサのI2Cアドレス
#define Addr_Gyro 0x69  // (JP1,JP2,JP3 = Openの時)
// BMX055　磁気センサのI2Cアドレス
#define Addr_Mag 0x13   // (JP1,JP2,JP3 = Openの時)

// センサーの値を保存するグローバル関数
float xAccl = 0.00;
float yAccl = 0.00;
float zAccl = 0.00;
float xGyro = 0.00;
float yGyro = 0.00;
float zGyro = 0.00;
float xMag  = 0.00;
float yMag  = 0.00;
float zMag  = 0.00;

int ROLL;
int PITCH;
int YAW;

Madgwick MadgwickFilter;

void setup() {
  Serial.begin(115200);

  Wire.begin(I2Cピン番号,I2Cピン番号);
  BMX055_Init();
  MadgwickFilter.begin(100);
  MadgwickFilter.setGain(0.1);

  delay(100);
}
void loop() {


  BMX055_Gyro();
  BMX055_Accl();
  BMX055_Mag();

  // put your main code here, to run repeatedly:
  MadgwickFilter.update(xGyro,yGyro,zGyro,xAccl,yAccl,zAccl,xMag,yMag,zMag);

  ROLL = MadgwickFilter.getRoll();
  PITCH = MadgwickFilter.getPitch();
  YAW = MadgwickFilter.getYaw();

  Serial.print(ROLL);Serial.print(",");
  Serial.print(PITCH);Serial.print(",");
  Serial.println(YAW);

  delay(1000);
}

void BMX055_Init()
{
  //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Accl);
  Wire.write(0x0F); // Select PMU_Range register
  Wire.write(0x03);   // Range = +/- 2g
  Wire.endTransmission();
  delay(100);
  //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Accl);
  Wire.write(0x10);  // Select PMU_BW register
  Wire.write(0x08);  // Bandwidth = 7.81 Hz
  Wire.endTransmission();
  delay(100);
  //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Accl);
  Wire.write(0x11);  // Select PMU_LPW register
  Wire.write(0x00);  // Normal mode, Sleep duration = 0.5ms
  Wire.endTransmission();
  delay(100);
  //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Gyro);
  Wire.write(0x0F);  // Select Range register
  Wire.write(0x04);  // Full scale = +/- 125 degree/s
  Wire.endTransmission();
  delay(100);
  //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Gyro);
  Wire.write(0x10);  // Select Bandwidth register
  Wire.write(0x07);  // ODR = 100 Hz
  Wire.endTransmission();
  delay(100);
  //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Gyro);
  Wire.write(0x11);  // Select LPM1 register
  Wire.write(0x00);  // Normal mode, Sleep duration = 2ms
  Wire.endTransmission();
  delay(100);
  //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Mag);
  Wire.write(0x4B);  // Select Mag register
  Wire.write(0x83);  // Soft reset
  Wire.endTransmission();
  delay(100);
  //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Mag);
  Wire.write(0x4B);  // Select Mag register
  Wire.write(0x01);  // Soft reset
  Wire.endTransmission();
  delay(100);
  //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Mag);
  Wire.write(0x4C);  // Select Mag register
  Wire.write(0x00);  // Normal Mode, ODR = 10 Hz
  Wire.endTransmission();
  //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Mag);
  Wire.write(0x4E);  // Select Mag register
  Wire.write(0x84);  // X, Y, Z-Axis enabled
  Wire.endTransmission();
  //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Mag);
  Wire.write(0x51);  // Select Mag register
  Wire.write(0x04);  // No. of Repetitions for X-Y Axis = 9
  Wire.endTransmission();
  //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Mag);
  Wire.write(0x52);  // Select Mag register
  Wire.write(0x16);  // No. of Repetitions for Z-Axis = 15
  Wire.endTransmission();
}
//=====================================================================================//
void BMX055_Accl()
{
  int data[6];
  for (int i = 0; i < 6; i++)
  {
    Wire.beginTransmission(Addr_Accl);
    Wire.write((2 + i));// Select data register
    Wire.endTransmission();
    Wire.requestFrom(Addr_Accl, 1);// Request 1 byte of data
    // Read 6 bytes of data
    // xAccl lsb, xAccl msb, yAccl lsb, yAccl msb, zAccl lsb, zAccl msb
    if (Wire.available() == 1)
      data[i] = Wire.read();
  }
  // Convert the data to 12-bits
  xAccl = ((data[1] * 256) + (data[0] & 0xF0)) / 16;
  if (xAccl > 2047)  xAccl -= 4096;
  yAccl = ((data[3] * 256) + (data[2] & 0xF0)) / 16;
  if (yAccl > 2047)  yAccl -= 4096;
  zAccl = ((data[5] * 256) + (data[4] & 0xF0)) / 16;
  if (zAccl > 2047)  zAccl -= 4096;
  xAccl = xAccl * 0.0098; // renge +-2g
  yAccl = yAccl * 0.0098; // renge +-2g
  zAccl = zAccl * 0.0098; // renge +-2g
}
//=====================================================================================//
void BMX055_Gyro()
{
  int data[6];
  for (int i = 0; i < 6; i++)
  {
    Wire.beginTransmission(Addr_Gyro);
    Wire.write((2 + i));    // Select data register
    Wire.endTransmission();
    Wire.requestFrom(Addr_Gyro, 1);    // Request 1 byte of data
    // Read 6 bytes of data
    // xGyro lsb, xGyro msb, yGyro lsb, yGyro msb, zGyro lsb, zGyro msb
    if (Wire.available() == 1)
      data[i] = Wire.read();
  }
  // Convert the data
  xGyro = (data[1] * 256) + data[0];
  if (xGyro > 32767)  xGyro -= 65536;
  yGyro = (data[3] * 256) + data[2];
  if (yGyro > 32767)  yGyro -= 65536;
  zGyro = (data[5] * 256) + data[4];
  if (zGyro > 32767)  zGyro -= 65536;
  xGyro = xGyro /131; //  Full scale = +/- 250 degree/s
  yGyro = yGyro /131; //  Full scale = +/- 250 degree/s
  zGyro = zGyro /131; //  Full scale = +/- 250 degree/s

  /*
  xGyro = xGyro * 0.0038; //  Full scale = +/- 125 degree/s
  yGyro = yGyro * 0.0038; //  Full scale = +/- 125 degree/s
  zGyro = zGyro * 0.0038; //  Full scale = +/- 125 degree/s
  */
}
//=====================================================================================//
void BMX055_Mag()
{
  int data[8];
  for (int i = 0; i < 8; i++)
  {
    Wire.beginTransmission(Addr_Mag);
    Wire.write((0x42 + i));    // Select data register
    Wire.endTransmission();
    Wire.requestFrom(Addr_Mag, 1);    // Request 1 byte of data
    // Read 6 bytes of data
    // xMag lsb, xMag msb, yMag lsb, yMag msb, zMag lsb, zMag msb
    if (Wire.available() == 1)
      data[i] = Wire.read();
  }
  // Convert the data
  xMag = ((data[1] << 5) | (data[0] >> 3));
  if (xMag > 4095)  xMag -= 8192;
  yMag = ((data[3] << 5) | (data[2] >> 3));
  if (yMag > 4095)  yMag -= 8192;
  zMag = ((data[5] << 7) | (data[4] >> 1));
  if (zMag > 16383)  zMag -= 32768;
}

ランキング参加中
マイコン電子工作

*1:アイエムユー：慣性計測装置「Inertial Measurement Unit」

*2:エーハース：姿勢方位基準装置「Attitude Heading Reference System」