mawei
/
ods777
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Controls - Always On Lateral 

Maintain openpilot lateral control when the brake or gas pedals are used. Deactivation occurs only through the 'Cruise Control' button.

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-06-08 07:01:59 -07:00
parent 275e346ed2
commit cf92c3af1b
39 changed files with 149 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
panda/board/safety.h
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
panda/board/safety/safety_chrysler.h
View File

@ -186,6 +186,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
panda/board/safety/safety_ford.h
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);
}

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

@ -143,7 +143,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) {

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

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

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

@ -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
panda/board/safety/safety_mazda.h
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
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(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
panda/board/safety/safety_subaru.h
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);
}

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

@ -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
panda/board/safety/safety_tesla.h
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
panda/board/safety/safety_toyota.h
View File

@ -53,6 +53,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
@ -71,6 +72,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 }}}, \
@ -176,6 +178,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
panda/board/safety_declarations.h
View File

@ -224,6 +224,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 32

1
panda/python/__init__.py
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:

5
selfdrive/car/card.py
View File

@ -61,6 +61,11 @@ class Car:
if not self.disengage_on_accelerator:
self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS
# FrogPilot alternative experiences
if self.params.get_bool("AlwaysOnLateral"):
self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL
self.params.put_bool("AlwaysOnLateralSet", True)
openpilot_enabled_toggle = self.params.get_bool("OpenpilotEnabledToggle")
controller_available = self.CI.CC is not None and openpilot_enabled_toggle and not self.CP.dashcamOnly

2
selfdrive/car/chrysler/carcontroller.py
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

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

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

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

@ -140,7 +140,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)
@ -175,7 +175,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,
}

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

@ -118,7 +118,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:
@ -149,11 +149,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:

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

@ -52,6 +52,9 @@ class CarState(CarStateBase):
self.params = CarControllerParams(CP)
# FrogPilot variables
self.main_enabled = False
def update(self, cp, cp_cam, frogpilot_toggles):
if self.CP.carFingerprint in CANFD_CAR:
return self.update_canfd(cp, cp_cam, frogpilot_toggles)
@ -103,7 +106,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 +167,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, fp_ret
@ -220,7 +226,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
@ -241,7 +247,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, 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,

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 enabled else 1 if lat_active else 0, # HDA2
"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 enabled else 1 if lat_active else 0, # HDA1
}
return packer.make_can_msg("LFAHDA_CLUSTER", CAN.ECAN, values)

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

@ -68,7 +68,7 @@ class CarController(CarControllerBase):
if self.CP.carFingerprint != CAR.NISSAN_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(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))

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

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

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

@ -73,13 +73,13 @@ 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,
"BARRIERS": 1 if enabled else 0,
"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 lat_active else 0,
# static signals
"SET_ME_X02": 2,

15
selfdrive/controls/controlsd.py
View File

@ -52,6 +52,7 @@ LaneChangeState = log.LaneChangeState
LaneChangeDirection = log.LaneChangeDirection
EventName = car.CarEvent.EventName
ButtonType = car.CarState.ButtonEvent.Type
GearShifter = car.CarState.GearShifter
SafetyModel = car.CarParams.SafetyModel
IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput)
@ -72,6 +73,7 @@ class Controls:
self.current_total_drives = self.params_tracking.get_int("FrogPilotDrives")
self.current_total_time = self.params_tracking.get_float("FrogPilotMinutes")
self.always_on_lateral_active = False
self.drive_added = False
self.openpilot_crashed_triggered = False
@ -540,7 +542,7 @@ class Controls:
# Check if openpilot is engaged and actuators are enabled
self.enabled = self.state in ENABLED_STATES
self.active = self.state in ACTIVE_STATES
if self.active:
if self.active or self.always_on_lateral_active:
self.current_alert_types.append(ET.WARNING)
def state_control(self, CS):
@ -567,7 +569,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.always_on_lateral_active) 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
@ -887,6 +889,14 @@ class Controls:
self.openpilot_crashed_triggered = True
def update_frogpilot_variables(self, CS):
driving_gear = CS.gearShifter not in (GearShifter.neutral, GearShifter.park, GearShifter.reverse, GearShifter.unknown)
self.always_on_lateral_active |= CS.cruiseState.enabled or self.frogpilot_toggles.always_on_lateral_main
self.always_on_lateral_active &= CS.cruiseState.available
self.always_on_lateral_active &= self.frogpilot_toggles.always_on_lateral
self.always_on_lateral_active &= driving_gear
self.always_on_lateral_active &= not (CS.brakePressed and CS.vEgo < self.frogpilot_toggles.always_on_lateral_pause_speed) or CS.standstill
self.drive_distance += CS.vEgo * DT_CTRL
self.drive_time += DT_CTRL
@ -907,6 +917,7 @@ class Controls:
self.drive_added = True
FPCC = custom.FrogPilotCarControl.new_message()
FPCC.alwaysOnLateral = self.always_on_lateral_active
return FPCC

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

