Skip to content

Commit

Permalink
controlsd/publish_logs: follow capnp best practices (#23372)
Browse files Browse the repository at this point in the history
* follow follow capnp best practices

* cleanup
  • Loading branch information
deanlee authored Jan 4, 2022
1 parent f561d84 commit 960e0cf
Showing 1 changed file with 29 additions and 22 deletions.
51 changes: 29 additions & 22 deletions selfdrive/controls/controlsd.py
Original file line number Diff line number Diff line change
Expand Up @@ -579,46 +579,51 @@ def publish_logs(self, CS, start_time, actuators, lac_log):
CC.active = self.active
CC.actuators = actuators

if len(self.sm['liveLocationKalman'].orientationNED.value) > 2:
CC.roll = self.sm['liveLocationKalman'].orientationNED.value[0]
CC.pitch = self.sm['liveLocationKalman'].orientationNED.value[1]
orientation_value = self.sm['liveLocationKalman'].orientationNED.value
if len(orientation_value) > 2:
CC.roll = orientation_value[0]
CC.pitch = orientation_value[1]

CC.cruiseControl.cancel = CS.cruiseState.enabled and (not self.enabled or not self.CP.pcmCruise)
if self.joystick_mode and self.sm.rcv_frame['testJoystick'] > 0 and self.sm['testJoystick'].buttons[0]:
CC.cruiseControl.cancel = True

CC.hudControl.setSpeed = float(self.v_cruise_kph * CV.KPH_TO_MS)
CC.hudControl.speedVisible = self.enabled
CC.hudControl.lanesVisible = self.enabled
CC.hudControl.leadVisible = self.sm['longitudinalPlan'].hasLead
hudControl = CC.hudControl
hudControl.setSpeed = float(self.v_cruise_kph * CV.KPH_TO_MS)
hudControl.speedVisible = self.enabled
hudControl.lanesVisible = self.enabled
hudControl.leadVisible = self.sm['longitudinalPlan'].hasLead

CC.hudControl.rightLaneVisible = True
CC.hudControl.leftLaneVisible = True
hudControl.rightLaneVisible = True
hudControl.leftLaneVisible = True

recent_blinker = (self.sm.frame - self.last_blinker_frame) * DT_CTRL < 5.0 # 5s blinker cooldown
ldw_allowed = self.is_ldw_enabled and CS.vEgo > LDW_MIN_SPEED and not recent_blinker \
and not self.active and self.sm['liveCalibration'].calStatus == Calibration.CALIBRATED

meta = self.sm['modelV2'].meta
if len(meta.desirePrediction) and ldw_allowed:
model_v2 = self.sm['modelV2']
desire_prediction = model_v2.meta.desirePrediction
if len(desire_prediction) and ldw_allowed:
right_lane_visible = self.sm['lateralPlan'].rProb > 0.5
left_lane_visible = self.sm['lateralPlan'].lProb > 0.5
l_lane_change_prob = meta.desirePrediction[Desire.laneChangeLeft - 1]
r_lane_change_prob = meta.desirePrediction[Desire.laneChangeRight - 1]
l_lane_close = left_lane_visible and (self.sm['modelV2'].laneLines[1].y[0] > -(1.08 + CAMERA_OFFSET))
r_lane_close = right_lane_visible and (self.sm['modelV2'].laneLines[2].y[0] < (1.08 - CAMERA_OFFSET))
l_lane_change_prob = desire_prediction[Desire.laneChangeLeft - 1]
r_lane_change_prob = desire_prediction[Desire.laneChangeRight - 1]

CC.hudControl.leftLaneDepart = bool(l_lane_change_prob > LANE_DEPARTURE_THRESHOLD and l_lane_close)
CC.hudControl.rightLaneDepart = bool(r_lane_change_prob > LANE_DEPARTURE_THRESHOLD and r_lane_close)
lane_lines = model_v2.laneLines
l_lane_close = left_lane_visible and (lane_lines[1].y[0] > -(1.08 + CAMERA_OFFSET))
r_lane_close = right_lane_visible and (lane_lines[2].y[0] < (1.08 - CAMERA_OFFSET))

if CC.hudControl.rightLaneDepart or CC.hudControl.leftLaneDepart:
hudControl.leftLaneDepart = bool(l_lane_change_prob > LANE_DEPARTURE_THRESHOLD and l_lane_close)
hudControl.rightLaneDepart = bool(r_lane_change_prob > LANE_DEPARTURE_THRESHOLD and r_lane_close)

if hudControl.rightLaneDepart or hudControl.leftLaneDepart:
self.events.add(EventName.ldw)

clear_event = ET.WARNING if ET.WARNING not in self.current_alert_types else None
alerts = self.events.create_alerts(self.current_alert_types, [self.CP, self.sm, self.is_metric, self.soft_disable_timer])
self.AM.add_many(self.sm.frame, alerts)
self.AM.process_alerts(self.sm.frame, clear_event)
CC.hudControl.visualAlert = self.AM.visual_alert
hudControl.visualAlert = self.AM.visual_alert

if not self.read_only and self.initialized:
# send car controls over can
Expand Down Expand Up @@ -665,16 +670,18 @@ def publish_logs(self, CS, start_time, actuators, lac_log):
controlsState.forceDecel = bool(force_decel)
controlsState.canErrorCounter = self.can_rcv_error_counter

lat_tuning = self.CP.lateralTuning.which()
if self.joystick_mode:
controlsState.lateralControlState.debugState = lac_log
elif self.CP.steerControlType == car.CarParams.SteerControlType.angle:
controlsState.lateralControlState.angleState = lac_log
elif self.CP.lateralTuning.which() == 'pid':
elif lat_tuning == 'pid':
controlsState.lateralControlState.pidState = lac_log
elif self.CP.lateralTuning.which() == 'lqr':
elif lat_tuning == 'lqr':
controlsState.lateralControlState.lqrState = lac_log
elif self.CP.lateralTuning.which() == 'indi':
elif lat_tuning == 'indi':
controlsState.lateralControlState.indiState = lac_log

self.pm.send('controlsState', dat)

# carState
Expand Down

0 comments on commit 960e0cf

Please sign in to comment.