diff --git a/panda/board/safety.h b/panda/board/safety.h index 1516fac..24ff779 100644 --- a/panda/board/safety.h +++ b/panda/board/safety.h @@ -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; } diff --git a/panda/board/safety/safety_chrysler.h b/panda/board/safety/safety_chrysler.h index fa0e153..b80e3e8 100644 --- a/panda/board/safety/safety_chrysler.h +++ b/panda/board/safety/safety_chrysler.h @@ -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); } diff --git a/panda/board/safety/safety_ford.h b/panda/board/safety/safety_ford.h index d6ee208..e17cdf3 100644 --- a/panda/board/safety/safety_ford.h +++ b/panda/board/safety/safety_ford.h @@ -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); } diff --git a/panda/board/safety/safety_gm.h b/panda/board/safety/safety_gm.h index 44e06cd..d722e36 100644 --- a/panda/board/safety/safety_gm.h +++ b/panda/board/safety/safety_gm.h @@ -144,7 +144,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) { diff --git a/panda/board/safety/safety_honda.h b/panda/board/safety/safety_honda.h index bd73ea4..ca4527b 100644 --- a/panda/board/safety/safety_honda.h +++ b/panda/board/safety/safety_honda.h @@ -339,7 +339,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; diff --git a/panda/board/safety/safety_hyundai_common.h b/panda/board/safety/safety_hyundai_common.h index 54ea0f0..f9ea3c7 100644 --- a/panda/board/safety/safety_hyundai_common.h +++ b/panda/board/safety/safety_hyundai_common.h @@ -63,6 +63,10 @@ void hyundai_common_cruise_state_check(const 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 { diff --git a/panda/board/safety/safety_mazda.h b/panda/board/safety/safety_mazda.h index 7c6d8be..b3a567f 100644 --- a/panda/board/safety/safety_mazda.h +++ b/panda/board/safety/safety_mazda.h @@ -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); } diff --git a/panda/board/safety/safety_nissan.h b/panda/board/safety/safety_nissan.h index c240626..b8db422 100644 --- a/panda/board/safety/safety_nissan.h +++ b/panda/board/safety/safety_nissan.h @@ -21,6 +21,8 @@ const CanMsg NISSAN_TX_MSGS[] = { // Signals duplicated below due to the fact that these messages can come in on either CAN bus, depending on car model. RxCheck nissan_rx_checks[] = { + {.msg = {{0x1b6, 0, 8, .frequency = 100U}, + {0x1b6, 1, 8, .frequency = 100U}, { 0 }}}, // PRO_PILOT (100HZ) {.msg = {{0x2, 0, 5, .frequency = 100U}, {0x2, 1, 5, .frequency = 100U}, { 0 }}}, // STEER_ANGLE_SENSOR {.msg = {{0x285, 0, 8, .frequency = 50U}, @@ -64,11 +66,16 @@ static void nissan_rx_hook(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; } } diff --git a/panda/board/safety/safety_subaru.h b/panda/board/safety/safety_subaru.h index c445bc4..924b767 100644 --- a/panda/board/safety/safety_subaru.h +++ b/panda/board/safety/safety_subaru.h @@ -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); } diff --git a/panda/board/safety/safety_subaru_preglobal.h b/panda/board/safety/safety_subaru_preglobal.h index 1047814..7d44fb0 100644 --- a/panda/board/safety/safety_subaru_preglobal.h +++ b/panda/board/safety/safety_subaru_preglobal.h @@ -56,6 +56,7 @@ static void subaru_preglobal_rx_hook(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); } diff --git a/panda/board/safety/safety_tesla.h b/panda/board/safety/safety_tesla.h index 652161f..4c3a157 100644 --- a/panda/board/safety/safety_tesla.h +++ b/panda/board/safety/safety_tesla.h @@ -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 diff --git a/panda/board/safety/safety_toyota.h b/panda/board/safety/safety_toyota.h index 50c00b3..0e9742c 100644 --- a/panda/board/safety/safety_toyota.h +++ b/panda/board/safety/safety_toyota.h @@ -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 diff --git a/panda/board/safety_declarations.h b/panda/board/safety_declarations.h index 64b55f2..686e4e1 100644 --- a/panda/board/safety_declarations.h +++ b/panda/board/safety_declarations.h @@ -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 diff --git a/panda/python/__init__.py b/panda/python/__init__.py index dd4e010..95ebe5d 100644 --- a/panda/python/__init__.py +++ b/panda/python/__init__.py @@ -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: diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 38a3efe..6ba7a7b 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -54,6 +54,10 @@ class CarD: if not disengage_on_accelerator: self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS + self.always_on_lateral = self.params.get_bool("AlwaysOnLateral") + if self.always_on_lateral: + self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL + car_recognized = self.CP.carName != 'mock' openpilot_enabled_toggle = self.params.get_bool("OpenpilotEnabledToggle") diff --git a/selfdrive/car/chrysler/carcontroller.py b/selfdrive/car/chrysler/carcontroller.py index f7ff245..8c72b76 100644 --- a/selfdrive/car/chrysler/carcontroller.py +++ b/selfdrive/car/chrysler/carcontroller.py @@ -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 diff --git a/selfdrive/car/chrysler/chryslercan.py b/selfdrive/car/chrysler/chryslercan.py index 96439f3..f1a5d8f 100644 --- a/selfdrive/car/chrysler/chryslercan.py +++ b/selfdrive/car/chrysler/chryslercan.py @@ -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) diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 5c7ece7..36c4280 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -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 diff --git a/selfdrive/car/honda/hondacan.py b/selfdrive/car/honda/hondacan.py index 515b52c..2374203 100644 --- a/selfdrive/car/honda/hondacan.py +++ b/selfdrive/car/honda/hondacan.py @@ -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, } diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 015e5ea..479cf2c 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -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: diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 1d7cce3..f484a48 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -52,6 +52,9 @@ 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: return self.update_canfd(cp, cp_cam, frogpilot_variables) @@ -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 @@ -163,7 +166,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 @@ -219,7 +225,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 +246,10 @@ class CarState(CarStateBase): self.prev_cruise_buttons = self.cruise_buttons[-1] self.cruise_buttons.extend(cp.vl_all[self.cruise_btns_msg_canfd]["CRUISE_BUTTONS"]) + self.prev_main_buttons = self.main_buttons[-1] self.main_buttons.extend(cp.vl_all[self.cruise_btns_msg_canfd]["ADAPTIVE_CRUISE_MAIN_BTN"]) + if self.prev_main_buttons == 0 and self.main_buttons[-1] != 0: + self.main_enabled = not self.main_enabled self.buttons_counter = cp.vl[self.cruise_btns_msg_canfd]["COUNTER"] ret.accFaulted = cp.vl["TCS"]["ACCEnable"] != 0 # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED diff --git a/selfdrive/car/hyundai/hyundaican.py b/selfdrive/car/hyundai/hyundaican.py index a57f3e2..0e75cc1 100644 --- a/selfdrive/car/hyundai/hyundaican.py +++ b/selfdrive/car/hyundai/hyundaican.py @@ -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, diff --git a/selfdrive/car/hyundai/hyundaicanfd.py b/selfdrive/car/hyundai/hyundaicanfd.py index 54804f9..c26c42c 100644 --- a/selfdrive/car/hyundai/hyundaicanfd.py +++ b/selfdrive/car/hyundai/hyundaicanfd.py @@ -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) diff --git a/selfdrive/car/nissan/carcontroller.py b/selfdrive/car/nissan/carcontroller.py index 869bcd8..0d4e857 100644 --- a/selfdrive/car/nissan/carcontroller.py +++ b/selfdrive/car/nissan/carcontroller.py @@ -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( diff --git a/selfdrive/car/nissan/nissancan.py b/selfdrive/car/nissan/nissancan.py index 49dcd6f..d7ff55e 100644 --- a/selfdrive/car/nissan/nissancan.py +++ b/selfdrive/car/nissan/nissancan.py @@ -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) diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index c25fb16..d6f612c 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -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)) diff --git a/selfdrive/car/subaru/subarucan.py b/selfdrive/car/subaru/subarucan.py index 86d39ff..4036d7c 100644 --- a/selfdrive/car/subaru/subarucan.py +++ b/selfdrive/car/subaru/subarucan.py @@ -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 diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 22583d9..a55afb3 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -195,7 +195,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)) diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index f7822da..9b0bfbe 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/selfdrive/car/toyota/toyotacan.py @@ -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 diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index f40135c..fc718b3 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -92,7 +92,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 diff --git a/selfdrive/car/volkswagen/mqbcan.py b/selfdrive/car/volkswagen/mqbcan.py index 6043533..a0aa144 100644 --- a/selfdrive/car/volkswagen/mqbcan.py +++ b/selfdrive/car/volkswagen/mqbcan.py @@ -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, diff --git a/selfdrive/car/volkswagen/pqcan.py b/selfdrive/car/volkswagen/pqcan.py index 307aaaa..8b97ba4 100644 --- a/selfdrive/car/volkswagen/pqcan.py +++ b/selfdrive/car/volkswagen/pqcan.py @@ -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, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index c774114..736f4bb 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -551,6 +551,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""" @@ -575,7 +578,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 @@ -913,6 +916,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.frogpilot_toggles.always_on_lateral_main + self.FPCC.alwaysOnLateral &= CS.cruiseState.available + self.FPCC.alwaysOnLateral &= self.card.always_on_lateral + self.FPCC.alwaysOnLateral &= self.driving_gear + self.FPCC.alwaysOnLateral &= 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 diff --git a/selfdrive/ui/qt/maps/map.cc b/selfdrive/ui/qt/maps/map.cc index 66b442f..9e54daf 100644 --- a/selfdrive/ui/qt/maps/map.cc +++ b/selfdrive/ui/qt/maps/map.cc @@ -165,7 +165,7 @@ void MapWindow::updateState(const UIState &s) { if (sm.updated("modelV2")) { // set path color on change, and show map on rising edge of navigate on openpilot bool nav_enabled = sm["modelV2"].getModelV2().getNavEnabled() && - sm["controlsState"].getControlsState().getEnabled(); + (sm["controlsState"].getControlsState().getEnabled() || sm["frogpilotCarControl"].getFrogpilotCarControl().getAlwaysOnLateral()); if (nav_enabled != uiState()->scene.navigate_on_openpilot) { if (loaded_once) { m_map->setPaintProperty("navLayer", "line-color", getNavPathColor(nav_enabled)); diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index 76c8ea7..4a29be0 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -157,6 +157,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
" diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 75f67b8..c1f2eab 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -171,7 +171,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; @@ -236,7 +236,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(); @@ -544,7 +544,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); @@ -749,13 +749,16 @@ 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); } @@ -786,6 +789,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; diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index 8d1d317..fae6eb2 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -118,8 +118,10 @@ private: QHBoxLayout *bottom_layout; + bool alwaysOnLateralActive; bool experimentalMode; bool mapOpen; + bool showAlwaysOnLateralStatusBar; float accelerationConversion; float distanceConversion; diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index fc726c5..ebeb932 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -223,6 +223,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(); @@ -259,6 +260,9 @@ void ui_update_params(UIState *s) { 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"); } void UIState::updateStatus() { @@ -267,6 +271,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; } diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 8348b42..e972f15 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -115,6 +115,7 @@ typedef enum UIStatus { STATUS_ENGAGED, // FrogPilot statuses + STATUS_ALWAYS_ON_LATERAL_ACTIVE, STATUS_EXPERIMENTAL_MODE_ACTIVE, STATUS_NAVIGATION_ACTIVE, } UIStatus; @@ -135,6 +136,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), }; @@ -180,12 +182,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;