@ -174,7 +174,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
selfdrive/ui/qt/offroad/settings.cc
View File

@ -145,6 +145,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>"

4
selfdrive/ui/qt/onroad/alerts.cc
View File

@ -14,6 +14,8 @@ void OnroadAlerts::updateState(const UIState &s) {
// FrogPilot variables
const UIScene &scene = s.scene;
showAOLStatusBar = scene.show_aol_status_bar;
}
void OnroadAlerts::clear() {
@ -70,7 +72,7 @@ void OnroadAlerts::paintEvent(QPaintEvent *event) {
int margin = 40;
int radius = 30;
int offset = true ? 25 : 0;
int offset = showAOLStatusBar ? 25 : 0;
if (alert.size == cereal::ControlsState::AlertSize::FULL) {
margin = 0;
radius = 0;

3
selfdrive/ui/qt/onroad/alerts.h
View File

@ -39,4 +39,7 @@ protected:
QColor bg;
Alert alert = {};
// FrogPilot variables
bool showAOLStatusBar;
};

11
selfdrive/ui/qt/onroad/annotated_camera.cc
View File

@ -289,7 +289,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);
@ -494,13 +494,16 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets(const UIScene &scene) {
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);
}
@ -531,6 +534,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) {
lastShownStatus = newStatus;
displayStatusText = true;

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

@ -53,8 +53,10 @@ private:
QHBoxLayout *bottom_layout;
bool alwaysOnLateralActive;
bool experimentalMode;
bool mapOpen;
bool showAlwaysOnLateralStatusBar;
float accelerationConversion;
float distanceConversion;

2
selfdrive/ui/qt/onroad/buttons.cc
View File

@ -34,7 +34,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() || s.scene.always_on_lateral_active;
if ((cs.getExperimentalMode() != experimental_mode) || (eng != engageable)) {
engageable = eng;
experimental_mode = cs.getExperimentalMode();

6
selfdrive/ui/ui.cc
View File

@ -221,6 +221,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("frogpilotCarState")) {
auto frogpilotCarState = sm["frogpilotCarState"].getFrogpilotCarState();
@ -260,6 +261,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");
scene.tethering_config = params.getInt("TetheringEnabled");
if (scene.tethering_config == 2) {
WifiManager(s).setTetheringEnabled(true);
@ -272,6 +276,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 = scene.enabled ? STATUS_ENGAGED : STATUS_DISENGAGED;
}

4
selfdrive/ui/ui.h
View File

@ -55,6 +55,7 @@ typedef enum UIStatus {
STATUS_ENGAGED,
// FrogPilot statuses
STATUS_ALWAYS_ON_LATERAL_ACTIVE,
STATUS_EXPERIMENTAL_MODE_ACTIVE,
STATUS_NAVIGATION_ACTIVE,
} UIStatus;
@ -76,6 +77,7 @@ const QColor bg_colors [] = {
[STATUS_ENGAGED] = QColor(0x17, 0x86, 0x44, 0xf1),
// FrogPilot colors
[STATUS_ALWAYS_ON_LATERAL_ACTIVE] = QColor(0x0a, 0xba, 0xb5, 0xf1),
[STATUS_EXPERIMENTAL_MODE_ACTIVE] = QColor(0xda, 0x6f, 0x25, 0xf1),
[STATUS_NAVIGATION_ACTIVE] = QColor(0x31, 0xa1, 0xee, 0xf1),
};
@ -115,12 +117,14 @@ typedef struct UIScene {
uint64_t started_frame;
// FrogPilot variables
bool always_on_lateral_active;
bool enabled;
bool experimental_mode;
bool map_open;
bool online;
bool parked;
bool right_hand_drive;
bool show_aol_status_bar;
bool tethering_enabled;
int alert_size;