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 <jacob@pfeifer.dev>
This commit is contained in:
FrogAi
2024-04-03 11:39:33 -07:00
parent b58bc95a58
commit 9eac8e4256
44 changed files with 190 additions and 52 deletions
+1
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 FrogPilotCarState @0xaedffd8f31e7b55d {
+4
View File
@@ -213,6 +213,8 @@ std::unordered_map<std::string, uint32_t> keys = {
{"AccelerationProfile", PERSISTENT},
{"AggressiveAcceleration", PERSISTENT},
{"AlertVolumeControl", PERSISTENT},
{"AlwaysOnLateral", PERSISTENT},
{"AlwaysOnLateralMain", PERSISTENT},
{"ApiCache_DriveStats", PERSISTENT},
{"CustomAlerts", PERSISTENT},
{"CustomPaths", PERSISTENT},
@@ -222,8 +224,10 @@ std::unordered_map<std::string, uint32_t> keys = {
{"EngageVolume", PERSISTENT},
{"FrogPilotTogglesUpdated", PERSISTENT},
{"FrogsGoMoo", PERSISTENT},
{"HideAOLStatusBar", PERSISTENT},
{"LateralTune", PERSISTENT},
{"LongitudinalTune", PERSISTENT},
{"PauseAOLOnBrake", PERSISTENT},
{"PromptDistractedVolume", PERSISTENT},
{"PromptVolume", PERSISTENT},
{"QOLControls", PERSISTENT},
+15 -4
View File
@@ -552,7 +552,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);
@@ -579,7 +585,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;
}
@@ -621,7 +627,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;
@@ -636,8 +642,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
@@ -680,7 +691,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
View File
@@ -185,6 +185,7 @@ static void chrysler_rx_hook(const 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);
bool cruise_engaged = GET_BIT(to_push, 21U);
pcm_cruise_check(cruise_engaged);
}
+1
View File
@@ -245,6 +245,7 @@ static void ford_rx_hook(const 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);
}
+5 -1
View File
@@ -141,7 +141,11 @@ static void gm_rx_hook(const CANPacket_t *to_push) {
}
if ((addr == 0xC9) && ((gm_hw == GM_CAM) || (gm_hw == GM_SDGM))) {
brake_pressed = GET_BIT(to_push, 40U);
brake_pressed = GET_BIT(to_push, 40U) != 0U;
}
if (addr == 0xC9) {
acc_main_on = GET_BIT(to_push, 29U) != 0U;
}
if (addr == 0x1C4) {
+2 -1
View File
@@ -350,7 +350,8 @@ static bool honda_tx_hook(const 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;
@@ -63,6 +63,10 @@ void hyundai_common_cruise_state_check(const bool cruise_engaged) {
}
void hyundai_common_cruise_buttons_check(const int cruise_button, const bool main_button) {
if (main_button && 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) {
hyundai_last_button_interaction = 0U;
} else {
+1
View File
@@ -52,6 +52,7 @@ static void mazda_rx_hook(const 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
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(const 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
View File
@@ -152,6 +152,7 @@ static void subaru_rx_hook(const 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);
bool cruise_engaged = GET_BIT(to_push, 41U);
pcm_cruise_check(cruise_engaged);
}
@@ -56,6 +56,7 @@ static void subaru_preglobal_rx_hook(const 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);
bool cruise_engaged = GET_BIT(to_push, 49U);
pcm_cruise_check(cruise_engaged);
}
+8
View File
@@ -100,6 +100,14 @@ static void tesla_rx_hook(const 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
View File
@@ -58,6 +58,7 @@ const int TOYOTA_GAS_INTERCEPTOR_THRSLD = 805;
{0x128, 1, 6}, {0x141, 1, 4}, {0x160, 1, 8}, {0x161, 1, 7}, {0x470, 1, 4}, /* DSU bus 1 */ \
{0x411, 0, 8}, /* PCS_HUD */ \
{0x750, 0, 8}, /* radar diagnostic address */ \
{0x1D3, 0, 8}, \
const CanMsg TOYOTA_TX_MSGS[] = {
TOYOTA_COMMON_TX_MSGS
@@ -76,6 +77,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 }}}, \
@@ -181,6 +183,9 @@ static void toyota_rx_hook(const 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
View File
@@ -225,6 +225,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
@@ -270,3 +271,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 32
+1
View File
@@ -111,6 +111,7 @@ class ALTERNATIVE_EXPERIENCE:
DISABLE_STOCK_AEB = 2
RAISE_LONGITUDINAL_LIMITS_TO_ISO_MAX = 8
ALLOW_AEB = 16
ALWAYS_ON_LATERAL = 32
class Panda:
+4
View File
@@ -54,6 +54,10 @@ class CarD:
if not disengage_on_accelerator:
self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS
always_on_lateral = self.params.get_bool("AlwaysOnLateral")
if always_on_lateral:
self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL
self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.RAISE_LONGITUDINAL_LIMITS_TO_ISO_MAX
car_recognized = self.CP.carName != 'mock'
+1 -1
View File
@@ -43,7 +43,7 @@ class CarController(CarControllerBase):
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
+3 -2
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)
+1 -1
View File
@@ -255,7 +255,7 @@ class CarController(CarControllerBase):
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, hud_control.leadDistanceBars)
can_sends.extend(hondacan.create_ui_commands(self.packer, self.CAN, 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.CAN, 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
+2 -2
View File
@@ -134,7 +134,7 @@ def create_bosch_supplemental_1(packer, CAN, car_fingerprint):
return packer.make_can_msg("BOSCH_SUPPLEMENTAL_1", bus, values)
def create_ui_commands(packer, CAN, CP, enabled, pcm_speed, hud, is_metric, acc_hud, lkas_hud):
def create_ui_commands(packer, CAN, CP, enabled, pcm_speed, hud, is_metric, acc_hud, lkas_hud, lat_active):
commands = []
radar_disabled = CP.carFingerprint in (HONDA_BOSCH - HONDA_BOSCH_RADARLESS) and CP.openpilotLongitudinalControl
bus_lkas = get_lkas_cmd_bus(CAN, CP.carFingerprint, radar_disabled)
@@ -169,7 +169,7 @@ def create_ui_commands(packer, CAN, CP, enabled, pcm_speed, hud, is_metric, acc_
lkas_hud_values = {
'SET_ME_X41': 0x41,
'STEERING_REQUIRED': hud.steer_required,
'SOLID_LANES': hud.lanes_visible,
'SOLID_LANES': lat_active,
'BEEP': 0,
}
+3 -3
View File
@@ -121,7 +121,7 @@ class CarController(CarControllerBase):
# 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:
@@ -152,11 +152,11 @@ class CarController(CarControllerBase):
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, 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:
+9 -2
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
+4 -4
View File
@@ -117,20 +117,20 @@ def create_clu11(packer, frame, clu11, button, CP):
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, hud_control, set_speed, stopping, long_override, use_fca):
def create_acc_commands(packer, enabled, accel, upper_jerk, idx, hud_control, set_speed, stopping, long_override, use_fca, cruise_available):
commands = []
scc11_values = {
"MainMode_ACC": 1,
"MainMode_ACC": 1 if cruise_available else 0,
"TauGapSet": hud_control.leadDistanceBars,
"VSetDis": set_speed if enabled else 0,
"AliveCounterACC": idx % 0x10,
+3 -3
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)
+1 -1
View File
@@ -68,7 +68,7 @@ class CarController(CarControllerBase):
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(
+4 -4
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)
+1 -1
View File
@@ -100,7 +100,7 @@ class CarController(CarControllerBase):
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))
+4 -2
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
+1 -1
View File
@@ -188,7 +188,7 @@ class CarController(CarControllerBase):
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))
+3 -3
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
+1 -1
View File
@@ -95,7 +95,7 @@ class CarController(CarControllerBase):
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
+3 -3
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,
+3 -3
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,
+15 -1
View File
@@ -180,6 +180,9 @@ class Controls:
self.block_user = self.branch == "FrogPilot-Development" and not self.params_storage.get_bool("FrogsGoMoo")
self.always_on_lateral = self.params.get_bool("AlwaysOnLateral")
self.always_on_lateral_main = self.always_on_lateral and self.params.get_bool("AlwaysOnLateralMain")
self.update_frogpilot_params()
def set_initial_state(self):
@@ -546,6 +549,9 @@ class Controls:
if self.active:
self.current_alert_types.append(ET.WARNING)
if self.FPCC.alwaysOnLateral:
self.current_alert_types.append(ET.WARNING)
def state_control(self, CS):
"""Given the state, this function returns a CarControl packet"""
@@ -570,7 +576,7 @@ class Controls:
# 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
@@ -883,6 +889,12 @@ class Controls:
def update_frogpilot_variables(self, CS):
self.driving_gear = CS.gearShifter not in (GearShifter.neutral, GearShifter.park, GearShifter.reverse, GearShifter.unknown)
self.FPCC.alwaysOnLateral |= CS.cruiseState.enabled or self.always_on_lateral_main
self.FPCC.alwaysOnLateral &= CS.cruiseState.available
self.FPCC.alwaysOnLateral &= self.always_on_lateral
self.FPCC.alwaysOnLateral &= self.driving_gear
self.FPCC.alwaysOnLateral &= not (CS.brakePressed and CS.vEgo < self.always_on_lateral_pause_speed) or CS.standstill
fpcc_send = messaging.new_message('frogpilotCarControl')
fpcc_send.valid = CS.canValid
fpcc_send.frogpilotCarControl = self.FPCC
@@ -892,6 +904,8 @@ class Controls:
self.update_frogpilot_params()
def update_frogpilot_params(self):
self.always_on_lateral_pause_speed = self.always_on_lateral and self.params.get_int("PauseAOLOnBrake")
custom_alerts = self.params.get_bool("CustomAlerts")
lateral_tune = self.params.get_bool("LateralTune")
Binary file not shown.

