|
|
@ -45,12 +45,14 @@ class LongitudinalMpc(): |
|
|
|
self.cur_state[0].v_ego = v_safe |
|
|
|
self.cur_state[0].v_ego = v_safe |
|
|
|
self.cur_state[0].a_ego = a_safe |
|
|
|
self.cur_state[0].a_ego = a_safe |
|
|
|
|
|
|
|
|
|
|
|
def update(self, carstate, model, v_cruise): |
|
|
|
def update(self, carstate, radarstate, v_cruise): |
|
|
|
v_cruise_clipped = np.clip(v_cruise, self.cur_state[0].v_ego - 10., self.cur_state[0].v_ego + 10.0) |
|
|
|
v_cruise_clipped = np.clip(v_cruise, self.cur_state[0].v_ego - 10., self.cur_state[0].v_ego + 10.0) |
|
|
|
poss = v_cruise_clipped * np.array(T_IDXS[:LON_MPC_N+1]) |
|
|
|
poss = v_cruise_clipped * np.array(T_IDXS[:LON_MPC_N+1]) |
|
|
|
speeds = v_cruise_clipped * np.ones(LON_MPC_N+1) |
|
|
|
speeds = v_cruise_clipped * np.ones(LON_MPC_N+1) |
|
|
|
accels = np.zeros(LON_MPC_N+1) |
|
|
|
accels = np.zeros(LON_MPC_N+1) |
|
|
|
|
|
|
|
self.update_with_xva(poss, speeds, accels) |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def update_with_xva(self, poss, speeds, accels): |
|
|
|
# Calculate mpc |
|
|
|
# Calculate mpc |
|
|
|
self.libmpc.run_mpc(self.cur_state, self.mpc_solution, |
|
|
|
self.libmpc.run_mpc(self.cur_state, self.mpc_solution, |
|
|
|
list(poss), list(speeds), list(accels), |
|
|
|
list(poss), list(speeds), list(accels), |
|
|
|