番外 Ｃ言語で方位を調べてみよう

方位センサー

ここでは、parallaxで販売しているHMC5883Lという方位センサーを使ってみます。このデバイスはI2Cプロトコルのデバイスですので「番外　C言語を用いてI2CでIO拡張する」のコントロールなどで理解しておいてください。


I2C言語でHMC5883Lをコントロールする

i2c.c
/* ----------------------------------------------------------------------
    i2c.c
    i2c バス処理
---------------------------------------------------------------------- */
#include "iodefine.h"


/* ----------------------------------------------------------------------
    i2c_checkBusCondition
    i2c バスが空くまで待つ
---------------------------------------------------------------------- */
void i2c_waitBusFree() 
{
    while (IIC2.ICCR2.BIT.BBSY == 1);
}

/* ----------------------------------------------------------------------
    i2c_sendStartCondition
    i2c バスに開始要求を送る
---------------------------------------------------------------------- */
void i2c_sendStartCondition() 
{
    IIC2.ICCR2.BYTE = 0xbd;
}

/* ----------------------------------------------------------------------
    i2c_sendStopCondition
    i2c バスに停止要求を送る
---------------------------------------------------------------------- */
void i2c_sendStopCondition() 
{
    IIC2.ICSR.BIT.STOP = 0; // STOP をクリアする
    IIC2.ICCR2.BYTE = 0x3D; // 停止条件を発行する（BBSY=0, SCP=0）
    while (IIC2.ICSR.BIT.STOP != 1) ;
}

/* ----------------------------------------------------------------------
    i2c_setTransmitMode
    i2c 送受信モードの設定

    0: スレーブ受信モード
    1: スレーブ送信モード
    2: マスタ受信モード
    3: マスタ送信モード
---------------------------------------------------------------------- */
void i2c_setTransmitMode(unsigned char tmode) 
{
  switch (tmode){
    case 0: tmode = 0x00;   // MST=0, TRS=0
        break;
    case 1: tmode = 0x10;   // MST=0, TRS=1
        break;
    case 2: tmode = 0x20;   // MST=1, TRS=0
        break;
    case 3: tmode = 0x30;   // MST=1, TRS=1
        break;
  }
  // レジスタに送受信モードを設定する
  IIC2.ICCR1.BYTE = ((IIC2.ICCR1.BYTE & 0xcf) | tmode);
}

/* ----------------------------------------------------------------------
    i2c_sendByte
    i2c バスへ1バイト送信
---------------------------------------------------------------------- */
unsigned char i2c_sendByte(unsigned char ch) 
{
    unsigned char tend;

    // データを送信する
    IIC2.ICDRT = ch;
    while(1) {
        tend = IIC2.ICSR.BIT.TEND;
        if(tend == 1) break;
    }
    // ACK を返す
    return(IIC2.ICIER.BIT.ACKBR);
}

/* ----------------------------------------------------------------------
    i2c_receiveByte
    i2c バスから1バイト受信
    
    ack: データ受信時相手に返すACKビット
    rcvd: 最終バイトをリードするときは1にするこの後停止条件発行可能になる
          連続してデータ受信する場合は0
---------------------------------------------------------------------- */
unsigned char i2c_receiveByte(unsigned char ack, unsigned char rcvd) 
{
    unsigned char rdt;

    // データ受信後に送信するACKフラグを設定する。
    IIC2.ICIER.BIT.ACKBT = ack;

    // 最終バイト読み込み時は1
    IIC2.ICCR1.BIT.RCVD = rcvd;
    
    // i2c バスから送られるデータの受信開始
    rdt = IIC2.ICDRR;

    // データが8bit転送されるまで待つ
    while(IIC2.ICSR.BIT.RDRF != 1) ;

    // 揃ったデータを取り出す    
    rdt = IIC2.ICDRR;

    return(rdt);
}

