brianhansonxyz/oscarpilot
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-01-12 22:39:30 -07:00
parent c33e3e45db
commit 42b3636995
28 changed files with 132 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
cereal/custom.capnp
View File

@@ -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 {

2
common/params.cc
View File

@@ -214,6 +214,8 @@ std::unordered_map<std::string, uint32_t> keys = {
// FrogPilot parameters
{"AccelerationProfile", PERSISTENT},
{"AggressiveAcceleration", PERSISTENT},
{"AlwaysOnLateral", PERSISTENT},
{"AlwaysOnLateralMain", PERSISTENT},
{"ApiCache_DriveStats", PERSISTENT},
{"FrogPilotTogglesUpdated", PERSISTENT},
{"GasRegenCmd", PERSISTENT},

19
panda/board/safety.h
View File

@@ -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;
}

1
panda/board/safety/safety_chrysler.h
View File

@@ -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);
}

1
panda/board/safety/safety_ford.h
View File

@@ -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);
}

4
panda/board/safety/safety_gm.h
View File

@@ -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;

3
panda/board/safety/safety_honda.h
View File

@@ -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;

4
panda/board/safety/safety_hyundai_common.h
View File

@@ -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;

1
panda/board/safety/safety_mazda.h
View File

@@ -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);
}

7
panda/board/safety/safety_nissan.h
View File

@@ -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;
}
}

1
panda/board/safety/safety_subaru.h
View File

@@ -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);
}

1
panda/board/safety/safety_subaru_preglobal.h
View File

@@ -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);
}

8
panda/board/safety/safety_tesla.h
View File

@@ -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

5
panda/board/safety/safety_toyota.h
View File

@@ -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

4
panda/board/safety_declarations.h
View File

@@ -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

1
panda/python/__init__.py
View File

@@ -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:

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

@@ -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:

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

@@ -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

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

@@ -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,

2
selfdrive/car/interfaces.py
View File

@@ -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]])

21
selfdrive/controls/controlsd.py
View File

@@ -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

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

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

@@ -3,6 +3,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"},
@@ -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<QString> aolToggles{tr("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 (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<std::string> rebootKeys = {};
std::set<std::string> 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)) {

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

@@ -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));

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

@@ -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;

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

@@ -117,6 +117,7 @@ private:
QHBoxLayout *bottom_layout;
bool alwaysOnLateral;
bool experimentalMode;
protected:

7
selfdrive/ui/ui.cc
View File

@@ -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;
}

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 = {
@@ -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;