diff --git a/panda/board/safety.h b/panda/board/safety.h index 1516fac84..24ff77907 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 60fac77bd..a1934a6c2 100644 --- a/panda/board/safety/safety_chrysler.h +++ b/panda/board/safety/safety_chrysler.h @@ -192,6 +192,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 d6ee20803..e17cdf3cb 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 426f1d9f6..c0aa8701d 100644 --- a/panda/board/safety/safety_gm.h +++ b/panda/board/safety/safety_gm.h @@ -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) { diff --git a/panda/board/safety/safety_honda.h b/panda/board/safety/safety_honda.h index c6a907fbd..0e081e742 100644 --- a/panda/board/safety/safety_honda.h +++ b/panda/board/safety/safety_honda.h @@ -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; diff --git a/panda/board/safety/safety_hyundai_common.h b/panda/board/safety/safety_hyundai_common.h index 54ea0f024..f9ea3c7c7 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 ea4ce92d0..19e2dd120 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 c2406260f..e33e933f8 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 581dc50b7..6cf7910ba 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 1047814ac..7d44fb0c8 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 652161ff2..4c3a1575c 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 50c00b38a..e6d457afb 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 }}}, \ @@ -177,6 +179,10 @@ static void toyota_rx_hook(const CANPacket_t *to_push) { } } + 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 if (addr == 0x1D2) { diff --git a/panda/board/safety_declarations.h b/panda/board/safety_declarations.h index 6df5b18f9..e9b5cbf55 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 5728cfad9..110d45f50 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 c5e656e12..a668fd269 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -58,6 +58,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 diff --git a/selfdrive/car/chrysler/carcontroller.py b/selfdrive/car/chrysler/carcontroller.py index 639e4a62f..1b954a252 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 05b4bcab1..56d14ebe3 100644 --- a/selfdrive/car/chrysler/chryslercan.py +++ b/selfdrive/car/chrysler/chryslercan.py @@ -4,7 +4,7 @@ from openpilot.selfdrive.car.chrysler.values import ChryslerFlags, 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 ba228afc4..8a65c28a9 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 656f18184..2b595e0e4 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 22927e544..61417cc62 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 3c817db8a..0f40cdd49 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_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 diff --git a/selfdrive/car/hyundai/hyundaican.py b/selfdrive/car/hyundai/hyundaican.py index b4b951f89..81ff568a6 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 54804f94f..c26c42c02 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 2c3cd3faa..187010961 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.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( diff --git a/selfdrive/car/nissan/nissancan.py b/selfdrive/car/nissan/nissancan.py index b9a1b4f84..676ed89bf 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 6b935cbc8..b6239366c 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 86d39ff88..4036d7c61 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 53bcfaf4a..2fb528b06 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -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)) diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index f84dd9949..0c2f936b2 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/selfdrive/car/toyota/toyotacan.py @@ -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, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 2bbe65062..fb297f8f0 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -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) @@ -187,6 +188,7 @@ class Controls: self.total_kilometers = self.params_tracking.get_float("FrogPilotKilometers") self.total_minutes = self.params_tracking.get_float("FrogPilotMinutes") + self.always_on_lateral_active = False self.drive_added = False self.openpilot_crashed_triggered = False self.update_toggles = False @@ -541,7 +543,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): @@ -568,7 +570,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 @@ -891,6 +893,13 @@ 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 |= self.frogpilot_toggles.always_on_lateral_main or CS.cruiseState.enabled + self.always_on_lateral_active &= self.frogpilot_toggles.always_on_lateral and CS.cruiseState.available + 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 @@ -909,6 +918,7 @@ class Controls: self.drive_added = True FPCC = custom.FrogPilotCarControl.new_message() + FPCC.alwaysOnLateral = self.always_on_lateral_active return FPCC diff --git a/selfdrive/ui/qt/maps/map.cc b/selfdrive/ui/qt/maps/map.cc index b0efab38d..04a5a4cbd 100644 --- a/selfdrive/ui/qt/maps/map.cc +++ b/selfdrive/ui/qt/maps/map.cc @@ -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)); diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index d7a15e4e5..933a49ae0 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -150,6 +150,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/alerts.cc b/selfdrive/ui/qt/onroad/alerts.cc index 4d05451a1..10d484aa6 100644 --- a/selfdrive/ui/qt/onroad/alerts.cc +++ b/selfdrive/ui/qt/onroad/alerts.cc @@ -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; diff --git a/selfdrive/ui/qt/onroad/alerts.h b/selfdrive/ui/qt/onroad/alerts.h index 569c4b413..7672ba663 100644 --- a/selfdrive/ui/qt/onroad/alerts.h +++ b/selfdrive/ui/qt/onroad/alerts.h @@ -39,4 +39,7 @@ protected: QColor bg; Alert alert = {}; + + // FrogPilot variables + bool showAOLStatusBar; }; diff --git a/selfdrive/ui/qt/onroad/annotated_camera.cc b/selfdrive/ui/qt/onroad/annotated_camera.cc index 9720fd377..180813f66 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.cc +++ b/selfdrive/ui/qt/onroad/annotated_camera.cc @@ -287,7 +287,7 @@ void AnnotatedCameraWidget::drawDriverState(QPainter &painter, const UIState *s) if (rightHandDM && map_settings_btn->isEnabled()) { x -= 250; } - 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); @@ -495,7 +495,9 @@ void AnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &painter, const UISce alertSize = scene.alert_size; - if (true) { + alwaysOnLateralActive = scene.always_on_lateral_active; + showAlwaysOnLateralStatusBar = scene.show_aol_status_bar; + if (showAlwaysOnLateralStatusBar) { drawStatusBar(painter); } @@ -529,6 +531,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; diff --git a/selfdrive/ui/qt/onroad/annotated_camera.h b/selfdrive/ui/qt/onroad/annotated_camera.h index ce949e518..89ba14c0a 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.h +++ b/selfdrive/ui/qt/onroad/annotated_camera.h @@ -52,8 +52,10 @@ private: QHBoxLayout *bottom_layout; + bool alwaysOnLateralActive; bool experimentalMode; bool mapOpen; + bool showAlwaysOnLateralStatusBar; float accelerationConversion; float distanceConversion; diff --git a/selfdrive/ui/qt/onroad/buttons.cc b/selfdrive/ui/qt/onroad/buttons.cc index c1653e4d9..177b9b92d 100644 --- a/selfdrive/ui/qt/onroad/buttons.cc +++ b/selfdrive/ui/qt/onroad/buttons.cc @@ -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(); diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index df00bf26b..619848da7 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -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(); @@ -269,6 +270,9 @@ void ui_update_frogpilot_params(UIState *s, Params ¶ms) { scene.longitudinal_control = hasLongitudinalControl(CP); } + 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); @@ -281,6 +285,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; } diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 414d2478a..0d27b0db1 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -56,6 +56,7 @@ typedef enum UIStatus { STATUS_ENGAGED, // FrogPilot statuses + STATUS_ALWAYS_ON_LATERAL_ACTIVE, STATUS_EXPERIMENTAL_MODE_ACTIVE, STATUS_NAVIGATION_ACTIVE, } UIStatus; @@ -77,6 +78,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), }; @@ -116,12 +118,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;