From 72fac6ed9da61a7ab41e70ec0551b040facc5ab4 Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Fri, 12 Jan 2024 22:39:30 -0700 Subject: [PATCH] Increase max speed by 5 on short press Added function to increase the value of the max speed by 5 instead of 1 by taping the "Max" speed icon in the onroad UI. --- common/params.cc | 1 + selfdrive/car/toyota/carcontroller.py | 6 ++++-- selfdrive/car/toyota/toyotacan.py | 4 ++-- selfdrive/controls/controlsd.py | 4 +++- selfdrive/controls/lib/drive_helpers.py | 10 ++++----- selfdrive/ui/qt/onroad.cc | 28 +++++++++++++++++++++---- selfdrive/ui/qt/onroad.h | 1 + 7 files changed, 40 insertions(+), 14 deletions(-) diff --git a/common/params.cc b/common/params.cc index a95a8a5..7761ba9 100644 --- a/common/params.cc +++ b/common/params.cc @@ -271,6 +271,7 @@ std::unordered_map keys = { {"PathWidth", PERSISTENT}, {"RelaxedFollow", PERSISTENT}, {"RelaxedJerk", PERSISTENT}, + {"ReverseCruise", PERSISTENT}, {"RoadEdgesWidth", PERSISTENT}, {"ScreenBrightness", PERSISTENT}, {"ShowCPU", PERSISTENT}, diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index bd64d98..880a08e 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -42,8 +42,10 @@ class CarController: self.accel = 0 # FrogPilot variables + self.reverse_cruise_increase = False def update_frogpilot_variables(self, params): + self.reverse_cruise_increase = params.get_bool("ReverseCruise") def update(self, CC, CS, now_nanos, sport_plus): actuators = CC.actuators @@ -149,10 +151,10 @@ class CarController: if pcm_cancel_cmd and self.CP.carFingerprint in UNSUPPORTED_DSU_CAR: can_sends.append(toyotacan.create_acc_cancel_command(self.packer)) elif self.CP.openpilotLongitudinalControl: - can_sends.append(toyotacan.create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, fcw_alert)) + can_sends.append(toyotacan.create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, fcw_alert, self.reverse_cruise_increase)) self.accel = pcm_accel_cmd else: - can_sends.append(toyotacan.create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, False)) + can_sends.append(toyotacan.create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, False, self.reverse_cruise_increase)) if self.frame % 2 == 0 and self.CP.enableGasInterceptor and self.CP.openpilotLongitudinalControl: # send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd. diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index e14e3e5..47a243d 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/selfdrive/car/toyota/toyotacan.py @@ -33,7 +33,7 @@ def create_lta_steer_command(packer, steer_control_type, steer_angle, steer_req, return packer.make_can_msg("STEERING_LTA", 0, values) -def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_type, fcw_alert): +def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_type, fcw_alert, reverse_cruise): # TODO: find the exact canceling bit that does not create a chime values = { "ACCEL_CMD": accel, @@ -43,7 +43,7 @@ def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_ty "PERMIT_BRAKING": 1, "RELEASE_STANDSTILL": not standstill_req, "CANCEL_REQ": pcm_cancel, - "ALLOW_LONG_PRESS": 1, + "ALLOW_LONG_PRESS": 2 if reverse_cruise else 1, "ACC_CUT_IN": fcw_alert, # only shown when ACC enabled } return packer.make_can_msg("ACC_CONTROL", 0, values) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 3300c09..2351f1e 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -513,7 +513,7 @@ class Controls: def state_transition(self, CS): """Compute conditional state transitions and execute actions on state transitions""" - self.v_cruise_helper.update_v_cruise(CS, self.enabled, self.is_metric) + self.v_cruise_helper.update_v_cruise(CS, self.enabled, self.is_metric, self.reverse_cruise_increase) # decrement the soft disable timer at every step, as it's reset on # entrance in SOFT_DISABLING state @@ -980,6 +980,8 @@ class Controls: longitudinal_tune = self.params.get_bool("LongitudinalTune") self.sport_plus = self.params.get_int("AccelerationProfile") == 3 and longitudinal_tune + self.reverse_cruise_increase = self.params.get_bool("ReverseCruise") + def main(): controls = Controls() controls.controlsd_thread() diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 202f34f..0e2862e 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -52,13 +52,13 @@ class VCruiseHelper: def v_cruise_initialized(self): return self.v_cruise_kph != V_CRUISE_UNSET - def update_v_cruise(self, CS, enabled, is_metric): + def update_v_cruise(self, CS, enabled, is_metric, reverse_cruise_increase): self.v_cruise_kph_last = self.v_cruise_kph if CS.cruiseState.available: if not self.CP.pcmCruise: # if stock cruise is completely disabled, then we can use our own set speed logic - self._update_v_cruise_non_pcm(CS, enabled, is_metric) + self._update_v_cruise_non_pcm(CS, enabled, is_metric, reverse_cruise_increase) self.v_cruise_cluster_kph = self.v_cruise_kph self.update_button_timers(CS, enabled) else: @@ -68,13 +68,13 @@ class VCruiseHelper: self.v_cruise_kph = V_CRUISE_UNSET self.v_cruise_cluster_kph = V_CRUISE_UNSET - def _update_v_cruise_non_pcm(self, CS, enabled, is_metric): + def _update_v_cruise_non_pcm(self, CS, enabled, is_metric, reverse_cruise_increase): # handle button presses. TODO: this should be in state_control, but a decelCruise press # would have the effect of both enabling and changing speed is checked after the state transition if not enabled: return - long_press = False + long_press = reverse_cruise_increase button_type = None v_cruise_delta = 1. if is_metric else IMPERIAL_INCREMENT @@ -89,7 +89,7 @@ class VCruiseHelper: for k in self.button_timers.keys(): if self.button_timers[k] and self.button_timers[k] % CRUISE_LONG_PRESS == 0: button_type = k - long_press = True + long_press = not reverse_cruise_increase break if button_type is None: diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index f2870d5..34a2e6d 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -106,13 +106,25 @@ void OnroadWindow::mousePressEvent(QMouseEvent* e) { // FrogPilot clickable widgets bool widgetClicked = false; + // Change cruise control increments button + QRect maxSpeedRect(7, 25, 225, 225); + bool isMaxSpeedClicked = maxSpeedRect.contains(e->pos()); + // Hide speed button QRect speedRect(rect().center().x() - 175, 50, 350, 350); bool isSpeedClicked = speedRect.contains(e->pos()); - if (isSpeedClicked) { - speedHidden = !params.getBool("HideSpeed"); - params.putBoolNonBlocking("HideSpeed", speedHidden); + if (isMaxSpeedClicked || isSpeedClicked) { + // Check if the click was within the max speed area + if (isMaxSpeedClicked) { + reverseCruise = !params.getBool("ReverseCruise"); + params.putBoolNonBlocking("ReverseCruise", reverseCruise); + paramsMemory.putBoolNonBlocking("FrogPilotTogglesUpdated", true); + // Check if the click was within the speed text area + } else { + speedHidden = !params.getBool("HideSpeed"); + params.putBoolNonBlocking("HideSpeed", speedHidden); + } widgetClicked = true; // If the click wasn't for anything specific, change the value of "ExperimentalMode" } else if (scene.experimental_mode_via_press && e->pos() != timeoutPoint) { @@ -500,7 +512,11 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) { int bottom_radius = has_eu_speed_limit ? 100 : 32; QRect set_speed_rect(QPoint(60 + (default_size.width() - set_speed_size.width()) / 2, 45), set_speed_size); - p.setPen(QPen(whiteColor(75), 6)); + if (reverseCruise) { + p.setPen(QPen(QColor(0, 150, 255), 6)); + } else { + p.setPen(QPen(whiteColor(75), 6)); + } p.setBrush(blackColor(166)); drawRoundedRect(p, set_speed_rect, top_radius, top_radius, bottom_radius, bottom_radius); @@ -1034,6 +1050,10 @@ void AnnotatedCameraWidget::initializeFrogPilotWidgets() { speedHidden = true; } + if (params.getBool("ReverseCruise")) { + reverseCruise = true; + } + // Custom themes configuration themeConfiguration = { {1, {QString("frog_theme"), {QColor(23, 134, 68, 242), {{0.0, QBrush(QColor::fromHslF(144 / 360., 0.71, 0.31, 0.9))}, diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index 70be466..1f7b462 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -16,6 +16,7 @@ const int btn_size = 192; const int img_size = (btn_size / 4) * 3; // FrogPilot global variables +static bool reverseCruise; static bool speedHidden; static double fps;