MemoryEnhancer > MEUI > ESP8266_180121.ino > v0.1-v0.4 > MPU-9250からQuaternionを取得し、UDP送信する > 回路 + コード + 動作確認

19903 ワード
borgSensorSystem1 DMP universalCircuit networkApp DMP テキストリンク
動作環境
ESP-WROOM-02
MPU-9250 https://strawberry-linux.com/catalog/items?code=12250
ライブラリ https://github.com/jrowberg/i2cdevlib/

Arduino IDE 1.6.6 on Windows 8.1 Pro(64bit)

Ubuntu 16.04 LTS desktop amd64
  - PyQt5インストール済

概要

プロジェクト開始: https://qiita.com/7of9/items/4077c92b9a773e054193

記憶力増強装置Memory Enhancer。そのUIとしてMPU-9250を使った加速度の値を読取り、通信をするように検討している。

ユニバーサル基板実装と通信ソフトについて記載する。

MEUIは「Memory Enchancer User Interface」の略称としている。

使用MCU, IC

使用品

ESP8266にはストレートピンヘッダを実装しており、様々なユニバーサル基板に付け替えできるようにしている。

左上の基板(茶色)はESP8266へのプログラムロードに利用している。右側のCR-123Aを電源としてプログラムロードする。

左下の基板(青色)は今回作成したユニバーサル基板。ダイセンのPU37x52を使用。付いている穴はグローブへの取り付けの縫製に使う。
青色基板の右上の緑色ICがMPU-9250。

茶色基板でプログラムロードした後に、青色基板を使って動作させる。

回路図

code v0.4

ESP8266用のコードは傾斜計 (v0.3) > MPU-9250の測定速度を1Hzにするを元にしている。MPU9150_9Axis_MotionApps41.hは同じもの(1Hz版)を利用している。wifiConfig.hは自分の環境に合わせてSSIDとパスワードを記載している。
そのコードにこちらで記載しているUDP送信機能を追加した。
UDP送信先は以下とした。

  • IP:192.168.0.9
  • Port:7000
esp8266_180121_MEUI.ino
/* v0.4 2018, Jan. 21
 *   - add WiFi_setup()
 *   - branched from [esp8266_160701_MPU9250]
 * v0.3 2016 Jul. 02
 *   - change interval from 100Hz to 1HZ
 *     + modify [MPU9150_9Axis_MotionApps41.h] for 1Hz
 *     + add MPU9150_9Axis_MotionApps41.h (copied from Arduino\libraries\)
 * v0.2 2016 Jul. 01
 *   - based on MPU6050_DMP6 (6/21/2012 by Jeff Rowberg <[email protected]>)
 */

#include "I2Cdev.h"
#include "MPU9150_9Axis_MotionApps41.h"
#include "Wire.h"
#include <ESP8266WiFi.h>

#include <WiFiUDP.h>
#include "wifiConfig.h"
/*
 * wifiConfig.hにおいて kWifiSsid, kWifiPass を char *[]型で定義。使用WiFiの設定を記載すること
 */

static WiFiUDP wifiUdp; 
static const char *kRemoteIpadr = "192.168.0.9";
static const int kRemoteUdpPort = 7000;

MPU9150 mpu;

#define INTERRUPT_PIN 14  // ESP8266

bool dmpReady = false;  // set true if DMP init was successful
uint16_t packetSize;
uint16_t fifoCount;

volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
    mpuInterrupt = true;
}