After

Width:  |  Height:  |  Size: 9.5 KiB

@@ -5,6 +5,11 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
isStaging = branch == "FrogPilot-Development" || branch == "FrogPilot-Staging" || branch == "FrogPilot-Testing";
const std::vector<std::tuple<QString, QString, QString, QString>> controlToggles {
{"AlwaysOnLateral", tr("Always on Lateral"), tr("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"},
{"AlwaysOnLateralMain", tr("Enable On Cruise Main"), tr("Enable 'Always On Lateral' by clicking your 'Cruise Control' button without requring openpilot to be enabled first."), ""},
{"PauseAOLOnBrake", tr("Pause On Brake"), tr("Pause 'Always On Lateral' when the brake pedal is being pressed below the set speed."), ""},
{"HideAOLStatusBar", tr("Hide the Status Bar"), tr("Don't use the status bar for 'Always On Lateral'."), ""},
{"LateralTune", tr("Lateral Tuning"), tr("Modify openpilot's steering behavior."), "../frogpilot/assets/toggle_icons/icon_lateral_tune.png"},
{"LongitudinalTune", tr("Longitudinal Tuning"), tr("Modify openpilot's acceleration and braking behavior."), "../frogpilot/assets/toggle_icons/icon_longitudinal_tune.png"},
@@ -18,7 +23,19 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
for (const auto &[param, title, desc, icon] : controlToggles) {
ParamControl *toggle;
if (param == "LateralTune") {
if (param == "AlwaysOnLateral") {
FrogPilotParamManageControl *aolToggle = new FrogPilotParamManageControl(param, title, desc, icon, this);
QObject::connect(aolToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() {
openParentToggle();
for (auto &[key, toggle] : toggles) {
toggle->setVisible(aolKeys.find(key.c_str()) != aolKeys.end());
}
});
toggle = aolToggle;
} else if (param == "PauseAOLOnBrake") {
toggle = new FrogPilotParamValueControl(param, title, desc, icon, 0, 99, std::map<int, QString>(), this, false, tr(" mph"));
} else if (param == "LateralTune") {
FrogPilotParamManageControl *lateralTuneToggle = new FrogPilotParamManageControl(param, title, desc, icon, this);
QObject::connect(lateralTuneToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() {
openParentToggle();
@@ -93,7 +110,15 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
});
}
std::set<QString> rebootKeys = {};
QObject::connect(toggles["AlwaysOnLateralMain"], &ToggleControl::toggleFlipped, [this]() {
if (params.getBool("AlwaysOnLateralMain")) {
FrogPilotConfirmationDialog::toggleAlert(
tr("WARNING: This isn't guaranteed to work, so if you run into any issues, please report it in the FrogPilot Discord!"),
tr("I understand the risks."), this);
}
});
std::set<QString> rebootKeys = {"AlwaysOnLateral", "AlwaysOnLateralMain"};
for (const QString &key : rebootKeys) {
QObject::connect(toggles[key.toStdString().c_str()], &ToggleControl::toggleFlipped, [this]() {
if (started) {
@@ -163,11 +188,19 @@ void FrogPilotControlsPanel::updateMetric() {
if (isMetric != previousIsMetric) {
double distanceConversion = isMetric ? FOOT_TO_METER : METER_TO_FOOT;
double speedConversion = isMetric ? MILE_TO_KM : KM_TO_MILE;
params.putIntNonBlocking("PauseAOLOnBrake", std::nearbyint(params.getInt("PauseAOLOnBrake") * speedConversion));
}
FrogPilotParamValueControl *pauseAOLOnBrakeToggle = static_cast<FrogPilotParamValueControl*>(toggles["PauseAOLOnBrake"]);
if (isMetric) {
pauseAOLOnBrakeToggle->updateControl(0, 99, tr(" kph"));
} else {
pauseAOLOnBrakeToggle->updateControl(0, 99, tr(" mph"));
}
pauseAOLOnBrakeToggle->refresh();
}
void FrogPilotControlsPanel::hideToggles() {
@@ -25,7 +25,7 @@ private:
void updateState();
void updateToggles();
std::set<QString> aolKeys = {};
std::set<QString> aolKeys = {"AlwaysOnLateralMain", "HideAOLStatusBar", "PauseAOLOnBrake"};
std::set<QString> conditionalExperimentalKeys = {};
std::set<QString> deviceManagementKeys = {};
std::set<QString> experimentalModeActivationKeys = {};
+1 -1
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));
+3
View File
@@ -154,6 +154,9 @@ void TogglesPanel::showEvent(QShowEvent *event) {
}
void TogglesPanel::updateToggles() {
auto disengage_on_accelerator_toggle = toggles["DisengageOnAccelerator"];
disengage_on_accelerator_toggle->setVisible(!params.getBool("AlwaysOnLateral"));
auto experimental_mode_toggle = toggles["ExperimentalMode"];
auto op_long_toggle = toggles["ExperimentalLongitudinalEnabled"];
const QString e2e_description = QString("%1<br>"
+11 -4
View File
@@ -173,7 +173,7 @@ void OnroadAlerts::paintEvent(QPaintEvent *event) {
int margin = 40;
int radius = 30;
int offset = true ? 25 : 0;
int offset = scene.show_aol_status_bar ? 25 : 0;
if (alert.size == cereal::ControlsState::AlertSize::FULL) {
margin = 0;
radius = 0;
@@ -238,7 +238,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();
@@ -547,7 +547,7 @@ void AnnotatedCameraWidget::drawDriverState(QPainter &painter, const UIState *s)
// base icon
int offset = UI_BORDER_SIZE + btn_size / 2;
int x = rightHandDM ? width() - offset : offset;
offset += true ? 25 : 0;
offset += showAlwaysOnLateralStatusBar ? 25 : 0;
int y = height() - offset;
float opacity = dmActive ? 0.65 : 0.2;
drawIcon(painter, QPoint(x, y), dm_img, blackColor(70), opacity);
@@ -734,13 +734,16 @@ void AnnotatedCameraWidget::initializeFrogPilotWidgets() {
void AnnotatedCameraWidget::updateFrogPilotWidgets() {
alertSize = scene.alert_size;
alwaysOnLateralActive = scene.always_on_lateral_active;
showAlwaysOnLateralStatusBar = scene.show_aol_status_bar;
experimentalMode = scene.experimental_mode;
mapOpen = scene.map_open;
}
void AnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &p) {
if (true) {
if (showAlwaysOnLateralStatusBar) {
drawStatusBar(p);
}
@@ -771,6 +774,10 @@ void AnnotatedCameraWidget::drawStatusBar(QPainter &p) {
p.setOpacity(1.0);
p.drawRoundedRect(statusBarRect, 30, 30);
if (alwaysOnLateralActive && showAlwaysOnLateralStatusBar) {
newStatus = tr("Always On Lateral active") + (mapOpen ? "" : tr(". Press the \"Cruise Control\" button to disable"));
}
if (newStatus != lastShownStatus) {
displayStatusText = true;
lastShownStatus = newStatus;
+2
View File
@@ -118,8 +118,10 @@ private:
QHBoxLayout *bottom_layout;
bool alwaysOnLateralActive;
bool experimentalMode;
bool mapOpen;
bool showAlwaysOnLateralStatusBar;
int alertSize;
+6
View File
@@ -219,6 +219,7 @@ static void update_state(UIState *s) {
}
if (sm.updated("frogpilotCarControl")) {
auto frogpilotCarControl = sm["frogpilotCarControl"].getFrogpilotCarControl();
scene.always_on_lateral_active = !scene.enabled && frogpilotCarControl.getAlwaysOnLateral();
}
if (sm.updated("frogpilotPlan")) {
auto frogpilotPlan = sm["frogpilotPlan"].getFrogpilotPlan();
@@ -250,6 +251,9 @@ void ui_update_frogpilot_params(UIState *s) {
Params params = Params();
UIScene &scene = s->scene;
bool always_on_lateral = params.getBool("AlwaysOnLateral");
scene.show_aol_status_bar = always_on_lateral && !params.getBool("HideAOLStatusBar");
bool custom_onroad_ui = params.getBool("CustomUI");
bool custom_paths = custom_onroad_ui && params.getBool("CustomPaths");
scene.acceleration_path = custom_paths && params.getBool("AccelerationPath");
@@ -265,6 +269,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_ALWAYS_ON_LATERAL_ACTIVE;
} else {
status = controls_state.getEnabled() ? STATUS_ENGAGED : STATUS_DISENGAGED;
}
+4
View File
@@ -107,6 +107,7 @@ typedef enum UIStatus {
STATUS_ENGAGED,
// FrogPilot statuses
STATUS_ALWAYS_ON_LATERAL_ACTIVE,
} UIStatus;
@@ -126,6 +127,7 @@ const QColor bg_colors [] = {
[STATUS_ENGAGED] = QColor(0x17, 0x86, 0x44, 0xf1),
// FrogPilot colors
[STATUS_ALWAYS_ON_LATERAL_ACTIVE] = QColor(0x0a, 0xba, 0xb5, 0xf1),
};
static std::map<cereal::ControlsState::AlertStatus, QColor> alert_colors = {
@@ -170,10 +172,12 @@ typedef struct UIScene {
// FrogPilot variables
bool acceleration_path;
bool always_on_lateral_active;
bool enabled;
bool experimental_mode;
bool map_open;
bool right_hand_drive;
bool show_aol_status_bar;
float acceleration;