00001
00002 import sys, importlib
00003
00004 if len(sys.argv) < 2:
00005 print("please provide mapping modulde as first argument, e.g schunk_mapping or elmo_mapping");
00006 exit()
00007
00008 PDOs = importlib.import_module(sys.argv[1]).PDOs
00009
00010 def hex_reverse(data):
00011 return ''.join(reversed([data[i:i+2] for i in xrange(0, len(data), 2)]))
00012
00013 def decode_state(id,data):
00014 state = int(data[0:2],16)
00015
00016 if state & 128:
00017 toggle =" T=1"
00018 state ^=128
00019 else:
00020 toggle = "T=0"
00021 if state == 0:
00022 state = 'boot-up'
00023 elif state == 4:
00024 state = 'stopped'
00025 elif state == 5:
00026 state = 'operational'
00027 elif state == 127:
00028 state = 'pre-op'
00029 elif state == 130:
00030 state = 'recom'
00031
00032 return['State', id - 1792, state, toggle]
00033
00034 def decode_nmt(id,data):
00035 command = int(data[0:2],16)
00036 if command == 1:
00037 command = 'start'
00038 elif command == 2:
00039 command = 'stop'
00040 elif command == 128:
00041 command = 'pre'
00042 elif command == 129:
00043 command = 'reset'
00044 elif command == 130:
00045 command = 'recom'
00046
00047 node = int(data[2:4],16)
00048 if node == 0:
00049 node ="all"
00050 return['NMT', command, node]
00051
00052 def decode_sync(id,data):
00053 if len(data):
00054 return ["SYNC", data]
00055 else:
00056 return ["SYNC"]
00057
00058 def decode_pdo(id,data, name, start_id):
00059 out = []
00060
00061 i = 0
00062 for d in PDOs[name][1:]:
00063 out += [d[1],hex_reverse(data[i:i+2*d[2] ])]
00064 i +=2*d[2]
00065 return [name, id-start_id]+out
00066
00067 def decode_sdo(id,data, name, start_id):
00068 command = int(data[0:2],16) >> 5
00069 out = [name, id-start_id]
00070 if command == 1 or command == 2 or command == 3:
00071 out += [hex_reverse(data[2:6]), data[6:8], hex_reverse(data[8:])]
00072 return out
00073
00074 def decode_emcy(id,data):
00075 return [ "EMCY", id - 0x80, "EEC:", hex_reverse(data[0:4]), "Reg:", data[4:6],"Msef: ",data[6:16]]
00076 pass
00077
00078 def decode_canopen(id,data):
00079 if id == 0:
00080 return decode_nmt(id,data)
00081 elif id > 1792 and id <= 1792 + 127:
00082 return decode_state(id,data)
00083 elif id == 0x80:
00084 return decode_sync(id,data)
00085 elif id > 0x80 and id <= 0x80 + 127:
00086 return decode_emcy(id,data)
00087
00088 elif id > 0x180 and id <= 0x180 + 127:
00089 return decode_pdo(id,data,"TPDO1", 0x180)
00090 elif id > 0x200 and id <= 0x200 + 127:
00091 return decode_pdo(id,data,"RPDO1", 0x200)
00092
00093 elif id > 0x280 and id <= 0x280 + 127:
00094 return decode_pdo(id,data,"TPDO2", 0x280)
00095 elif id > 0x300 and id <= 0x300 + 127:
00096 return decode_pdo(id,data,"RPDO2", 0x300)
00097
00098 elif id > 0x380 and id <= 0x380 + 127:
00099 return decode_pdo(id,data,"TPDO3", 0x380)
00100 elif id > 0x400 and id <= 0x400 + 127:
00101 return decode_pdo(id,data,"RPDO3", 0x400)
00102
00103 elif id > 0x480 and id <= 0x480 + 127:
00104 return decode_pdo(id,data,"TPDO4", 0x480)
00105 elif id > 0x500 and id <= 0x500 + 127:
00106 return decode_pdo(id,data,"RPDO4", 0x500)
00107
00108 elif id > 0x580 and id <= 0x580 + 127:
00109 return decode_sdo(id,data,"TSDO", 0x580)
00110 elif id > 0x600 and id <= 0x600 + 127:
00111 return decode_sdo(id,data,"RSDO", 0x600)
00112 return [id,data]
00113
00114 start = -1
00115 for line in sys.stdin:
00116 parts = line.split()
00117 if start < 0:
00118 for i in range(len(parts)):
00119 if 'can' in parts[i]:
00120 start = i
00121 break
00122 if '#' in parts[start+1]:
00123 otherparts = parts[start+1].split("#")
00124 id = int(otherparts[0],16)
00125 data = otherparts[1]
00126 else:
00127 id = int(parts[start+1],16)
00128 data = ''.join(parts[start+3:])
00129 print parts[:start+1] + decode_canopen(id,data)