LoginSignup
1
2

More than 3 years have passed since last update.

@poruruba

M5StickCのボタン押下でスマホから現在地をしゃべらせる(2/2)

Last updated at Posted at 2020-06-01

前回の投稿 M5StickCのボタン押下でスマホから現在地をしゃべらせる(1/2) の続きです。

今回は、M5Stick-C側のArduinoの実装をまとめます。

ソースコード一式をGitHubに上げておきました。

poruruba/orientation_navigator
　https://github.com/poruruba/orientation_navigator
※Android、Arduino という名前のフォルダの下です。

BLEペリフェラルの実装

Arduinoで実装しています。
まずは、BLEペリフェラルの準備です。

void taskServer(void*) {
  BLEDevice::init("M5Stick-C");

  BLEServer *pServer = BLEDevice::createServer();
  pServer->setCallbacks(new MyCallbacks());

  BLEService *pService = pServer->createService(UUID_SERVICE);

  pCharacteristic_write = pService->createCharacteristic( UUID_WRITE, BLECharacteristic::PROPERTY_WRITE );
  pCharacteristic_write->setAccessPermissions(ESP_GATT_PERM_WRITE);
  pCharacteristic_write->setValue(value_write, sizeof(value_write));
  pCharacteristic_write->setCallbacks(new MyCharacteristicCallbacks());

  pCharacteristic_read = pService->createCharacteristic( UUID_READ, BLECharacteristic::PROPERTY_READ );
  pCharacteristic_read->setAccessPermissions(ESP_GATT_PERM_READ);
  pCharacteristic_read->setValue(value_read, sizeof(value_read));

  pCharacteristic_notify = pService->createCharacteristic( UUID_NOTIFY, BLECharacteristic::PROPERTY_NOTIFY );
  pCharacteristic_notify->addDescriptor(new BLE2902());

  pService->start();

  BLEAdvertising *pAdvertising = pServer->getAdvertising();
  pAdvertising->addServiceUUID(UUID_SERVICE);
  pAdvertising->start();

  vTaskDelay(portMAX_DELAY); //delay(portMAX_DELAY);
}

デバイス名を「M5Stick-C」としています。BLEセントラルからスキャンするとき、この名前で探しています。

pServer->setCallbacks(new MyCallbacks());の部分で、コールバック関数を指定しています。
これは、BLEセントラルから接続あるいは切断されたときに呼ばれます。
接続・切断時に、M5Stick-CのLCDにその旨表示したり、Notificationが有効のままになっているのを切断時に解除したりしています。ここら辺の実装は自由です。

class MyCallbacks : public BLEServerCallbacks {
  void onConnect(BLEServer* pServer){
    connected = true;
    Serial.println("Connected\n");
    M5.Lcd.fillScreen(BLACK);
    M5.Lcd.setTextSize(2);
    M5.Lcd.setCursor(0, 0);
    M5.Lcd.print("Connected");
  }

  void onDisconnect(BLEServer* pServer){
    connected = false;
    isGyroscope = false;
    isAccelerometer = false;

    BLE2902* desc = (BLE2902*)pCharacteristic_notify->getDescriptorByUUID(BLEUUID((uint16_t)0x2902));
    desc->setNotifications(false);

    Serial.println("Disconnected\n");
    M5.Lcd.fillScreen(BLACK);
    M5.Lcd.setTextSize(2);
    M5.Lcd.setCursor(0, 0);
    M5.Lcd.print("Disconnected");
  }
};

もう一つコールバック関数を指定しています。
pCharacteristic_write->setCallbacks(new MyCharacteristicCallbacks());

BLEセントラルから、BLEペリフェラルのキャラクタリスティックに対して、WriteやRead、Notification有効化等をした時に呼び出されます。
今回は、Writeのみをカスタマイズしています。必須ではないのですが、今回何をしているかというと、BLEセントラル側から、「フォントサイズ,X座標,Y座標,文字列」が送られてきたときに、LCDに文字列を表示しています。

