NOBのArduino日記!

NOBのArduino日記!

趣味は車・バイク・自転車・ラジコン・電子工作です。

MARGセンサーの使い方!(ATD-M4S,GY-80)(接続編)

イメージ 3
マルチ機能(MARG)センサー(ATD-M4S,GY-80)
 
 ドローン用センサーとしては定番?のMARGセンサー(aitendoさん品名:ATD-M4S、海外での品名:GY-80)を使ってみました
 
MARG(英:Magnetic, Angular Rate and Gravity)センサーとは、3軸角速度計と3軸加速度計からなるIMU(慣性計測装置、英: inertial measurement unit)と、さらに3軸地磁気計によって構成されるマルチセンサーアレイの事。

1. マルチ機能センサーとは

 マルチ機能センサー(ATD-M4S,GY-80)は、表1に示す4種のセンサーチップを搭載し、計10軸の物理量を測定可能なIMU(慣性計測装置、英: inertial measurement unit)です。
 

 

表1:マルチ機能センサー(ATD-M4S,GY-80)に含まれるチップとI2Cアドレス
センサーチップ 機能 I2C アドレス
L3G4200D 3軸角速度センサ 0x69
ADXL345 3軸デジタル加速度計     0x53
HMC5883L 3軸デジタルコンパス 0x1E
BMP085 気圧センサ 0x77

※「GY80」のサイトより引用させて頂きました

 
 これら4つのセンサーチップは、表1に示すI2Cデジタルインターフェイスによって接続可能で、3.3V / 5.0Vマイクロコントローラをサポートするレギュレータおよびロジックコンバータを内蔵している事から接続が容易です。
 用途としては、自己バランスロボットやクワッドコピーターなどの多くのプロジェクトに使用されています。
 
1.1 外観及び寸法
 マルチ機能センサー(ATD-M4S,GY-80)の外観及び寸法を図1に示します。
 PCB基盤には、Φ3mmの取付穴2箇所空いています。
 2.54mmピンインターフェイスに付属のピンヘッダーを取り付ければ、ブレッドボードに直接差し込む事が出来ます。
イメージ 4
図1:マルチ機能センサー(ATD-M4S,GY-80)の外観及び寸法(実測値)
 
1.2 内部回路
 マルチ機能センサー(ATD-M4S,GY-80)の内部回路図を図2に示します。
イメージ 7
図2:マルチ機能センサー(ATD-M4S,GY-80)の内部回路図

2. ライブラリのインストール

 マルチ機能センサー(ATD-M4S,GY-80)用のArduinoライブラリが無償で公開されていましたので試しにインストールしてみました!有志の方々ありがとうございます!
 
2.1 「GY80.h」ライブラリ入手!
 GitHubさんに「GY80」用のArduinoライブラリが公開されていますので図3の様にダウンロードします。
 方法は、図3ダウンロードページにアクセス→右上の緑のボタン「Clone or download」をクリック→ポップアップ中の「Download ZIP」をクリックしてダウンロード開始→ダウンロードフォルダに「GY-80 master.zip」ファイルがダウンロードされます。
イメージ 1
図3:「GY80」用のArduinoライブラリのダウンロード
 
2.2 「GY80.h」ライブラリのインストール!
 ArduinoIDEを起動して、図3でダウンロードした「GY-80 master.zip」ファイルを図4の様にインストールします
 方法は、メニューの「スケッチ」→「ライブラリをインクルード」→「.Zip形式のライブラリをインストール」→ダウンロードフォルダから「GY-80 master.zip」ファイルを選択して「開く」ボタンをクリック!でインストール完了です。
 インストールされているか確認する為には、再度「ライブラリをインクルード」を選択して、一覧の中に「GY-80 master」が登録されていればOKです。
イメージ 2
図4:GY-80 masterライブラリのインストール!

