def traj_derivation(traj):
moving_window=2
veh_length=5
(speed_time, speed, front_space_time, front_space, relative_speed)=traj
v = [s/3.6 for s in speed]#m/s
original_location = [0]
for i in range(len(speed) - 1):
forward = (speed_time[i + 1] - speed_time[i]) * v[i]
original_location.append(original_location[-1] + forward) # in meter
t = speed_time
d = original_location
draw_fig(t, '', front_space, 'space(m)')
front_space = fill_front_space_missing_signal(front_space,expected_frequency, high_threshold=100)
space = front_space
# r_v=relative_speed
draw_fig(t, '', space, 'revised space (m)')
d_LV = [d[i] + space[i] + veh_length for i in range(len(d))]
# best_ita_parameter(t, d_LV, t, d, sim_freq=1/expected_frequency)
# d_LV=moving_average(d_LV,100)
# d_LV_derived=[d[0]+space[0]]
# for i in range(len(d)-1):
# d_LV_derived.append(d_LV_derived[i]+(v[i]+r_v[i])/3.6*0.01)
# diff=np.mean(d_LV)-np.mean(d_LV_derived)
# d_LV_derived=[d+diff for d in d_LV_derived]
t_ita, ita = cal_ita(t, d_LV, t, d, sim_freq=1/expected_frequency, w=5, k=.2)
# t_ita_derived,ita_derived=cal_ita(t,d_LV_derived,t,d,sim_freq=0.01,w=5,k=0.1333)
# v_LV_measured=[v[i]+r_v[i] for i in range(len(v))]
v_LV_derived = [(d_LV[i + 1] - d_LV[i - 1]) * expected_frequency / 2 for i in range(1, len(d_LV) - 1)]
v_LV_derived = [v_LV_derived[0]] + v_LV_derived + [v_LV_derived[-1]]
v_LV_derived = moving_average(v_LV_derived, moving_window*expected_frequency)
v_LV_derived = [max(vlv,0) for vlv in v_LV_derived]
return t,v,d,v_LV_derived,d_LV,t_ita,ita
def analyze_STEER_ANGLE_SENSOR(message_array):
steer_angle = []
operation_time = []
for m in message_array:
steer = hex_to_int(m[1], 71, 3, 12, signed=True) * 1.5
steer_angle.append(steer)
operation_time.append(m[0])
draw_fig(operation_time, '', steer_angle, 'steer angle (deg)')
def analyze_STANDSTILL(message_array):
wheels_moving = []
for m in message_array:
message = m[1].split('x')[1]
length = m[2]
WHEELS_MOVING = hex_to_byte(message[1], 8)[3]
wheels_moving.append(WHEELS_MOVING)
draw_fig(range(len(wheels_moving)), '', wheels_moving, 'wheels moving')
def analyze_POWERTRAIN_DATA(message_array):
brake_on = []
for m in message_array:
message = m[1].split('x')[1]
length = m[2]
BRAKE_SWITCH = hex_to_byte(message[4],8)[-1]
brake_on.append(BRAKE_SWITCH)
draw_fig(range(len(brake_on)), '', brake_on, 'brake on')
def analyze_ENGINE_DATA(message_array):
speed=[]
rpm=[]
for m in message_array:
message = m[1].split('x')[1]
length = m[2]
XMISSION_SPEED=int(message[0:4],16)*0.01
ENGINE_RPM=int(message[4:8],16)
speed.append(XMISSION_SPEED)
rpm.append(ENGINE_RPM)
draw_fig(range(len(speed)),'',speed,'speed (kph)')
def analyze_KINEMATICS(message_array):
long_accel = []
for m in message_array:
message = m[1].split('x')[1] #66, get the hex number for KINEMATICS
bin1 = hex_to_byte(message, 64) #66, transform the hex to 64 binary numbers
bin2 = bin1[30:40] #66, get the 31~40 binary numbers which correspond to longtitudinal acceleration
dec1 = int(bin2, 2) #66, transform the 31~40 binary value to decimal
factor = -0.035 #66, this is read from CABANA
offset = 17.92 #66, this is read from CABANA
result = factor * dec1 + offset #66, compute the real longitudinal acceleration value
long_accel.append(result)
draw_fig(range(len(long_accel)),'time step',long_accel,'long_accel')
def analyze_GAS_PEDAL_2(message_array):
torque_estimate=[]
torque_request=[]
for m in message_array:
message = m[1].split('x')[1]
length = m[2]
ENGINE_TORQUE_ESTIMATE=int(message[0:4],16)
ENGINE_TORQUE_REQUEST=int(message[4:8],16)
torque_estimate.append(ENGINE_TORQUE_ESTIMATE)
torque_request.append(ENGINE_TORQUE_REQUEST)
draw_fig(range(len(torque_estimate)),'',torque_estimate,'estimated torque(Nm)')
draw_fig(range(len(torque_request)),'',torque_request,'request torque(Nm)')
def analyze_PCM_CRUISE_2(message_array):
cruise_active = []
set_speed = []
operation_time = []
for m in message_array:
active = hex_to_int(m[1], 71, 15, 1, signed=False)
set_v = hex_to_int(m[1], 71, 23, 8, signed=False)
cruise_active.append(active)
set_speed.append(set_v)
operation_time.append(m[0])
draw_fig(operation_time, '', cruise_active, 'ACC ready to use')
draw_fig(operation_time, '', set_speed, 'set speed (kph)')
return operation_time, cruise_active
def analyze_SEATBELT_STATUS(message_array):
driver_seatbelt_lamp = []
driver_seatbelt_unlatched = []
driver_seatbelt_latched = []
for m in message_array:
message = m[1].split('x')[1]
length = m[2]
SEATBELT_DRIVER_LAMP = hex_to_byte(message[0], 8)[0]
SEATBELT_DRIVER_LATCHED = hex_to_byte(message[1], 8)[2]
SEATBELT_DRIVER_UNLATCHED = hex_to_byte(message[1], 8)[3]
driver_seatbelt_lamp.append(SEATBELT_DRIVER_LAMP)
driver_seatbelt_latched.append(SEATBELT_DRIVER_LATCHED)
driver_seatbelt_unlatched.append(SEATBELT_DRIVER_UNLATCHED)
draw_fig(range(len(driver_seatbelt_lamp)), '', driver_seatbelt_lamp, 'lamp')
draw_fig(range(len(driver_seatbelt_latched)), '', driver_seatbelt_latched, 'latched')
draw_fig(range(len(driver_seatbelt_unlatched)), '', driver_seatbelt_unlatched, 'unlatched')
def draw_traj_2(t,f_speed,f_front_space,f_relative_speed,l_speed,l_front_space,l_relative_speed,fig_name):
d=[0]
for i in range(len(f_speed)-1):
forward = (t[i + 1] - t[i]) * f_speed[i] / 3.6
d.append(d[-1]+forward) #in meter
draw_fig(t,'',f_front_space,'original space (m)')
f_front_space=fill_front_space_missing_signal(f_front_space,high_threshold=100)
# l_front_space=fill_front_space_missing_signal(l_front_space,high_threshold=200)
draw_fig(t,'',f_front_space,'revised space (m)')
v_LV_back_measured=[f_speed[i]+f_relative_speed[i] for i in range(len(f_speed))]
diff=np.mean(v_LV_back_measured)-np.mean(l_speed)
l_speed_revised=[s+diff for s in l_speed]
d_LV=[d[i]+f_front_space[i] for i in range(len(d))]
d_LV_derived=[d[0]+f_front_space[0]]
for i in range(len(d)-1):
d_LV_derived.append(d_LV_derived[i]+(f_speed[i]+f_relative_speed[i])/3.6*0.01)
diff=np.mean(d_LV)-np.mean(d_LV_derived)
d_LV_derived=[d+diff for d in d_LV_derived]
d_LV_front_measured=[d[0]+f_front_space[0]]
for i in range(len(d)-1):
d_LV_front_measured.append(d_LV_front_measured[i]+l_speed_revised[i]/3.6*0.01)
diff=np.mean(d_LV)-np.mean(d_LV_front_measured)
d_LV_front_measured=[d+diff for d in d_LV_front_measured]
v_LV_derived = [(d_LV[i + 1] - d_LV[i-1]) / 0.01 * 3.6/2 for i in range(1,len(d_LV) - 1)]
v_LV_derived=[v_LV_derived[0]]+v_LV_derived+[v_LV_derived[-1]]
v_LV_derived = moving_average(v_LV_derived, 200)
t_ita,ita=cal_ita(t,d_LV,t,d,sim_freq=0.01,w=5,k=0.1333)
t_ita_derived,ita_derived=cal_ita(t,d_LV_derived,t,d,sim_freq=0.01,w=5,k=0.1333)
t_ita_front_measured,ita_front_measured=cal_ita(t,d_LV_front_measured,t,d,sim_freq=0.01,w=5,k=0.1333)
fig = plt.figure(figsize=(8, 12), dpi=300)
ax = fig.add_subplot(311)
plt.plot(t, d, color='r', label='FV')
plt.plot(t, d_LV, color='g', label='LV (direct measured from spacing)')
plt.plot(t, d_LV_derived, color='k', label='LV (integrated from relative speed)')
plt.plot(t, d_LV_front_measured, color='c', label='LV (integrated from shifted lead self speed)')
plt.ylabel('location(m)', fontsize=24)
plt.legend()
plt.xlim([t[0]+3,t[-1]])
ax = fig.add_subplot(312)
plt.plot(t_ita, ita, color='g',label='direct measured from spacing')
plt.plot(t_ita_derived, ita_derived, color='k', label='integrated from relative speed')
plt.plot(t_ita_front_measured, ita_front_measured, color='c', label='integrated from shifted lead self speed')
plt.ylabel(r'$\eta$', fontsize=24)
plt.xlim([t[0]+3, t[-1]])
plt.ylim([0.5,2])
plt.legend()
ax = fig.add_subplot(313)
plt.plot(t, f_speed, color='r', label='FV')
plt.plot(t, v_LV_derived, color='g', label='LV (derived from distance)')
plt.plot(t, v_LV_back_measured, color='k', label='LV (measured from FV radar)')
# plt.plot(t, l_speed, color='b', label='LV (direct measured from LV CANBUS)')
plt.plot(t, l_speed_revised, color='c', label='LV (LV CANBUS shift)')
plt.xlabel('time (s)', fontsize=24)
plt.ylabel('speed(kph)', fontsize=24)
plt.legend()
plt.xlim([t[0] + 3, t[-1]])
plt.ylim([max(0,np.mean(f_speed)-40),np.mean(f_speed)+40])
plt.savefig(fig_name + '.png')
plt.close()