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 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 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 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
class CanSender:
'''
Continuously and periodically send CAN velocity and gear message for testing MTLT335.
'''
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 msg_handler(self, msg):
return
def send_Carola_CAN_msg(self):
'''
1. Sending fake velocity and gear message of Carola.
2. Message rate of velocity messate is 100Hz, fake data: [1, 2, ... 250, 1, 2, ... 250 ...] km/hr
3. Message rate of gear messate is 10Hz, fake data: [D, R, N, P, D ...]
'''
speed_id = 0XAA
gear_id = 0X3BC
msg_rate = 100 # 100 Hz
send_data_circle = 100 #
gear_data = [0] * 8
self.lines = 0
offset = -67.67 # km/hr
idx = 0
for t in range(send_data_circle):
for s in range(1, 251): # speed range: [1, 250] km/hr
# Construct wheel speed CAN message.
wheel_raw_speed = (s - offset)/0.01
wheel_data = [int(wheel_raw_speed/256), int(wheel_raw_speed%256)] * 4
# Send wheel speed CAN message.
m = can.Message(arbitration_id = speed_id, data = wheel_data, extended_id = False)
self.can0.send(m)
if 0 == idx%10: # Sending gear msg at 10 Hz.
# Construct Gear CAN message.
if 0 == t%4:
gear_data[1] = 0 # Gear: D
elif 1 == t%4:
gear_data[1] = 16 # Gear: R
elif 2 == t%4:
gear_data[1] = 8 # Gear: N
elif 3 == t%4:
gear_data[1] = 32 # Gear: P
else:
gear_data[1] = 0 # Gear: D
# Send Gear CAN message.
m = can.Message(arbitration_id = gear_id, data = gear_data, extended_id = False)
self.can0.send(m)
idx += 1
time.sleep(1/msg_rate) #
##
self.lines += 1
if self.lines % 1000 == 0:
print("[{0}]:Sending wheel speed msg counter: {1}".format(datetime.datetime.now().strftime('%Y-%m-%d %H:%M:%S'), self.lines))
sys.stdout.flush()
pass
def send_pgn65215_msg(self):
'''
1. Sending PGN65215 wheel speed and PGN61445 gear CAN message.
2. Message rate of PGN65215 messate is 10Hz, fake data: [1, 2, ... 250, 1, 2, ... 250 ...] km/hr
3. Message rate of PGN61445 messate is 10Hz, fake data: [1, -1, 2, 3, 4, 5, 1, 1 ...]
'''
msg_rate = 100 # 10 Hz
send_data_circle = 1000 #
gear_data = [0] * 8
self.lines = 0
speed_id = self.can_parser.generate_PDU(Priority = 6, PGN = 65215, SA = 0X88)
gear_id = self.can_parser.generate_PDU(Priority = 6, PGN = 61445, SA = 0X88)
for t in range(send_data_circle):
for s in range(1, 251): # speed range: [1, 250] km/hr
# Construct wheel speed CAN message, assume all relative speed are zero.
offset = -7.8125 # km/hr
front_axle_speed = s
front_left_wheel_speed = int((0 - offset) * 16 + 0.5)
front_right_wheel_speed = front_left_wheel_speed
rear_left1_wheel_speed = front_left_wheel_speed
rear_right1_wheel_speed = front_left_wheel_speed
rear_left2_wheel_speed = rear_left1_wheel_speed
rear_right2_wheel_speed = rear_right1_wheel_speed
front_axle_speed = int(front_axle_speed * 256 + 0.5)
wheel_data = []
wheel_data.append(front_axle_speed % 256)
wheel_data.append(int(front_axle_speed/256))
wheel_data.append(front_left_wheel_speed)
wheel_data.append(front_right_wheel_speed)
wheel_data.append(rear_left1_wheel_speed)
wheel_data.append(rear_right1_wheel_speed)
wheel_data.append(rear_left2_wheel_speed)
wheel_data.append(rear_right2_wheel_speed)
# Send wheel speed CAN message.
m = can.Message(arbitration_id = speed_id, data = wheel_data, extended_id = True)
self.can0.send(m)
# arbitration_id = j1939.ArbitrationID(priority=6, pgn=65215)
# m = j1939.PDU(arbitration_id=arbitration_id, data=wheel_data)
# self.bus.send(m)
# Construct Gear CAN message.
offset = -125
if 0 == t:
gear_data[3] = 1 # Gear: 1
gear_data[3] -= offset
elif 1 == t:
gear_data[3] = -1 # Gear: -1
gear_data[3] -= offset
elif 2 == t:
gear_data[3] = 2 # Gear: 2
gear_data[3] -= offset
elif 3 == t:
gear_data[3] = 3 # Gear: 3
gear_data[3] -= offset
elif 4 == t:
gear_data[3] = 4 # Gear: 4
gear_data[3] -= offset
elif 5 == t:
gear_data[3] = 5 # Gear: 5
gear_data[3] -= offset
elif 6 == t:
gear_data[3] = 251 # Gear: 251 0XFB, Park
else:
gear_data[3] = 1 # Gear: 1
gear_data[3] -= offset
# Send Gear CAN message.
m = can.Message(arbitration_id = gear_id, data = gear_data, extended_id = True)
self.can0.send(m)
# arbitration_id = j1939.ArbitrationID(priority=6, pgn=61445)
# m = j1939.PDU(arbitration_id=arbitration_id, data=gear_data)
# self.bus.send(m)
time.sleep(1/msg_rate) #
##
self.lines += 1
if self.lines % 1000 == 0:
print("[{0}]:Sending wheel speed msg counter: {1}".format(datetime.datetime.now().strftime('%Y-%m-%d %H:%M:%S'), self.lines))
sys.stdout.flush()
pass
def send_pgn65265_msg(self):
'''
1. Sending PGN65265 wheel speed and PGN61445 gear CAN message.
2. Message rate of PGN65265 messate is 10Hz, fake data: [1, 2, ... 250, 1, 2, ... 250 ...] km/hr
3. Message rate of PGN61445 messate is 10Hz, fake data: [1, -1, 2, 3, 4, 5, 1, 1 ...]
'''
msg_rate = 100 # 10 Hz
send_data_circle = 1000 #
gear_data = [0] * 8
wheel_data = [0] * 8
self.lines = 0
speed_id = self.can_parser.generate_PDU(Priority = 6, PGN = 65265, SA = 0X88)
gear_id = self.can_parser.generate_PDU(Priority = 6, PGN = 61445, SA = 0X88)
for t in range(send_data_circle):
for s in range(1, 251): # speed range: [1, 250] km/hr
# Construct wheel speed CAN message.
speed = int(s * 256 + 0.5) # km/hr
wheel_data[1] = speed%256
wheel_data[2] = int(speed/256)
# Send wheel speed CAN message.
m = can.Message(arbitration_id = speed_id, data = wheel_data, extended_id = True)
self.can0.send(m)
# Construct Gear CAN message.
offset = -125
if 0 == t:
gear_data[3] = 1 # Gear: 1
gear_data[3] -= offset
elif 1 == t:
gear_data[3] = -1 # Gear: -1
gear_data[3] -= offset
elif 2 == t:
gear_data[3] = 2 # Gear: 2
gear_data[3] -= offset
elif 3 == t:
gear_data[3] = 3 # Gear: 3
gear_data[3] -= offset
elif 4 == t:
gear_data[3] = 4 # Gear: 4
gear_data[3] -= offset
elif 5 == t:
gear_data[3] = 5 # Gear: 5
gear_data[3] -= offset
elif 6 == t:
gear_data[3] = 251 # Gear: 251 0XFB, Park
else:
gear_data[3] = 1 # Gear: 1
gear_data[3] -= offset
# Send Gear CAN message.
m = can.Message(arbitration_id = gear_id, data = gear_data, extended_id = True)
self.can0.send(m)
time.sleep(1/msg_rate) #
##
self.lines += 1
if self.lines % 1000 == 0:
print("[{0}]:Sending wheel speed msg counter: {1}".format(datetime.datetime.now().strftime('%Y-%m-%d %H:%M:%S'), self.lines))
sys.stdout.flush()
pass
def send_pgn126720_msg(self):
'''
1. Sending PGN126720 JD accelerations message.
2. Message rate of PGN126720 messate is 100Hz, fake data: [-320, -319, ... 321, 322, -320, -319, ... 321, 322 ...] m/s^2
3. Note:
. JD Accelerations data order in payload : [acc_Y, acc_X, acc_Z]
. JD Accelerations is in NWU frame but not NED.
'''
msg_rate = 100 # 100 Hz
send_data_circle = 100 #
acc_data = [0] * 8
self.lines = 0
acc_id = self.can_parser.generate_PDU(Priority = 6, PGN = 126720, SA = 0X88)
for t in range(send_data_circle):
for s in range(-320, 323): # range: [-320, 322] m/s^2
# Construct JD acceleration CAN message.
acc = (s + 320) * 100 # m/s^2
# Accel Order in payload : [acc_Y, acc_X, acc_Z]
# acc_data[1] = acc%256
# acc_data[2] = int(acc/256)
acc_data[1] = acc & 0XFF
acc_data[2] = acc >> 8
acc_data[3] = acc_data[1]
acc_data[4] = acc_data[2]
acc_data[5] = acc_data[1]
acc_data[6] = acc_data[2]
# Send JD acceleration CAN message.
m = can.Message(arbitration_id = acc_id, data = acc_data, extended_id = True)
self.can0.send(m)
# print(s, (acc_data[2] * 256 + acc_data[1]) * 0.01 - 320)
time.sleep(1/msg_rate) #
##
self.lines += 1
if self.lines % 1000 == 0:
print("[{0}]:Sending JD accel msg counter: {1}".format(datetime.datetime.now().strftime('%Y-%m-%d %H:%M:%S'), self.lines))
sys.stdout.flush()
pass
class CANReceiver:
'''
Receive and parse standard J1939 CAN message.
'''
def __init__(self):
## 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.create_log_files()
self.can_parser = CANParser()
self.last_speed_65215 = 0
self.last_gear = 0
self.idx = 0
def msg_handler(self, msg):
self.idx += 1
(_Priority, _PGN, _PF, _PS,
_SA) = self.can_parser.parse_PDU(msg.arbitration_id)
if msg.arbitration_id == CarolaCANID.WS.value: # Carola wheel speed.
self.handle_Carola_wheel_speed(msg)
pass
elif msg.arbitration_id == CarolaCANID.GEAR.value: # Carola Gear message.
self.handle_Carola_gear(msg)
pass
elif _PGN == PGNType.WSI.value: # PGN 65215 Wheel speed message.
self.handle_J1939_WSI(msg)
pass
elif _PGN == PGNType.CCVS1.value: # PGN 65265 Wheel speed message.
self.handle_J1939_CCVS1(msg)
pass
elif _PGN == PGNType.GEAR.value: # PGN 61445 Gear message.
self.handle_J1939_gear(msg)
pass
else:
pass
pass
def handle_Carola_wheel_speed(self, msg): # Carola wheel speed.
'''
'''
str = '{0:f},'.format(msg.timestamp)
data = self.can_parser.parse_wheel_speed_carola(msg.data)
str += ','.join('{0:f}'.format(i) for i in data) + '\n'
self.log_file_carola_vel.write(str)
self.log_file_carola_vel.flush()
# Test
(speed_fr, speed_fl, speed_rr, speed_rl) = data
pass
def handle_Carola_gear(self, msg): # Carola Gear message.
'''
'''
str = '{0:f},'.format(msg.timestamp)
data = self.can_parser.parse_gear_carola(msg.data)
str += ','.join('{0:f}'.format(i) for i in data) + '\n'
self.log_file_carola_gear.write(str)
self.log_file_carola_gear.flush()
# Test
(gear, ) = data
pass
def handle_J1939_WSI(self, msg): # PGN 65215
'''
'''
str = '{0:f},'.format(msg.timestamp)
data = self.can_parser.parse_velocity2(msg.data)
str += ','.join('{0:f}'.format(i) for i in data) + '\n'
self.log_file_65215_vel.write(str)
self.log_file_65215_vel.flush()
# Test
(front_axle_speed, front_left_wheel_speed, front_right_wheel_speed, \
rear_left1_wheel_speed, rear_right1_wheel_speed, \
rear_left2_wheel_speed, rear_right2_wheel_speed) = data
if math.fabs(rear_left1_wheel_speed - self.last_speed_65215) > 1.0001:
print(rear_left1_wheel_speed - self.last_speed_65215)
self.last_speed_65215 = rear_left1_wheel_speed
pass
def handle_J1939_CCVS1(self, msg): # PGN 65265
'''
'''
str = '{0:f},'.format(msg.timestamp)
data = self.can_parser.parse_velocity1(msg.data)
str += ','.join('{0:f}'.format(i) for i in data) + '\n'
self.log_file_65265_vel.write(str)
self.log_file_65265_vel.flush()
pass
def handle_J1939_gear(self, msg): # PGN 61445
'''
'''
str = '{0:f},'.format(msg.timestamp)
data = self.can_parser.parse_gear(msg.data)
str += ','.join('{0:f}'.format(i) for i in data) + '\n'
self.log_file_61445_gear.write(str)
self.log_file_61445_gear.flush()
pass
def create_log_files(self):
'''
create log files.
'''
if not os.path.exists('data/'):
os.mkdir('data/')
start_time = datetime.datetime.now().strftime('%Y%m%d_%H%M%S')
# file_dir = os.path.join('data', start_time + '.csv')
file_dir_carola_vel = os.path.join('data',
start_time + '_carola_vel' + '.csv')
file_dir_carola_gear = os.path.join(
'data', start_time + '_carola_gear' + '.csv')
file_dir_65215_vel = os.path.join('data',
start_time + '_65215_vel' + '.csv')
file_dir_65265_vel = os.path.join('data',
start_time + '_65265_vel' + '.csv')
file_dir_61445_gear = os.path.join('data',
start_time + '_61445_gear' + '.csv')
# self.log_file_all = open(file_dir, 'w')
self.log_file_carola_vel = open(file_dir_carola_vel, 'w')
self.log_file_carola_gear = open(file_dir_carola_gear, 'w')
self.log_file_65215_vel = open(file_dir_65215_vel, 'w')
self.log_file_65265_vel = open(file_dir_65265_vel, 'w')
self.log_file_61445_gear = open(file_dir_61445_gear, 'w')
# print('Start logging: {0}'.format(file_dir))
# header = 'time, payload'.replace(' ', '')
# self.log_file_all.write(header + '\n')
# self.log_file_all.flush()
header = 'time, speed_fr, speed_fl, speed_rr, speed_rl, gear'.replace(
' ', '')
self.log_file_carola_vel.write(header + '\n')
self.log_file_carola_vel.flush()
header = 'time, gear'.replace(' ', '')
self.log_file_carola_gear.write(header + '\n')
self.log_file_carola_gear.flush()
header = 'time, speed_fa, speed_fl, speed_fr, speed_rl1, speed_rr1, speed_rl2, speed_rr2, gear'.replace(
' ', '')
self.log_file_65215_vel.write(header + '\n')
self.log_file_65215_vel.flush()
header = 'time, speed, gear'.replace(' ', '')
self.log_file_65265_vel.write(header + '\n')
self.log_file_65265_vel.flush()
header = 'time, gear'.replace(' ', '')
self.log_file_61445_gear.write(header + '\n')
self.log_file_61445_gear.flush()
pass
def close_log_files(self):
# self.log_file_all.close()
self.log_file_carola_vel.close()
self.log_file_carola_gear.close()
self.log_file_65215_vel.close()
self.log_file_65265_vel.close()
self.log_file_61445_gear.close()
class CarolaDriver:
'''
Parse Carola CAN message.
Convert velocity and gear message to standard J1939 CAN message.
'''
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.create_log_files()
self.lines = 0
self.gear = 0
self.last_speed = 0
self.last_v = np.array([0, 0, 0], dtype=np.float32)
self.w = np.array([0, 0, 0], dtype=np.float32)
def msg_handler(self, msg):
return
def convert_speed_msg(self, msg):
'''
Convert 100Hz Carola speed message to J1939 Odometer/Accel message.
'''
if msg.arbitration_id != 0XAA:
return
self.cnt += 1
# if self.cnt % 10 != 0: # 100Hz -> 10Hz
# return
# time = datetime.datetime.fromtimestamp(msg.timestamp)
# print(time)
# parse wheels speed from msg.
(speed_fr, speed_fl, speed_rr,
speed_rl) = self.can_parser.parse_wheel_speed_carola(msg.data)
# print("speed:", speed_fr, speed_fl, speed_rr, speed_rl)
# Test
#if math.fabs(speed_fl - self.last_speed) > 1.0001:
# print(speed_fl - self.last_speed)
#self.last_speed = speed_fl
#return
#### Send PGN-65215 msg
self.convert_pgn65215(speed_fr, speed_fl, speed_rr, speed_rl)
#### Send PGN-65265 msg
self.convert_pgn65265(speed_rr, speed_rl)
#### Send PGN-126720 msg
self.convert_pgn126720(speed_rr, speed_rl)
#### save to log files.
s = datetime.datetime.now().strftime('%H:%M:%S.%f')
s += ',' + str(msg.arbitration_id)
s += ','.join('{0:f}'.format(i)
for i in (speed_fr, speed_fl, speed_rr, speed_rl))
s += ',' + str(self.gear) + '\n'
self.log_file_vel.write(s)
self.log_file_vel.flush()
self.log_file_all.write(s)
self.log_file_all.flush()
##
self.lines += 1
if self.lines % 100 == 0:
print("[{0}]:Log counter of: {1}".format(
datetime.datetime.now().strftime('%Y-%m-%d %H:%M:%S'),
self.lines))
sys.stdout.flush()
def convert_pgn65215(self, speed_fr, speed_fl, speed_rr, speed_rl):
'''
Convert Corolla speed to PGN65215 CAN speed.
'''
### construct PGN-65215 id and data
# SA CAN NOT be 0X80 which equal to MTLT's address.
id = self.can_parser.generate_PDU(Priority=6, PGN=65215, SA=0X88)
offset = -7.8125 #km/h
front_axle_speed = (speed_fr + speed_fl) / 2
front_left_wheel_speed = int((speed_fl - front_axle_speed - offset) *
16 + 0.5)
front_right_wheel_speed = int((speed_fr - front_axle_speed - offset) *
16 + 0.5)
rear_left1_wheel_speed = int((speed_rl - front_axle_speed - offset) *
16 + 0.5)
rear_right1_wheel_speed = int((speed_rr - front_axle_speed - offset) *
16 + 0.5)
rear_left2_wheel_speed = rear_left1_wheel_speed
rear_right2_wheel_speed = rear_right1_wheel_speed
front_axle_speed = int(front_axle_speed * 256 + 0.5)
data = []
data.append(front_axle_speed % 256)
data.append(int(front_axle_speed / 256))
data.append(front_left_wheel_speed)
data.append(front_right_wheel_speed)
data.append(rear_left1_wheel_speed)
data.append(rear_right1_wheel_speed)
data.append(rear_left2_wheel_speed)
data.append(rear_right2_wheel_speed)
m = can.Message(arbitration_id=id, data=data, extended_id=True)
self.can0.send(m)
# print(m)
return
def convert_pgn65265(self, speed_rr, speed_rl):
'''
Convert Corolla speed to PGN65265 CAN speed.
'''
# SA CAN NOT be 0X80 which equal to MTLT's address.
id = self.can_parser.generate_PDU(Priority=6, PGN=65265, SA=0X88)
speed = int((speed_rr + speed_rl) / 2 * 256 + 0.5)
data = [0] * 8
data[1] = speed % 256
data[2] = int(speed / 256)
m = can.Message(arbitration_id=id, data=data, extended_id=True)
self.can0.send(m)
# print(m)
return
def convert_pgn126720(self, speed_rr, speed_rl):
'''
Convert Corolla speed to JD PGN126720 CAN accelerations msg.
'''
dt = 0.01 # [second]. Note! Modify dt according to actual message rate of signal.
# speed = int((speed_rr + speed_rl)/2 + 0.5)/3.6 # m/s
speed = (speed_rr + speed_rl) / 2 / 3.6 # m/s
v = np.array([speed, 0, 0], dtype=np.float32)
# a = at + an
# = dv/dt + w x v
at = (v - self.last_v) / dt
an = np.cross(self.w * D2R, v)
a = at + an # in NED
a[1] = -a[1] # Transform to NWU
a[2] = -a[2]
self.last_v = v
# limit the range of acceleration.
for i in range(3):
if a[i] > 322.55:
a[i] = 322.55
if a[i] < -320:
a[i] = -320
# print(self.w, v, a)
id = self.can_parser.generate_PDU(Priority=6, PGN=126720,
SA=0X88) # 435093640
data = [0] * 8
#
a = np.round((a + 320) * 100)
a = a.astype(np.int64)
# Accel Order in payload : [acc_Y, acc_X, acc_Z]
data[1] = a[1] & 0XFF
data[2] = a[1] >> 8
data[3] = a[0] & 0XFF
data[4] = a[0] >> 8
data[5] = a[2] & 0XFF
data[6] = a[2] >> 8
m = can.Message(arbitration_id=id, data=data, extended_id=True)
self.can0.send(m)
#### save to log files.
s = datetime.datetime.now().strftime('%H:%M:%S.%f') + ','
s += ','.join('{0:f}'.format(i)
for i in (speed_rr, speed_rl, speed, at[0], at[1], at[2],
an[0], an[1], an[2])) + ','
s += ','.join('{0:d}'.format(i) for i in a) + ','
s += ','.join('{0:f}'.format(i) for i in self.w) + ','
s += ','.join('{0:d}'.format(i) for i in data) + '\n'
self.log_file_JD_acc.write(s)
self.log_file_JD_acc.flush()
return
def convert_gear_msg(self, msg):
'''
parse carola gear from msg.
Carola -> PGN61445
P -> 251
R -> -1
N -> 0
D -> 1
'''
if msg.arbitration_id != 0X3BC:
return
(gear, ) = self.can_parser.parse_gear_carola(msg.data)
self.gear = gear
offset = -125
if gear == 32: # P
gear = 251
pass
elif gear == 16: # R
gear = -1 - offset
pass
elif gear == 8: # N
gear = 0 - offset
pass
elif gear == 0: # D
gear = 1 - offset
pass
else:
return
data = [0] * 8
data[3] = gear
#### construct PGN-61445 id and data
id = self.can_parser.generate_PDU(Priority=6, PGN=61445, SA=0X88)
m = can.Message(arbitration_id=id, data=data, extended_id=True)
self.can0.send(m)
# print(m)
#### save to log files.
s = datetime.datetime.now().strftime('%H:%M:%S.%f')
s += ',' + str(msg.arbitration_id)
s += ',' + str(gear) + '\n'
self.log_file_gear.write(s)
self.log_file_gear.flush()
self.log_file_all.write(s)
self.log_file_all.flush()
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
def handle_speed_msg(self):
self.notifier.listeners.append(self.convert_speed_msg)
def handle_gear_msg(self):
self.notifier.listeners.append(self.convert_gear_msg)
def handle_gyro_msg(self):
self.notifier.listeners.append(self.get_gyro_data)
def create_log_files(self):
'''
create log files.
'''
if not os.path.exists('data/'):
os.mkdir('data/')
start_time = datetime.datetime.now().strftime('%Y%m%d_%H%M%S')
file_dir = os.path.join('data', start_time + '.csv')
file_dir_vel = os.path.join('data', start_time + '_vel' + '.csv')
file_dir_gear = os.path.join('data', start_time + '_gear' + '.csv')
file_dir_JD_acc = os.path.join('data', start_time + '_JD_acc' + '.csv')
self.log_file_all = open(file_dir, 'w')
self.log_file_vel = open(file_dir_vel, 'w')
self.log_file_gear = open(file_dir_gear, 'w')
self.log_file_JD_acc = open(file_dir_JD_acc, 'w')
print('Start logging: {0}'.format(file_dir))
header = 'time, ID, payload'.replace(' ', '')
self.log_file_all.write(header + '\n')
self.log_file_all.flush()
header = 'time, ID, speed_fr, speed_fl, speed_rr, speed_rl, gear'.replace(
' ', '')
self.log_file_vel.write(header + '\n')
self.log_file_vel.flush()
header = 'time, ID, gear'.replace(' ', '')
self.log_file_gear.write(header + '\n')
self.log_file_gear.flush()
header = 'time, ID, gear'.replace(' ', '')
self.log_file_JD_acc.write(header + '\n')
self.log_file_JD_acc.flush()
pass
def close_log_files(self):
self.log_file_all.close()
self.log_file_vel.close()
self.log_dir_gear.close()
self.log_file_JD_acc.close()