3. 実験!

 マルチ機能センサー(ATD-M4S,GY-80)への接続は、4つの3.3V駆動センサーチップと5V駆動のArduinoUNO接続しなければならないので一見大変そうです。
 しかし実際にはI2C接続で4つのセンサーチップの配線が共用化されており、かつロジックレベルコンバータも内蔵したモジュールなので、接続は直に4本だけと超お手軽です!べんり!
 
3.1 回路
 回路接続図をFritzingを使って図5の様に描いてみました。 
 また図5の通り実際に繋げた状態を図6に示します。
イメージ 5
図5:ブレッドボード回路図
 
イメージ 6
図6:実際に作ったもの
 
3.2 プログラム
 図6の回路で、ArduinoUNOに書き込むプログラムを図7に示します。
 内容としては、「GY80.h」ライブラリをインクルードして、sensor.begin関数を実行するだけで4つのセンサーチップとのI2C通信が開始されます。
 この状態で、「GY80_scaled val = sensor.read_scaled();」を実行する事ですべてのセンサーから値を取得します。
 最後に測定した結果をSerial.print関数により、図8の様にシリアルモニター上に出力します。
#include <Wire.h>
#include <GY80.h>
GY80 sensor = GY80(); //create GY80 instance

void setup() { // 毎秒9600ビットでシリアル通信を初期化する:
  Serial.begin(9600);
  sensor.begin();
}  //センサーを初期化する

void loop() {
  GY80_scaled val = sensor.read_scaled(); //すべてのセンサーから値を取得する
  // 以下値をシリアル出力する
  Serial.print("Mag:");    //磁力計の値
  Serial.print(val.m_x, 2);
  Serial.print(',');
  Serial.print(val.m_y, 2);
  Serial.print(',');
  Serial.print(val.m_z, 2);
  Serial.print(' ');
  Serial.print("Acc:");    //加速度計の値
  Serial.print(val.a_x, 3);
  Serial.print(',');
  Serial.print(val.a_y, 3);
  Serial.print(',');
  Serial.print(val.a_z, 3);
  Serial.print(' ');
  Serial.print("Gyro:");   //ジャイロスコープの値
  Serial.print(val.g_x, 1);
  Serial.print(',');
  Serial.print(val.g_y, 1);
  Serial.print(',');
  Serial.print(val.g_z, 1);
  Serial.print(' ');
  Serial.print("P:");      //気圧の値
  Serial.print(val.p, 5);
  Serial.print(' ');
  Serial.print("T:");      //気温の値
  Serial.println(val.t, 1);
  delay(250);
}              // 安定性のための読み取り間の遅延
イメージ 1
図7:プログラム例
※プログラムはコチラを参考にさせて頂きました
 
イメージ 8
図8:Arduino IDE シリアルモニター(図6右側を北に向けた状態で静置し測定)

4. まとめ

 先日aitendoさんで買って来たマルチ機能センサー(ATD-M4S,GY-80)は、ライブラリのお陰で、10軸(実際は気温も取れるので11軸!)もの高機能が簡単に使えて便利でした。
 これでドローンの姿勢制御も簡単に出来たら良いのですが
 
■2024年4月23日追記
 ESP32でもGY-80を使いたかったので、表2ピンアサインで接続(21,22は10kΩ3.3Vでプルアップ)し、ArduinoIDEでボードを「ESP32 Dev Module」として図7GY-80ライブラリを含むプログラムをコンパイルした所エラーは出ませんでしたが、書き込むとcore1 panicになりますね・・・(;'∀')
 どうやら「uint8_t* read(uint8_t device, uint8_t address, uint8_t length){}」と「void write(uint8_t device, uint8_t address, uint8_t data) {}」辺りでCPUがパニックになっているようです。
 半日掛けてようやく図11の通りGY-80の全データをESP32で吸い出す事が出来ました!(^^)/
 ※参考に修正版のコードを図12に貼っておきます。
表2:ESP32 to GY-80 ピンアサイン一覧
ESP32 GY-80
3V3 VCC_3.3V
GND GND
22 SCL
21 SDA
 

