5 changed files with 899 additions and 10 deletions
@ -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