def __init__(self):
self.cnt = 0
## close can0
os.system('sudo ifconfig can0 down')
## set bitrate of can0
os.system('sudo ip link set can0 type can bitrate 500000')
## open can0
os.system('sudo ifconfig can0 up')
# os.system('sudo /sbin/ip link set can0 up type can bitrate 250000')
## show details can0 for debug.
# os.system('sudo ip -details link show can0')
if 0:
## set up CAN Bus of J1939
self.bus = j1939.Bus(channel='can0', bustype='socketcan_native')
# set up Notifier
self.notifier = can.Notifier(self.bus, [self.msg_handler])
else:
# set up can interface.
self.can0 = can.interface.Bus(channel = 'can0', bustype = 'socketcan_ctypes')# socketcan_native socketcan_ctypes
## set up Notifier
self.notifier = can.Notifier(self.can0, [self.msg_handler])
self.can_parser = CANParser()
self.lines = 0
def parse_payload(self, msg):
'''
Parse CAN msg to get gyro/accel/tilt/velocity data.
in: CAN msg
'''
can_parser = CANParser()
(_Priority, _PGN, _PF, _PS,
_SA) = can_parser.parse_PDU(msg.arbitration_id)
data = None
str = '{0:f},{1},{2},'.format(msg.timestamp, hex(msg.arbitration_id),
_PGN)
if _PGN == PGNType.SSI2.value: # Slope Sensor Information 2
data = can_parser.parse_tilt(msg.data)
str += ','.join('{0:f}'.format(i) for i in data) + '\n'
self.log_file_tilt.write(str)
self.log_file_tilt.flush()
elif _PGN == PGNType.ARI.value: # Angular Rate
data = can_parser.parse_gyro(msg.data)
str += ','.join('{0:f}'.format(i) for i in data) + '\n'
self.log_file_gyro.write(str)
self.log_file_gyro.flush()
elif _PGN == PGNType.ACCS.value: # Acceleration Sensor
data = can_parser.parse_accel(msg.data)
str += ','.join('{0:f}'.format(i) for i in data) + '\n'
self.log_file_accel.write(str)
self.log_file_accel.flush()
elif _PGN == PGNType.CCVS1.value: # Cruise Control/Vehicle Speed 1
data = can_parser.parse_velocity1(msg.data)
str += ','.join('{0:f}'.format(i) for i in data) + '\n'
self.log_file_vel1.write(str)
self.log_file_vel1.flush()
elif _PGN == PGNType.WSI.value: # Wheel Speed Information
data = can_parser.parse_velocity2(msg.data)
str += ','.join('{0:f}'.format(i) for i in data) + '\n'
self.log_file_vel2.write(str)
self.log_file_vel2.flush()
elif _PGN == PGNType.GEAR.value:
data = can_parser.parse_gear(msg.data)
str += ','.join('{0:f}'.format(i) for i in data) + '\n'
self.log_dir_gear.write(str)
self.log_dir_gear.flush()
pass
elif _PGN == PGNType.TACHOGRAPH.value:
data = can_parser.parse_tachograph(msg.data)
str += ','.join('{0:f}'.format(i) for i in data) + '\n'
self.log_dir_tacho.write(str)
self.log_dir_tacho.flush()
pass
else: # unknown PGN msg
pass
if data is not None:
self.log_file_all.write(str)
self.log_file_all.flush()
pass
def read(self, ):
can_parser = CANParser()
with open(self.file, 'r', encoding='utf-8') as f:
idx = 0
line = f.readline()
while line:
try:
idx += 1
# ignore the first line header.
if idx == 1:
line = f.readline()
continue
item = line.split(',')
# get system time.
sys_time = item[2]
sys_time = sys_time.split(':')
second = sys_time[2].split('.')
sys_time = float(sys_time[0]) * 3600 + float(
sys_time[1]) * 60 + float(second[0]) + float(
second[1]) / 1e3 + float(second[2]) / 1e6
# get frame id
id = item[3]
id = int(id, 16)
# # get msg
msg = item[7].split(' ')[0:-1]
msg = [int(p, 16) for p in msg]
data = None
s = '{0},{1},{2},'.format(item[2], sys_time, id)
if id == 0XAA: # 170
data = can_parser.parse_wheel_speed_carola(msg)
s += ','.join('{0:f}'.format(i) for i in data)
s += ',' + str(self.gear) + '\n'
self.log_file_vel.write(s)
self.log_file_vel.flush()
# print(speed_fr, speed_fl, speed_rr, speed_rl)
elif id == 0X3BC: # 956
data = can_parser.parse_gear_carola(msg)
s += ','.join('{0:d}'.format(i) for i in data) + '\n'
self.log_dir_gear.write(s)
self.log_dir_gear.flush()
self.gear = data[0]
# print(data)
else: # unknown id
pass
if data is not None:
self.log_file_all.write(s)
self.log_file_all.flush()
pass
line = f.readline()
except Exception as e:
print('Error at line {0} :{1}'.format(idx, e))
def get_gyro_data(self, msg):
'''
Parse gyro data on CAN bus.
'''
# if msg.arbitration_id != 0X0CF02A80:
# return
can_parser = CANParser()
(_Priority, _PGN, _PF, _PS,
_SA) = can_parser.parse_PDU(msg.arbitration_id)
if _PGN == PGNType.ARI.value: # Angular Rate
data = can_parser.parse_gyro(msg.data)
# self.w = np.array(data, dtype = np.float32)
# Note the order of rate data is YXZ
self.w = np.array([data[1], data[0], data[2]], dtype=np.float32)
# print(self.w)
return