図10:GY-80からESP32に全データ取得できました!(^^)/
#include <Wire.h>
class GY80 {
  public:  
#define GY80_dev_m 0x1E
#define GY80_dev_a 0x53
#define GY80_dev_g 0x69
#define GY80_dev_p 0x77
#define GY80_m_reg_cfgA 0x00
#define GY80_m_reg_cfgB 0x01
#define GY80_m_reg_mode 0x02
#define GY80_m_reg_data 0x03
#define GY80_m_mode_continous 0x00
#define GY80_m_mode_single 0x01
#define GY80_m_mode_idle 0x03
#define GY80_m_scale_0_88 0x00
#define GY80_m_scale_1_3 0x01
#define GY80_m_scale_1_9 0x02
#define GY80_m_scale_2_5 0x03
#define GY80_m_scale_4_0 0x04
#define GY80_m_scale_4_7 0x05
#define GY80_m_scale_5_6 0x06
#define GY80_m_scale_8_1 0x07
#define GY80_g_reg_ctrl1 0x20
#define GY80_g_reg_ctrl2 0x21
#define GY80_g_reg_ctrl3 0x22
#define GY80_g_reg_ctrl4 0x23
#define GY80_g_reg_ctrl5 0x24
#define GY80_g_reg_datax 0x28
#define GY80_g_reg_datay 0x2A
#define GY80_g_reg_dataz 0x2C
#define GY80_g_scale_250 0x00
#define GY80_g_scale_500 0x10
#define GY80_g_scale_2000 0x30
#define GY80_a_reg_pwrctrl 0x2D
#define GY80_a_reg_data 0x32
#define GY80_a_reg_format 0x31
#define GY80_a_reg_bw 0x2C
#define GY80_a_bw_3200 0b1111 // 1600Hz Bandwidth   140µA IDD
#define GY80_a_bw_1600 0b1110 //  800Hz Bandwidth    90µA IDD
#define GY80_a_bw_800  0b1101 //  400Hz Bandwidth   140µA IDD
#define GY80_a_bw_400  0b1100 //  200Hz Bandwidth   140µA IDD
#define GY80_a_bw_200  0b1011 //  100Hz Bandwidth   140µA IDD
#define GY80_a_bw_100  0b1010 //   50Hz Bandwidth   140µA IDD
#define GY80_a_bw_50   0b1001 //   25Hz Bandwidth    90µA IDD
#define GY80_a_bw_25   0b1000 // 12.5Hz Bandwidth    60µA IDD
#define GY80_a_bw_12_5 0b0111 // 6.25Hz Bandwidth    50µA IDD
#define GY80_a_bw_6_25 0b0110 // 3.13Hz Bandwidth    45µA IDD
#define GY80_a_bw_3_13 0b0101 // 1.56Hz Bandwidth    40µA IDD
#define GY80_a_bw_1_56 0b0100 // 0.78Hz Bandwidth    34µA IDD
#define GY80_a_bw_0_78 0b0011 // 0.39Hz Bandwidth    23µA IDD
#define GY80_a_bw_0_39 0b0010 // 0.20Hz Bandwidth    23µA IDD
#define GY80_a_bw_0_20 0b0001 // 0.10Hz Bandwidth    23µA IDD
#define GY80_a_bw_0_10 0b0000  // 0.05Hz Bandwidth    23µA IDD (default value)
#define GY80_a_scale 0.004
#define GY80_a_scale_2 0x00
#define GY80_a_scale_4 0x01
#define GY80_a_scale_8 0x02
#define GY80_a_scale_16 0x03

    struct GY80_scaled {
      float a_x;
      float a_y;
      float a_z;
      float m_x;
      float m_y;
      float m_z;
      float g_x;
      float g_y;
      float g_z;
      float p;
      float t;
    };
    GY80_scaled val;

