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 0000000..1e55e3f Binary files /dev/null and b/selfdrive/frogpilot/assets/toggle_icons/icon_always_on_lateral.png differ diff --git a/selfdrive/frogpilot/ui/control_settings.cc b/selfdrive/frogpilot/ui/control_settings.cc index 4ad1aa7..f8645e6 100644 --- a/selfdrive/frogpilot/ui/control_settings.cc +++ b/selfdrive/frogpilot/ui/control_settings.cc @@ -3,6 +3,8 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPilotListWidget(parent) { const std::vector> 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;