傳感器無線采集與UDP傳輸實戰(zhàn))
最近在給一臺小型機械臂做姿態(tài)反饋需要把JY61P姿態(tài)傳感器的角度數(shù)據(jù)實時送到電腦上畫曲線繞了一圈最后定了ESP32WiFi這套方案。這個項目一句話就能說清楚用ESP32讀取JY61P模塊的串口輸出解析出roll、pitch、yaw三個姿態(tài)角再通過WiFi把數(shù)據(jù)發(fā)到同一局域網(wǎng)內(nèi)的電腦上供上位機處理或顯示。整套東西做下來難度不大非常適合剛接觸物聯(lián)網(wǎng)的人練手也適合需要快速搭建無線姿態(tài)采集鏈路的機器人愛好者參考。先說結(jié)論這套方案的成本不到五十塊錢硬件連接只有四根線軟件部分核心就是串口解析和UDP發(fā)包兩個動作。如果之前玩過Arduino基本一個晚上就能跑通。下面我把完整的選型思路、接線方式、協(xié)議解析、代碼實現(xiàn)和踩坑記錄都寫出來照著做就行。1. 項目整體設(shè)計與方案選型1.1 為什么是ESP32JY61P這套組合先聊選型。姿態(tài)采集方案其實有很多種常見的有直接用MPU6050裸傳感器配STM32、用帶藍(lán)牙的模塊、用ESP32直接讀取MPU6050以及現(xiàn)在這套ESP32JY61P組合。我為什么最后選了后者ESP32的優(yōu)勢在于它天生就是為物聯(lián)網(wǎng)設(shè)計的雙核240MHz自帶WiFi和藍(lán)牙Arduino生態(tài)非常成熟一塊開發(fā)板二十幾塊錢資料多到看不完。更重要的是它自帶硬件UART接口讀串口傳感器非常方便根本不用外接USB轉(zhuǎn)TTL芯片。JY61P則是維特智能推出的一款六軸姿態(tài)傳感器模塊內(nèi)部已經(jīng)把MPU6050的原始數(shù)據(jù)做了姿態(tài)解算直接通過串口輸出歐拉角。換句話說你不需要自己去寫卡爾曼濾波或互補濾波不用跟四元數(shù)死磕插上電就能拿到穩(wěn)定的角度數(shù)據(jù)。這點在工程上太重要了因為姿態(tài)解算看著簡單實際調(diào)起來非常耗時間尤其是動態(tài)漂移的問題沒有點功底很難處理好。JY61P相當(dāng)于把這塊最費腦子的部分封裝好了你花錢買時間劃算。對比一下直接讀MPU6050的方案雖然能省下模塊的錢但你要自己處理I2C時序、自己寫濾波算法、自己校準(zhǔn)零漂。我早期做過一次光調(diào)試靜態(tài)穩(wěn)定性就花了兩天。如果項目重點是無線鏈路的搭建而不是姿態(tài)解算本身那直接用JY61P是更理性的選擇。1.2 通信鏈路串口采集、WiFi轉(zhuǎn)發(fā)整個數(shù)據(jù)流是這樣的JY61P通過UART串口把姿態(tài)數(shù)據(jù)實時輸出給ESP32ESP32在這里扮演的其實是網(wǎng)關(guān)的角色它先把串口數(shù)據(jù)解析成有意義的姿態(tài)角再打包成UDP數(shù)據(jù)包通過WiFi發(fā)到電腦上。為什么中間非要加一個ESP32而不是讓JY61P直接通過USB連電腦因為JY61P本身不具備聯(lián)網(wǎng)能力它的接口只有串口而且它的供電電壓是3.3V TTL電平插到電腦USB口上還得額外加轉(zhuǎn)換芯片。加了ESP32之后整個系統(tǒng)就完全無線化了傳感器可以裝在移動設(shè)備上、機械臂末端、小車底盤上數(shù)據(jù)照樣能回到電腦端。在傳輸層協(xié)議上我選的是UDP而不是TCP。原因很簡單傳感器數(shù)據(jù)是周期性的連續(xù)流每一幀數(shù)據(jù)之間有很強的時間相關(guān)性偶爾丟一兩個包完全不影響整體效果下一幀馬上就能補上。UDP是無連接的發(fā)送方只管把數(shù)據(jù)丟出去不需要握手、確認(rèn)和重傳延遲低、開銷小。TCP雖然可靠但它的重傳機制在這種高頻數(shù)據(jù)流場景下反而是負(fù)擔(dān)一旦網(wǎng)絡(luò)抖動TCP會拼命重傳舊數(shù)據(jù)導(dǎo)致新數(shù)據(jù)排隊延遲越來越大。打個不嚴(yán)謹(jǐn)?shù)谋确絋CP像快遞簽收每一件都要確認(rèn)收到UDP像廣播喇叭喊一嗓子就完事聽不聽得到看現(xiàn)場環(huán)境。姿態(tài)采集這種場景顯然廣播喇叭更合適。1.3 典型應(yīng)用場景這個項目本質(zhì)上是一條通用的“串口傳感器WiFi無線傳輸”鏈路所以可復(fù)用的場景非常多機械臂關(guān)節(jié)角度反饋實時監(jiān)控每個軸的轉(zhuǎn)動狀態(tài)自平衡小車/兩輪車的姿態(tài)環(huán)數(shù)據(jù)采集代替有線連接云臺角度閉環(huán)控制把傳感器裝在負(fù)載端通過WiFi把角度回傳主控人體姿態(tài)捕捉的簡易原型多個JY61P掛在四肢上數(shù)據(jù)匯集到PC端旋轉(zhuǎn)平臺、天線轉(zhuǎn)臺的遠(yuǎn)程角度監(jiān)控。只要傳感器是串口輸出的這套代碼框架都能直接用改一下協(xié)議解析部分就行。2. 硬件準(zhǔn)備與關(guān)鍵接線2.1 硬件清單先列一下需要的材料都很常見ESP32開發(fā)板一塊推薦DevKit V1這種30引腳的標(biāo)準(zhǔn)板某寶二十幾塊錢注意選帶USB轉(zhuǎn)串口芯片的版本用起來省心JY61P模塊一個維特智能出品搜索型號就能找到三四十塊錢杜邦線若干母對母的最方便建議準(zhǔn)備四根面包板一塊可選方便固定和供電MicroUSB或Type-C數(shù)據(jù)線一根用于給ESP32供電和燒錄程序手機充電頭或充電寶一個5V/1A以上即可。2.2 引腳定義與接線表JY61P模塊常用的引腳有四個VCC、GND、TX、RX。其中VCC接電源正極GND接地TX是模塊的串口發(fā)送腳RX是接收腳。接線方式很簡單核心是“交叉連接共地供電”。我用的ESP32的UART2接口對應(yīng)GPIO16RX2和GPIO17TX2。接線表如下JY61P引腳接ESP32引腳說明VCC3.3V模塊供電3.3V更穩(wěn)妥GNDGND必須共地否則數(shù)據(jù)亂碼TXGPIO16RX2模塊發(fā)送ESP32接收交叉接RXGPIO17TX2模塊接收如果要配置模塊才需要接平時可懸空為什么選擇GPIO16和GPIO17因為這兩個引腳對應(yīng)的是ESP32的UART2和燒錄用的UART0互不干擾。ESP32的UART0默認(rèn)接在USB轉(zhuǎn)串口芯片上用來和電腦通信和燒錄程序。如果我把JY61P接到UART0的TX/RX引腳上每次燒錄程序還要把線拔掉非常麻煩。UART2就是專門的通用串口留給外設(shè)正合適。2.3 接線注意事項與供電細(xì)節(jié)接線部分有幾個細(xì)節(jié)特別值得注意都是我用實踐換來的教訓(xùn)。第一共地必須強調(diào)一萬遍。ESP32的GND和JY61P的GND一定要接在一起否則兩個模塊的電平基準(zhǔn)不同串口數(shù)據(jù)必然亂碼或者完全沒反應(yīng)。很多人說“我接好了但收不到數(shù)據(jù)”十有八九是共地沒做好。第二電平匹配問題。JY61P支持3.3V~5V供電但它的串口電平是TTL 3.3V而ESP32的GPIO也是3.3V電平兩者直連沒有問題。我建議給模塊統(tǒng)一供3.3V這樣最安全。不要用5V去給JY61P供電然后直連ESP32雖然有些板子內(nèi)部有電平處理但跨壓操作始終不放心。第三注意上電順序。我習(xí)慣先把JY61P的供電接好然后再給ESP32上電燒錄。因為USB枚舉瞬間的電壓抖動可能導(dǎo)致模塊異常復(fù)位如果恰好卡在燒錄的時候會莫名其妙地出現(xiàn)連接失敗。第四千萬別把JY61P的TX接到GPIO0或者GPIO12這類啟動時有關(guān)鍵電平要求的引腳上。GPIO0在ESP32下載程序時要求是低電平如果這個引腳被外部設(shè)備拉高或拉低會造成燒錄失敗或者啟動異?，F(xiàn)象很詭異程序燒不進(jìn)去或者燒進(jìn)去跑不起來。所以老老實實用GPIO16和GPIO17最省事。3. 軟件環(huán)境準(zhǔn)備Arduino IDE與ESP32支持包3.1 Arduino IDE安裝與ESP32開發(fā)板支持軟件部分我用的Arduino IDE因為ESP32的Arduino支持包非常成熟代碼寫起來直觀調(diào)試也方便。如果你還沒安裝Arduino IDE建議直接裝2.x版本界面和包管理比1.8好很多。安裝完IDE之后第一步是添加ESP32開發(fā)板支持包。打開“文件”-“首選項”在“附加開發(fā)板管理器網(wǎng)址”里添加以下地址https://dl.espressif.com/dl/package_esp32_index.json然后打開“工具”-“開發(fā)板”-“開發(fā)板管理器”搜索“esp32”找到“esp32 by Espressif Systems”安裝。這個包比較大下載時間取決于網(wǎng)絡(luò)情況耐心等待即可。安裝完成后在開發(fā)板列表里選擇“ESP32 Dev Module”就完成了基礎(chǔ)配置。有一個小坑剛安裝完支持包后編譯速度會偏慢因為第一次要編譯很多底層的庫后面就快了。如果編譯報錯提示找不到“WiFi.h”說明開發(fā)板支持包沒裝好重新在開發(fā)板管理器里檢查一下。3.2 燒錄與串口驗證在正式寫代碼之前建議先用一個最簡單的Blink程序驗證工具鏈?zhǔn)欠裾?。Arduino自帶的Blink示例就可以只是注意ESP32板載LED的引腳通常接在GPIO2上要把示例里的LED_BUILTIN換成2或者直接用LED_BUILTIN不同開發(fā)板定義不一樣看自己的板子說明書。編譯通過后連接ESP32到電腦在“工具”-“端口”里選擇對應(yīng)的COM口。點擊上傳按鈕如果出現(xiàn)連接失敗試著按住開發(fā)板上的BOOT鍵再點上傳等待出現(xiàn)“Connecting..._____”之后松手。燒錄完成后板載LED應(yīng)該開始閃爍這說明環(huán)境已經(jīng)OK了。4. JY61P數(shù)據(jù)協(xié)議解析與讀取代碼4.1 JY61P的輸出幀結(jié)構(gòu)JY61P默認(rèn)波特率是115200數(shù)據(jù)位8停止位1無校驗也就是常說的115200 8N1。模塊上電后會主動向上位機發(fā)送數(shù)據(jù)包不需要發(fā)送任何配置指令。這點很方便特別是對新手來說少了一道交互流程。它的數(shù)據(jù)包以幀頭0x55起始第二個字節(jié)是數(shù)據(jù)類型標(biāo)識常見的三種0x51加速度數(shù)據(jù)包0x52角速度數(shù)據(jù)包0x53角度數(shù)據(jù)包本項目只需要角度數(shù)據(jù)所以關(guān)注0x53這個類型即可。一個完整的角度數(shù)據(jù)包總長11字節(jié)結(jié)構(gòu)如下字節(jié)序號內(nèi)容說明00x55幀頭10x53數(shù)據(jù)類型角度包2RollLRoll角度低字節(jié)3RollHRoll角度高字節(jié)4PitchLPitch角度低字節(jié)5PitchHPitch角度高字節(jié)6YawLYaw角度低字節(jié)7YawHYaw角度高字節(jié)8TL溫度低字節(jié)9TH溫度高字節(jié)10SUM校驗和角度數(shù)據(jù)是int16類型小端序存儲實際角度值的換算公式是角度 原始值 / 32768 * 180度。因為模塊內(nèi)部的角度量程是±180度而int16的滿量程正好是±32768對應(yīng)關(guān)系非常直接。溫度值也是int16換算公式是 溫度 原始值 / 100攝氏度不過本項目的重點是姿態(tài)角溫度不處理也沒關(guān)系。校驗和的算法是從幀頭0x55開始一直到第9字節(jié)的TH所有字節(jié)累加取累加和的低8位結(jié)果等于第10字節(jié)的SUM。也就是說SUM (0x55 0x53 RollL RollH PitchL PitchH YawL YawH TL TH) 0xFF這個校驗非常重要因為串口通信在干擾環(huán)境下偶爾會出錯如果不對校驗和可能解析出完全離譜的角度值導(dǎo)致上位機數(shù)據(jù)亂跳。4.2 解析思路狀態(tài)機處理串口字節(jié)流串口的本質(zhì)是一個字節(jié)一個字節(jié)地往外吐數(shù)據(jù)中間沒有明顯的“幀”邊界。如果直接在循環(huán)里一個個找0x55非常容易出錯因為數(shù)據(jù)內(nèi)容里也可能出現(xiàn)0x55這個字節(jié)比如角度值恰好是0x55XX。這時候就需要用狀態(tài)機來切幀。狀態(tài)機的思路是定義一個狀態(tài)變量記錄當(dāng)前解析到了幀的哪個位置。一開始處于“等待幀頭”狀態(tài)只有收到0x55才進(jìn)入“等待類型”狀態(tài)收到合法的類型字節(jié)0x51/0x52/0x53后進(jìn)入“接收數(shù)據(jù)”狀態(tài)連續(xù)接收8個數(shù)據(jù)字節(jié)后進(jìn)入“等待校驗”狀態(tài)收到校驗字節(jié)后判斷是否正確然后無論對錯都回到“等待幀頭”狀態(tài)。這樣做的好處是嚴(yán)格的同步機制即使中間某一幀壞了下一幀的幀頭也能重新對齊不會導(dǎo)致后續(xù)所有數(shù)據(jù)全部錯位。這也是工業(yè)串口協(xié)議解析的標(biāo)準(zhǔn)做法。4.3 Arduino讀取代碼示例下面給出一段獨立的JY61P讀取代碼使用ESP32的UART2GPIO16接收把解析出的角度通過USB串口打印到Arduino IDE的串口監(jiān)視器上。先跑通這一步再上WiFi。// ESP32 JY61P 串口讀取和解析 // 使用 UART2: RXGPIO16, TXGPIO17 #define RXD2 16 #define TXD2 17 // 解析狀態(tài)機 enum State { WAIT_HEAD, WAIT_TYPE, WAIT_DATA, WAIT_CHECK }; State state WAIT_HEAD; uint8_t frameBuf[11]; int frameIndex 0; uint8_t frameType 0; uint8_t checkSum 0; void handleData(uint8_t *buf) { // 0x53 角度包 if (buf[1] 0x53) { int16_t rollRaw (buf[3] 8) | buf[2]; int16_t pitchRaw (buf[5] 8) | buf[4]; int16_t yawRaw (buf[7] 8) | buf[6]; float roll rollRaw / 32768.0 * 180.0; float pitch pitchRaw / 32768.0 * 180.0; float yaw yawRaw / 32768.0 * 180.0; Serial.printf(roll%.2f pitch%.2f yaw%.2f\n, roll, pitch, yaw); } } void parseByte(uint8_t c) { switch (state) { case WAIT_HEAD: if (c 0x55) { frameBuf[0] c; checkSum c; state WAIT_TYPE; } break; case WAIT_TYPE: if (c 0x51 || c 0x52 || c 0x53) { frameBuf[1] c; checkSum c; frameType c; frameIndex 2; state WAIT_DATA; } else { state WAIT_HEAD; // 類型不合法重新找?guī)^ } break; case WAIT_DATA: frameBuf[frameIndex] c; checkSum c; frameIndex; if (frameIndex 10) { state WAIT_CHECK; } break; case WAIT_CHECK: if (c (checkSum 0xFF)) { handleData(frameBuf); } state WAIT_HEAD; break; } } void setup() { Serial.begin(115200); Serial2.begin(115200, SERIAL_8N1, RXD2, TXD2); Serial.println(JY61P Test Start); } void loop() { while (Serial2.available()) { parseByte(Serial2.read()); } }燒錄到ESP32后打開串口監(jiān)視器波特率選115200如果接線沒問題可以看到不斷滾動的roll、pitch、yaw數(shù)據(jù)。這時候轉(zhuǎn)動JY61P對應(yīng)的角度值應(yīng)該跟著變化。如果這里能跑通說明整個鏈路的一半已經(jīng)完成了。4.4 備選方案使用WitMotion官方庫有朋友嫌自己寫狀態(tài)機麻煩其實維特智能官方也提供了Arduino庫名字叫“WitMotion”在Arduino庫管理器里可以直接搜到。使用官方庫的代碼如下#include WitMotion.h WitMotion jy61p; void setup() { Serial.begin(115200); jy61p.begin(Serial2); } void loop() { if (jy61p.update()) { Serial.printf(roll%.2f pitch%.2f yaw%.2f\n, jy61p.roll, jy61p.pitch, jy61p.yaw); } delay(10); }官方庫把協(xié)議解析封裝好了配置也簡單適合不想深入了解協(xié)議的人。不過我依然建議自己寫一遍解析代碼因為工業(yè)場景里串口傳感器種類很多幀格式各有不同掌握了狀態(tài)機拆幀的技能換任何傳感器都能應(yīng)對。官方庫是一個黑盒一旦出了問題不好排查。5. 連接WiFi并通過UDP發(fā)送數(shù)據(jù)到電腦5.1 WiFi連接流程串口數(shù)據(jù)解析通了接下來就是給ESP32聯(lián)網(wǎng)。ESP32連接WiFi的代碼很固定無非是這幾步設(shè)置工作模式為STA、調(diào)用begin傳入SSID和密碼、循環(huán)等待連接成功。#include WiFi.h const char* ssid your_wifi_name; const char* password your_wifi_password; void setup() { Serial.begin(115200); WiFi.mode(WIFI_STA); WiFi.begin(ssid, password); Serial.print(Connecting to WiFi); while (WiFi.status() ! WL_CONNECTED) { delay(500); Serial.print(.); } Serial.println(); Serial.print(Connected, IP: ); Serial.println(WiFi.localIP()); }唯一需要特別注意的是頻段問題。ESP32只支持2.4GHz頻段的WiFi如果路由器開了5GHz或者“雙頻合一”模式ESP32很可能連不上或者連上后頻繁掉線。解決辦法是登錄路由器后臺把WiFi頻段固定為2.4GHz或者給ESP32單獨開一個訪客網(wǎng)絡(luò)。這個坑我踩過折騰了半天最后發(fā)現(xiàn)是路由器把2.4G和5G合并了。為了方便調(diào)試建議在路由器里給ESP32分配一個固定IP或者在代碼里手動配置靜態(tài)IP。這樣電腦端監(jiān)聽程序不用每次去查IP變化。但項目初步階段動態(tài)IP也沒問題ESP32上電后打印一下自己的IP就行。5.2 為什么推薦UDP而不是TCP前面提過UDP的優(yōu)勢這里再展開說一下。姿態(tài)數(shù)據(jù)的典型特征是高頻率、小數(shù)據(jù)包、允許少量丟失。JY61P默認(rèn)輸出頻率是100Hz也就是每秒100幀數(shù)據(jù)。如果每幀都用TCP發(fā)送每包都要經(jīng)過三次握手建立的連接通道雖然連接是長連接但TCP的確認(rèn)機制和擁塞控制還是會引入額外延遲尤其是WiFi環(huán)境本身就不穩(wěn)定TCP重傳會造成數(shù)據(jù)包的排隊和亂序。UDP則完全沒有這些負(fù)擔(dān)。數(shù)據(jù)包發(fā)出去就結(jié)束1對1也好1對多也好代碼實現(xiàn)都非常簡單。在局域網(wǎng)這種低延遲、低丟包的環(huán)境下UDP的可靠性其實相當(dāng)高。實測下來100Hz的數(shù)據(jù)流丟包率遠(yuǎn)低于千分之一對姿態(tài)顯示沒有任何影響。當(dāng)然了如果后續(xù)要做的是指令下發(fā)、參數(shù)配置這類要求可靠傳輸?shù)墓δ苣蔷屠侠蠈崒嵱肨CP或者HTTP。UDP適合的是傳感器數(shù)據(jù)上行這種場景要區(qū)分清楚。5.3 UDP發(fā)送代碼與電腦端監(jiān)聽UDP發(fā)送用Arduino的WiFiUDP庫核心就三個方法beginPacket、print/printf、endPacket。示例代碼如下#include WiFiUdp.h WiFiUDP udp; const IPAddress pcIP(192, 168, 1, 100); // 改成你電腦的IP const unsigned int pcPort 8888; void setup() { Serial.begin(115200); WiFi.mode(WIFI_STA); WiFi.begin(ssid, password); while (WiFi.status() ! WL_CONNECTED) { delay(500); } udp.begin(pcPort); // 本機綁定端口可選 } void loop() { udp.beginPacket(pcIP, pcPort); udp.printf(hello from esp32\n); udp.endPacket(); delay(100); }電腦端的IP地址查看方法Windows在命令行里輸入ipconfig找“無線局域網(wǎng)適配器”里的IPv4地址macOS或Linux在終端里輸入ifconfig找en0或wlan0的inet地址。電腦和ESP32必須在同一個網(wǎng)段下比如路由器分配的IP都是192.168.1.x。電腦端用Python寫一個最簡單的UDP監(jiān)聽腳本import socket sock socket.socket(socket.AF_INET, socket.SOCK_DGRAM) sock.bind((0.0.0.0, 8888)) print(Listening on UDP port 8888...) while True: data, addr sock.recvfrom(1024) print(data.decode().strip())運行這個腳本后如果ESP32的WiFi連上了電腦端的防火墻也沒攔UDP包終端里應(yīng)該能不斷打印出ESP32發(fā)來的“hello”字符串。這一步通了說明“傳感器數(shù)據(jù)無線回傳”的核心鏈路已經(jīng)打通。6. 整體聯(lián)調(diào)JY61P數(shù)據(jù)經(jīng)WiFi回傳電腦6.1 完整代碼整合把前幾節(jié)的代碼整合起來就是一個完整的ESP32JY61PWiFi無線姿態(tài)采集程序。代碼里已經(jīng)包含了SSID、密碼、電腦IP和端口按自己的環(huán)境修改即可。#include WiFi.h #include WiFiUdp.h // WiFi配置 const char* ssid your_wifi_name; const char* password your_wifi_password; // 電腦端配置 const IPAddress pcIP(192, 168, 1, 100); const unsigned int pcPort 8888; WiFiUDP udp; // JY61P 串口引腳 #define RXD2 16 #define TXD2 17 // 解析狀態(tài)機 enum State { WAIT_HEAD, WAIT_TYPE, WAIT_DATA, WAIT_CHECK }; State state WAIT_HEAD; uint8_t frameBuf[11]; int frameIndex 0; uint8_t frameType 0; uint8_t checkSum 0; void sendAngleByUDP(uint8_t *buf) { int16_t rollRaw (buf[3] 8) | buf[2]; int16_t pitchRaw (buf[5] 8) | buf[4]; int16_t yawRaw (buf[7] 8) | buf[6]; float roll rollRaw / 32768.0 * 180.0; float pitch pitchRaw / 32768.0 * 180.0; float yaw yawRaw / 32768.0 * 180.0; // 數(shù)據(jù)格式: roll,pitch,yaw udp.beginPacket(pcIP, pcPort); udp.printf(%.2f,%.2f,%.2f\n, roll, pitch, yaw); udp.endPacket(); } void parseByte(uint8_t c) { switch (state) { case WAIT_HEAD: if (c 0x55) { frameBuf[0] c; checkSum c; state WAIT_TYPE; } break; case WAIT_TYPE: if (c 0x51 || c 0x52 || c 0x53) { frameBuf[1] c; checkSum c; frameType c; frameIndex 2; state WAIT_DATA; } else { state WAIT_HEAD; } break; case WAIT_DATA: frameBuf[frameIndex] c; checkSum c; frameIndex; if (frameIndex 10) { state WAIT_CHECK; } break; case WAIT_CHECK: if (c (checkSum 0xFF)) { if (frameType 0x53) { sendAngleByUDP(frameBuf); } } state WAIT_HEAD; break; } } void setup() { Serial.begin(115200); Serial2.begin(115200, SERIAL_8N1, RXD2, TXD2); // 連接WiFi WiFi.mode(WIFI_STA); WiFi.begin(ssid, password); Serial.print(Connecting to WiFi); while (WiFi.status() ! WL_CONNECTED) { delay(500); Serial.print(.); } Serial.println(); Serial.print(ESP32 IP: ); Serial.println(WiFi.localIP()); udp.begin(pcPort); } void loop() { while (Serial2.available()) { parseByte(Serial2.read()); } }代碼邏輯并不復(fù)雜setup里完成串口、WiFi和UDP的初始化loop里持續(xù)從Serial2讀取JY61P發(fā)來的字節(jié)交給狀態(tài)機解析解析到合法的0x53角度包后計算三個歐拉角通過UDP發(fā)送到電腦端。Serial.printf打印調(diào)試信息依舊走USB串口完全不影響WiFi發(fā)送。6.2 電腦端接收與可視化測試電腦端運行前面那個Python監(jiān)聽腳本接收波特率匹配那次我們已經(jīng)確認(rèn)了。如果一切正常終端里會出現(xiàn)類似下面的輸出12.34,5.67,-89.01 12.35,5.68,-89.02 12.36,5.69,-89.03三列數(shù)據(jù)分別對應(yīng)roll、pitch、yaw。物理上轉(zhuǎn)動JY61P對應(yīng)數(shù)值會實時變化響應(yīng)延遲在毫秒級肉眼幾乎感覺不到延遲。想看可視化的曲線可以用Python的matplotlib庫做一個簡單的實時繪圖窗口。這個屬于擴(kuò)展內(nèi)容網(wǎng)上有很多現(xiàn)成的例子關(guān)鍵詞搜“matplotlib 實時繪圖 UDP”就能找到。如果想做更專業(yè)的上位機可以用pyqtgraph或Processing效果會好很多。6.3 發(fā)送頻率與數(shù)據(jù)包格式設(shè)計JY61P默認(rèn)輸出頻率是100Hz但實際上并不是每個0x53角度包都干凈利落地被發(fā)送。串口中斷和WiFi發(fā)送都需要時間如果每收到一幀就立刻UDP發(fā)送可能會出現(xiàn)微小的節(jié)奏抖動。實測下來100Hz完全能扛住但如果電腦端的接收緩沖區(qū)不夠大長時間運行有掉包風(fēng)險。我建議根據(jù)實際需求控制發(fā)送頻率。如果只是簡單監(jiān)控可以加一個簡單的節(jié)流判斷比如每50ms發(fā)送一次最新角度也就是20Hz。如果做機械臂姿態(tài)反饋或者自動控制那100Hz是底線建議保持全速發(fā)送。UDP包本身很小一個包不到30字節(jié)100包/秒也就是每秒3KB流量對WiFi來說九牛一毛。數(shù)據(jù)包格式建議用CSV也就是純文本加逗號分隔。因為UDP本身沒有規(guī)定格式用CSV最直觀電腦端一行split(,)就能解析成三個浮點數(shù)。如果以后要傳輸更多字段比如加速度、角速度、溫度可以繼續(xù)往后面追加格式保持兼容即可。7. 常見問題與排查技巧實錄做一個項目不踩坑是不可能的。我把實際過程中遇到的典型問題整理成了速查表方便大家對照排查。現(xiàn)象可能原因解決辦法串口監(jiān)視器沒有任何輸出JY61P供電沒接好檢查VCC和GND確認(rèn)共地串口輸出全是亂碼波特率不匹配確認(rèn)JY61P是115200串口監(jiān)視器也選115200數(shù)據(jù)偶發(fā)跳變干擾或接線過長杜邦線盡量短避開電機等強干擾源校驗和頻繁錯誤供電不足導(dǎo)致模塊異常換優(yōu)質(zhì)電源不要用劣質(zhì)USB口供電WiFi連不上路由器開了5GHz或雙頻合一固定到2.4GHz頻段或開訪客網(wǎng)絡(luò)WiFi連上但收不到UDPWindows防火墻攔截在防火墻入站規(guī)則里放行UDP 8888端口燒錄程序失敗GPIO引腳沖突下載時按住BOOT鍵檢查外設(shè)是否占用GPIO0程序燒錄后反復(fù)重啟供電電流不夠換5V/2A電源避免電腦USB口供電不足7.1 串口調(diào)試通了再上WiFi這里分享一個我自己的實戰(zhàn)習(xí)慣永遠(yuǎn)先串口后WiFi。也就是先把JY61P的數(shù)據(jù)通過USB串口打印在電腦上看確認(rèn)硬件接線和解析邏輯完全正確再去連WiFi、寫UDP。這樣一旦出了問題排查范圍能縮小一半。很多新手一上來就把WiFi和UDP代碼全寫上結(jié)果收不到數(shù)據(jù)根本分不清到底是傳感器沒接好還是解析代碼有bug還是WiFi配置有問題。分層驗證是最穩(wěn)的方案雖然多花幾分鐘但能省下一個晚上的排查時間。7.2 供電不足引起的詭異問題ESP32的開發(fā)板在WiFi發(fā)射的時候瞬時電流可能到幾百毫安如果電源質(zhì)量不好電壓會被拉低導(dǎo)致JY61P也工作不穩(wěn)定。癥狀很典型上電時一切正常一旦WiFi連上開始發(fā)送數(shù)據(jù)板子就重啟或者數(shù)據(jù)開始出現(xiàn)校驗錯誤。解決辦法很簡單給ESP32用一個靠譜的5V電源比如手機充電頭或者大容量充電寶盡量不要用電腦前置USB口的供電。JY61P的VCC接在ESP32的3.3V引腳上所以ESP32的電源質(zhì)量直接決定JY61P的穩(wěn)定性。如果單獨給JY61P供3.3V記得GND還是要和ESP32連在一起。7.3 防火墻與端口設(shè)置電腦端收不到UDP數(shù)據(jù)排除WiFi問題后十有八九是防火墻攔截了入站UDP包。Windows的防火墻默認(rèn)情況下會攔截陌生程序的入站流量尤其Python解釋器或者Java虛擬機這類沒有在允許列表里的程序。臨時解決辦法是關(guān)閉防火墻測試注意生產(chǎn)環(huán)境不建議這么干或者更規(guī)范的做法是在“Windows Defender防火墻”的“高級設(shè)置”里添加入站規(guī)則放行UDP協(xié)議、指定本地端口8888、允許所有程序。這樣既安全又能正常接收數(shù)據(jù)。8. 項目擴(kuò)展思路8.1 數(shù)據(jù)記錄與回放如果要做數(shù)據(jù)分析或?qū)嶒瀼?fù)盤可以在電腦端把接收到的數(shù)據(jù)實時寫入CSV文件。Python監(jiān)聽腳本里加幾行文件寫入代碼就行每收到一行數(shù)據(jù)就追加到文件末尾。這樣跑完一次實驗就能拿到一份完整的姿態(tài)數(shù)據(jù)導(dǎo)入Excel或者M(jìn)atlab分析都很方便。8.2 多傳感器接入與多軸可視化一個ESP32的UART2只能接一個JY61P但ESP32其實有多個UART。比如UART1、UART2都可以接只是注意不要和Flash芯片占用的引腳沖突GPIO6~GPIO11默認(rèn)給Flash用了盡量避開。如果你需要監(jiān)測多個姿態(tài)點比如人體四肢可以買多個JY61P模塊分別接到不同的UART上然后在ESP32上按端口區(qū)分?jǐn)?shù)據(jù)來源統(tǒng)一通過UDP發(fā)到電腦。電腦端接收時可以根據(jù)數(shù)據(jù)包里的設(shè)備標(biāo)識來區(qū)分不同的傳感器節(jié)點。這樣一套系統(tǒng)就能同時監(jiān)控多個姿態(tài)點做人體姿態(tài)捕捉的基礎(chǔ)原型完全夠用。8.3 與ROS 2/micro-ROS結(jié)合如果你做機器人開發(fā)可以把這套無線姿態(tài)采集方案接入ROS 2生態(tài)。ESP32上運行micro-ROS節(jié)點把roll、pitch、yaw發(fā)布為sensor_msgs/Imu或者自定義的Float32MultiArray話題上位機直接用ROS 2工具鏈訂閱數(shù)據(jù)。這個方向適合已經(jīng)接觸過ROS 2的朋友初期不建議直接上手先把基礎(chǔ)鏈路跑通再說。8.4 增加自動校準(zhǔn)與交互功能JY61P出廠前已經(jīng)做了標(biāo)定零點漂移很小直接可用。但如果你追求更高的精度可以加上電自校準(zhǔn)邏輯在模塊靜止?fàn)顟B(tài)下采集100幀數(shù)據(jù)求平均作為零偏然后在后續(xù)數(shù)據(jù)中扣除。這個校準(zhǔn)邏輯放在ESP32解析代碼里實現(xiàn)即可不需要改模塊配置。另外如果想讓系統(tǒng)具備交互能力可以把ESP32的藍(lán)牙也打開通過手機App直接看角度。有了WiFi和藍(lán)牙雙重通道這套系統(tǒng)的應(yīng)用范圍會寬很多。這個項目看著簡單但真正動手做一遍串口協(xié)議解析、狀態(tài)機設(shè)計、WiFi通信這幾塊基本功都能得到實打?qū)嵉腻憻?。我做完之后的體會是任何一個環(huán)節(jié)不扎實都會出問題尤其是串口字節(jié)處理寫代碼時覺得思路很清晰一上板子各種邊角情況全冒出來了。建議你從串口讀數(shù)據(jù)開始一步一步往上加功能不要一口吃成胖子。無線數(shù)據(jù)傳輸這條鏈路打通之后后面想接什么傳感器、做什么應(yīng)用都只是換個協(xié)議解析的事。 ESP32+JY61P姿態(tài)傳感器無線采集與UDP傳輸實戰(zhàn)
最近在給一臺小型機械臂做姿態(tài)反饋需要把JY61P姿態(tài)傳感器的角度數(shù)據(jù)實時送到電腦上畫曲線繞了一圈最后定了ESP32WiFi這套方案。這個項目一句話就能說清楚用ESP32讀取JY61P模塊的串口輸出解析出roll、pitch、yaw三個姿態(tài)角再通過WiFi把數(shù)據(jù)發(fā)到同一局域網(wǎng)內(nèi)的電腦上供上位機處理或顯示。整套東西做下來難度不大非常適合剛接觸物聯(lián)網(wǎng)的人練手也適合需要快速搭建無線姿態(tài)采集鏈路的機器人愛好者參考。先說結(jié)論這套方案的成本不到五十塊錢硬件連接只有四根線軟件部分核心就是串口解析和UDP發(fā)包兩個動作。如果之前玩過Arduino基本一個晚上就能跑通。下面我把完整的選型思路、接線方式、協(xié)議解析、代碼實現(xiàn)和踩坑記錄都寫出來照著做就行。1. 項目整體設(shè)計與方案選型1.1 為什么是ESP32JY61P這套組合先聊選型。姿態(tài)采集方案其實有很多種常見的有直接用MPU6050裸傳感器配STM32、用帶藍(lán)牙的模塊、用ESP32直接讀取MPU6050以及現(xiàn)在這套ESP32JY61P組合。我為什么最后選了后者ESP32的優(yōu)勢在于它天生就是為物聯(lián)網(wǎng)設(shè)計的雙核240MHz自帶WiFi和藍(lán)牙Arduino生態(tài)非常成熟一塊開發(fā)板二十幾塊錢資料多到看不完。更重要的是它自帶硬件UART接口讀串口傳感器非常方便根本不用外接USB轉(zhuǎn)TTL芯片。JY61P則是維特智能推出的一款六軸姿態(tài)傳感器模塊內(nèi)部已經(jīng)把MPU6050的原始數(shù)據(jù)做了姿態(tài)解算直接通過串口輸出歐拉角。換句話說你不需要自己去寫卡爾曼濾波或互補濾波不用跟四元數(shù)死磕插上電就能拿到穩(wěn)定的角度數(shù)據(jù)。這點在工程上太重要了因為姿態(tài)解算看著簡單實際調(diào)起來非常耗時間尤其是動態(tài)漂移的問題沒有點功底很難處理好。JY61P相當(dāng)于把這塊最費腦子的部分封裝好了你花錢買時間劃算。對比一下直接讀MPU6050的方案雖然能省下模塊的錢但你要自己處理I2C時序、自己寫濾波算法、自己校準(zhǔn)零漂。我早期做過一次光調(diào)試靜態(tài)穩(wěn)定性就花了兩天。如果項目重點是無線鏈路的搭建而不是姿態(tài)解算本身那直接用JY61P是更理性的選擇。1.2 通信鏈路串口采集、WiFi轉(zhuǎn)發(fā)整個數(shù)據(jù)流是這樣的JY61P通過UART串口把姿態(tài)數(shù)據(jù)實時輸出給ESP32ESP32在這里扮演的其實是網(wǎng)關(guān)的角色它先把串口數(shù)據(jù)解析成有意義的姿態(tài)角再打包成UDP數(shù)據(jù)包通過WiFi發(fā)到電腦上。為什么中間非要加一個ESP32而不是讓JY61P直接通過USB連電腦因為JY61P本身不具備聯(lián)網(wǎng)能力它的接口只有串口而且它的供電電壓是3.3V TTL電平插到電腦USB口上還得額外加轉(zhuǎn)換芯片。加了ESP32之后整個系統(tǒng)就完全無線化了傳感器可以裝在移動設(shè)備上、機械臂末端、小車底盤上數(shù)據(jù)照樣能回到電腦端。在傳輸層協(xié)議上我選的是UDP而不是TCP。原因很簡單傳感器數(shù)據(jù)是周期性的連續(xù)流每一幀數(shù)據(jù)之間有很強的時間相關(guān)性偶爾丟一兩個包完全不影響整體效果下一幀馬上就能補上。UDP是無連接的發(fā)送方只管把數(shù)據(jù)丟出去不需要握手、確認(rèn)和重傳延遲低、開銷小。TCP雖然可靠但它的重傳機制在這種高頻數(shù)據(jù)流場景下反而是負(fù)擔(dān)一旦網(wǎng)絡(luò)抖動TCP會拼命重傳舊數(shù)據(jù)導(dǎo)致新數(shù)據(jù)排隊延遲越來越大。打個不嚴(yán)謹(jǐn)?shù)谋确絋CP像快遞簽收每一件都要確認(rèn)收到UDP像廣播喇叭喊一嗓子就完事聽不聽得到看現(xiàn)場環(huán)境。姿態(tài)采集這種場景顯然廣播喇叭更合適。1.3 典型應(yīng)用場景這個項目本質(zhì)上是一條通用的“串口傳感器WiFi無線傳輸”鏈路所以可復(fù)用的場景非常多機械臂關(guān)節(jié)角度反饋實時監(jiān)控每個軸的轉(zhuǎn)動狀態(tài)自平衡小車/兩輪車的姿態(tài)環(huán)數(shù)據(jù)采集代替有線連接云臺角度閉環(huán)控制把傳感器裝在負(fù)載端通過WiFi把角度回傳主控人體姿態(tài)捕捉的簡易原型多個JY61P掛在四肢上數(shù)據(jù)匯集到PC端旋轉(zhuǎn)平臺、天線轉(zhuǎn)臺的遠(yuǎn)程角度監(jiān)控。只要傳感器是串口輸出的這套代碼框架都能直接用改一下協(xié)議解析部分就行。2. 硬件準(zhǔn)備與關(guān)鍵接線2.1 硬件清單先列一下需要的材料都很常見ESP32開發(fā)板一塊推薦DevKit V1這種30引腳的標(biāo)準(zhǔn)板某寶二十幾塊錢注意選帶USB轉(zhuǎn)串口芯片的版本用起來省心JY61P模塊一個維特智能出品搜索型號就能找到三四十塊錢杜邦線若干母對母的最方便建議準(zhǔn)備四根面包板一塊可選方便固定和供電MicroUSB或Type-C數(shù)據(jù)線一根用于給ESP32供電和燒錄程序手機充電頭或充電寶一個5V/1A以上即可。2.2 引腳定義與接線表JY61P模塊常用的引腳有四個VCC、GND、TX、RX。其中VCC接電源正極GND接地TX是模塊的串口發(fā)送腳RX是接收腳。接線方式很簡單核心是“交叉連接共地供電”。我用的ESP32的UART2接口對應(yīng)GPIO16RX2和GPIO17TX2。接線表如下JY61P引腳接ESP32引腳說明VCC3.3V模塊供電3.3V更穩(wěn)妥GNDGND必須共地否則數(shù)據(jù)亂碼TXGPIO16RX2模塊發(fā)送ESP32接收交叉接RXGPIO17TX2模塊接收如果要配置模塊才需要接平時可懸空為什么選擇GPIO16和GPIO17因為這兩個引腳對應(yīng)的是ESP32的UART2和燒錄用的UART0互不干擾。ESP32的UART0默認(rèn)接在USB轉(zhuǎn)串口芯片上用來和電腦通信和燒錄程序。如果我把JY61P接到UART0的TX/RX引腳上每次燒錄程序還要把線拔掉非常麻煩。UART2就是專門的通用串口留給外設(shè)正合適。2.3 接線注意事項與供電細(xì)節(jié)接線部分有幾個細(xì)節(jié)特別值得注意都是我用實踐換來的教訓(xùn)。第一共地必須強調(diào)一萬遍。ESP32的GND和JY61P的GND一定要接在一起否則兩個模塊的電平基準(zhǔn)不同串口數(shù)據(jù)必然亂碼或者完全沒反應(yīng)。很多人說“我接好了但收不到數(shù)據(jù)”十有八九是共地沒做好。第二電平匹配問題。JY61P支持3.3V~5V供電但它的串口電平是TTL 3.3V而ESP32的GPIO也是3.3V電平兩者直連沒有問題。我建議給模塊統(tǒng)一供3.3V這樣最安全。不要用5V去給JY61P供電然后直連ESP32雖然有些板子內(nèi)部有電平處理但跨壓操作始終不放心。第三注意上電順序。我習(xí)慣先把JY61P的供電接好然后再給ESP32上電燒錄。因為USB枚舉瞬間的電壓抖動可能導(dǎo)致模塊異常復(fù)位如果恰好卡在燒錄的時候會莫名其妙地出現(xiàn)連接失敗。第四千萬別把JY61P的TX接到GPIO0或者GPIO12這類啟動時有關(guān)鍵電平要求的引腳上。GPIO0在ESP32下載程序時要求是低電平如果這個引腳被外部設(shè)備拉高或拉低會造成燒錄失敗或者啟動異?，F(xiàn)象很詭異程序燒不進(jìn)去或者燒進(jìn)去跑不起來。所以老老實實用GPIO16和GPIO17最省事。3. 軟件環(huán)境準(zhǔn)備Arduino IDE與ESP32支持包3.1 Arduino IDE安裝與ESP32開發(fā)板支持軟件部分我用的Arduino IDE因為ESP32的Arduino支持包非常成熟代碼寫起來直觀調(diào)試也方便。如果你還沒安裝Arduino IDE建議直接裝2.x版本界面和包管理比1.8好很多。安裝完IDE之后第一步是添加ESP32開發(fā)板支持包。打開“文件”-“首選項”在“附加開發(fā)板管理器網(wǎng)址”里添加以下地址https://dl.espressif.com/dl/package_esp32_index.json然后打開“工具”-“開發(fā)板”-“開發(fā)板管理器”搜索“esp32”找到“esp32 by Espressif Systems”安裝。這個包比較大下載時間取決于網(wǎng)絡(luò)情況耐心等待即可。安裝完成后在開發(fā)板列表里選擇“ESP32 Dev Module”就完成了基礎(chǔ)配置。有一個小坑剛安裝完支持包后編譯速度會偏慢因為第一次要編譯很多底層的庫后面就快了。如果編譯報錯提示找不到“WiFi.h”說明開發(fā)板支持包沒裝好重新在開發(fā)板管理器里檢查一下。3.2 燒錄與串口驗證在正式寫代碼之前建議先用一個最簡單的Blink程序驗證工具鏈?zhǔn)欠裾?。Arduino自帶的Blink示例就可以只是注意ESP32板載LED的引腳通常接在GPIO2上要把示例里的LED_BUILTIN換成2或者直接用LED_BUILTIN不同開發(fā)板定義不一樣看自己的板子說明書。編譯通過后連接ESP32到電腦在“工具”-“端口”里選擇對應(yīng)的COM口。點擊上傳按鈕如果出現(xiàn)連接失敗試著按住開發(fā)板上的BOOT鍵再點上傳等待出現(xiàn)“Connecting..._____”之后松手。燒錄完成后板載LED應(yīng)該開始閃爍這說明環(huán)境已經(jīng)OK了。4. JY61P數(shù)據(jù)協(xié)議解析與讀取代碼4.1 JY61P的輸出幀結(jié)構(gòu)JY61P默認(rèn)波特率是115200數(shù)據(jù)位8停止位1無校驗也就是常說的115200 8N1。模塊上電后會主動向上位機發(fā)送數(shù)據(jù)包不需要發(fā)送任何配置指令。這點很方便特別是對新手來說少了一道交互流程。它的數(shù)據(jù)包以幀頭0x55起始第二個字節(jié)是數(shù)據(jù)類型標(biāo)識常見的三種0x51加速度數(shù)據(jù)包0x52角速度數(shù)據(jù)包0x53角度數(shù)據(jù)包本項目只需要角度數(shù)據(jù)所以關(guān)注0x53這個類型即可。一個完整的角度數(shù)據(jù)包總長11字節(jié)結(jié)構(gòu)如下字節(jié)序號內(nèi)容說明00x55幀頭10x53數(shù)據(jù)類型角度包2RollLRoll角度低字節(jié)3RollHRoll角度高字節(jié)4PitchLPitch角度低字節(jié)5PitchHPitch角度高字節(jié)6YawLYaw角度低字節(jié)7YawHYaw角度高字節(jié)8TL溫度低字節(jié)9TH溫度高字節(jié)10SUM校驗和角度數(shù)據(jù)是int16類型小端序存儲實際角度值的換算公式是角度 原始值 / 32768 * 180度。因為模塊內(nèi)部的角度量程是±180度而int16的滿量程正好是±32768對應(yīng)關(guān)系非常直接。溫度值也是int16換算公式是 溫度 原始值 / 100攝氏度不過本項目的重點是姿態(tài)角溫度不處理也沒關(guān)系。校驗和的算法是從幀頭0x55開始一直到第9字節(jié)的TH所有字節(jié)累加取累加和的低8位結(jié)果等于第10字節(jié)的SUM。也就是說SUM (0x55 0x53 RollL RollH PitchL PitchH YawL YawH TL TH) 0xFF這個校驗非常重要因為串口通信在干擾環(huán)境下偶爾會出錯如果不對校驗和可能解析出完全離譜的角度值導(dǎo)致上位機數(shù)據(jù)亂跳。4.2 解析思路狀態(tài)機處理串口字節(jié)流串口的本質(zhì)是一個字節(jié)一個字節(jié)地往外吐數(shù)據(jù)中間沒有明顯的“幀”邊界。如果直接在循環(huán)里一個個找0x55非常容易出錯因為數(shù)據(jù)內(nèi)容里也可能出現(xiàn)0x55這個字節(jié)比如角度值恰好是0x55XX。這時候就需要用狀態(tài)機來切幀。狀態(tài)機的思路是定義一個狀態(tài)變量記錄當(dāng)前解析到了幀的哪個位置。一開始處于“等待幀頭”狀態(tài)只有收到0x55才進(jìn)入“等待類型”狀態(tài)收到合法的類型字節(jié)0x51/0x52/0x53后進(jìn)入“接收數(shù)據(jù)”狀態(tài)連續(xù)接收8個數(shù)據(jù)字節(jié)后進(jìn)入“等待校驗”狀態(tài)收到校驗字節(jié)后判斷是否正確然后無論對錯都回到“等待幀頭”狀態(tài)。這樣做的好處是嚴(yán)格的同步機制即使中間某一幀壞了下一幀的幀頭也能重新對齊不會導(dǎo)致后續(xù)所有數(shù)據(jù)全部錯位。這也是工業(yè)串口協(xié)議解析的標(biāo)準(zhǔn)做法。4.3 Arduino讀取代碼示例下面給出一段獨立的JY61P讀取代碼使用ESP32的UART2GPIO16接收把解析出的角度通過USB串口打印到Arduino IDE的串口監(jiān)視器上。先跑通這一步再上WiFi。// ESP32 JY61P 串口讀取和解析 // 使用 UART2: RXGPIO16, TXGPIO17 #define RXD2 16 #define TXD2 17 // 解析狀態(tài)機 enum State { WAIT_HEAD, WAIT_TYPE, WAIT_DATA, WAIT_CHECK }; State state WAIT_HEAD; uint8_t frameBuf[11]; int frameIndex 0; uint8_t frameType 0; uint8_t checkSum 0; void handleData(uint8_t *buf) { // 0x53 角度包 if (buf[1] 0x53) { int16_t rollRaw (buf[3] 8) | buf[2]; int16_t pitchRaw (buf[5] 8) | buf[4]; int16_t yawRaw (buf[7] 8) | buf[6]; float roll rollRaw / 32768.0 * 180.0; float pitch pitchRaw / 32768.0 * 180.0; float yaw yawRaw / 32768.0 * 180.0; Serial.printf(roll%.2f pitch%.2f yaw%.2f\n, roll, pitch, yaw); } } void parseByte(uint8_t c) { switch (state) { case WAIT_HEAD: if (c 0x55) { frameBuf[0] c; checkSum c; state WAIT_TYPE; } break; case WAIT_TYPE: if (c 0x51 || c 0x52 || c 0x53) { frameBuf[1] c; checkSum c; frameType c; frameIndex 2; state WAIT_DATA; } else { state WAIT_HEAD; // 類型不合法重新找?guī)^ } break; case WAIT_DATA: frameBuf[frameIndex] c; checkSum c; frameIndex; if (frameIndex 10) { state WAIT_CHECK; } break; case WAIT_CHECK: if (c (checkSum 0xFF)) { handleData(frameBuf); } state WAIT_HEAD; break; } } void setup() { Serial.begin(115200); Serial2.begin(115200, SERIAL_8N1, RXD2, TXD2); Serial.println(JY61P Test Start); } void loop() { while (Serial2.available()) { parseByte(Serial2.read()); } }燒錄到ESP32后打開串口監(jiān)視器波特率選115200如果接線沒問題可以看到不斷滾動的roll、pitch、yaw數(shù)據(jù)。這時候轉(zhuǎn)動JY61P對應(yīng)的角度值應(yīng)該跟著變化。如果這里能跑通說明整個鏈路的一半已經(jīng)完成了。4.4 備選方案使用WitMotion官方庫有朋友嫌自己寫狀態(tài)機麻煩其實維特智能官方也提供了Arduino庫名字叫“WitMotion”在Arduino庫管理器里可以直接搜到。使用官方庫的代碼如下#include WitMotion.h WitMotion jy61p; void setup() { Serial.begin(115200); jy61p.begin(Serial2); } void loop() { if (jy61p.update()) { Serial.printf(roll%.2f pitch%.2f yaw%.2f\n, jy61p.roll, jy61p.pitch, jy61p.yaw); } delay(10); }官方庫把協(xié)議解析封裝好了配置也簡單適合不想深入了解協(xié)議的人。不過我依然建議自己寫一遍解析代碼因為工業(yè)場景里串口傳感器種類很多幀格式各有不同掌握了狀態(tài)機拆幀的技能換任何傳感器都能應(yīng)對。官方庫是一個黑盒一旦出了問題不好排查。5. 連接WiFi并通過UDP發(fā)送數(shù)據(jù)到電腦5.1 WiFi連接流程串口數(shù)據(jù)解析通了接下來就是給ESP32聯(lián)網(wǎng)。ESP32連接WiFi的代碼很固定無非是這幾步設(shè)置工作模式為STA、調(diào)用begin傳入SSID和密碼、循環(huán)等待連接成功。#include WiFi.h const char* ssid your_wifi_name; const char* password your_wifi_password; void setup() { Serial.begin(115200); WiFi.mode(WIFI_STA); WiFi.begin(ssid, password); Serial.print(Connecting to WiFi); while (WiFi.status() ! WL_CONNECTED) { delay(500); Serial.print(.); } Serial.println(); Serial.print(Connected, IP: ); Serial.println(WiFi.localIP()); }唯一需要特別注意的是頻段問題。ESP32只支持2.4GHz頻段的WiFi如果路由器開了5GHz或者“雙頻合一”模式ESP32很可能連不上或者連上后頻繁掉線。解決辦法是登錄路由器后臺把WiFi頻段固定為2.4GHz或者給ESP32單獨開一個訪客網(wǎng)絡(luò)。這個坑我踩過折騰了半天最后發(fā)現(xiàn)是路由器把2.4G和5G合并了。為了方便調(diào)試建議在路由器里給ESP32分配一個固定IP或者在代碼里手動配置靜態(tài)IP。這樣電腦端監(jiān)聽程序不用每次去查IP變化。但項目初步階段動態(tài)IP也沒問題ESP32上電后打印一下自己的IP就行。5.2 為什么推薦UDP而不是TCP前面提過UDP的優(yōu)勢這里再展開說一下。姿態(tài)數(shù)據(jù)的典型特征是高頻率、小數(shù)據(jù)包、允許少量丟失。JY61P默認(rèn)輸出頻率是100Hz也就是每秒100幀數(shù)據(jù)。如果每幀都用TCP發(fā)送每包都要經(jīng)過三次握手建立的連接通道雖然連接是長連接但TCP的確認(rèn)機制和擁塞控制還是會引入額外延遲尤其是WiFi環(huán)境本身就不穩(wěn)定TCP重傳會造成數(shù)據(jù)包的排隊和亂序。UDP則完全沒有這些負(fù)擔(dān)。數(shù)據(jù)包發(fā)出去就結(jié)束1對1也好1對多也好代碼實現(xiàn)都非常簡單。在局域網(wǎng)這種低延遲、低丟包的環(huán)境下UDP的可靠性其實相當(dāng)高。實測下來100Hz的數(shù)據(jù)流丟包率遠(yuǎn)低于千分之一對姿態(tài)顯示沒有任何影響。當(dāng)然了如果后續(xù)要做的是指令下發(fā)、參數(shù)配置這類要求可靠傳輸?shù)墓δ苣蔷屠侠蠈崒嵱肨CP或者HTTP。UDP適合的是傳感器數(shù)據(jù)上行這種場景要區(qū)分清楚。5.3 UDP發(fā)送代碼與電腦端監(jiān)聽UDP發(fā)送用Arduino的WiFiUDP庫核心就三個方法beginPacket、print/printf、endPacket。示例代碼如下#include WiFiUdp.h WiFiUDP udp; const IPAddress pcIP(192, 168, 1, 100); // 改成你電腦的IP const unsigned int pcPort 8888; void setup() { Serial.begin(115200); WiFi.mode(WIFI_STA); WiFi.begin(ssid, password); while (WiFi.status() ! WL_CONNECTED) { delay(500); } udp.begin(pcPort); // 本機綁定端口可選 } void loop() { udp.beginPacket(pcIP, pcPort); udp.printf(hello from esp32\n); udp.endPacket(); delay(100); }電腦端的IP地址查看方法Windows在命令行里輸入ipconfig找“無線局域網(wǎng)適配器”里的IPv4地址macOS或Linux在終端里輸入ifconfig找en0或wlan0的inet地址。電腦和ESP32必須在同一個網(wǎng)段下比如路由器分配的IP都是192.168.1.x。電腦端用Python寫一個最簡單的UDP監(jiān)聽腳本import socket sock socket.socket(socket.AF_INET, socket.SOCK_DGRAM) sock.bind((0.0.0.0, 8888)) print(Listening on UDP port 8888...) while True: data, addr sock.recvfrom(1024) print(data.decode().strip())運行這個腳本后如果ESP32的WiFi連上了電腦端的防火墻也沒攔UDP包終端里應(yīng)該能不斷打印出ESP32發(fā)來的“hello”字符串。這一步通了說明“傳感器數(shù)據(jù)無線回傳”的核心鏈路已經(jīng)打通。6. 整體聯(lián)調(diào)JY61P數(shù)據(jù)經(jīng)WiFi回傳電腦6.1 完整代碼整合把前幾節(jié)的代碼整合起來就是一個完整的ESP32JY61PWiFi無線姿態(tài)采集程序。代碼里已經(jīng)包含了SSID、密碼、電腦IP和端口按自己的環(huán)境修改即可。#include WiFi.h #include WiFiUdp.h // WiFi配置 const char* ssid your_wifi_name; const char* password your_wifi_password; // 電腦端配置 const IPAddress pcIP(192, 168, 1, 100); const unsigned int pcPort 8888; WiFiUDP udp; // JY61P 串口引腳 #define RXD2 16 #define TXD2 17 // 解析狀態(tài)機 enum State { WAIT_HEAD, WAIT_TYPE, WAIT_DATA, WAIT_CHECK }; State state WAIT_HEAD; uint8_t frameBuf[11]; int frameIndex 0; uint8_t frameType 0; uint8_t checkSum 0; void sendAngleByUDP(uint8_t *buf) { int16_t rollRaw (buf[3] 8) | buf[2]; int16_t pitchRaw (buf[5] 8) | buf[4]; int16_t yawRaw (buf[7] 8) | buf[6]; float roll rollRaw / 32768.0 * 180.0; float pitch pitchRaw / 32768.0 * 180.0; float yaw yawRaw / 32768.0 * 180.0; // 數(shù)據(jù)格式: roll,pitch,yaw udp.beginPacket(pcIP, pcPort); udp.printf(%.2f,%.2f,%.2f\n, roll, pitch, yaw); udp.endPacket(); } void parseByte(uint8_t c) { switch (state) { case WAIT_HEAD: if (c 0x55) { frameBuf[0] c; checkSum c; state WAIT_TYPE; } break; case WAIT_TYPE: if (c 0x51 || c 0x52 || c 0x53) { frameBuf[1] c; checkSum c; frameType c; frameIndex 2; state WAIT_DATA; } else { state WAIT_HEAD; } break; case WAIT_DATA: frameBuf[frameIndex] c; checkSum c; frameIndex; if (frameIndex 10) { state WAIT_CHECK; } break; case WAIT_CHECK: if (c (checkSum 0xFF)) { if (frameType 0x53) { sendAngleByUDP(frameBuf); } } state WAIT_HEAD; break; } } void setup() { Serial.begin(115200); Serial2.begin(115200, SERIAL_8N1, RXD2, TXD2); // 連接WiFi WiFi.mode(WIFI_STA); WiFi.begin(ssid, password); Serial.print(Connecting to WiFi); while (WiFi.status() ! WL_CONNECTED) { delay(500); Serial.print(.); } Serial.println(); Serial.print(ESP32 IP: ); Serial.println(WiFi.localIP()); udp.begin(pcPort); } void loop() { while (Serial2.available()) { parseByte(Serial2.read()); } }代碼邏輯并不復(fù)雜setup里完成串口、WiFi和UDP的初始化loop里持續(xù)從Serial2讀取JY61P發(fā)來的字節(jié)交給狀態(tài)機解析解析到合法的0x53角度包后計算三個歐拉角通過UDP發(fā)送到電腦端。Serial.printf打印調(diào)試信息依舊走USB串口完全不影響WiFi發(fā)送。6.2 電腦端接收與可視化測試電腦端運行前面那個Python監(jiān)聽腳本接收波特率匹配那次我們已經(jīng)確認(rèn)了。如果一切正常終端里會出現(xiàn)類似下面的輸出12.34,5.67,-89.01 12.35,5.68,-89.02 12.36,5.69,-89.03三列數(shù)據(jù)分別對應(yīng)roll、pitch、yaw。物理上轉(zhuǎn)動JY61P對應(yīng)數(shù)值會實時變化響應(yīng)延遲在毫秒級肉眼幾乎感覺不到延遲。想看可視化的曲線可以用Python的matplotlib庫做一個簡單的實時繪圖窗口。這個屬于擴(kuò)展內(nèi)容網(wǎng)上有很多現(xiàn)成的例子關(guān)鍵詞搜“matplotlib 實時繪圖 UDP”就能找到。如果想做更專業(yè)的上位機可以用pyqtgraph或Processing效果會好很多。6.3 發(fā)送頻率與數(shù)據(jù)包格式設(shè)計JY61P默認(rèn)輸出頻率是100Hz但實際上并不是每個0x53角度包都干凈利落地被發(fā)送。串口中斷和WiFi發(fā)送都需要時間如果每收到一幀就立刻UDP發(fā)送可能會出現(xiàn)微小的節(jié)奏抖動。實測下來100Hz完全能扛住但如果電腦端的接收緩沖區(qū)不夠大長時間運行有掉包風(fēng)險。我建議根據(jù)實際需求控制發(fā)送頻率。如果只是簡單監(jiān)控可以加一個簡單的節(jié)流判斷比如每50ms發(fā)送一次最新角度也就是20Hz。如果做機械臂姿態(tài)反饋或者自動控制那100Hz是底線建議保持全速發(fā)送。UDP包本身很小一個包不到30字節(jié)100包/秒也就是每秒3KB流量對WiFi來說九牛一毛。數(shù)據(jù)包格式建議用CSV也就是純文本加逗號分隔。因為UDP本身沒有規(guī)定格式用CSV最直觀電腦端一行split(,)就能解析成三個浮點數(shù)。如果以后要傳輸更多字段比如加速度、角速度、溫度可以繼續(xù)往后面追加格式保持兼容即可。7. 常見問題與排查技巧實錄做一個項目不踩坑是不可能的。我把實際過程中遇到的典型問題整理成了速查表方便大家對照排查。現(xiàn)象可能原因解決辦法串口監(jiān)視器沒有任何輸出JY61P供電沒接好檢查VCC和GND確認(rèn)共地串口輸出全是亂碼波特率不匹配確認(rèn)JY61P是115200串口監(jiān)視器也選115200數(shù)據(jù)偶發(fā)跳變干擾或接線過長杜邦線盡量短避開電機等強干擾源校驗和頻繁錯誤供電不足導(dǎo)致模塊異常換優(yōu)質(zhì)電源不要用劣質(zhì)USB口供電WiFi連不上路由器開了5GHz或雙頻合一固定到2.4GHz頻段或開訪客網(wǎng)絡(luò)WiFi連上但收不到UDPWindows防火墻攔截在防火墻入站規(guī)則里放行UDP 8888端口燒錄程序失敗GPIO引腳沖突下載時按住BOOT鍵檢查外設(shè)是否占用GPIO0程序燒錄后反復(fù)重啟供電電流不夠換5V/2A電源避免電腦USB口供電不足7.1 串口調(diào)試通了再上WiFi這里分享一個我自己的實戰(zhàn)習(xí)慣永遠(yuǎn)先串口后WiFi。也就是先把JY61P的數(shù)據(jù)通過USB串口打印在電腦上看確認(rèn)硬件接線和解析邏輯完全正確再去連WiFi、寫UDP。這樣一旦出了問題排查范圍能縮小一半。很多新手一上來就把WiFi和UDP代碼全寫上結(jié)果收不到數(shù)據(jù)根本分不清到底是傳感器沒接好還是解析代碼有bug還是WiFi配置有問題。分層驗證是最穩(wěn)的方案雖然多花幾分鐘但能省下一個晚上的排查時間。7.2 供電不足引起的詭異問題ESP32的開發(fā)板在WiFi發(fā)射的時候瞬時電流可能到幾百毫安如果電源質(zhì)量不好電壓會被拉低導(dǎo)致JY61P也工作不穩(wěn)定。癥狀很典型上電時一切正常一旦WiFi連上開始發(fā)送數(shù)據(jù)板子就重啟或者數(shù)據(jù)開始出現(xiàn)校驗錯誤。解決辦法很簡單給ESP32用一個靠譜的5V電源比如手機充電頭或者大容量充電寶盡量不要用電腦前置USB口的供電。JY61P的VCC接在ESP32的3.3V引腳上所以ESP32的電源質(zhì)量直接決定JY61P的穩(wěn)定性。如果單獨給JY61P供3.3V記得GND還是要和ESP32連在一起。7.3 防火墻與端口設(shè)置電腦端收不到UDP數(shù)據(jù)排除WiFi問題后十有八九是防火墻攔截了入站UDP包。Windows的防火墻默認(rèn)情況下會攔截陌生程序的入站流量尤其Python解釋器或者Java虛擬機這類沒有在允許列表里的程序。臨時解決辦法是關(guān)閉防火墻測試注意生產(chǎn)環(huán)境不建議這么干或者更規(guī)范的做法是在“Windows Defender防火墻”的“高級設(shè)置”里添加入站規(guī)則放行UDP協(xié)議、指定本地端口8888、允許所有程序。這樣既安全又能正常接收數(shù)據(jù)。8. 項目擴(kuò)展思路8.1 數(shù)據(jù)記錄與回放如果要做數(shù)據(jù)分析或?qū)嶒瀼?fù)盤可以在電腦端把接收到的數(shù)據(jù)實時寫入CSV文件。Python監(jiān)聽腳本里加幾行文件寫入代碼就行每收到一行數(shù)據(jù)就追加到文件末尾。這樣跑完一次實驗就能拿到一份完整的姿態(tài)數(shù)據(jù)導(dǎo)入Excel或者M(jìn)atlab分析都很方便。8.2 多傳感器接入與多軸可視化一個ESP32的UART2只能接一個JY61P但ESP32其實有多個UART。比如UART1、UART2都可以接只是注意不要和Flash芯片占用的引腳沖突GPIO6~GPIO11默認(rèn)給Flash用了盡量避開。如果你需要監(jiān)測多個姿態(tài)點比如人體四肢可以買多個JY61P模塊分別接到不同的UART上然后在ESP32上按端口區(qū)分?jǐn)?shù)據(jù)來源統(tǒng)一通過UDP發(fā)到電腦。電腦端接收時可以根據(jù)數(shù)據(jù)包里的設(shè)備標(biāo)識來區(qū)分不同的傳感器節(jié)點。這樣一套系統(tǒng)就能同時監(jiān)控多個姿態(tài)點做人體姿態(tài)捕捉的基礎(chǔ)原型完全夠用。8.3 與ROS 2/micro-ROS結(jié)合如果你做機器人開發(fā)可以把這套無線姿態(tài)采集方案接入ROS 2生態(tài)。ESP32上運行micro-ROS節(jié)點把roll、pitch、yaw發(fā)布為sensor_msgs/Imu或者自定義的Float32MultiArray話題上位機直接用ROS 2工具鏈訂閱數(shù)據(jù)。這個方向適合已經(jīng)接觸過ROS 2的朋友初期不建議直接上手先把基礎(chǔ)鏈路跑通再說。8.4 增加自動校準(zhǔn)與交互功能JY61P出廠前已經(jīng)做了標(biāo)定零點漂移很小直接可用。但如果你追求更高的精度可以加上電自校準(zhǔn)邏輯在模塊靜止?fàn)顟B(tài)下采集100幀數(shù)據(jù)求平均作為零偏然后在后續(xù)數(shù)據(jù)中扣除。這個校準(zhǔn)邏輯放在ESP32解析代碼里實現(xiàn)即可不需要改模塊配置。另外如果想讓系統(tǒng)具備交互能力可以把ESP32的藍(lán)牙也打開通過手機App直接看角度。有了WiFi和藍(lán)牙雙重通道這套系統(tǒng)的應(yīng)用范圍會寬很多。這個項目看著簡單但真正動手做一遍串口協(xié)議解析、狀態(tài)機設(shè)計、WiFi通信這幾塊基本功都能得到實打?qū)嵉腻憻?。我做完之后的體會是任何一個環(huán)節(jié)不扎實都會出問題尤其是串口字節(jié)處理寫代碼時覺得思路很清晰一上板子各種邊角情況全冒出來了。建議你從串口讀數(shù)據(jù)開始一步一步往上加功能不要一口吃成胖子。無線數(shù)據(jù)傳輸這條鏈路打通之后后面想接什么傳感器、做什么應(yīng)用都只是換個協(xié)議解析的事。