下図は、手元に有ったアナログな水準器の写真です。
アルコール等の液体中に気泡が封入されています。気泡には浮力が有るので、重力加速度と反対側に浮上🎈する力が働いています。パッケージの天井部分が円弧状になっているので、気泡は水準器の鉛直上方向を示します。
このデジタル版装置を、M5StickC Plusに搭載されたIMU(加速度センサ)を使って作成してみます。
M5StickC Plusはオールインワンのマイコンボードだし、内蔵のIMUを使うだけなので、ソフトウェアの設計だけで終了します。原理も制御も簡単なので、今回もUIFlowを使って作成してみようと思います。
加速度センサは、センサ本体に掛かる加速度を検出するもの、なのですが、地球の重力に引かれて加速する、重力加速度も検出してしまいます。どういうことか？と言うと、M5StickC Plus(のIMU)を机の上などにそっと置いて、動かさなかったとしても、重力加速度の方向が検出されて読み出されるのです。動いて(加速して)いないのに！
本来は、机からの抗力によって打ち消されてしまう重力加速度が検出されてしまう事で、ちょっと制御が難しくなる場面があるのですが、その議論についてはこちらもご確認ください。
今回の水準器では、重力加速度が観測可能なことで動作が成立するので、悪いことばかりでは無い様です。
M5StickC PlusのIMUを読み出すと、重力加速度の相対的な方向が判るので、M5StickC PlusのTFTディスプレイ面が水平であると仮定した場合と、重力加速度ベクタの成す角度を知ることができれば、M5StickC Plusが水平かどうか知ることができます。
測定した結果をTFTディスプレイに表示すれば終了です。
気泡円の動き方は、アナログな水準器の気泡の動きと同じ方向になる様に調整しておきます。
加速度センサは、MEMS(Micro Electro Mechanical Systems)と言う、LSIチップ内に形成した極小のばねや重りなどの装置で加速度を計測するセンサです。測定値は、すごく繊細なので、読み出す度に微小に値が変化します。
そこで、割り込みを使って一定期間毎に加速度値を読み出し、適切な期間の移動平均値を取って、これを現在の加速度値と見なします。
タイマは以下図のStart periodブロックで生成することができ、指定した時間間隔で、コールバックブロック内のコードが実行されます。コールバックブロック内の処理については、後ほど登場します。コールバックブロック内の処理時間よりもタイマの割り込み周期を短くしてしまうと破綻しますので、適度な周期を保ってください。
上コードの初期設定で、移動平均に使用するリスト(lst_acc)も初期化しています。上図では、値0のデータを10個持ったリストを作りました。これが、移動平均に使用するデータ数となります。上図の設定によって、タイマ割り込みで100ms周期にサンプリングした加速度データから、10区間の移動平均を取っています。
※移動平均の区間を15区間にしたいなら、値0のデータを15個で初期化してください。20区間なら20個です。
前節で解説した通り、一定期間毎に測定した加速度の移動平均を取ってありますので、loop関数内で加速度センサの移動平均値を読み出して、TFTディスプレイ表示を更新すればOKです。
だがしかし、ここで少し落ち着いて考えてみてください。loop関数内で移動平均値にアクセスすることと、割り込み処理は非同期です。移動平均値にアクセスしている、その最中に割り込みが発生してしまうと、割り込み前・後で読み出した移動平均値が不整合を起こしてしまいます。
すなわち、loop関数内での移動平均値読み出しは、割り込みとの競合を回避しながら実行する必要があるのです。
まず、タイマ割り込みのイベントコールバックブロックを見てみましょう。
リストと言うデータ構造を使って、計測した加速度値の移動平均を取っていることは前の節で解説済みです。上図の処理は、リストの最後(last)に最新データを追加(insert at)して、最初(first)のデータを削除(get and remove)しています(つまり、FIFOバッファ)。1増1減なので、リスト内の項目数は不変です。
リスト内の項目数(すなわち移動平均区間)は、初期化の時に作成したリストの項目数(＝10個)で決まっています。
肝となるのは、コールバックブロックを抜ける直前に、変数isr_cntをカウントアップしてあるところです。こうしておくことで、割り込みがかかる度に、変数isr_cntの値が＋1されて行きます。
すると、競合を回避した、loop関数内での移動平均値へのアクセスは、以下図の様な処理となります。
最悪、移動平均値の読み出し中に割り込み処理が実行されてしまった場合、もう一度「やり直し」となりますが、読み出したデータの整合性は保つことが可能です。
本来、正しく計測するためには、装置(M5StickC Plus)を観測したい面に沿わせて、しっかり固定しなくては、なりません。
一方、測定対象面に固定する前の段階や、測定面自体がふらふらと動いてしまっている場合(船の上とか、走行中の車中とか)等、計測値が安定していない状態では、気泡円を大きく描画して、注意を促す様にします。逆に、計測データが安定すると、気泡円を小さく描画する事にします。
このため、測定した加速度値accと成す角度を2種類計測します。
重力加速度と成す角度をTFTディスプレイに表示することで、アナログな水準器と比べて、数値として定量的に評価できるという利点も生じます。M5StickC Plusを使ってデジタル化した甲斐があったという事になるでしょう。
デバッグしていて気が付いたのですが、UIFlowは演算ができない場合等、気に食わないことがあると、ランタイムエラーを発生して止まってしまいます。エラーメッセージだけ表示して、プログラム中のどこで発生したエラーなのか表示してくれないため、その様な状態に陥ってしまった場合のデバッグには手がかかります。
今回は、2ベクタの成す角度を計算するために使ったacosブロック(逆余弦数学関数)で「math domain error」が発生しました。
関数acos (x)の定義域は、ご存知の通り、−1≦x≦1です。ランタイムエラー発生個所は、現在の加速度値accと、前回の加速度値pre_accの内積をxとして使っていました。理論上、accとpre_accの内積xは定義域内(−1≦x≦1)のハズなのです。しかし、浮動小数点数の演算誤差蓄積などで、まれに定義域を微小に超えてしまう場合があった模様です。
そこで以下図の様に、該当するacosブロック実行箇所でconstrainブロックを使って回避したところ、「math domain error」が発生する事は無くなりました。
※math domain error＝数学領域エラーとは、「引数が定義域外」だよって言うエラーだったのね……。英語圏の皆様には、当然の用語なのかしら？
他にも、「divide by zero」等のランタイムエラーが発生する場合があるので、ランタイムエラーで停止したくない場合には、該当箇所への対処が必要です。
C言語などの寛容な処理系だと、「定義域外の値が渡された場合の動作は保証しない」とか言っておきながら、定義域外の値が渡されても、その後の動作が(なるべく)矛盾しない様な値を返してくれたりします。その様な処理系では逆に、「そんなことでは、バグを見過ごしてしまいかねない」、「他の処理系に移植した時に問題が発覚する」など、良し悪しなのですけどね……。
そんなこんなで完成したプログラムを以下にリンクしておきます。
完成プログラム：spirit_lev.m5f
UIFlowで読み込んで、M5StickC Plusへ転送してお使いください。
完成したプログラムを使って、M5StickC Plusをデジタルな水準器として使って見ましょう。以下、実験中の様子です。
写真に写っている様に、アナログな水準器をM5StickC Plusと同じ面に当てながら、以下実験サンプルの様に動作を確認しました。
実験サンプル：
大したことは、していないのですが、実験中の様子を動画にしておきましたので、ご確認ください。
ちゃんと、それらしく動いている様です。UIFlowは、この程度の簡単なプログラムを組むには、楽で便利で楽しいのですｗ（１＾−＾）ｗ。
今回、M5StickC Plusの2個のスイッチには何の動作も割り当てていないので、水平の校正機能や、スナップショット機能、等の応用に使えます。ご自身で機能拡張して実用性を高める場合には、ご活用ください。
UIFlowでは、そもそも、「大作」のプログラムを組むのは困難です。何しろ、ブロックを組み上げて作っていく開発環境ですから。しかも、大量のバグに阻まれることも覚悟しておく必要があります。今回みたいに、ちょっとしたプログラムを組む場合には、今後もちょくちょく使って行きたいと考えています。
