diff --git a/selfdrive/car/chrysler/carstate.py b/selfdrive/car/chrysler/carstate.py index 0e2a82bf2..3085a320a 100644 --- a/selfdrive/car/chrysler/carstate.py +++ b/selfdrive/car/chrysler/carstate.py @@ -104,6 +104,8 @@ class CarState(CarStateBase): self.button_counter = cp.vl[self.button_message]["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 5bbbf7740..253a0ae93 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -274,6 +274,12 @@ class CarState(CarStateBase): ret.rightBlindspot = cp_body.vl["BSM_STATUS_RIGHT"]["BSM_ALERT"] == 1 # FrogPilot CarState functions + brake_light_cars = (CAR.HONDA_CIVIC, CAR.HONDA_ODYSSEY, CAR.HONDA_ODYSSEY_CHN, CAR.HONDA_CRV_5G, CAR.HONDA_ACCORD, CAR.HONDA_CIVIC_BOSCH, + CAR.HONDA_CIVIC_BOSCH_DIESEL, CAR.HONDA_CRV_HYBRID, CAR.ACURA_RDX_3G, CAR.HONDA_E) + + if self.CP.carFingerprint in brake_light_cars or (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 4149dec39..a9663a9d9 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -190,6 +190,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 @@ -286,6 +288,8 @@ class CarState(CarStateBase): else cp_cam.vl["CAM_0x2a4"]) # FrogPilot CarState functions + fp_ret.brakeLights = bool(cp.vl["TCS"]["DriverBraking"]) + 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 9c96076ae..8553a3616 100644 --- a/selfdrive/car/subaru/carstate.py +++ b/selfdrive/car/subaru/carstate.py @@ -129,7 +129,10 @@ class CarState(CarStateBase): # FrogPilot CarState functions self.lkas_previously_enabled = self.lkas_enabled if self.car_fingerprint not in PREGLOBAL_CARS: + fp_ret.brakeLights = bool(cp_cam.vl["ES_DashStatus"]["Brake_Lights"]) 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 @@ -202,6 +205,7 @@ class CarState(CarStateBase): messages = [ ("ES_DashStatus", 20), ("ES_Distance", 20), + ("ES_Brake", 20), ] else: messages = [ diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index a9a9e6c96..4faa40019 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -209,6 +209,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_decreased = self.pcm_acc_status == 10 self.cruise_increased_previously = self.cruise_increased diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py index db9c61243..4a6122093 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -154,6 +154,8 @@ class CarState(CarStateBase): self.upscale_lead_car_signal = bool(pt_cp.vl["Kombi_03"]["KBI_Variante"]) # FrogPilot CarState functions + fp_ret.brakeLights = bool(pt_cp.vl["ESP_05"]['ESP_Status_Bremsdruck']) + self.prev_distance_button = self.distance_button self.distance_button = bool(pt_cp.vl["GRA_ACC_01"]["GRA_Verstellung_Zeitluecke"]) @@ -258,6 +260,8 @@ class CarState(CarStateBase): ret.espDisabled = bool(pt_cp.vl["Bremse_1"]["ESP_Passiv_getastet"]) # FrogPilot CarState functions + fp_ret.brakeLights = bool(pt_cp.vl["Motor_2"]['Bremstestschalter']) + self.prev_distance_button = self.distance_button self.distance_button = bool(pt_cp.vl["GRA_Neu"]["GRA_Zeitluecke"]) 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 000000000..9c0ee24d1 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 000000000..754a0bbf7 Binary files /dev/null and b/selfdrive/frogpilot/assets/other_images/gas_pedal.png differ diff --git a/selfdrive/ui/qt/onroad/annotated_camera.cc b/selfdrive/ui/qt/onroad/annotated_camera.cc index 37942a86b..32f44b86e 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.cc +++ b/selfdrive/ui/qt/onroad/annotated_camera.cc @@ -17,8 +17,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); @@ -642,6 +655,12 @@ void AnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &painter, const UISce bottom_layout->setAlignment(distance_btn, (rightHandDM ? Qt::AlignRight : Qt::AlignLeft) | Qt::AlignBottom); } + bool enablePedalIcons = scene.pedals_on_ui; + pedal_icons->setVisible(enablePedalIcons); + if (enablePedalIcons) { + pedal_icons->updateState(scene); + } + reverseCruise = scene.reverse_cruise; speedLimitController = scene.speed_limit_controller; @@ -755,6 +774,56 @@ void Compass::paintEvent(QPaintEvent *event) { } } +PedalIcons::PedalIcons(QWidget *parent) : QWidget(parent) { + 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(const UIScene &scene) { + acceleration = scene.acceleration; + brakeLightOn = scene.brake_lights_on; + dynamicPedals = scene.dynamic_pedals_on_ui; + standstill = scene.standstill; + staticPedals = scene.static_pedals_on_ui; + + accelerating = acceleration > 0.25f; + decelerating = acceleration < -0.25f; + + 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 (dynamicPedals) { + brakeOpacity = standstill ? 1.0f : decelerating ? std::max(0.25f, std::abs(acceleration)) : 0.25f; + gasOpacity = accelerating ? std::max(0.25f, acceleration) : 0.25f; + } else if (staticPedals) { + brakeOpacity = standstill || brakeLightOn || acceleration < -0.5f ? 1.0f : 0.25f; + gasOpacity = !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/annotated_camera.h b/selfdrive/ui/qt/onroad/annotated_camera.h index f8d28b9c4..d36789a61 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.h +++ b/selfdrive/ui/qt/onroad/annotated_camera.h @@ -29,6 +29,29 @@ private: QPixmap staticElements; }; +class PedalIcons : public QWidget { + Q_OBJECT + +public: + explicit PedalIcons(QWidget *parent = 0); + void updateState(const UIScene &scene); + +private: + void paintEvent(QPaintEvent *event) override; + + QPixmap brake_pedal_img; + QPixmap gas_pedal_img; + + bool accelerating; + bool brakeLightOn; + bool decelerating; + bool dynamicPedals; + bool standstill; + bool staticPedals; + + float acceleration; +}; + class AnnotatedCameraWidget : public CameraWidget { Q_OBJECT @@ -76,6 +99,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 8f5c4e8e4..d9e9a11c5 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -222,6 +222,8 @@ 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.reverse = carState.getGearShifter() == cereal::CarState::GearShifter::REVERSE; + scene.standstill = carState.getStandstill() && !scene.reverse; } if (sm.updated("controlsState")) { auto controlsState = sm["controlsState"].getControlsState(); @@ -240,6 +242,7 @@ static void update_state(UIState *s) { } if (sm.updated("frogpilotCarState")) { auto frogpilotCarState = sm["frogpilotCarState"].getFrogpilotCarState(); + scene.brake_lights_on = frogpilotCarState.getBrakeLights(); scene.traffic_mode_active = frogpilotCarState.getTrafficModeActive(); } if (sm.updated("frogpilotPlan")) { @@ -314,6 +317,9 @@ void ui_update_frogpilot_params(UIState *s, Params ¶ms) { 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 22a4043d0..778f49305 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -129,10 +129,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; @@ -142,6 +144,8 @@ typedef struct UIScene { bool online; bool onroad_distance_button; bool parked; + bool pedals_on_ui; + bool reverse; bool reverse_cruise; bool reverse_cruise_ui; bool right_hand_drive; @@ -152,6 +156,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;