[real] 整理 Python Sim2Real v2
This commit is contained in:
@@ -0,0 +1,3 @@
|
||||
from drivers.motor_driver import RobStrideDriver, RobStrideMotor, MotorState
|
||||
from drivers.motor_params import CommunicationType, ParamIndex, RunMode
|
||||
from drivers.usb_can_adapter import DmUsbAdapter
|
||||
@@ -0,0 +1,371 @@
|
||||
import struct
|
||||
import time
|
||||
import queue
|
||||
import numpy as np
|
||||
from typing import Dict, Optional, Any, List
|
||||
from dataclasses import dataclass
|
||||
|
||||
from drivers.usb_can_adapter import DmUsbAdapter
|
||||
from drivers.motor_params import (
|
||||
CommunicationType, ParamIndex, ParamType,
|
||||
MODEL_MIT_POSITION_TABLE, MODEL_MIT_VELOCITY_TABLE,
|
||||
MODEL_MIT_TORQUE_TABLE, MODEL_MIT_KP_TABLE, MODEL_MIT_KD_TABLE,
|
||||
get_pack_format, PARAM_TABLE
|
||||
)
|
||||
|
||||
@dataclass
|
||||
class MotorState:
|
||||
position: float = 0.0
|
||||
velocity: float = 0.0
|
||||
torque: float = 0.0
|
||||
temperature: float = 0.0
|
||||
current: float = 0.0
|
||||
update_count: int = 0
|
||||
|
||||
class RobStrideMotor:
|
||||
def __init__(self, name: str, motor_id: int, model: str):
|
||||
"""
|
||||
初始化电机对象。
|
||||
|
||||
:param name: 电机名称 (例如 "knee")
|
||||
:param motor_id: 电机 ID
|
||||
:param model: 电机型号 (例如 "rs-06")
|
||||
"""
|
||||
self.name = name
|
||||
self.id = motor_id
|
||||
self.model = model
|
||||
self.state = MotorState()
|
||||
|
||||
def update_state(self, pos: float, vel: float, torque: float, temp: float, current: float = 0.0):
|
||||
"""
|
||||
更新电机状态。
|
||||
"""
|
||||
self.state.position = pos
|
||||
self.state.velocity = vel
|
||||
self.state.torque = torque
|
||||
self.state.temperature = temp
|
||||
self.state.update_count += 1
|
||||
if current != 0.0:
|
||||
self.state.current = current
|
||||
|
||||
class RobStrideDriver:
|
||||
def __init__(self, port: str, debug: bool = False):
|
||||
"""
|
||||
初始化驱动器。
|
||||
|
||||
:param port: 串口名称
|
||||
:param debug: 是否开启调试模式
|
||||
"""
|
||||
self.adapter = DmUsbAdapter(port, debug=debug)
|
||||
self.motors: Dict[str, RobStrideMotor] = {}
|
||||
self.motors_by_id: Dict[int, RobStrideMotor] = {}
|
||||
self.host_id = 0xFD # 根据文档，主机 ID 默认为 0xFD
|
||||
|
||||
self.parameter_values = {} # 读取参数缓存: (motor_id, param_index) -> value
|
||||
|
||||
def connect(self):
|
||||
"""连接到底层适配器。"""
|
||||
self.adapter.open()
|
||||
print(f"已连接到 RobStride 驱动器，端口: {self.adapter.serial.port}")
|
||||
# 设置 CAN 波特率为 1000kbps (Index 0)
|
||||
self.adapter.set_can_baudrate(0)
|
||||
|
||||
def disconnect(self):
|
||||
"""断开连接。"""
|
||||
self.adapter.close()
|
||||
print("已断开 RobStride 驱动器连接")
|
||||
|
||||
def set_can_id(self, current_id: int, new_id: int):
|
||||
"""
|
||||
设置电机 CAN ID。
|
||||
|
||||
:param current_id: 当前电机 ID
|
||||
:param new_id: 新电机 ID
|
||||
"""
|
||||
# Type 7: Set CAN ID
|
||||
# Bits 23-16: New ID (Preset ID)
|
||||
# Bits 15-8: Master ID
|
||||
# Bits 7-0: Target ID
|
||||
extra_data = (new_id << 8) | self.host_id
|
||||
self._send_command(CommunicationType.SET_CAN_ID, extra_data, current_id)
|
||||
print(f"已发送 ID 修改指令: {current_id} -> {new_id} (Master: {self.host_id})")
|
||||
|
||||
def scan_motors(self, timeout: float = 0.1) -> List[int]:
|
||||
"""
|
||||
快速扫描总线上的电机 (ID 1-127)。
|
||||
|
||||
:param timeout: 等待响应的超时时间
|
||||
:return: 发现的电机 ID 列表
|
||||
"""
|
||||
found_ids = []
|
||||
print("正在快速扫描所有电机 (ID 1-127)...")
|
||||
|
||||
# 清空缓冲区
|
||||
while self.adapter.read_can_frame():
|
||||
pass
|
||||
|
||||
# 快速发送查询指令
|
||||
for dev_id in range(1, 128):
|
||||
# 发送获取设备 ID 命令
|
||||
self._send_command(CommunicationType.GET_DEVICE_ID, self.host_id, dev_id)
|
||||
|
||||
# 等待响应
|
||||
start_time = time.time()
|
||||
while time.time() - start_time < timeout:
|
||||
frame = self.adapter.read_can_frame()
|
||||
if frame:
|
||||
can_id, data, cmd, ide, rtr = frame
|
||||
if not ide: continue
|
||||
|
||||
# 解析回复
|
||||
# 通信类型 0 (GET_DEVICE_ID/Status)
|
||||
comm_type = (can_id >> 24) & 0x1F
|
||||
|
||||
if comm_type == CommunicationType.GET_DEVICE_ID: # Type 0
|
||||
# Type 0 回复格式:
|
||||
# Bits 23-8: Status info
|
||||
# Bits 7-0: Motor ID
|
||||
extra_data = (can_id >> 8) & 0xFFFF
|
||||
motor_id = extra_data & 0xFF # Device ID
|
||||
|
||||
if motor_id not in found_ids:
|
||||
print(f"发现电机 ID: {motor_id}")
|
||||
found_ids.append(motor_id)
|
||||
|
||||
return sorted(found_ids)
|
||||
|
||||
|
||||
def add_motor(self, name: str, motor_id: int, model: str):
|
||||
"""
|
||||
添加电机到控制列表。
|
||||
|
||||
:param name: 电机名称
|
||||
:param motor_id: 电机 ID
|
||||
:param model: 电机型号
|
||||
"""
|
||||
motor = RobStrideMotor(name, motor_id, model)
|
||||
self.motors[name] = motor
|
||||
self.motors_by_id[motor_id] = motor
|
||||
|
||||
def _send_command(self, comm_type: int, extra_data: int, device_id: int, data: bytes = b''):
|
||||
# 构建 29 位扩展 CAN ID
|
||||
# Bits 28-24: 通信类型 (Communication Type)
|
||||
# Bits 23-8: 额外数据 (Extra Data)
|
||||
# Bits 7-0: 设备 ID (Device ID)
|
||||
can_id = (comm_type << 24) | (extra_data << 8) | device_id
|
||||
|
||||
# 通过适配器发送
|
||||
# RobStride 使用扩展帧
|
||||
self.adapter.send_can_frame(can_id, data, extended=True)
|
||||
|
||||
def enable(self, motor_name: str):
|
||||
"""使能电机。"""
|
||||
motor = self.motors[motor_name]
|
||||
self._send_command(CommunicationType.ENABLE, self.host_id, motor.id)
|
||||
|
||||
def disable(self, motor_name: str):
|
||||
"""失能电机 (Type 4: Stop)。"""
|
||||
motor = self.motors[motor_name]
|
||||
# Data: 全 0
|
||||
data = bytes([0x00]*8)
|
||||
self._send_command(CommunicationType.DISABLE, self.host_id, motor.id, data)
|
||||
|
||||
def clear_warnings(self, motor_name: str):
|
||||
"""
|
||||
清除警告/故障 (Type 4: Stop Motor with Byte0=1)。
|
||||
根据文档 Type 4: Byte[0]=1 时清除故障。
|
||||
"""
|
||||
motor = self.motors[motor_name]
|
||||
data = bytes([0x01] + [0x00]*7)
|
||||
self._send_command(CommunicationType.DISABLE, self.host_id, motor.id, data)
|
||||
|
||||
def set_zero_position(self, motor_name: str):
|
||||
"""设置电机当前位置为零点。"""
|
||||
motor = self.motors[motor_name]
|
||||
# Type 6: Set Zero Position
|
||||
# Data: Byte0=1
|
||||
data = bytes([0x01] + [0x00]*7)
|
||||
self._send_command(CommunicationType.SET_ZERO_POSITION, self.host_id, motor.id, data)
|
||||
|
||||
def control_mit(self, motor_name: str,
|
||||
position: float, velocity: float,
|
||||
kp: float, kd: float, torque: float):
|
||||
"""
|
||||
发送 MIT 控制指令。
|
||||
|
||||
:param motor_name: 电机名称
|
||||
:param position: 期望位置 (rad)
|
||||
:param velocity: 期望速度 (rad/s)
|
||||
:param kp: 位置增益
|
||||
:param kd: 速度增益
|
||||
:param torque: 前馈力矩 (Nm)
|
||||
"""
|
||||
motor = self.motors[motor_name]
|
||||
model = motor.model
|
||||
|
||||
# 获取限制值
|
||||
p_limit = MODEL_MIT_POSITION_TABLE.get(model, 12.5)
|
||||
v_limit = MODEL_MIT_VELOCITY_TABLE.get(model, 50.0)
|
||||
t_limit = MODEL_MIT_TORQUE_TABLE.get(model, 60.0)
|
||||
kp_limit = MODEL_MIT_KP_TABLE.get(model, 500.0)
|
||||
kd_limit = MODEL_MIT_KD_TABLE.get(model, 5.0)
|
||||
|
||||
# 限幅
|
||||
position = np.clip(position, -p_limit, p_limit)
|
||||
velocity = np.clip(velocity, -v_limit, v_limit)
|
||||
kp = np.clip(kp, 0, kp_limit)
|
||||
kd = np.clip(kd, 0, kd_limit)
|
||||
torque = np.clip(torque, -t_limit, t_limit)
|
||||
|
||||
# 转换为 uint16
|
||||
# Position: [-L, L] -> [0, 65535]
|
||||
p_u16 = int(((position / p_limit) + 1.0) * 32767.0)
|
||||
p_u16 = np.clip(p_u16, 0, 65535)
|
||||
|
||||
# Velocity: [-L, L] -> [0, 65535]
|
||||
v_u16 = int(((velocity / v_limit) + 1.0) * 32767.0)
|
||||
v_u16 = np.clip(v_u16, 0, 65535)
|
||||
|
||||
# Kp: [0, L] -> [0, 65535]
|
||||
kp_u16 = int((kp / kp_limit) * 65535.0)
|
||||
kp_u16 = np.clip(kp_u16, 0, 65535)
|
||||
|
||||
# Kd: [0, L] -> [0, 65535]
|
||||
kd_u16 = int((kd / kd_limit) * 65535.0)
|
||||
kd_u16 = np.clip(kd_u16, 0, 65535)
|
||||
|
||||
# Torque: [-L, L] -> [0, 65535] (发送在 Extra Data 域)
|
||||
t_u16 = int(((torque / t_limit) + 1.0) * 32767.0)
|
||||
t_u16 = np.clip(t_u16, 0, 65535)
|
||||
|
||||
# 打包数据 (大端序)
|
||||
data = struct.pack('>HHHH', p_u16, v_u16, kp_u16, kd_u16)
|
||||
|
||||
# 发送
|
||||
self._send_command(CommunicationType.OPERATION_CONTROL, t_u16, motor.id, data)
|
||||
|
||||
def read_parameter(self, motor_id: int, param_index: int):
|
||||
"""
|
||||
发送读取参数指令 (Type 17)。
|
||||
"""
|
||||
# Type 17
|
||||
# Data: Index (2B) + 00 00 + 00 00 00 00
|
||||
data = struct.pack('<H', param_index) + b'\x00\x00\x00\x00\x00\x00'
|
||||
self._send_command(CommunicationType.READ_PARAMETER, self.host_id, motor_id, data)
|
||||
|
||||
def write_parameter(self, motor_id: int, param_index: int, value: Any):
|
||||
"""
|
||||
发送写入参数指令 (Type 18)。
|
||||
"""
|
||||
param_info = PARAM_TABLE.get(param_index)
|
||||
if not param_info:
|
||||
print(f"未知参数索引: {param_index}")
|
||||
return
|
||||
|
||||
# motor_params.py format: (name, p_type, size)
|
||||
name, p_type, size = param_info
|
||||
|
||||
fmt, _ = get_pack_format(p_type)
|
||||
if not fmt:
|
||||
print(f"不支持的参数类型: {p_type}")
|
||||
return
|
||||
|
||||
# 注意：不再进行范围检查，因为 motor_params.py 中没有定义范围
|
||||
|
||||
# 打包数据
|
||||
val_bytes = struct.pack(fmt, value)
|
||||
# 填充 val_bytes 到 4 字节
|
||||
if len(val_bytes) < 4:
|
||||
val_bytes += b'\x00' * (4 - len(val_bytes))
|
||||
|
||||
# Index (2B) + 00 00 + Value (4B)
|
||||
data = struct.pack('<H', param_index) + b'\x00\x00' + val_bytes
|
||||
|
||||
self._send_command(CommunicationType.WRITE_PARAMETER, self.host_id, motor_id, data)
|
||||
|
||||
def save_parameters(self, motor_id: int):
|
||||
"""
|
||||
保存参数到 EEPROM (Type 22)。
|
||||
"""
|
||||
data = bytes([0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08])
|
||||
self._send_command(CommunicationType.SAVE_PARAMETERS, self.host_id, motor_id, data)
|
||||
|
||||
def process_messages(self, max_messages=50):
|
||||
"""
|
||||
从 CAN 总线读取消息并更新电机状态。
|
||||
"""
|
||||
count = 0
|
||||
while count < max_messages:
|
||||
frame = self.adapter.read_can_frame()
|
||||
if not frame:
|
||||
break
|
||||
|
||||
can_id, data, cmd, ide, rtr = frame
|
||||
|
||||
if not ide:
|
||||
continue # 跳过标准帧
|
||||
|
||||
# 解析扩展 ID
|
||||
comm_type = (can_id >> 24) & 0x1F
|
||||
|
||||
if comm_type == CommunicationType.READ_PARAMETER:
|
||||
# 解析参数读取反馈 (Type 17)
|
||||
extra_data = (can_id >> 8) & 0xFFFF
|
||||
success_flag = (extra_data >> 8) & 0xFF
|
||||
motor_id = extra_data & 0xFF
|
||||
|
||||
if success_flag == 0: # 0 表示成功
|
||||
if len(data) >= 8:
|
||||
param_index = struct.unpack('<H', data[0:2])[0]
|
||||
raw_value = data[4:8]
|
||||
|
||||
param_info = PARAM_TABLE.get(param_index)
|
||||
if param_info:
|
||||
name, p_type, size = param_info
|
||||
fmt, _ = get_pack_format(p_type)
|
||||
if fmt:
|
||||
try:
|
||||
# 根据类型大小解包
|
||||
val_size = struct.calcsize(fmt)
|
||||
val = struct.unpack(fmt, raw_value[:val_size])[0]
|
||||
self.parameter_values[(motor_id, param_index)] = val
|
||||
|
||||
# 如果是 IQF (电流)，更新电机状态
|
||||
if param_index == ParamIndex.IQF:
|
||||
if motor_id in self.motors_by_id:
|
||||
self.motors_by_id[motor_id].state.current = val
|
||||
|
||||
except Exception as e:
|
||||
print(f"解析参数失败: {e}")
|
||||
else:
|
||||
print(f"读取参数失败，错误码: {success_flag}")
|
||||
|
||||
elif comm_type == CommunicationType.OPERATION_STATUS:
|
||||
# 处理电机反馈
|
||||
extra_data = (can_id >> 8) & 0xFFFF
|
||||
motor_id = extra_data & 0xFF
|
||||
if motor_id in self.motors_by_id:
|
||||
motor = self.motors_by_id[motor_id]
|
||||
self._parse_feedback(motor, data)
|
||||
|
||||
count += 1
|
||||
|
||||
def _parse_feedback(self, motor: RobStrideMotor, data: bytes):
|
||||
if len(data) < 8:
|
||||
return
|
||||
|
||||
# 解包大端序数据
|
||||
p_u16, v_u16, t_i16, temp_u16 = struct.unpack('>HHHH', data)
|
||||
|
||||
model = motor.model
|
||||
p_limit = MODEL_MIT_POSITION_TABLE.get(model, 12.5)
|
||||
v_limit = MODEL_MIT_VELOCITY_TABLE.get(model, 50.0)
|
||||
t_limit = MODEL_MIT_TORQUE_TABLE.get(model, 60.0)
|
||||
|
||||
# 转换回浮点数
|
||||
pos = (float(p_u16) / 32767.0 - 1.0) * p_limit
|
||||
vel = (float(v_u16) / 32767.0 - 1.0) * v_limit
|
||||
torque = (float(t_i16) / 32767.0 - 1.0) * t_limit
|
||||
temp = float(temp_u16) * 0.1
|
||||
|
||||
motor.update_state(pos, vel, torque, temp)
|
||||
@@ -0,0 +1,422 @@
|
||||
import numpy as np
|
||||
import struct
|
||||
|
||||
class CommunicationType:
|
||||
"""
|
||||
电机通信类型定义 (Bit28~24)
|
||||
参考说明书 4.1 章节
|
||||
|
||||
通信 ID 结构 (29位扩展帧):
|
||||
| Bit 28-24 | Bit 23-8 | Bit 7-0 |
|
||||
| 通信类型 | 数据区2 | 目标地址 |
|
||||
"""
|
||||
GET_DEVICE_ID = 0 # 获取设备 ID 和 64 位 MCU 唯一标识符 (Type 0)
|
||||
OPERATION_CONTROL = 1 # 运控模式电机控制指令 (MIT 模式) (Type 1)
|
||||
OPERATION_STATUS = 2 # 电机反馈数据 (标准反馈帧) (Type 2)
|
||||
ENABLE = 3 # 电机使能运行 (Type 3)
|
||||
DISABLE = 4 # 电机停止运行 (可用于清除故障) (Type 4)
|
||||
SET_ZERO_POSITION = 6 # 设置电机机械零位 (设置当前位置为零点) (Type 6)
|
||||
SET_CAN_ID = 7 # 设置电机 CAN ID (立即生效，需保存) (Type 7)
|
||||
READ_PARAMETER = 17 # 单个参数读取 (Type 17, 0x11)
|
||||
WRITE_PARAMETER = 18 # 单个参数写入 (Type 18, 0x12, 掉电丢失)
|
||||
FAULT_REPORT = 21 # 故障反馈帧 (Type 21, 0x15)
|
||||
SAVE_PARAMETERS = 22 # 电机数据保存帧 (保存所有参数到 Flash) (Type 22)
|
||||
SET_BAUDRATE = 23 # 电机波特率修改帧 (重新上电生效) (Type 23)
|
||||
ACTIVE_REPORT = 24 # 电机主动上报设置帧 (开启/关闭主动上报) (Type 24)
|
||||
PROTOCOL_SWITCH = 25 # 电机协议修改帧 (切换 Canopen/MIT/私有协议) (Type 25)
|
||||
READ_VERSION = 26 # 版本号读取帧 (Type 26)
|
||||
|
||||
class RunMode:
|
||||
"""
|
||||
电机运行模式 (参数索引 0x7005)
|
||||
参考说明书 4.3 章节
|
||||
"""
|
||||
MIT = 0 # 运控模式 (默认): 适用于高动态响应控制
|
||||
POS_PP = 1 # 位置模式 (PP): 梯形加减速位置控制
|
||||
SPEED = 2 # 速度模式: 闭环速度控制
|
||||
CURRENT = 3 # 电流模式: 闭环力矩(电流)控制
|
||||
POS_CSP = 5 # 位置模式 (CSP): 循环同步位置模式 (适用于周期性指令)
|
||||
|
||||
class BaudRate:
|
||||
"""
|
||||
电机波特率 (通信类型 23)
|
||||
参考说明书 4.1 通信类型 23
|
||||
注意: 修改后需重新上电生效
|
||||
"""
|
||||
BAUD_1M = 1 # 1 Mbps (默认)
|
||||
BAUD_500K = 2 # 500 Kbps
|
||||
BAUD_250K = 3 # 250 Kbps
|
||||
BAUD_125K = 4 # 125 Kbps
|
||||
|
||||
class ActiveReportStatus:
|
||||
"""
|
||||
电机主动上报状态 (通信类型 24)
|
||||
参考说明书 4.1 通信类型 24
|
||||
"""
|
||||
DISABLE = 0 # 关闭主动上报 (默认)
|
||||
ENABLE = 1 # 开启主动上报 (默认间隔 10ms, 可通过 EP_SCAN_TIME 修改)
|
||||
|
||||
class ProtocolType:
|
||||
"""
|
||||
电机协议类型 (通信类型 25)
|
||||
参考说明书 4.2.4 章节
|
||||
注意: 切换协议后需重新上电生效
|
||||
"""
|
||||
PRIVATE = 0 # 私有协议 (默认): 使用 29 位扩展帧
|
||||
CANOPEN = 1 # CANopen 协议: 符合 CiA 402 标准
|
||||
MIT = 2 # MIT 协议 (标准帧): 使用 11 位标准帧
|
||||
|
||||
class ParamType:
|
||||
"""
|
||||
参数数据类型定义
|
||||
|
||||
- 私有协议 (Type 17/18) 参数表主要使用 UINT8/UINT16/UINT32/FLOAT
|
||||
- CANopen 对象字典会用到有符号类型 (INTEGER8/16/32)
|
||||
"""
|
||||
UINT8 = 0 # 无符号 8 位整数
|
||||
UINT16 = 1 # 无符号 16 位整数
|
||||
UINT32 = 2 # 无符号 32 位整数
|
||||
FLOAT = 3 # 32 位浮点数 (IEEE 754)
|
||||
INT8 = 4 # 有符号 8 位整数
|
||||
INT16 = 5 # 有符号 16 位整数
|
||||
INT32 = 6 # 有符号 32 位整数
|
||||
|
||||
class ErrorCode:
|
||||
"""
|
||||
异常状态 fault 值位定义
|
||||
|
||||
说明书位置:
|
||||
- 章节 6 (Mit) 的“异常状态应答帧”对 fault 值 bit 位做了明确描述
|
||||
- 私有协议 Type 21 故障反馈帧也会携带 fault/warning 值
|
||||
"""
|
||||
OVER_TEMP = 1 << 0 # bit0: 电机过温故障 (默认 >145°C)
|
||||
DRIVE_CHIP = 1 << 1 # bit1: 驱动芯片故障 (DRV8353 等报告错误)
|
||||
UNDER_VOLTAGE = 1 << 2 # bit2: 欠压故障 (电压 < 12V)
|
||||
OVER_VOLTAGE = 1 << 3 # bit3: 过压故障 (电压 > 60V)
|
||||
CURRENT_B_OVER = 1 << 4 # bit4: B 相电流采样过流
|
||||
CURRENT_C_OVER = 1 << 5 # bit5: C 相电流采样过流
|
||||
ENCODER_NOT_CALIB = 1 << 7 # bit7: 编码器未标定
|
||||
HARDWARE_ERR = 1 << 8 # bit8: 硬件识别故障
|
||||
POS_INIT_ERR = 1 << 9 # bit9: 位置初始化故障
|
||||
LOAD_BLOCK = 1 << 14 # bit14: 堵转过载算法保护
|
||||
CURRENT_A_OVER = 1 << 16 # bit16: A 相电流采样过流
|
||||
|
||||
class WarningCode:
|
||||
"""
|
||||
预警状态 warning 值位定义 (Type 21 Byte 4-7)
|
||||
"""
|
||||
OVER_TEMP_WARNING = 1 << 0 # bit0: 电机过温预警 (默认 >135°C)
|
||||
|
||||
class DriveFault1:
|
||||
"""
|
||||
驱动芯片故障码 1 (0x3024) - DRV8353 状态寄存器 1
|
||||
参考说明书 3.3.7 章节
|
||||
"""
|
||||
VDS_LC = 1 << 0 # VDS overcurrent on C low-side (C相下管VDS过流)
|
||||
VDS_HC = 1 << 1 # VDS overcurrent on C high-side (C相上管VDS过流)
|
||||
VDS_LB = 1 << 2 # VDS overcurrent on B low-side (B相下管VDS过流)
|
||||
VDS_HB = 1 << 3 # VDS overcurrent on B high-side (B相上管VDS过流)
|
||||
VDS_LA = 1 << 4 # VDS overcurrent on A low-side (A相下管VDS过流)
|
||||
VDS_HA = 1 << 5 # VDS overcurrent on A high-side (A相上管VDS过流)
|
||||
OTSD = 1 << 6 # Overtemperature shutdown (过温关断)
|
||||
UVLO = 1 << 7 # Undervoltage lockout (欠压锁定)
|
||||
GDF = 1 << 8 # Gate drive fault (栅极驱动故障)
|
||||
VDS_OCP = 1 << 9 # VDS monitor overcurrent (VDS 监控过流)
|
||||
FAULT = 1 << 10 # Logic OR of FAULT status (故障状态逻辑或)
|
||||
|
||||
class DriveFault2:
|
||||
"""
|
||||
驱动芯片故障码 2 (0x3025) - DRV8353 状态寄存器 2
|
||||
参考说明书 3.3.7 章节
|
||||
"""
|
||||
VGS_LC = 1 << 0 # Gate drive fault on C low-side (C相下管栅极故障)
|
||||
VGS_HC = 1 << 1 # Gate drive fault on C high-side (C相上管栅极故障)
|
||||
VGS_LB = 1 << 2 # Gate drive fault on B low-side (B相下管栅极故障)
|
||||
VGS_HB = 1 << 3 # Gate drive fault on B high-side (B相上管栅极故障)
|
||||
VGS_LA = 1 << 4 # Gate drive fault on A low-side (A相下管栅极故障)
|
||||
VGS_HA = 1 << 5 # Gate drive fault on A high-side (A相上管栅极故障)
|
||||
GDUV = 1 << 6 # VCP charge pump / VGLS undervoltage (电荷泵欠压)
|
||||
OTW = 1 << 7 # Overtemperature warning (过温预警)
|
||||
SC_OC = 1 << 8 # Overcurrent on phase C sense amplifier (C相采样过流)
|
||||
SB_OC = 1 << 9 # Overcurrent on phase B sense amplifier (B相采样过流)
|
||||
SA_OC = 1 << 10 # Overcurrent on phase A sense amplifier (A相采样过流)
|
||||
|
||||
class MotorParams:
|
||||
"""
|
||||
电机物理参数限制 (用于 MIT 模式数据压缩)
|
||||
参考说明书 4.1 通信类型 1
|
||||
|
||||
注意:
|
||||
- P_MIN/MAX: 位置范围 (RS03: -12.57 ~ 12.57 rad)
|
||||
- V_MIN/MAX: 速度范围 (RS03: -20 ~ 20 rad/s)
|
||||
- T_MIN/MAX: 力矩范围 (RS03: -60 ~ 60 Nm)
|
||||
- KP/KD: 刚度和阻尼系数范围
|
||||
"""
|
||||
def __init__(self,
|
||||
p_min: float = -12.57,
|
||||
p_max: float = 12.57, # RS03: -12.57 ~ 12.57 rad (约 -4pi ~ 4pi)
|
||||
v_min: float = -20.0,
|
||||
v_max: float = 20.0, # RS03: -20 ~ 20 rad/s
|
||||
kp_min: float = 0.0,
|
||||
kp_max: float = 5000.0, # RS03: 0 ~ 5000
|
||||
kd_min: float = 0.0,
|
||||
kd_max: float = 100.0, # RS03: 0 ~ 100
|
||||
t_min: float = -60.0,
|
||||
t_max: float = 60.0): # RS03: -60 ~ 60 Nm
|
||||
self.P_MIN = p_min
|
||||
self.P_MAX = p_max
|
||||
self.V_MIN = v_min
|
||||
self.V_MAX = v_max
|
||||
self.KP_MIN = kp_min
|
||||
self.KP_MAX = kp_max
|
||||
self.KD_MIN = kd_min
|
||||
self.KD_MAX = kd_max
|
||||
self.T_MIN = t_min
|
||||
self.T_MAX = t_max
|
||||
|
||||
class ParamIndex:
|
||||
"""
|
||||
电机参数索引表 (Index)
|
||||
参考说明书 4.1 可读写单个参数列表
|
||||
"""
|
||||
RUN_MODE = 0x7005 # 运行模式: 0:运控, 1:PP, 2:速度, 3:电流, 5:CSP (W/R)
|
||||
IQ_REF = 0x7006 # 电流模式 Iq 指令 (-43~43A) (W/R)
|
||||
SPD_REF = 0x700A # 转速模式转速指令 (-20~20rad/s) (W/R)
|
||||
LIMIT_TORQUE = 0x700B # 转矩限制 (0~60Nm) (W/R)
|
||||
CUR_KP = 0x7010 # 电流 Kp (默认 0.17) (W/R)
|
||||
CUR_KI = 0x7011 # 电流 Ki (默认 0.012) (W/R)
|
||||
CUR_FILT_GAIN = 0x7014 # 电流滤波系数 (0~1.0, 默认 0.1) (W/R)
|
||||
LOC_REF = 0x7016 # 位置模式角度指令 (rad) (W/R)
|
||||
LIMIT_SPD = 0x7017 # 位置模式(CSP)速度限制 (0~20rad/s) (W/R)
|
||||
LIMIT_CUR = 0x7018 # 速度/位置模式电流限制 (0~43A) (W/R)
|
||||
MECH_POS = 0x7019 # 负载端计圈机械角度 (rad) (Read Only)
|
||||
IQF = 0x701A # Iq 滤波值 (A) (Read Only)
|
||||
MECH_VEL = 0x701B # 负载端转速 (rad/s) (Read Only)
|
||||
VBUS = 0x701C # 母线电压 (V) (Read Only)
|
||||
LOC_KP = 0x701E # 位置环 Kp (默认 60) (W/R)
|
||||
SPD_KP = 0x701F # 速度环 Kp (默认 6) (W/R)
|
||||
SPD_KI = 0x7020 # 速度环 Ki (默认 0.02) (W/R)
|
||||
SPD_FILT_GAIN = 0x7021 # 速度滤波值 (默认 0.1) (W/R)
|
||||
ACC_RAD = 0x7022 # 速度模式加速度 (默认 20rad/s^2) (W/R)
|
||||
VEL_MAX = 0x7024 # 位置模式(PP)速度 (默认 10rad/s) (W/R)
|
||||
ACC_SET = 0x7025 # 位置模式(PP)加速度 (默认 10rad/s^2) (W/R)
|
||||
EP_SCAN_TIME = 0x7026 # 主动上报时间 (1=10ms, +1=+5ms) (W)
|
||||
CAN_TIMEOUT = 0x7028 # CAN 超时阈值 (20000=1s, 0=禁用) (W)
|
||||
ZERO_STA = 0x7029 # 零点标志位 (0: 0~2pi, 1: -pi~pi) (W)
|
||||
DAMPER = 0x702A # 阻尼开关 (1: 取消关机反驱保护) (W/R)
|
||||
ADD_OFFSET = 0x702B # 零位偏置 (rad) (W/R)
|
||||
|
||||
class CanopenIndex:
|
||||
"""
|
||||
CANopen 对象字典常用索引
|
||||
参考说明书第 5 章 (Canopen)
|
||||
"""
|
||||
ERROR_CODE = 0x603F # 错误码
|
||||
CONTROLWORD = 0x6040 # 控制字
|
||||
STATUSWORD = 0x6041 # 状态字
|
||||
MODES_OF_OPERATION = 0x6060 # 运行模式
|
||||
MODES_OF_OPERATION_DISPLAY = 0x6061 # 当前运行模式显示
|
||||
POSITION_DEMAND_VALUE = 0x6062 # 位置指令值
|
||||
POSITION_ACTUAL_VALUE = 0x6064 # 位置实际值
|
||||
POSITION_WINDOW = 0x6067 # 位置窗口
|
||||
POSITION_WINDOW_TIME = 0x6068 # 位置窗口时间
|
||||
VELOCITY_DEMAND_VALUE = 0x606B # 速度指令值
|
||||
VELOCITY_ACTUAL_VALUE = 0x606C # 速度实际值
|
||||
TARGET_TORQUE = 0x6071 # 目标力矩 (0.1% 额定力矩)
|
||||
TORQUE_ACTUAL_VALUE = 0x6077 # 力矩实际值
|
||||
CURRENT_ACTUAL_VALUE = 0x6078 # 电流实际值
|
||||
DC_LINK_CIRCUIT_VOLTAGE = 0x6079 # 母线电压
|
||||
TARGET_POSITION = 0x607A # 目标位置
|
||||
PROFILE_VELOCITY = 0x6081 # 轮廓速度
|
||||
PROFILE_ACCELERATION = 0x6083 # 轮廓加速度
|
||||
TARGET_VELOCITY = 0x60FF # 目标速度
|
||||
|
||||
class CanopenModeOfOperation:
|
||||
"""CANopen 模式 (6060)"""
|
||||
PP = 1 # Profile Position Mode
|
||||
SPEED = 3 # Profile Velocity Mode
|
||||
TORQUE = 4 # Profile Torque Mode
|
||||
CSP = 5 # Cyclic Synchronous Position Mode
|
||||
HOMING = 6 # Homing Mode
|
||||
|
||||
class CanopenControlword:
|
||||
"""CANopen 控制字 (6040) 常用值"""
|
||||
SHUTDOWN = 0x0006 # Shutdown
|
||||
SWITCH_ON = 0x0007 # Switch On
|
||||
ENABLE_OPERATION = 0x000F # Enable Operation
|
||||
DISABLE_VOLTAGE = 0x0001 # Disable Voltage
|
||||
QUICK_STOP = 0x000B # Quick Stop
|
||||
|
||||
# CANopen 协议切换帧 (扩展帧)
|
||||
# 说明书 5.10: 29 位 ID 为 0xFFF，数据区 Byte0~6 固定 01~06，Byte7=F_CMD(协议类型)
|
||||
CANOPEN_PROTOCOL_SWITCH_EXT_ID = 0xFFF
|
||||
|
||||
class MitStdCommandType:
|
||||
"""
|
||||
MIT 标准帧指令类型 (对应说明书第 6 章的指令 1~11)
|
||||
|
||||
标准帧 ID (11位) 结构:
|
||||
| Bit 10-8 | Bit 7-0 |
|
||||
| 模式/指令 | 电机 ID |
|
||||
|
||||
注意:
|
||||
- 指令 1~9: CAN ID 的 Bit10~8 为 0，通过数据区 Payload 区分功能
|
||||
- 指令 10: CAN ID 的 Bit10~8 为 1 (位置模式)
|
||||
- 指令 11: CAN ID 的 Bit10~8 为 2 (速度模式)
|
||||
"""
|
||||
ENABLE = 1 # 指令 1: 电机使能运行
|
||||
STOP = 2 # 指令 2: 电机停止运行
|
||||
DYNAMIC_PARAM = 3 # 指令 3: MIT 动态参数
|
||||
SET_ZERO = 4 # 指令 4: 设置零点 (非位置模式)
|
||||
CLEAR_ERROR_OR_READ_STATUS = 5 # 指令 5: 清错 / 读取异常状态
|
||||
SET_RUN_MODE = 6 # 指令 6: 设置运行模式
|
||||
SET_MOTOR_CAN_ID = 7 # 指令 7: 修改电机 CANID
|
||||
SET_PROTOCOL = 8 # 指令 8: 修改电机协议 (重新上电生效)
|
||||
SET_MASTER_CAN_ID = 9 # 指令 9: 修改主机 CANID
|
||||
POS_CONTROL = 10 # 指令 10: 位置模式控制指令 (ID Bit10-8=1)
|
||||
SPEED_CONTROL = 11 # 指令 11: 速度模式控制指令 (ID Bit10-8=2)
|
||||
|
||||
def get_mit_can_id_mode(cmd_type: int) -> int:
|
||||
"""
|
||||
获取 MIT 标准帧 CAN ID 的 Bit10~8 值
|
||||
|
||||
:param cmd_type: MitStdCommandType 枚举值
|
||||
:return: 模式位 (0, 1, 或 2)
|
||||
"""
|
||||
if cmd_type in (MitStdCommandType.POS_CONTROL,):
|
||||
return 1
|
||||
elif cmd_type in (MitStdCommandType.SPEED_CONTROL,):
|
||||
return 2
|
||||
else:
|
||||
# 指令 1~9 (以及其他潜在指令) 默认为 0
|
||||
return 0
|
||||
|
||||
def build_mit_std_id(cmd_type: int, motor_id: int) -> int:
|
||||
"""
|
||||
构建 MIT 标准帧 11 位 CAN ID
|
||||
|
||||
:param cmd_type: MitStdCommandType 枚举值
|
||||
:param motor_id: 电机 ID (0~127)
|
||||
:return: 11 位 CAN ID
|
||||
"""
|
||||
mode = get_mit_can_id_mode(cmd_type)
|
||||
return ((mode & 0x07) << 8) | (motor_id & 0xFF)
|
||||
|
||||
class MitPayloads:
|
||||
"""
|
||||
MIT 协议特殊指令的固定 Payload 定义 (指令 1, 2, 4, 5, 6, 7, 8, 9)
|
||||
部分指令的 Payload 末尾字节需要根据参数动态修改
|
||||
"""
|
||||
# 指令 1: FF FF FF FF FF FF FF FC
|
||||
ENABLE = b'\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFC'
|
||||
|
||||
# 指令 2: FF FF FF FF FF FF FF FD
|
||||
STOP = b'\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFD'
|
||||
|
||||
# 指令 3: 动态参数 (全 0 或根据参数设置)
|
||||
DYNAMIC_PARAM_ZERO = b'\x00\x00\x00\x00\x00\x00\x00\x00'
|
||||
|
||||
# 指令 4: FF FF FF FF FF FF FF FE
|
||||
SET_ZERO = b'\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFE'
|
||||
|
||||
# 指令 5: FF FF FF FF FF FF FF FB (清除错误)
|
||||
# 若 F_CMD (Byte6) 为 0xFF 则清除错误，否则为读取异常状态
|
||||
CLEAR_ERROR = b'\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFB'
|
||||
|
||||
# 指令 6: FF FF FF FF FF FF [Mode] FC
|
||||
# Template, last 2 bytes are [Mode, FC]
|
||||
SET_RUN_MODE_PREFIX = b'\xFF\xFF\xFF\xFF\xFF\xFF'
|
||||
|
||||
# 指令 7: FF FF FF FF FF FF [NewID] FA
|
||||
SET_MOTOR_CAN_ID_PREFIX = b'\xFF\xFF\xFF\xFF\xFF\xFF'
|
||||
|
||||
# 指令 8: FF FF FF FF FF FF [Protocol] FD
|
||||
SET_PROTOCOL_PREFIX = b'\xFF\xFF\xFF\xFF\xFF\xFF'
|
||||
|
||||
# 指令 9: FF FF FF FF FF FF [MasterID] 01
|
||||
SET_MASTER_CAN_ID_PREFIX = b'\xFF\xFF\xFF\xFF\xFF\xFF'
|
||||
|
||||
|
||||
# 参数表配置: (参数名, 数据类型, 字节数)
|
||||
PARAM_TABLE = {
|
||||
ParamIndex.RUN_MODE: ("run_mode", ParamType.UINT8, 1),
|
||||
ParamIndex.IQ_REF: ("iq_ref", ParamType.FLOAT, 4),
|
||||
ParamIndex.SPD_REF: ("spd_ref", ParamType.FLOAT, 4),
|
||||
ParamIndex.LIMIT_TORQUE: ("limit_torque", ParamType.FLOAT, 4),
|
||||
ParamIndex.CUR_KP: ("cur_kp", ParamType.FLOAT, 4),
|
||||
ParamIndex.CUR_KI: ("cur_ki", ParamType.FLOAT, 4),
|
||||
ParamIndex.CUR_FILT_GAIN: ("cur_filt_gain", ParamType.FLOAT, 4),
|
||||
ParamIndex.LOC_REF: ("loc_ref", ParamType.FLOAT, 4),
|
||||
ParamIndex.LIMIT_SPD: ("limit_spd", ParamType.FLOAT, 4),
|
||||
ParamIndex.LIMIT_CUR: ("limit_cur", ParamType.FLOAT, 4),
|
||||
ParamIndex.MECH_POS: ("mechPos", ParamType.FLOAT, 4),
|
||||
ParamIndex.IQF: ("iqf", ParamType.FLOAT, 4),
|
||||
ParamIndex.MECH_VEL: ("mechVel", ParamType.FLOAT, 4),
|
||||
ParamIndex.VBUS: ("VBUS", ParamType.FLOAT, 4),
|
||||
ParamIndex.LOC_KP: ("loc_kp", ParamType.FLOAT, 4),
|
||||
ParamIndex.SPD_KP: ("spd_kp", ParamType.FLOAT, 4),
|
||||
ParamIndex.SPD_KI: ("spd_ki", ParamType.FLOAT, 4),
|
||||
ParamIndex.SPD_FILT_GAIN: ("spd_filt_gain", ParamType.FLOAT, 4),
|
||||
ParamIndex.ACC_RAD: ("acc_rad", ParamType.FLOAT, 4),
|
||||
ParamIndex.VEL_MAX: ("vel_max", ParamType.FLOAT, 4),
|
||||
ParamIndex.ACC_SET: ("acc_set", ParamType.FLOAT, 4),
|
||||
ParamIndex.EP_SCAN_TIME: ("EPScan_time", ParamType.UINT16, 2),
|
||||
ParamIndex.CAN_TIMEOUT: ("cantimeout", ParamType.UINT32, 4),
|
||||
ParamIndex.ZERO_STA: ("zero_sta", ParamType.UINT8, 1),
|
||||
ParamIndex.DAMPER: ("damper", ParamType.UINT8, 1),
|
||||
ParamIndex.ADD_OFFSET: ("add_offset", ParamType.FLOAT, 4),
|
||||
}
|
||||
|
||||
MODEL_MIT_POSITION_TABLE = {
|
||||
"rs-00": 4 * np.pi, "rs-01": 4 * np.pi, "rs-02": 4 * np.pi,
|
||||
"rs-03": 4 * np.pi, "rs-04": 4 * np.pi, "rs-05": 4 * np.pi, "rs-06": 4 * np.pi,
|
||||
"el-05": 4 * np.pi,
|
||||
}
|
||||
|
||||
MODEL_MIT_VELOCITY_TABLE = {
|
||||
"rs-00": 50, "rs-01": 44, "rs-02": 44,
|
||||
"rs-03": 50, "rs-04": 15, "rs-05": 33, "rs-06": 20,
|
||||
"el-05": 50,
|
||||
}
|
||||
|
||||
MODEL_MIT_TORQUE_TABLE = {
|
||||
"rs-00": 17, "rs-01": 17, "rs-02": 17,
|
||||
"rs-03": 60, "rs-04": 120, "rs-05": 17, "rs-06": 60,
|
||||
"el-05": 6,
|
||||
}
|
||||
|
||||
MODEL_MIT_KP_TABLE = {
|
||||
"rs-00": 500.0, "rs-01": 500.0, "rs-02": 500.0,
|
||||
"rs-03": 5000.0, "rs-04": 5000.0, "rs-05": 500.0, "rs-06": 5000.0,
|
||||
"el-05": 500.0,
|
||||
}
|
||||
|
||||
MODEL_MIT_KD_TABLE = {
|
||||
"rs-00": 5.0, "rs-01": 5.0, "rs-02": 5.0,
|
||||
"rs-03": 100.0, "rs-04": 100.0, "rs-05": 5.0, "rs-06": 100.0,
|
||||
"el-05": 5.0,
|
||||
}
|
||||
|
||||
def get_pack_format(param_type):
|
||||
"""
|
||||
获取 struct.pack 的格式字符串和字节大小
|
||||
|
||||
说明:
|
||||
- Type 17/18 参数读写使用小端序
|
||||
- CANopen SDO 数据同样通常按小端序解释 (取决于实现)
|
||||
"""
|
||||
if param_type == ParamType.UINT8:
|
||||
return '<B', 1
|
||||
elif param_type == ParamType.UINT16:
|
||||
return '<H', 2
|
||||
elif param_type == ParamType.UINT32:
|
||||
return '<I', 4
|
||||
elif param_type == ParamType.INT8:
|
||||
return '<b', 1
|
||||
elif param_type == ParamType.INT16:
|
||||
return '<h', 2
|
||||
elif param_type == ParamType.INT32:
|
||||
return '<i', 4
|
||||
elif param_type == ParamType.FLOAT:
|
||||
return '<f', 4
|
||||
return None, 0
|
||||
@@ -0,0 +1,185 @@
|
||||
import serial
|
||||
import struct
|
||||
import time
|
||||
from typing import Optional, Tuple
|
||||
|
||||
class DmUsbAdapter:
|
||||
"""
|
||||
达妙 USB 转 CAN 适配器驱动。
|
||||
处理底层串口通信和帧的封装/解包。
|
||||
"""
|
||||
|
||||
# 帧常量
|
||||
SEND_HEADER = b'\x55\xAA'
|
||||
SEND_FRAME_LEN = 30
|
||||
RECV_HEADER = 0xAA
|
||||
RECV_TAIL = 0x55
|
||||
RECV_FRAME_LEN = 16
|
||||
|
||||
def __init__(self, port: str, baudrate: int = 921600, timeout: float = 0.01, debug: bool = False):
|
||||
"""
|
||||
初始化 USB 转 CAN 适配器。
|
||||
|
||||
:param port: 串口名称 (例如 "COM3")
|
||||
:param baudrate: 串口波特率 (默认 921600)
|
||||
:param timeout: 读取超时时间 (秒)
|
||||
:param debug: 是否打印调试信息
|
||||
"""
|
||||
self.serial = serial.Serial()
|
||||
self.serial.port = port
|
||||
self.serial.baudrate = baudrate
|
||||
self.serial.timeout = timeout
|
||||
self.data_buffer = bytearray()
|
||||
self.debug = debug
|
||||
|
||||
def open(self):
|
||||
"""打开串口连接。"""
|
||||
if not self.serial.is_open:
|
||||
try:
|
||||
self.serial.open()
|
||||
if self.debug:
|
||||
print(f"[DEBUG] 串口 {self.serial.port} 已打开")
|
||||
except Exception as e:
|
||||
print(f"[ERROR] 无法打开串口 {self.serial.port}: {e}")
|
||||
raise
|
||||
|
||||
def close(self):
|
||||
"""关闭串口连接。"""
|
||||
if self.serial.is_open:
|
||||
self.serial.close()
|
||||
if self.debug:
|
||||
print(f"[DEBUG] 串口 {self.serial.port} 已关闭")
|
||||
|
||||
def set_can_baudrate(self, index: int = 0):
|
||||
"""
|
||||
设置 CAN 波特率。
|
||||
|
||||
索引对照表:
|
||||
0: 1000 kbps
|
||||
1: 800 kbps
|
||||
2: 666 kbps
|
||||
3: 500 kbps
|
||||
...
|
||||
63:
|
||||
|
||||
:param index: 波特率索引 (默认 0, 即 1000kbps)
|
||||
"""
|
||||
# 构建设置波特率指令: 55 05 Index(1byte) AA 55
|
||||
cmd = bytearray([0x55, 0x05, index & 0xFF, 0xAA, 0x55])
|
||||
self.serial.write(cmd)
|
||||
if self.debug:
|
||||
print(f"[DEBUG] 发送设置波特率指令: {cmd.hex()}")
|
||||
time.sleep(0.1) # 等待生效
|
||||
|
||||
def send_can_frame(self, can_id: int, data: bytes,
|
||||
extended: bool = True, remote: bool = False,
|
||||
feedback: bool = False) -> None:
|
||||
"""
|
||||
发送 CAN 帧。
|
||||
|
||||
:param can_id: CAN 标识符 (标准帧或扩展帧)
|
||||
:param data: 数据负载 (最多 8 字节)
|
||||
:param extended: True 为扩展帧 (29位), False 为标准帧 (11位)
|
||||
:param remote: True 为远程帧, False 为数据帧
|
||||
:param feedback: True 请求设备反馈 (CMD 0x01), False 不反馈 (CMD 0x03)
|
||||
"""
|
||||
if len(data) > 8:
|
||||
raise ValueError("CAN 数据不能超过 8 字节")
|
||||
|
||||
# 填充数据到 8 字节
|
||||
data_padded = data + b'\x00' * (8 - len(data))
|
||||
|
||||
cmd = 0x01 if feedback else 0x03
|
||||
send_count = 1
|
||||
interval = 10 # 默认 10ms
|
||||
id_type = 1 if extended else 0
|
||||
frame_type = 1 if remote else 0
|
||||
data_len = len(data)
|
||||
|
||||
# 构建帧 (30 字节)
|
||||
frame = bytearray(30)
|
||||
frame[0] = 0x55
|
||||
frame[1] = 0xAA
|
||||
frame[2] = 0x1E # 长度
|
||||
frame[3] = cmd
|
||||
|
||||
# 发送次数 (4 字节, 小端序)
|
||||
frame[4:8] = struct.pack('<I', send_count)
|
||||
|
||||
# 时间间隔 (4 字节, 小端序)
|
||||
frame[8:12] = struct.pack('<I', interval)
|
||||
|
||||
frame[12] = id_type
|
||||
|
||||
# CAN ID (4 字节, 小端序)
|
||||
frame[13:17] = struct.pack('<I', can_id)
|
||||
|
||||
frame[17] = frame_type
|
||||
frame[18] = data_len
|
||||
# 19, 20 为保留位 0
|
||||
|
||||
frame[21:29] = data_padded
|
||||
frame[29] = 0x00 # CRC (任意值)
|
||||
|
||||
self.serial.write(frame)
|
||||
|
||||
if self.debug:
|
||||
print(f"[DEBUG] 发送帧: ID=0x{can_id:08X} Data={data.hex()} Raw={frame.hex()}")
|
||||
|
||||
def read_can_frame(self) -> Optional[Tuple[int, bytes, int, bool, bool]]:
|
||||
"""
|
||||
如果缓冲区中有可用数据，读取一帧 CAN 数据。
|
||||
|
||||
:return: 元组 (can_id, data, cmd, extended, remote) 或者 None (如果没有完整帧)
|
||||
"""
|
||||
# 读取可用数据
|
||||
if self.serial.in_waiting:
|
||||
raw_data = self.serial.read(self.serial.in_waiting)
|
||||
self.data_buffer.extend(raw_data)
|
||||
|
||||
# 检查完整帧 (16 字节)
|
||||
while len(self.data_buffer) >= self.RECV_FRAME_LEN:
|
||||
# 查找帧头
|
||||
try:
|
||||
header_idx = self.data_buffer.index(self.RECV_HEADER)
|
||||
except ValueError:
|
||||
# 没有找到帧头，清空缓冲区（保留最后几个字节以防截断）
|
||||
self.data_buffer = self.data_buffer[-(self.RECV_FRAME_LEN-1):]
|
||||
return None
|
||||
|
||||
# 检查从帧头开始是否有足够字节
|
||||
if len(self.data_buffer) - header_idx < self.RECV_FRAME_LEN:
|
||||
# 保留从帧头开始的数据
|
||||
self.data_buffer = self.data_buffer[header_idx:]
|
||||
return None
|
||||
|
||||
# 检查帧尾
|
||||
if self.data_buffer[header_idx + self.RECV_FRAME_LEN - 1] != self.RECV_TAIL:
|
||||
# 无效帧，跳过该帧头继续查找
|
||||
self.data_buffer = self.data_buffer[header_idx + 1:]
|
||||
continue
|
||||
|
||||
# 提取有效帧
|
||||
frame = self.data_buffer[header_idx : header_idx + self.RECV_FRAME_LEN]
|
||||
self.data_buffer = self.data_buffer[header_idx + self.RECV_FRAME_LEN:]
|
||||
|
||||
if self.debug:
|
||||
print(f"[DEBUG] 解析帧: {frame.hex()}")
|
||||
|
||||
# 解析帧
|
||||
cmd = frame[1]
|
||||
format_byte = frame[2]
|
||||
|
||||
data_len = format_byte & 0x3F
|
||||
ide = bool((format_byte >> 6) & 0x01)
|
||||
rtr = bool((format_byte >> 7) & 0x01)
|
||||
|
||||
can_id = struct.unpack('<I', frame[3:7])[0]
|
||||
data = bytes(frame[7:15])
|
||||
|
||||
if data_len < 8:
|
||||
data = data[:data_len]
|
||||
|
||||
return (can_id, data, cmd, ide, rtr)
|
||||
|
||||
return None
|
||||
@@ -0,0 +1,36 @@
|
||||
cmake_minimum_required(VERSION 3.16)
|
||||
project(odin1 LANGUAGES C CXX)
|
||||
|
||||
set(CMAKE_CXX_STANDARD 17)
|
||||
set(CMAKE_CXX_STANDARD_REQUIRED ON)
|
||||
set(CMAKE_POSITION_INDEPENDENT_CODE ON)
|
||||
|
||||
find_package(PkgConfig REQUIRED)
|
||||
find_package(OpenSSL REQUIRED)
|
||||
pkg_check_modules(LIBUSB REQUIRED libusb-1.0)
|
||||
|
||||
add_library(odin1_imu_bridge SHARED
|
||||
src/odin1_imu_bridge.cpp
|
||||
)
|
||||
|
||||
target_include_directories(odin1_imu_bridge
|
||||
PUBLIC
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/include
|
||||
${LIBUSB_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
target_link_directories(odin1_imu_bridge
|
||||
PRIVATE
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/lib
|
||||
)
|
||||
|
||||
target_link_libraries(odin1_imu_bridge
|
||||
PRIVATE
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/lib/liblydHostApi_arm.a
|
||||
${LIBUSB_LIBRARIES}
|
||||
OpenSSL::SSL
|
||||
OpenSSL::Crypto
|
||||
pthread
|
||||
rt
|
||||
dl
|
||||
)
|
||||
@@ -0,0 +1,8 @@
|
||||
#!/usr/bin/env bash
|
||||
set -euo pipefail
|
||||
|
||||
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
|
||||
BUILD_DIR="${SCRIPT_DIR}/build"
|
||||
|
||||
cmake -S "${SCRIPT_DIR}" -B "${BUILD_DIR}" -DCMAKE_BUILD_TYPE=Release
|
||||
cmake --build "${BUILD_DIR}" -j"$(nproc)"
|
||||
@@ -0,0 +1,308 @@
|
||||
/*
|
||||
Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
*/
|
||||
#ifndef LIDAR_API_H
|
||||
#define LIDAR_API_H
|
||||
|
||||
/**
|
||||
* @file lidar_api.h
|
||||
* @brief LiDAR device API for controlling and accessing LiDAR sensor data
|
||||
*
|
||||
* This header provides the public interface for interacting with LiDAR devices.
|
||||
* It includes functions for device management, data streaming control, and
|
||||
* device configuration.
|
||||
*
|
||||
* @copyright Copyright (c) 2025, Manifold Tech Limited, All Rights Reserved
|
||||
* @version 1.0
|
||||
*/
|
||||
|
||||
#include "lidar_api_type.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/**
|
||||
* @brief Initialize the LiDAR system
|
||||
*
|
||||
* Must be called before any other lidar function to set up the system resources.
|
||||
*
|
||||
* @param cb Callback function for device events (connection, disconnection)
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_system_init(lidar_device_callback_t cb);
|
||||
|
||||
/**
|
||||
* @brief Deinitialize the LiDAR system
|
||||
*
|
||||
* Releases all resources allocated by the system. Should be called when
|
||||
* application is shutting down.
|
||||
*
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_system_deinit(void);
|
||||
|
||||
/**
|
||||
* @brief Create a handle for a LiDAR device
|
||||
*
|
||||
* @param dev_info Information about the LiDAR device to create
|
||||
* @param device Pointer to receive the device handle upon success
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_create_device(lidar_device_info_t *dev_info, device_handle *device);
|
||||
|
||||
/**
|
||||
* @brief Destroy a LiDAR device handle
|
||||
*
|
||||
* Releases resources associated with the device handle. Must be called
|
||||
* when the device is no longer needed.
|
||||
*
|
||||
* @param device Handle to the device to destroy
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_destory_device(device_handle device);
|
||||
|
||||
/**
|
||||
* @brief Register callback function for receiving LiDAR data streams
|
||||
*
|
||||
* Sets up a callback function that will be called when new data is available.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param cb Callback information containing function pointers for different data types
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_register_stream_callback(device_handle device, lidar_data_callback_info_t cb);
|
||||
|
||||
/**
|
||||
* @brief Unregister stream callback for a device
|
||||
*
|
||||
* Stops the device from calling back when new data is available.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_unregister_stream_callback(device_handle device);
|
||||
|
||||
/**
|
||||
* @brief Open a LiDAR device for communication
|
||||
*
|
||||
* Establishes a connection to the physical device.
|
||||
*
|
||||
* @param device Handle to the device to open
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_open_device(device_handle device);
|
||||
|
||||
/**
|
||||
* @brief Close a LiDAR device
|
||||
*
|
||||
* Closes the connection to the physical device.
|
||||
*
|
||||
* @param device Handle to the device to close
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_close_device(device_handle device);
|
||||
|
||||
/**
|
||||
* @brief Set the operating mode of the LiDAR device
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param mode Operating mode to set (see mode definitions in lidar_api_type.h)
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_set_mode(device_handle device, int mode);
|
||||
|
||||
/**
|
||||
* @brief Start data streaming from the device
|
||||
*
|
||||
* Begins the flow of data from the device for the specified type.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param type Type of data stream to start (see stream type definitions in lidar_api_type.h)
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_start_stream(device_handle device, int type, uint32_t &dtof_subframe_odr);
|
||||
|
||||
/**
|
||||
* @brief Stop data streaming from the device
|
||||
*
|
||||
* Stops the flow of data from the device for the specified type.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param type Type of data stream to stop
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_stop_stream(device_handle device, int type);
|
||||
|
||||
/**
|
||||
* @brief Activate a specific stream type on the device
|
||||
*
|
||||
* Enables a specific data stream type in the device configuration.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param type Type of data stream to activate
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_activate_stream_type(device_handle device, int type);
|
||||
|
||||
/**
|
||||
* @brief Deactivate a specific stream type on the device
|
||||
*
|
||||
* Disables a specific data stream type in the device configuration.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param type Type of data stream to deactivate
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_deactivate_stream_type(device_handle device, int type);
|
||||
|
||||
/**
|
||||
* @brief Get calibration file from the device
|
||||
*
|
||||
* Retrieves the calibration file from the device.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param path Path to save the calibration file
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_get_calib_file(device_handle device, const char* path);
|
||||
|
||||
/**
|
||||
* @brief Set log verbosity level
|
||||
*
|
||||
* Controls the amount of log information generated by the LiDAR API.
|
||||
*
|
||||
* @param level Log level to set (see level definitions in lidar_api_type.h)
|
||||
*/
|
||||
void lidar_log_set_level(lidar_log_level_e level);
|
||||
|
||||
/**
|
||||
* @brief Get the version information of the LiDAR device
|
||||
*
|
||||
* Retrieves version information including firmware, system, and application versions.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param version struct Pointer to receive the version information
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_get_version(device_handle device,lidar_fireware_version_t *version);
|
||||
|
||||
/**
|
||||
* @brief Set custom algorithm parameters for the device
|
||||
*
|
||||
* Sends custom parameter settings to the device.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param param_name String name of the parameter to set
|
||||
* @param value_data Pointer to the value data to set for the parameter
|
||||
* @param value_length Length of the value data in bytes
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_set_custom_parameter(device_handle device, const char* param_name, const void* value_data, size_t value_length);
|
||||
|
||||
/**
|
||||
* @brief Get custom algorithm parameters for the device
|
||||
*
|
||||
* Get custom parameter settings from the device.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param param_name String name of the parameter to get
|
||||
* @param value Integer value to get for the parameter
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_get_custom_parameter(device_handle device, const char* param_name, int* value);
|
||||
|
||||
/**
|
||||
* @brief Set the map file used for relocalization
|
||||
*
|
||||
* Read & send specified map file to device for relocalization
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param abs_path Absolute path to the map file
|
||||
* @return int 0 on success, otherwise on failure
|
||||
*/
|
||||
int lidar_set_relocalization_map(device_handle device, const char* abs_path);
|
||||
|
||||
/**
|
||||
* @brief Get the mapping result file from device
|
||||
*
|
||||
* Read & send specified map file from device to host
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param dest_dir Destination directory to save the map file
|
||||
* @param file_name File name to save the map file
|
||||
* @return int 0 on success, -1 on failure without error code, error code (> 0) otherwise
|
||||
*/
|
||||
int lidar_get_mapping_result(device_handle device, const char* dest_dir, const char* file_name);
|
||||
|
||||
/**
|
||||
* @brief Set the image mask file for the device
|
||||
*
|
||||
* Read & send specified image mask file to device
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param abs_path Absolute path to the image mask file (e.g., mask.png)
|
||||
* @return int 0 on success, -1 on failure, -2 if file transfer in progress
|
||||
*/
|
||||
int lidar_set_image_mask(device_handle device, const char* abs_path);
|
||||
|
||||
|
||||
/**
|
||||
* @brief enable device log
|
||||
*
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param dest_dir Destination directory to save the logs
|
||||
* @return int 0 on success, -1 on failure
|
||||
*/
|
||||
int lidar_enable_encrypted_device_log(device_handle device, const char* dest_dir);
|
||||
|
||||
|
||||
/**
|
||||
* @brief Set the depth parameters for the device
|
||||
*
|
||||
* This function must be called before starting data stream.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param params Pointer to the depth parameters to set
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_set_depth_parameter(device_handle device, const lidar_depth_para_t *params);
|
||||
|
||||
/**
|
||||
* @brief Enable or disable IMU smooth sending feature
|
||||
*
|
||||
* When enabled, IMU data will be sent at precise intervals (default 400Hz)
|
||||
* using a dedicated high-priority thread to reduce jitter and timing variance.
|
||||
* When disabled, IMU data will be sent immediately upon reception.
|
||||
*
|
||||
* @param enable 1 to enable smooth sending, 0 to disable
|
||||
* @return int 0 on success, -1 on failure
|
||||
*/
|
||||
int lidar_enable_imu_smooth_sending(int enable);
|
||||
|
||||
/**
|
||||
* @brief Set IMU smooth sending frequency
|
||||
*
|
||||
* Set the target frequency for IMU smooth sending. Only effective when
|
||||
* smooth sending is enabled via lidar_enable_imu_smooth_sending().
|
||||
*
|
||||
* @param frequency_hz Target frequency in Hz (1-1000 Hz, recommended 400 Hz)
|
||||
* @return int 0 on success, -1 on failure
|
||||
*/
|
||||
int lidar_set_imu_smooth_frequency(uint32_t frequency_hz);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // LIDAR_API_H
|
||||
@@ -0,0 +1,242 @@
|
||||
/*
|
||||
Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
*/
|
||||
#ifndef LIDAR_TYPES_H
|
||||
#define LIDAR_TYPES_H
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#define LIDAR_SERIAL_MAX 64
|
||||
#define LIDAR_MODEL_MAX 64
|
||||
#define LIDAR_IP_MAX 64
|
||||
|
||||
typedef void * device_handle;
|
||||
|
||||
typedef enum {
|
||||
LIDAR_LOG_ERROR = 0,
|
||||
LIDAR_LOG_WARN,
|
||||
LIDAR_LOG_INFO,
|
||||
LIDAR_LOG_DEBUG,
|
||||
} lidar_log_level_e;
|
||||
|
||||
typedef enum {
|
||||
LIDAR_OTA_ALGORITHM,
|
||||
LIDAR_OTA_FIRMWARE,
|
||||
LIDAR_OTA_SCRIPT,
|
||||
LIDAR_OTA_CALIBRATION
|
||||
} lidar_ota_type_e;
|
||||
|
||||
typedef enum {
|
||||
LIDAR_MODE_RAW,
|
||||
LIDAR_MODE_SLAM,
|
||||
} lidar_mode_e;
|
||||
|
||||
typedef enum {
|
||||
LIDAR_DT_NONE = 0,
|
||||
LIDAR_DT_RAW_RGB,
|
||||
LIDAR_DT_RAW_IMU,
|
||||
LIDAR_DT_RAW_DTOF,
|
||||
LIDAR_DT_SLAM_CLOUD,
|
||||
LIDAR_DT_SLAM_ODOMETRY,
|
||||
LIDAR_DT_DEV_STATUS,
|
||||
LIDAR_DT_SLAM_ODOMETRY_HIGHFREQ,
|
||||
LIDAR_DT_SLAM_ODOMETRY_TF,
|
||||
LIDAR_DT_SLAM_WIWC,
|
||||
LIDAR_DT_NTP
|
||||
} lidar_data_type_e;
|
||||
|
||||
typedef struct {
|
||||
int8_t serial[LIDAR_SERIAL_MAX];
|
||||
int8_t model[LIDAR_MODEL_MAX];
|
||||
bool online;
|
||||
uint32_t initial_state;
|
||||
} lidar_device_info_t;
|
||||
|
||||
typedef struct {
|
||||
float x, y, z;
|
||||
float intensity;
|
||||
} lidar_point_t;
|
||||
|
||||
|
||||
typedef struct {
|
||||
float intrinsics[9];
|
||||
float extrinsics[16];
|
||||
} lidar_calibration_t;
|
||||
|
||||
|
||||
#define DEVICE_MAX_CH_NUMBER 4
|
||||
|
||||
typedef struct {
|
||||
uint64_t timestamp_ns;
|
||||
int64_t pos[3];
|
||||
int64_t orient[4];
|
||||
} ros2_odom_convert_t;
|
||||
|
||||
typedef struct {
|
||||
uint64_t timestamp_ns;
|
||||
int64_t pos[3];
|
||||
int64_t orient[4];
|
||||
int64_t linear_velocity[3];
|
||||
int64_t angular_velocity[3];
|
||||
double pose_cov[36];
|
||||
double twist_cov[36];
|
||||
} ros_odom_convert_complete_t;
|
||||
|
||||
typedef struct {
|
||||
float accel_x;
|
||||
float accel_y;
|
||||
float accel_z;
|
||||
float gyro_x;
|
||||
float gyro_y;
|
||||
float gyro_z;
|
||||
uint64_t stamp;
|
||||
uint64_t sequence;
|
||||
} imu_convert_data_t;
|
||||
|
||||
typedef struct {
|
||||
uint32_t length;
|
||||
uint64_t sequence;
|
||||
uint64_t timestamp;
|
||||
uint64_t interval;
|
||||
void* pAddr;
|
||||
uint32_t width;
|
||||
uint32_t height;
|
||||
} buffer_List_t;
|
||||
|
||||
typedef struct {
|
||||
double delay;
|
||||
double offset;
|
||||
} ptp_sync_data_t;
|
||||
|
||||
typedef struct capture_Image_List_t {
|
||||
uint32_t imageCount;
|
||||
buffer_List_t imageList[DEVICE_MAX_CH_NUMBER];
|
||||
} capture_Image_List_t;
|
||||
|
||||
typedef struct {
|
||||
uint32_t type;
|
||||
capture_Image_List_t stream;
|
||||
} lidar_data_t;
|
||||
|
||||
typedef void (*lidar_device_callback_t)(const lidar_device_info_t* device, bool attach);
|
||||
typedef void (*lidar_data_callback_t)(const lidar_data_t *data, void *user_data);
|
||||
|
||||
typedef struct {
|
||||
lidar_data_callback_t data_callback;
|
||||
void *user_data;
|
||||
} lidar_data_callback_info_t;
|
||||
|
||||
typedef struct {
|
||||
int major;
|
||||
int minor;
|
||||
int patch;
|
||||
}lidar_version_t;
|
||||
|
||||
typedef struct {
|
||||
lidar_version_t kernel_version;
|
||||
lidar_version_t mcu_version;
|
||||
lidar_version_t soc_version;
|
||||
lidar_version_t Daemon_proc_version;
|
||||
lidar_version_t slam_version;
|
||||
} lidar_fireware_version_t;
|
||||
|
||||
/**
|
||||
* @brief RGB image sensor frame rate
|
||||
*
|
||||
*/
|
||||
typedef struct{
|
||||
|
||||
int configured_odr; /* rgb image sensor configured output data rate */
|
||||
int tx_odr; /* rgb image sensor tx output data rate */
|
||||
|
||||
} lidar_rgb_sensor_status_t;
|
||||
|
||||
/**
|
||||
* @brief DTOF Lidar frame rate
|
||||
*
|
||||
*/
|
||||
typedef struct{
|
||||
|
||||
int configured_odr; /* dtof lidar sensor configured output data rate */
|
||||
int tx_odr; /* dtof lidar sensor tx output data rate */
|
||||
int subframe_odr; /* dtof lidar sensor subframe output data rate */
|
||||
short tx_temp; /* dtof lidar tx module temp */
|
||||
short rx_temp; /* dtof lidar rx module temp */
|
||||
|
||||
} lidar_dtof_sensor_status_t;
|
||||
|
||||
/**
|
||||
* @brief IMU Sensor
|
||||
*
|
||||
*/
|
||||
typedef struct{
|
||||
|
||||
int configured_odr; /* imu sensor configured output data rate */
|
||||
int tx_odr; /* imu sensor tx output data rate */
|
||||
|
||||
} lidar_imu_sensor_status_t;
|
||||
|
||||
typedef struct{
|
||||
|
||||
int package_temp; /* soc package temp */
|
||||
int cpu_temp; /* cpu temp */
|
||||
int center_temp; /* center temp */
|
||||
int gpu_temp; /* gpu temp */
|
||||
int npu_temp; /* npu temp */
|
||||
|
||||
} lidar_soc_thermal_t;
|
||||
typedef struct
|
||||
{
|
||||
double uptime_seconds;
|
||||
lidar_soc_thermal_t soc_thermal;
|
||||
|
||||
int cpu_use_rate[8]; /* cpu usage rate */
|
||||
int ram_use_rate; /* ram usage rate */
|
||||
|
||||
lidar_rgb_sensor_status_t rgb_sensor;
|
||||
lidar_dtof_sensor_status_t dtof_sensor;
|
||||
lidar_imu_sensor_status_t imu_sensor;
|
||||
|
||||
int slam_cloud_tx_odr; /* slam cloud tx output data rate */
|
||||
int slam_odom_tx_odr; /* slam odom tx output data rate */
|
||||
int slam_odom_highfreq_tx_odr; /* slam odom high freq tx output data rate */
|
||||
|
||||
} lidar_device_status_t;
|
||||
|
||||
typedef enum {
|
||||
LIDAR_DEVICE_NONE = 0,
|
||||
LIDAR_DEVICE_NOT_INITIALIZED,
|
||||
LIDAR_DEVICE_INITIALIZED,
|
||||
LIDAR_DEVICE_STREAMING,
|
||||
LIDAR_DEVICE_STREAM_STOPPED,
|
||||
} lidar_device_initial_state_e;
|
||||
|
||||
typedef enum {
|
||||
LIDAR_DEPTH_ODR_10HZ = 0,
|
||||
LIDAR_DEPTH_ODR_14_5HZ,
|
||||
} lidar_depth_odr_e;
|
||||
|
||||
typedef struct {
|
||||
lidar_depth_odr_e odr;
|
||||
} lidar_depth_para_t;
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,159 @@
|
||||
#ifndef ODIN1_IMU_BRIDGE_H
|
||||
#define ODIN1_IMU_BRIDGE_H
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/**
|
||||
* 输入: 无
|
||||
* 输出: odin1_imu_sample_t
|
||||
* 作用: 描述一帧 IMU 数据, 供 C/C++/Python 共享使用
|
||||
* 注意: 坐标系已从 SDK 重映射为 ROS 标准 (right-handed: x前 y左 z上)
|
||||
* 重映射规则: accel_x←SDK_accel_y, accel_y←-SDK_accel_x, accel_z←SDK_accel_z
|
||||
* gyro_x ←SDK_gyro_y, gyro_y ←-SDK_gyro_x, gyro_z ←SDK_gyro_z
|
||||
*/
|
||||
typedef struct odin1_imu_sample_t {
|
||||
float accel_x; /* m/s², ROS 标准坐标系 */
|
||||
float accel_y;
|
||||
float accel_z;
|
||||
float gyro_x; /* rad/s, ROS 标准坐标系 */
|
||||
float gyro_y;
|
||||
float gyro_z;
|
||||
uint64_t stamp_ns;
|
||||
uint64_t sequence;
|
||||
} odin1_imu_sample_t;
|
||||
|
||||
/**
|
||||
* 输入: 无
|
||||
* 输出: odin1_odom_type_e
|
||||
* 作用: 标识里程计数据类型
|
||||
*/
|
||||
typedef enum {
|
||||
ODIN1_ODOM_STANDARD = 0, /* 标准里程计, 含位置/姿态/速度/协方差 */
|
||||
ODIN1_ODOM_HIGHFREQ, /* 高频里程计, 含位置/姿态 */
|
||||
ODIN1_ODOM_TF, /* TF 变换(重定位后), 全局坐标系下位姿 */
|
||||
} odin1_odom_type_e;
|
||||
|
||||
/**
|
||||
* 输入: 无
|
||||
* 输出: odin1_odom_sample_t
|
||||
* 作用: 描述一帧里程计数据, 统一容纳三种类型, 供 C/C++/Python 共享使用
|
||||
* 缩放: SDK 原始 int64 值已按 ÷1e6 转换为 double (位置:米, 姿态:四元数, 速度:m/s, rad/s)
|
||||
* 注意: 速度与协方差仅 ODIN1_ODOM_STANDARD 有效, 其他类型为零
|
||||
*/
|
||||
typedef struct odin1_odom_sample_t {
|
||||
odin1_odom_type_e type;
|
||||
uint64_t stamp_ns;
|
||||
double pos_x; /* 米, odom 坐标系 */
|
||||
double pos_y;
|
||||
double pos_z;
|
||||
double orient_w; /* 单位四元数 */
|
||||
double orient_x;
|
||||
double orient_y;
|
||||
double orient_z;
|
||||
double linear_vel_x; /* m/s (仅 STANDARD 有效) */
|
||||
double linear_vel_y;
|
||||
double linear_vel_z;
|
||||
double angular_vel_x; /* rad/s (仅 STANDARD 有效) */
|
||||
double angular_vel_y;
|
||||
double angular_vel_z;
|
||||
double pose_cov[36]; /* 位姿协方差 (仅 STANDARD 有效) */
|
||||
double twist_cov[36]; /* 速度协方差 (仅 STANDARD 有效) */
|
||||
} odin1_odom_sample_t;
|
||||
|
||||
/**
|
||||
* 输入: 无
|
||||
* 输出: const char*
|
||||
* 作用: 返回当前 bridge 的版本字符串
|
||||
*/
|
||||
const char* odin1_imu_version(void);
|
||||
|
||||
/**
|
||||
* 输入: timeout_ms[int]
|
||||
* 输出: int, 0 表示成功, 非 0 表示失败
|
||||
* 作用: 初始化 SDK, 等待设备连接并开始 IMU 数据流
|
||||
*/
|
||||
int odin1_imu_start(int timeout_ms);
|
||||
|
||||
/**
|
||||
* 输入: 无
|
||||
* 输出: 无
|
||||
* 作用: 停止数据流并释放 SDK 资源
|
||||
*/
|
||||
void odin1_imu_stop(void);
|
||||
|
||||
/**
|
||||
* 输入: 无
|
||||
* 输出: int, 1 表示运行中, 0 表示未运行
|
||||
* 作用: 返回当前 bridge 是否处于运行状态
|
||||
*/
|
||||
int odin1_imu_is_running(void);
|
||||
|
||||
/**
|
||||
* 输入: timeout_ms[int]
|
||||
* 输出: int, 1 表示有数据可读, 0 表示超时, 负数表示异常
|
||||
* 作用: 阻塞等待 IMU 数据到达
|
||||
*/
|
||||
int odin1_imu_wait_for_data(int timeout_ms);
|
||||
|
||||
/**
|
||||
* 输入: out_sample[odin1_imu_sample_t*]
|
||||
* 输出: int, 1 表示成功取出一帧, 0 表示队列为空, 负数表示异常
|
||||
* 作用: 从内部队列中弹出一帧 IMU 数据
|
||||
*/
|
||||
int odin1_imu_pop_sample(odin1_imu_sample_t* out_sample);
|
||||
|
||||
/**
|
||||
* 输入: out_sample[odin1_imu_sample_t*]
|
||||
* 输出: int, 1 表示成功读取, 0 表示当前还没有数据, 负数表示异常
|
||||
* 作用: 获取最近一帧 IMU 数据, 不会从队列中删除
|
||||
*/
|
||||
int odin1_imu_get_latest(odin1_imu_sample_t* out_sample);
|
||||
|
||||
/**
|
||||
* 输入: 无
|
||||
* 输出: const char*
|
||||
* 作用: 返回最近一次错误信息
|
||||
*/
|
||||
const char* odin1_imu_last_error(void);
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Odom (里程计) 接口
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
/**
|
||||
* 输入: timeout_ms[int]
|
||||
* 输出: int, 1 表示有数据可读, 0 表示超时, 负数表示异常
|
||||
* 作用: 阻塞等待里程计数据到达（任意类型）
|
||||
*/
|
||||
int odin1_odom_wait_for_data(int timeout_ms);
|
||||
|
||||
/**
|
||||
* 输入: out_sample[odin1_odom_sample_t*]
|
||||
* 输出: int, 1 表示成功取出一帧, 0 表示队列为空, 负数表示异常
|
||||
* 作用: 从内部队列中弹出一帧里程计数据
|
||||
*/
|
||||
int odin1_odom_pop_sample(odin1_odom_sample_t* out_sample);
|
||||
|
||||
/**
|
||||
* 输入: out_sample[odin1_odom_sample_t*]
|
||||
* 输出: int, 1 表示成功读取, 0 表示当前还没有数据, 负数表示异常
|
||||
* 作用: 获取最近一帧里程计数据, 不会从队列中删除
|
||||
*/
|
||||
int odin1_odom_get_latest(odin1_odom_sample_t* out_sample);
|
||||
|
||||
/**
|
||||
* 输入: 无
|
||||
* 输出: const char*
|
||||
* 作用: 返回最近一次里程计错误信息
|
||||
*/
|
||||
const char* odin1_odom_last_error(void);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
Binary file not shown.
@@ -0,0 +1,230 @@
|
||||
#!/usr/bin/python3
|
||||
"""ODIN1 IMU / Odom ctypes 封装."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import ctypes
|
||||
from enum import IntEnum
|
||||
from pathlib import Path
|
||||
from typing import Iterator, Optional
|
||||
|
||||
|
||||
class Odin1ImuSample(ctypes.Structure):
|
||||
"""输入: 无; 输出: Odin1ImuSample; 作用: 映射 C++ bridge 的 IMU 结构体."""
|
||||
|
||||
_fields_ = [
|
||||
("accel_x", ctypes.c_float),
|
||||
("accel_y", ctypes.c_float),
|
||||
("accel_z", ctypes.c_float),
|
||||
("gyro_x", ctypes.c_float),
|
||||
("gyro_y", ctypes.c_float),
|
||||
("gyro_z", ctypes.c_float),
|
||||
("stamp_ns", ctypes.c_uint64),
|
||||
("sequence", ctypes.c_uint64),
|
||||
]
|
||||
|
||||
|
||||
class Odin1OdomType(IntEnum):
|
||||
"""输入: 无; 输出: Odin1OdomType; 作用: 匹配 C 端 odin1_odom_type_e."""
|
||||
|
||||
STANDARD = 0
|
||||
HIGHFREQ = 1
|
||||
TF = 2
|
||||
|
||||
|
||||
class Odin1OdomSample(ctypes.Structure):
|
||||
"""输入: 无; 输出: Odin1OdomSample; 作用: 映射 C++ bridge 的里程计结构体.
|
||||
位置: 米, 姿态: 单位四元数, 速度: m/s 和 rad/s, 协方差: 原始 double."""
|
||||
|
||||
_fields_ = [
|
||||
("type", ctypes.c_int),
|
||||
("stamp_ns", ctypes.c_uint64),
|
||||
("pos_x", ctypes.c_double),
|
||||
("pos_y", ctypes.c_double),
|
||||
("pos_z", ctypes.c_double),
|
||||
("orient_w", ctypes.c_double),
|
||||
("orient_x", ctypes.c_double),
|
||||
("orient_y", ctypes.c_double),
|
||||
("orient_z", ctypes.c_double),
|
||||
("linear_vel_x", ctypes.c_double),
|
||||
("linear_vel_y", ctypes.c_double),
|
||||
("linear_vel_z", ctypes.c_double),
|
||||
("angular_vel_x", ctypes.c_double),
|
||||
("angular_vel_y", ctypes.c_double),
|
||||
("angular_vel_z", ctypes.c_double),
|
||||
("pose_cov", ctypes.c_double * 36),
|
||||
("twist_cov", ctypes.c_double * 36),
|
||||
]
|
||||
|
||||
|
||||
class Odin1ImuClient:
|
||||
"""输入: lib_path[Optional[str|Path]]; 输出: Odin1ImuClient; 作用: 提供 Python 对 ODIN1 IMU bridge 的访问接口."""
|
||||
|
||||
def __init__(self, lib_path: Optional[str | Path] = None) -> None:
|
||||
self._project_root = Path(__file__).resolve().parents[1]
|
||||
resolved_path = Path(lib_path) if lib_path else self._project_root / "build" / "libodin1_imu_bridge.so"
|
||||
self._lib = ctypes.CDLL(str(resolved_path))
|
||||
self._configure_signatures()
|
||||
|
||||
def _configure_signatures(self) -> None:
|
||||
"""输入: 无; 输出: 无; 作用: 配置 ctypes 函数签名."""
|
||||
|
||||
self._lib.odin1_imu_version.restype = ctypes.c_char_p
|
||||
|
||||
self._lib.odin1_imu_start.argtypes = [ctypes.c_int]
|
||||
self._lib.odin1_imu_start.restype = ctypes.c_int
|
||||
|
||||
self._lib.odin1_imu_stop.argtypes = []
|
||||
self._lib.odin1_imu_stop.restype = None
|
||||
|
||||
self._lib.odin1_imu_is_running.argtypes = []
|
||||
self._lib.odin1_imu_is_running.restype = ctypes.c_int
|
||||
|
||||
self._lib.odin1_imu_wait_for_data.argtypes = [ctypes.c_int]
|
||||
self._lib.odin1_imu_wait_for_data.restype = ctypes.c_int
|
||||
|
||||
self._lib.odin1_imu_pop_sample.argtypes = [ctypes.POINTER(Odin1ImuSample)]
|
||||
self._lib.odin1_imu_pop_sample.restype = ctypes.c_int
|
||||
|
||||
self._lib.odin1_imu_get_latest.argtypes = [ctypes.POINTER(Odin1ImuSample)]
|
||||
self._lib.odin1_imu_get_latest.restype = ctypes.c_int
|
||||
|
||||
self._lib.odin1_imu_last_error.argtypes = []
|
||||
self._lib.odin1_imu_last_error.restype = ctypes.c_char_p
|
||||
|
||||
# ---- Odom ----
|
||||
self._lib.odin1_odom_wait_for_data.argtypes = [ctypes.c_int]
|
||||
self._lib.odin1_odom_wait_for_data.restype = ctypes.c_int
|
||||
|
||||
self._lib.odin1_odom_pop_sample.argtypes = [ctypes.POINTER(Odin1OdomSample)]
|
||||
self._lib.odin1_odom_pop_sample.restype = ctypes.c_int
|
||||
|
||||
self._lib.odin1_odom_get_latest.argtypes = [ctypes.POINTER(Odin1OdomSample)]
|
||||
self._lib.odin1_odom_get_latest.restype = ctypes.c_int
|
||||
|
||||
self._lib.odin1_odom_last_error.argtypes = []
|
||||
self._lib.odin1_odom_last_error.restype = ctypes.c_char_p
|
||||
|
||||
def version(self) -> str:
|
||||
"""输入: 无; 输出: str; 作用: 获取 C++ bridge 版本号."""
|
||||
|
||||
return self._lib.odin1_imu_version().decode("utf-8")
|
||||
|
||||
def last_error(self) -> str:
|
||||
"""输入: 无; 输出: str; 作用: 获取最近一次 bridge 错误信息."""
|
||||
|
||||
return self._lib.odin1_imu_last_error().decode("utf-8")
|
||||
|
||||
def start(self, timeout_ms: int = 5000) -> None:
|
||||
"""输入: timeout_ms[int]; 输出: 无; 作用: 启动 IMU 数据接收."""
|
||||
|
||||
result = self._lib.odin1_imu_start(timeout_ms)
|
||||
if result != 0:
|
||||
raise RuntimeError(f"启动 ODIN1 IMU 失败: {self.last_error()} (code={result})")
|
||||
|
||||
def stop(self) -> None:
|
||||
"""输入: 无; 输出: 无; 作用: 停止 IMU 数据接收."""
|
||||
|
||||
self._lib.odin1_imu_stop()
|
||||
|
||||
def is_running(self) -> bool:
|
||||
"""输入: 无; 输出: bool; 作用: 返回 bridge 是否仍在运行."""
|
||||
|
||||
return bool(self._lib.odin1_imu_is_running())
|
||||
|
||||
def wait_for_data(self, timeout_ms: int = 1000) -> bool:
|
||||
"""输入: timeout_ms[int]; 输出: bool; 作用: 等待 IMU 数据到达（bridge 停止时返回 False 而非抛异常）."""
|
||||
|
||||
result = self._lib.odin1_imu_wait_for_data(timeout_ms)
|
||||
if result < 0:
|
||||
if not self.is_running():
|
||||
return False
|
||||
raise RuntimeError(f"等待 IMU 数据失败: {self.last_error()} (code={result})")
|
||||
return bool(result)
|
||||
|
||||
def pop_sample(self) -> Optional[Odin1ImuSample]:
|
||||
"""输入: 无; 输出: Optional[Odin1ImuSample]; 作用: 从队列中取出一帧 IMU 数据."""
|
||||
|
||||
sample = Odin1ImuSample()
|
||||
result = self._lib.odin1_imu_pop_sample(ctypes.byref(sample))
|
||||
if result < 0:
|
||||
raise RuntimeError(f"读取 IMU 队列失败: {self.last_error()} (code={result})")
|
||||
return sample if result == 1 else None
|
||||
|
||||
def get_latest(self) -> Optional[Odin1ImuSample]:
|
||||
"""输入: 无; 输出: Optional[Odin1ImuSample]; 作用: 获取最近一帧 IMU 数据（无缝锁，永不等锁）."""
|
||||
|
||||
sample = Odin1ImuSample()
|
||||
result = self._lib.odin1_imu_get_latest(ctypes.byref(sample))
|
||||
if result < 0:
|
||||
raise RuntimeError(f"读取最新 IMU 数据失败: {self.last_error()} (code={result})")
|
||||
return sample if result == 1 else None
|
||||
|
||||
@property
|
||||
def latest(self) -> Optional[Odin1ImuSample]:
|
||||
"""输入: 无; 输出: Optional[Odin1ImuSample]; 作用: get_latest 的属性形式，控制循环中推荐使用."""
|
||||
return self.get_latest()
|
||||
|
||||
def iter_samples(self, timeout_ms: int = 1000) -> Iterator[Odin1ImuSample]:
|
||||
"""输入: timeout_ms[int]; 输出: Iterator[Odin1ImuSample]; 作用: 连续迭代输出 IMU 数据."""
|
||||
|
||||
while self.is_running():
|
||||
if not self.wait_for_data(timeout_ms):
|
||||
continue
|
||||
while True:
|
||||
sample = self.pop_sample()
|
||||
if sample is None:
|
||||
break
|
||||
yield sample
|
||||
|
||||
# ---- Odom (里程计) 方法 ----
|
||||
|
||||
def odom_last_error(self) -> str:
|
||||
"""输入: 无; 输出: str; 作用: 获取最近一次里程计错误信息."""
|
||||
|
||||
return self._lib.odin1_odom_last_error().decode("utf-8")
|
||||
|
||||
def odom_wait_for_data(self, timeout_ms: int = 1000) -> bool:
|
||||
"""输入: timeout_ms[int]; 输出: bool; 作用: 等待里程计数据到达（bridge 停止时返回 False 而非抛异常）."""
|
||||
|
||||
result = self._lib.odin1_odom_wait_for_data(timeout_ms)
|
||||
if result < 0:
|
||||
if not self.is_running():
|
||||
return False
|
||||
raise RuntimeError(f"等待里程计数据失败: {self.odom_last_error()} (code={result})")
|
||||
return bool(result)
|
||||
|
||||
def odom_pop_sample(self) -> Optional[Odin1OdomSample]:
|
||||
"""输入: 无; 输出: Optional[Odin1OdomSample]; 作用: 从队列中取出一帧里程计数据."""
|
||||
|
||||
sample = Odin1OdomSample()
|
||||
result = self._lib.odin1_odom_pop_sample(ctypes.byref(sample))
|
||||
if result < 0:
|
||||
raise RuntimeError(f"读取里程计队列失败: {self.odom_last_error()} (code={result})")
|
||||
return sample if result == 1 else None
|
||||
|
||||
def odom_get_latest(self) -> Optional[Odin1OdomSample]:
|
||||
"""输入: 无; 输出: Optional[Odin1OdomSample]; 作用: 获取最近一帧里程计数据（无缝锁）."""
|
||||
|
||||
sample = Odin1OdomSample()
|
||||
result = self._lib.odin1_odom_get_latest(ctypes.byref(sample))
|
||||
if result < 0:
|
||||
raise RuntimeError(f"读取最新里程计数据失败: {self.odom_last_error()} (code={result})")
|
||||
return sample if result == 1 else None
|
||||
|
||||
@property
|
||||
def odom_latest(self) -> Optional[Odin1OdomSample]:
|
||||
"""输入: 无; 输出: Optional[Odin1OdomSample]; 作用: odom_get_latest 的属性形式."""
|
||||
return self.odom_get_latest()
|
||||
|
||||
def iter_odom(self, timeout_ms: int = 1000) -> Iterator[Odin1OdomSample]:
|
||||
"""输入: timeout_ms[int]; 输出: Iterator[Odin1OdomSample]; 作用: 连续迭代输出里程计数据."""
|
||||
|
||||
while self.is_running():
|
||||
if not self.odom_wait_for_data(timeout_ms):
|
||||
continue
|
||||
while True:
|
||||
sample = self.odom_pop_sample()
|
||||
if sample is None:
|
||||
break
|
||||
yield sample
|
||||
@@ -0,0 +1,655 @@
|
||||
#include "odin1_imu_bridge.h"
|
||||
|
||||
#include "lidar_api.h"
|
||||
#include "lidar_api_type.h"
|
||||
|
||||
#include <atomic>
|
||||
#include <chrono>
|
||||
#include <condition_variable>
|
||||
#include <cstdio>
|
||||
#include <cstring>
|
||||
#include <deque>
|
||||
#include <mutex>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
|
||||
namespace {
|
||||
|
||||
constexpr const char* kBridgeVersion = "0.2.0";
|
||||
constexpr std::size_t kMaxQueueSize = 1024;
|
||||
constexpr std::size_t kMaxOdomQueueSize = 2048;
|
||||
constexpr int kDefaultMode = LIDAR_MODE_SLAM;
|
||||
|
||||
std::atomic<bool> g_running{false};
|
||||
std::atomic<bool> g_sdk_initialized{false};
|
||||
std::atomic<bool> g_device_connected{false};
|
||||
std::atomic<bool> g_stream_started{false};
|
||||
|
||||
device_handle g_device = nullptr;
|
||||
|
||||
std::mutex g_state_mutex;
|
||||
std::mutex g_queue_mutex;
|
||||
std::condition_variable g_queue_cv;
|
||||
std::deque<odin1_imu_sample_t> g_queue;
|
||||
odin1_imu_sample_t g_latest_sample{};
|
||||
bool g_has_latest_sample = false;
|
||||
|
||||
// 无缝锁 (seqlock) 专用：让 get_latest 与 SDK 回调完全无争抢
|
||||
std::atomic<std::uint64_t> g_latest_seq{0};
|
||||
odin1_imu_sample_t g_latest_lockfree{};
|
||||
|
||||
std::mutex g_error_mutex;
|
||||
std::string g_last_error = "bridge not started";
|
||||
|
||||
// ---- Odom (里程计) 专用状态 ----
|
||||
std::mutex g_odom_queue_mutex;
|
||||
std::condition_variable g_odom_queue_cv;
|
||||
std::deque<odin1_odom_sample_t> g_odom_queue;
|
||||
bool g_odom_enabled = false;
|
||||
|
||||
// 无缝锁 (seqlock) 专用：让 get_latest 与 SDK 回调完全无争抢
|
||||
std::atomic<std::uint64_t> g_odom_latest_seq{0};
|
||||
odin1_odom_sample_t g_odom_latest_lockfree{};
|
||||
|
||||
std::mutex g_odom_error_mutex;
|
||||
std::string g_odom_last_error = "odom bridge not started";
|
||||
|
||||
/**
|
||||
* 输入: message[const std::string&]
|
||||
* 输出: 无
|
||||
* 作用: 线程安全地记录最近一次错误信息
|
||||
*/
|
||||
void set_last_error(const std::string& message) {
|
||||
std::lock_guard<std::mutex> lock(g_error_mutex);
|
||||
g_last_error = message;
|
||||
}
|
||||
|
||||
/**
|
||||
* 输入: message[const std::string&]
|
||||
* 输出: 无
|
||||
* 作用: 线程安全地记录最近一次 odom 错误信息
|
||||
*/
|
||||
void set_odom_last_error(const std::string& message) {
|
||||
std::lock_guard<std::mutex> lock(g_odom_error_mutex);
|
||||
g_odom_last_error = message;
|
||||
}
|
||||
|
||||
/**
|
||||
* 输入: 无
|
||||
* 输出: 无
|
||||
* 作用: 清空内部 IMU 队列和最近一帧缓存
|
||||
*/
|
||||
void clear_queue_locked_state() {
|
||||
std::lock_guard<std::mutex> lock(g_queue_mutex);
|
||||
g_queue.clear();
|
||||
g_latest_sample = {};
|
||||
g_has_latest_sample = false;
|
||||
}
|
||||
|
||||
/**
|
||||
* 输入: 无
|
||||
* 输出: 无
|
||||
* 作用: 清空内部 Odom 队列
|
||||
*/
|
||||
void clear_odom_queue_locked_state() {
|
||||
std::lock_guard<std::mutex> lock(g_odom_queue_mutex);
|
||||
g_odom_queue.clear();
|
||||
}
|
||||
|
||||
/**
|
||||
* 输入: raw_sample[const imu_convert_data_t*]
|
||||
* 输出: odin1_imu_sample_t
|
||||
* 作用: 将 SDK IMU 结构转换为 bridge 对外结构, 并重映射为 ROS 标准坐标系
|
||||
* 坐标系: x前 y左 z上 (right-handed)
|
||||
* 重映射: accel_x←SDK_accel_y, accel_y←-SDK_accel_x (来源: host_sdk_sample.h publishImu)
|
||||
*/
|
||||
odin1_imu_sample_t convert_sample(const imu_convert_data_t* raw_sample) {
|
||||
odin1_imu_sample_t converted{};
|
||||
if (raw_sample == nullptr) {
|
||||
return converted;
|
||||
}
|
||||
|
||||
// 加速度轴重映射 (SDK → ROS 标准)
|
||||
converted.accel_x = raw_sample->accel_y;
|
||||
converted.accel_y = -raw_sample->accel_x;
|
||||
converted.accel_z = raw_sample->accel_z;
|
||||
// 陀螺仪轴重映射
|
||||
converted.gyro_x = raw_sample->gyro_y;
|
||||
converted.gyro_y = -raw_sample->gyro_x;
|
||||
converted.gyro_z = raw_sample->gyro_z;
|
||||
converted.stamp_ns = raw_sample->stamp;
|
||||
converted.sequence = raw_sample->sequence;
|
||||
return converted;
|
||||
}
|
||||
|
||||
/**
|
||||
* 输入: raw[const ros_odom_convert_complete_t*]
|
||||
* 输出: odin1_odom_sample_t
|
||||
* 作用: 将 SDK 标准里程计结构转换为 bridge 对外结构
|
||||
* 缩放因子: ÷1e6 (来源: host_sdk_sample.h publishOdometry)
|
||||
*/
|
||||
odin1_odom_sample_t convert_odom_standard(const ros_odom_convert_complete_t* raw) {
|
||||
odin1_odom_sample_t out{};
|
||||
if (raw == nullptr) return out;
|
||||
out.type = ODIN1_ODOM_STANDARD;
|
||||
out.stamp_ns = raw->timestamp_ns;
|
||||
out.pos_x = static_cast<double>(raw->pos[0]) / 1e6;
|
||||
out.pos_y = static_cast<double>(raw->pos[1]) / 1e6;
|
||||
out.pos_z = static_cast<double>(raw->pos[2]) / 1e6;
|
||||
out.orient_w = static_cast<double>(raw->orient[0]) / 1e6;
|
||||
out.orient_x = static_cast<double>(raw->orient[1]) / 1e6;
|
||||
out.orient_y = static_cast<double>(raw->orient[2]) / 1e6;
|
||||
out.orient_z = static_cast<double>(raw->orient[3]) / 1e6;
|
||||
out.linear_vel_x = static_cast<double>(raw->linear_velocity[0]) / 1e6;
|
||||
out.linear_vel_y = static_cast<double>(raw->linear_velocity[1]) / 1e6;
|
||||
out.linear_vel_z = static_cast<double>(raw->linear_velocity[2]) / 1e6;
|
||||
out.angular_vel_x = static_cast<double>(raw->angular_velocity[0]) / 1e6;
|
||||
out.angular_vel_y = static_cast<double>(raw->angular_velocity[1]) / 1e6;
|
||||
out.angular_vel_z = static_cast<double>(raw->angular_velocity[2]) / 1e6;
|
||||
for (int i = 0; i < 36; ++i) {
|
||||
out.pose_cov[i] = raw->pose_cov[i];
|
||||
out.twist_cov[i] = raw->twist_cov[i];
|
||||
}
|
||||
return out;
|
||||
}
|
||||
|
||||
/**
|
||||
* 输入: raw[const ros2_odom_convert_t*], type[odin1_odom_type_e]
|
||||
* 输出: odin1_odom_sample_t
|
||||
* 作用: 将 SDK 高频/TF 里程计结构转换为 bridge 对外结构
|
||||
* 缩放因子: ÷1e6 (来源: host_sdk_sample.h publishOdometry)
|
||||
*/
|
||||
odin1_odom_sample_t convert_odom_compact(const ros2_odom_convert_t* raw, odin1_odom_type_e type) {
|
||||
odin1_odom_sample_t out{};
|
||||
if (raw == nullptr) return out;
|
||||
out.type = type;
|
||||
out.stamp_ns = raw->timestamp_ns;
|
||||
out.pos_x = static_cast<double>(raw->pos[0]) / 1e6;
|
||||
out.pos_y = static_cast<double>(raw->pos[1]) / 1e6;
|
||||
out.pos_z = static_cast<double>(raw->pos[2]) / 1e6;
|
||||
out.orient_w = static_cast<double>(raw->orient[0]) / 1e6;
|
||||
out.orient_x = static_cast<double>(raw->orient[1]) / 1e6;
|
||||
out.orient_y = static_cast<double>(raw->orient[2]) / 1e6;
|
||||
out.orient_z = static_cast<double>(raw->orient[3]) / 1e6;
|
||||
return out;
|
||||
}
|
||||
|
||||
/**
|
||||
* 输入: 无
|
||||
* 输出: 无
|
||||
* 作用: 安全关闭当前设备与 SDK 资源
|
||||
*/
|
||||
void cleanup_device_and_sdk() {
|
||||
std::lock_guard<std::mutex> lock(g_state_mutex);
|
||||
|
||||
if (g_device != nullptr) {
|
||||
if (g_stream_started.load()) {
|
||||
lidar_deactivate_stream_type(g_device, LIDAR_DT_RAW_IMU); // SDK接口，来源: include/lidar_api.h
|
||||
if (g_odom_enabled) {
|
||||
lidar_deactivate_stream_type(g_device, LIDAR_DT_SLAM_ODOMETRY); // SDK接口
|
||||
lidar_deactivate_stream_type(g_device, LIDAR_DT_SLAM_ODOMETRY_HIGHFREQ); // SDK接口
|
||||
lidar_deactivate_stream_type(g_device, LIDAR_DT_SLAM_ODOMETRY_TF); // SDK接口
|
||||
}
|
||||
lidar_stop_stream(g_device, kDefaultMode); // SDK接口，来源: include/lidar_api.h
|
||||
g_stream_started = false;
|
||||
}
|
||||
|
||||
lidar_unregister_stream_callback(g_device); // SDK接口，来源: include/lidar_api.h
|
||||
lidar_close_device(g_device); // SDK接口，来源: include/lidar_api.h
|
||||
lidar_destory_device(g_device); // SDK接口，来源: include/lidar_api.h
|
||||
g_device = nullptr;
|
||||
}
|
||||
|
||||
if (g_sdk_initialized.load()) {
|
||||
lidar_system_deinit(); // SDK接口，来源: include/lidar_api.h
|
||||
g_sdk_initialized = false;
|
||||
}
|
||||
|
||||
g_device_connected = false;
|
||||
}
|
||||
|
||||
/**
|
||||
* 输入: data[const lidar_data_t*], user_data[void*]
|
||||
* 输出: 无
|
||||
* 作用: 接收 SDK 回调中的 IMU 数据并写入内部缓存队列
|
||||
*/
|
||||
void lidar_data_callback(const lidar_data_t* data, void* user_data) {
|
||||
(void)user_data;
|
||||
|
||||
if (!g_running.load() || data == nullptr) {
|
||||
return;
|
||||
}
|
||||
|
||||
// ---- IMU 处理 ----
|
||||
if (data->type == LIDAR_DT_RAW_IMU) {
|
||||
if (data->stream.imageList[0].pAddr == nullptr) {
|
||||
set_last_error("sdk imu callback returned null payload");
|
||||
return;
|
||||
}
|
||||
|
||||
const auto* raw_sample =
|
||||
static_cast<const imu_convert_data_t*>(data->stream.imageList[0].pAddr);
|
||||
odin1_imu_sample_t sample = convert_sample(raw_sample);
|
||||
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(g_queue_mutex);
|
||||
if (g_queue.size() >= kMaxQueueSize) {
|
||||
g_queue.pop_front();
|
||||
}
|
||||
g_queue.push_back(sample);
|
||||
g_latest_sample = sample;
|
||||
g_has_latest_sample = true;
|
||||
}
|
||||
|
||||
// 无缝锁写入：Python get_latest 可同时读取，永不等锁
|
||||
{
|
||||
std::uint64_t seq = g_latest_seq.fetch_add(1, std::memory_order_acquire) + 1;
|
||||
g_latest_lockfree = sample;
|
||||
g_latest_seq.store(seq + 1, std::memory_order_release);
|
||||
}
|
||||
|
||||
g_queue_cv.notify_all();
|
||||
return;
|
||||
}
|
||||
|
||||
// ---- Odom 处理 ----
|
||||
if (g_odom_enabled) {
|
||||
odin1_odom_sample_t odom_sample{};
|
||||
bool valid = false;
|
||||
|
||||
switch (data->type) {
|
||||
case LIDAR_DT_SLAM_ODOMETRY: {
|
||||
if (data->stream.imageList[0].pAddr == nullptr) {
|
||||
set_odom_last_error("sdk standard odom callback returned null payload");
|
||||
return;
|
||||
}
|
||||
const auto* raw =
|
||||
static_cast<const ros_odom_convert_complete_t*>(data->stream.imageList[0].pAddr);
|
||||
odom_sample = convert_odom_standard(raw);
|
||||
valid = true;
|
||||
break;
|
||||
}
|
||||
case LIDAR_DT_SLAM_ODOMETRY_HIGHFREQ: {
|
||||
if (data->stream.imageList[0].pAddr == nullptr) {
|
||||
set_odom_last_error("sdk high-freq odom callback returned null payload");
|
||||
return;
|
||||
}
|
||||
const auto* raw =
|
||||
static_cast<const ros2_odom_convert_t*>(data->stream.imageList[0].pAddr);
|
||||
odom_sample = convert_odom_compact(raw, ODIN1_ODOM_HIGHFREQ);
|
||||
valid = true;
|
||||
break;
|
||||
}
|
||||
case LIDAR_DT_SLAM_ODOMETRY_TF: {
|
||||
if (data->stream.imageList[0].pAddr == nullptr) {
|
||||
set_odom_last_error("sdk tf odom callback returned null payload");
|
||||
return;
|
||||
}
|
||||
const auto* raw =
|
||||
static_cast<const ros2_odom_convert_t*>(data->stream.imageList[0].pAddr);
|
||||
odom_sample = convert_odom_compact(raw, ODIN1_ODOM_TF);
|
||||
valid = true;
|
||||
break;
|
||||
}
|
||||
default:
|
||||
return;
|
||||
}
|
||||
|
||||
if (!valid) return;
|
||||
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(g_odom_queue_mutex);
|
||||
if (g_odom_queue.size() >= kMaxOdomQueueSize) {
|
||||
g_odom_queue.pop_front();
|
||||
}
|
||||
g_odom_queue.push_back(odom_sample);
|
||||
}
|
||||
|
||||
// 无缝锁写入
|
||||
{
|
||||
std::uint64_t seq = g_odom_latest_seq.fetch_add(1, std::memory_order_acquire) + 1;
|
||||
g_odom_latest_lockfree = odom_sample;
|
||||
g_odom_latest_seq.store(seq + 1, std::memory_order_release);
|
||||
}
|
||||
|
||||
g_odom_queue_cv.notify_all();
|
||||
return;
|
||||
}
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
/**
|
||||
* 输入: device_info[const lidar_device_info_t*], attach[bool]
|
||||
* 输出: 无
|
||||
* 作用: 响应 SDK 设备插拔事件并启动 IMU 数据流
|
||||
*/
|
||||
void lidar_device_callback(const lidar_device_info_t* device_info, bool attach) {
|
||||
if (!g_running.load()) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (!attach) {
|
||||
g_device_connected = false;
|
||||
g_stream_started = false;
|
||||
return;
|
||||
}
|
||||
|
||||
if (device_info == nullptr) {
|
||||
set_last_error("sdk device callback returned null device info");
|
||||
return;
|
||||
}
|
||||
|
||||
std::lock_guard<std::mutex> lock(g_state_mutex);
|
||||
|
||||
if (g_device != nullptr) {
|
||||
return;
|
||||
}
|
||||
|
||||
device_handle device_handle_local = nullptr;
|
||||
if (lidar_create_device(const_cast<lidar_device_info_t*>(device_info), &device_handle_local) != 0) { // SDK接口，来源: include/lidar_api.h
|
||||
set_last_error("lidar_create_device failed");
|
||||
return;
|
||||
}
|
||||
|
||||
if (lidar_open_device(device_handle_local) != 0) { // SDK接口，来源: include/lidar_api.h
|
||||
set_last_error("lidar_open_device failed");
|
||||
lidar_destory_device(device_handle_local); // SDK接口，来源: include/lidar_api.h
|
||||
return;
|
||||
}
|
||||
|
||||
lidar_data_callback_info_t callback_info{};
|
||||
callback_info.data_callback = lidar_data_callback;
|
||||
callback_info.user_data = nullptr;
|
||||
if (lidar_register_stream_callback(device_handle_local, callback_info) != 0) { // SDK接口，来源: include/lidar_api.h
|
||||
set_last_error("lidar_register_stream_callback failed");
|
||||
lidar_close_device(device_handle_local); // SDK接口，来源: include/lidar_api.h
|
||||
lidar_destory_device(device_handle_local); // SDK接口，来源: include/lidar_api.h
|
||||
return;
|
||||
}
|
||||
|
||||
uint32_t dtof_subframe_odr = 0;
|
||||
if (lidar_start_stream(device_handle_local, kDefaultMode, dtof_subframe_odr) != 0) { // SDK接口，来源: include/lidar_api.h
|
||||
(void)dtof_subframe_odr;
|
||||
set_last_error("lidar_start_stream failed");
|
||||
lidar_unregister_stream_callback(device_handle_local); // SDK接口，来源: include/lidar_api.h
|
||||
lidar_close_device(device_handle_local); // SDK接口，来源: include/lidar_api.h
|
||||
lidar_destory_device(device_handle_local); // SDK接口，来源: include/lidar_api.h
|
||||
return;
|
||||
}
|
||||
|
||||
if (lidar_activate_stream_type(device_handle_local, LIDAR_DT_RAW_IMU) != 0) { // SDK接口，来源: include/lidar_api.h
|
||||
set_last_error("lidar_activate_stream_type(raw_imu) failed");
|
||||
lidar_stop_stream(device_handle_local, kDefaultMode); // SDK接口，来源: include/lidar_api.h
|
||||
lidar_unregister_stream_callback(device_handle_local); // SDK接口，来源: include/lidar_api.h
|
||||
lidar_close_device(device_handle_local); // SDK接口，来源: include/lidar_api.h
|
||||
lidar_destory_device(device_handle_local); // SDK接口，来源: include/lidar_api.h
|
||||
return;
|
||||
}
|
||||
|
||||
// 激活里程计数据流（STANDARD 类型，SDK 会自动启用 HIGHFREQ 和 TF）
|
||||
// 参考: official ROS driver 只激活 LIDAR_DT_SLAM_ODOMETRY
|
||||
if (lidar_activate_stream_type(device_handle_local, LIDAR_DT_SLAM_ODOMETRY) != 0) { // SDK接口
|
||||
set_odom_last_error("lidar_activate_stream_type(slam_odometry) failed, odom disabled");
|
||||
g_odom_enabled = false;
|
||||
fprintf(stderr, "[bridge] WARNING: odom stream activation failed, odom disabled\n");
|
||||
} else {
|
||||
g_odom_enabled = true;
|
||||
set_odom_last_error("");
|
||||
clear_odom_queue_locked_state();
|
||||
fprintf(stdout, "[bridge] odom stream activated (STANDARD + HIGHFREQ + TF)\n");
|
||||
}
|
||||
|
||||
g_device = device_handle_local;
|
||||
g_stream_started = true;
|
||||
g_device_connected = true;
|
||||
set_last_error("");
|
||||
g_queue_cv.notify_all();
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
extern "C" {
|
||||
|
||||
/**
|
||||
* 输入: 无
|
||||
* 输出: const char*
|
||||
* 作用: 返回当前 bridge 的版本字符串
|
||||
*/
|
||||
const char* odin1_imu_version(void) {
|
||||
return kBridgeVersion;
|
||||
}
|
||||
|
||||
/**
|
||||
* 输入: timeout_ms[int]
|
||||
* 输出: int, 0 表示成功, 非 0 表示失败
|
||||
* 作用: 初始化 SDK, 等待设备连接并开始 IMU 数据流
|
||||
*/
|
||||
int odin1_imu_start(int timeout_ms) {
|
||||
if (timeout_ms <= 0) {
|
||||
timeout_ms = 5000;
|
||||
}
|
||||
|
||||
if (g_running.load()) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
clear_queue_locked_state();
|
||||
set_last_error("waiting for odin1 device");
|
||||
|
||||
if (lidar_system_init(lidar_device_callback) != 0) { // SDK接口，来源: include/lidar_api.h
|
||||
set_last_error("lidar_system_init failed");
|
||||
return -1;
|
||||
}
|
||||
|
||||
g_sdk_initialized = true;
|
||||
g_running = true;
|
||||
|
||||
const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(timeout_ms);
|
||||
while (std::chrono::steady_clock::now() < deadline) {
|
||||
if (g_device_connected.load()) {
|
||||
return 0;
|
||||
}
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(50));
|
||||
}
|
||||
|
||||
set_last_error("timeout waiting for odin1 imu stream");
|
||||
odin1_imu_stop();
|
||||
return -2;
|
||||
}
|
||||
|
||||
/**
|
||||
* 输入: 无
|
||||
* 输出: 无
|
||||
* 作用: 停止数据流并释放 SDK 资源
|
||||
*/
|
||||
void odin1_imu_stop(void) {
|
||||
g_running = false;
|
||||
g_odom_enabled = false;
|
||||
cleanup_device_and_sdk();
|
||||
clear_queue_locked_state();
|
||||
clear_odom_queue_locked_state();
|
||||
g_queue_cv.notify_all();
|
||||
g_odom_queue_cv.notify_all();
|
||||
}
|
||||
|
||||
/**
|
||||
* 输入: 无
|
||||
* 输出: int, 1 表示运行中, 0 表示未运行
|
||||
* 作用: 返回当前 bridge 是否处于运行状态
|
||||
*/
|
||||
int odin1_imu_is_running(void) {
|
||||
return g_running.load() ? 1 : 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* 输入: timeout_ms[int]
|
||||
* 输出: int, 1 表示有数据可读, 0 表示超时, 负数表示异常
|
||||
* 作用: 阻塞等待 IMU 数据到达
|
||||
*/
|
||||
int odin1_imu_wait_for_data(int timeout_ms) {
|
||||
if (!g_running.load()) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
std::unique_lock<std::mutex> lock(g_queue_mutex);
|
||||
const bool ready = g_queue_cv.wait_for(
|
||||
lock,
|
||||
std::chrono::milliseconds(timeout_ms > 0 ? timeout_ms : 1000),
|
||||
[] { return !g_queue.empty() || !g_running.load(); });
|
||||
|
||||
if (!g_running.load()) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
return ready && !g_queue.empty() ? 1 : 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* 输入: out_sample[odin1_imu_sample_t*]
|
||||
* 输出: int, 1 表示成功取出一帧, 0 表示队列为空, 负数表示异常
|
||||
* 作用: 从内部队列中弹出一帧 IMU 数据
|
||||
*/
|
||||
int odin1_imu_pop_sample(odin1_imu_sample_t* out_sample) {
|
||||
if (out_sample == nullptr) {
|
||||
set_last_error("odin1_imu_pop_sample received null output pointer");
|
||||
return -1;
|
||||
}
|
||||
|
||||
std::lock_guard<std::mutex> lock(g_queue_mutex);
|
||||
if (g_queue.empty()) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
*out_sample = g_queue.front();
|
||||
g_queue.pop_front();
|
||||
return 1;
|
||||
}
|
||||
|
||||
/**
|
||||
* 输入: out_sample[odin1_imu_sample_t*]
|
||||
* 输出: int, 1 表示成功读取, 0 表示当前还没有数据, 负数表示异常
|
||||
* 作用: 获取最近一帧 IMU 数据, 不会从队列中删除
|
||||
*/
|
||||
int odin1_imu_get_latest(odin1_imu_sample_t* out_sample) {
|
||||
if (out_sample == nullptr) {
|
||||
set_last_error("odin1_imu_get_latest received null output pointer");
|
||||
return -1;
|
||||
}
|
||||
|
||||
// 无缝锁读取：与 SDK 回调无锁争抢，延迟最低
|
||||
std::uint64_t before, after;
|
||||
do {
|
||||
before = g_latest_seq.load(std::memory_order_acquire);
|
||||
if (before & 1) {
|
||||
// 写者正在写，自旋等待（纳秒级，200Hz 写入下概率极低）
|
||||
continue;
|
||||
}
|
||||
if (before == 0) {
|
||||
// 还没有任何数据写入
|
||||
return 0;
|
||||
}
|
||||
*out_sample = g_latest_lockfree;
|
||||
after = g_latest_seq.load(std::memory_order_acquire);
|
||||
} while (before != after);
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
/**
|
||||
* 输入: 无
|
||||
* 输出: const char*
|
||||
* 作用: 返回最近一次错误信息
|
||||
*/
|
||||
const char* odin1_imu_last_error(void) {
|
||||
std::lock_guard<std::mutex> lock(g_error_mutex);
|
||||
return g_last_error.c_str();
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Odom (里程计) 接口实现
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
/**
|
||||
* 输入: timeout_ms[int]
|
||||
* 输出: int, 1 表示有数据可读, 0 表示超时, 负数表示异常
|
||||
* 作用: 阻塞等待里程计数据到达（任意类型）
|
||||
*/
|
||||
int odin1_odom_wait_for_data(int timeout_ms) {
|
||||
if (!g_running.load()) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
std::unique_lock<std::mutex> lock(g_odom_queue_mutex);
|
||||
const bool ready = g_odom_queue_cv.wait_for(
|
||||
lock,
|
||||
std::chrono::milliseconds(timeout_ms > 0 ? timeout_ms : 1000),
|
||||
[] { return !g_odom_queue.empty() || !g_running.load(); });
|
||||
|
||||
if (!g_running.load()) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
return ready && !g_odom_queue.empty() ? 1 : 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* 输入: out_sample[odin1_odom_sample_t*]
|
||||
* 输出: int, 1 表示成功取出一帧, 0 表示队列为空, 负数表示异常
|
||||
* 作用: 从内部队列中弹出一帧里程计数据
|
||||
*/
|
||||
int odin1_odom_pop_sample(odin1_odom_sample_t* out_sample) {
|
||||
if (out_sample == nullptr) {
|
||||
set_odom_last_error("odin1_odom_pop_sample received null output pointer");
|
||||
return -1;
|
||||
}
|
||||
|
||||
std::lock_guard<std::mutex> lock(g_odom_queue_mutex);
|
||||
if (g_odom_queue.empty()) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
*out_sample = g_odom_queue.front();
|
||||
g_odom_queue.pop_front();
|
||||
return 1;
|
||||
}
|
||||
|
||||
/**
|
||||
* 输入: out_sample[odin1_odom_sample_t*]
|
||||
* 输出: int, 1 表示成功读取, 0 表示当前还没有数据, 负数表示异常
|
||||
* 作用: 获取最近一帧里程计数据, 不会从队列中删除
|
||||
*/
|
||||
int odin1_odom_get_latest(odin1_odom_sample_t* out_sample) {
|
||||
if (out_sample == nullptr) {
|
||||
set_odom_last_error("odin1_odom_get_latest received null output pointer");
|
||||
return -1;
|
||||
}
|
||||
|
||||
// 无缝锁读取：与 SDK 回调无锁争抢，延迟最低
|
||||
std::uint64_t before, after;
|
||||
do {
|
||||
before = g_odom_latest_seq.load(std::memory_order_acquire);
|
||||
if (before & 1) {
|
||||
continue;
|
||||
}
|
||||
if (before == 0) {
|
||||
return 0;
|
||||
}
|
||||
*out_sample = g_odom_latest_lockfree;
|
||||
after = g_odom_latest_seq.load(std::memory_order_acquire);
|
||||
} while (before != after);
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
/**
|
||||
* 输入: 无
|
||||
* 输出: const char*
|
||||
* 作用: 返回最近一次里程计错误信息
|
||||
*/
|
||||
const char* odin1_odom_last_error(void) {
|
||||
std::lock_guard<std::mutex> lock(g_odom_error_mutex);
|
||||
return g_odom_last_error.c_str();
|
||||
}
|
||||
|
||||
} // extern "C"
|
||||
Reference in New Issue
Block a user