matek_optflow.py

from signal import signal, SIGINT
from sys import exit
from enum import Enum
import serial

PORT_NAME = '/dev/tty.usbserial-00001014'

'''
                        quality is not used
                        sensor data = distance / 10
                        Q  Distance (negative value for out of range)
S  M  A     CMD   SIZE  5              CKSUM
24 58 3c 00 01 1f 05 00 ff ff ff ff ff b1

                        quality = Q * 100 / 255
                        motionY : sensorData.flowRateRAw[0] = X sensorData.flowRateRAw[1] = Y sensorData.flowRateRAw[2] = 0
                        Q  motionX
S  M  A     CMD   SIZE  9                          CKSUM
24 58 3c 00 02 1f 09 00 9e 02 00 00 00 00 00 00 00 39
'''
class MSP_State(Enum):
    IDLE = 0
    HEADER_START = 1       # 8bit
    HEADER_M = 2           # 8bit
    HEADER_ARROW = 3       # 8bit
    HEADER_CMD = 4         # New MSP 16bit
    HEADER_SIZE = 5        # New MSP 16bit (CMD <-> Size swapped)

ser = serial.Serial(PORT_NAME, 115200)

def handler(signal_received, frame):
    # Handle any cleanup here
    ser.close()
    # outfile.close()
    print('bye')
    exit(0)

signal(SIGINT, handler)

state = MSP_State.IDLE
index = 0

while(True):
    s = ser.read(1)[0]
    if state == MSP_State.IDLE :
        if s == 0x24 :
            state = MSP_State.HEADER_START
    elif state == MSP_State.HEADER_START :
        if s == 0x58 :
            state = MSP_State.HEADER_M
        else :
            state = MSP_State.IDLE
    elif state == MSP_State.HEADER_M :
        if s == 0x3C :
            state = MSP_State.HEADER_ARROW
        else :
            state = MSP_State.IDLE
    elif state == MSP_State.HEADER_ARROW :
        if s != 0x0 :
            state = MSP_State.IDLE
            continue

        low  = ser.read(1)[0]
        high = ser.read(1)[0]
        cmd  = high << 8 | low
        low  = ser.read(1)[0]
        high = ser.read(1)[0]
        size = high << 8 | low

        if cmd == 0x1f01 and size == 5 :
            quality = ser.read(1)[0]
            b0 = ser.read(1)[0]
            b1 = ser.read(1)[0]
            b2 = ser.read(1)[0]
            b3 = ser.read(1)[0]
            distance = b3 << 24 | b2 << 16 | b1 << 8 | b0
            # print(index, hex(cmd), hex(size), quality, distance)
            index += 1

        if cmd == 0x1f02 and size == 9 :
            quality = ser.read(1)[0]
            b0 = ser.read(1)[0]
            b1 = ser.read(1)[0]
            b2 = ser.read(1)[0]
            b3 = ser.read(1)[0]
            motionX = b3 << 24 | b2 << 16 | b1 << 8 | b0
            if (motionX & 0x80000000):
                motionX -= 2**32
            b0 = ser.read(1)[0]
            b1 = ser.read(1)[0]
            b2 = ser.read(1)[0]
            b3 = ser.read(1)[0]
            motionY = b3 << 24 | b2 << 16 | b1 << 8 | b0
            if (motionY & 0x80000000):
                motionY -= 2**32
            # print(index, hex(cmd), hex(size), quality, motionX, motionY)
            print("%3d %3d %3d" %(quality, motionX, motionY))
            index += 1