test_matek_3901_10x.py

import serial
import struct

def crc8_dvb_s2(crc, ch):
    """CRC for MSPV2
    *copied from inav-configurator
    """
    crc ^= ch
    for _ in range(8):
        if (crc & 0x80):
            crc = ((crc << 1) & 0xFF) ^ 0xD5
        else:
            crc = (crc << 1) & 0xFF
    return crc
    
ser = serial.Serial('/dev/ttyUSB0',115200,timeout=1)  # open serial port


MSP_MSG_TYPES= {
    "0x1f01": "MSP2_SENSOR_RANGEFINDER",
    "0x1f02": "MSP2_SENSOR_OPTIC_FLOW",
    "0x1f03": "MSP2_SENSOR_GPS",
    "0x1f04": "MSP2_SENSOR_COMPASS",
    "0x1f05": "MSP2_SENSOR_BAROMETER",
    "0x1f06": "MSP2_SENSOR_AIRSPEED" } 

for i in range(1000):
    ser.flush()
    header_str = 'ccccHH'
    header_len = struct.calcsize(header_str)
    header_raw = ser.read(header_len)
    if (len(header_raw) == header_len):
        msp_c1,msp_c2,msp_c3,msp_flag,msp_function,msp_payload_size = struct.unpack(header_str, header_raw)
        if (msp_c1==b'$' and msp_c2==b'X' and msp_c3==b'<'):
            payload = ser.read(msp_payload_size+1)
            checksum = payload[-1]
            payload = payload[:-1]
            #compute crc
            crc = 0
            crc_data = [msp_flag,msp_function,msp_payload_size]+list(payload)

            # for c in crc_data:
            #     crc = crc8_dvb_s2(crc, int(c))
            type_name= MSP_MSG_TYPES[hex(msp_function)]
            # if(type_name=="MSP2_SENSOR_RANGEFINDER"):
            #     quality, reading = struct.unpack('=BI', payload)
            #     print ("MSP2_SENSOR_RANGEFINDER : %d %d"%(quality,reading))
            if(type_name=="MSP2_SENSOR_OPTIC_FLOW"):
                quality, reading_X,reading_Y = struct.unpack('=Bii', payload)
                print ("MSP2_SENSOR_RANGEFINDER : %d %d %d"%(quality,reading_X,reading_Y))
                
        #print (type_name, msp_payload_size, payload, checksum)
    #print (header_raw)