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()