บทเรียน 3.2 — gyro ฟิลเตอร์ complementary และโค้ดเครื่องวัดระดับ

โครงสร้างในชิปถูกขับให้ สั่นในแนวหนึ่ง ตลอดเวลา ถ้าชิปไม่หมุน มันก็สั่นอยู่แนวเดิม

พอชิปเริ่มหมุน แรง Coriolis ผลักมวลที่กำลังสั่นให้เบนออกไปใน แนวตั้งฉาก — วงจรวัดการเบนนั้นแล้วแปลงเป็น deg/s

ภาพซ้ายคือ Coriolis ในกรอบหมุน (มุมมองนิ่ง เทียบกับ มุมมองหมุน) ภาพกลางคือโครงสร้าง tuning fork ที่ใช้จริง

accel — ถูกในระยะยาว แต่กระตุกในระยะสั้น
อ้างอิงแรงโน้มถ่วงซึ่งไม่มีวันเปลี่ยน วางทิ้งสามชั่วโมงค่าก็ยังถูก แต่มันวัด "แรงทั้งหมด" ไม่ได้แยกว่าอันไหนคือแรงโน้มถ่วง อันไหนคือมือที่เขย่า พัดลม เคาะโต๊ะ หรือ noise ทางไฟฟ้า

gyro — นิ่งและไวในระยะสั้น แต่หนีในระยะยาว
วัดอัตราการหมุนโดยตรง แรงสั่นภายนอกรบกวนได้น้อย ตอบสนองทันที แต่การจะได้ มุม ต้องเอาอัตรามาบวกสะสม

$$\theta_{\text{gyro}} = \sum_{k} \omega_k\,\Delta t$$

ตัวเลขจาก Eva Kit (บอร์ดที่ใช้เขียนสไลด์): วางนิ่งแล้ว gz ไม่เคยเป็น 0.00 เป๊ะ มันเป็น 0.03-0.05 deg/s สมมติ bias = 0.05 deg/s

$$\text{ผิดหลัง 60 s} = 0.05 \times 60 = 3.0^\circ \qquad \text{ผิดหลัง 600 s} = 30^\circ$$

ไทย: ความผิดพลาดจิ๋วที่ถูกบวกสะสมทุกรอบ ไม่มีอะไรดึงกลับ ปรากฏการณ์นี้ชื่อ drift

$$\theta_t = a\bigl(\theta_{t-1} + \omega_t\,\Delta t\bigr) + (1-a)\,\theta_{\text{accel}}$$

ไทย: เชื่อ gyro ในช่วงสั้น ๆ เพราะมันนิ่งไม่สั่น แล้วให้ accel คอยดึงกลับในระยะยาวเพราะ gyro ไหลไปเรื่อย ๆ

• ส่วน $a(\theta + \omega \Delta t)$ ทำหน้าที่ high-pass ต่อ gyro — ปล่อยการเปลี่ยนแปลงเร็วผ่าน ตัด drift ช้าทิ้ง
• ส่วน $(1-a)\theta_{\text{accel}}$ ทำหน้าที่ low-pass ต่อ accel — เก็บทิศระยะยาว ตัดการกระตุกทิ้ง

สองครึ่งนี้บวกกันได้พอดีหนึ่งเสมอ จึงเรียกว่า complementary

ความถี่ตัดของการผสม

$$\tau = \frac{a\,\Delta t}{1-a}$$

ไทย: τ คือเส้นแบ่ง — เร็วกว่านี้ฟังจาก gyro ช้ากว่านี้ฟังจาก accel
ตัวเลขของลูปเรา: $a = 0.98,\ \Delta t = 0.2$ s $\Rightarrow \tau = \dfrac{0.98 \times 0.2}{0.02} = 9.8$ s

บน MicroPython ชุดบทเรียนนี้เราใช้ dsp.tilt() ซึ่งคำนวณจาก accel อย่างเดียว แล้วต่อท้ายด้วย dsp.EMA เพื่อไล่การกระตุกออก พูดให้ตรงคือเราทำแค่ ครึ่ง low-pass ไม่ได้ทำครึ่ง gyro

$$y_t = \alpha x_t + (1-\alpha)\,y_{t-1} \qquad \tau \approx \frac{T_s}{\alpha}$$

