DSView-decoders/xy2-100-E/pd.py

##
## This file is part of the libsigrokdecode project.
##
## Copyright (C) 2019 Uli Huber
## Copyright (C) 2020 Soeren Apel
##
## This program is free software; you can redistribute it and/or modify
## it under the terms of the GNU General Public License as published by
## the Free Software Foundation; either version 2 of the License, or
## (at your option) any later version.
##
## This program is distributed in the hope that it will be useful,
## but WITHOUT ANY WARRANTY; without even the implied warranty of
## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
## GNU General Public License for more details.
##
## You should have received a copy of the GNU General Public License
## along with this program; if not, see <http://www.gnu.org/licenses/>.
##

import sigrokdecode as srd

ann_bit, ann_bit2, ann_bit3, ann_bit4, ann_stat_bit, ann_type, ann_command, ann_parameter, ann_parity, ann_pos, ann_pos2, ann_pos3, ann_pos4,  ann_status, ann_warning = range(15)
frame_type_none, frame_type_command, frame_type_16bit_pos, frame_type_18bit_pos = range(4)

class Decoder(srd.Decoder):
    api_version = 3
    id = 'xy2-100-E'
    name = 'XY2-100-E'
    longname = 'XY2-100-E(E) and XY-200(E) galvanometer protocol'
    desc = 'Serial protocol for galvanometer positioning in laser systems by chenyue'
    license = 'gplv2+'
    inputs = ['logic']
    outputs = []
    tags = ['Embedded/industrial']

    # 你要的 options
    options = (
        {'id': 'worksize', 'desc': '振镜区域大小', 'default': 60},
        {'id': 'resolution', 'desc': '振镜分辨率', 'default': 65536},
        {'id': 'filename', 'desc': '解码输出文件名','default':'d:/xy2-100.csv'},
    )

    channels = (
        {'id': 'clk', 'name': 'CLK', 'desc': 'Clock'},
        {'id': 'sync', 'name': 'SYNC', 'desc': 'Sync'},
        {'id': 'data', 'name': 'DATA X', 'desc': 'X axis data'},
        {'id': 'data2', 'name': 'DATA Y', 'desc': 'Y axis data'},
        {'id': 'data3', 'name': 'DATA X return', 'desc': 'X axis data return'},
        {'id': 'data4', 'name': 'DATA Y return', 'desc': 'Y axis data return'},
    )
    optional_channels = (
        {'id': 'status', 'name': 'STAT', 'desc': 'X, Y or Z axis status'},
    )

    annotations = (
        ('bit', 'Data Bit X'),
        ('bit2', 'Data Bit Y'),
        ('bit3', 'Data Bit X Return'),
        ('bit4', 'Data Bit Y Return'),
        ('stat_bit', 'Status Bit'),
        ('type', 'Frame Type'),
        ('command', 'Command'),
        ('parameter', 'Parameter'),
        ('parity', 'Parity'),
        ('position', 'Position X'),
        ('position2', 'Position Y'),
        ('position3', 'Position X Return'),
        ('position4', 'Position Y Return'),
        ('status', 'Status'),
        ('warning', 'Human-readable warnings'),
    )

    annotation_rows = (
        ('bits', 'Data Bits X', (ann_bit,)),
        ('bits2', 'Data Bits Y', (ann_bit2,)),
        ('bits3', 'Data Bits X Return', (ann_bit3,)),
        ('bits4', 'Data Bits Y Return', (ann_bit4,)),
        ('stat_bits', 'Status Bits', (ann_stat_bit,)),
        ('data', 'Data', (ann_type, ann_command, ann_parameter, ann_parity)),
        ('positions', 'Positions X', (ann_pos,)),
        ('positions2', 'Positions Y', (ann_pos2,)),
        ('positions3', 'Positions X Return', (ann_pos3,)),
        ('positions4', 'Positions Y Return', (ann_pos4,)),
        ('statuses', 'Statuses', (ann_status,)),
        ('warnings', 'Warnings', (ann_warning,)),
    )

    def __init__(self):
        self.samplerate = None
        self.filename = ""
        self.file = None        
        self.x=0
        self.y=0
        self.x1=0
        self.y1=0
        self.reset()

    def reset(self):
        self.bits = []
        self.bits2 = []
        self.bits3 = []
        self.bits4 = []
        self.stat_bits = []
        self.stat_skip_bit = True        

    def metadata(self, key, value):
        if key == srd.SRD_CONF_SAMPLERATE:
            self.samplerate = value
        self.filename = self.options['filename']

    def start(self):
        self.out_ann = self.register(srd.OUTPUT_ANN)
        # 读取配置参数
        self.worksize = self.options['worksize']
        self.resolution = self.options['resolution']
        self.half_res = self.resolution / 2

    def put_ann(self, ss, es, ann_class, value):
        self.put(ss, es, self.out_ann, [ann_class, value])

    # 计算逻辑坐标
    def calc_logic(self, raw_pos):
        return (raw_pos - self.half_res) * self.worksize / self.resolution

    def process_bit(self, sync, bit_ss, bit_es, bit_value, bit_value2):
        # X 轴
        self.put_ann(bit_ss, bit_es, ann_bit, ['%d' % bit_value])
        self.bits.append((bit_ss, bit_es, bit_value))
        # Y 轴
        self.put_ann(bit_ss, bit_es, ann_bit2, ['%d' % bit_value2])
        self.bits2.append((bit_ss, bit_es, bit_value2))

        if sync == 0:
            self.process_frame(self.bits, ann_pos,  0)
            self.process_frame(self.bits2, ann_pos2,  1)
            self.process_frame(self.bits3, ann_pos3,  2)
            self.process_frame(self.bits4, ann_pos4,  3)
            self.reset()
    def process_bit2(self, sync, bit_ss, bit_es, bit_value3, bit_value4):
        self.put_ann(bit_ss, bit_es, ann_bit3, ['%d' % bit_value3])
        self.bits3.append((bit_ss, bit_es, bit_value3))
        self.put_ann(bit_ss, bit_es, ann_bit4, ['%d' % bit_value4])
        self.bits4.append((bit_ss, bit_es, bit_value4))

    def process_frame(self, bits, pos_ann_id, ch):
        if len(bits) < 20:
            if bits:
                self.put_ann(bits[0][0], bits[-1][1], ann_warning, ['Not enough data bits'])
            return

        parity = 0
        for ss, es, value in bits[:-1]:
            parity ^= value

        par_ss, par_es, par_value = bits[19]
        parity_even = (par_value == parity)
        parity_odd = not parity_even

        type_1_value = bits[0][2]
        type_3_value = (bits[0][2] << 2) | (bits[1][2] << 1) | bits[2][2]

        type = frame_type_none
        parity_status = ['OK'] if parity_even else ['NOK']
        type_ss = bits[0][0]
        type_es = bits[2][1]

        if (type_1_value == 1) and (parity_odd == 1):
            type = frame_type_18bit_pos
            type_es = bits[0][1]
        elif (type_3_value == 1):
            type = frame_type_16bit_pos
            #if not parity_even:
             #   self.put_ann(bits[0][0], bits[-1][1], ann_warning, ['Parity error'])
        elif (type_3_value == 7) and (parity_even == 1):
            type = frame_type_command
        else:        
            type = frame_type_16bit_pos
            # self.put_ann(bits[0][0], bits[-1][1], ann_warning, ['Unknown frame'])
            # return

        if type == frame_type_16bit_pos:
            self.put_ann(type_ss, type_es, ann_type, ['16bit Position'])
        if type == frame_type_18bit_pos:
            self.put_ann(type_ss, type_es, ann_type, ['18bit Position'])
        if type == frame_type_command:
            self.put_ann(type_ss, type_es, ann_type, ['Command'])

        self.put_ann(par_ss, par_es, ann_parity, parity_status)

        pos = 0
        if type == frame_type_16bit_pos:
            count = 15
            for ss, es, value in bits[3:19]:
                pos |= value << count
                count -= 1
        elif type == frame_type_18bit_pos:
            count = 17
            for ss, es, value in bits[3:19]:
                pos |= value << count
                count -= 1
            pos = pos if pos < 131072 else pos - 262144
        if ch == 0:
            self.x=pos
        elif ch == 1:
            self.y=pos
        elif ch == 2:
            if pos>=32768:
                pos=pos-32768
            else :            
                pos=pos+32768
            self.x1=pos
        elif ch == 3:
            if pos>=32768:
                pos=pos-32768
            else :            
                pos=pos+32768
            self.y1=pos
                
            if self.file is not None:
                self.file.write('%d,%d,%d,%d\n' % (self.x, self.y, self.x1, self.y1))

        # 显示原始位置
        if type in (frame_type_16bit_pos, frame_type_18bit_pos):
            self.put_ann(type_es, par_ss, pos_ann_id, ['%d' % pos])        
            
        if type == frame_type_command:
            count = 7
            cmd = 0
            cmd_es = 0
            for ss, es, value in bits[3:11]:
                cmd |= value << count
                count -= 1
                cmd_es = es
            self.put_ann(type_es, cmd_es, ann_command, ['Cmd 0x%X' % cmd])

            count = 7
            param = 0
            for ss, es, value in bits[11:19]:
                param |= value << count
                count -= 1
            self.put_ann(cmd_es, par_ss, ann_parameter, ['Param 0x%X' % param])

    def process_stat_bit(self, sync, bit_ss, bit_es, bit_value):
        if self.stat_skip_bit:
            self.stat_skip_bit = False
            return

        self.put_ann(bit_ss, bit_es, ann_stat_bit, ['%d' % bit_value])
        self.stat_bits.append((bit_ss, bit_es, bit_value))

        if (sync == 0) and (len(self.stat_bits) == 19):
            stat_ss = self.stat_bits[0][0]
            stat_es = self.stat_bits[18][1]
            status = 0
            count = 18
            for ss, es, value in self.stat_bits:
                status |= value << count
                count -= 1
            self.put_ann(stat_ss, stat_es, ann_status, ['Status 0x%X' % status])

    def decode(self):
        try:
            self.decode_impl()
        finally:
            if self.file is not None:
                self.file.close()
                self.file = None

    def decode_impl(self):
        if self.filename != "":
            try:
                self.file = open(self.filename, 'w')
                self.file.write('x,y,x1,y1\n')
            except OSError as e:
                print('xy2-100-E: cannot open %s: %s' % (self.filename, e))
                self.file = None
        bit_ss = None
        bit2_ss = None
        bit_value = 0
        bit_value2 = 0
        stat_ss = None
        stat_value = 0
        sync_value = 0
        has_stat = self.has_channel(6)

        while True:
            clk, sync, data, data2, data3, data4, stat = self.wait({0: 'e'})

            if clk == 1:# rising edge                

                stat_value = stat
                bit_es = self.samplenum
                if bit_ss is not None:
                    self.process_bit(sync_value, bit_ss, bit_es, bit_value, bit_value2)
                bit_ss = self.samplenum
            else:# falling edge
                bit_value = data
                bit_value2 = data2
                bit_value3 = data3
                bit_value4 = data4
                sync_value = sync

                bit2_es = self.samplenum
                if bit2_ss is not None:
                    self.process_bit2(sync_value, bit2_ss, bit2_es, bit_value3, bit_value4)
                bit2_ss = self.samplenum

                if stat_ss is not None and has_stat:
                    stat_es = self.samplenum
                    self.process_stat_bit(sync_value, stat_ss, stat_es, stat_value)
                stat_ss = self.samplenum