def step(self):
start_time = sec_since_boot()
self.prof.checkpoint("Ratekeeper", ignore=True)
# Sample data from sockets and get a carState
CS = self.data_sample()
self.prof.checkpoint("Sample")
if self.read_only:
self.hyundai_lkas = self.read_only
elif CS.cruiseState.enabled and self.hyundai_lkas:
self.CP = CarInterface.live_tune(self.CP, True)
self.hyundai_lkas = False
self.init_flag = 1
self.update_events(CS)
if not self.hyundai_lkas:
# Update control state
self.state_transition(CS)
self.prof.checkpoint("State transition")
# Compute actuators (runs PID loops and lateral MPC)
actuators, v_acc, a_acc, lac_log = self.state_control(CS)
self.prof.checkpoint("State Control")
# Publish data
self.publish_logs(CS, start_time, actuators, v_acc, a_acc, lac_log)
self.prof.checkpoint("Sent")
if not CS.cruiseState.enabled and not self.hyundai_lkas:
self.hyundai_lkas = True
def model_polyfit(points):
path_pinv = compute_path_pinv()
return np.dot(path_pinv, map(float, points))
xx = []
yy = []
deltas = []
psis = []
times = []
CP = CarInterface.get_params(CAR.GRANDEUR_HEV)
print('CP.steerRatio1={}'.format(CP.steerRatio))
CP = CarInterface.live_tune(CP)
print('CP.steerRatio2={}'.format(CP.steerRatio))
VM = VehicleModel(CP)
v_ref = 32.00 # 45 mph
curvature_factor = VM.curvature_factor(v_ref)
print(curvature_factor)
LANE_WIDTH = 3.9
p_l = map(float, model_polyfit(points_l))
p_r = map(float, model_polyfit(points_r))
p_p = map(float, model_polyfit(points_c))
l_poly = libmpc_py.ffi.new("double[4]", p_l)