ไทย: ค่าใหม่มีน้ำหนัก α ที่เหลือคือความจำของค่าเก่า · ตัวเลขของลูปเรา: $T_s = 0.2$ s, $\alpha = 0.2 \Rightarrow \tau \approx 1.0$ s

ได้อะไร เส้นนิ่ง ไม่มี drift สะสมเลยแม้เปิดทิ้งทั้งวัน เพราะไม่ได้อินทิเกรตอะไร · เสียอะไร มี lag ราวหนึ่งวินาที
ถ้าอยากได้ fusion เต็มรูป เฟิร์มแวร์มี dsp.Madgwick ให้ต่อยอด — สไลด์ถัดไปว่าด้วยเรื่องนั้นโดยตรง

กับดักที่หนึ่ง — หน่วยของ gyro update() ต้องการ gyro หน่วย เรเดียนต่อวินาที แต่ motion() คืน องศาต่อวินาที ลืมแปลง = ป้อนใหญ่เกินจริงราว 57 เท่า มุมหมุนติ้ว และ ไม่มี error ให้เห็น — ต้องเขียน math.radians(gx) เอง

กับดักที่สอง — หน่วยของ threshold ค่าตั้งต้น 1.5 คือหน่วย g ถ้าป้อน m/s² (วางนิ่งก็ 9.81 แล้ว) มันจะข้ามเกณฑ์ค้างตั้งแต่รอบแรกและนับก้าวเรื่อย ๆ ทั้งที่บอร์ดไม่ขยับ · เกณฑ์ปล่อยตรึงไว้ที่ 80% ของ threshold ตั้งเองไม่ได้ · fs ต้องเท่าคาบลูปจริง — ลูป 200 ms คือ fs=5.0 ไม่ใช่ค่าตั้งต้น 100

วางบอร์ดบนโต๊ะที่คิดว่าราบแล้วอ่านค่า จะเจอ 1.5° บ้าง 2.3° บ้าง แทบไม่มีทางได้ 0.0 เป๊ะ สาเหตุมีสามชั้นซ้อนกัน

1. โต๊ะไม่ราบจริง พื้นห้องเรียนแทบทุกที่เอียงเล็กน้อย
2. ชิปไม่ได้ติดตั้งตรงเป๊ะบน PCB คลาดเคลื่อนเศษหนึ่งส่วนสิบองศาเป็นเรื่องปกติของงานประกอบ
3. ตัวชิปมี zero-g offset ติดมาจากโรงงาน เป็นสเปกที่ผู้ผลิตประกาศไว้ ไม่ใช่ของเสีย

$$\phi_{\text{แสดงผล}} = \phi - \phi_{0} \qquad \theta_{\text{แสดงผล}} = \theta - \theta_{0}$$

ไทย: จำมุมตอนกดปุ่ม "ตั้งศูนย์" ไว้ แล้วลบออกทุกครั้ง
ตัวเลขจาก Eva Kit (บอร์ดที่ใช้เขียนสไลด์): กดตั้งศูนย์ตอน $\phi = 1.8^\circ$ → ต่อจากนั้น 1.8° กลายเป็น 0.0° และ 4.3° กลายเป็น 2.5°

roll_zero = roll_f          # จำท่าปัจจุบันไว้เป็นจุดอ้างอิง
roll_show = roll_f - roll_zero

ราวปี 1661 Melchisédech Thévenot ทำเครื่องวัดระดับแบบหลอดแก้วใส่ของเหลวที่มีฟองอากาศ — ฟองเบากว่าของเหลว มันจึงลอยไปอยู่จุดสูงสุดของหลอดเสมอ ช่างไม้ทั่วโลกใช้มันมาสามร้อยกว่าปีโดยไม่ต้องมีแบตเตอรี่

ปี 1991 Analog Devices ออก ADXL50 ซึ่งเป็น accelerometer แบบ MEMS ตัวแรกที่ผลิตขายจำนวนมาก เป้าหมายแรกคือถุงลมนิรภัย — ต้องแยก "ชนจริง" ออกจาก "ตกหลุมถนน" ให้ได้ในไม่กี่มิลลิวินาที

ภาพขวาบนคือ IMU ทั้งชุดวางเทียบกับเหรียญเพนนี ภาพขวาล่างคือภาพถ่ายไดจริงของ IMU ที่เปิดฝาออก

