5 changed files with 899 additions and 10 deletions
@ -0,0 +1,393 @@
@@ -0,0 +1,393 @@
|
||||
/** |
||||
* 姿态传感器通信模块 (Modbus RTU) |
||||
* 协议文档: https://wit-motion.yuque.com/wumwnr/ltst03/oex3ay
|
||||
* 产品: HWT6053-485 (高精度Modbus协议) |
||||
* 数据格式: 8N1, 从站地址默认 0x50 |
||||
* 指令必须在 10S 内完成，否则自动上锁，需先解锁 |
||||
*/ |
||||
const { SerialPort } = require('serialport') |
||||
|
||||
// ======================== CRC-16 Modbus ========================
|
||||
const crc16Table = new Uint16Array(256) |
||||
for (let i = 0; i < 256; i++) { |
||||
let crc = i |
||||
for (let j = 0; j < 8; j++) { |
||||
crc = (crc & 1) ? (0xA001 ^ (crc >> 1)) : (crc >> 1) |
||||
} |
||||
crc16Table[i] = crc |
||||
} |
||||
|
||||
/** |
||||
* CRC-16 Modbus 计算 |
||||
* @param {Buffer} data |
||||
* @returns {Buffer} [CRCH, CRCL] — 高字节在前（文档要求 CRCH 在前） |
||||
*/ |
||||
function crc16Modbus(data) { |
||||
let crc = 0xFFFF |
||||
for (const b of data) { |
||||
crc = crc16Table[(crc ^ b) & 0xFF] ^ (crc >> 8) |
||||
} |
||||
return Buffer.from([(crc >> 8) & 0xFF, crc & 0xFF]) // [CRCH, CRCL]
|
||||
} |
||||
|
||||
// ======================== 常量 ========================
|
||||
const DEV_ADDR = 0x50 // 从站地址（默认）
|
||||
|
||||
// ======================== Modbus 命令构建 ========================
|
||||
|
||||
/** 解锁指令 (写寄存器 0x69, 值 0xB588) */ |
||||
function buildUnlockCmd() { |
||||
const buf = Buffer.from([DEV_ADDR, 0x06, 0x00, 0x69, 0xB5, 0x88]) |
||||
return Buffer.concat([buf, crc16Modbus(buf)]) |
||||
} |
||||
|
||||
/** 保存指令 (写寄存器 0x00, 值 0x0000) */ |
||||
function buildSaveCmd() { |
||||
const buf = Buffer.from([DEV_ADDR, 0x06, 0x00, 0x00, 0x00, 0x00]) |
||||
return Buffer.concat([buf, crc16Modbus(buf)]) |
||||
} |
||||
|
||||
/** |
||||
* Modbus 读保持寄存器命令 (功能码 0x03) |
||||
* @param {number} startAddr - 起始寄存器地址 |
||||
* @param {number} count - 读取寄存器数量 |
||||
*/ |
||||
function buildReadRegistersCmd(startAddr, count) { |
||||
const buf = Buffer.from([DEV_ADDR, 0x03, (startAddr >> 8) & 0xFF, startAddr & 0xFF, (count >> 8) & 0xFF, count & 0xFF]) |
||||
return Buffer.concat([buf, crc16Modbus(buf)]) |
||||
} |
||||
|
||||
// ======================== Modbus 响应解析 ========================
|
||||
|
||||
/** |
||||
* 解析 Modbus 读寄存器响应 |
||||
* 响应格式: 地址(1B) | 功能码(1B) | 字节数(1B) | 数据(NB) | CRC16(2B) |
||||
* 返回原始 16-bit 寄存器值数组 |
||||
*/ |
||||
function parseModbusResponse(buf) { |
||||
if (buf.length < 5) return null |
||||
|
||||
// 校验地址和功能码
|
||||
if (buf[0] !== DEV_ADDR || buf[1] !== 0x03) return null |
||||
|
||||
// CRC 校验
|
||||
const crcRcv = buf.slice(buf.length - 2) |
||||
const crcCal = crc16Modbus(buf.slice(0, buf.length - 2)) |
||||
if (crcRcv[0] !== crcCal[0] || crcRcv[1] !== crcCal[1]) { |
||||
console.log('[sensor] CRC校验失败: 收到=' + crcRcv.toString('hex').toUpperCase() + ' 计算=' + crcCal.toString('hex').toUpperCase()) |
||||
return null |
||||
} |
||||
|
||||
const byteCount = buf[2] |
||||
if (buf.length !== 5 + byteCount) { |
||||
console.log('[sensor] 长度不匹配: 期望=' + (5 + byteCount) + ' 收到=' + buf.length) |
||||
return null |
||||
} |
||||
|
||||
const data = [] |
||||
for (let i = 0; i < byteCount; i += 2) { |
||||
data.push((buf[3 + i] << 8) | buf[3 + i + 1]) |
||||
} |
||||
return data |
||||
} |
||||
|
||||
/** |
||||
* 将 16-bit 有符号值转为 int16 |
||||
*/ |
||||
function toInt16(val) { |
||||
return val >= 0x8000 ? val - 0x10000 : val |
||||
} |
||||
|
||||
/** |
||||
* 将 32-bit 值（高位+低位组合）转为有符号 int32 |
||||
*/ |
||||
function toInt32(hi, lo) { |
||||
let val = (hi << 16) | (lo & 0xFFFF) |
||||
return val >= 0x80000000 ? val - 0x100000000 : val |
||||
} |
||||
|
||||
// ======================== 传感器类 ========================
|
||||
class WitMotionSensor { |
||||
/** |
||||
* @param {Object} options |
||||
* @param {Function} options.onData - 回调函数，接收 { method, sensor, value } |
||||
* @param {Function} options.onLog - 日志回调 (level, msg) |
||||
*/ |
||||
constructor(options = {}) { |
||||
this.onData = options.onData || (() => {}) |
||||
this.onLog = options.onLog || ((level, msg) => console.log(`[sensor] ${msg}`)) |
||||
this.port = null |
||||
this.timer = null |
||||
this.connected = false |
||||
|
||||
// Modbus 响应缓冲
|
||||
this._modbusBuf = Buffer.alloc(0) |
||||
|
||||
// 设备类型
|
||||
this.sensorName = 'HWT6053' |
||||
} |
||||
|
||||
/** 日志辅助 */ |
||||
_log(msg) { this.onLog('info', msg) } |
||||
_warn(msg) { this.onLog('warn', msg) } |
||||
_error(msg) { this.onLog('error', msg) } |
||||
|
||||
/** 扫描并连接传感器 */ |
||||
async connect() { |
||||
try { |
||||
this._log('开始扫描串口设备...') |
||||
const ports = await SerialPort.list() |
||||
this._log(`检测到 ${ports.length} 个串口:`) |
||||
for (const p of ports) { |
||||
this._log(` ${p.path} VID=${p.vendorId || 'N/A'} PID=${p.productId || 'N/A'} 制造商=${p.manufacturer || 'N/A'}`) |
||||
} |
||||
|
||||
// 优先找 CH340 / CP210x / FTDI 芯片的串口
|
||||
let sensorPort = ports.find(p => |
||||
p.vendorId && ['1a86', '10c4', '0403'].includes(p.vendorId.toLowerCase()) |
||||
) |
||||
if (!sensorPort) { |
||||
sensorPort = ports.find(p => p.path && /^COM\d+$/i.test(p.path)) |
||||
} |
||||
if (!sensorPort) { |
||||
this._error('未找到串口设备') |
||||
return false |
||||
} |
||||
|
||||
this._log(`选定串口: ${sensorPort.path}`) |
||||
|
||||
// 按优先级尝试波特率
|
||||
const baudRates = [9600, 115200] // 默认 9600，试试 115200
|
||||
for (const baud of baudRates) { |
||||
const ok = await this._tryOpen(sensorPort.path, baud) |
||||
if (ok) return true |
||||
} |
||||
|
||||
this._error('所有波特率均失败，请检查串口连接') |
||||
return false |
||||
} catch (err) { |
||||
this._error('扫描失败: ' + err.message) |
||||
return false |
||||
} |
||||
} |
||||
|
||||
/** 尝试以指定波特率打开串口 */ |
||||
_tryOpen(path, baudRate) { |
||||
return new Promise((resolve) => { |
||||
this._log(`尝试 ${path} @ ${baudRate}...`) |
||||
|
||||
const port = new SerialPort({ |
||||
path, |
||||
baudRate, |
||||
dataBits: 8, |
||||
parity: 'none', |
||||
stopBits: 1, |
||||
autoOpen: false, |
||||
dtr: false, |
||||
rts: false |
||||
}) |
||||
|
||||
port.open((err) => { |
||||
if (err) { |
||||
this._warn(`${path} @ ${baudRate} 打开失败: ${err.message}`) |
||||
resolve(false) |
||||
return |
||||
} |
||||
|
||||
this._log(`=== 已连接: ${path} @ ${baudRate} ===`) |
||||
this.port = port |
||||
this.connected = true |
||||
this._modbusBuf = Buffer.alloc(0) |
||||
this._setupListeners() |
||||
|
||||
// 连接后先解锁并查询数据
|
||||
this._initialSetup() |
||||
|
||||
resolve(true) |
||||
}) |
||||
}) |
||||
} |
||||
|
||||
/** 初始配置：解锁 + 延时后验证 + 启动轮询 */ |
||||
_initialSetup() { |
||||
if (!this.port) return |
||||
|
||||
// 1) 解锁
|
||||
this._log('步骤1: 发送解锁指令...') |
||||
try { this.port.write(buildUnlockCmd()) } catch (e) { this._error('解锁发送失败: ' + e.message) } |
||||
|
||||
// 2) 500ms 后发读命令验证连接
|
||||
setTimeout(() => { |
||||
this._log('步骤2: 发送读指令验证连接...') |
||||
this._queryAll() |
||||
}, 500) |
||||
|
||||
// 3) 1.5s 后如果收到数据则启动轮询，否则重试解锁
|
||||
setTimeout(() => { |
||||
if (this.connected) { |
||||
this._log('步骤3: 启动定时轮询 (每200ms)...') |
||||
// 再发一次解锁（确保后续写操作不超时）
|
||||
try { this.port.write(buildUnlockCmd()) } catch {} |
||||
this._startPolling() |
||||
} |
||||
}, 1500) |
||||
} |
||||
|
||||
/** 查询所有传感器数据（一次性读取 0x34 ~ 0x43 共 16 个寄存器） */ |
||||
_queryAll() { |
||||
if (!this.port || !this.connected) return |
||||
try { |
||||
// 一次读取: AX~TEMP (寄存器 0x34~0x43 = 52~67 = 16个寄存器)
|
||||
// 包含: 加速度(3) + 角速度(3) + 磁场(3) + 角度(6) + 温度(1) = 16
|
||||
const cmd = buildReadRegistersCmd(0x34, 16) |
||||
this.port.write(cmd) |
||||
} catch (e) { |
||||
this._error('查询发送失败: ' + e.message) |
||||
} |
||||
} |
||||
|
||||
/** 启动定时轮询 */ |
||||
_startPolling() { |
||||
if (!this.connected) return |
||||
this._queryAll() |
||||
if (this.timer) clearInterval(this.timer) |
||||
this.timer = setInterval(() => this._queryAll(), 200) |
||||
} |
||||
|
||||
/** 注册串口事件监听 */ |
||||
_setupListeners() { |
||||
let dataCount = 0 |
||||
this.port.on('data', (chunk) => { |
||||
dataCount++ |
||||
if (dataCount <= 3) { |
||||
this._log(`收到原始数据 #${dataCount}: ${chunk.length} 字节 [${chunk.toString('hex').toUpperCase()}]`) |
||||
} |
||||
this._onData(chunk) |
||||
}) |
||||
this.port.on('error', (err) => this._error('串口错误: ' + err.message)) |
||||
this.port.on('close', (err) => { |
||||
this._warn(`串口已断开${err ? ': ' + err.message : ''}`) |
||||
this.connected = false |
||||
if (this.timer) { clearInterval(this.timer); this.timer = null } |
||||
}) |
||||
} |
||||
|
||||
/** 数据接收处理 */ |
||||
_onData(chunk) { |
||||
this._modbusBuf = Buffer.concat([this._modbusBuf, chunk]) |
||||
|
||||
// 尝试从缓冲中解析 Modbus 帧
|
||||
while (this._modbusBuf.length >= 5) { |
||||
// 查找帧头 (设备地址)
|
||||
const idx = this._modbusBuf.indexOf(DEV_ADDR) |
||||
if (idx === -1) { this._modbusBuf = Buffer.alloc(0); break } |
||||
if (idx > 0) { |
||||
this._log(`跳过 ${idx} 个非帧头字节`) |
||||
this._modbusBuf = this._modbusBuf.slice(idx) |
||||
} |
||||
|
||||
if (this._modbusBuf.length < 5) break |
||||
|
||||
// 功能码必须是 0x03
|
||||
if (this._modbusBuf[1] !== 0x03) { |
||||
this._log(`非 0x03 响应, 功能码=0x${this._modbusBuf[1].toString(16)}`) |
||||
this._modbusBuf = this._modbusBuf.slice(1) |
||||
continue |
||||
} |
||||
|
||||
const byteCount = this._modbusBuf[2] |
||||
const expectedLen = 3 + 1 + byteCount + 2 // addr + func + byteCount + data + crc = 5 + byteCount
|
||||
if (this._modbusBuf.length < expectedLen) break // 等待更多数据
|
||||
|
||||
const frame = this._modbusBuf.slice(0, expectedLen) |
||||
this._modbusBuf = this._modbusBuf.slice(expectedLen) |
||||
|
||||
// 解析
|
||||
const regs = parseModbusResponse(frame) |
||||
if (regs) { |
||||
this._log(`Modbus解析成功: ${regs.length} 个寄存器: [${regs.map(r => '0x' + r.toString(16).padStart(4, '0').toUpperCase()).join(', ')}]`) |
||||
this._emitModbusData(regs) |
||||
} else { |
||||
this._log(`Modbus解析失败, 原始帧: [${frame.toString('hex').toUpperCase()}]`) |
||||
} |
||||
} |
||||
|
||||
// 防止无限增长
|
||||
if (this._modbusBuf.length > 512) { |
||||
this._log('缓冲溢出，重置') |
||||
this._modbusBuf = Buffer.alloc(0) |
||||
} |
||||
} |
||||
|
||||
/** 发射 Modbus 解析结果，根据寄存器数量推断数据类型 */ |
||||
_emitModbusData(regs) { |
||||
// 寄存器布局: [AX, AY, AZ, GX, GY, GZ, HX, HY, HZ, LRoll, HRoll, LPitch, HPitch, LYaw, HYaw, TEMP]
|
||||
// 索引: 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15
|
||||
const len = regs.length |
||||
|
||||
if (len >= 16) { |
||||
// 16 个寄存器：完整数据
|
||||
const ax = toInt16(regs[0]), ay = toInt16(regs[1]), az = toInt16(regs[2]) |
||||
const gx = toInt16(regs[3]), gy = toInt16(regs[4]), gz = toInt16(regs[5]) |
||||
const hx = toInt16(regs[6]), hy = toInt16(regs[7]), hz = toInt16(regs[8]) |
||||
const lroll = regs[9], hroll = regs[10] |
||||
const lpitch = regs[11], hpitch = regs[12] |
||||
const lyaw = regs[13], hyaw = regs[14] |
||||
const temp = toInt16(regs[15]) |
||||
|
||||
// 加速度 = 原始值/32768 * 16g
|
||||
const accelVal = { |
||||
ax: +(ax / 32768 * 16).toFixed(4), |
||||
ay: +(ay / 32768 * 16).toFixed(4), |
||||
az: +(az / 32768 * 16).toFixed(4) |
||||
} |
||||
this._log('→ 加速度: ' + JSON.stringify(accelVal)) |
||||
this.onData({ method: 'acceleration', sensor: this.sensorName, value: accelVal }) |
||||
|
||||
// 角速度 = 原始值/32768 * 2000°/s
|
||||
const gyroVal = { |
||||
wx: +(gx / 32768 * 2000).toFixed(2), |
||||
wy: +(gy / 32768 * 2000).toFixed(2), |
||||
wz: +(gz / 32768 * 2000).toFixed(2) |
||||
} |
||||
this._log('→ 角速度: ' + JSON.stringify(gyroVal)) |
||||
this.onData({ method: 'angularVelocity', sensor: this.sensorName, value: gyroVal }) |
||||
|
||||
// 磁场
|
||||
const magVal = { hx, hy, hz } |
||||
this._log('→ 磁场: ' + JSON.stringify(magVal)) |
||||
this.onData({ method: 'magnetic', sensor: this.sensorName, value: magVal }) |
||||
|
||||
// 角度 = (高16位<<16 | 低16位) / 1000°
|
||||
const roll = toInt32(hroll, lroll) |
||||
const pitch = toInt32(hpitch, lpitch) |
||||
const yaw = toInt32(hyaw, lyaw) |
||||
const angleVal = { |
||||
roll: +(roll / 1000).toFixed(2), |
||||
pitch: +(pitch / 1000).toFixed(2), |
||||
yaw: +(yaw / 1000).toFixed(2) |
||||
} |
||||
this._log('→ 角度: ' + JSON.stringify(angleVal)) |
||||
this.onData({ method: 'angle', sensor: this.sensorName, value: angleVal }) |
||||
|
||||
// 温度 = 原始值/100 ℃
|
||||
const tempVal = { temp: +(temp / 100).toFixed(2) } |
||||
this._log('→ 温度: ' + JSON.stringify(tempVal)) |
||||
this.onData({ method: 'temperature', sensor: this.sensorName, value: tempVal }) |
||||
|
||||
} else { |
||||
this._log('未知寄存器数量: ' + len) |
||||
} |
||||
} |
||||
|
||||
/** 断开连接 */ |
||||
disconnect() { |
||||
this.connected = false |
||||
if (this.timer) { clearInterval(this.timer); this.timer = null } |
||||
if (this.port && this.port.isOpen) { |
||||
try { this.port.close() } catch {} |
||||
} |
||||
} |
||||
} |
||||
|
||||
module.exports = { WitMotionSensor } |
||||
Loading…
Reference in new issue