brianhansonxyz/clearpilot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Always On Lateral

Browse Source 
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 <jacob@pfeifer.dev>
This commit is contained in:
FrogAi
2024-02-27 16:34:47 -07:00
parent a76689801d
commit a6aa132b3d
41 changed files with 170 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
selfdrive/car/chrysler/carcontroller.py
View File

@@ -42,7 +42,7 @@ class CarController:
if self.frame % 25 == 0:
if CS.lkas_car_model != -1:
can_sends.append(chryslercan.create_lkas_hud(self.packer, self.CP, lkas_active, CC.hudControl.visualAlert,
self.hud_count, CS.lkas_car_model, CS.auto_high_beam))
self.hud_count, CS.lkas_car_model, CS.auto_high_beam, CC.latActive))
self.hud_count += 1
# steering

5
selfdrive/car/chrysler/chryslercan.py
View File

@@ -4,7 +4,7 @@ from openpilot.selfdrive.car.chrysler.values import RAM_CARS
GearShifter = car.CarState.GearShifter
VisualAlert = car.CarControl.HUDControl.VisualAlert
def create_lkas_hud(packer, CP, lkas_active, hud_alert, hud_count, car_model, auto_high_beam):
def create_lkas_hud(packer, CP, lkas_active, hud_alert, hud_count, car_model, auto_high_beam, lat_active):
# LKAS_HUD - Controls what lane-keeping icon is displayed
# == Color ==
@@ -27,7 +27,7 @@ def create_lkas_hud(packer, CP, lkas_active, hud_alert, hud_count, car_model, au
# 7 Normal
# 6 lane departure place hands on wheel
color = 2 if lkas_active else 1
color = 2 if lkas_active else 1 if lat_active else 0
lines = 3 if lkas_active else 0
alerts = 7 if lkas_active else 0
@@ -48,6 +48,7 @@ def create_lkas_hud(packer, CP, lkas_active, hud_alert, hud_count, car_model, au
if CP.carFingerprint in RAM_CARS:
values['AUTO_HIGH_BEAM_ON'] = auto_high_beam
values['LKAS_DISABLED'] = 0 if lat_active else 1
return packer.make_can_msg("DAS_6", 0, values)

2
selfdrive/car/honda/carcontroller.py
View File

@@ -253,7 +253,7 @@ class CarController:
if self.frame % 10 == 0:
hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_control.leadVisible,
hud_control.lanesVisible, fcw_display, acc_alert, steer_required)
can_sends.extend(hondacan.create_ui_commands(self.packer, self.CP, CC.enabled, pcm_speed, hud, CS.is_metric, CS.acc_hud, CS.lkas_hud))
can_sends.extend(hondacan.create_ui_commands(self.packer, self.CP, CC.enabled, pcm_speed, hud, CS.is_metric, CS.acc_hud, CS.lkas_hud, CC.latActive))
if self.CP.openpilotLongitudinalControl and self.CP.carFingerprint not in HONDA_BOSCH:
self.speed = pcm_speed

4
selfdrive/car/honda/hondacan.py
View File

@@ -116,7 +116,7 @@ def create_bosch_supplemental_1(packer, car_fingerprint):
return packer.make_can_msg("BOSCH_SUPPLEMENTAL_1", bus, values)
def create_ui_commands(packer, CP, enabled, pcm_speed, hud, is_metric, acc_hud, lkas_hud):
def create_ui_commands(packer, CP, enabled, pcm_speed, hud, is_metric, acc_hud, lkas_hud, lat_active):
commands = []
bus_pt = get_pt_bus(CP.carFingerprint)
radar_disabled = CP.carFingerprint in (HONDA_BOSCH - HONDA_BOSCH_RADARLESS) and CP.openpilotLongitudinalControl
@@ -149,7 +149,7 @@ def create_ui_commands(packer, CP, enabled, pcm_speed, hud, is_metric, acc_hud,
lkas_hud_values = {
'SET_ME_X41': 0x41,
'STEERING_REQUIRED': hud.steer_required,
'SOLID_LANES': hud.lanes_visible,
'SOLID_LANES': lat_active,
'BEEP': 0,
}

6
selfdrive/car/hyundai/carcontroller.py
View File

@@ -120,7 +120,7 @@ class CarController:
# LFA and HDA icons
if self.frame % 5 == 0 and (not hda2 or hda2_long):
can_sends.append(hyundaicanfd.create_lfahda_cluster(self.packer, self.CAN, CC.enabled))
can_sends.append(hyundaicanfd.create_lfahda_cluster(self.packer, self.CAN, CC.enabled, CC.latActive))
# blinkers
if hda2 and self.CP.flags & HyundaiFlags.ENABLE_BLINKERS:
@@ -151,11 +151,11 @@ 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:
can_sends.append(hyundaican.create_lfahda_mfc(self.packer, CC.enabled))
can_sends.append(hyundaican.create_lfahda_mfc(self.packer, CC.enabled, CC.latActive))
# 5 Hz ACC options
if self.frame % 20 == 0 and self.CP.openpilotLongitudinalControl:

11
selfdrive/car/hyundai/carstate.py
View File

@@ -53,6 +53,7 @@ class CarState(CarStateBase):
self.params = CarControllerParams(CP)
# FrogPilot variables
self.main_enabled = False
def update(self, cp, cp_cam, frogpilot_variables):
if self.CP.carFingerprint in CANFD_CAR:
@@ -104,7 +105,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
@@ -164,7 +165,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
@@ -219,7 +223,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
@@ -240,7 +244,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

8
selfdrive/car/hyundai/hyundaican.py
View File

@@ -117,20 +117,20 @@ def create_clu11(packer, frame, clu11, button, car_fingerprint):
return packer.make_can_msg("CLU11", bus, values)
def create_lfahda_mfc(packer, enabled, hda_set_speed=0):
def create_lfahda_mfc(packer, enabled, lat_active, hda_set_speed=0):
values = {
"LFA_Icon_State": 2 if enabled else 0,
"LFA_Icon_State": 2 if lat_active else 0,
"HDA_Active": 1 if hda_set_speed else 0,
"HDA_Icon_State": 2 if hda_set_speed else 0,
"HDA_VSetReq": hda_set_speed,
}
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,

6
selfdrive/car/hyundai/hyundaicanfd.py
View File

@@ -41,7 +41,7 @@ def create_steering_messages(packer, CP, CAN, enabled, lat_active, apply_steer):
values = {
"LKA_MODE": 2,
"LKA_ICON": 2 if enabled else 1,
"LKA_ICON": 2 if lat_active else 1,
"TORQUE_REQUEST": apply_steer,
"LKA_ASSIST": 0,
"STEER_REQ": 1 if lat_active else 0,
@@ -113,10 +113,10 @@ def create_acc_cancel(packer, CP, CAN, cruise_info_copy):
})
return packer.make_can_msg("SCC_CONTROL", CAN.ECAN, values)
def create_lfahda_cluster(packer, CAN, enabled):
def create_lfahda_cluster(packer, CAN, enabled, lat_active):
values = {
"HDA_ICON": 1 if enabled else 0,
"LFA_ICON": 2 if enabled else 0,
"LFA_ICON": 2 if lat_active else 0,
}
return packer.make_can_msg("LFAHDA_CLUSTER", CAN.ECAN, values)

2
selfdrive/car/nissan/carcontroller.py
View File

@@ -67,7 +67,7 @@ class CarController:
if self.CP.carFingerprint != CAR.ALTIMA:
if self.frame % 2 == 0:
can_sends.append(nissancan.create_lkas_hud_msg(self.packer, CS.lkas_hud_msg, CC.enabled, hud_control.leftLaneVisible, hud_control.rightLaneVisible,
hud_control.leftLaneDepart, hud_control.rightLaneDepart))
hud_control.leftLaneDepart, hud_control.rightLaneDepart, CC.latActive))
if self.frame % 50 == 0:
can_sends.append(nissancan.create_lkas_hud_info_msg(

8
selfdrive/car/nissan/nissancan.py
View File

@@ -65,7 +65,7 @@ def create_cancel_msg(packer, cancel_msg, cruise_cancel):
return packer.make_can_msg("CANCEL_MSG", 2, values)
def create_lkas_hud_msg(packer, lkas_hud_msg, enabled, left_line, right_line, left_lane_depart, right_lane_depart):
def create_lkas_hud_msg(packer, lkas_hud_msg, enabled, left_line, right_line, left_lane_depart, right_lane_depart, lat_active):
values = {s: lkas_hud_msg[s] for s in [
"LARGE_WARNING_FLASHING",
"SIDE_RADAR_ERROR_FLASHING1",
@@ -98,9 +98,9 @@ def create_lkas_hud_msg(packer, lkas_hud_msg, enabled, left_line, right_line, le
values["RIGHT_LANE_YELLOW_FLASH"] = 1 if right_lane_depart else 0
values["LEFT_LANE_YELLOW_FLASH"] = 1 if left_lane_depart else 0
values["LARGE_STEERING_WHEEL_ICON"] = 2 if enabled else 0
values["RIGHT_LANE_GREEN"] = 1 if right_line and enabled else 0
values["LEFT_LANE_GREEN"] = 1 if left_line and enabled else 0
values["LARGE_STEERING_WHEEL_ICON"] = 2 if lat_active else 0
values["RIGHT_LANE_GREEN"] = 1 if right_line and lat_active else 0
values["LEFT_LANE_GREEN"] = 1 if left_line and lat_active else 0
return packer.make_can_msg("PROPILOT_HUD", 0, values)

2
selfdrive/car/subaru/carcontroller.py
View File

@@ -100,7 +100,7 @@ class CarController:
can_sends.append(subarucan.create_es_lkas_state(self.packer, self.frame // 10, CS.es_lkas_state_msg, CC.enabled, hud_control.visualAlert,
hud_control.leftLaneVisible, hud_control.rightLaneVisible,
hud_control.leftLaneDepart, hud_control.rightLaneDepart))
hud_control.leftLaneDepart, hud_control.rightLaneDepart, CC.latActive))
if self.CP.flags & SubaruFlags.SEND_INFOTAINMENT:
can_sends.append(subarucan.create_es_infotainment(self.packer, self.frame // 10, CS.es_infotainment_msg, hud_control.visualAlert))

6
selfdrive/car/subaru/subarucan.py
View File

@@ -66,7 +66,7 @@ def create_es_distance(packer, frame, es_distance_msg, bus, pcm_cancel_cmd, long
return packer.make_can_msg("ES_Distance", bus, values)
def create_es_lkas_state(packer, frame, es_lkas_state_msg, enabled, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart):
def create_es_lkas_state(packer, frame, es_lkas_state_msg, enabled, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart, lat_active):
values = {s: es_lkas_state_msg[s] for s in [
"CHECKSUM",
"LKAS_Alert_Msg",
@@ -118,9 +118,11 @@ def create_es_lkas_state(packer, frame, es_lkas_state_msg, enabled, visual_alert
elif right_lane_depart:
values["LKAS_Alert"] = 11 # Right lane departure dash alert
if enabled:
if lat_active:
values["LKAS_ACTIVE"] = 1 # Show LKAS lane lines
values["LKAS_Dash_State"] = 2 # Green enabled indicator
values["LKAS_Left_Line_Enable"] = 1
values["LKAS_Right_Line_Enable"] = 1
else:
values["LKAS_Dash_State"] = 0 # LKAS Not enabled

2
selfdrive/car/toyota/carcontroller.py
View File

@@ -179,7 +179,7 @@ class CarController:
if self.frame % 20 == 0 or send_ui:
can_sends.append(toyotacan.create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, hud_control.leftLaneVisible,
hud_control.rightLaneVisible, hud_control.leftLaneDepart,
hud_control.rightLaneDepart, CC.enabled, CS.lkas_hud))
hud_control.rightLaneDepart, CC.enabled, CS.lkas_hud, lat_active))
if (self.frame % 100 == 0 or send_ui) and (self.CP.enableDsu or self.CP.flags & ToyotaFlags.DISABLE_RADAR.value):
can_sends.append(toyotacan.create_fcw_command(self.packer, fcw_alert))

6
selfdrive/car/toyota/toyotacan.py
View File

@@ -73,12 +73,12 @@ def create_fcw_command(packer, fcw):
return packer.make_can_msg("PCS_HUD", 0, values)
def create_ui_command(packer, steer, chime, left_line, right_line, left_lane_depart, right_lane_depart, enabled, stock_lkas_hud):
def create_ui_command(packer, steer, chime, left_line, right_line, left_lane_depart, right_lane_depart, enabled, stock_lkas_hud, lat_active):
values = {
"TWO_BEEPS": chime,
"LDA_ALERT": steer,
"RIGHT_LINE": 3 if right_lane_depart else 1 if right_line else 2,
"LEFT_LINE": 3 if left_lane_depart else 1 if left_line else 2,
"RIGHT_LINE": 0 if not lat_active else 3 if right_lane_depart else 1 if right_line else 2,
"LEFT_LINE": 0 if not lat_active else 3 if left_lane_depart else 1 if left_line else 2,
"BARRIERS": 1 if enabled else 0,
# static signals

2
selfdrive/car/volkswagen/carcontroller.py
View File

@@ -94,7 +94,7 @@ class CarController:
if hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw):
hud_alert = self.CCP.LDW_MESSAGES["laneAssistTakeOver"]
can_sends.append(self.CCS.create_lka_hud_control(self.packer_pt, CANBUS.pt, CS.ldw_stock_values, CC.enabled,
CS.out.steeringPressed, hud_alert, hud_control))
CS.out.steeringPressed, hud_alert, hud_control, CC.latActive))
if self.frame % self.CCP.ACC_HUD_STEP == 0 and self.CP.openpilotLongitudinalControl:
lead_distance = 0

6
selfdrive/car/volkswagen/mqbcan.py
View File

@@ -28,7 +28,7 @@ def create_eps_update(packer, bus, eps_stock_values, ea_simulated_torque):
return packer.make_can_msg("LH_EPS_03", bus, values)
def create_lka_hud_control(packer, bus, ldw_stock_values, enabled, steering_pressed, hud_alert, hud_control):
def create_lka_hud_control(packer, bus, ldw_stock_values, enabled, steering_pressed, hud_alert, hud_control, lat_active):
values = {}
if len(ldw_stock_values):
values = {s: ldw_stock_values[s] for s in [
@@ -40,8 +40,8 @@ def create_lka_hud_control(packer, bus, ldw_stock_values, enabled, steering_pres
]}
values.update({
"LDW_Status_LED_gelb": 1 if enabled and steering_pressed else 0,
"LDW_Status_LED_gruen": 1 if enabled and not steering_pressed else 0,
"LDW_Status_LED_gelb": 1 if lat_active and steering_pressed else 0,
"LDW_Status_LED_gruen": 1 if lat_active and not steering_pressed else 0,
"LDW_Lernmodus_links": 3 if hud_control.leftLaneDepart else 1 + hud_control.leftLaneVisible,
"LDW_Lernmodus_rechts": 3 if hud_control.rightLaneDepart else 1 + hud_control.rightLaneVisible,
"LDW_Texte": hud_alert,

6
selfdrive/car/volkswagen/pqcan.py
View File

@@ -9,7 +9,7 @@ def create_steering_control(packer, bus, apply_steer, lkas_enabled):
return packer.make_can_msg("HCA_1", bus, values)
def create_lka_hud_control(packer, bus, ldw_stock_values, enabled, steering_pressed, hud_alert, hud_control):
def create_lka_hud_control(packer, bus, ldw_stock_values, enabled, steering_pressed, hud_alert, hud_control, lat_active):
values = {}
if len(ldw_stock_values):
values = {s: ldw_stock_values[s] for s in [
@@ -21,8 +21,8 @@ def create_lka_hud_control(packer, bus, ldw_stock_values, enabled, steering_pres
]}
values.update({
"LDW_Lampe_gelb": 1 if enabled and steering_pressed else 0,
"LDW_Lampe_gruen": 1 if enabled and not steering_pressed else 0,
"LDW_Lampe_gelb": 1 if lat_active and steering_pressed else 0,
"LDW_Lampe_gruen": 1 if lat_active and not steering_pressed else 0,
"LDW_Lernmodus_links": 3 if hud_control.leftLaneDepart else 1 + hud_control.leftLaneVisible,
"LDW_Lernmodus_rechts": 3 if hud_control.rightLaneDepart else 1 + hud_control.rightLaneVisible,
"LDW_Textbits": hud_alert,

21
selfdrive/controls/controlsd.py
View File

@@ -197,6 +197,15 @@ 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")
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
@@ -684,9 +693,19 @@ class Controls:
# Gear Check
self.driving_gear = CS.gearShifter not in (GearShifter.neutral, GearShifter.park, GearShifter.reverse, GearShifter.unknown)
# Always on lateral
self.FPCC.alwaysOnLateral |= CS.cruiseState.enabled or self.always_on_lateral_main
self.FPCC.alwaysOnLateral &= self.always_on_lateral
self.FPCC.alwaysOnLateral &= CS.cruiseState.available
self.FPCC.alwaysOnLateral &= self.driving_gear
self.FPCC.alwaysOnLateral &= not self.openpilot_crashed
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

BIN
selfdrive/frogpilot/assets/toggle_icons/icon_always_on_lateral.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 9.5 KiB

23
selfdrive/frogpilot/ui/control_settings.cc
View File

@@ -2,6 +2,8 @@
FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPilotListWidget(parent) {
const std::vector<std::tuple<QString, QString, QString, QString>> 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"},
@@ -15,7 +17,24 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
for (const auto &[param, title, desc, icon] : controlToggles) {
ParamControl *toggle;
if (param == "LateralTune") {
if (param == "AlwaysOnLateral") {
std::vector<QString> aolToggles{"AlwaysOnLateralMain"};
std::vector<QString> aolToggleNames{tr("Enable On Cruise Main")};
toggle = new FrogPilotParamToggleControl(param, title, desc, icon, aolToggles, aolToggleNames);
QObject::connect(static_cast<FrogPilotParamToggleControl*>(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 (started) {
if (FrogPilotConfirmationDialog::toggle("Reboot required to take effect.", "Reboot Now", this)) {
Hardware::soft_reboot();
}
}
});
} else if (param == "LateralTune") {
FrogPilotParamManageControl *lateralTuneToggle = new FrogPilotParamManageControl(param, title, desc, icon, this);
QObject::connect(lateralTuneToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() {
parentToggleClicked();
@@ -88,7 +107,7 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
});
}
std::set<std::string> rebootKeys = {};
std::set<std::string> rebootKeys = {"AlwaysOnLateral"};
for (const std::string &key : rebootKeys) {
QObject::connect(toggles[key], &ToggleControl::toggleFlipped, [this, key]() {
if (started) {

2
selfdrive/ui/qt/maps/map.cc
View File

@@ -121,7 +121,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));

18
selfdrive/ui/qt/onroad.cc
View File

@@ -175,7 +175,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;
@@ -242,7 +242,7 @@ void ExperimentalButton::changeMode() {
void ExperimentalButton::updateState(const UIState &s) {
const auto cs = (*s.sm)["controlsState"].getControlsState();
bool eng = cs.getEngageable() || cs.getEnabled();
bool eng = cs.getEngageable() || cs.getEnabled() || scene.always_on_lateral_active;
if ((cs.getExperimentalMode() != experimental_mode) || (eng != engageable)) {
engageable = eng;
experimental_mode = cs.getExperimentalMode();
@@ -255,7 +255,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);
}
@@ -545,7 +545,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;
@@ -734,9 +734,12 @@ void AnnotatedCameraWidget::initializeFrogPilotWidgets() {
}
void AnnotatedCameraWidget::updateFrogPilotWidgets(QPainter &p) {
alwaysOnLateral = scene.always_on_lateral;
alwaysOnLateralActive = scene.always_on_lateral_active;
experimentalMode = scene.experimental_mode;
if (true) {
if (alwaysOnLateral) {
drawStatusBar(p);
}
@@ -768,6 +771,11 @@ void AnnotatedCameraWidget::drawStatusBar(QPainter &p) {
p.setOpacity(1.0);
p.drawRoundedRect(statusBarRect, 30, 30);
// Update status text
if (alwaysOnLateralActive) {
newStatus = QString("Always On Lateral active. Press the \"Cruise Control\" button to disable");
}
// Check if status has changed
if (newStatus != lastShownStatus) {
displayStatusText = true;

2
selfdrive/ui/qt/onroad.h
View File

@@ -118,6 +118,8 @@ private:
QHBoxLayout *bottom_layout;
bool alwaysOnLateral;
bool alwaysOnLateralActive;
bool experimentalMode;
protected:

7
selfdrive/ui/ui.cc
View File

@@ -210,6 +210,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("frogpilotPlan")) {
auto frogpilotPlan = sm["frogpilotPlan"].getFrogpilotPlan();
@@ -243,6 +246,8 @@ void ui_update_frogpilot_params(UIState *s) {
Params params = Params();
UIScene &scene = s->scene;
scene.always_on_lateral = params.getBool("AlwaysOnLateral");
bool custom_onroad_ui = params.getBool("CustomUI");
scene.acceleration_path = custom_onroad_ui && params.getBool("AccelerationPath");
@@ -257,6 +262,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;
}

4
selfdrive/ui/ui.h
View File

@@ -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<cereal::ControlsState::AlertStatus, QColor> alert_colors = {
@@ -168,6 +170,8 @@ typedef struct UIScene {
// FrogPilot variables
bool acceleration_path;
bool always_on_lateral;
bool always_on_lateral_active;
bool enabled;
bool experimental_mode;