三轴磁场加速度传感器

roboio_SetRBVer（RB_110）; // 使用 RB-110
i2c_Initialize（I2CIRQ_DISABLE）; // 初始化 RB-110 的 I2C 裝置 i2c0_SetSpeed（I2CMODE_FAST, 400000L）; // 設定 I2C 的傳輸速度為 400k
printf（"Z axis :%5d\n", （（d5 & 0x80） != 0） ? （（（~0）>>16）<<16） | （（d5<<8）+d6）: （d5<<8）+d6）;
wait_ms （100）;
i2c0master_StartN（i2c_gsensor_addr, I2C_WRITE, 1）; // 對 ADXL345 取 值
Page: 6/12
圖五 地球磁極與磁力線分佈
計算 Compass 水平時的方位角
接著，我們開始介紹方位角的計算。當 RM-G144 在保持水平的情形下，圖六 的 Yh 軸與 Compass 的 Y 軸是疊合的，因此我們可以從 HMC5843 上讀取 X 軸與 Y 軸磁力，再直接利用 C 語言函式庫裡的 atan2 函數（arctan）做計算，便可以得 到方位角的角度。如下列程式碼所示（注意：本文的方位角是以順時針方向表示， 所以 HMC5843 的 X 軸磁力分量需取負號，才可計算出正確的角度）：
Page: 3/12
printf（"X axis :%5d\n", （（d1 & 0x80） != 0） ? （（（~0）>>16）<<16） | （（d1<<8）+d2）: （d1<<8）+d2）;
printf（"Y axis :%5d\n", （（d3 & 0x80） != 0） ? （（（~0）>>16）<<16） | （（d3<<8）+d4）: （d3<<8）+d4）;
#include <stdio.h> #include <conio.h> #include <roboard.h> #include <windows.h>
void wait_ms（unsigned long ms） { // 針對Windows環境所使用的計時函數（單位 ms）
unsigned long nowtime = GetTickCount（）; while （（GetTickCount（） - nowtime） <= ms）; }
程式執行結果如下圖三所示，電子羅盤所量測到的是在 xyz 軸上的磁力大小
Page: 4/12
（單位：0.5 milli-gauss），加速度計所量測到的是在 xyz 軸上的加速度大小（單 位：4 mg）。
圖三 旋轉 RM-G144 所得到的測量值 利用 RM-G144 計算方位角
在我們可以用 RM-G144 量測到三軸磁力的強度與三軸重力的大小之後，便能 進一歩利用這些數值得到羅盤的方位角。本文中，方位角定義為 Compass 座標系 的 Yh 軸（見圖六，Yh 可視為指南針的指針）與北極所夾的角度，若方位角 0 度 即是正北方，90 度即是正東方，180 度即南方，270 度即是正西方。需要注意的 是，Compass 的座標系與 G-sensor 的座標系是相反的，如圖四所示。
i2c0master_SetRestartN（I2C_READ, 6）; // Restart 後, 讀取6筆 data i2c0master_WriteN（0x32）; // 從暫存器 0x32 開始往下讀值 d1 = i2c0master_ReadN（）; // 讀取X軸的 LSB d2 = i2c0master_ReadN（）; // 讀取X軸的 MSB d3 = i2c0master_ReadN（）; // 讀取Y軸的 LSB d4 = i2c0master_ReadN（）; // 讀取Y軸的 MSB d5 = i2c0master_ReadN（）; // 讀取Z軸的 LSB d6 = i2c0master_ReadN（）; // 讀取Z軸的 MSB // ADXL345回傳值是以 2 的補數法表示，所以負號部分須做符號延伸 // 回傳值範圍 : -512 ~ 511（± 2 g），若加速度超過表示範圍，將會回傳其 最大值
如圖五所示，地球磁場的南北極與地理的南北極是有些偏離的，在這裡我們 所講的南北極是指地磁的南北極，不要跟地理的南北極搞混了。而地磁的南北極 是會随時間而改變的。在 2001 年時，地磁的北極在北緯 81.3 度、西經 110.8 度 的位置。而 2005 年時，地磁的北極轉移到北緯 82.7 度、西經 114.4 度。所以我 們利用 RM-G144 所得到的方位角是指向地磁北極的角度，如果要得到指向地理 北極的角度，需要再進一步的作換算，在此我們不討論這個部份。
int main（void）{ unsigned int d1,d2,d3,d4,d5,d6; unsigned char i2c_compass_addr = 0x1E; // HMC5843 的位址（7 bits address） unsigned char i2c_gsensor_addr = 0x53; // ADXL345 的位址（7 bits address）
printf（"Acc of X-axis :%5d\n", （（d2 & 0x80） != 0） ? （（（~0）>>16）<<16） | （（d2<<8）
+d1）: （d2<<8）+d1）; printf（"Acc of Y-axis :%5d\n", （（d4 & 0x80） != 0） ? （（（~0）>>16）<<16） | （（d4<<8）
RM-G144 簡介
RM-G144 為六軸電子羅盤與加速度計模組，使用一顆量測 3 軸加速度的 ADXL345，以及一顆量測 3 軸磁力場的電子羅盤 HMC5843，模組上除了感測晶片 外，周圍還多加了保護電路，可避免電壓接錯而誤將感測晶片燒毀的情況發生（將 內部電路短路則不在此範圍）。
整個 RM-G144 大小為 2 cm x 2 cm，以 I2C 介面通訊，最高傳輸速度可達 400 kbps，具雙向對接插座，可串聯相同介面的 sensor，並具有 2 個固定孔。電壓 輸入為 DC 5V，下圖一為 RM-G144 外觀，對外連接腳位 1~6 分別是 5V、GND、SCL、 SDA、RESEVERED、RESERVERED。圖二為 RB-110 連接 RM-G144 的方式（紅圈處為 RB-110 的 I2C 輸出埠）。
// ----------------開始讀取測量值----------------// do {
i2c0master_StartN（i2c_compass_addr, I2C_WRITE, 1）; // 對 HMC5843 取 值
i2c0master_SetRestartN（I2C_READ, 6）; // 讀取6筆 i2c0master_WriteN（0x03）; // 從暫存器0x03開始往下讀值 d1 = i2c0master_ReadN（）; // 讀取X軸的 MSB d2 = i2c0master_ReadN（）; // 讀取X軸的 LSB d3 = i2c0master_ReadN（）; // 讀取Y軸的 MSB d4 = i2c0master_ReadN（）; // 讀取Y軸的 LSB d5 = i2c0master_ReadN（）; // 讀取Z軸的 MSB d6 = i2c0master_ReadN（）; // 讀取Z軸的 LSB // HMC5843回傳值是以2的補數法表示，所以負號部分須做符號延伸 // 回傳值範圍：-2048~2047，若外在磁力超過表示範圍，將會回傳-4096
Page: 2/12
i2c0master_StartN（i2c_compass_addr, I2C_WRITE, 2）;// 對 HMC5843 寫入 2 bytes （初始化）
i2c0master_WriteN（0x02）; // 設定模式（暫存器位置為 0x02） i2c0master_WriteN（0x00）; // 設定為 continue-measureture 模式 i2c0master_StartN（i2c_gsensor_addr, I2C_WRITE, 2）; // 對ADXL345寫入2 bytes（初始化） i2c0master_WriteN（0x2d）; // 設定 Pwoer_Control （暫存器位置為0x2d） i2c0master_WriteN（0x28）; // 設定為 link and measure 模式 i2c0master_StartN（i2c_gsensor_addr, I2C_WRITE, 2）; // 對 ADXL345 寫入 2 bytes （初始化） i2c0master_WriteN（0x31）; // 設定 Data_Format （暫存器位置為0x31） i2c0master_WriteN（0x08）; // 設定為 Full_Resolution 模式, 量測範圍為± 2 g, Resolution = 10 bits
+d3）: （d4<<8）+d3）; printf（"Acc of Z-axis :%5d\n", （（d6 & 0x80） != 0） ? （（（~0）>>16）<<16） | （（d6<<8）
+d5）: （d6<<8）+d5）; }while（_getch（） != 27）;