    struct GY80_raw {
      int16_t a_x;
      int16_t a_y;
      int16_t a_z;
      int16_t m_x;
      int16_t m_y;
      int16_t m_z;
      int16_t g_x;
      int16_t g_y;
      int16_t g_z;
      uint32_t p;
      uint16_t t;
    };

    struct GY80_single_raw {
      int16_t x;
      int16_t y;
      int16_t z;
    };

    struct GY80_single_scaled {
      float x;
      float y;
      float z;
    };

    struct GY80_p_calibration_type {
      int ac1;
      int ac2;
      int ac3;
      unsigned int ac4;
      unsigned int ac5;
      unsigned int ac6;
      int b1;
      int b2;
      int mb;
      int mc;
      int md;
      long b5;
    };
    /*-------------------------------------------------------------------------*/
    /*-------------------------------------------------------------------------*/
    /*-------------------------------------------------------------------------*/
    GY80() {
      m_scale = 1;
      //a_scale = 0;
      g_scale = 1;
      m_scale_values[0] = 0;
      m_scale_values[1] = 100. / 1100;
      m_scale_values[2] = 100. / 855;
      m_scale_values[3] = 100. / 670;
      m_scale_values[4] = 100. / 450;
      m_scale_values[5] = 100. / 400;
      m_scale_values[6] = 100. / 330;
      m_scale_values[7] = 100. / 230;
      m_scale_values_z[0] = 0;
      m_scale_values_z[1] = 100. / 980;
      m_scale_values_z[2] = 100. / 760;
      m_scale_values_z[3] = 100. / 600;
      m_scale_values_z[4] = 100. / 400;
      m_scale_values_z[5] = 100. / 355;
      m_scale_values_z[6] = 100. / 295;
      m_scale_values_z[7] = 100. / 205;
      g_scale_values[0] = 0.007629;
      g_scale_values[1] = 0.015259;
      g_scale_values[2] = 0.061035;
      p_oss = 0;
    }

    void begin() {
      Wire.begin();
      Wire.setClock(400000);     // 400000 = 400kHz I2C clock. 通信異常がある場合は100000(100kHz)に変更する
      m_init();
      a_init();
      g_init();
      p_init();
    }

    GY80_raw read_raw() {
      GY80_single_raw single;
      GY80_raw raw;
      single = m_read_raw();
      raw.m_x = single.x;
      raw.m_y = single.y;
      raw.m_z = single.z;
      single = g_read_raw();
      raw.g_x = single.x;
      raw.g_y = single.y;
      raw.g_z = single.z;
      single = a_read_raw();
      raw.a_x = single.x;
      raw.a_y = single.y;
      raw.a_z = single.z;
      raw.p = p_read_raw();
      raw.t = t_read_raw();
      return raw;
    }

    GY80_scaled read_scaled() {
      GY80_single_scaled single;
      GY80_scaled scaled;
      single = m_read_scaled();
      scaled.m_x = single.x;
      scaled.m_y = single.y;
      scaled.m_z = single.z;
      single = g_read_scaled();
      scaled.g_x = single.x;
      scaled.g_y = single.y;
      scaled.g_z = single.z;
      single = a_read_scaled();
      scaled.a_x = single.x;
      scaled.a_y = single.y;
      scaled.a_z = single.z;
      scaled.p = p_read_scaled();
      scaled.t = t_read_scaled();
      return scaled;
    }

    //######################################################### MAGNETOMETER CODE
    //magnetometer HMC5883L
    void m_set_mode(uint8_t mode) {
      //★write(GY80_dev_m, GY80_m_reg_mode, mode);
      Write(GY80_dev_m, GY80_m_reg_mode, &mode, sizeof(mode));
      
    }
    void m_set_scale(uint8_t scale) {
      scale &= 0x07; //only lower 3 bits
      m_scale = m_scale_values[scale];
      m_scale_z = m_scale_values_z[scale];
      //★write(GY80_dev_m, GY80_m_reg_cfgB, (scale << 5));
      scale=scale << 5;
      Write(GY80_dev_m, GY80_m_reg_cfgB, &scale, 1);
    }

