diff --git a/selfdrive/car/chrysler/carstate.py b/selfdrive/car/chrysler/carstate.py index f1d1ee5..953704d 100644 --- a/selfdrive/car/chrysler/carstate.py +++ b/selfdrive/car/chrysler/carstate.py @@ -103,6 +103,8 @@ class CarState(CarStateBase): self.button_counter = cp.vl["CRUISE_BUTTONS"]["COUNTER"] # FrogPilot carstate functions + fp_ret.brakeLights = bool(cp.vl["ESP_1"]["BRAKE_PRESSED_ACC"]) + self.lkas_previously_enabled = self.lkas_enabled if self.CP.carFingerprint in RAM_CARS: self.lkas_enabled = cp.vl["Center_Stack_2"]["LKAS_Button"] or cp.vl["Center_Stack_1"]["LKAS_Button"] diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index 9029f43..790cead 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -274,6 +274,14 @@ class CarState(CarStateBase): ret.rightBlindspot = cp_body.vl["BSM_STATUS_RIGHT"]["BSM_ALERT"] == 1 # FrogPilot carstate functions + brake_light_cars = (CAR.CIVIC, CAR.ODYSSEY, CAR.ODYSSEY_CHN, CAR.CRV_5G, CAR.ACCORD, CAR.CIVIC_BOSCH, + CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.ACURA_RDX_3G, CAR.HONDA_E) + + if self.CP.carFingerprint in brake_light_cars: + fp_ret.brakeLights = bool(cp.vl["ACC_CONTROL"]['BRAKE_LIGHTS'] != 0 or ret.brake > 0.4) if not self.CP.openpilotLongitudinalControl else bool(ret.brake > 0.4) + elif self.CP.carFingerprint in HONDA_BOSCH and self.CP.carFingerprint not in HONDA_BOSCH_RADARLESS: + fp_ret.brakeLights = bool(cp.vl["ACC_CONTROL"]['BRAKE_LIGHTS'] != 0 or ret.brake > 0.4) if not self.CP.openpilotLongitudinalControl else bool(ret.brake > 0.4) + self.prev_distance_button = self.distance_button self.distance_button = self.cruise_setting == 3 diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index ff282a8..17a80bf 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -186,6 +186,8 @@ class CarState(CarStateBase): self.main_enabled = not self.main_enabled # FrogPilot carstate functions + fp_ret.brakeLights = bool(cp.vl["TCS13"]["BrakeLight"]) + fp_ret.dashboardSpeedLimit = self.calculate_speed_limit(cp, cp_cam) * speed_conv self.prev_distance_button = self.distance_button @@ -282,6 +284,8 @@ class CarState(CarStateBase): else cp_cam.vl["CAM_0x2a4"]) # FrogPilot carstate functions + fp_ret.brakeLights = cp.vl["TCS"]["DriverBraking"] == 1 + fp_ret.dashboardSpeedLimit = self.calculate_speed_limit(cp, cp_cam) * speed_factor self.prev_distance_button = self.distance_button diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py index 8ce62dd..28f75ff 100644 --- a/selfdrive/car/subaru/carstate.py +++ b/selfdrive/car/subaru/carstate.py @@ -128,8 +128,11 @@ class CarState(CarStateBase): # FrogPilot carstate functions if self.car_fingerprint not in PREGLOBAL_CARS: + fp_ret.brakeLights = bool(cp_cam.vl["ES_DashStatus"]["Brake_Lights"]) self.lkas_previously_enabled = self.lkas_enabled self.lkas_enabled = cp_cam.vl["ES_LKAS_State"]["LKAS_Dash_State"] + else: + fp_ret.brakeLights = bool(cp_cam.vl["ES_Brake"]["Cruise_Brake_Lights"]) return ret, fp_ret diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 99d9e1e..c7843e2 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -208,6 +208,8 @@ class CarState(CarStateBase): self.distance_button = cp.vl["SDSU"]["FD_BUTTON"] # FrogPilot carstate functions + fp_ret.brakeLights = bool(cp.vl["ESP_CONTROL"]["BRAKE_LIGHTS_ACC"]) + self.cruise_decreased_previously = self.cruise_decreased self.cruise_increased_previously = self.cruise_increased diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py index f42eb86..4bea1aa 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -151,6 +151,10 @@ class CarState(CarStateBase): self.upscale_lead_car_signal = bool(pt_cp.vl["Kombi_03"]["KBI_Variante"]) self.frame += 1 + + # FrogPilot carstate functions + fp_ret.brakeLights = bool(pt_cp.vl["ESP_05"]['ESP_Status_Bremsdruck']) + return ret, fp_ret def update_pq(self, pt_cp, cam_cp, ext_cp, trans_type, frogpilot_variables): @@ -251,6 +255,10 @@ class CarState(CarStateBase): ret.espDisabled = bool(pt_cp.vl["Bremse_1"]["ESP_Passiv_getastet"]) self.frame += 1 + + # FrogPilot carstate functions + fp_ret.brakeLights = bool(pt_cp.vl["Motor_2"]['Bremstestschalter']) + return ret, fp_ret def update_hca_state(self, hca_status): diff --git a/selfdrive/frogpilot/assets/other_images/brake_pedal.png b/selfdrive/frogpilot/assets/other_images/brake_pedal.png new file mode 100644 index 0000000..f4fe0ef Binary files /dev/null and b/selfdrive/frogpilot/assets/other_images/brake_pedal.png differ diff --git a/selfdrive/frogpilot/assets/other_images/gas_pedal.png b/selfdrive/frogpilot/assets/other_images/gas_pedal.png new file mode 100644 index 0000000..ef1a87e Binary files /dev/null and b/selfdrive/frogpilot/assets/other_images/gas_pedal.png differ diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 12b7e55..6204883 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -339,8 +339,21 @@ AnnotatedCameraWidget::AnnotatedCameraWidget(VisionStreamType type, QWidget* par main_layout->setMargin(UI_BORDER_SIZE); main_layout->setSpacing(0); + QHBoxLayout *buttons_layout = new QHBoxLayout(); + buttons_layout->setSpacing(0); + experimental_btn = new ExperimentalButton(this); - main_layout->addWidget(experimental_btn, 0, Qt::AlignTop | Qt::AlignRight); + buttons_layout->addWidget(experimental_btn); + + QVBoxLayout *top_right_layout = new QVBoxLayout(); + top_right_layout->setSpacing(0); + top_right_layout->addLayout(buttons_layout); + + pedal_icons = new PedalIcons(this); + top_right_layout->addWidget(pedal_icons, 0, Qt::AlignRight); + + main_layout->addLayout(top_right_layout, 0); + main_layout->setAlignment(top_right_layout, Qt::AlignTop | Qt::AlignRight); map_settings_btn = new MapSettingsButton(this); main_layout->addWidget(map_settings_btn, 0, Qt::AlignBottom | Qt::AlignRight); @@ -970,6 +983,12 @@ void AnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &p) { bottom_layout->setAlignment(distance_btn, (rightHandDM ? Qt::AlignRight : Qt::AlignLeft)); } + bool enablePedalIcons = scene.pedals_on_ui && !bigMapOpen; + pedal_icons->setVisible(enablePedalIcons); + if (enablePedalIcons) { + pedal_icons->updateState(); + } + map_settings_btn_bottom->setEnabled(map_settings_btn->isEnabled()); if (map_settings_btn_bottom->isEnabled()) { map_settings_btn_bottom->setVisible(!hideBottomIcons && !compass); @@ -1153,6 +1172,52 @@ void DistanceButton::paintEvent(QPaintEvent *event) { } } +PedalIcons::PedalIcons(QWidget *parent) : QWidget(parent), scene(uiState()->scene) { + setFixedSize(btn_size, btn_size); + + brake_pedal_img = loadPixmap("../frogpilot/assets/other_images/brake_pedal.png", QSize(img_size, img_size)); + gas_pedal_img = loadPixmap("../frogpilot/assets/other_images/gas_pedal.png", QSize(img_size, img_size)); +} + +void PedalIcons::updateState() { + acceleration = scene.acceleration; + + accelerating = acceleration > 0.25; + decelerating = acceleration < -0.25; + + if (accelerating || decelerating) { + update(); + } +} + +void PedalIcons::paintEvent(QPaintEvent *event) { + QPainter p(this); + p.setRenderHint(QPainter::Antialiasing); + + int totalWidth = 2 * img_size; + int startX = (width() - totalWidth) / 2; + + int brakeX = startX + img_size / 2; + int gasX = startX + img_size; + + float brakeOpacity = 1.0f; + float gasOpacity = 1.0f; + + if (scene.dynamic_pedals_on_ui) { + brakeOpacity = scene.standstill ? 1.0f : decelerating ? std::max(0.25f, std::abs(acceleration)) : 0.25f; + gasOpacity = accelerating ? std::max(0.25f, acceleration) : 0.25f; + } else if (scene.static_pedals_on_ui) { + brakeOpacity = scene.standstill || scene.brake_lights_on || acceleration < -0.5 ? 1.0f : 0.25f; + gasOpacity = !scene.standstill && acceleration > 0 ? 1.0f : 0.25f; + } + + p.setOpacity(brakeOpacity); + p.drawPixmap(brakeX, (height() - img_size) / 2, brake_pedal_img); + + p.setOpacity(gasOpacity); + p.drawPixmap(gasX, (height() - img_size) / 2, gas_pedal_img); +} + void AnnotatedCameraWidget::drawSLCConfirmation(QPainter &p) { p.save(); diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index f20d34f..6c83430 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -119,6 +119,27 @@ private: QPixmap settings_img; }; +class PedalIcons : public QWidget { + Q_OBJECT + +public: + explicit PedalIcons(QWidget *parent = 0); + void updateState(); + +private: + void paintEvent(QPaintEvent *event) override; + + QPixmap brake_pedal_img; + QPixmap gas_pedal_img; + + UIScene &scene; + + bool accelerating; + bool decelerating; + + float acceleration; +}; + // container window for the NVG UI class AnnotatedCameraWidget : public CameraWidget { Q_OBJECT @@ -169,6 +190,7 @@ private: Compass *compass_img; DistanceButton *distance_btn; + PedalIcons *pedal_icons; QHBoxLayout *bottom_layout; diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 1008b60..76105f1 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -223,6 +223,7 @@ static void update_state(UIState *s) { scene.blind_spot_left = carState.getLeftBlindspot(); scene.blind_spot_right = carState.getRightBlindspot(); scene.parked = carState.getGearShifter() == cereal::CarState::GearShifter::PARK; + scene.standstill = carState.getStandstill(); } if (sm.updated("controlsState")) { auto controlsState = sm["controlsState"].getControlsState(); @@ -242,6 +243,7 @@ static void update_state(UIState *s) { } if (sm.updated("frogpilotCarState")) { auto frogpilotCarState = sm["frogpilotCarState"].getFrogpilotCarState(); + scene.brake_lights_on = frogpilotCarState.getBrakeLights(); } if (sm.updated("frogpilotPlan")) { auto frogpilotPlan = sm["frogpilotPlan"].getFrogpilotPlan(); @@ -304,6 +306,9 @@ void ui_update_frogpilot_params(UIState *s) { scene.adjacent_path_metrics = scene.adjacent_path && params.getBool("AdjacentPathMetrics"); scene.blind_spot_path = custom_paths && params.getBool("BlindSpotPath"); scene.compass = custom_onroad_ui && params.getBool("Compass"); + scene.pedals_on_ui = custom_onroad_ui && params.getBool("PedalsOnUI"); + scene.dynamic_pedals_on_ui = scene.pedals_on_ui && params.getBool("DynamicPedalsOnUI"); + scene.static_pedals_on_ui = scene.pedals_on_ui && params.getBool("StaticPedalsOnUI"); scene.disable_smoothing_mtsc = params.getBool("MTSCEnabled") && params.getBool("DisableMTSCSmoothing"); scene.disable_smoothing_vtsc = params.getBool("VisionTurnControl") && params.getBool("DisableVTSCSmoothing"); diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 8757d55..4fb458b 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -193,10 +193,12 @@ typedef struct UIScene { bool blind_spot_left; bool blind_spot_path; bool blind_spot_right; + bool brake_lights_on; bool compass; bool conditional_experimental; bool disable_smoothing_mtsc; bool disable_smoothing_vtsc; + bool dynamic_pedals_on_ui; bool enabled; bool experimental_mode; bool experimental_mode_via_screen; @@ -204,6 +206,7 @@ typedef struct UIScene { bool online; bool onroad_distance_button; bool parked; + bool pedals_on_ui; bool reverse_cruise; bool reverse_cruise_ui; bool right_hand_drive; @@ -214,6 +217,8 @@ typedef struct UIScene { bool speed_limit_changed; bool speed_limit_controller; bool speed_limit_overridden; + bool standstill; + bool static_pedals_on_ui; bool tethering_enabled; bool traffic_mode; bool traffic_mode_active;