でむさんのHMC5883L方位取得関数を追試TJ3B

C-Style Ver.20130611でビルドしています。
オリジナルよりコメントを一部変更させていたたきました。
まだ、HMC5883Lの製品を購入していません。どなたかお試しくださいませんか？
オプション　Setupボタンの表示　AdvancedModeチェック　Dir.Sensorチェック
適当にプログラムをビルドしてください。動かなかったらごめんなさい。ここまでしか
力がありません。センサーモニターでも動作を確認できるようです。
書き換えるのは、D_I2C.cだけてす。バックアップを忘れずに自己責任でどうぞ。
もちろんキャリプレーションしてからじゃないと意味ありませんよ。
でむさん、ありがとうごさいます。

D_I2C.cの中の
#include "D_Main.h"
#include "D_I2C.h"
の次の行に
#include "C:\Daisen\mcc18_v337\h\math.h"
の一行を追加する。人によってファイルのある場所が違うかもしれないので
そのときはmath.hのある場所を自分で探して直してください。

D_I2C.cの中の
#ifdef ADV_SETUP
の前の行に次のプログラムを挿入してください。

//マニアの為の園芸教室様やsnafさん様のPIC18F2550ブログのプログラム等を参考
//にさせて頂きました。karabisaa
//------------------------------------------------------------------------------
char read_EE(unsigned char addr)
{
        EEADR = addr;
　　　EECON1bits.EEPGD = 0; //　EEPROMポイント
　　　EECON1bits.CFGS = 0; //　EEPROMアクセス
       EECON1bits.RD = 1;　　　//　EEPROMを読み込む
       return EEDATA;　　　　　//　読み込んだデータを返す
}

void write_EE(unsigned char addr, char data)
{
        EEADR = addr;
       EEDATA = data;
　　　EECON1bits.EEPGD = 0; //　EEPROMポイント
　　　EECON1bits.CFGS = 0; //　EEPROMアクセス
　　　EECON1bits.WREN = 1; //　EEPROMへの書き込みを許可する
　　　INTCONbits.GIE = 0; //　割り込みを禁止する
　　　EECON2 = 0x55;        //　
  　　EECON2 = 0xaa;        //　
　　　EECON1bits.WR = 1; //　EEPROMへ書き込む
        INTCONbits.GIE = 1; //　割り込みを許可する
       EECON1bits.WREN = 0; //　EEPROMへの書き込みを禁止する
}
//------------------------------------------------------------------------------

D_I2C.cの中の

// dno:0(dir), 1(pitch), 2(roll)
UINT get_dir(BYTE dno)
{
U_UINT d;
BYTE adrs = DIR_ADRS;
BYTE n = 0;

d.W = 999;
gI2C_Buf[n++] = 0x20 + dno * 2;
if (i2c_send(adrs, n) == false) return(d.W);
if (i2c_recv(adrs, 2) == false) return(d.W);
n = 0;
d.H = gI2C_Buf[n++];
d.L = gI2C_Buf[n++];
if (0 < dno){
d.W += 90;
}
return(d.W);
}

を次のプログラムに書き換えて上書きして保存してください。


// ハネウェル社の３軸デジタルコンパスモジュール HMC5883L用の方位取得関数
// コンパスモジュールのy軸がロボットの進行方向とし、地面と水平に取り付ける。
// dnoが0(dir)のときだけ方位を計算、ロール、ピッチは計算しないで999を返す。
// 作成者：でむ
#define PI 3.14159
UINT get_dir(BYTE dno) // get_dit_HMC5883L
{
int dir; // 方位
U_UINT d, dx,dy,dz; // 地磁気センサ　x, y, z軸の値を記憶する変数
// 地磁気センサをz軸を中心に360°回転させたときx, y軸は軌跡は楕円となる。
// offset_x, offset_yは楕円の中心
U_UINT offset_x, offset_y;
BYTE adrs_write  = 0x3C;   // HMC5883L I2C address 8bit write
BYTE adrs_read   = 0x3D;   // HMC5993L I2C address 8bit read
static float counter = 0;  // この関数の呼び出される回数を数える変数

offset_x.W = 999, offset_y.W = 999; // 初期値　999はエラー

if ((dno == 1) || (dno == 2)) return 999; // ロール、ピッチは計算しません

// EEPROMから保存していたオフセット値を読み込む
if (counter == 0) {
offset_x.H = read_EE(0x00); // x軸の値
offset_x.L = read_EE(0x01); // x軸の値
offset_y.H = read_EE(0x02); // y軸の値
offset_y.L = read_EE(0x03); // y軸の値
}

// 初期化。連続計測モードに設定。他は標準設定。
if (counter++ < 5) {
gI2C_Buf[0] = 0x02; // モードレジスタ
gI2C_Buf[1] = 0x00; // 連続計測モード
if (i2c_send(adrs_write, 2) == false) return 999;
Delay100TCYx(480);  // 6ms待ち
}

if (i2c_recv(adrs_read, 6) == false) return 999; // I2Cからデータ6バイトの読み込み

dx.H = gI2C_Buf[0]; // BYTE型、実はunsigned char. D_main.hで定義
dx.L = gI2C_Buf[1]; // x軸の値
dz.H = gI2C_Buf[2]; // z軸の値　上位バイト
dz.L = gI2C_Buf[3]; // z軸の値　下位バイト
dy.H = gI2C_Buf[4]; // y軸の値　上位バイト
dy.L = gI2C_Buf[5]; // y軸の値　下位バイト

// 方位計算
if ((offset_x.W == 999) || (offset_y.W == 999))
dir = (int) (atan2((int)dy.W, (int)dx.W) * (180.0/PI));
else
dir = (int) (atan2((int)dy.W - (int) offset_y.W,
                           (int)dx.W - (int) offset_x.W) * (180.0/PI));

if (dir <   0) dir += 360; // 角度を0°～359°にするための処理
if (dir > 360) dir -= 360; // 角度を0°～359°にするための処理

gI2C_Buf[0] = 0x03; // 最初のデータが格納されているデータレジスタ03へ指す
if (i2c_send(adrs_write, 1) == false) return 999; // 999はエラー

Delay1KTCYx(536); //　67ms待つ。HMC5883Lは標準で毎秒15回データ取得

return (UINT) dir;
}