diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 93cd22708e..f70a220499 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -579,46 +579,51 @@ class Controls: 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 @@ -665,16 +670,18 @@ class Controls: 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