From 42b3636995b77733e5e3757df3fce7f01ebbf57d 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] Always on Lateral Added toggle for Always On Lateral / No disengage lateral on gas and brake. Lots of credit goes to "pfeiferj"! Couldn't of done it without him! https: //github.com/pfeiferj/openpilot Co-Authored-By: Jacob Pfeifer --- cereal/custom.capnp | 1 + common/params.cc | 2 ++ panda/board/safety.h | 19 ++++++++++++---- panda/board/safety/safety_chrysler.h | 1 + panda/board/safety/safety_ford.h | 1 + panda/board/safety/safety_gm.h | 4 ++++ panda/board/safety/safety_honda.h | 3 ++- panda/board/safety/safety_hyundai_common.h | 4 ++++ panda/board/safety/safety_mazda.h | 1 + panda/board/safety/safety_nissan.h | 7 ++++++ panda/board/safety/safety_subaru.h | 1 + panda/board/safety/safety_subaru_preglobal.h | 1 + panda/board/safety/safety_tesla.h | 8 +++++++ panda/board/safety/safety_toyota.h | 5 +++++ panda/board/safety_declarations.h | 4 ++++ panda/python/__init__.py | 1 + selfdrive/car/hyundai/carcontroller.py | 2 +- selfdrive/car/hyundai/carstate.py | 10 +++++++-- selfdrive/car/hyundai/hyundaican.py | 4 ++-- selfdrive/car/interfaces.py | 2 ++ selfdrive/controls/controlsd.py | 21 +++++++++++++++++- .../toggle_icons/icon_always_on_lateral.png | Bin 0 -> 9723 bytes selfdrive/frogpilot/ui/control_settings.cc | 21 ++++++++++++++++-- selfdrive/ui/qt/maps/map.cc | 2 +- selfdrive/ui/qt/onroad.cc | 13 +++++++---- selfdrive/ui/qt/onroad.h | 1 + selfdrive/ui/ui.cc | 7 ++++++ selfdrive/ui/ui.h | 4 ++++ 28 files changed, 132 insertions(+), 18 deletions(-) create mode 100644 selfdrive/frogpilot/assets/toggle_icons/icon_always_on_lateral.png diff --git a/cereal/custom.capnp b/cereal/custom.capnp index d76b77e..4d71d8c 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -9,6 +9,7 @@ $Cxx.namespace("cereal"); # you can rename the struct, but don't change the identifier struct FrogPilotCarControl @0x81c2f05a394cf4af { + alwaysOnLateral @0: Bool; } struct FrogPilotDeviceState @0xaedffd8f31e7b55d { diff --git a/common/params.cc b/common/params.cc index 3b5dd72..551d22f 100644 --- a/common/params.cc +++ b/common/params.cc @@ -214,6 +214,8 @@ std::unordered_map keys = { // FrogPilot parameters {"AccelerationProfile", PERSISTENT}, {"AggressiveAcceleration", PERSISTENT}, + {"AlwaysOnLateral", PERSISTENT}, + {"AlwaysOnLateralMain", PERSISTENT}, {"ApiCache_DriveStats", PERSISTENT}, {"FrogPilotTogglesUpdated", PERSISTENT}, {"GasRegenCmd", PERSISTENT}, diff --git a/panda/board/safety.h b/panda/board/safety.h index abbd51d..7b60b11 100644 --- a/panda/board/safety.h +++ b/panda/board/safety.h @@ -556,7 +556,13 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const SteeringLi bool violation = false; uint32_t ts = microsecond_timer_get(); + bool aol_allowed = acc_main_on && (alternative_experience & ALT_EXP_ALWAYS_ON_LATERAL); if (controls_allowed) { + // acc main must be on if controls are allowed + acc_main_on = controls_allowed; + } + + if (controls_allowed || aol_allowed) { // *** global torque limit check *** violation |= max_limit_check(desired_torque, limits.max_steer, -limits.max_steer); @@ -583,7 +589,7 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const SteeringLi } // no torque if controls is not allowed - if (!controls_allowed && (desired_torque != 0)) { + if (!(controls_allowed || aol_allowed) && (desired_torque != 0)) { violation = true; } @@ -625,7 +631,7 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const SteeringLi } // reset to 0 if either controls is not allowed or there's a violation - if (violation || !controls_allowed) { + if (violation || !(controls_allowed || aol_allowed)) { valid_steer_req_count = 0; invalid_steer_req_count = 0; desired_torque_last = 0; @@ -640,8 +646,13 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const SteeringLi // Safety checks for angle-based steering commands bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const SteeringLimits limits) { bool violation = false; + bool aol_allowed = acc_main_on && (alternative_experience & ALT_EXP_ALWAYS_ON_LATERAL); + if (controls_allowed) { + // acc main must be on if controls are allowed + acc_main_on = controls_allowed; + } - if (controls_allowed && steer_control_enabled) { + if ((controls_allowed || aol_allowed) && steer_control_enabled) { // convert floating point angle rate limits to integers in the scale of the desired angle on CAN, // add 1 to not false trigger the violation. also fudge the speed by 1 m/s so rate limits are // always slightly above openpilot's in case we read an updated speed in between angle commands @@ -684,7 +695,7 @@ bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const } // No angle control allowed when controls are not allowed - violation |= !controls_allowed && steer_control_enabled; + violation |= !(controls_allowed || aol_allowed) && steer_control_enabled; return violation; } diff --git a/panda/board/safety/safety_chrysler.h b/panda/board/safety/safety_chrysler.h index 7fb237c..c50b3e7 100644 --- a/panda/board/safety/safety_chrysler.h +++ b/panda/board/safety/safety_chrysler.h @@ -185,6 +185,7 @@ static void chrysler_rx_hook(CANPacket_t *to_push) { // enter controls on rising edge of ACC, exit controls on ACC off const int das_3_bus = (chrysler_platform == CHRYSLER_PACIFICA) ? 0 : 2; if ((bus == das_3_bus) && (addr == chrysler_addrs->DAS_3)) { + acc_main_on = GET_BIT(to_push, 20U) == 1U; bool cruise_engaged = GET_BIT(to_push, 21U) == 1U; pcm_cruise_check(cruise_engaged); } diff --git a/panda/board/safety/safety_ford.h b/panda/board/safety/safety_ford.h index f6c7a29..ee4450c 100644 --- a/panda/board/safety/safety_ford.h +++ b/panda/board/safety/safety_ford.h @@ -250,6 +250,7 @@ static void ford_rx_hook(CANPacket_t *to_push) { // Signal: CcStat_D_Actl unsigned int cruise_state = GET_BYTE(to_push, 1) & 0x07U; + acc_main_on = (cruise_state == 3U) ||(cruise_state == 4U) || (cruise_state == 5U); bool cruise_engaged = (cruise_state == 4U) || (cruise_state == 5U); pcm_cruise_check(cruise_engaged); } diff --git a/panda/board/safety/safety_gm.h b/panda/board/safety/safety_gm.h index 2114cfa..f889862 100644 --- a/panda/board/safety/safety_gm.h +++ b/panda/board/safety/safety_gm.h @@ -127,6 +127,10 @@ static void gm_rx_hook(CANPacket_t *to_push) { brake_pressed = GET_BIT(to_push, 40U) != 0U; } + if (addr == 0xC9) { + acc_main_on = GET_BIT(to_push, 29U) != 0U; + } + if (addr == 0x1C4) { gas_pressed = GET_BYTE(to_push, 5) != 0U; diff --git a/panda/board/safety/safety_honda.h b/panda/board/safety/safety_honda.h index c8071af..cbb3ee2 100644 --- a/panda/board/safety/safety_honda.h +++ b/panda/board/safety/safety_honda.h @@ -331,7 +331,8 @@ static bool honda_tx_hook(CANPacket_t *to_send) { // STEER: safety check if ((addr == 0xE4) || (addr == 0x194)) { - if (!controls_allowed) { + bool aol_allowed = acc_main_on && (alternative_experience & ALT_EXP_DISABLE_DISENGAGE_ON_GAS); + if (!(controls_allowed || aol_allowed)) { bool steer_applied = GET_BYTE(to_send, 0) | GET_BYTE(to_send, 1); if (steer_applied) { tx = false; diff --git a/panda/board/safety/safety_hyundai_common.h b/panda/board/safety/safety_hyundai_common.h index 0b9116d..6d05a66 100644 --- a/panda/board/safety/safety_hyundai_common.h +++ b/panda/board/safety/safety_hyundai_common.h @@ -63,6 +63,10 @@ void hyundai_common_cruise_state_check(const int cruise_engaged) { } void hyundai_common_cruise_buttons_check(const int cruise_button, const int main_button) { + if (main_button != 0 && main_button != cruise_main_prev) { + acc_main_on = !acc_main_on; + } + cruise_main_prev = main_button; if ((cruise_button == HYUNDAI_BTN_RESUME) || (cruise_button == HYUNDAI_BTN_SET) || (cruise_button == HYUNDAI_BTN_CANCEL) || (main_button != 0)) { hyundai_last_button_interaction = 0U; diff --git a/panda/board/safety/safety_mazda.h b/panda/board/safety/safety_mazda.h index 67561a1..e4efb9a 100644 --- a/panda/board/safety/safety_mazda.h +++ b/panda/board/safety/safety_mazda.h @@ -52,6 +52,7 @@ static void mazda_rx_hook(CANPacket_t *to_push) { // enter controls on rising edge of ACC, exit controls on ACC off if (addr == MAZDA_CRZ_CTRL) { + acc_main_on = GET_BIT(to_push, 17U); bool cruise_engaged = GET_BYTE(to_push, 0) & 0x8U; pcm_cruise_check(cruise_engaged); } diff --git a/panda/board/safety/safety_nissan.h b/panda/board/safety/safety_nissan.h index 91b52b7..a8537c4 100644 --- a/panda/board/safety/safety_nissan.h +++ b/panda/board/safety/safety_nissan.h @@ -21,6 +21,8 @@ const CanMsg NISSAN_TX_MSGS[] = { // Signals duplicated below due to the fact that these messages can come in on either CAN bus, depending on car model. RxCheck nissan_rx_checks[] = { + {.msg = {{0x1b6, 0, 8, .frequency = 100U}, + {0x1b6, 1, 8, .frequency = 100U}, { 0 }}}, // PRO_PILOT (100HZ) {.msg = {{0x2, 0, 5, .frequency = 100U}, {0x2, 1, 5, .frequency = 100U}, { 0 }}}, // STEER_ANGLE_SENSOR {.msg = {{0x285, 0, 8, .frequency = 50U}, @@ -64,11 +66,16 @@ static void nissan_rx_hook(CANPacket_t *to_push) { UPDATE_VEHICLE_SPEED((right_rear + left_rear) / 2.0 * 0.005 / 3.6); } + if (addr == 0x1b6) { + acc_main_on = GET_BIT(to_push, 36U); + } + // X-Trail 0x15c, Leaf 0x239 if ((addr == 0x15c) || (addr == 0x239)) { if (addr == 0x15c){ gas_pressed = ((GET_BYTE(to_push, 5) << 2) | ((GET_BYTE(to_push, 6) >> 6) & 0x3U)) > 3U; } else { + acc_main_on = GET_BIT(to_push, 17U); gas_pressed = GET_BYTE(to_push, 0) > 3U; } } diff --git a/panda/board/safety/safety_subaru.h b/panda/board/safety/safety_subaru.h index 12868d4..3592f0c 100644 --- a/panda/board/safety/safety_subaru.h +++ b/panda/board/safety/safety_subaru.h @@ -152,6 +152,7 @@ static void subaru_rx_hook(CANPacket_t *to_push) { // enter controls on rising edge of ACC, exit controls on ACC off if ((addr == MSG_SUBARU_CruiseControl) && (bus == alt_main_bus)) { + acc_main_on = GET_BIT(to_push, 40U) != 0U; bool cruise_engaged = GET_BIT(to_push, 41U) != 0U; pcm_cruise_check(cruise_engaged); } diff --git a/panda/board/safety/safety_subaru_preglobal.h b/panda/board/safety/safety_subaru_preglobal.h index 16a1688..b587734 100644 --- a/panda/board/safety/safety_subaru_preglobal.h +++ b/panda/board/safety/safety_subaru_preglobal.h @@ -56,6 +56,7 @@ static void subaru_preglobal_rx_hook(CANPacket_t *to_push) { // enter controls on rising edge of ACC, exit controls on ACC off if (addr == MSG_SUBARU_PG_CruiseControl) { + acc_main_on = GET_BIT(to_push, 48U) != 0U; bool cruise_engaged = GET_BIT(to_push, 49U) != 0U; pcm_cruise_check(cruise_engaged); } diff --git a/panda/board/safety/safety_tesla.h b/panda/board/safety/safety_tesla.h index 66d1196..4b4bed2 100644 --- a/panda/board/safety/safety_tesla.h +++ b/panda/board/safety/safety_tesla.h @@ -88,6 +88,14 @@ static void tesla_rx_hook(CANPacket_t *to_push) { if(addr == (tesla_powertrain ? 0x256 : 0x368)) { // Cruise state int cruise_state = (GET_BYTE(to_push, 1) >> 4); + + acc_main_on = (cruise_state == 1) || // STANDBY + (cruise_state == 2) || // ENABLED + (cruise_state == 3) || // STANDSTILL + (cruise_state == 4) || // OVERRIDE + (cruise_state == 6) || // PRE_FAULT + (cruise_state == 7); // PRE_CANCEL + bool cruise_engaged = (cruise_state == 2) || // ENABLED (cruise_state == 3) || // STANDSTILL (cruise_state == 4) || // OVERRIDE diff --git a/panda/board/safety/safety_toyota.h b/panda/board/safety/safety_toyota.h index 4f33e8f..f025a5b 100644 --- a/panda/board/safety/safety_toyota.h +++ b/panda/board/safety/safety_toyota.h @@ -47,6 +47,7 @@ const int TOYOTA_GAS_INTERCEPTOR_THRSLD = 805; {0x283, 0, 7}, {0x2E6, 0, 8}, {0x2E7, 0, 8}, {0x33E, 0, 7}, {0x344, 0, 8}, {0x365, 0, 7}, {0x366, 0, 7}, {0x4CB, 0, 8}, /* DSU bus 0 */ \ {0x128, 1, 6}, {0x141, 1, 4}, {0x160, 1, 8}, {0x161, 1, 7}, {0x470, 1, 4}, /* DSU bus 1 */ \ {0x2E4, 0, 5}, {0x191, 0, 8}, {0x411, 0, 8}, {0x412, 0, 8}, {0x343, 0, 8}, {0x1D2, 0, 8}, /* LKAS + ACC */ \ + {0x1D3, 0, 8}, \ const CanMsg TOYOTA_TX_MSGS[] = { TOYOTA_COMMON_TX_MSGS @@ -61,6 +62,7 @@ const CanMsg TOYOTA_INTERCEPTOR_TX_MSGS[] = { {.msg = {{ 0xaa, 0, 8, .check_checksum = false, .frequency = 83U}, { 0 }, { 0 }}}, \ {.msg = {{0x260, 0, 8, .check_checksum = true, .quality_flag = (lta), .frequency = 50U}, { 0 }, { 0 }}}, \ {.msg = {{0x1D2, 0, 8, .check_checksum = true, .frequency = 33U}, { 0 }, { 0 }}}, \ + {.msg = {{0x1D3, 0, 8, .check_checksum = true, .frequency = 33U}, { 0 }, { 0 }}}, \ {.msg = {{0x224, 0, 8, .check_checksum = false, .frequency = 40U}, \ {0x226, 0, 8, .check_checksum = false, .frequency = 40U}, { 0 }}}, \ @@ -166,6 +168,9 @@ static void toyota_rx_hook(CANPacket_t *to_push) { update_sample(&angle_meas, angle_meas_new); } } + if (addr == 0x1D3) { + acc_main_on = GET_BIT(to_push, 15U); + } // enter controls on rising edge of ACC, exit controls on ACC off // exit controls on rising edge of gas press diff --git a/panda/board/safety_declarations.h b/panda/board/safety_declarations.h index 5c3eebe..11250be 100644 --- a/panda/board/safety_declarations.h +++ b/panda/board/safety_declarations.h @@ -227,6 +227,7 @@ struct sample_t vehicle_speed; bool vehicle_moving = false; bool acc_main_on = false; // referred to as "ACC off" in ISO 15622:2018 int cruise_button_prev = 0; +int cruise_main_prev = 0; bool safety_rx_checks_invalid = false; // for safety modes with torque steering control @@ -269,3 +270,6 @@ int alternative_experience = 0; uint32_t safety_mode_cnt = 0U; // allow 1s of transition timeout after relay changes state before assessing malfunctioning const uint32_t RELAY_TRNS_TIMEOUT = 1U; + +// Always on Lateral +#define ALT_EXP_ALWAYS_ON_LATERAL 16 diff --git a/panda/python/__init__.py b/panda/python/__init__.py index 80aee45..eb5527c 100644 --- a/panda/python/__init__.py +++ b/panda/python/__init__.py @@ -111,6 +111,7 @@ class ALTERNATIVE_EXPERIENCE: DISABLE_DISENGAGE_ON_GAS = 1 DISABLE_STOCK_AEB = 2 RAISE_LONGITUDINAL_LIMITS_TO_ISO_MAX = 8 + ALWAYS_ON_LATERAL = 16 class Panda: diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index e956678..0c6caf0 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -151,7 +151,7 @@ class CarController: use_fca = self.CP.flags & HyundaiFlags.USE_FCA.value can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled, accel, jerk, int(self.frame / 2), hud_control.leadVisible, set_speed_in_units, stopping, - CC.cruiseControl.override, use_fca)) + CC.cruiseControl.override, use_fca, CS.out.cruiseState.available)) # 20 Hz LFA MFA message if self.frame % 5 == 0 and self.CP.flags & HyundaiFlags.SEND_LFA.value: diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 2f1fb10..55c0d7f 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -102,7 +102,7 @@ class CarState(CarStateBase): # cruise state if self.CP.openpilotLongitudinalControl: # These are not used for engage/disengage since openpilot keeps track of state using the buttons - ret.cruiseState.available = cp.vl["TCS13"]["ACCEnable"] == 0 + ret.cruiseState.available = self.main_enabled ret.cruiseState.enabled = cp.vl["TCS13"]["ACC_REQ"] == 1 ret.cruiseState.standstill = False ret.cruiseState.nonAdaptive = False @@ -162,7 +162,10 @@ class CarState(CarStateBase): self.steer_state = cp.vl["MDPS12"]["CF_Mdps_ToiActive"] # 0 NOT ACTIVE, 1 ACTIVE self.prev_cruise_buttons = self.cruise_buttons[-1] self.cruise_buttons.extend(cp.vl_all["CLU11"]["CF_Clu_CruiseSwState"]) + self.prev_main_buttons = self.main_buttons[-1] self.main_buttons.extend(cp.vl_all["CLU11"]["CF_Clu_CruiseSwMain"]) + if self.prev_main_buttons == 0 and self.main_buttons[-1] != 0: + self.main_enabled = not self.main_enabled return ret @@ -217,7 +220,7 @@ class CarState(CarStateBase): # cruise state # CAN FD cars enable on main button press, set available if no TCS faults preventing engagement - ret.cruiseState.available = cp.vl["TCS"]["ACCEnable"] == 0 + ret.cruiseState.available = self.main_enabled if self.CP.openpilotLongitudinalControl: # These are not used for engage/disengage since openpilot keeps track of state using the buttons ret.cruiseState.enabled = cp.vl["TCS"]["ACC_REQ"] == 1 @@ -238,7 +241,10 @@ class CarState(CarStateBase): self.prev_cruise_buttons = self.cruise_buttons[-1] self.cruise_buttons.extend(cp.vl_all[self.cruise_btns_msg_canfd]["CRUISE_BUTTONS"]) + self.prev_main_buttons = self.main_buttons[-1] self.main_buttons.extend(cp.vl_all[self.cruise_btns_msg_canfd]["ADAPTIVE_CRUISE_MAIN_BTN"]) + if self.prev_main_buttons == 0 and self.main_buttons[-1] != 0: + self.main_enabled = not self.main_enabled self.buttons_counter = cp.vl[self.cruise_btns_msg_canfd]["COUNTER"] ret.accFaulted = cp.vl["TCS"]["ACCEnable"] != 0 # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED diff --git a/selfdrive/car/hyundai/hyundaican.py b/selfdrive/car/hyundai/hyundaican.py index bc29aeb..dcc7adf 100644 --- a/selfdrive/car/hyundai/hyundaican.py +++ b/selfdrive/car/hyundai/hyundaican.py @@ -126,11 +126,11 @@ def create_lfahda_mfc(packer, enabled, hda_set_speed=0): } return packer.make_can_msg("LFAHDA_MFC", 0, values) -def create_acc_commands(packer, enabled, accel, upper_jerk, idx, lead_visible, set_speed, stopping, long_override, use_fca): +def create_acc_commands(packer, enabled, accel, upper_jerk, idx, lead_visible, set_speed, stopping, long_override, use_fca, cruise_available): commands = [] scc11_values = { - "MainMode_ACC": 1, + "MainMode_ACC": 1 if cruise_available else 0, "TauGapSet": 4, "VSetDis": set_speed if enabled else 0, "AliveCounterACC": idx % 0x10, diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 070a6f0..fca337e 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -360,6 +360,8 @@ class CarStateBase(ABC): self.param = Params() self.param_memory = Params("/dev/shm/params") + self.main_enabled = False + def update_speed_kf(self, v_ego_raw): if abs(v_ego_raw - self.v_ego_kf.x[0][0]) > 2.0: # Prevent large accelerations when car starts at non zero speed self.v_ego_kf.set_x([[v_ego_raw], [0.0]]) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index e6ba4fc..737223a 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -108,6 +108,16 @@ class Controls: if not self.disengage_on_accelerator: self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS + # Set "Always On Lateral" conditions + self.always_on_lateral = self.params.get_bool("AlwaysOnLateral") + self.always_on_lateral_main = self.params.get_bool("AlwaysOnLateralMain") + self.lateral_allowed = False + if self.always_on_lateral: + self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL + if self.disengage_on_accelerator: + self.disengage_on_accelerator = False + self.params.put_bool("DisengageOnAccelerator", False) + self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.RAISE_LONGITUDINAL_LIMITS_TO_ISO_MAX # read params @@ -596,9 +606,18 @@ class Controls: gear = car.CarState.GearShifter driving_gear = CS.gearShifter not in (gear.neutral, gear.park, gear.reverse, gear.unknown) + # Always on lateral + if self.always_on_lateral: + self.lateral_allowed &= CS.cruiseState.available + self.lateral_allowed |= CS.cruiseState.enabled or (CS.cruiseState.available and self.always_on_lateral_main) + + self.FPCC.alwaysOnLateral = self.lateral_allowed and driving_gear + if self.FPCC.alwaysOnLateral: + self.current_alert_types.append(ET.WARNING) + # Check which actuators can be enabled standstill = CS.vEgo <= max(self.CP.minSteerSpeed, MIN_LATERAL_CONTROL_SPEED) or CS.standstill - CC.latActive = self.active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \ + CC.latActive = (self.active or self.FPCC.alwaysOnLateral) and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \ (not standstill or self.joystick_mode) CC.longActive = self.enabled and not self.events.contains(ET.OVERRIDE_LONGITUDINAL) and self.CP.openpilotLongitudinalControl diff --git a/selfdrive/frogpilot/assets/toggle_icons/icon_always_on_lateral.png b/selfdrive/frogpilot/assets/toggle_icons/icon_always_on_lateral.png new file mode 100644 index 0000000000000000000000000000000000000000..1e55e3feb958850c75f576a8854224c2becc4cee GIT binary patch literal 9723 zcmZ{qWl$VV*M=eJqG5prf&^I>3GVK)XmEE8?yif=LKb&RuoxEH-7P_aI|SDUx0lrS z`|GNnuBn>po>MhFr_X)ev1+RFxL9wnkdTmY6`(R2FXQ09j*0$q{h?Qp|1uzZXvj+; z&kwPLyxag>q52+3NZ5q`IxMJZPS|F~ppx$dnPC?6TDg1ujR@79o2$^>*!!lSn$r?t{+#QKYHO&jUWD;@IS7b$a;JXy!xx@Zd3Ve;85B2=T1DFn*RTqz9%`i&FG$s z`O1LfT$TAI-;UBK3i$v9X_|I?Nf9S>8&`rQ*c64T2lm>c<_V3;7)+6#sXQJ z|K)xVa(IKl%D(U7KohB!Z`%BXgw7z&e9%Wz#anej>)}+7?3eb|_O$-$aqS@NXwh=C z+3O%1@;*P1@9t$5O~sV)y1F_KZ_WXz1xr4W!q53=p^hJiLckSqa;tDA zqoOi)n4X@FQO$n+C;>?e;S`Z$y{Wm&uT1sbeQ@4k>c8&MwHFzh{_}?)^~-)Ei=g>@ zjrH3H;>~->cU~rD73N{n0{S3N{boV>Y3m)a9W13_x!f&)afI2 zny|p6>F{Z$?oIxGM1XNiJSvf!h!c#mHe|Z}TDz&zyZq zeDN31*V@QE!`i%GhZ3}oc7_W=8;pa=cQ37; z!5mZIHEUdSCR{jJaa60qsL6e1^(7Wbwr4Zfr=}GBR^PK^GHx5?lRuTFA<>(YcDs{B zF1{Ej7`<&ccw_AYG#MS7dP|abys;R$yldS@0UO9mLmgdf@f=?J(;oV*Z0!RWYdEfF zeI)El-PCi#*8;Gh(L%{^^gF7Y8gxV8YGOn*nTW5eHbAjgkI!llq3Ng8Kx!ybaQ5yL zo-gckEV8<(p6J=VvizsFJO7+uho%2oBJ+3I`vR(X5bM`_u1$yd<}7n{p*yPh8ne^2 zE^T~nxA(MH<7)aFg%qiKPhMBfnx7M%zWb(RZydiD3}AT4*il( zo(6^l|K@UUJMR=I8(HG|q$yij#xb8*oq|3()Vz*sHtPzzUGl_O4Y~%TQopr+>h0w` z79boe5D$T!1ZNOUV)YT<;~EFOIhjl+%-Y%6xpZS88oeA&d#8JI?7=i?zF6;6;4MnC zhN0Zd5!YjyDh8z|ioS9W&!QOhQrDQVnGY|WEmcp#1LrAFJ6GBi<8r1`UvuB zgUc%adh&Y9XKNYxTrwl)77p7bRi9PLOBLOWcodToU5<8P{fe~`lmP|Ziwu46(UsEg z`R#fvF%3H-DHg2?y=n_V3SQ$;OPiLN<8d(MjYRuEU+BhnH%h|d=uG9JMemp z$;--Rfg9elcstSz-mo$Wnv(`)YzcA*4jC1G&2f1&aQ*73sP(7 z$dGpXln+@BiWcUStkecg?v6Y0kLa~+3T5sZaP|KH{#(6gs!rU>NN^gVgSDZwfHzXVTKcwed~1)Nx0u@$0@19u?e4u_f3RgHCucEP7zB>Ith;*QA>T z&#A}zVhIa3>hTZ~QwCw@iqA(|!GpUSdN%yjr~S5JX!Ek4uwwa-y#lS@&2Kn*;im17 zU>erSdqm+Djg<*H*QboJ19+@UGFeNw0AKu!Sb*)UhXUrZK2;wSX-5=5={{7ZxAs0P zHwNx4Yb~7vzPKRE}gs?(&hr-qAnh9eKc_h{D_TazkRWqCA#7;@717Rxy}dQ{X+rCtus>TUgVT$Yf`llr71C)$r~p5Zf*p z1W22B)s!4pAl}_c>vnZIuebn1^{ud)epScf=y}+H^H6_&QjjVuWnCqq*IyBsRjyaP z?CuVcTv?Nb!yE^o$eam(ek`wBN1rSNJPYXM7g&pF>j;#> zl(Qs(c!%W>hXpcfLe8ClB(+%f*TDT{N>CK;B6oRc-kd^VVKp_~%u8ZuM7)n=<5Ogn zCZU3s`kpmG%l{)8gaXsu4VP4$S&k_kGqKf5*-rHbmeUOynw$Mg6stlAC;|Wd4QL?XEIKNshKwXz*9+>0jvTH~)9M*YaM4{|g+bfn)uOmH^W1m+pI@CuX3(c5D3e3hCB5kj4r%?OBo*JJ z1!&r_AsWN~n3a4K8QDX^kePDF#=NX!{p0qMNpYh}2V@5CNz0_r`$zy!N2!{sWMBPF znXA#HXb%()>GR22WlUXK#B=yv+Q(!V8>fXY|1%!Q#EhvNxH;P#5G3S?mWoFWC-Qz; z580RKeh)}NiWX@UhELol{P&7xefG>B$a#o=>V;wIQbtPn%gvA%a$${xcj&Wlj6WV% zYMM0jO}Da|RX1I6`kI?y{FZ_3W0kB%c{Xz_h9G}F+LIjnxpv+K`Ind1nE)N)z2ABi zu77(f7DNA$qhZQ|%QT95?E9tiy$=^e%6cjLuw|f-_WNE;7wby1ne3o3I4e%p2|fCG zFLg8`3P$=LLDuRJ8zMHaO>K=|d~?Ll{4Fq8S2b>d(M*&Lh9MH6^%nP21`$mEDWgXOjbL09*IQCQ0%HNGb-v|?C2W-VHtFhtC30P_a_BxBx5O$YdQiqMy- zBFuUNX~*jm3NwW94)NsS9u@u=~d2A&1Z1C z*-G-&cG0#230erUO%^6!g72H{*gzl-DSRk_>gP za6izFEVS@C%zo!3z?4TftnuXdLB!*{)OgmHbA}y1pA```WUpOJaT>V*O(ZnAcx0!2 zj=l&~lNqp=Bbwt@c64Xwg5f%W!d6w}UQT?00voUh)-7O72eeAybgkKh0y?<%y|$i) zWbwGtF5yZc1(IjS5@9!q73Xb08b3#UAHTJ`5uHKph*LX@m{aax7Et<*?72kpLYf?s(;)OYIOcGJX_oaVbSJRc*;$u8tVnLOn+5?u(U&N81h=XX zpR=7(ik)vCzqR+I4azkBJGLw&i)*$Y$=Y2=_nQ}y53Fu?NYu27MeN`INpa0(Ar zdz30T^Ev+VTs-=Z`Sapw25C|BJ~R{2dld!zW< z^%2I+XzTBNhJze#8@&KXHy)*s))U$tsK1z#?inQ8*mQBL@xvjyYCb4%x%ZDg(a$3( zbYMJ{X-~_|9niGl!VAq51?!=mfBp-VH43Q~d%Q=h+Ulf?S;Uzlf7BxF) zbU+TO#zCfycQg#23gs!w#~8Q%7D2ZxM~tfy0f(Z#y$t; zk*;E5y>_U0SEW&cO!)S@JZZG0D~|KNidDP7c^8u9H*ZSQ8!O>6nvMIbO$RK~Dqbi0 zvq}81`!NKj;JN`P!&bXMEgjJeBWohfpp}K_WjPL{h)glDZjp33Y6HqTJm$GRqG)`D z8Qk@UYOSE(7&E2|70kykB!>bGCgtkPVgVs&(q^KK^$Wufj6tK_N{4d8XW zf+#&h8aO&{W0z^Cr0EE7SaC^@dO*htPxR|}cU8EFJylu{h_%QQ^M#A#iqP_8TwK}!zesIvl)S#xiFi29Ze1qz+$>*f@LbE@M1tJv}lwFzXFx;k)WVi&72)u^|(Be*LJ?Se8Z}<4(^q;xXipz?GlkrXw5oyMC zOCRt^T4kB{piDwR6_H`I}%yA6VvrAMPm~V&zGmEf#dg6K#oLWxBdRl(G-s z;?^2fZO`eTM7!DL2f9X1tnP^BUFWn<*H__!{+n)HZ8O%0Vxrmf%vUHQO8SE&h)O@} zDIB^q^&2;fh$&0N?|dF_fcpOQ1l#ToX}h=qTm*?6{;A!-p3L)i$cH($Qj)D)SR?6z z{H9qn`IIGl)AY@C?0k_B2V)teMmadSrB zWqm^&2yV$nEMHiyMqN-4L*Wv%He33U0Y&(eK-zK2__y_M!NJ%EB;y9L3OJ;TuQ=x| z`jYmeBUp>o;zC41i%dCzZ#YuvFZSZrv2IEtn=?{gc2qS5-Xn)ea8v{q87RmXtFr++ zEQ2?M*;4h~+R&sTnn-*Q?6-Y$c$p`ZU;To)36~s^#rT1>yWZeNs&kLS zvqgTR@FNPx)lJBsBZqE@f?sb-7EZD)Kc4?qjYrz!cqz0SUY1SH{?n@_;Ih)%gf~!# z3kIS*G=&Pdio=@uc?{qYW2gb1zOeM3dTiT)N<@`n->t? zE7m+o+a(DcO8G|nL`6Th7J4JCXmUnIiFe_}9cvIdoR$Y= zZ`oUHd~dYqi%a7ZW<`lT0&R?c9w*lOFQ$W$^ayHEv?f-i-pDop!f*|6*>=EJ6`{$5OH+8en8WXm+WgUkrSDjwrAA@f3q!Un;mAr(~L4R4CS}l zI8F>S;1mAwDmGep{{hv3cjbE$RT-Fyy0;fB_&1e#F4%kout{?$rRLgM$aIh#o-u)@ zzFEp?sv{cGz|kE3dF$92$WiC;r($CMu$CFT5+}|~3o+7On*))>tI`sA3#JwHE0HkD ze775B+;FJqF_CMqR7BQ<&@?DO-^hQWKmxvz-;VDtqQDDxP5C&wD$~u2YD?4_l#*`3 z;9T`sb8ePmtul8_wyesOCdu*TF)G3Xk_UFn?-V1GWv}L+RSZ_{Cixb%AcPi$ti;2- zB&R9@K%u`n+%BflL9_j7&^GVHosUM$Ae5QOkebYwS^PKzUjHpLnuHnEosRBe?+J1~ z5~n7J@U%Y@j#%*!i5+aUh<0vFpcAQw#mxbsq)cbqexVToN&%L6<3#R1Dk1byQ_z8H`ujH{JL=`haI% zeUnT)J(E&3EY1BN062T^J{dUV`f>ju3Qp;bHH)`*O3S~8(Aa3~Ct|<5&TJBb@R@lvwKEe0xvMD@5l!hxoJT^v-;=ypxMJGw%L+l&up{7pn zne3vf_V#wk25#4JTrG`{uTr*V$oZWg5cU`fZBp-ARn^zE1jm?STeZx;_+Y`5w|$Z$ zP6z<}psiMaL~@qa0{ZpWM881+{W=)6d9$-^B8A|2u6wwtr6;+got>Xro#^idvrQVA zZj^0WK5#oD=0ut?qh6{S>|<&pe=2HK8TEgc)?@AT#GBKqR`YSep}≻}>t1hIjeO zw{Fs4qxdaqi`^(lOu}iYsNh3NM!Hj+p<64-a4-B=#M~!Bm2M{#+cyGg>EYQo?lZTz zWP~}Uh+$pQ83F1?EiyGnVf`lFCYw4rylO|1{qeIrUAo649p=0;`aBVLj(I z?dVLyVJP656bWJagE+U$2=aI>IBHvZo2Yn;!f#2f;-Afbg;NV(dl!jgJJui;Q>TV*_i;OA^hJ=U zg*!H&Get?&V;J}6i|$8a_(<0D&7axu@{$qBCoaMG+TS?rH^71q~O?FGp( zD$zmG{>cix3Dhja&KFh9B!-=OjIKixz?O9_EA`|X3-g>%kJs>Gg}O2`9QK(k-Ds_~ z(Qk`IL2&O8h?u_FKT*na5pq^95B!jNUejh8jYS@tNbre?@YYVyF^pwF9HNr#S*j5IPXUi_&_GI2_A0EdK4YFj zrarrhqKHZN^@1BNwf#>lxD#eD`>*^Fh^ zK`66D1H2QwH&0zS6_8O4<9@)_7QGm%(O|-HN2hwNU(`vcBxMv`_<_t80zy=fCT>)` zM`38=Hi=VwuOM1can;j3+`Bh(z(7YO>V&hIQtE3#imK4i_w*r4cm>k4S=As*LZ3R6 zbPb_}n;2P-Ung*}#u_5V^p!@TJSJ(Lou5^8>l6_)7eJ!0fC}x+rfaTL^Q~5TEu9jB z6Q_@kMG|Puf5rhG$#I!n+cd_E8XJCu z?ygCQQUY3YUX_-vFabMK)&x7v1=D=VN#*AOlpt#jF9@Q71(o~4hrc%`4xSqK7i-qn zgz98#;Xg7=RhoA2X(Jq^Y4`E_MI9v_qX##-A}`ioVSU-vlBqB<#Lx%@ErKipqDV<8 z#Lgm91Fm0x79qp#g)rny0c6EuvbzTwiL`&Jbx;b)pO5A{KFnUVmT(bU_WM<0kIfBIr$qa8lCybU!3=+%%arbr6Y3aw82A) zOn|`1A#ITk+Z%kO^Ux!CTA<4~&9X2?3R;4SrMcnQg*_b>YJELTL zXc{n?TH1|6Yy2_hwR~4QUF=3cqUhipXm~;BlrQ?~7$=fX?K_64$ntG`F8?{A>x>d^ zW14tw2$}R^XF?TIfJhz9|NI-TgEk!AOHOv{josXw;wl_%xk!`JPK0#l`(r~AtkqlY z6HH)eOkpAEswkU-|jJaVw5-f!;DiL8_>N992p!Ubu z|FRi4vXM(xI*7jq8&QNlo3lJUq5A#8#QtATS3R zoZ-VCuRiMsO!trx0txZ-B!ImQ@c{#}dTCCKWT!wu_D@a6O@`k%j98L?k`h)LyS@-1 z2FGEdSOriE3sl>M7A2MxkamGc+iwXhfCZYzBP$6ljjzYV6uYB1Ev-saa0b%6(pq#$ zyycMpS*ZJn7|j}W;sH=tZ)G*NbPC6(3<%YZmbTtF z@}6?v4o+om1_pmOXBuBFionXK4uB`s~gSsTDvW&WKYgagcem$oZ(JGyL*+O|_m zR`Njt^f)Nv`qO62KV=+w-ZP1M;Y1g1-*U^t$Hz94))i3!xefS9UD~K8ok|!lDdpkS zAw@XRNEx*yEmZ{DLg!CbdzPw;w{H~KlDEI?hm(2Sy(L|~<<$op);r!dI=AR^nc{z| zBQ8;YJd8TyN8bj0TA<cMwPD!e6;AB2(_ft)*h9@!wu&a_Cwxf0*j}9{^n#qYAR)JQTE2~Y4w;4#Js zLMy4KN>y^~KWpB6KQA-xCq@<12C1IBfWv(Y=_Bkl4R!}>hNAii zZ|o=~R55*ps@&fo-T30BCcm~H_r_f}RAr3JjzJLfTxOfML(wwzhz9jno8=>DB=NnV z`|FcuZ$xUiAB~w`&a)$EM-J}*R+zr#$Nj$apu{`>~ABC(f4Q-D8xvHas@qBc3Ph099LRz`EpQdd`?#G z!#!Kl1Oi!-4ars?>m}vMX;PZ3>=PN^E@>_S|I*sMfJYF2R=I!dLZMhp!rE6OHIY7V zU^9`&h90=X3LS@Z@{f<{k1$2;N$o|7a%r5JquXP{;j=~~a3xo)hfHmsEIfg++Gy?3 zkLm>iG7W^^h4B3)qei}4dQlH=k8?EDoQAJDX^KJg#;9J8Z07 zrw9l$e}N&<@lD@um4Z%DmyOwgU21dX)n@=MM4*J$GXj>L-d1V!I& zmDj$oE56M~mWw4Mm)QPUP1^&&t@=Uy&-8YP>|?*j8lXu4|n>8`-ri zgZwxJv{_o^CBv~e?pcyOB-c0HawXZ-kP;3`Cwa*5xh=ZOtnvq2!E&k=3#ALR{t`;M zIAySzeMSwg#gY>wObspi&RE&lc==PlXh6F1jxTC@5fKu|-^eze17U_dzZmpUUO!yA zcq~<_S>hs$=>8x0nwgpTc@w(&x$(cVP$!!Gd6yRh3!PQO_x1to^fN60@NJsw+gNFS zcII*O$3+e3jn`A@hNn*TtlWMT?)g7rR z;1(&Q^IH0Kp9^X4M2{4_a_m0y_Tn#lrl7|Ty!u6kzQsq6!=%VWaqHg;t0Xvl9gcxt z@#dcu$$E|nuZxUG`)<@q-v|*uyju1f6f^f3iKaDf@ZqIR{HNd z-Y6j361J8EiBlYr`HC0%s`$@BIXKwF`fq~u7Rc4mi{(0g7kzpuH`g&Y@4J|M)IQN_ zs_-qVrcvj)F(ytKm$V`A6NudxseN4)+G5h#_2<2K_3|LISHA5Va`FuH?R9NLvV7S; zYYxMUi^Jdx^8N59+4%i1p3E;n950k)pPifI^fve=84D+|d_=?8Saj+-u*CP(zIpMG z$mmTlb;>Kem2Wyfn#HvLlx}Ij__yz)Lq z%PM~5I}@vVZdXT~-#=yJAjGzuFZx9r4O4z~^IGdAM>y+t#ETmReJ_KykMDG{?@!6g z-%#|^27;#VI@}Z!HGqZ(2`}+Rv3Q6{BXE+yqzUuW!iy_r%+%WKT@2kjB~akZzlzyN zNogb;Du%32t%-2X(EuvjYIR6)5HC$9Nnr?fTyeOfgv518g$awanx4_$>7qZlJn%n| z2ZyhAw^9BxF&#t(NNMMNshXMhvDMW0f0|> controlToggles { + {"AlwaysOnLateral", "Always on Lateral", "Maintain openpilot lateral control when the brake or gas pedals are used.\n\nDeactivation occurs only through the 'Cruise Control' button.", "../frogpilot/assets/toggle_icons/icon_always_on_lateral.png"}, + {"LateralTune", "Lateral Tuning", "Modify openpilot's steering behavior.", "../frogpilot/assets/toggle_icons/icon_lateral_tune.png"}, {"LongitudinalTune", "Longitudinal Tuning", "Modify openpilot's acceleration and braking behavior.", "../frogpilot/assets/toggle_icons/icon_longitudinal_tune.png"}, @@ -13,7 +15,22 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil for (const auto &[param, title, desc, icon] : controlToggles) { ParamControl *toggle; - if (param == "LateralTune") { + if (param == "AlwaysOnLateral") { + std::vector aolToggles{tr("AlwaysOnLateralMain")}; + std::vector aolToggleNames{tr("Enable On Cruise Main")}; + toggle = new FrogPilotParamToggleControl(param, title, desc, icon, aolToggles, aolToggleNames); + + QObject::connect(static_cast(toggle), &FrogPilotParamToggleControl::buttonClicked, [this](const bool checked) { + if (checked) { + FrogPilotConfirmationDialog::toggleAlert("WARNING: This is very experimental and isn't guaranteed to work. If you run into any issues, please report it in the FrogPilot Discord!", + "I understand the risks.", this); + } + if (FrogPilotConfirmationDialog::toggle("Reboot required to take effect.", "Reboot Now", this)) { + Hardware::reboot(); + } + }); + + } else if (param == "LateralTune") { FrogPilotParamManageControl *lateralTuneToggle = new FrogPilotParamManageControl(param, title, desc, icon, this); QObject::connect(lateralTuneToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() { parentToggleClicked(); @@ -68,7 +85,7 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil }); } - std::set rebootKeys = {}; + std::set rebootKeys = {"AlwaysOnLateral"}; for (const std::string &key : rebootKeys) { QObject::connect(toggles[key], &ToggleControl::toggleFlipped, [this]() { if (FrogPilotConfirmationDialog::toggle("Reboot required to take effect.", "Reboot Now", this)) { diff --git a/selfdrive/ui/qt/maps/map.cc b/selfdrive/ui/qt/maps/map.cc index db56fcd..64ad371 100644 --- a/selfdrive/ui/qt/maps/map.cc +++ b/selfdrive/ui/qt/maps/map.cc @@ -124,7 +124,7 @@ void MapWindow::updateState(const UIState &s) { if (sm.updated("modelV2")) { // set path color on change, and show map on rising edge of navigate on openpilot bool nav_enabled = sm["modelV2"].getModelV2().getNavEnabled() && - sm["controlsState"].getControlsState().getEnabled(); + (sm["controlsState"].getControlsState().getEnabled() || sm["frogpilotCarControl"].getFrogpilotCarControl().getAlwaysOnLateral()); if (nav_enabled != uiState()->scene.navigate_on_openpilot) { if (loaded_once) { m_map->setPaintProperty("navLayer", "line-color", getNavPathColor(nav_enabled)); diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index ba07a76..a61d804 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -178,7 +178,7 @@ void OnroadAlerts::paintEvent(QPaintEvent *event) { int margin = 40; int radius = 30; - int offset = true ? 25 : 0; + int offset = scene.always_on_lateral ? 25 : 0; if (alert.size == cereal::ControlsState::AlertSize::FULL) { margin = 0; radius = 0; @@ -258,7 +258,7 @@ void ExperimentalButton::updateState(const UIState &s) { void ExperimentalButton::paintEvent(QPaintEvent *event) { QPainter p(this); QPixmap img = experimental_mode ? experimental_img : engage_img; - drawIcon(p, QPoint(btn_size / 2, btn_size / 2), img, QColor(0, 0, 0, 166), (isDown() || !engageable) ? 0.6 : 1.0); + drawIcon(p, QPoint(btn_size / 2, btn_size / 2), img, QColor(0, 0, 0, 166), (isDown() || (!engageable && !scene.always_on_lateral_active)) ? 0.6 : 1.0); } @@ -548,7 +548,7 @@ void AnnotatedCameraWidget::drawDriverState(QPainter &painter, const UIState *s) // base icon int offset = UI_BORDER_SIZE + btn_size / 2; - offset += true ? 25 : 0; + offset += alwaysOnLateral ? 25 : 0; int x = rightHandDM ? width() - offset : offset; int y = height() - offset; float opacity = dmActive ? 0.65 : 0.2; @@ -739,9 +739,10 @@ void AnnotatedCameraWidget::initializeFrogPilotWidgets() { } void AnnotatedCameraWidget::updateFrogPilotWidgets(QPainter &p) { + alwaysOnLateral = scene.always_on_lateral_active; experimentalMode = scene.experimental_mode; - if (true) { + if (alwaysOnLateral) { drawStatusBar(p); } @@ -772,6 +773,10 @@ void AnnotatedCameraWidget::drawStatusBar(QPainter &p) { p.setOpacity(1.0); p.drawRoundedRect(statusBarRect, 30, 30); + if (alwaysOnLateral) { + newStatus = QString("Always On Lateral active") + (". Press the \"Cruise Control\" button to disable"); + } + // Check if status has changed if (newStatus != lastShownStatus) { displayStatusText = true; diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index ef8be08..af6ebe5 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -117,6 +117,7 @@ private: QHBoxLayout *bottom_layout; + bool alwaysOnLateral; bool experimentalMode; protected: diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 2caad9b..9d38679 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -213,6 +213,9 @@ static void update_state(UIState *s) { } if (sm.updated("frogpilotCarControl")) { auto frogpilotCarControl = sm["frogpilotCarControl"].getFrogpilotCarControl(); + if (scene.always_on_lateral) { + scene.always_on_lateral_active = !scene.enabled && frogpilotCarControl.getAlwaysOnLateral(); + } } if (sm.updated("frogpilotLateralPlan")) { auto frogpilotLateralPlan = sm["frogpilotLateralPlan"].getFrogpilotLateralPlan(); @@ -244,6 +247,8 @@ void ui_update_params(UIState *s) { // FrogPilot variables UIScene &scene = s->scene; + + scene.always_on_lateral = params.getBool("AlwaysOnLateral"); } void UIState::updateStatus() { @@ -252,6 +257,8 @@ void UIState::updateStatus() { auto state = controls_state.getState(); if (state == cereal::ControlsState::OpenpilotState::PRE_ENABLED || state == cereal::ControlsState::OpenpilotState::OVERRIDING) { status = STATUS_OVERRIDE; + } else if (scene.always_on_lateral_active) { + status = STATUS_LATERAL_ACTIVE; } else { status = controls_state.getEnabled() ? STATUS_ENGAGED : STATUS_DISENGAGED; } diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index b6f3bef..6be81e7 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -107,6 +107,7 @@ typedef enum UIStatus { STATUS_ENGAGED, // FrogPilot statuses + STATUS_LATERAL_ACTIVE, } UIStatus; enum PrimeType { @@ -125,6 +126,7 @@ const QColor bg_colors [] = { [STATUS_ENGAGED] = QColor(0x17, 0x86, 0x44, 0xf1), // FrogPilot colors + [STATUS_LATERAL_ACTIVE] = QColor(0x0a, 0xba, 0xb5, 0xf1), }; static std::map alert_colors = { @@ -167,6 +169,8 @@ typedef struct UIScene { uint64_t started_frame; // FrogPilot variables + bool always_on_lateral; + bool always_on_lateral_active; bool enabled; bool experimental_mode;