บทความนี้เป็นการอธิบายการใช้งานเซ็นเซอร์เข็มทิศดิจิทัลรุ่น GY-271 ด้วยภาษาไพธอนของ MicroPython เพื่อใช้งานกับ ESP8266 หรือ ESP32 (และได้ทดลองกับ STM32F411CEU6 กับ Raspberry Pi 3B+ และ 4B แล้วสามารถใช้งานได้เหมือนกัน) เพื่อตั้งค่าการทำงานและอ่านค่าแกน X,Y และ Z จากเซ็นเซอร์ หลังจากนั้นนำมาคำนวณเป็นค่าองศาของทิศเหนือ
เซ็นเซอร์ GY-271
GY-271 (ดังภาพที่ 1) เป็นเซ็นเซอร์เข็มทิศดิจิทัลแบบ 3 แกน (3-Axis Digital Compass) ที่ใช้ไอซีเบอร์ QMC5883L ที่เป็นเซ็นเซอร์วัดสนามแม่เหล็กและเข็มทิศ ทำงานด้วยความเร็วสูงสุด 160Hz เชื่อมต่อผ่านบัส I2C มีความละเอียดในการแปลงสัญญาณแอนาล็อกเป็นดิจิทัลที่ 16 บิต ใช้แรงดัน 2.16 – 3.6VDC กินกระแส 75/100uA,150/250uA, 250/450uA และ450/850uA ที่10Hz, 50Hz, 100Hz และ 200Hz และสามารถทำงานได้ที่อุณหภูมิ -40 ถึง 80 องศาเซลเซียส
เรจิสเตอร์ของ QMC5883L
QMC5883L มีเรจิสเตอร์สำหรับสั่งงานและอ่านค่าดังนี้
| ตำแหน่ง | หน้าที่ |
|---|---|
| 0x00 | ไบต์ล่างของค่าแกน X (LSB of X-Axis) |
| 0x01 | ไบต์บนของค่าแกน X (MSB of X-Axis) |
| 0x02 | ไบต์ล่างของค่าแกน Y (LSB of Y-Axis) |
| 0x03 | ไบต์บนของค่าแกน Y (MSB of Y-Axis) |
| 0x04 | ไบต์ล่างของค่าแกน Z (LSB of Z-Axis) |
| 0x05 | ไบต์บนของค่าแกน Z (MSB of Z-Axis) |
| 0x09 | เรจิสเตอร์ควบคุม1 |
| 0x0A | เรจิสเตอร์ควบคุม2 |
| 0x0B | เรจิสเตอร์ตั้งค่า/ล้างค่าคาบเวลา (Set/Reset Period Register) |
หมายเหตุ
จากเอกสารของ QMC5883L แนะนำให้ตั้งค่าของเรจิสเตอร์ตำแหน่ง 0x0B เป็น 0x01
เรจิสเตอร์ควบคุม1
เรจิสเตอร์ควบคุม 1 ประกอบไปด้วย 4 เรจิสเตอร์ดังนี้
| Bit No. | Register |
|---|---|
| 0..1 | Mode |
| 2..3 | ODR (Output Data Rate) |
| 4..5 | RNG (Fill scale) |
| 6..7 | OSR (Over Sample Ratio) |
โดยค่าเป็นดังนี้
| การตั้งค่า | ค่าในเลขฐาน 2 |
|---|---|
| Mode_Standby | 0b00000000 |
| Mode_Continuous | 0b00000001 |
| ODR_10Hz | 0b00000000 |
| ODR_50Hz | 0b00000100 |
| ODR_100Hz | 0b00001000 |
| ODR_200Hz | 0b00001100 |
| RNG_2G | 0b00000000 |
| RNG_8G | 0b00010000 |
| OSR_512 | 0b00000000 |
| OSR_256 | 0b01000000 |
| OSR_128 | 0b10000000 |
| OSR_64 | 0b11000000 |
เรจิสเตอร์ควบคุม2
ประกอบด้วยเรจิสเตอร์ดังต่อไปนี้
| Bit No. | Register |
|---|---|
| 0 | INT_ENB |
| 6 | Rol_PNT |
| 7 | Soft_RS |
การเชื่อมต่อ
การเชื่อมต่อผ่านบัส I2C ดังภาพที่ 3 และโมดูลเซ็นเซอร์มีค่าตำแหน่งสำหรับการทำงาน 0x0D โดยทำการเชื่อมต่อกับ ESP8266 ดังนี้
| GY-271 | ESP8266 |
|---|---|
| Vcc | 3V3 |
| GND | GND |
| SDA | D2 |
| SCL | D1 |
การอ่านค่า
วิธีการอ่านค่ากระทำโดยการอ่านจากตำแหน่ง 0x00 โดยเรียงจากไบต์ต่ำและไบต์สูงของแกน X, Y และ Z ดังนี้
buffer = i2c.readfrom_mem(QMC5883L_ADDR,RegXLo,6)
xLo = buffer[0]
xHi = buffer[1] <<8
yLo = buffer[2]
yHi = buffer[3] <<8
zLo = buffer[4]
zHi = buffer[5] <<8
x = xLo+xHi
y = yLo+yHi
z = zLo+zHiการแปลงค่า
การแปลงค่าจากค่าของแกน X,Y และ Z ให้เป็นทิศที่ทิศเหนืออยู่กระทำดังนี้
heading = math.atan2(y, x)
heading = heading + declinationAngle
#Due to declination check for >360 degree
if(heading > 2*pi):
heading = heading - 2*pi
#check for sign
if(heading < 0):
heading = heading + 2*pi
#convert into angle
headingAngle = (heading * 180/pi)ตัวอย่างโปรแกรม
จากการแปลงโค้ด C++ ของ e-Gizmo Mechatronix Central ซึ่งเป็น C++ สำหรับ Arduino มาเป็นภาษาไพธอนสำหรับ MicroPython ได้ดังนี้
###############################################################################
# GY-271/QMC5883L
# (C) 2021, JarutEx
# Ref: https://github.com/e-Gizmo/QMC5883L-GY-271-Compass-module
###############################################################################
import machine as mc
import sys
import math
from time import sleep #import sleep
pinSDA = mc.Pin(4)
pinSCL = mc.Pin(5)
QMC5883L_ADDR = 0x0D
i2c = mc.I2C(freq=2000000, scl=pinSCL, sda=pinSDA)
devices = i2c.scan()
if not (QMC5883L_ADDR in devices):
print("Not found GY-271 (QMC5883L)!")
sys.exit(1)
############## Register Location
RegCTRL1 = 0x09 # Control Register--> MSB(OSR:2,RNG:2,ODR:2,MODE:2)LSB
RegCTRL2 = 0x0A # Control Register2--> MSB(Soft_RS:1,Rol_PNT:1,none:5,INT_ENB:1)LSB
RegFBR = 0x0B # SET/RESET Period Register--> MSB(FBR:8)LSB
RegXLo = 0x00
RegXHi = 0x01
RegYLo = 0x02
RegYHi = 0x03
RegZLo = 0x04
RegZHi = 0x05
############## Cpntrol Register Value
Mode_Standby = 0b00000000
Mode_Continuous = 0b00000001
ODR_10Hz = 0b00000000
ODR_50Hz = 0b00000100
ODR_100Hz = 0b00001000
ODR_200Hz = 0b00001100
RNG_2G = 0b00000000
RNG_8G = 0b00010000
OSR_512 = 0b00000000
OSR_256 = 0b01000000
OSR_128 = 0b10000000
OSR_64 = 0b11000000
declinationAngle = 0.0404
pi = 3.14159265359
########### Init
ctrl1 = bytearray([Mode_Continuous|ODR_200Hz|RNG_8G|OSR_512])
i2c.writeto_mem(QMC5883L_ADDR,RegCTRL1, ctrl1)
i2c.writeto_mem(QMC5883L_ADDR,RegFBR, b'\x01')
########### Read
buffer = i2c.readfrom_mem(QMC5883L_ADDR,RegXLo,6)
xLo = buffer[0]
xHi = buffer[1] <<8
yLo = buffer[2]
yHi = buffer[3] <<8
zLo = buffer[4]
zHi = buffer[5] <<8
x = xLo+xHi
y = yLo+yHi
z = zLo+zHi
########### Convert
heading = math.atan2(y, x)
heading = heading + declinationAngle
#Due to declination check for >360 degree
if(heading > 2*pi):
heading = heading - 2*pi
#check for sign
if(heading < 0):
heading = heading + 2*pi
#convert into angle
headingAngle = (heading * 180/pi)
########### Show result
print("3-axis : x={}/{},{} y={}/{},{} z={}/{},{}".format(x,xHi,xLo,y,yHi,yLo,z,zHi,zLo))
print ("Heading Angle = {}°".format(headingAngle))
ผลลัพธ์ของการทำงานของโค้ดตัวอย่างเป็นดังนี้
สรุป
จากบทความนี้จะพบว่า การเขียนโปรแกรมภาษาไพธอนมีข้อดี คือ สามารถนำโค้ดที่ได้นำกลับมาใช้ซ้ำกับบอร์ดที่ติดตั้งระบบตัวแปลภาษาไพธอนได้ง่ายกว่าการปรับแต่งโค้ดภาษา C/C++ แต่อย่างไรก็ดี ผู้เขียนพบว่าโมดูล GY-271 นั้นมีไอซี 2 รุ่น คือ QMC5883L และ HMC5883 ซึ่งไอซีทั้ง 2 นี้มีการออกแบบไม่เหมือนกัน และวิธีการแปลงค่ามีความแตกต่างกัน ดังนั้น ผู้อ่านต้องตรวจสอบรุ่นของไอซีบนโมดูลให้แน่ใจเพื่อให้โค้ดทำงานได้ถูกต้อง สุดท้ายนี้ขอให้สนุกกับการเขียนโปรแกรมครับ
อ้างอิง
(C) 2020-2021, โดย อ.ดนัย เจษฎาฐิติกุล/อ.จารุต บุศราทิจ
ปรับปรุงเมื่อ 2021-06-15, 2021-09-30