/* ----------------------------------------------------------------------
    i2c_init   
    i2c初期化
---------------------------------------------------------------------- */
void i2c_init(void) 
{
    // I2C の転送レートをΦ/200(Φ=20MHz, 100kHz)に設定する
    IIC2.ICCR1.BIT.CKS = 0x0d;

  // IIC バスモードをMSB ファースト、ウェイト挿入しない、
  // ビット数: 9 ビットに設定する
    IIC2.ICMR.BIT.MLS  = 0; // MSB First
    IIC2.ICMR.BIT.WAIT = 0; // WAIT 無し
    IIC2.ICMR.BIT.BCWP = 0; // ビットカウンタライトプロテクト解除
    IIC2.ICMR.BIT.BC = 0;   // 9ビット
    IIC2.ICMR.BIT.BCWP = 1; // ビットカウンタライトプロテクト

  // IIC2 I/F モジュールの動作状態を転送動作可能状態
  // (SCL/SDA は、バス駆動状態)に設定する
    IIC2.ICCR1.BIT.ICE = 1;
}

/*----------------------------------------------------------------
  ここからHMC5883L専用ルーチン
-----------------------------------------------------------------*/


void sendByte(int Device, int address, int data)
{
    unsigned char rdt;

    i2c_waitBusFree();                      // i2c バス空き待ち
    do {
        i2c_setTransmitMode(3);             // マスタ送信モード
        i2c_sendStartCondition();           // i2c バスを開始条件にする
        rdt = i2c_sendByte(Device);         // スレーブアドレスを送信
    } while(rdt == 1);                      // デバイスからACKが返るまでガンバル
    i2c_sendByte(address);                  // デバイス内アドレス番号を送信
    i2c_sendByte(data);                     // 設定する値を送信
    IIC2.ICSR.BIT.TEND = 0;
    i2c_sendStopCondition();                // i2c バスを停止条件にする。
    i2c_setTransmitMode(0);                 // スレーブ受信モードにする
}

int DeviceAddress = 0x3C;

unsigned char readByte(unsigned  reg) {

    unsigned char rdt;

    i2c_waitBusFree();                      // i2c バス空き待ち
    do {
        i2c_setTransmitMode(3);             // マスタ送信モード
        i2c_sendStartCondition();           // i2c バスを開始条件にする
        rdt = i2c_sendByte(DeviceAddress);  // スレーブアドレスを送信
    } while(rdt == 1);                      // デバイスからACKが返るまでガンバル
    i2c_sendByte(reg);                      // レジスタ番号を送信
    i2c_setTransmitMode(3);                 // マスタ送信モード
    i2c_sendStartCondition();               // i2c バスを開始条件にする
    i2c_sendByte(DeviceAddress|1);             // スレーブアドレスを送信
    i2c_setTransmitMode(2);                 // マスタ受信モード
    IIC2.ICSR.BIT.TDRE = 0;
    rdt = i2c_receiveByte(1, 1);            // ACK=1, RCVD = 1で受信する。
    i2c_sendStopCondition();                // i2c バスを停止条件にする。
    IIC2.ICCR1.BIT.RCVD = 0;
    i2c_setTransmitMode(0);                 // スレーブ受信モードにする

    return(rdt);
}

