STM32F3 Discoveryで電子コンパスを作ってみよう。

-
STM32 Eclipse開発環境で実際にマイコン・プログラミングをしてみましょう。

今回は、STM32F3 Discovery を使った電子コンパスを作成します。

STM32F3 Discovery には、STマイクロ製のLSM303DLHCというセンサーが搭載されています。
この、LSM303DLHCはSTM32F3とI2Cで接続されており、3軸の地磁気と加速度を検出することができます。

また、基板上には方角を示すような８つのLEDも搭載されています。

今回は、LSM303DLHCの地磁気センサーとLEDを使って簡単な電子コンパスを作成してみようと思います。

LSM303DLHCの地磁気センサーの使い方


LSM303DLHCの地磁気センサーは、３軸に対する地磁気を検出します。

この磁気センサーは各軸と地磁気の磁束密度を検出するようにできています。
このため、各軸が北を向いたときに最大値となり、逆（南）を向いた時はマイナスの最大値となります。

3つの軸から北を知るには、XYZの3軸で検出された磁束密度の強さをベクトル合成したものが北となります。


STM32F3 Discoveryボード上のLSM303DLHCをよく見ると、１番ピンが下向きにマウントされています。


このため、ボードの上側（USB給電/デバッグポート側）を北に向けたときに、X軸がマイナス値の最大となります。

今回の実装仕様


実装を簡単にするため、XとYだけを使うこととします。
つまり、ボードを水平にした状態での北向き検出します。

この状態で”北”と判断した方角のLEDを点灯し、他は消灯します。

計算方法


LEDは8つしかありません。
従って、LED間の角度は45です。
磁気センサーは、軸が南方向を向くと取得値がマイナスとなるので、数値の正負だけで4方向が特定できます。
このため、数値の正負で4方向を特定し、取得値の絶対値でベクトルを算出してCOS値から詳細な角度を出せばよいのですが、1方向のLEDは0度と45度と90度の3つしかないので、3つのLEDの間、22.5度（0.9238795325...）と67.5度（0.3826834323...）だけ判定できればよいことになります。

I2C通信方法


1つのデバイスですが、地磁気と加速度センサーは別々のI2Cアドレスを持っています。
地磁気センサーは書き込みが0x3C、読み込みが0x3Dです。

地磁気センサーを設定します。レジスタ、0x00に0x0C、0x01に0x20、0x02に0x00を書き込みます。
計測値は、0x03～0x08の連続したレジスタがそれぞれ、XH、XL、ZH、ZL、YH、YLになります。

読み込みは先頭の0x03を指定して6バイトを連続で読み出すことができます。

プロジェクトの生成

STM32 CubeMX を使ってプロジェクトを生成します。
デフォルトの STM32F3 Discovery 設定としてもよいですが、今回必要なPINは下記のとおりです。

　・I2C(Connectivity I2C1 enabled)
　　　PB7 --> I2C1 SDA
　　　PB6 --> I2C1 SCL

　・LED(GPIO Output)
　　　PB8 --> LD4 Blue LED
　　　PB9 --> LD3 Red LED
　　　PB10 --> LD5 Orange LED
　　　PB11 --> LD7 Green LED
　　　PB12 --> LD9 Blue LED
　　　PB13 --> LD10 Red LED
　　　PB14 --> LD8 Orange LED
　　　PB15 --> LD6 Green LED

プロジェクトタブでプロジェクト名をつけて、
Toolchain/IDEにSW4STM32を選択、Generate Under Rootのチェックを外します。
"GENERATE CODE"を実行してプロジェクトを生成してください。

C/C++パースペクティブなどでプロジェクトエクスプローラーに生成したプロジェクトができていることを確認してください。

コード記述


プロジェクトエクスプローラーから、作成したプロジェクトを開きます。
Application --> User --> main.c を選択、ダブルクリックで開きます。
このソースに対して、コードを追加します。

DefineでCOS値を設定
#define COS_1 0.38268343f
#define COS_2 0.92387953f

