アタリをM5Atomで可視化
実際に釣りをしている風景
今回の記事は前回の記事
の続編になります。どういうものなのかについては前回の記事をご参照いいただきたく思います。
問題点
前回の記事ではM5StickCを竿の根本に取り付け、ADXL３４５をGroveケーブルで接続したものを作成しました。
２つの問題点が出てきました。
- Groveケーブルの長さが足りない
- Groveケーブルの重さと取り回しが気になる
Groveケーブルは売ってる中で一番長い200cmのものを使いましたがそれでも想定より竿が長く、M5StickCを取り付けたい位置まで足りませんでした。足りない分はUSB延長ケーブルをつかって延長しましたがシンプルな方がいいはずです。釣り竿としては2mというのは短いほうで、長いものだと4-5mあるのでこのままでは汎用性が落ちます。
そこで、脱GroveケーブルということでADXL345をGroveケーブルで接続せず、M5Atom本体だけで作り直してみようと思います。
早速購入
ということで早速スイッチサイエンスで注文しました。
例のごとく、送料を無料にするためだけにUSB-Type-Cの変換アダプタがインジェクションされています。環境センサーという気温、湿度、気圧が測れるセンサーもついでに買っておきました。これは別のものに利用する予定です。M5Atom LiteとM5Atom Matrix両方買ってしまいました。
ちなみにコロナの影響かよくわからないですが東京からというのに昼に注文して岡山へは翌日の午後に届きました。宅配業者空いてるんでしょうか、速すぎです。
誤算
今回の作戦は一番安いM5Atom Liteで作ろうとしていたのですが、買った後よく見てみたらM5Atom Liteには加速度センサーが入ってませんでした。
というわけでこれ単体で作ることはできないようです。（ADXL345と接続して使うことはできますがGroveケーブルが必要になります。）
Matrixの方にはちゃんと入ってるので、こっちで作る必要があるということがわかりました。
パソコン無しでアタリ検出
前回のプロトタイプでは、アタリ解析等はリソースが余ってるパソコンに任せてましたが、その弊害として手元ではアタリを確認できませんでした。できれば、竿側で確認したいと思います、竿側で判定できればM5MatrixのLEDで知らせたりできそうなので、夜釣りとかでも活躍しそうです。
オンラインアルゴリズムを使う
アタリの判定アルゴリズム（平均・標準偏差計算）は逐一変動するデータで毎回すべてのデータ（およそ２００個）から足し上げ計算するとかなりの計算量になります。実験はできてませんが計算量が多くて処理が間に合わない可能性があります。
そこで調べてみると少しずつ更新されるデータの場合に有効な、逐次計算法（オンラインアルゴリズム）というのが出てきました。
これを使えばM5Atomのスペックでも判定ができそうです。
参考になったページ
ただし、今回の用途ですと平均と言っても移動平均がほしいのです。一度加速度すべての平均で実験してみましたが、これは魚を釣り上げたときの加速度も計算に含まれてしまうので、ほしい平均ではないです。
竿を置いて静かになればその環境（風や揺れ）に順応するようにしたいのです。つまり数秒前からの平均値と標準偏差である移動平均と移動標準偏差がほしいわけです。
上のページでは移動平均のオンラインアルゴリズムと効率の良い配列の更新方法が説明されていましたのでこれを利用しない手はありません。
移動平均
上記参考ページより引用（注釈を入れてます）
if (cnt == TOTAL_NUM)
cnt = 0; // 移動平均のデータ長になるとカウンターをリセット
sum -= data[cnt]; // 一番古いデータを合計値から引く
data[cnt] = value; // 新しいデータをその位置に挿入
sum += data[cnt]; // 新しいデータを合計値に足す
cnt++; // カウンターを進める
average = sum / TOTAL_NUM; // 移動平均の計算
上のページで紹介されていた方法ですが、とてもすばらしい計算法だと思います。配列の操作と平均値の計算が融合してる感じです。移動平均を計算しつつ、配列も順繰りに使えてて計算はとても速そうです。
単純ですが聞かないとなかなか思いつきませんね、これは。
移動標準偏差
上記の例では移動平均を計算することができましたが標準偏差となると・・・、難しい。
考えましたがどうすればいいのかよくわかりませんでした。
調べてみたところ英語のwikiにまさしくその答えが乗ってることを見つけました。上とはすこし違った表記ですが移動平均も同時に得られます。
さっそくこれを参考にpythonのクラスを書いて実験です。それはつぎのようなものです。
まさしく追い求めていた移動標準偏差と移動平均を導くためのコード
class Online:
def __init__(self):
self.K = 0
self.n = 0
self.Ex = 0
self.Ex2 = 0
def add_variable(self, x):
if (self.n == 0):
self.K = x
self.n += 1
self.Ex += x - self.K
self.Ex2 += (x - self.K) * (x - self.K)
def remove_variable(self, x):
self.n -= 1
self.Ex -= (x - self.K)
self.Ex2 -= (x - self.K) * (x - self.K)
def get_mean(self):
return self.K + self.Ex / self.n
def get_variance(self):
return (self.Ex2 - (self.Ex * self.Ex) / self.n) / self.n
上の移動平均のアルゴリズム例の拡張になっていることがわかるかと思います。そしてこれを上の例のように入ってくるデーターの配列を順繰りに使うように考えます。
online = Online()
と定義した上で上と同じように、配列を順繰りに使います。
すこし込み入ったコードになってしまいましたが、とりあえずテストしてみます。
オンラインアルゴリズムの検証
移動平均・移動標準偏差のテストコード
import time
import random
import numpy as np
class Online:
def __init__(self):
# 統計量を計算するための一時保存変数
self.K = 0
self.n = 0
self.Ex = 0
self.Ex2 = 0
def add_variable(self, x):
# 新しい測定値xを追加する関数
if (self.n == 0):
self.K = x
self.n += 1
self.Ex += x - self.K
self.Ex2 += (x - self.K) * (x - self.K)
def remove_variable(self, x):
# 古い測定値xを削除するための関数
self.n -= 1
self.Ex -= (x - self.K)
self.Ex2 -= (x - self.K) * (x - self.K)
def get_mean(self):
# 平均値の計算
return self.K + self.Ex / self.n
def get_variance(self):
# 分散の計算
return (self.Ex2 - (self.Ex * self.Ex) / self.n) / self.n
TOTAL_NUM = 100
cnt = 0
data = [0] * TOTAL_NUM
y = np.array([])
online = Online()
while True:
# 新しく入力される値
val = random.random()
# numpyで計算する方法
if len(y) == TOTAL_NUM:
y = np.delete(y, 0)
y = np.append(y, val)
print(y.mean(), y.std(), y.var())
# オンラインアルゴリズムで計算する方法
# cntは測定値を書き込み、読み込みする位置を表している
if cnt == TOTAL_NUM:
cnt = 0 # 最後まで行くと0に戻す
# 長さがTOTAL_NUMになるまでは消さない
if online.n == TOTAL_NUM:
online.remove_variable(data[cnt]) # 統計計算値から一番古い値の分を削除
# 新しい測定点を保存
data[cnt] = val
# 新しい測定点の分を追加
online.add_variable(data[cnt])
# 平均と標準偏差（分散の二乗根）の計算
average = online.get_mean()
var = online.get_variance()
print(average, np.sqrt(var), var)
# cntの位置を一つ進める
cnt += 1
time.sleep(0.05)
これを実行すると
出力の一例
0.48835181584517806 0.2923841435164583 0.08548848737985289 # numpy
0.48835181584517817 0.29238414351645853 0.08548848737985303 # オンラインアルゴリズム
というふうに同じような値が計算できているようです。
これにてなんとか計算方法がわかりました！後はこれをC++に移行するのみです。
移動平均・標準偏差（C言語）
実験のためにpythonでやってみましたが、実際にはスケッチに書かないといけないのでこれをC言語版にします。
C++ではクラスが利用できるのでそれでかけばいいのですが・・・、今回はC言語みたいな形で書いてしまいました。
移動平均、移動標準偏差のオンラインアルゴリズム
// 統計量計算のための変数
int K = 0;
int n = 0;
double Ex = 0;
double Ex2 = 0;
void add_variable(int* x)
{
if (n == 0)
K = *x;
n += 1;
Ex += *x - K;
Ex2 += (*x - K) * (*x - K);
}
void remove_variable(int* x)
{
n -= 1;
Ex -= (*x - K);
Ex2 -= (*x - K) * (*x - K);
}
double get_mean(void)
{
return K + Ex / n;
}
double get_variance(void)
{
return (Ex2 - (Ex * Ex) / n) / n;
}
void get_stat(int* x, double* mean, double* std)
{
# 操作する配列の位置を表す
static int pos = 0;
# 測定値の保存配列（固定長）
static int data[BUFFER_SIZE] = {};
if(pos == BUFFER_SIZE)
pos = 0;
if(n == BUFFER_SIZE)
remove_variable(&data[pos]);
data[pos] = *x;
add_variable(&data[pos]);
*mean = get_mean();
*std = sqrt(get_variance());
pos++;
}
関数が実行された後も同じ値を保持しているstatic変数を利用して配列を関数内で保持すればC言語でもある程度短く書けました。
グローバル変数が４つ残っていますが、これらも本当は構造体とかで書いたほうが良さそうですが、今回は４つなのでこのままにしました。
使う場合は
上記関数群の呼び出し方
double mean = 0.0;
double standard = 0.0;
get_stat(&scalar, &mean, &standard);
という感じで呼び出します。ここでscalarは加速度の大きさの測定値です。
M5ATOM用のスケッチ
というわけで移動標準偏差値と移動平均値を逐次計算するためのコードがわかったので、スケッチの製作に入ります。
前回のコードを利用して改良します。
そして出来上がったものがこちら、
atom用のスケッチ（２０２０年９月１１日差し替え）
/*****************************************************************************/
// Function: Get the accelemeter of X/Y/Z axis and print out on the
// serial monitor and bluetooth.
// Usage: This program is for fishing. Use the accelerometer at the end
// of the rod to see if the fish is caught. Acceleration is
// transmitted in real time via Bluetooth and can be monitored
// from a laptop.
// Hardware: M5Atom + ADXL345(Grove)
// Arduino IDE: Arduino-1.8.13
// Author: Hideto Manjo
// Date: Aug 9, 2020
// Version: v0.2
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.
// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
//
/*******************************************************************************/
#include <M5Atom.h>
#include <FastLED.h>
#include <BLEDevice.h>
#include <BLEServer.h>
#include <BLE2902.h>
#include <ADXL345.h>
#include <Wire.h>
#define SERVICE_UUID "8da64251-bc69-4312-9c78-cbfc45cd56ff"
#define CHARACTERISTIC_UUID "deb894ea-987c-4339-ab49-2393bcc6ad26"
#define DEVICE_NAME "Tsurido"
// device select
#define USE_INTERNAL_IMU false // M5Atom Matrix Only
// basic
#define DELAY 50 // milliseconds
#define SERIAL true // With/without serial communication
#define BAUDRATE 115200 // Serial communication baud rate
// warning
#define WARN true // Enable warning LED
#define BUFFER_SIZE 200 // Buffer size for statics
#define TH_WARN 5 // Warning threshold(sigma)
// LED
#define LED_BRIGHTNESS 10 // Brightness Max is 20
#define COLOR_NEON_RED 0x00FF00 // GRB
#define COLOR_NEON_GREEN 0xFF0B01 // GRB
#define COLOR_NEON_BLUE 0x1E01FE // GRB
#define COLOR_NEON_YELLOW 0xFEFD02 // GRB
#define SCALAR(x, y, z) sqrt(x*x + y*y + z*z)
CRGB color_error = CRGB(COLOR_NEON_RED);
CRGB color_warning = CRGB(COLOR_NEON_YELLOW);
CRGB color_success = CRGB(COLOR_NEON_GREEN);
CRGB color_bluetooh = CRGB(COLOR_NEON_BLUE);
BLEServer* pServer = NULL;
BLECharacteristic* pCharacteristic = NULL;
ADXL345 adxl;
// FLAGS
bool deviceConnected = false;
bool oldDeviceConnected = false;
// online algorism
int K = 0;
int n = 0;
double Ex = 0;
double Ex2 = 0;
void fillColor(CRGB color)
{
static CRGB lastcolor = CRGB(0xFFFFFF);
if (color != lastcolor) {
for(int i = 0; i < 25; i++){
M5.dis.drawpix(i, color);
}
lastcolor = color;
}
}
void add_variable(int* x)
{
if (n == 0)
K = *x;
n += 1;
Ex += *x - K;
Ex2 += (*x - K) * (*x - K);
}
void remove_variable(int* x)
{
n -= 1;
Ex -= (*x - K);
Ex2 -= (*x - K) * (*x - K);
}
double get_mean(void)
{
return K + Ex / n;
}
double get_variance(void)
{
return (Ex2 - (Ex * Ex) / n) / n;
}
void get_stat(int* x, double* mean, double* std)
{
static int pos = 0;
static int data[BUFFER_SIZE] = {};
if(pos == BUFFER_SIZE)
pos = 0;
if(n == BUFFER_SIZE)
remove_variable(&data[pos]);
data[pos] = *x;
add_variable(&data[pos]);
*mean = get_mean();
*std = sqrt(get_variance());
pos++;
}
bool warn(int* val, double* standard) {
static long lastring = 0;
static bool ring = false;
static bool state = false;
if (micros() - lastring > 2000 * 1000) {
if (*val > TH_WARN * (*standard)) {
lastring = micros();
ring = true;
}else{
ring = false;
}
}
if (ring) {
if (state) {
M5.dis.setBrightness(0);
} else {
M5.dis.setBrightness(LED_BRIGHTNESS);
}
state = !state;
return true;
}
M5.dis.setBrightness(LED_BRIGHTNESS);
return false;
}
class MyServerCallbacks: public BLEServerCallbacks
{
void onConnect(BLEServer* pServer)
{
deviceConnected = true;
BLEDevice::startAdvertising();
}
void onDisconnect(BLEServer* pServer)
{
deviceConnected = false;
}
};
void setup_acc()
{
if (USE_INTERNAL_IMU) {
M5.IMU.Init();
return;
}
adxl.powerOn();
}
void setup_ble()
{
BLEDevice::init(DEVICE_NAME);
BLEServer *pServer = BLEDevice::createServer();
pServer->setCallbacks(new MyServerCallbacks());
BLEService *pService = pServer->createService(SERVICE_UUID);
pCharacteristic = pService->createCharacteristic(
CHARACTERISTIC_UUID,
BLECharacteristic::PROPERTY_READ |
BLECharacteristic::PROPERTY_WRITE |
BLECharacteristic::PROPERTY_NOTIFY |
BLECharacteristic::PROPERTY_INDICATE
);
pCharacteristic->addDescriptor(new BLE2902());
pService->start();
BLEAdvertising *pAdvertising = BLEDevice::getAdvertising();
pAdvertising->addServiceUUID(SERVICE_UUID);
pAdvertising->setScanResponse(false);
pAdvertising->setMinPreferred(0x0);
BLEDevice::startAdvertising();
}
void read_acc(int* x, int* y, int* z)
{
if (USE_INTERNAL_IMU) {
int16_t ax, ay, az;
M5.IMU.getAccelAdc(&ax, &ay, &az);
*x = (int) ax;
*y = (int) ay;
*z = (int) az;
return;
}
adxl.readXYZ(x, y, z);
}
void setup()
{
M5.begin(false, false, true);
fillColor(CRGB(0x888888));
M5.dis.setBrightness(LED_BRIGHTNESS);
Wire.begin(26, 32);
Serial.begin(BAUDRATE);
Serial.flush();
setup_acc();
setup_ble();
}
void loop()
{
int x = 0;
int y = 0;
int z = 0;
int scalar = 0;
char msg[128];
int diff = 0;
double mean = 0.0;
double standard = 0.0;
long wait;
long t = micros();
read_acc(&x, &y, &z);
scalar = SCALAR(x, y, z);
sprintf(msg, "Ax, Ay, Az, A: %d, %d, %d, %d", x, y, z, scalar);
if (scalar != 0) {
if (deviceConnected) {
fillColor(color_bluetooh);
} else {
fillColor(color_success);
}
} else {
fillColor(color_error);
}
if (SERIAL)
Serial.println(msg);
if (WARN)
get_stat(&scalar, &mean, &standard);
diff = (int) abs(scalar - mean);
warn(&diff, &standard);
if (deviceConnected) {
pCharacteristic->setValue(msg);
pCharacteristic->notify();
}
if (!deviceConnected && oldDeviceConnected) {
fillColor(color_warning);
delay(500);
pServer->startAdvertising();
oldDeviceConnected = deviceConnected;
}
if (deviceConnected && !oldDeviceConnected) {
oldDeviceConnected = deviceConnected;
}
wait = DELAY * 1000 - (micros() - t);
if (wait > 0)
delayMicroseconds(wait);
}
LEDの点灯
つい最近のこと、M5.Lcd.fillpixという便利な関数がM5Stack公式リポジトリの方には追加されたのですが、まだIDEの方では更新がかかってないのでdrawpixでLEDの点灯をしています。
LEDの色の指定は#include <FastLED.h>とした上で
LEDの色の指定（色の順番に注意）
#define COLOR_NEON_RED 0x00FF00 // GRB
#define COLOR_NEON_GREEN 0xFF0B01 // GRB
#define COLOR_NEON_BLUE 0x1E01FE // GRB
#define COLOR_NEON_YELLOW 0xFEFD02 // GRB
CRGB color_error = CRGB(COLOR_NEON_RED);
CRGB color_warning = CRGB(COLOR_NEON_YELLOW);
CRGB color_success = CRGB(COLOR_NEON_GREEN);
CRGB color_bluetooh = CRGB(COLOR_NEON_BLUE);
などとして指定しています。このCRGBを使えば
LEDの点灯
for(int i = 0; i < 25; i++) {
M5.dis.drawpix(i, color);
}
という感じでLEDを制御できます。合計3byte(24bits)の色指定はなぜかRGBではなく、GRBの順序になっているので、その順番で記述しています。
この現象の説明に詳しいサイト
どうも本家のソースもGRBの指定をコミットしてるようなのでそのうち直るかもしれないです。
AtomLiteはLEDが１個しかないですが、本家のコードもなぜか#define LED_NUMS=25という感じで、２５個で統一されているので、そのままにしています。AtomLiteの場合は1でも動きます。
前回のスケッチからの変更点
IMUの利用
内蔵のMPU6886を利用できるようにしました。USE_INTERNAL_IMUをtrueに設定すると、内蔵のセンサーから加速度を読み出します。
M5AtomMatrixではGroveケーブル接続のセンサーなしで単体で動作するようになりました。竿先に取り付けるだけで使えます。
BLEマルチコネクト
GithubにあるBLEライブラリの作成例を見ていたら、マルチコネクトの作例があったので
同時にたくさんのクライアントを接続できるように書き直しました。
グローバル変数
bool deviceConnected = false;
bool oldDeviceConnected = false;
このoldDeviceConnectedという変数を追加しておきます。
bleのセットアップ部分
void setup_ble()
{
BLEDevice::init(DEVICE_NAME);
BLEServer *pServer = BLEDevice::createServer();
pServer->setCallbacks(new MyServerCallbacks());
BLEService *pService = pServer->createService(SERVICE_UUID);
pCharacteristic = pService->createCharacteristic(
CHARACTERISTIC_UUID,
BLECharacteristic::PROPERTY_READ |
BLECharacteristic::PROPERTY_WRITE |
BLECharacteristic::PROPERTY_NOTIFY |
BLECharacteristic::PROPERTY_INDICATE
);
pCharacteristic->addDescriptor(new BLE2902());
pService->start();
BLEAdvertising *pAdvertising = BLEDevice::getAdvertising();
pAdvertising->addServiceUUID(SERVICE_UUID);
pAdvertising->setScanResponse(false);
pAdvertising->setMinPreferred(0x0);
BLEDevice::startAdvertising();
}
セットアップ部分も基本的には同じですが、少し変えています。そして、一番重要な部分はコネクションのさばき方です。
マルチコネクションへ対応するために変更したところloop()内
void loop() {
// 省略
if (deviceConnected) {
pCharacteristic->setValue(msg);
pCharacteristic->notify();
}
if (!deviceConnected && oldDeviceConnected) {
delay(500);
pServer->startAdvertising();
oldDeviceConnected = deviceConnected;
}
if (deviceConnected && !oldDeviceConnected) {
oldDeviceConnected = deviceConnected;
}
// 省略
}
と言う感じに書き直しました。これと合わせてコールバックを下記のように書き直しています。
マルチコネクション対応するための
class MyServerCallbacks: public BLEServerCallbacks
{
void onConnect(BLEServer* pServer)
{
deviceConnected = true;
BLEDevice::startAdvertising();
}
// 省略
};
これらのコードは参照元そのままのコードになっています。いまいち動作が理解できてないですが、接続が切れたり新しい接続がある度に新しいアドバタイジングを送信開始にしている感じのようです。
これで、一つの加速度センサーへ複数の機器から同時に接続できるようになりました。
モニターしたり、アラームを鳴らしたり別々の機器で連携できそうです。今回の例では簡略化のため、双方向通信については行っていないですが、そちらもコールバックを追加するだけでできるので、外からLEDの色を変えたりしきい値を設定し直すなども可能そうです。
小型のセンサーでBLEが使えるのは応用の幅が広がりますね。
BLEの作例については上で紹介した
がとても参考になりました。気になる方は是非ご参照ください。
Delayを正確に
今までは処理時間を考慮せずにwaitしてましたが、これからは処理時間も考慮して止まったほうが良さそうです。そこで、簡易的ではありますがmicros()というもっと正確な起動からの経過時間を取得する関数が用意されているようなのでそちらを使ったものに書き換えました。
ポイントとしては、DELAYで設定した値より超過するとwaitはしないようにしています。delayMicroseconds()に負の値が入るとエラーで止まるみたいです。micros()はマイクロ秒単位なのでDELAY(単位はms)の1000倍しています。
すこしだけ正確にdelayする方法
void loop() {
long wait;
long t = micros();
// メインの処理
// 省略
wait = DELAY * 1000 - (micros() - t);
if (wait > 0)
delayMicroseconds(wait);
}
M5StickCでプロット
センサー側での当たり判定がうまいこといったので、画面があるM5StickCではプロット機能もつけてみます。
ということで検索してみると、何やらそのまま使えそうな記事が出てきました。
このやり方を利用しました。（上のmicro秒のディレイもこの記事を参考にしてます。）プロットのやり方は同じですが、warningのしきい値の表示などを追加しました。
M5StickC用のスケッチ
結果としてできたコードは下記のようなものです。（少し長くなってしまいました。）
CPUクロックを下げて省電力モードに落ちる機能もつけてみました。その他、ADXL345ではなくて内蔵のIMUユニットMPU6886にも対応しました。
m5stickc版のコード（２０２０年９月１１日差し替え）
/*****************************************************************************/
// Function: Get the accelemeter of X/Y/Z axis and print out on the
// serial monitor and bluetooth.
// Usage: This program is for fishing. Use the accelerometer at the end
// of the rod to see if the fish is caught. Acceleration is
// transmitted in real time via Bluetooth and can be monitored
// from a laptop.
// Hardware: M5StickC + ADXL345(Grove)
// Arduino IDE: Arduino-1.8.13
// Author: Hideto Manjo
// Date: Aug 9, 2020
// Version: v0.2
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.
// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
//
/*******************************************************************************/
#include <M5StickC.h>
#include <BLEDevice.h>
#include <BLEServer.h>
#include <BLE2902.h>
#include <ADXL345.h>
#include <Wire.h>
#define SERVICE_UUID "8da64251-bc69-4312-9c78-cbfc45cd56ff"
#define CHARACTERISTIC_UUID "deb894ea-987c-4339-ab49-2393bcc6ad26"
#define DEVICE_NAME "Tsurido"
// device select
#define USE_INTERNAL_IMU true // Use internal IMU unit as acc sensor
// basic
#define LCD_ROTATION 0 // 90 * num (degree) [Counterclockwise]
#define SCREENBREATH 12 // LCD brightness (max 12)
#define DELAY 50 // milliseconds
#define SERIAL true // With/without serial communication
#define BAUDRATE 115200 // Serial communication baud rate
#define CPU_FREQ 240 // Set FREQ MHz -> 240 or 160
// (80, 40, 20, 10) is not work normally
// warning
#define WARN true // Enable warning LED
#define BUFFER_SIZE 200 // Buffer size for statics
// plot
#define X0 5 // Plot left padding
#define TH_WARN 5 // Warning threshold(sigma)
#define TH_MAX 10 // Maxrange(sigma)
// battery
#define BATT_FULL_VOLTAGE 4.2 // Battery full voltage
#define BATT_LOW_VOLTAGE 3.4 // Battery low voltage
// menu offsets
#define OFFSET_MENU 2 // Menu
#define OFFSET_MAIN 12 // Main
#define SCALAR(x, y, z) sqrt(x*x + y*y + z*z)
#define BATT_CHARGE(v, low, full) ((v - low) / (full - low) * 100)
BLEServer* pServer = NULL;
BLECharacteristic* pCharacteristic = NULL;
ADXL345 adxl;
// FLAGS
bool deviceConnected = false;
bool oldDeviceConnected = false;
bool plotEnabled = true;
bool lowEnergyMode = false;
// batt
int batt_charge = 100;
// online algorism
int K = 0;
int n = 0;
double Ex = 0;
double Ex2 = 0;
void changeCPUFreq(int freq)
{
while(!setCpuFrequencyMhz(freq)) {
;
}
}
void add_variable(int* x)
{
if (n == 0)
K = *x;
n += 1;
Ex += *x - K;
Ex2 += (*x - K) * (*x - K);
}
void remove_variable(int* x)
{
n -= 1;
Ex -= (*x - K);
Ex2 -= (*x - K) * (*x - K);
}
double get_mean(void)
{
return K + Ex / n;
}
double get_variance(void)
{
return (Ex2 - (Ex * Ex) / n) / n;
}
void get_stat(int* x, double* mean, double* std)
{
static int pos = 0;
static int data[BUFFER_SIZE] = {};
if(pos == BUFFER_SIZE)
pos = 0;
if(n == BUFFER_SIZE)
remove_variable(&data[pos]);
data[pos] = *x;
add_variable(&data[pos]);
*mean = get_mean();
*std = sqrt(get_variance());
pos++;
}
void updateStateBLE()
{
if (!lowEnergyMode) {
if (deviceConnected) {
M5.Lcd.setCursor(M5.Lcd.width() - 22, OFFSET_MENU);
M5.Lcd.setTextColor(WHITE, BLUE);
M5.Lcd.println("BLE");
} else {
M5.Lcd.setCursor(M5.Lcd.width() - 22, OFFSET_MENU);
M5.Lcd.setTextColor(WHITE, BLACK);
M5.Lcd.println(" ");
}
}
}
void updateStateBATT()
{
if (!lowEnergyMode) {
M5.Lcd.setCursor(0, OFFSET_MENU);
if (batt_charge > 33) {
M5.Lcd.setTextColor(WHITE, BLACK);
} else {
M5.Lcd.setTextColor(RED, BLACK);
}
M5.Lcd.printf("%d%%", batt_charge);
}
}
class MyServerCallbacks: public BLEServerCallbacks
{
void onConnect(BLEServer* pServer)
{
deviceConnected = true;
updateStateBLE();
}
void onDisconnect(BLEServer* pServer)
{
deviceConnected = false;
updateStateBLE();
}
};
void setup_acc()
{
if (USE_INTERNAL_IMU) {
M5.IMU.Init();
return;
}
adxl.powerOn();
}
void setup_ble()
{
BLEDevice::init(DEVICE_NAME);
BLEServer *pServer = BLEDevice::createServer();
pServer->setCallbacks(new MyServerCallbacks());
BLEService *pService = pServer->createService(SERVICE_UUID);
pCharacteristic = pService->createCharacteristic(
CHARACTERISTIC_UUID,
BLECharacteristic::PROPERTY_READ |
BLECharacteristic::PROPERTY_WRITE |
BLECharacteristic::PROPERTY_NOTIFY |
BLECharacteristic::PROPERTY_INDICATE
);
pCharacteristic->addDescriptor(new BLE2902());
pService->start();
BLEAdvertising *pAdvertising = BLEDevice::getAdvertising();
pAdvertising->addServiceUUID(SERVICE_UUID);
pAdvertising->setScanResponse(false);
pAdvertising->setMinPreferred(0x0);
BLEDevice::startAdvertising();
}
void read_acc(int* x, int* y, int* z)
{
if (USE_INTERNAL_IMU) {
int16_t ax, ay, az;
M5.IMU.getAccelAdc(&ax, &ay, &az);
*x = (int) ax;
*y = (int) ay;
*z = (int) az;
return;
}
adxl.readXYZ(x, y, z);
}
void plot(int* val, double* standard)
{
static int i = 0;
static int diff[BUFFER_SIZE] = {};
int sigma = 0;
int y0 = 0;
int y1 = 0;
int top = (int) TH_MAX * (*standard);
int height = M5.Lcd.height() - 5;
int width = M5.Lcd.width() - X0;
if(i == width)
i = 0;
diff[i] = *val;
if (i != 0) {
// plot
y0 = map((int)(diff[i - 1]), 0, top, height, 0);
y1 = map((int)(diff[i]), 0, top, height, 0);
M5.Lcd.drawLine(i - 1 + X0, y0, i + X0, y1, GREEN);
} else {
// new page
M5.Lcd.fillScreen(BLACK);
updateStateBLE();
updateStateBATT();
sigma = map((int)TH_WARN * (*standard), 0, top, height, 0);
M5.Lcd.drawLine(X0, sigma, width + X0, sigma, YELLOW);
}
M5.Lcd.setCursor(0, OFFSET_MAIN);
M5.Lcd.setTextColor(YELLOW, BLACK);
M5.Lcd.printf("Warn %4.0lf ", TH_WARN * (*standard));
M5.Lcd.setCursor(0, OFFSET_MAIN + 10);
M5.Lcd.setTextColor(GREEN, BLACK);
M5.Lcd.printf("Value%4d ", *val);
i++;
}
bool warn(int* val, double* standard) {
static long lastring = 0;
static bool ring = false;
if (micros() - lastring > 2000 * 1000) {
if (*val > TH_WARN * (*standard)) {
lastring = micros();
ring = true;
}else{
ring = false;
}
}
if (ring) {
digitalWrite(GPIO_NUM_10, !digitalRead(GPIO_NUM_10));
return true;
}
digitalWrite(GPIO_NUM_10, HIGH);
return false;
}
int battery_charge()
{
double vbat = M5.Axp.GetVbatData() * 1.1 / 1000;
int charge = BATT_CHARGE(vbat, BATT_LOW_VOLTAGE, BATT_FULL_VOLTAGE);
if(charge > 100) {
return 100;
}else if(charge < 0) {
return 1;
}
return charge;
}
void setup()
{
M5.begin(true, true, false);
// Check i2c pin assignment SDA=32, SDL=33
Wire.begin(32, 33);
Serial.begin(BAUDRATE);
Serial.flush();
setup_acc();
setup_ble();
changeCPUFreq(CPU_FREQ);
pinMode(GPIO_NUM_10, OUTPUT);
M5.Axp.ScreenBreath(SCREENBREATH);
M5.Lcd.setRotation(LCD_ROTATION);
M5.Lcd.fillScreen(BLACK);
}
void loop()
{
// sensor
int x = 0;
int y = 0;
int z = 0;
int scalar = 0;
char msg[128];
// plot
int diff = 0;
double mean = 0.0;
double standard = 0.0;
long t = micros();
long wait = 0;
// get ADXL345 data
read_acc(&x, &y, &z);
scalar = SCALAR(x, y, z);
// get battery charge(%)
batt_charge = battery_charge();
updateStateBATT();
sprintf(msg, "Ax, Ay, Az, A: %d, %d, %d, %d", x, y, z, scalar);
if (M5.BtnB.wasPressed()) {
if (lowEnergyMode) {
changeCPUFreq(CPU_FREQ);
M5.Axp.ScreenBreath(SCREENBREATH);
lowEnergyMode = false;
} else {
M5.Axp.ScreenBreath(0);
changeCPUFreq(80);
digitalWrite(GPIO_NUM_10, HIGH);
lowEnergyMode = true;
}
}
if (!lowEnergyMode) {
if (M5.BtnA.wasPressed()) {
plotEnabled = !plotEnabled;
M5.Lcd.fillScreen(BLACK);
updateStateBLE();
updateStateBATT();
if (!plotEnabled) {
M5.Lcd.setTextSize(2);
M5.Lcd.setTextColor(WHITE, BLACK);
M5.Lcd.setCursor(M5.Lcd.width() / 2 - 32,
OFFSET_MAIN);
M5.Lcd.printf("Tsuri");
M5.Lcd.setCursor(M5.Lcd.width() / 2 - 32,
OFFSET_MAIN + 20);
M5.Lcd.printf(" do");
M5.Lcd.setTextSize(1);
M5.Lcd.setTextColor(YELLOW, BLACK);
M5.Lcd.setCursor(0, M5.Lcd.height() - 20);
M5.Lcd.printf("Side button\n"
"-> power save");
}
}
if ((WARN || plotEnabled)) {
get_stat(&scalar, &mean, &standard);
diff = (int) abs(scalar - mean);
}
if (WARN)
warn(&diff, &standard);
if (plotEnabled) {
plot(&diff, &standard);
}else{
M5.Lcd.setTextSize(2);
M5.Lcd.setCursor(M5.Lcd.width() / 2 - 8 * 4,
M5.Lcd.height() / 2);
M5.Lcd.setTextColor(WHITE, BLACK);
M5.Lcd.printf("%5d", scalar);
M5.Lcd.setTextSize(1);
}
if (SERIAL)
Serial.println(msg);
}
if (deviceConnected) {
pCharacteristic->setValue(msg);
pCharacteristic->notify();
}
if (!deviceConnected && oldDeviceConnected) {
delay(500);
pServer->startAdvertising();
oldDeviceConnected = deviceConnected;
}
if (deviceConnected && !oldDeviceConnected)
oldDeviceConnected = deviceConnected;
M5.update();
wait = DELAY * 1000 - (micros() - t);
if (wait > 0)
delayMicroseconds(wait);
}
プロットの動作風景
プロットが見えにくくて申し訳ないですが、こんな感じでモニターリングできます。
プロットモード
緑の線が加速度の偏差です。常に標準偏差に合わせてスケールを変えて描画していますので、環境に順応します。結果としてこのプロットは前回の記事で作ったpythonクライアントのmatplotlibのプロットと同じ絵になります。
黄色い線が5σのラインを表していて、超えるとLEDが２秒間点滅します。左上の%はバッテリーの残量を表示しています。
加速度表示モード
こちらは加速度だけ表示モード
省電力モード
このほか、Bボタンを押すことで、画面を切って省電力モードに落ちるモードも追加しました。電源ケーブルもないほうが釣りしやすいのでバッテリー動作を狙っています。
現状では１時間程度持つようです。もう少し伸ばせられればよいのですが・・・
終わりに
前回の記事をベースにスケッチを改良してみました。オンラインアルゴリズムをつかうことで、当たり判定を機器に搭載できたので、パソコンなしの構成も可能になりました。M5StickCはパソコンと同じようなプロットも可能になりました。
実戦投入が楽しみです！
USBケーブルの問題点
M5AtomMatrixを使うと加速度センサーが搭載されているので、かなりコンパクトにできました。それでも、釣り場についてからUSBケーブルを竿先まで引くのが結構やってみると面倒です。
実際に使うためにはセンサーはバッテリー動作が手軽だと感じました。M5StickCのLCDを消灯モードで１時間程度持ちますが、最低でも釣りに使うためには２時間程度は持ってほしい感じです。外部バッテリーなども試して見る価値はありそうです。
ソースコードの公開
スケッチやクライアントの最新版はgithubのリポジトリに置いてますので
似たようなものを作る時など何かの参考にしていただければ幸いです。