(YDLidar G2) Check code (데이터 무결성 검사)

#  YDLidar G2용
# 데이터 무결점 유무를 체크하는 코드
# 각 필드를 XOR 연산하여 cs value와 비교함

import serial
import re
import math

s = serial.Serial('/dev/ttyUSB0', 230400)
s.write(b'\xa5\x60')  # start scanning, output point cloud data

MIN_LENGTH = 13 # 한 패킷의 최소 길이. 헤더 정보 10바이트와 점 한 개 3바이트 해서 총 13바이트에 못 미치면 해당 패킷은 건너뛴다.
header_size = 7 # response 헤드 bytes
cloud_header = 10  # 클라우드 데이터 헤드 bytes
point_size = 3  # 점 하나 데이터 bytes (거리 및 밝기 정보)

while True:
  read_data = s.read(512)

  # parse response header
  idx = read_data.find(b'\xa5\x5a') # search for starting point
  response_mode = read_data[5] >> 6
  #print("response mode(1=continuous): ", response_mode)
  #print("type code : ", hex(read_data[6]))

  indexes = [m.start() for m in re.finditer(b"\xaa\x55", read_data)]
  count = len(indexes)
  len_read = len(read_data)
  #print("indexes: ", indexes)
  #print("len(indexes)", count)
  #print("len_read: ", len_read)

  for i in range(count):
    if i == count - 1: # 마지막 패킷이라면, end 인덱스가 없으므로 끝지점을 직접 지정
      data = read_data[indexes[i] : len_read]
      #print("last packet length : ", len(data))
    else :
      data = read_data[indexes[i] : indexes[i+1]] # header부터 다음 header 직전까지 읽음
    len_data = len(data)
    print("len_data: ", len_data)

    if len_data <= MIN_LENGTH:
      continue

    frequency = (data[2] >> 1) / 10.0  # current frequency
    #print("current frequency: ", frequency)
    packet_type = data[2] & 0b01  # 하위 1비트만 얻음
    #print("current packet type: ", packet_type)
    lsn = data[3] # sample quantity
    print("-------")
    print("lsn(sample quantity): ", lsn)
    
    if lsn != 1: # 한 패킷 안의 샘플링 데이터 갯수. 없으면 lsn=1
      # 앵글 계산 end_angle , start_angle
      fsa1 = data[4]  # LSB : start angle
      fsa2 = data[5]  # MSB : start angle
      lsa1 = data[6]  # LSB : end angle
      lsa2 = data[7]  # MSB : end angle
      
      #--------------------------------
      #  check code로 데이터 무결성 검토
      #  cs_xor_sequence 함수로 뺄 듯? : G2 Deveopment Manual <FIG 8 XOR SEQUENCE> 참조
      #--------------------------------
      # cs (check code) 구하기
      cs = data[8] | (data[9] << 8)
      ph = data[0] | (data[1] << 8)  # Packet header, G2에서는 0x55AA임
      print("ph = ", hex(ph))
      # XOR 연산 시작
      tmp_cs = ph ^ (data[2] | (data[3] << 8))  # (1) ct(f&c) 와 lsn
      tmp_cs = tmp_cs ^ (fsa1 | (fsa2 << 8)) # (2)
      tmp_cs = tmp_cs ^ (lsa1 | (lsa2 << 8)) # (3)
      # 거리 및 밝기 정보와 XOR 연산
      for n in range(0, lsn):
        if 10+3*n+2 >= len_data: # 아래에서 발생할 수 있는 인덱싱 에러 방지. 인덱스가 데이터 길이를 벗어나지 않도록 체크
          print("인덱싱 범위 벗어남")
          break
        tmp_cs ^= data[10+3*n]    # (4)
        tmp_cs ^= (data[10+3*n+1] | data[10+3*n+2] << 8) # (5)
      
      print("check code = ", hex(cs))
      print("tmp_cs = ", hex(tmp_cs))
      if cs == tmp_cs:
        print("데이터에 결함이 없습니다.")
      else:
        print("데이터에 결함이 있습니다.")

      for j in range(0, lsn):
        if 10+3*j+2 >= len_data: # 아래에서 발생할 수 있는 인덱싱 에러 방지. 인덱스가 데이터 길이를 벗어나지 않도록 체크
          break
       
        # Luminous intensity 계산
        # 첫 번째 바이트를 전부 취하고 (최대 256 표현), 두 번째 바이트의 하위 2개 비트에 256을 곱해서(= 왼쪽으로 8번 비트 쉬프트해서), 
        # 이 둘을 더해줌. 2^8은 256이므로, 해당 비트들을 왼쪽으로 8번 쉬프트하는 것이나, 256을 곱하는 것이나 동일함. 
        # 0b11은 이진수 표현. 16진수로 표현하자면 0x03이 됨.
        # 표현 범위: 0~1023
        intensity = data[10+3*j] + (data[10+3*j+1] & 0b11) * 256 # 두 번째 바이트 하위 2비트 얻어서 256을 곱해서 첫번째 바이트와 더함
        #print("Luminous intensity: ", intensity) 
        # 거리 계산
        # 두 번째 바이트 상위 6비트를 밑으로 내리고, 세번째 바이트 비트 전부를 여섯 비트(계단) 올려서 이 둘을 비트 조합(OR 연산)함.
        # mm를 m로 환산 (소수점 살림)
        distance = ((data[10+3*j+1] >> 2) | (data[10+3*j+2] << 6))
        #print("distance: ", distance)  # 단위 mm


s.write(b'\xa5\x65')  # stop motor

s.close()

# 모터 중지 코드를 넣어야 하는데, 종료 뒤에도 모터가 돈다면, 아래 파이썬 스크립트를 실행하면 됨.
'''
#!/bin/python3
# filename: stop
# chmod +x stop 해서 $PATH 걸린 곳에 복사할 것.
import serial

s = serial.Serial('/dev/ttyUSB0', 230400)
s.write(b'\xa5\x65')  # stop motor
s.close()
'''