    GY80_single_raw m_read_raw() {
      //★uint8_t* buffer = read(GY80_dev_m, GY80_m_reg_data, 6);
      uint8_t buffer[6];
      Read(GY80_dev_m, GY80_m_reg_data,  buffer, 6);
      
      GY80_single_raw raw;
      raw.x = (buffer[0] << 8) | buffer[1];
      raw.z = (buffer[2] << 8) | buffer[3];
      raw.y = (buffer[4] << 8) | buffer[5];
      return raw;
    }

    GY80_single_scaled m_read_scaled() {
      GY80_single_raw raw = m_read_raw();
      GY80_single_scaled scaled;
      scaled.x = raw.x * m_scale;
      scaled.y = raw.y * m_scale;
      scaled.z = raw.z * m_scale_z;
      return scaled;
    }


    //accelerometer
    void a_set_scale(uint8_t scale) {
      //★uint8_t format = *read(GY80_dev_a, GY80_a_reg_format, 1);
      uint8_t format;
      Read(GY80_dev_m, GY80_m_reg_data,  &format, 1);
            
      scale &= 0x03;
      format &= ~0x0F;
      format |= scale;
      format |= 0x08;
      //★write(GY80_dev_a, GY80_a_reg_format, format);
      Write(GY80_dev_a, GY80_a_reg_format, &format, 1);
    }

    void a_set_bw(uint8_t bw) {
      bw &= 0x0F;
      //★write(GY80_dev_a, GY80_a_reg_bw, bw);
      Write(GY80_dev_a, GY80_a_reg_bw, &bw, 1);
    }

    GY80_single_raw a_read_raw() {
      GY80_single_raw raw;
      //★uint8_t * buf;
      //★buf = read(GY80_dev_a, GY80_a_reg_data, 6); //read the acceleration data from the ADXL345
      uint8_t buf[6];
      Read(GY80_dev_a, GY80_a_reg_data,  buf, 6);
      
      // each axis reading comes in 10 bit resolution, ie 2 bytes.  Least Significat Byte first!!
      // thus we are converting both bytes in to one int
      raw.x = (((int16_t)buf[1]) << 8) | buf[0];
      raw.y = (((int16_t)buf[3]) << 8) | buf[2];
      raw.z = (((int16_t)buf[5]) << 8) | buf[4];
      return raw;
    }

    GY80_single_scaled a_read_scaled() {
      GY80_single_scaled scaled;
      GY80_single_raw raw = a_read_raw();
      scaled.x = raw.x * GY80_a_scale;
      scaled.y = raw.y * GY80_a_scale;
      scaled.z = raw.z * GY80_a_scale;
      return scaled;
    }

    //gyro
    void g_set_scale(uint8_t scale) {
      switch (scale)
      {
        case GY80_g_scale_250:
          g_scale = g_scale_values[0];
          break;
        case GY80_g_scale_500:
          g_scale = g_scale_values[1];
          break;
        case GY80_g_scale_2000:
          g_scale = g_scale_values[2];
          break;
        default:
          scale = GY80_g_scale_2000;
          g_scale = g_scale_values[2];
          break;
      }
      //★write(GY80_dev_g, GY80_g_reg_ctrl4, scale);
      Write(GY80_dev_g, GY80_g_reg_ctrl4, &scale, 1);
      
    }

    GY80_single_raw g_read_raw() {
      uint8_t buffer[6];
      for (uint8_t i = 0; i < 6; i++) {
        //★buffer[i] = *read(GY80_dev_g, GY80_g_reg_datax + i, 1);
        Read(GY80_dev_g, GY80_g_reg_datax,  &buffer[i], 1);
        
      }
      GY80_single_raw raw;
      raw.x = (buffer[1] << 8) | buffer[0];
      raw.y = (buffer[3] << 8) | buffer[2];
      raw.z = (buffer[5] << 8) | buffer[4];
      return raw;
    }

