From 6570ee324f782c4bc89afb3fba22db3c9c24c7dc Mon Sep 17 00:00:00 2001 From: Rick Lan Date: Wed, 12 Jun 2024 12:14:32 +0800 Subject: [PATCH] added ALKA --- selfdrive/car/card.py | 3 +++ selfdrive/controls/controlsd.py | 27 ++++++++++++++++++++++++++- selfdrive/ui/qt/onroad/onroad_home.cc | 5 ++++- selfdrive/ui/ui.cc | 7 +++++++ selfdrive/ui/ui.h | 5 +++++ 5 files changed, 45 insertions(+), 2 deletions(-) diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index ed916a6fe..632bdfd6d 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -55,6 +55,9 @@ class CarD: if not self.disengage_on_accelerator: self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS + if self.params.get_bool("dp_alka"): + self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ALKA + openpilot_enabled_toggle = self.params.get_bool("OpenpilotEnabledToggle") controller_available = self.CI.CC is not None and openpilot_enabled_toggle and not self.CP.dashcamOnly diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 92c933112..5dd7779fc 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -65,6 +65,10 @@ class Controls: self.params = Params() + # dp + self._dp_alka = self.params.get_bool("dp_alka") + self._dp_alka_active = True + with car.CarParams.from_bytes(self.params.get("CarParams", block=True)) as msg: # TODO: this shouldn't need to be a builder self.CP = msg.as_builder() @@ -511,7 +515,7 @@ class Controls: # Check if openpilot is engaged and actuators are enabled self.enabled = self.state in ENABLED_STATES self.active = self.state in ACTIVE_STATES - if self.active: + if self.active or self._dp_alka_active: self.current_alert_types.append(ET.WARNING) def state_control(self, CS): @@ -542,6 +546,19 @@ class Controls: (not standstill or self.joystick_mode) CC.longActive = self.enabled and not self.events.contains(ET.OVERRIDE_LONGITUDINAL) and self.CP.openpilotLongitudinalControl + # rick - alka + if not self.CP.passive and self.initialized and self._dp_alka and self._dp_alka_active and not standstill and CS.cruiseState.available: + if self.sm['liveCalibration'].calStatus != log.LiveCalibrationData.Status.calibrated: + pass + elif abs(CS.steeringAngleDeg) >= 450: + pass + elif CS.steerFaultTemporary or CS.steerFaultPermanent: + pass + elif CS.gearShifter == car.CarState.GearShifter.reverse: + pass + else: + CC.latActive = True + actuators = CC.actuators actuators.longControlState = self.LoC.long_control_state @@ -773,6 +790,14 @@ class Controls: self.pm.send('controlsState', dat) + # dp - controlsStateExt + dat = messaging.new_message('controlsStateExt') + dat.valid = CS.canValid + controlsStateExt = dat.controlsStateExt + controlsStateExt.alkaActive = self._dp_alka_active + controlsStateExt.alkaEnabled = self._dp_alka + self.pm.send('controlsStateExt', dat) + # onroadEvents - logged every second or on change if (self.sm.frame % int(1. / DT_CTRL) == 0) or (self.events.names != self.events_prev): ce_send = messaging.new_message('onroadEvents', len(self.events)) diff --git a/selfdrive/ui/qt/onroad/onroad_home.cc b/selfdrive/ui/qt/onroad/onroad_home.cc index 66eb1812e..5175fa71d 100644 --- a/selfdrive/ui/qt/onroad/onroad_home.cc +++ b/selfdrive/ui/qt/onroad/onroad_home.cc @@ -64,7 +64,10 @@ void OnroadWindow::updateState(const UIState &s) { alerts->updateState(s); nvg->updateState(s); - QColor bgColor = bg_colors[s.status]; + // dp - alka colored border + const SubMaster &sm = *(s.sm); + bool alka_active = sm["carControl"].getCarControl().getLatActive() && sm["controlsStateExt"].getControlsStateExt().getAlkaActive(); + QColor bgColor = bg_colors[alka_active && s.status == STATUS_DISENGAGED? STATUS_ALKA : s.status]; if (bg != bgColor) { // repaint border bg = bgColor; diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 33f66df6e..4186f962e 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -215,6 +215,9 @@ static void update_state(UIState *s) { sm.rcv_frame("liveCalibration") > scene.started_frame && sm.rcv_frame("modelV2") > scene.started_frame && sm.rcv_frame("uiPlan") > scene.started_frame); + + // dp + scene.alka_enabled = sm["controlsStateExt"].getControlsStateExt().getAlkaEnabled(); } void ui_update_params(UIState *s) { @@ -251,6 +254,10 @@ UIState::UIState(QObject *parent) : QObject(parent) { "modelV2", "controlsState", "liveCalibration", "radarState", "deviceState", "pandaStates", "carParams", "driverMonitoringState", "carState", "liveLocationKalman", "driverStateV2", "wideRoadCameraState", "managerState", "navInstruction", "navRoute", "uiPlan", + + // dp + // alka + "carControl", "controlsStateExt", }); Params params; diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 512c09a1c..4607dd71c 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -55,6 +55,7 @@ typedef enum UIStatus { STATUS_DISENGAGED, STATUS_OVERRIDE, STATUS_ENGAGED, + STATUS_ALKA, } UIStatus; enum PrimeType { @@ -72,6 +73,7 @@ const QColor bg_colors [] = { [STATUS_DISENGAGED] = QColor(0x17, 0x33, 0x49, 0xc8), [STATUS_OVERRIDE] = QColor(0x91, 0x9b, 0x95, 0xf1), [STATUS_ENGAGED] = QColor(0x17, 0x86, 0x44, 0xf1), + [STATUS_ALKA] = QColor(0x22, 0xa0, 0xdc, 0xf1), }; @@ -106,6 +108,9 @@ typedef struct UIScene { bool started, ignition, is_metric, map_on_left, longitudinal_control; bool world_objects_visible = false; uint64_t started_frame; + + // dp + bool alka_enabled = false; } UIScene; class UIState : public QObject {