mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-09-30 19:33:45 +08:00
Deprecate controlsState state fields (#33437)
* Deprecate controlsState state fields * sim works * update refs * one more * these too * update sim old-commit-hash: 3924ac587b735d1e735af4cb77faf6ccf053f656
This commit is contained in:
@@ -733,22 +733,10 @@ class Controls:
|
||||
dat = messaging.new_message('controlsState')
|
||||
dat.valid = CS.canValid
|
||||
controlsState = dat.controlsState
|
||||
if current_alert:
|
||||
controlsState.alertText1 = current_alert.alert_text_1
|
||||
controlsState.alertText2 = current_alert.alert_text_2
|
||||
controlsState.alertSize = current_alert.alert_size
|
||||
controlsState.alertStatus = current_alert.alert_status
|
||||
controlsState.alertType = current_alert.alert_type
|
||||
controlsState.alertSound = current_alert.audible_alert
|
||||
|
||||
controlsState.longitudinalPlanMonoTime = self.sm.logMonoTime['longitudinalPlan']
|
||||
controlsState.lateralPlanMonoTime = self.sm.logMonoTime['modelV2']
|
||||
controlsState.enabled = self.enabled
|
||||
controlsState.active = self.active
|
||||
controlsState.curvature = curvature
|
||||
controlsState.desiredCurvature = self.desired_curvature
|
||||
controlsState.state = self.state
|
||||
controlsState.engageable = not self.events.contains(ET.NO_ENTRY)
|
||||
controlsState.longControlState = self.LoC.long_control_state
|
||||
controlsState.upAccelCmd = float(self.LoC.pid.p)
|
||||
controlsState.uiAccelCmd = float(self.LoC.pid.i)
|
||||
@@ -756,8 +744,6 @@ class Controls:
|
||||
controlsState.cumLagMs = -self.rk.remaining * 1000.
|
||||
controlsState.startMonoTime = int(start_time * 1e9)
|
||||
controlsState.forceDecel = bool(force_decel)
|
||||
controlsState.experimentalMode = self.experimental_mode
|
||||
controlsState.personality = self.personality
|
||||
|
||||
lat_tuning = self.CP.lateralTuning.which()
|
||||
if self.joystick_mode:
|
||||
|
||||
@@ -96,7 +96,7 @@ class LongitudinalPlanner:
|
||||
return x, v, a, j
|
||||
|
||||
def update(self, sm):
|
||||
self.mpc.mode = 'blended' if sm['controlsState'].experimentalMode else 'acc'
|
||||
self.mpc.mode = 'blended' if sm['selfdriveState'].experimentalMode else 'acc'
|
||||
|
||||
v_ego = sm['carState'].vEgo
|
||||
v_cruise_kph = min(sm['carState'].vCruise, V_CRUISE_MAX)
|
||||
@@ -106,7 +106,7 @@ class LongitudinalPlanner:
|
||||
force_slow_decel = sm['controlsState'].forceDecel
|
||||
|
||||
# Reset current state when not engaged, or user is controlling the speed
|
||||
reset_state = long_control_off if self.CP.openpilotLongitudinalControl else not sm['controlsState'].enabled
|
||||
reset_state = long_control_off if self.CP.openpilotLongitudinalControl else not sm['selfdriveState'].enabled
|
||||
|
||||
# No change cost when user is controlling the speed, or when standstill
|
||||
prev_accel_constraint = not (reset_state or sm['carState'].standstill)
|
||||
@@ -134,11 +134,11 @@ class LongitudinalPlanner:
|
||||
accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05)
|
||||
accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05)
|
||||
|
||||
self.mpc.set_weights(prev_accel_constraint, personality=sm['controlsState'].personality)
|
||||
self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality)
|
||||
self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
|
||||
self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
|
||||
x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error)
|
||||
self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=sm['controlsState'].personality)
|
||||
self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=sm['selfdriveState'].personality)
|
||||
|
||||
self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution)
|
||||
self.a_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution)
|
||||
@@ -157,7 +157,7 @@ class LongitudinalPlanner:
|
||||
def publish(self, sm, pm):
|
||||
plan_send = messaging.new_message('longitudinalPlan')
|
||||
|
||||
plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState'])
|
||||
plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState', 'selfdriveState'])
|
||||
|
||||
longitudinalPlan = plan_send.longitudinalPlan
|
||||
longitudinalPlan.modelMonoTime = sm.logMonoTime['modelV2']
|
||||
|
||||
@@ -17,7 +17,7 @@ def plannerd_thread():
|
||||
|
||||
longitudinal_planner = LongitudinalPlanner(CP)
|
||||
pm = messaging.PubMaster(['longitudinalPlan'])
|
||||
sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2'],
|
||||
sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2', 'selfdriveState'],
|
||||
poll='modelV2', ignore_avg_freq=['radarState'])
|
||||
|
||||
while True:
|
||||
|
||||
Reference in New Issue
Block a user