    GY80_single_scaled g_read_scaled() {
      GY80_single_raw raw = g_read_raw();
      GY80_single_scaled scaled;
      scaled.x = raw.x * g_scale;
      scaled.y = raw.y * g_scale;
      scaled.z = raw.z * g_scale;
      return scaled;
    }

    //pressure
    uint32_t p_read_raw() {
      //★uint8_t *buffer;
      uint32_t up = 0;
      // Write 0x34+(OSS<<6) into register 0xF4
      // Request a pressure reading w/ oversampling setting
      //★write(GY80_dev_p, 0xF4, (0x34 + (p_oss << 6)));
      uint8_t data = 0x34 + (p_oss << 6);
      Write(GY80_dev_p, 0xF4, &data, 1);
      
      // Wait for conversion, delay time dependent on OSS
      delay(2 + (3 << p_oss));
      // Read register 0xF6 (MSB), 0xF7 (LSB), and 0xF8 (XLSB)
      //★buffer = read(GY80_dev_p, 0xF6, 3);
      uint8_t buffer[6];
      Read(GY80_dev_p, 0xF6,  buffer, 6);
      
      up = (((unsigned long) buffer[0] << 16) | ((unsigned long) buffer[1] << 8) | (unsigned long) buffer[2]) >> (8 - p_oss);
      return up;
    }

    float p_read_scaled() {
      long x1, x2, x3, b3, b6, p;
      unsigned long b4, b7;
      uint32_t up = p_read_raw();
      b6 = p_calib.b5 - 4000;
      // Calculate B3
      x1 = (p_calib.b2 * (b6 * b6) >> 12) >> 11;
      x2 = (p_calib.ac2 * b6) >> 11;
      x3 = x1 + x2;
      b3 = (((((long)p_calib.ac1) * 4 + x3) << p_oss) + 2) >> 2;
      // Calculate B4
      x1 = (p_calib.ac3 * b6) >> 13;
      x2 = (p_calib.b1 * ((b6 * b6) >> 12)) >> 16;
      x3 = ((x1 + x2) + 2) >> 2;
      b4 = (p_calib.ac4 * (unsigned long)(x3 + 32768)) >> 15;
      b7 = ((unsigned long)(up - b3) * (50000 >> p_oss));
      if (b7 < 0x80000000)
        p = (b7 << 1) / b4;
      else
        p = (b7 / b4) << 1;
      x1 = (p >> 8) * (p >> 8);
      x1 = (x1 * 3038) >> 16;
      x2 = (-7357 * p) >> 16;
      p += (x1 + x2 + 3791) >> 4;
      float temp = ((float)p) / 100000.0;
      return temp;
    }

    float p_read_altitude() {
      float A = p_read_scaled() / 1.01325;
      float B = 1 / 5.25588;
      float C = pow(A, B);
      C = 1 - C;
      C = C / 0.0000225577;
      return C;
    }

    //temperature
    uint16_t t_read_raw() {
      uint16_t ut;
      //★uint8_t *buffer;
      // Write 0x2E into Register 0xF4
      // This requests a temperature reading
      //★write(GY80_dev_p, 0xF4, 0x2E);
      uint8_t data = 0x2E;
      Write(GY80_dev_p, 0xF4, &data, 1);
      
      // Wait at least 4.5ms
      delay(5);
      uint8_t buffer[2];
      //★buffer = read(GY80_dev_p, 0xF6, 2);
      Read(GY80_dev_p, 0xF6,  buffer, 2);
      
      // Read two bytes from registers 0xF6 and 0xF7
      ut = (buffer[0] << 8) | buffer[1];
      return ut;
    }

    float t_read_scaled() {
      uint16_t ut = t_read_raw();
      long x1, x2;
      x1 = (((long)ut - (long)p_calib.ac6) * (long)p_calib.ac5) >> 15;
      x2 = ((long)p_calib.mc << 11) / (x1 + p_calib.md);
      p_calib.b5 = x1 + x2;
      float temp = ((p_calib.b5 + 8) >> 4);
      temp = temp / 10;
      return temp;
    }