I2Cコマンド定義
uint8_t mag_init1[] = {0x00, 0x0C};
uint8_t mag_init2[] = {0x01, 0x20};
uint8_t mag_init3[] = {0x02, 0x00};
uint8_t mag_data[] = {0x03};

LSM303DLHC地磁気センサー設定 メインループの前で、センサーを設定します。
HAL_I2C_Master_Transmit(&hi2c1, 0x3C, mag_init1, 2, 1000);
HAL_I2C_Master_Transmit(&hi2c1, 0x3C, mag_init2, 2, 1000);
HAL_I2C_Master_Transmit(&hi2c1, 0x3C, mag_init3, 2, 1000);

メインループ

  uint8_t buff[6];      // 受信バッファ
  short X, Y, Z;        // 受信データHL結合用
  int ix, iy;           // 取得値の正負
  float xf, yf, l, q;   // 計算用

  while (1) {
      X = Y = Z = 0;

      memset(buff, 0x00 , sizeof(buff));
      HAL_I2C_Master_Transmit(&hi2c1, 0x3C, &mag_data[0], 1, 1000);
      HAL_I2C_Master_Receive(&hi2c1, 0x3D, &buff, 6, 1000);
      X = (int)buff[0] << 8;
      X |= (int)buff[1];
      Z = (int)buff[2] << 8;
      Z |= (int)buff[3];
      Y = (int)buff[4] << 8;
      Y |= (int)buff[5];

      if(X < 0) {
          ix = 1;
      }
      else {
          ix = 0;
      }
      xf = fabsf((float)X);

      if(Y < 0) {
          iy = 1;
      }
      else {
          iy = 0;
      }
      yf = fabsf((float)Y);


      l = sqrtf(powf(xf, 2.0) + powf(yf, 2.0));
      q = xf / l;

      if(ix) {
          HAL_GPIO_WritePin(GPIOE, GPIO_PIN_12 | GPIO_PIN_13 | GPIO_PIN_14, GPIO_PIN_RESET);
          if(iy) {
              HAL_GPIO_WritePin(GPIOE, GPIO_PIN_10 | GPIO_PIN_11, GPIO_PIN_RESET);
              if(q < COS_1) {
                  HAL_GPIO_WritePin(GPIOE, GPIO_PIN_15, GPIO_PIN_SET);
                  HAL_GPIO_WritePin(GPIOE, GPIO_PIN_8 | GPIO_PIN_9, GPIO_PIN_RESET);
              }
              else if (q > COS_2) {
                  HAL_GPIO_WritePin(GPIOE, GPIO_PIN_9, GPIO_PIN_SET);
                  HAL_GPIO_WritePin(GPIOE, GPIO_PIN_8 | GPIO_PIN_15, GPIO_PIN_RESET);
              }
              else {
                  HAL_GPIO_WritePin(GPIOE, GPIO_PIN_8, GPIO_PIN_SET);
                  HAL_GPIO_WritePin(GPIOE, GPIO_PIN_9 | GPIO_PIN_15, GPIO_PIN_RESET);
              }
          }
          else {
              HAL_GPIO_WritePin(GPIOE, GPIO_PIN_8 | GPIO_PIN_15, GPIO_PIN_RESET);
              if(q < COS_1) {
                  HAL_GPIO_WritePin(GPIOE, GPIO_PIN_11, GPIO_PIN_SET);
                  HAL_GPIO_WritePin(GPIOE, GPIO_PIN_9 | GPIO_PIN_10, GPIO_PIN_RESET);
              }
              else if (q > COS_2) {
                  HAL_GPIO_WritePin(GPIOE, GPIO_PIN_9, GPIO_PIN_SET);
                  HAL_GPIO_WritePin(GPIOE, GPIO_PIN_10 | GPIO_PIN_11, GPIO_PIN_RESET);
              }
              else {
                  HAL_GPIO_WritePin(GPIOE, GPIO_PIN_10, GPIO_PIN_SET);
                  HAL_GPIO_WritePin(GPIOE, GPIO_PIN_9 | GPIO_PIN_11, GPIO_PIN_RESET);
              }
          }
      }
      else {
          HAL_GPIO_WritePin(GPIOE, GPIO_PIN_8 | GPIO_PIN_9 | GPIO_PIN_10, GPIO_PIN_RESET);
          if(iy) {
              HAL_GPIO_WritePin(GPIOE, GPIO_PIN_11 | GPIO_PIN_12, GPIO_PIN_RESET);
              if(q < COS_1) {
                  HAL_GPIO_WritePin(GPIOE, GPIO_PIN_15, GPIO_PIN_SET);
                  HAL_GPIO_WritePin(GPIOE, GPIO_PIN_13 | GPIO_PIN_14, GPIO_PIN_RESET);
              }
              else if (q > COS_2) {
                  HAL_GPIO_WritePin(GPIOE, GPIO_PIN_13, GPIO_PIN_SET);
                  HAL_GPIO_WritePin(GPIOE, GPIO_PIN_14 | GPIO_PIN_15, GPIO_PIN_RESET);
              }
              else {
                  HAL_GPIO_WritePin(GPIOE, GPIO_PIN_14, GPIO_PIN_SET);
                  HAL_GPIO_WritePin(GPIOE, GPIO_PIN_13 | GPIO_PIN_15, GPIO_PIN_RESET);
              }
          }
          else {
              HAL_GPIO_WritePin(GPIOE, GPIO_PIN_14 | GPIO_PIN_15, GPIO_PIN_RESET);
              if(q < COS_1) {
                  HAL_GPIO_WritePin(GPIOE, GPIO_PIN_11, GPIO_PIN_SET);
                  HAL_GPIO_WritePin(GPIOE, GPIO_PIN_12 | GPIO_PIN_13, GPIO_PIN_RESET);
              }
              else if (q > COS_2) {
                  HAL_GPIO_WritePin(GPIOE, GPIO_PIN_13, GPIO_PIN_SET);
                  HAL_GPIO_WritePin(GPIOE, GPIO_PIN_11 | GPIO_PIN_12, GPIO_PIN_RESET);
              }
              else {
                  HAL_GPIO_WritePin(GPIOE, GPIO_PIN_12, GPIO_PIN_SET);
                  HAL_GPIO_WritePin(GPIOE, GPIO_PIN_11 | GPIO_PIN_13, GPIO_PIN_RESET);
              }
          }
      }
  }