int getData(unsigned func, unsigned param, unsigned char Data[], unsigned n)
{
  int i, n1;
  unsigned char rdt;
  n1 = n-1;
  i2c_waitBusFree();                    // i2c バス空き待ち
  i=0;
  do {
    i2c_setTransmitMode(3);             // マスタ送信モード
    i2c_sendStartCondition();           // i2c バスを開始条件にする
    rdt = i2c_sendByte(DeviceAddress);  // スレーブアドレスを送信
    if(i++ == 100) {                    // 返事が無かったらあきらめる
      i2c_sendStopCondition();          // i2c バスを停止条件にする。
      i2c_setTransmitMode(0);           // スレーブ受信モードにする
      return 1;
    }
  } while(rdt == 1);                    // デバイスからACKが返るまでガンバル
  i2c_sendByte(func);                   // レジスタ番号を送信
  i2c_sendByte(param);
  i2c_sendStartCondition();             // i2c バスを開始条件にする
  i2c_sendByte(DeviceAddress|1);        // スレーブアドレスを送信
  i2c_setTransmitMode(2);               // マスタ受信モード
  for(i=0; i< n; i++) {
    IIC2.ICSR.BIT.TDRE = 0;
    if(i==n1) Data[i] = i2c_receiveByte(1, 1);    // ACK=1, RCVD = 1で受信する。
    else Data[i]  = i2c_receiveByte(0, 0); 
    IIC2.ICCR1.BIT.RCVD = 0;
  }
  i2c_sendStopCondition();                // i2c バスを停止条件にする。
  i2c_setTransmitMode(0);                 // スレーブ受信モードにする
  return 0;
}

int ReadAzimuth() {
	unsigned char data[6];
	int ans,x,y;
	getData(2, 1, data, 6);
	x = data[0]<<8 | data[1];
	y = data[4]<<8 | data[5];
	ans =atan2(x,y);
	return ans;
}

void HMC5883Linit()
{
  i2c_init();                       // I2Cの初期設定
  sendByte(DeviceAddress, 0,0x70);  // 8-average,15Hz default, normal measurement mode
  sendByte(DeviceAddress, 1,0xa0);  // Gain=5
}

 1 
 2 
 3 
 4 
 5 
 6 
 7 
 8 
 9 
10 
11 
12 
13 
14 
15 
16 
17 
18 
19 
20 
21 
22 
23 
24 
25 
26 
27 
28 
29 
30 
31 
32 
33 
34 
35 
36 
37 
38 
39 
40 
41 
42 
43 
44 
45 
46 
47 
48 
49 
50 
51 
52 
53 
54 
55 
56 
57 
58 
59 
60 
61 
62 
63 
64 
65 
66 
67 
68 
69 
70 
71 
72 
73 
74 
75 
76 
77 
78 
79 
80 
81 
82 
83 
84 
85 
86 
87 
88 
89 
90 
91 
92 
93 
94 
95 
96 
97 
98 
99 
100 
101
102
103 
104 
105 
106 
107 
108 
109 
110 
111 
112 
113 
114 
115 
116 
117 
118 
119 
120 
121 
122 
123 
124 
125 
126 
127 
128 
129 
130 
131 
132 
133 
134 
135 
136 
137 内部レジスタ書き込み関数
138 
139 
140 
141 
142 
143 
144 
145 
146 
147 
148 
149 
150 
151 
152 
153 
154 HMC5883Lのデバイスのアドレス
155 
156 内部レジスタの読み込み関数
157 
158 
159 
160 
161 
162 
163 
164 
165 
166 
167 
168 
169 
170 
171 
172 
173 
174 
175 
176 
177 
178 
179 
180 複数データ読み込み関数
181 
182 
183 
184 
185 
186 
187 
188 
189 
190 
191 
192 
193 
194 
195 
196 
197 
198 
199 
200 
201 
202 
203 
204 
205 
206 
207 
208 
209 
210 
211 
212 
213 方位を計測し角度を求める
214 
215 
216 
217 
218 
219 
220 
221 
222 
223 HMC5883Lの初期化
224 
225 
226 
227 
228 
229


整数型での数学関数ルーチン



IntegerMath.c
typedef unsigned char bool;
#define false 0
#define true  1