  protected:
    void m_init() {
      m_set_scale(GY80_m_scale_8_1);
      m_set_mode(GY80_m_mode_continous);
    }

    void a_init() {
      //★write(GY80_dev_a, GY80_a_reg_pwrctrl, 0x00);
      //★write(GY80_dev_a, GY80_a_reg_pwrctrl, 0x10);
      //★write(GY80_dev_a, GY80_a_reg_pwrctrl, 0x08);
      uint8_t data = 0x00;Write(GY80_dev_a, GY80_a_reg_pwrctrl, &data, 1);
              data = 0x10;Write(GY80_dev_a, GY80_a_reg_pwrctrl, &data, 1);
              data = 0x08;Write(GY80_dev_a, GY80_a_reg_pwrctrl, &data, 1);
      
      a_set_scale(GY80_a_scale_16);
      a_set_bw(GY80_a_bw_12_5);
    }

    void g_init() {
      //★write(GY80_dev_g, GY80_g_reg_ctrl1, 0x0F); //power on, enable x,y,z
      //★write(GY80_dev_g, GY80_g_reg_ctrl2, 0x00);
      //★write(GY80_dev_g, GY80_g_reg_ctrl3, 0x00);
      //★write(GY80_dev_g, GY80_g_reg_ctrl5, 0x00);
      uint8_t data = 0x0F;Write(GY80_dev_g, GY80_g_reg_ctrl1, &data, 1);
              data = 0x00;Write(GY80_dev_g, GY80_g_reg_ctrl2, &data, 1);
              data = 0x00;Write(GY80_dev_g, GY80_g_reg_ctrl3, &data, 1);
              data = 0x00;Write(GY80_dev_g, GY80_g_reg_ctrl5, &data, 1);
      
      g_set_scale(GY80_g_scale_2000);
    }

    void p_init() {
      //Wire.begin();
      //★uint8_t * ptr;
      //★ptr = read(GY80_dev_p, 0xAA, 22);
      uint8_t ptr[22];
      Read(GY80_dev_p, 0xAA,  ptr, 22);
      p_calib.ac1 = (ptr[0] << 8) | ptr[1];
      p_calib.ac2 = (ptr[2] << 8) | ptr[3];
      p_calib.ac3 = (ptr[4] << 8) | ptr[5];
      p_calib.ac4 = (ptr[6] << 8) | ptr[7];
      p_calib.ac5 = (ptr[8] << 8) | ptr[9];
      p_calib.ac6 = (ptr[10] << 8) | ptr[11];
      p_calib.b1 = (ptr[12] << 8) | ptr[13];
      p_calib.b2 = (ptr[14] << 8) | ptr[15];
      p_calib.mb = (ptr[16] << 8) | ptr[17];
      p_calib.mc = (ptr[18] << 8) | ptr[19];
      p_calib.md = (ptr[20] << 8) | ptr[21];
      t_read_scaled();
    }

//    ★void write(uint8_t device, uint8_t address, uint8_t data) {
//      Wire.beginTransmission(device);
//      Wire.write(address);
//      Wire.write(data);
//      Wire.endTransmission();
//    }
  bool Write(uint8_t ID, uint8_t reg, uint8_t* data, uint8_t datasize = 1) {      // datasize:デフォルト引数=1
    Wire.beginTransmission(ID);               // スレーブアドレス・バイト
    Wire.write(reg);                    // インストラクション・バイト
    if (datasize == 1) {
      Wire.write(*data);                  // データ・バイト
#if false//DEBUG
      Serial.print("ID:0x"); Serial.print(ID, HEX); Serial.print(" 1Byte:"); Serial.print(*data);
#endif
    }
    else {
#if false//DEBUG
      Serial.print("ID:0x"); Serial.print(ID, HEX); Serial.print(" anyBytes:");
#endif
      for (uint8_t i = 0; i < datasize; i++) {
        Wire.write(data[i]);              // データ・バイト
        Serial.print(data[i]); Serial.print(",");
      }
    }
#if false//DEBUG
    Serial.println("");
#endif
    uint8_t error = Wire.endTransmission();            // 送信停止
#if DEBUG
    if (error == 0) {
    } else if (error == 4) {
      Serial.print("error 0x");
      if (ID < 16)
        Serial.print("0");
      Serial.println(ID, HEX);
    }
#endif
    return error;
  }

//    ★uint8_t* read(uint8_t device, uint8_t address, uint8_t length) {
//      Wire.beginTransmission(device);
//      Wire.write(address);
//      Wire.endTransmission();
//      //Wire.beginTransmission(device);
//      Wire.requestFrom(device, length);
//      uint8_t buffer[length];
//      while (Wire.available() < length);
//      if (Wire.available() == length)
//      {
//        for (uint8_t i = 0; i < length; i++)
//        {
//          buffer[i] = Wire.read();
//        }
//      }
//      Wire.endTransmission();
//      return buffer;
//    }
  bool Read(uint8_t ID, uint8_t reg, uint8_t* data, uint8_t datasize = 1) {
    Wire.beginTransmission(ID);   // 1.Start I2C transmission
    Wire.write(reg);        // 2.Select data register
    Wire.endTransmission(false);  // 3.Stop I2C transmission
    Wire.requestFrom(ID, datasize); // 4.Request datasize byte of data
    for (int i = 0; i < datasize; i++) {
      data[i] = Wire.read();    // 5.Read 1 byte of data
    }
    uint8_t error = Wire.endTransmission(true);    // 6.Stop I2C transmission
#if DEBUG
    if (error == 0) {
    } else if (error == 4) {
      Serial.print("error 0x");
      if (ID < 16)
        Serial.print("0");
      Serial.println(ID, HEX);
    }
#endif
    return error;
  }