なんだかいろいろやっているように見えますが、LEDのON/OFFのコードがほとんどです。

使い方


STM32F3 Discoveryボードを水平に持つだけです。
LEDが大体の北を示します。
Z軸は無視しているので、ボードを立てると正しく検出できなくなります。
8つのLEDの表現でZ軸も表すのものムリかと。。。

何に使えるか？


普通にコンパスとして使う以外の使いみちとして、
・太陽光パネルの方角制御
・ロボットなどおおよその向きを知るために使う。
・鬼門除け
など、方角が関わることに利用できます。

最後に

このコンパスを使って登山しようと思う人はいないと思いますが。。。。

このコンパスはLED表示解像度が低いこともあり、２つのCOS値だけで作成しています。
従って精度を上げるには、詳細な三角関数表を持つか、毎回ACOS値を求めるなど必要になります。
また、実際の磁極は真北ではないためさらに精度を上げるには補正が必要となりますが、磁極のズレに対応できるところはマイコン・コンパスならではと言えますね。





Have a Happy Hucking!!

 Lightning Brains

コメント

コメントを投稿

このブログの人気の投稿

Linuxシステムコール、共有メモリの使い方

-
UNIX / Linux システムコール・プログラミング プロセス間通信(IPC)、共有メモリ 共有メモリとは？ カーネルが管理するメモリ空間をユーザープロセスに ”見えるようにする” ことで、複数のプロセス間で同じメモリ空間を利用することができます。 プロセスからは通常のメモリと同様に見えるため、同じエリアに対しての同時書き込みなどについては、排他制御を行う、あるいはアトミックな操作とするなどの対応が必要になります。 共有メモリは SYSTEM V IPC です。 それでは、早速サンプルコードについて解説します。 今回のコードは2つのプログラムで構成されています。 shm_r.c 共有メモリは、shmget()システムコールにより共有メモリを取得します。このときの引数の指定で”ない場合は作成する”、パーミッションを与えるなどの指定を行います。生成された共有メモリにアクセスするため、shmat()システムコールでポインタを得るます。 以降、このプロセスはshmat()システムコールで得たポインタを通じ、自由に共有メモリにアクセスすることができます。 この、サンプルshm_r.cは、共有メモリを生成・アタッチすると１秒毎にメモリの更新をチェックし、更新された場合は内容を表示します。このときの更新内容が "Quit"であった場合、共有メモリをデタッチし、共有メモリを破棄します。 #include <stdio.h> #include <stdlib.h> #include <string.h> #include <sys/types.h> #include <sys/ipc.h> #include <sys/shm.h> #include <unistd.h> #define DATA_SIZE 2048 int main(void) { int id; char *shmData; char data[DATA_SIZE]; // Shared memory create a new with IPC_CREATE if((id = shmget(IPC_PRIVATE, DA
続きを読む

