小车串口发送传感器数据并接收线速度和角速度

  1. //需要发送的数据
  2. unsigned char infrared_data[48] = {0x7a,0x01,10};
  3. //发送小车数据
  4. void send_speed(void){
  5.     //3
  6.     float x = 1.123;
  7.     //7
  8.     float y = 2.123;
  9.     //11
  10.     float z = 3.123;
  11.     //15
  12.     float ax = 4.123;
  13.     //20
  14.     float ay = 5.123;
  15.     //24
  16.     float az = 6.123;
  17.     //28
  18.     float gx = 7.123;
  19.     //32
  20.     float gy = 8.123;
  21.     //36
  22.     float gz = 9.123;
  23.     //40
  24.     float v = 10.123;
  25.     //44
  26.     float check_sum = 0x01+10+x+y+z+ax+ay+az+gx+gy+gz+v;
  28.     //数据准备
  29.     memcpy(infrared_data+3,&x,4);
  30.     memcpy(infrared_data+7,&y,4);
  31.     memcpy(infrared_data+11,&z,4);
  32.     memcpy(infrared_data+15,&ax,4);
  33.     memcpy(infrared_data+19,&ay,4);
  34.     memcpy(infrared_data+23,&az,4);
  35.     memcpy(infrared_data+27,&gx,4);
  36.     memcpy(infrared_data+31,&gy,4);
  37.     memcpy(infrared_data+35,&gz,4);
  38.     memcpy(infrared_data+39,&v,4);
  39.     memcpy(infrared_data+43,&check_sum,4);
  41.     //帧尾
  42.     infrared_data[47] = 0x7b;
  43.     //infrared_data[48] = '\n';
  44.     //printf("%s",infrared_data);
  45.     uart5_dma_send(infrared_data,sizeof(infrared_data));
  46. }

鲁班猫定义串口节点接收传感器数据并发送线速度和角速度

  1. import rclpy
  2. from rclpy.node import Node
  3. import serial 
  4. import struct
  5. import time
  6. import threading
  8. class SerialNode(Node):
  9.     def __init__(self,node_name):
  10.         super().__init__(node_name)
  11.         # 创建串口对象
  12.         self.ser = serial.Serial("/dev/ttyS3",115200,timeout = 5)
  13.         self.ser.flushInput()
  15.         self.i = 0
  16.         # 需要发送的头 类型和数据长度
  17.         self.head = struct.pack('<b',0x7b)
  18.         self.instruction = struct.pack('<b',0x00)
  19.         self.length = struct.pack('<b',3)
  20.         self.tail = struct.pack('<b',0x7a)
  22.         self.t1 = threading.Thread(target=self.recv_speed)
  23.         self.t2 = threading.Thread(target=self.send_speed,args=(-10.743,-4.786,9.295))
  24.         # 守护线程 主线程挂掉 子线程也挂掉 需要在start之前设置
  25.         self.t1.setDaemon(True)
  26.         self.t1.setDaemon(True)
  27.         self.t1.start()
  28.         self.t2.start()
  30.     # TODO 发送真实的操作指令
  31.     def send_speed(self,x,y,z):
  32.         while True:
  33.             data = struct.pack('<f',x)+struct.pack('<f',y)+struct.pack('<f',z)
  34.             # 校验和校验
  35.             check_num = 0x00+3+x+y+z
  36.             check = struct.pack('<f',check_num)
  37.             # 需要发送的数据
  38.             data = self.head+self.instruction+self.length+data+check+self.tail
  39.             # 发送串口数据
  40.             self.ser.write(data)
  41.             time.sleep(0.06)
  43.     def recv_speed(self):
  44.         print('start')
  45.         print(f'is_open:{self.ser.is_open}')
  46.         # while True:
  47.         #     print(f'i:{self.i}')
  48.         #     self.i += 1
  49.         #     time.sleep(0.06)
  50.         while True:
  51.             self.i += 1
  52.             # 获取需要接收的数据个数
  53.             num = self.ser.inWaiting()
  54.             # print(f'i:{self.i} start num:{num}')
  55.             # num = self.ser.in_waiting()
  56.             # if num<=0 or num != 48:
  57.             #     continue
  59.             if num<=0 or num<48:
  60.                 continue
  61.             if num>48:
  62.                 # 清除
  63.                 self.ser.read(num)
  64.                 continue
  65.             # 正常的一帧数据
  66.             print(f'num:{num}')
  67.             data = self.ser.read(num)
  68.             # 解析数据
  69.             self.parse_speed(data)
  70.             print('end')
  71.             time.sleep(0.06)
  72.     # TODO 通过获取到的数据进行决策
  73.     def parse_speed(self,data):
  74.         # 帧头
  75.         head = struct.unpack('<B',data[:1])[0]
  76.         if head!=122:
  77.             return
  78.         # 命令
  79.         ins = struct.unpack('<B',data[1:2])[0]
  80.         if ins!=1:
  81.             return
  82.         # 数据长度
  83.         length = struct.unpack('<B',data[2:3])[0]
  84.         if length!=10:
  85.             return
  86.         # 数据
  87.         want_data = struct.unpack('<ffffffffff',data[3:43])
  88.         # 校验位
  89.         check = struct.unpack('<f',data[43:47])[0]
  90.         # 帧尾
  91.         tail = struct.unpack('<B',data[47:])[0]
  92.         # self.get_logger().info(f'i:{self.i},帧头:{head},命令:{ins},数据长度:{length},数据:{want_data},校验位:{check},帧尾:{tail}')
  93.         print(f'i:{self.i},帧头:{head},命令:{ins},数据长度:{length},数据:{want_data},校验位:{check},帧尾:{tail}')
  94.         print(f'i:{self.i} end')
  96. def main():
  97.     # 初始化rclpy
  98.     rclpy.init()
  99.     # 参数:节点名称
  100.     node = SerialNode('serial_node')
  102.     # 节点代码执行完之后需要销毁节点
  103.     node.destroy_node()
  104.     # 关闭rclpy
  105.     rclpy.shutdown()
  107. if __name__ == '__main__':
  108.     main()