  private:
    float m_scale;
    float m_scale_values[8];
    float m_scale_z;
    float m_scale_values_z[8];
    float g_scale;
    float g_scale_values[3];
    unsigned char p_oss;
    GY80_p_calibration_type p_calib;
};


/*###########################################################*/
/*###########################################################*/
/*###########################################################*/
bool toggle = false;
const int pinLed = 16;
GY80 Sensor = GY80(); //create GY80 instance
void setup() {
  Serial.begin(115200);
  pinMode(pinLed, OUTPUT);
  Sensor.begin();       //initialize sensors
}



void loop() {
  delay(1000);
  toggle = !toggle;
  digitalWrite(pinLed, toggle);
    Sensor.val = Sensor.read_scaled();       //get values from all sensors
    Serial.print("Mag:");                         //magnetometer values
    Serial.print(Sensor.val.m_x,2);
    Serial.print(',');
    Serial.print(Sensor.val.m_y,2);
    Serial.print(',');
    Serial.print(Sensor.val.m_z,2);
    Serial.print(' ');
    Serial.print("Acc:");                         //accelerometer values
    Serial.print(Sensor.val.a_x,3);
    Serial.print(',');
    Serial.print(Sensor.val.a_y,3);
    Serial.print(',');
    Serial.print(Sensor.val.a_z,3);
    Serial.print(' ');
    Serial.print("Gyro:");                        //gyroscope values
    Serial.print(Sensor.val.g_x,1);
    Serial.print(',');
    Serial.print(Sensor.val.g_y,1);
    Serial.print(',');
    Serial.print(Sensor.val.g_z,1);
    Serial.print(' ');
    Serial.print("P:");                           //pressure values
    Serial.print(Sensor.val.p,5);
    Serial.print(' ');
    Serial.print("T:");                           //temperature values
    Serial.println(Sensor.val.t,1);
}
イメージ 1
図11:ESP32プログラム例
 
イメージ 1 イメージ 3
励みになりますのでよければクリック下さい(^o^)/

↩【NOBのArduino日記!】目次に戻る