Linuxシステムコール、メッセージキューの使い方

-
UNIX / Linux システムコール・プログラミング プロセス間通信(IPC)、メッセージキュー メッセージキューとは？ いわゆるメッセージボックスのような動作を行う IPC です。 プロセス間で一意に識別できるメッセージキューに対してメッセージを書き込み、受け取りができます。 また、メッセージキューは SYSTEM V IPC です。 メッセージキューを使うためには、まずメッセージキューを作成してユニークなキューIDを作成する必要があります。msgget()というシステムコールでキューを作成します。 メッセージを送信するためには msgsnd()、メッセージを受信するためには msgrcv()、最後にメッセージキューを削除するために msgctl() を使います。 メッセージの受信側プロセスのサンプルコードを下記に示します。 #include <sys/types.h> #include <stdio.h> #include <sys/ipc.h> #include <sys/msg.h> #include <string.h> #include <unistd.h> #define MSGBUFSIZ 256 int main(int argc, char** argv) { int msqid; key_t msgkey; struct msgbuf{ long mtype; char mdata[MSGBUFSIZ]; }; struct msgbuf msgdata,*p; p = &msgdata; // Make Message queue key. msgkey = ftok("msgq.key",'X'); // Get Message queue ID. msqid = msgget(msgkey,IPC_CREAT|0666); // Message receive loop wile get "Quit" massage. while(1) { // Receive a Message.
続きを読む

Linuxシステムコール、セマフォの使い方

-
イメージ
UNIX / Linux システムコール・プログラミング プロセス間通信(IPC)、セマフォ セマフォとは？ セマフォ、聞き慣れない言葉ですが大昔の鉄道の信号機のことです。 単線区間で列車同士が衝突しないようにするため、信号機が使われました。 意味合い的には同様で、コンピューター内のある資源（メモリなど）に別々のプロセスが同時にアクセスしないようにするために利用します。 セマフォはSYSTEM V IPC です。 この、セマフォを利用することで複数のプロセスが同期して処理を進めることができます。 Linuxのセマフォは、セマフォ集合として生成して利用します。 このため、1つのセマフォ集合に複数のセマフォを含めることができ、Linuxのセマフォはカウンティングセマフォであるため、複数の許可を与えることも可能です。 ただし、この記事のサンプルコードは1つの集合に1つだけのセマフォ、許可数は1つだけとなっています。 まず、セマフォ生成のサンプルコードを示します。 seminit.c #include <sys/types.h> #include <sys/ipc.h> #include <sys/sem.h> #include <stdio.h> #include <stdlib.h> union semun { int val; struct semid_ds *buf; unsigned short *array; }; int main() { key_t key; int semid; union semun arg; if ((key = ftok("sem.dat", 'S')) == -1) { perror("ftok"); exit(1); } /* create a semaphore set with 1 semaphore: */ if ((semid = semget(key, 1, 0666 | IPC_CREAT)) == -1) { p
続きを読む