e-gadgetやTJ3で地磁気センサHMC5883Lを使う方法2/3（キャリブレーション）

ロボカップジュニアで良く使われているダイセン電子工業のe-gadgetやTJ3でハネウエルの地磁気センサモジュールHMC5883Lを使えるようにしましたので紹介します。PICを始めて使ったので間違い等があるかもしれません。ご使用される場合は自己責任でお願いします。試合に負けたり、損害や被害があった場合など、私は保証できません。

次の３ステップが必要です。
１． セルフテスト
２. キャリブレーション
３． 実際に使用する

まず、ここではHMC5883Lのキャリブレーションの方法を説明します。以下のget_dir_cal関数をc:\Daisen\C-Style for e-Gadget\Build\Build_V120911\D_I2C.cのUINT get_dir(BYTE dno)と置き換えて保存してください。もとのget_dir関数は消さないでget_dir_org()などと名前を変えると良いでしょう。

なお、ここでのキャリブレーションは、地磁気センサx, y軸のセンサ値をブロットしてできる楕円の中心をオフセットしています。具体的にはｘ, y軸とも（最大値+最小値)/2としています。本来はノイズなどがあるので中間値（メディアン）を取る方が望ましいのですが、PICで大きな配列を簡単に使えなかったので、簡単な方法をとっています。また、楕円のままだと、方位の精度にばらつきがでるので円に変換すると更に良くなると思います。

また、数学関数atan2を使っているので以下をこのファイル(D_I2C.c)の28行目#include “D_I2C.hの下に次の１行を追加してください。
#include “../../../mcc18_v337/h/math.h”

後は、C-styleを使いビルドし、ロボットへダウンロードして、STARTボタンを動かすとロボットが２回程度回転し、オフセット値となる地磁気センサx, y軸の値の平均をEEPROMに書き込み、キャリブレーションが終わると成功するとSUCCCEEDEDと表示され、失敗するとFAILEDと表示され、続いてPOWER OFFと表示されるので電源を切ってください。

次回は実際にセンサを使ってみましょう。

// HMC5883L用のキャリブレーション関数 by でむ　UINT get_dir_cal(BYTE dno)
// 実行するときはこの関数名をget_dirに変更し、ビルドして生成されて実行ファイル(Hex)
// を自分の好きな名前に例えばHMC5883L.Hexに変更して保存する。
// C-styleのダウンロードボタンを押し、HMC5883L.Hexを選び、ロボットにダウンロードして、
// 実行するれば。ロボット(e-gadget-TT, e-gadget-RB)はその場回転してキャリブレーションを
// 行いパラメータをEEPROMに保存して終了する。キャリブレーションが成功したときは
// LCD画面に"SUCCEEDED", 失敗したときは"FAILED"と表示して終了する。
// キャリブレーションは場所が変わる毎に１回実施すること。
// 制作者：でむ
// なお、関数readEE(), writeEE()は次の書籍のＰ473と同じなので省略しています。
// 標準的なEEPROMへの読み書きの関数です。詳細は以下の資料をご覧ください。
// 改定版　電子工作のためのPIC18F 本格活用ガイド
// 後閉哲也著、技術評論社　ISBN978-4-7741-3449-9
UINT get_dir_cal(BYTE dno) // キャリブレーション HMC5883L用 get_dir_cal
{
	int speed = 8; // ロボットのスピード。適宜変更すること。
	int steps = 300; // キャリプレーションのループ数。上と合わせて適宜変更すること。
	int dir, i, data_no;
	int x_min = 1000, x_max = -1000, y_min = 1000, y_max = -1000;
	int success = 1;

	U_UINT dx,dy,dz;
	// 地磁気センサを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; // HMC5883L I2C address 8bit read　変更不可
	float ave_x, ave_y;
	static float counter = 0;

	if ((dno == 1) || (dno == 2)) return 999;

	dx.W = 999; dy.W = 999;
	offset_x.W = 999, offset_y.W = 999;

	if (counter == 0) {
		offset_x.H = readEE(0x00);
		offset_x.L = readEE(0x01);
		offset_y.H = readEE(0x02);
		offset_y.L = readEE(0x03);
		lcd_putX(1, (ROMC *) "READ OFFSET"); 
		myprintLCD2(2, offset_x.W, offset_y.W);
		wait_ms(3000); // wait 3s
	}

	motor(-speed, speed);      // その場回転

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

	set_Led(2,1);
	lcd_putX(1, (ROMC *) "CALIB. BEGIN"); 
    for (i = 0; i < steps; i++)
	{
		
		if (i2c_recv(adrs_read, 6) == false) {
			success = 0;
			break;
		}

		dx.H = gI2C_Buf[0]; // BYTE型、実はunsigned char. D_main.hで定義
		dx.L = gI2C_Buf[1];
		dz.H = gI2C_Buf[2];
		dz.L = gI2C_Buf[3];
		dy.H = gI2C_Buf[4];
		dy.L = gI2C_Buf[5];

		if (dx.W == 999) continue;
		if (dy.W == 999) continue;
		if ((int) dx.W < x_min) x_min = (int) dx.W;
		if ((int) dx.W > x_max) x_max = (int) dx.W;
		if ((int) dy.W < y_min) y_min = (int) dy.W;
		if ((int) dy.W > y_max) y_max = (int) dy.W;

		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;
		if (dir > 360) dir -= 360;
	
		// LCDに値を表示。デバッグ用。
		myprintLCD(2, dir);
		// myprintLCD2(2, dx.W, dy.W);

		// ログを取る場合はコメントを外す。デバッグ用。
		// log_serial((int) dx.W, (int) dy.W);
	 
		gI2C_Buf[0] = 0x03; // 最初のデータが格納されているデータレジスタ03へ指す
		if (i2c_send(adrs_write, 1) == false) {
			success = 0;
			break;
		}

		// ログを取る場合はコメントを外す。デバッグ用。
		log_serial((int) dx.W, (int) dy.W);

		Delay1KTCYx(536*2); //　67×2ms待つ。HMC5883Lは標準で毎秒15回データ取得
	}
	motor(0, 0);

	ave_x = (x_min + x_max)/2;
	ave_y = (y_min + y_max)/2;

	set_Led(2,0);

	//myprintLCD2(2, ave_x, ave_y);
	// 最小値、最大値、平均の表示
	myprintLCD3(1, x_min, x_max, ave_x); // X軸成分
	myprintLCD3(2, y_min, y_max, ave_y); // Y軸成分

	wait_ms(1000);
	offset_x.W = ave_x;
	offset_y.W = ave_y;

	writeEE(0x00, offset_x.H);
 	Delay10KTCYx(40); // 50ms
	writeEE(0x01, offset_x.L);
	Delay10KTCYx(40); // 50ms
	writeEE(0x02, offset_y.H);
	Delay10KTCYx(40); // 50ms
	writeEE(0x03, offset_y.L);

	// 電源を切れとLCDに表示
	while (1) { // 無限ループ
		set_Led(2,1);
		lcd_putX(1, (ROMC *) "   CALIBRATION"); 
		if (success == 1) lcd_putX(2, (ROMC *) "*** SUCCEEDED ***");
 		else              lcd_putX(2, (ROMC *) "***   FAILED  ***");

		wait_ms(1000);
		set_Led(2,0);
		lcd_putX(1, (ROMC *)  "*** POWER OFF ***"); 
		myprintLCD2(2, (int) offset_x.W, (int) offset_y.W); 
		wait_ms(1000);
	}

	return (UINT) dir;
}