static int abs(int x) {
  if (x == -32768) x = 0;
  return (x < 0) ? -x : x;
}
  /* x: (0-359)
   * 戻り値 truncated int(100*sin(angle))
   * 範囲　 -100 to +100
   */
  static int sign(int x) {
    return (x == 0) ? 0 : (x<0) ? -1 : 1;
  }
  static int cos(int angle) {
    int qop = angle;
    int div = 101;

    /* perform quadrant conversion */
    /* (input is 0 to 359) */

    if (angle > 90) {
      qop = 180 - angle;
      if (angle < 270) div = -101;
    }
    if (angle > 180) qop = angle - 180;
    if (angle > 270) qop = 360 - angle;

    /* quadratic fit for quadrant */
    return ((10188 - qop * (qop + 23)) / div);
  }
  static int sin(int angle) {
    return cos((angle+270)%360);
  }
  /**
   * arctan(y/x)
   *
   * y: y-value
   * x: x-value (0であってはいけない)
   */
  /* オーバーフローを防ぐため引数は1000以下にする */
  int atan2(int y, int x) {
    int ax;
    int ay;
    int aq=0;
    bool flip = false;

    // 第一象限で計算する
    ax = abs(x);
    ay = abs(y);

    // ax >= ay の場合で近似する
    if (ax < ay) {
      aq = ax;
      ax = ay;
      ay = aq;
      flip = true;
    }

    // 0の値の対処をする */

    if (ay != 0) {
      aq = (30 * ax) / ay;
      if (aq == 0) {
        aq = 90;
      }
      else {
        if (aq >= 82) {
          aq = 1720 / aq;
        }
        else {
          aq = 1720 / (aq + 8);
        }
      }
    }
/*
// こっちの方が精度がいいかも
    if (ay != 0) {
      aq = (40 * ax) / ay;
      if (aq == 0) {
        aq = 90;
      }
      else {
        if (aq >= 114) { // 20度
          aq = 2291 / aq;
        }
        else {
          aq = 2291 / (aq + 10);
        }
      }
    }
*/
    else {
      // atan2(0,0) は駄目
      //if (ax == 0) //aq = aq / 0;
      aq = 0;
    }

    // ひっくり返したものを元に戻す
    if (flip) aq = 90 - aq;

    // 正しい象限に戻す
    if (x < 0) {
      if (y < 0) {
        // 第三象限
        aq = 180 + aq;
      }
      else {
        // 第二象限
        aq = 180 - aq;
      }
    }
    else {
      if (y < 0) aq = 360 - aq;
    }

    // 0～359の間にする
    return (aq % 360);
}

 1 
 2 
 3 
 4 
 5 
 6 
 7 
 8 
 9 
10 
11 
12 
13 
14 
15 
16 
17 
18 
19 
20 
21 
22 
23 
24 
25 
26 
27 
28 
29 
30 
31 
32 
33 
34 
35 
36 
37 
38 
39 
40 
41 
42 
43 
44 
45 
46 
47 
48 
49 
50 
51 
52 
53 
54 
55 
56 
57 
58 
59 
60 
61 
62 
63 
64 
65 
66 
67 
68 
69 
70 
71 
72 
73 
74 
75 
76 
77 
78 
79 
80 
81 
82 
83 
84 
85 
86 
87 
88 
89 
90 
91 
92 
93 
94 
95 
96 
97 
98 
99 
100 
101
102
103 
104 
105 
106 
107 
108 
109 
110 
111 
112 
113 
114 
115 
116 
117 
118 
119 
120


メインプログラム


main.c
#include "vs-wrc003.h"

void startInit(char* str)
{
  const BYTE MainCycle = 100;
  Init((BYTE)MainCycle);  //初期設定
  InitSci3(CBR_115200,non,1);
  Mtr_Run(0, 0, 0, 0);
  while(!getSW());
  printf(str);
}

void main(void)
{
  int degree;
  startInit("*** HMC5883L test program ***\n");
  HMC5883Linit();	
  while(1)
  {
    degree= ReadAzimuth();
    printf("%d\n", degree);
    LED(1);
    Wait(50);
    LED(2);
    Wait(50);
  }
}

 1 
 2 
 3 
 4 
 5 
 6 
 7 
 8 
 9 
10 
11 
12 
13 
14 
15 
16 
17 HMC5883Lの初期化
18 
19 
20 方位を測定
21 表示
22 
23 
24 
25 
26 
27 
28