You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
393 lines
12 KiB
393 lines
12 KiB
/** |
|
* 姿态传感器通信模块 (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 }
|
|
|