class MyCharacteristicCallbacks : public BLECharacteristicCallbacks{
  void onWrite(BLECharacteristic* pCharacteristic){
・・・
  // onStatusは必要に応じて
  //void onStatus(BLECharacteristic* pCharacteristic, Status s, uint32_t code){
・・・
　// onReadは必要に応じて
　//void onRead(BLECharacteristic *pCharacteristic){
・・・

あとは、setup()の中で呼び出してあげるだけです。

void setup() {
  M5.begin();

・・・

  xTaskCreate(taskServer, "server", 20000, NULL, 5, NULL);
}

ここら辺の詳細は、例えば、以下の投稿を参考にしてみてください。

(ご参考）
　ESP32でキーボードショートカットを作ってしまおう
　FIDOデバイスエミュレータを作成してみた（だけどもうちょっと。）
　BLEでESP32から接続するWiFiのSSID・パスワードを設定する
　M5Stick-Cでパスワード記憶装置 兼 自動入力装置を作る

ボタン押下時のNotification

loop()の中で、ボタンを検知し、BLEのNotificationを発行しています。

void loop() {
  M5.update();

  if ( M5.BtnA.wasPressed() )
    sendButton(BTNID_FN_BASE + 0);

上記のsendButtonの中では、以下の処理に行きつきます。

    pCharacteristic_notify->setValue(value_write, packet_size);
    pCharacteristic_notify->notify();

1回のNotificationで通知できるデータサイズに制限があるので、最大20バイトに分割して送信しています。ですので、そのためのヘッダを付けてシーケンス制御しています。
（FIDOの通信プロトコルを利活用していますが。。。）

その他処理

その他もろもろの処理が入っていますが、今回は気にしなくてもよいです。
以下でAndroidで実装しているジャイロスコープ、加速度センサをBLEセントラル側に通知していたのを、Arduinoに移植したものです。。。

　Androidをペンタブレットにする(1/3)

ということで、ソース一式載せてしまいます。

//#include <Arduino.h>
#include <M5StickC.h>
#include <BLEDevice.h>
#include <BLEUtils.h>
#include <BLEServer.h>
#include "BLE2902.h"

#define CMD_TOAST         0x0c
#define CMD_SENSOR_MASK   0x0e
#define CMD_TEXT          0x30

#define RSP_ACK           0x00
#define RSP_GYROSCOPE     0x14
#define RSP_ACCELEROMETER 0x15
#define RSP_BUTTON_EVENT  0x18

#define BTNID_FN_BASE   0x20

bool isGyroscope = false;
bool isAccelerometer = false;

void sendBuffer(uint8_t *p_value, uint16_t len);
void processCommaind(void);

#define UUID_SERVICE "08030900-7d3b-4ebf-94e9-18abc4cebede"
#define UUID_WRITE "08030901-7d3b-4ebf-94e9-18abc4cebede"
#define UUID_READ "08030902-7d3b-4ebf-94e9-18abc4cebede"
#define UUID_NOTIFY "08030903-7d3b-4ebf-94e9-18abc4cebede"

#define CAP_GYROSCOPE     0x00000002
#define CAP_ACCELEROMETER 0x00000004
#define CAP_BUTTON        0x00000040

BLECharacteristic *pCharacteristic_write;
BLECharacteristic *pCharacteristic_read;
BLECharacteristic *pCharacteristic_notify;

bool connected = false;

class MyCallbacks : public BLEServerCallbacks {
  void onConnect(BLEServer* pServer){
    connected = true;
    Serial.println("Connected\n");
    M5.Lcd.fillScreen(BLACK);
    M5.Lcd.setTextSize(2);
    M5.Lcd.setCursor(0, 0);
    M5.Lcd.print("Connected");
  }

  void onDisconnect(BLEServer* pServer){
    connected = false;
    isGyroscope = false;
    isAccelerometer = false;

    BLE2902* desc = (BLE2902*)pCharacteristic_notify->getDescriptorByUUID(BLEUUID((uint16_t)0x2902));
    desc->setNotifications(false);

    Serial.println("Disconnected\n");
    M5.Lcd.fillScreen(BLACK);
    M5.Lcd.setTextSize(2);
    M5.Lcd.setCursor(0, 0);
    M5.Lcd.print("Disconnected");
  }
};

unsigned short recv_len = 0;
unsigned short expected_len = 0;
unsigned char expected_slot = 0;
unsigned char recv_buffer[1024];

class MyCharacteristicCallbacks : public BLECharacteristicCallbacks{
  void onWrite(BLECharacteristic* pCharacteristic){
    Serial.println("onWrite");
    uint8_t* value = pCharacteristic->getData();
    std::string str = pCharacteristic->getValue(); 

    if( expected_len > 0 && value[0] != expected_slot )
        expected_len = 0;

    if( expected_len == 0 ){
      if( value[0] != 0x83 )
          return;
      recv_len = 0;
      expected_len = (value[1] << 8) | value[2];
      memmove(&recv_buffer[recv_len], &value[3], str.length() - 3);
      recv_len += str.length() - 3;
      expected_slot = 0;
      if( recv_len < expected_len )
        return;
    }else{
      memmove(&recv_buffer[recv_len], &value[1], str.length() - 1);
      recv_len += str.length() - 1;
      expected_slot++;
      if( recv_len < expected_len )
        return;
    }
    expected_len = 0;

    processCommaind();
  }
/*
  void onStatus(BLECharacteristic* pCharacteristic, Status s, uint32_t code){
  }
  void onRead(BLECharacteristic *pCharacteristic){
  }
*/
};

int str_split(char *p_data, char delimiter, char **pp_array, int max_num){
    int index = 0;
    int datalength = strlen(p_data);
    pp_array[0] = &p_data[0];
    for (int i = 0; i < datalength; i++) {
        char tmp = p_data[i];
        if ( tmp == delimiter ) {
          if( (index + 1) >= max_num )
            break;
          p_data[i] = '\0';
          index++;
          pp_array[index] = &p_data[i + 1];
        }
    }
    return (index + 1);
}

void processCommaind(void){
  switch(recv_buffer[0]){
    case CMD_SENSOR_MASK:{
      isGyroscope = (recv_buffer[1] & 0x02);
      isAccelerometer = (recv_buffer[1] & 0x04);
      break;
    }
    case CMD_TEXT:{
      recv_buffer[recv_len + 1] = '\0';
      char *arry[4];
      int num = str_split((char*)&recv_buffer[1], ',', arry, 4);
      int font = atoi(arry[0]);
      if( font <= 0 ){
        M5.Lcd.fillScreen(BLACK);
      }else{
        Serial.println(num);
        if( num < 4 )
          break;
        int x = atoi(arry[1]);
        int y = atoi(arry[2]);
        Serial.println(x);
        Serial.println(y);
        Serial.println(arry[3]);
        M5.Lcd.setTextSize(font);
        M5.Lcd.setCursor(x, y);
        M5.Lcd.print(arry[3]);
      }
      break;
    }
  }
  uint8_t ack = RSP_ACK;
  sendBuffer(&ack, 1);
}



#define UUID_VALUE_SIZE 20
uint8_t value_write[UUID_VALUE_SIZE];
uint32_t capability = CAP_BUTTON | CAP_GYROSCOPE | CAP_ACCELEROMETER;
uint8_t value_read[] = { (uint8_t)((UUID_VALUE_SIZE >> 8) & 0xff), (uint8_t)(UUID_VALUE_SIZE & 0xff), 
                      (uint8_t)((capability >> 24) & 0xff), (uint8_t)((capability >> 16) & 0xff), (uint8_t)((capability >> 8) & 0xff), (uint8_t)(capability & 0xff) };

void taskServer(void*) {
  BLEDevice::init("M5Stick-C");

  BLEServer *pServer = BLEDevice::createServer();
  pServer->setCallbacks(new MyCallbacks());

  BLEService *pService = pServer->createService(UUID_SERVICE);

  pCharacteristic_write = pService->createCharacteristic( UUID_WRITE, BLECharacteristic::PROPERTY_WRITE );
  pCharacteristic_write->setAccessPermissions(ESP_GATT_PERM_WRITE);
  pCharacteristic_write->setValue(value_write, sizeof(value_write));
  pCharacteristic_write->setCallbacks(new MyCharacteristicCallbacks());

  pCharacteristic_read = pService->createCharacteristic( UUID_READ, BLECharacteristic::PROPERTY_READ );
  pCharacteristic_read->setAccessPermissions(ESP_GATT_PERM_READ);
  pCharacteristic_read->setValue(value_read, sizeof(value_read));

  pCharacteristic_notify = pService->createCharacteristic( UUID_NOTIFY, BLECharacteristic::PROPERTY_NOTIFY );
  pCharacteristic_notify->addDescriptor(new BLE2902());

  pService->start();

  BLEAdvertising *pAdvertising = pServer->getAdvertising();
  pAdvertising->addServiceUUID(UUID_SERVICE);
  pAdvertising->start();

  vTaskDelay(portMAX_DELAY); //delay(portMAX_DELAY);
}

void setFloatBytes(float f, uint8_t *p_bin){
  uint8_t *p_ptr = (uint8_t*)&f;

  p_bin[0] = p_ptr[3];
  p_bin[1] = p_ptr[2];
  p_bin[2] = p_ptr[1];
  p_bin[3] = p_ptr[0];
}

void sendBuffer(uint8_t *p_value, uint16_t len){
  Serial.println("SendBuffer");

  BLE2902* desc = (BLE2902*)pCharacteristic_notify->getDescriptorByUUID(BLEUUID((uint16_t)0x2902));
  if( !desc->getNotifications() )
    return;

  int offset = 0;
  int slot = 0;
  int packet_size = 0;
  do{
    if( offset == 0){
      value_write[0] = 0x83;
      value_write[1] = (len >> 8) & 0xff;
      value_write[2] = len & 0xff;
      packet_size = len - offset;
      if( packet_size > (UUID_VALUE_SIZE - 3) )
        packet_size = UUID_VALUE_SIZE - 3;
      memmove(&value_write[3], &p_value[offset], packet_size);

      offset += packet_size;
      packet_size += 3;

    }else{
      value_write[0] = slot++;
      packet_size = len - offset;
      if( packet_size > (UUID_VALUE_SIZE - 1) )
        packet_size = UUID_VALUE_SIZE - 1;
      memmove(&value_write[1], &p_value[offset], packet_size);

      offset += packet_size;
      packet_size += 1;
    }

    pCharacteristic_notify->setValue(value_write, packet_size);
    pCharacteristic_notify->notify();

  }while(packet_size >= UUID_VALUE_SIZE);  
}

void sendButton(uint8_t btn){
  uint8_t send_buffer[2];
  send_buffer[0] = RSP_BUTTON_EVENT;
  send_buffer[1] = btn;
  sendBuffer(send_buffer, sizeof(send_buffer));
}

void sendIMU(){
  float gyroX, gyroY, gyroZ;
  float acclX, acclY, acclZ;

  M5.IMU.getGyroData(&gyroX, &gyroY, &gyroZ);
  M5.IMU.getAccelData(&acclX, &acclY, &acclZ);

  M5.Lcd.fillScreen(BLACK);
  M5.Lcd.setTextSize(1);
  M5.Lcd.setCursor(0, 0);
  M5.Lcd.printf("g:%+4.2f %+4.2f %+4.2f\n", gyroX, gyroY, gyroZ);
  M5.Lcd.printf("a:%+4.2f %+4.2f %+4.2f\n", acclX, acclY, acclZ);

  uint8_t sensor_buffer[13];

  if( isGyroscope ){
    sensor_buffer[0] = RSP_GYROSCOPE;
    setFloatBytes(gyroX, &sensor_buffer[1]);
    setFloatBytes(gyroY, &sensor_buffer[5]);
    setFloatBytes(gyroZ, &sensor_buffer[9]);
    sendBuffer(sensor_buffer, sizeof(sensor_buffer));
  }
  if( isAccelerometer ){
    sensor_buffer[0] = RSP_ACCELEROMETER;
    setFloatBytes(acclX, &sensor_buffer[1]);
    setFloatBytes(acclY, &sensor_buffer[5]);
    setFloatBytes(acclZ, &sensor_buffer[9]);
    sendBuffer(sensor_buffer, sizeof(sensor_buffer));
  }
}

void setup() {
  M5.begin();

  M5.IMU.Init();
  M5.Axp.ScreenBreath(9);

  M5.Lcd.setRotation(3);
  M5.Lcd.fillScreen(BLACK);
  M5.Lcd.setTextSize(2);
  M5.Lcd.println("General Button");
  delay(1000);

//  M5.Lcd.println("start Serial");
  Serial.begin(9600);
  Serial.println("setup");

  xTaskCreate(taskServer, "server", 20000, NULL, 5, NULL);
}

uint32_t prevTime = 0;
#define SENSOR_DELAY  100

void loop() {
  M5.update();

  if ( M5.BtnA.wasPressed() )
    sendButton(BTNID_FN_BASE + 0);

  if ( M5.BtnB.wasPressed() )
    sendButton(BTNID_FN_BASE + 1);

  if( isGyroscope || isAccelerometer ){
    long time = millis();
    if( time - prevTime > SENSOR_DELAY ){
      sendIMU();
      prevTime = time;
    }
  }
}

Androidのソースコードの構成

ついでに、Android側のソースコードの構成を補足しておきます。

MainActivity.java
　アプリを起動して最初に起動するアクティビティです。
　このアクティビティから、フォアグラウンドサービスを起動させます。

LocationService.java
　これがフォアグラウンドサービスの本体です。
　Notificationを生成して、startForeground()を呼び出して稼働状態にします。
　その後、GPS取得を開始したり、BLEセントラルとして、M5StickCに接続したりしています。
　M5StickCでボタンを押したときに通知されるNotificationをトリガとして、「目的地まで100メートル、北西方向です。」としゃべるようにしています。
　定期的に取得する現在地情報は、CheckPoints.javaのstaticフィールドに保持しています。

ResultActivity.java
　Notificationをタップすると表示されるアクティビティです。
　現在地や目的地までの距離・方位などを、音声ではなく文字で表示します。
　また、フォアグラウンドサービスの終了もこのアクティビティからできるようにしています。

CheckPoints.java
　取得した現在地情報を保持したり、サーバにHTTP Post通信して、目的地情報をJSONで取得したりしています。

あとは、チェックポイントのリストを走査したりししていますが、本題ではないので、気になる方は以下をご参照ください。

　GoogleMapを使ってサイクリングに出かけよう

Androidで2点間の距離と方位の計算

Androidには、2点の緯度経度から距離と方位を計算してくれる便利なAPIがあります。
CheckPionts.javaに実装しています。

CheckPoints.java
       distances = new float[3];
       Location.distanceBetween(location.getLatitude(), location.getLongitude(), checkpoints[next_point].lat, checkpoints[next_point].lng, distances);

方位は、0～360 の°で取得されるので、音声でしゃべれるように、以下のような関数を作って、方角を8分割しています。

CheckPoints.java
    static final String[] DIRECTION_TEXT = new String[]{"北", "北東", "東", "南東", "南", "南西", "西", "北西" };

    public static String getDirectionText(){
        if( distances != null ) {
            double target = distances[1] + (360 / 16);
            return DIRECTION_TEXT[(int)(int)target / (360 / 8)];
        }else{
            return null;
        }
    }

AndroidでHTTP Post通信

application/json でのHTTP POSTのコードも示しておきます。

HttpPostJson.java
    public static JSONObject doPost(String requestUrl, JSONObject input, int connTimeout) throws Exception{
        BufferedReader reader = null;
        OutputStream os = null;
        HttpURLConnection urlCon = null;

        try {
            URL url = new URL(requestUrl);

            urlCon = (HttpURLConnection) url.openConnection();
            urlCon.setConnectTimeout(connTimeout);
//            urlCon.setReadTimeout(10000);
            urlCon.setRequestMethod("POST");

            urlCon.setDoInput( true );
            urlCon.setDoOutput(input != null);
            urlCon.setRequestProperty("Content-Type", "application/json; charset=UTF-8");
            urlCon.setUseCaches(false);

            urlCon.connect();

            if( input != null ) {
                PrintStream ps = new PrintStream(urlCon.getOutputStream());
                ps.print(input.toString());
                ps.close();
            }

            int status = urlCon.getResponseCode();
            if( status != HttpURLConnection.HTTP_OK)
                throw new Exception("HTTP Status Error");

            InputStream is = urlCon.getInputStream();
            reader = new BufferedReader(new InputStreamReader(is));

            StringBuffer buffer = new StringBuffer();
            String str;
            while (null != (str = reader.readLine())) {
                buffer.append(str);
            }
            is.close();

            return new JSONObject(buffer.toString());
        } catch (Exception ex) {
            throw ex;
        } finally {
            try {
                if (reader != null)
                    reader.close();
                if (os != null)
                    os.close();
                if (urlCon != null)
                    urlCon.disconnect();
            } catch (IOException e) {
            }
        }
    }

HTTP接続先が、HTTPSではなくHTTPの場合には警告が出てつなげないようです。
その場合は、以下のファイルを作って、その中で接続先のホスト名を指定し、

app/src/main/res/xml/network_security_config.xml
<?xml version="1.0" encoding="utf-8"?>
<network-security-config>
    <domain-config cleartextTrafficPermitted="true">
        <domain includeSubdomains="true">localhost</domain>
    </domain-config>
</network-security-config>

AndroidManifest.xmlで、以下のように指定すればよいようです。

AndroidManifest.xml
    <application
・・・
        android:networkSecurityConfig="@xml/network_security_config"
・・・

(参考) Android デベロッパー： ネットワーク セキュリティ構成
https://developer.android.com/training/articles/security-config?hl=ja

以上

1
2
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
1
2