เชื่อมกับวันนี้: โปรแกรมที่เราจะเขียนอีกครู่คือหลอดแก้วของ Thévenot ที่ถูกเขียนใหม่ด้วย Python สิบกว่าบรรทัด และมันทำสิ่งที่หลอดแก้วทำไม่ได้ — บอกเป็นตัวเลของศา จำค่าอ้างอิงได้ และส่งค่าขึ้นเครือข่ายได้ในบทเรียนที่ 10

import ui
ui.screen()
import time
import sensors
import dsp
ui.clear()
time.sleep_ms(200)
...
try:                                 # ไม่มี sensors.init() ทั้งสองบอร์ด
    sensors.bmi270.motion()          # อุ่นเครื่อง ครั้งแรกหลังรีเซ็ตอาจต้องรอ
except OSError:
    print("อ่านเซนเซอร์รอบแรกยังไม่ได้ - ลองใหม่ในลูป")

ui.Label("ROLL เอียงซ้าย-ขวา (องศา)", x=40, y=60, color=COL_DIM, value=16)
roll_bar = ui.Bar(x=40, y=96, w=160, h=16, color=0x4A9EFF, min=-90, max=90, value=0)
roll_scale = ui.Scale(x=40, y=112, w=160, h=44, color=COL_TEXT, min=-90, max=90)
roll_scale.ticks(9, 4)           # -90 0 90
roll_seg = ui.Seg7("+00.0", x=216, y=96, w=96, h=56, color=COL_TEXT)
# pitch ชุดเดียวกัน เลื่อนไปขวา 288 พิกเซล
...
led_in = ui.Led(x=632, y=72, w=48, h=48, color=COL_OK, value=1)
led_out = ui.Led(x=704, y=72, w=48, h=48, color=COL_BAD, value=0)

min=-90, max=90 คือหัวใจของแถบตัวนี้ ค่าเริ่มต้นของ ui.Bar คือ 0-100 ถ้าไม่กำหนดช่วงเอง พอส่ง −30 องศาเข้าไปมันจะถูกปัดเป็น 0 แล้วแถบนิ่งสนิททั้งที่โค้ดคำนวณถูก

ui.Scale แนวนอนคือไม้บรรทัด ไม่ใช่หน้าปัด ไม่รับ .value() ตัวที่ขยับคือ ui.Bar ที่วางทับ ประโยชน์คือพา พิสัย มาอยู่บนจอ (แบบวงกลมมีเข็มจริง — อีกสองสไลด์) · .ticks(9, 4) คือ 9 ขีด ใส่ตัวเลขทุกขีดที่สี่ ได้ -90 0 90 — สองแกนวางเคียงกัน ไม้บรรทัดกว้างแค่ 160 พิกเซล ใส่ถี่กว่านี้ตัวเลขจะเบียดกัน

ui.Seg7 รับสตริง จึงใส่ + − และทศนิยมได้ · ไฟสองดวงแทนตัวหนังสือสี ถ่ายจอเป็นขาวดำแล้วยังแยกออกว่าดวงไหนติด

SC_X, SC_Y, SC_W = CX - 100, 95, 200
sc = ui.Scale(x=SC_X, y=SC_Y, w=SC_W, h=SC_W, color=0x1565C0, min=0, max=100)
sc.prop(ui.PROP_SCALE_MODE, ui.SCALE_ROUND_OUT)
sc.ticks(21, 5)
...
NEEDLE = 85
sc.prop(ui.PROP_SCALE_NEEDLE_COLOR, 0xFF9800)   # ครั้งเดียว
...
def draw_needle(v):
    # ทุกครั้งที่ค่าเปลี่ยน: เขียน prop เดียว เข็มหมุนเอง
    sc.prop(ui.PROP_SCALE_NEEDLE, (NEEDLE << 16) | (int(v) & 0xFFFF))

ค่าที่ส่งแพ็กสองเรื่องในเลขเดียว — สูง 16 บิตคือความยาวเข็มเป็นพิกเซล (NEEDLE = 85) ต่ำ 16 บิตคือค่าบนสเกล · CX = 396 คือกึ่งกลางจอ

เข็มของ lv_scale หมุนอยู่กับที่ จอวาดซ้ำเฉพาะบริเวณเข็ม — วิธีเก่าที่ลบ ui.Line แล้วสร้างใหม่ทุกค่า ทำให้ทั้งหน้าปัดวาบทุกจังหวะ (เจอจริงบนบอร์ด 2026-08-20)