void MPU_setup() {
  Wire.begin();
  Wire.setClock(400000L); // 400kHz

  mpu.initialize();
  pinMode(INTERRUPT_PIN, INPUT);

  if (mpu.testConnection()) { // TODO: NG occurred
    Serial.println( F("MPU connect OK") );
  } else {
    Serial.println( F("MPU connect NG") );
  }

  uint8_t devStatus = mpu.dmpInitialize();

  // for calibration
//  mpu.setXGyroOffset(0);
//  mpu.setYGyroOffset(0);
//  mpu.setZGyroOffset(0);
//  mpu.setZAccelOffset(0);

  if (devStatus == 0) { // OK
    mpu.setDMPEnabled(true);
    attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING); 
//    uint8_t mpuIntStatus = mpu.getIntStatus();
    dmpReady = true;
    packetSize = mpu.dmpGetFIFOPacketSize();
    Serial.println( F("DMP initialize: OK") );
  } else {
    // error
    Serial.println( F("DMP initialize: NG") );
  }
}

void WiFi_setup(){
  static const int kLocalPort = 9000;

  WiFi.begin(kWifiSsid, kWifiPass);
  while(WiFi.status() != WL_CONNECTED) {
    delay(500);
  }
  wifiUdp.begin(kLocalPort);
}

void setup() {
  WiFi.disconnect();
  Serial.begin(115200);
  WiFi_setup();
  MPU_setup();
}

void loop() {
  uint8_t mpuIntStatus;
  uint8_t fifoBuffer[64];
// uint16_t fifoCount; // declaring fifoCount here causes FIFO overflow

  if (!dmpReady) {
    return;
  }

  while(!mpuInterrupt && fifoCount < packetSize) 
    ;

  mpuInterrupt = false;
  mpuIntStatus = mpu.getIntStatus();
  fifoCount = mpu.getFIFOCount();

  if ( (mpuIntStatus & 0x10) || (fifoCount == 1024) ) {
    mpu.resetFIFO();
    Serial.println( F("FIFO overflow!") );
  } else if (mpuIntStatus & 0x02) {
      while(fifoCount < packetSize) {
        fifoCount = mpu.getFIFOCount();
      }

      mpu.getFIFOBytes(fifoBuffer, packetSize);
      fifoCount -= packetSize;

      Quaternion qtn; // [w, x, y, z]
      mpu.dmpGetQuaternion(&qtn, fifoBuffer);
      Serial.print("quat\t");
      Serial.print(qtn.w);      
      Serial.print("\t");
      Serial.print(qtn.x);      
      Serial.print("\t");
      Serial.print(qtn.y);      
      Serial.print("\t");
      Serial.println(qtn.z);      

      wifiUdp.beginPacket(kRemoteIpadr, kRemoteUdpPort);
      wifiUdp.write("qtn,");
      char buf[10];
      dtostrf(qtn.x, 2, 2, buf);
      wifiUdp.write(buf);
      wifiUdp.write(",");
      dtostrf(qtn.y, 2, 2, buf);
      wifiUdp.write(buf);
      wifiUdp.write(",");
      dtostrf(qtn.z, 2, 2, buf);
      wifiUdp.write(buf);
      wifiUdp.write("\r\n");      
      wifiUdp.endPacket();
  }
}

動作検証

PyQt5で実装したUDP echo serverを使った。
PyQt5 > UDP > echo server > Multicast版? | non-Multicast版

udp_echoserver_180119.pyを実行してから、青色基板へCR-123A電源を供給する。

$python3 udp_echoserver_180119.py 
<PyQt5.QtNetwork.QHostAddress object at 0x7f295f16c898>
qtn,-0.06,-0.23,-0.16

<PyQt5.QtNetwork.QHostAddress object at 0x7f295f16c828>
qtn,-0.22,0.29,-0.16

<PyQt5.QtNetwork.QHostAddress object at 0x7f295f16c898>
qtn,0.20,-0.21,-0.24

<PyQt5.QtNetwork.QHostAddress object at 0x7f295f16c828>
qtn,-0.23,0.07,-0.11

<PyQt5.QtNetwork.QHostAddress object at 0x7f295f16c898>
qtn,0.03,-0.34,-0.21

<PyQt5.QtNetwork.QHostAddress object at 0x7f295f16c828>
qtn,0.47,-0.46,-0.04

Quaternionの(x,y,z)値がESP8266から送信され、Ubuntuで受信できた。