ต้องใช้ firmware 2026-08-20 ขึ้นไป — รุ่นก่อนหน้าใช้ ui.Bar ทับแบบหน้าที่แล้วไปก่อน

ของจริงทั้งไฟล์: ex10_scale_pitch.py — pitch จาก dsp.tilt ขับเข็มสด

# --- ท่าที่ 3: อ่านหกแกนใน lock เดียว แล้วแปลงเป็นองศา ---
ax, ay, az, gx, gy, gz = sensors.bmi270.motion()
roll, pitch = dsp.tilt(ax, ay, az)

สองบรรทัดนี้คือแก่นของทั้งบทเรียน บรรทัดบนได้ภาพนิ่งของบอร์ด บรรทัดล่างเปลี่ยนมันเป็นองศา

dsp.tilt() รับแค่ สามค่าแรก เท่านั้น มันไม่ต้องการ gyro เพราะคำนวณจากแรงโน้มถ่วงล้วน ๆ ส่วน gx, gy, gz ที่รับมาด้วยเราจะเอาไปแสดงเป็นข้อมูลอ้างอิงท้ายจอ

การอ่าน I2C พลาดได้เป็นครั้งคราวโดยเฉพาะตอนบัสหนาแน่น ในโค้ดจริงเราจึงห่อบรรทัดนี้ด้วย try / except OSError แล้วข้ามรอบนั้นไป

# --- ท่าที่ 4: กรองความสั่น แล้วหักค่าอ้างอิงที่ตั้งศูนย์ไว้ ---
roll_f  = ema_roll.update(roll)
pitch_f = ema_pitch.update(pitch)

roll_show  = roll_f  - roll_zero
pitch_show = pitch_f - pitch_zero

.update() ทำสองอย่างพร้อมกัน: จำสถานะใหม่ไว้ในตัวมันเอง และคืนค่าที่กรองแล้วออกมาทันที

ลำดับสำคัญมาก ต้องกรองก่อน แล้วค่อยหักค่าศูนย์ เพราะตอนกดปุ่มตั้งศูนย์เราต้องเก็บ roll_f (ค่าที่กรองแล้ว) ไม่ใช่ค่าดิบ — ถ้าเก็บค่าดิบที่กำลังสั่นอยู่พอดี จุดอ้างอิงจะเพี้ยนตามความบังเอิญของเสี้ยววินาทีนั้น

# --- ท่าที่ 5: รับเหตุการณ์จากจอ ---
for ev in ui.poll():
    h = ev['handle']
    if h == zero_id:
        roll_zero  = roll_f
        pitch_zero = pitch_f
        ref.text("อ้างอิง R {:+.1f}  P {:+.1f}".format(roll_zero, pitch_zero))
    elif h == exit_id:
        running = False
    elif h == btn_up.id():
        tol = min(TOL_MAX, tol + TOL_STEP)
        sp_tol.value(tol)
    elif h == btn_dn.id():
        tol = max(TOL_MIN, tol - TOL_STEP)
        sp_tol.value(tol)

ui.poll() คืน list ของ dict แต่ละใบมีคีย์ handle, type, value (สูงสุด 8 event ต่อครั้ง)

# --- ท่าที่ 6: ส่งค่าขึ้นหน้าปัด แล้วเว้นจังหวะ ---
roll_bar.value(int(clamp90(roll_show)))       # แถบขยับทุกรอบ
pitch_bar.value(int(clamp90(pitch_show)))
in_tol = abs(roll_show) <= tol and abs(pitch_show) <= tol
led_in.value(1 if in_tol else 0)              # ติดทีละดวงเท่านั้น
led_out.value(0 if in_tol else 1)
sec = time.ticks_ms() // 1000                 # ตัวเลขวินาทีละครั้ง
if sec != last_sec:
    last_sec = sec
    roll_seg.text("{:+.1f}".format(roll_show))
    pitch_seg.text("{:+.1f}".format(pitch_show))
    if not ok_read:                           # รอบนี้อ่านไม่ได้ - บอกเป็นคำ
        lbl_health.color(COL_WARN)
        health = "ค่าค้าง นี่คือค่าล่าสุด"
lbl_health.text(health)      # ชีพจร - ส่งข้อความเดิมซ้ำทุกรอบ
time.sleep_ms(200)