diff --git a/opendbc_repo/opendbc/car/hyundai/carcontroller.py b/opendbc_repo/opendbc/car/hyundai/carcontroller.py index 3ae2fb092..750cfdde8 100644 --- a/opendbc_repo/opendbc/car/hyundai/carcontroller.py +++ b/opendbc_repo/opendbc/car/hyundai/carcontroller.py @@ -48,6 +48,7 @@ IONIQ_6_DYNAMIC_LOWER_JERK_BP = [-2.0, -1.5, -1.0, -0.25, -0.1, -0.025, -0.01, - IONIQ_6_DYNAMIC_LOWER_JERK_V = [3.3, 1.5, 1.0, 0.8, 0.7, 0.65, 0.55, 0.5] IONIQ_6_LAUNCH_HOLD_SPEED_BP = [0.0, 0.6, 1.25, 2.5] IONIQ_6_LAUNCH_HOLD_SPEED_V = [0.75, 0.6, 0.4, 0.0] +IONIQ_6_STOP_BRAKE_CAP_MAX_SPEED = 2.0 IONIQ_6_STOP_BRAKE_CAP_SPEED_BP = [0.0, 0.08, 0.25, 0.6, 1.2, 2.0, 3.0] IONIQ_6_STOP_BRAKE_CAP_ACCEL_V = [-0.09, -0.10, -0.11, -0.22, -0.50, -0.95, -1.40] IONIQ_6_STOP_HOLD_JERK_BP = [0.0, 0.15, 0.6, 1.2, 2.0, 3.0] @@ -133,9 +134,12 @@ def update_ioniq_6_longitudinal_tuning(state: Ioniq6LongitudinalTuningState, acc state.jerk_lower = min(dynamic_lower_jerk, lower_speed_limit) if state.stopping: - stop_brake_cap = float(np.interp(v_ego, IONIQ_6_STOP_BRAKE_CAP_SPEED_BP, IONIQ_6_STOP_BRAKE_CAP_ACCEL_V)) - state.desired_accel = min(0.0, max(accel_cmd, stop_brake_cap)) - state.jerk_upper = min(state.jerk_upper, float(np.interp(v_ego, IONIQ_6_STOP_HOLD_JERK_BP, IONIQ_6_STOP_HOLD_JERK_V)) * IONIQ_6_RESPONSE_MULTIPLIER) + if v_ego <= IONIQ_6_STOP_BRAKE_CAP_MAX_SPEED: + stop_brake_cap = float(np.interp(v_ego, IONIQ_6_STOP_BRAKE_CAP_SPEED_BP, IONIQ_6_STOP_BRAKE_CAP_ACCEL_V)) + state.desired_accel = min(0.0, max(accel_cmd, stop_brake_cap)) + state.jerk_upper = min(state.jerk_upper, float(np.interp(v_ego, IONIQ_6_STOP_HOLD_JERK_BP, IONIQ_6_STOP_HOLD_JERK_V)) * IONIQ_6_RESPONSE_MULTIPLIER) + else: + state.desired_accel = float(np.clip(accel_cmd, CarControllerParams.ACCEL_MIN, 0.0)) else: state.desired_accel = float(np.clip(accel_cmd, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)) if state.launch_active: diff --git a/opendbc_repo/opendbc/car/hyundai/interface.py b/opendbc_repo/opendbc/car/hyundai/interface.py index 4b8364031..a4d68dc44 100644 --- a/opendbc_repo/opendbc/car/hyundai/interface.py +++ b/opendbc_repo/opendbc/car/hyundai/interface.py @@ -208,8 +208,8 @@ class CarInterface(CarInterfaceBase): ret.vEgoStopping = 0.35 if candidate == CAR.KIA_NIRO_PHEV_2022: - ret.stopAccel = -1.5 - ret.stoppingDecelRate = 0.55 + ret.stopAccel = -1.4 + ret.stoppingDecelRate = 0.5 ret.vEgoStopping = 0.7 if candidate == CAR.KIA_OPTIMA_G4_FL: diff --git a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py index 745357f36..99147bc24 100644 --- a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py @@ -183,9 +183,9 @@ class TestHyundaiFingerprint: toggles = get_test_toggles() CP = CarInterface.get_params(CAR.KIA_NIRO_PHEV_2022, gen_empty_fingerprint(), [], True, False, False, toggles) - assert CP.stopAccel == pytest.approx(-1.5) + assert CP.stopAccel == pytest.approx(-1.4) assert CP.vEgoStopping == pytest.approx(0.7) - assert CP.stoppingDecelRate == pytest.approx(0.55) + assert CP.stoppingDecelRate == pytest.approx(0.5) def test_kia_forte_no_scc_fw_match(self): car_fw = [ @@ -390,20 +390,30 @@ class TestHyundaiFingerprint: assert state.jerk_upper == pytest.approx(0.42) assert state.actual_accel == pytest.approx(-0.099) - def test_ioniq_6_longitudinal_tuning_helper_caps_late_low_speed_stop_brake(self): + def test_ioniq_6_longitudinal_tuning_helper_caps_final_low_speed_stop_brake(self): state = Ioniq6LongitudinalTuningState(actual_accel=-2.82, accel_last=-2.82, long_control_state_last=LongCtrlState.pid) - state = update_ioniq_6_longitudinal_tuning(state, accel_cmd=-2.82, v_ego=2.5, a_ego=-2.4, + state = update_ioniq_6_longitudinal_tuning(state, accel_cmd=-2.82, v_ego=1.8, a_ego=-2.4, long_control_state=LongCtrlState.stopping, long_active=True) assert state.stopping - assert state.desired_accel == pytest.approx(-1.175) + assert state.desired_accel == pytest.approx(-0.8375) for _ in range(10): - state = update_ioniq_6_longitudinal_tuning(state, accel_cmd=-2.82, v_ego=2.5, a_ego=-2.4, + state = update_ioniq_6_longitudinal_tuning(state, accel_cmd=-2.82, v_ego=1.8, a_ego=-2.4, long_control_state=LongCtrlState.stopping, long_active=True) assert state.actual_accel == pytest.approx(-2.49) + def test_ioniq_6_longitudinal_tuning_helper_keeps_full_stop_authority_above_final_band(self): + state = Ioniq6LongitudinalTuningState(actual_accel=-1.8, accel_last=-1.8, + long_control_state_last=LongCtrlState.pid) + + state = update_ioniq_6_longitudinal_tuning(state, accel_cmd=-2.82, v_ego=3.8, a_ego=-1.8, + long_control_state=LongCtrlState.stopping, long_active=True) + assert state.stopping + assert state.desired_accel == pytest.approx(-2.82) + assert state.actual_accel < -1.8 + def test_genesis_g90_longitudinal_tuning_softens_final_stop_hold(self): state = GenesisG90LongitudinalTuningState() diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index d2c7db9ea..dcdeed71d 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -66,16 +66,16 @@ CIVIC_BOSCH_MODIFIED_B_TURN_IN_FRICTION_BOOST_LEFT = 0.02 CIVIC_BOSCH_MODIFIED_B_TURN_IN_FRICTION_BOOST_RIGHT = 0.00 CIVIC_BOSCH_MODIFIED_B_UNWIND_FRICTION_REDUCTION_LEFT = 0.26 CIVIC_BOSCH_MODIFIED_B_UNWIND_FRICTION_REDUCTION_RIGHT = 0.40 -CIVIC_BOSCH_MODIFIED_A_VARIANT_FF_RESTORE_LEFT = 0.00 -CIVIC_BOSCH_MODIFIED_A_VARIANT_FF_RESTORE_RIGHT = 0.00 +CIVIC_BOSCH_MODIFIED_A_VARIANT_FF_RESTORE_LEFT = -0.03 +CIVIC_BOSCH_MODIFIED_A_VARIANT_FF_RESTORE_RIGHT = 0.05 CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_BOOST_LEFT = 0.00 -CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_BOOST_RIGHT = 0.00 -CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_TAPER_LEFT = 0.04 -CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_TAPER_RIGHT = 0.10 +CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_BOOST_RIGHT = 0.04 +CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_TAPER_LEFT = 0.10 +CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_TAPER_RIGHT = 0.04 CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_FRICTION_BOOST_LEFT = 0.00 -CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_FRICTION_BOOST_RIGHT = 0.00 -CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_FRICTION_REDUCTION_LEFT = 0.03 -CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_FRICTION_REDUCTION_RIGHT = 0.07 +CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_FRICTION_BOOST_RIGHT = 0.03 +CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_FRICTION_REDUCTION_LEFT = 0.08 +CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_FRICTION_REDUCTION_RIGHT = 0.03 CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_MAX = 0.12 CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_LAT = 0.24 CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_LAT_WIDTH = 0.05 @@ -116,6 +116,10 @@ GENESIS_G90_CARS = ( IONIQ_5_CARS = ( HYUNDAI_CAR.HYUNDAI_IONIQ_5, ) +IONIQ_EV_OLD_CARS = ( + HYUNDAI_CAR.HYUNDAI_IONIQ_EV_LTD, + HYUNDAI_CAR.HYUNDAI_IONIQ_EV_2020, +) IONIQ_6_CARS = ( HYUNDAI_CAR.HYUNDAI_IONIQ_6, ) @@ -234,18 +238,18 @@ VOLT_STANDARD_CENTER_TAPER_SPEED = 20.0 VOLT_STANDARD_CENTER_TAPER_SPEED_WIDTH = 2.5 SONATA_HYBRID_BASE_LAT_ACCEL_FACTOR_MULT = 1.04 -SONATA_HYBRID_FF_REDUCTION_LEFT = 0.09 -SONATA_HYBRID_FF_REDUCTION_RIGHT = 0.20 +SONATA_HYBRID_FF_REDUCTION_LEFT = 0.07 +SONATA_HYBRID_FF_REDUCTION_RIGHT = 0.22 SONATA_HYBRID_FF_ONSET = 0.22 SONATA_HYBRID_FF_ONSET_WIDTH = 0.08 SONATA_HYBRID_FF_CUTOFF = 1.35 SONATA_HYBRID_FF_CUTOFF_WIDTH = 0.40 SONATA_HYBRID_TRANSITION_SPEED = 8.0 SONATA_HYBRID_PHASE_SCALE = 0.12 -SONATA_HYBRID_TURN_IN_BOOST_LEFT = 0.08 +SONATA_HYBRID_TURN_IN_BOOST_LEFT = 0.12 SONATA_HYBRID_TURN_IN_BOOST_RIGHT = 0.00 -SONATA_HYBRID_UNWIND_TAPER_LEFT = 0.14 -SONATA_HYBRID_UNWIND_TAPER_RIGHT = 0.12 +SONATA_HYBRID_UNWIND_TAPER_LEFT = 0.18 +SONATA_HYBRID_UNWIND_TAPER_RIGHT = 0.10 SONATA_HYBRID_CENTER_TAPER_MAX = 0.05 SONATA_HYBRID_CENTER_TAPER_LAT = 0.14 SONATA_HYBRID_CENTER_TAPER_LAT_WIDTH = 0.025 @@ -304,6 +308,25 @@ IONIQ_5_TURN_IN_FRICTION_BOOST_RIGHT = 0.00 IONIQ_5_UNWIND_FRICTION_REDUCTION_LEFT = 0.15 IONIQ_5_UNWIND_FRICTION_REDUCTION_RIGHT = 0.26 +IONIQ_EV_OLD_BASE_LAT_ACCEL_FACTOR_MULT = 1.08 +IONIQ_EV_OLD_FF_REDUCTION_LEFT = 0.08 +IONIQ_EV_OLD_FF_REDUCTION_RIGHT = 0.16 +IONIQ_EV_OLD_FF_ONSET = 0.14 +IONIQ_EV_OLD_FF_ONSET_WIDTH = 0.05 +IONIQ_EV_OLD_FF_CUTOFF = 1.10 +IONIQ_EV_OLD_FF_CUTOFF_WIDTH = 0.30 +IONIQ_EV_OLD_TRANSITION_SPEED = 10.0 +IONIQ_EV_OLD_PHASE_SCALE = 0.10 +IONIQ_EV_OLD_TURN_IN_BOOST_LEFT = 0.06 +IONIQ_EV_OLD_TURN_IN_BOOST_RIGHT = 0.00 +IONIQ_EV_OLD_UNWIND_TAPER_LEFT = 0.18 +IONIQ_EV_OLD_UNWIND_TAPER_RIGHT = 0.08 +IONIQ_EV_OLD_CENTER_TAPER_MAX = 0.10 +IONIQ_EV_OLD_CENTER_TAPER_LAT = 0.14 +IONIQ_EV_OLD_CENTER_TAPER_LAT_WIDTH = 0.03 +IONIQ_EV_OLD_CENTER_TAPER_SPEED = 24.0 +IONIQ_EV_OLD_CENTER_TAPER_SPEED_WIDTH = 2.2 + IONIQ_6_FF_GAIN_LEFT = 0.045 IONIQ_6_FF_GAIN_RIGHT = 0.015 IONIQ_6_BASE_LAT_ACCEL_FACTOR_MULT = 1.22 @@ -1060,6 +1083,48 @@ def get_ioniq_5_friction_scale(v_ego: float, desired_lateral_accel: float, desir return min(max(friction_scale, 0.86), 1.04) +def _ioniq_ev_old_sigmoid(x: float) -> float: + return _sigmoid(x) + + +def _ioniq_ev_old_low_speed_factor(v_ego: float) -> float: + return 1.0 / (1.0 + (max(v_ego, 0.0) / IONIQ_EV_OLD_TRANSITION_SPEED) ** 2) + + +def _ioniq_ev_old_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + return math.tanh((desired_lateral_accel * desired_lateral_jerk) / IONIQ_EV_OLD_PHASE_SCALE) + + +def _ioniq_ev_old_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float: + return left_value if desired_lateral_accel >= 0.0 else right_value + + +def get_ioniq_ev_old_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: + if desired_lateral_accel == 0.0: + return 1.0 + + abs_lateral_accel = abs(desired_lateral_accel) + onset = _ioniq_ev_old_sigmoid((abs_lateral_accel - IONIQ_EV_OLD_FF_ONSET) / IONIQ_EV_OLD_FF_ONSET_WIDTH) + cutoff = _ioniq_ev_old_sigmoid((IONIQ_EV_OLD_FF_CUTOFF - abs_lateral_accel) / IONIQ_EV_OLD_FF_CUTOFF_WIDTH) + base_reduction = _ioniq_ev_old_side_value(desired_lateral_accel, IONIQ_EV_OLD_FF_REDUCTION_LEFT, IONIQ_EV_OLD_FF_REDUCTION_RIGHT) * onset * cutoff + phase = _ioniq_ev_old_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + low_speed_factor = _ioniq_ev_old_low_speed_factor(v_ego) + turn_in_boost = 1.0 + (_ioniq_ev_old_side_value(desired_lateral_accel, IONIQ_EV_OLD_TURN_IN_BOOST_LEFT, IONIQ_EV_OLD_TURN_IN_BOOST_RIGHT) * + turn_in_weight * (0.35 + 0.65 * low_speed_factor)) + unwind_taper = 1.0 - (_ioniq_ev_old_side_value(desired_lateral_accel, IONIQ_EV_OLD_UNWIND_TAPER_LEFT, IONIQ_EV_OLD_UNWIND_TAPER_RIGHT) * + unwind_weight * (0.35 + 0.65 * low_speed_factor)) + return (1.0 - base_reduction) * turn_in_boost * max(unwind_taper, 0.0) + + +def get_ioniq_ev_old_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: + speed_weight = _ioniq_ev_old_sigmoid((v_ego - IONIQ_EV_OLD_CENTER_TAPER_SPEED) / IONIQ_EV_OLD_CENTER_TAPER_SPEED_WIDTH) + center_weight = _ioniq_ev_old_sigmoid((IONIQ_EV_OLD_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / IONIQ_EV_OLD_CENTER_TAPER_LAT_WIDTH) + reduction = IONIQ_EV_OLD_CENTER_TAPER_MAX * speed_weight * center_weight + return 1.0 - reduction + + def _ioniq_6_sigmoid(x: float) -> float: return _sigmoid(x) @@ -1352,6 +1417,7 @@ class LatControlTorque(LatControl): self.is_volt_standard = CP.carFingerprint in VOLT_STANDARD_CARS self.is_genesis_g90 = CP.carFingerprint in GENESIS_G90_CARS self.is_ioniq_5 = CP.carFingerprint in IONIQ_5_CARS + self.is_ioniq_ev_old = CP.carFingerprint in IONIQ_EV_OLD_CARS self.is_ioniq_6 = CP.carFingerprint in IONIQ_6_CARS self.is_sonata_hybrid = CP.carFingerprint in SONATA_HYBRID_CARS self.is_kia_ev6 = CP.carFingerprint in KIA_EV6_CARS @@ -1366,6 +1432,8 @@ class LatControlTorque(LatControl): self.torque_ki_mult = 1.0 if self.is_ioniq_5: self.torque_params.latAccelFactor *= IONIQ_5_BASE_LAT_ACCEL_FACTOR_MULT + if self.is_ioniq_ev_old: + self.torque_params.latAccelFactor *= IONIQ_EV_OLD_BASE_LAT_ACCEL_FACTOR_MULT if self.is_ioniq_6: self.torque_params.latAccelFactor *= IONIQ_6_BASE_LAT_ACCEL_FACTOR_MULT if self.is_sonata_hybrid: @@ -1389,6 +1457,8 @@ class LatControlTorque(LatControl): def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction): if self.is_ioniq_5: latAccelFactor *= IONIQ_5_BASE_LAT_ACCEL_FACTOR_MULT + if self.is_ioniq_ev_old: + latAccelFactor *= IONIQ_EV_OLD_BASE_LAT_ACCEL_FACTOR_MULT if self.is_ioniq_6: latAccelFactor *= IONIQ_6_BASE_LAT_ACCEL_FACTOR_MULT if self.is_sonata_hybrid: @@ -1467,11 +1537,13 @@ class LatControlTorque(LatControl): volt_standard_test_active = self.is_volt_standard and volt_standard_lateral_testing_ground_active() genesis_g90_test_active = self.is_genesis_g90 and genesis_g90_lateral_testing_ground_active() ioniq_5_active = self.is_ioniq_5 + ioniq_ev_old_active = self.is_ioniq_ev_old ioniq_6_active = self.is_ioniq_6 sonata_hybrid_active = self.is_sonata_hybrid kia_ev6_test_active = self.is_kia_ev6 and kia_ev6_lateral_testing_ground_active() volt_plexy_test_active = self.is_volt_cc and volt_plexy_lateral_testing_ground_active() volt_standard_center_taper = get_volt_standard_center_taper_scale(setpoint, CS.vEgo) if volt_standard_test_active else 1.0 + ioniq_ev_old_center_taper = get_ioniq_ev_old_center_taper_scale(setpoint, CS.vEgo) if ioniq_ev_old_active else 1.0 ioniq_6_center_taper = get_ioniq_6_center_taper_scale(setpoint, CS.vEgo) if ioniq_6_active else 1.0 sonata_hybrid_center_taper = get_sonata_hybrid_center_taper_scale(setpoint, CS.vEgo) if sonata_hybrid_active else 1.0 civic_bosch_modified_a_center_taper = get_civic_bosch_modified_a_center_taper_scale(setpoint, CS.vEgo) if ( @@ -1499,6 +1571,9 @@ class LatControlTorque(LatControl): ff *= get_ioniq_5_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) friction_threshold = get_ioniq_5_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) friction_scale = get_ioniq_5_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk) + elif ioniq_ev_old_active: + ff *= get_ioniq_ev_old_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) * ioniq_ev_old_center_taper + friction_scale = 1.0 + ((friction_scale - 1.0) * ioniq_ev_old_center_taper) elif ioniq_6_active: ff *= get_ioniq_6_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) * ioniq_6_center_taper friction_threshold = get_ioniq_6_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) / max(ioniq_6_center_taper, 1e-3) diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index af74f3ed8..7816a5a78 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -13,6 +13,9 @@ clip = np.clip interp = np.interp STOPPING_RELEASE_HYSTERESIS = 0.35 STOPPING_RELEASE_MIN_ACCEL = 0.15 +MOVING_STOP_FOLLOW_MIN_GAP = 0.25 +NEGATIVE_TARGET_CREEP_GUARD_SPEED = 0.35 +NEGATIVE_TARGET_CREEP_GUARD_DECEL = 0.40 LongCtrlState = car.CarControl.Actuators.LongControlState @@ -184,6 +187,17 @@ class LongControl: return self.stop_release_counter >= int(round(STOPPING_RELEASE_HYSTERESIS / DT_CTRL)) + @staticmethod + def _apply_moving_stop_target_follow(output_accel, a_target, should_stop, CS, starpilot_toggles): + follow_min_speed = max(1.5, starpilot_toggles.vEgoStopping + 1.0) + if not should_stop or CS.brakePressed or CS.vEgo <= follow_min_speed: + return output_accel + if a_target >= output_accel - MOVING_STOP_FOLLOW_MIN_GAP: + return output_accel + + follow_step = interp(CS.vEgo, [follow_min_speed, 3.0, 6.0, 10.0], [0.02, 0.03, 0.05, 0.07]) + return max(float(a_target), output_accel - float(follow_step)) + def _get_pedal_long_freeze(self, a_target, error, v_ego, accel_limits): volt_test_tune_handoff = self.is_volt and testing_ground.use_2 @@ -228,7 +242,7 @@ class LongControl: return if a_target >= -0.05 or error >= -0.25: return - if CS.vEgo <= 8.0 or CS.aEgo <= 0.15: + if CS.vEgo <= NEGATIVE_TARGET_CREEP_GUARD_SPEED and a_target > -NEGATIVE_TARGET_CREEP_GUARD_DECEL: return # If the planner has already crossed into decel but the car is still @@ -260,12 +274,12 @@ class LongControl: return output_accel if a_target >= -0.10 or error >= -0.35: return output_accel - if CS.vEgo <= 8.0 or CS.aEgo <= 0.15: + if CS.vEgo <= NEGATIVE_TARGET_CREEP_GUARD_SPEED and a_target > -NEGATIVE_TARGET_CREEP_GUARD_DECEL: return output_accel # Once the planner is asking for real decel, don't keep feeding positive # drive torque while we're still accelerating away from the target. - positive_cap = interp(a_target, [-0.6, -0.1], [0.0, 0.05]) + positive_cap = interp(a_target, [-1.5, -0.6, -0.1], [0.0, 0.0, 0.05]) return min(output_accel, float(positive_cap)) def update(self, active, CS, a_target, should_stop, accel_limits, starpilot_toggles): @@ -287,6 +301,7 @@ class LongControl: if output_accel > starpilot_toggles.stopAccel: output_accel = min(output_accel, 0.0) output_accel -= starpilot_toggles.stoppingDecelRate * DT_CTRL + output_accel = self._apply_moving_stop_target_follow(output_accel, a_target, should_stop, CS, starpilot_toggles) self.reset(preserve_stop_release=True) elif self.long_control_state == LongCtrlState.starting: diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 2de094b5a..fd47c4916 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -33,6 +33,11 @@ RAW_LEAD_SAFETY_TTC = 7.0 RAW_LEAD_SAFETY_DISTANCE = 40.0 STANDSTILL_LEAD_NUDGE_ACCEL = 0.05 STANDSTILL_LEAD_NUDGE_MIN_SPEED = 0.0 +STANDSTILL_LEAD_DEPART_MIN_ACCEL = 0.20 +STANDSTILL_LEAD_DEPART_MAX_EGO_SPEED = 1.5 +STANDSTILL_LEAD_DEPART_MIN_LEAD_SPEED = 0.6 +STANDSTILL_LEAD_DEPART_MIN_GAP_MARGIN = 1.5 +STANDSTILL_LEAD_DEPART_MIN_MODEL_ACCEL = 0.08 CLOSE_LEAD_BRAKE_CAP_MAX_TTC = 25.0 VISION_LEAD_APPROACH_MIN_CLOSING_SPEED = 2.0 VISION_LEAD_APPROACH_TRIGGER_TIME = 4.5 @@ -113,6 +118,9 @@ VISION_LOW_SPEED_STOP_BUFFER_RELEASE_MARGIN = 0.9 VISION_LOW_SPEED_STOP_BUFFER_HOLD_TIME = 0.8 VISION_LOW_SPEED_STOP_BUFFER_MIN_BRAKE = 1.25 VISION_LOW_SPEED_STOP_BUFFER_BRAKE_GAIN = 0.25 +MANUAL_STOP_RESUME_OVERRIDE_TIME = 3.0 +MANUAL_STOP_RESUME_OVERRIDE_MAX_SPEED = 2.0 +MANUAL_STOP_RESUME_OVERRIDE_MIN_ACCEL = 0.2 LEAD_CATCHUP_ACCEL_MIN_EGO = 8.0 LEAD_CATCHUP_ACCEL_MIN_LEAD_DELTA = -0.5 LEAD_CATCHUP_ACCEL_MAX_GAP_BUFFER_MIN = 4.0 @@ -172,6 +180,30 @@ FAR_LEAD_COMFORT_BRAKE_CAP_FULL_HEADWAY_MARGIN = 1.00 FAR_LEAD_COMFORT_BRAKE_CAP_MIN_DECEL = 0.05 FAR_LEAD_COMFORT_BRAKE_CAP_MAX_DECEL = 0.18 FAR_LEAD_COMFORT_BRAKE_CAP_FULL_RELAX_DECEL = 0.05 +TRACKED_VISION_MODEL_FLOOR_MIN_SPEED = 10.0 +TRACKED_VISION_MODEL_FLOOR_MIN_MODEL_PROB = 0.95 +TRACKED_VISION_MODEL_FLOOR_MIN_MODEL_DECEL = 0.80 +TRACKED_VISION_MODEL_FLOOR_MIN_CLOSING_SPEED = 1.0 +TRACKED_VISION_MODEL_FLOOR_MAX_TTC = 22.0 +TRACKED_VISION_MODEL_FLOOR_MIN_GAP_MARGIN = -2.0 +TRACKED_VISION_MODEL_FLOOR_MAX_GAP_BUFFER_MIN = 4.0 +TRACKED_VISION_MODEL_FLOOR_MAX_GAP_BUFFER_GAIN = 0.25 +TRACKED_VISION_MODEL_FLOOR_MIN_DECEL = 0.35 +TRACKED_VISION_MODEL_FLOOR_MAX_DECEL = 1.10 +TRACKED_VISION_MODEL_FLOOR_LEAD_BRAKE_MAX = 0.18 +TRACKED_VISION_MODEL_CAP_MIN_SPEED = 14.0 +TRACKED_VISION_MODEL_CAP_MAX_SPEED = 32.0 +TRACKED_VISION_MODEL_CAP_MIN_MODEL_PROB = 0.95 +TRACKED_VISION_MODEL_CAP_MAX_MODEL_DECEL = 0.75 +TRACKED_VISION_MODEL_CAP_MIN_CLOSING_SPEED = 0.75 +TRACKED_VISION_MODEL_CAP_MAX_CLOSING_SPEED = 4.0 +TRACKED_VISION_MODEL_CAP_MAX_LEAD_BRAKE = 0.55 +TRACKED_VISION_MODEL_CAP_MIN_TTC = 7.0 +TRACKED_VISION_MODEL_CAP_MIN_GAP_MARGIN = -1.5 +TRACKED_VISION_MODEL_CAP_MAX_GAP_BUFFER_MIN = 4.0 +TRACKED_VISION_MODEL_CAP_MAX_GAP_BUFFER_GAIN = 0.25 +TRACKED_VISION_MODEL_CAP_MIN_DECEL = 0.45 +TRACKED_VISION_MODEL_CAP_MAX_DECEL = 1.10 # Lookup table for turns _A_TOTAL_MAX_V = [3.5, 3.5, 3.2] @@ -345,6 +377,7 @@ class LongitudinalPlanner: self.vision_low_speed_stop_hold_until = 0.0 self.vision_lead_approach_confirm_t = 0.0 self.untracked_slow_lead_confirm_t = 0.0 + self.manual_stop_resume_override_until = 0.0 if self.is_preap: try: @@ -680,6 +713,29 @@ class LongitudinalPlanner: min_stop_brake = VISION_LOW_SPEED_STOP_BUFFER_MIN_BRAKE + VISION_LOW_SPEED_STOP_BUFFER_BRAKE_GAIN * float(v_ego) return max(accel_min, -min_stop_brake), True + def _update_manual_stop_resume_override(self, sm): + now_t = time.monotonic() + lead = sm["radarState"].leadOne + no_lead = not bool(getattr(lead, "status", False)) + try: + starpilot_car_state = sm["starpilotCarState"] + except KeyError: + starpilot_car_state = None + accel_pressed = bool(getattr(starpilot_car_state, "accelPressed", False)) + model_should_stop = bool(getattr(sm["modelV2"].action, "shouldStop", False)) + standstill = bool(getattr(sm["carState"], "standstill", False)) + forcing_stop = bool(getattr(sm["starpilotPlan"], "forcingStop", False)) + red_light = bool(getattr(sm["starpilotPlan"], "redLight", False)) + + if standstill and no_lead and accel_pressed and (forcing_stop or red_light or model_should_stop): + self.manual_stop_resume_override_until = now_t + MANUAL_STOP_RESUME_OVERRIDE_TIME + + return bool( + no_lead and + float(getattr(sm["carState"], "vEgo", 0.0)) < MANUAL_STOP_RESUME_OVERRIDE_MAX_SPEED and + now_t < self.manual_stop_resume_override_until + ) + def get_lead_catchup_accel_cap(self, lead, v_ego, t_follow): if lead is None or not lead.status: return None @@ -902,6 +958,91 @@ class LongitudinalPlanner: )) return -max(0.0, cap_decel - relax_decel) + def get_tracked_vision_model_brake_floor(self, lead, v_ego, accel_min, t_follow, model_desired): + if lead is None or not lead.status or bool(getattr(lead, "radar", False)): + return None + if float(v_ego) < TRACKED_VISION_MODEL_FLOOR_MIN_SPEED: + return None + + lead_prob = float(getattr(lead, "modelProb", 0.0)) + if lead_prob < TRACKED_VISION_MODEL_FLOOR_MIN_MODEL_PROB: + return None + + model_brake = max(0.0, -float(model_desired)) + if model_brake < TRACKED_VISION_MODEL_FLOOR_MIN_MODEL_DECEL: + return None + + lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) + reaction_t = max(self.CP.longitudinalActuatorDelay, self.dt) + projected_closing_speed = max(0.0, float(v_ego) - float(lead.vLead)) + lead_brake * reaction_t + if projected_closing_speed < TRACKED_VISION_MODEL_FLOOR_MIN_CLOSING_SPEED: + return None + + desired_gap = float(desired_follow_distance(v_ego, lead.vLead, t_follow)) + gap_margin = float(lead.dRel) - desired_gap + max_gap_margin = max(TRACKED_VISION_MODEL_FLOOR_MAX_GAP_BUFFER_MIN, + TRACKED_VISION_MODEL_FLOOR_MAX_GAP_BUFFER_GAIN * float(v_ego)) + if gap_margin < TRACKED_VISION_MODEL_FLOOR_MIN_GAP_MARGIN or gap_margin > max_gap_margin: + return None + + projected_ttc = float(lead.dRel) / max(projected_closing_speed, 0.1) + if projected_ttc > TRACKED_VISION_MODEL_FLOOR_MAX_TTC: + return None + + floor_decel = float(np.interp( + model_brake, + [TRACKED_VISION_MODEL_FLOOR_MIN_MODEL_DECEL, 1.6], + [TRACKED_VISION_MODEL_FLOOR_MIN_DECEL, TRACKED_VISION_MODEL_FLOOR_MAX_DECEL], + )) + floor_decel += float(np.interp(lead_brake, [0.0, 0.8], [0.0, TRACKED_VISION_MODEL_FLOOR_LEAD_BRAKE_MAX])) + return max(accel_min, -min(TRACKED_VISION_MODEL_FLOOR_MAX_DECEL, floor_decel)) + + def get_tracked_vision_model_brake_cap(self, lead, v_ego, t_follow, model_desired): + if lead is None or not lead.status or bool(getattr(lead, "radar", False)): + return None + if not (TRACKED_VISION_MODEL_CAP_MIN_SPEED <= float(v_ego) <= TRACKED_VISION_MODEL_CAP_MAX_SPEED): + return None + + lead_prob = float(getattr(lead, "modelProb", 0.0)) + if lead_prob < TRACKED_VISION_MODEL_CAP_MIN_MODEL_PROB: + return None + + model_brake = max(0.0, -float(model_desired)) + if model_brake > TRACKED_VISION_MODEL_CAP_MAX_MODEL_DECEL: + return None + + lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) + if lead_brake > TRACKED_VISION_MODEL_CAP_MAX_LEAD_BRAKE: + return None + + reaction_t = max(self.CP.longitudinalActuatorDelay, self.dt) + projected_closing_speed = max(0.0, float(v_ego) - float(lead.vLead)) + lead_brake * reaction_t + if not (TRACKED_VISION_MODEL_CAP_MIN_CLOSING_SPEED <= projected_closing_speed <= TRACKED_VISION_MODEL_CAP_MAX_CLOSING_SPEED): + return None + + desired_gap = float(desired_follow_distance(v_ego, lead.vLead, t_follow)) + gap_margin = float(lead.dRel) - desired_gap + max_gap_margin = max(TRACKED_VISION_MODEL_CAP_MAX_GAP_BUFFER_MIN, + TRACKED_VISION_MODEL_CAP_MAX_GAP_BUFFER_GAIN * float(v_ego)) + if gap_margin < TRACKED_VISION_MODEL_CAP_MIN_GAP_MARGIN or gap_margin > max_gap_margin: + return None + + projected_ttc = float(lead.dRel) / max(projected_closing_speed, 0.1) + if projected_ttc < TRACKED_VISION_MODEL_CAP_MIN_TTC: + return None + + cap_decel = float(np.interp( + model_brake, + [0.0, TRACKED_VISION_MODEL_CAP_MAX_MODEL_DECEL], + [TRACKED_VISION_MODEL_CAP_MIN_DECEL, TRACKED_VISION_MODEL_CAP_MAX_DECEL], + )) + cap_decel += float(np.interp( + projected_closing_speed, + [TRACKED_VISION_MODEL_CAP_MIN_CLOSING_SPEED, TRACKED_VISION_MODEL_CAP_MAX_CLOSING_SPEED], + [0.0, 0.08], + )) + return -min(TRACKED_VISION_MODEL_CAP_MAX_DECEL, cap_decel) + @staticmethod def raw_close_lead_needs_control(lead, v_ego): if lead is None or not lead.status: @@ -1263,6 +1404,7 @@ class LongitudinalPlanner: comfort_output_accel_min = get_vehicle_min_accel(self.CP, v_ego) if experimental_mlsim else accel_limits_turns[0] vision_cap_accel_min = min(comfort_output_accel_min, get_vehicle_min_accel(self.CP, v_ego)) output_accel_min = comfort_output_accel_min + model_desired_accel = float(sm['modelV2'].action.desiredAcceleration) if not tracking_lead: pretracking_vision_caps = [] @@ -1341,12 +1483,33 @@ class LongitudinalPlanner: self.a_desired = min(self.a_desired, close_lead_brake_cap) output_a_target = min(output_a_target, close_lead_brake_cap) - if lead_control_active and sm['carState'].standstill: - standstill_nudge_gap = max(float(getattr(starpilot_toggles, "stop_distance", STOP_DISTANCE)), STOP_DISTANCE) - 0.5 - moving_leads = [lead for lead in (self.lead_one, self.lead_two) - if lead.status and lead.vLead > STANDSTILL_LEAD_NUDGE_MIN_SPEED and lead.dRel >= standstill_nudge_gap] - if moving_leads: - output_a_target = max(output_a_target, STANDSTILL_LEAD_NUDGE_ACCEL) + standstill_nudge_gap = max(float(getattr(starpilot_toggles, "stop_distance", STOP_DISTANCE)), STOP_DISTANCE) - 0.5 + moving_leads = [lead for lead in (self.lead_one, self.lead_two) + if lead.status and lead.vLead > STANDSTILL_LEAD_NUDGE_MIN_SPEED and lead.dRel >= standstill_nudge_gap] + lead_depart_ready = any( + lead.status and + lead.vLead >= STANDSTILL_LEAD_DEPART_MIN_LEAD_SPEED and + lead.dRel >= standstill_nudge_gap + STANDSTILL_LEAD_DEPART_MIN_GAP_MARGIN + for lead in (self.lead_one, self.lead_two) + ) + + if lead_control_active and sm['carState'].standstill and moving_leads: + output_a_target = max(output_a_target, STANDSTILL_LEAD_NUDGE_ACCEL) + + if ( + lead_control_active and + sm['carState'].standstill and + lead_depart_ready and + not bool(getattr(sm['starpilotPlan'], 'forcingStop', False)) and + not bool(getattr(sm['starpilotPlan'], 'redLight', False)) and + model_desired_accel >= STANDSTILL_LEAD_DEPART_MIN_MODEL_ACCEL + ): + vision_low_speed_stop_active = False + output_should_stop = False + output_a_target = max(output_a_target, STANDSTILL_LEAD_DEPART_MIN_ACCEL) + + if lead_control_active and lead_depart_ready and not output_should_stop and float(sm['carState'].vEgo) <= STANDSTILL_LEAD_DEPART_MAX_EGO_SPEED: + output_a_target = max(output_a_target, STANDSTILL_LEAD_DEPART_MIN_ACCEL) if lead_one_active: lead_catchup_accel_cap = self.get_lead_catchup_accel_cap(self.lead_one, scene_v_ego, effective_t_follow) @@ -1365,6 +1528,18 @@ class LongitudinalPlanner: follow_control_lead = self.get_follow_control_lead(lead_control_active, scene_v_ego, effective_t_follow) if follow_control_lead is not None and not panic_bypass: + if not output_should_stop and not vision_low_speed_stop_active: + tracked_vision_model_brake_floor = self.get_tracked_vision_model_brake_floor( + follow_control_lead, + scene_v_ego, + output_accel_min, + effective_t_follow, + model_desired_accel, + ) + if tracked_vision_model_brake_floor is not None: + self.a_desired = min(self.a_desired, tracked_vision_model_brake_floor) + output_a_target = min(output_a_target, tracked_vision_model_brake_floor) + matched_follow_brake_cap = self.get_matched_follow_brake_cap(follow_control_lead, scene_v_ego, effective_t_follow) if matched_follow_brake_cap is not None: self.a_desired = max(self.a_desired, matched_follow_brake_cap) @@ -1389,9 +1564,25 @@ class LongitudinalPlanner: self.a_desired = max(self.a_desired, far_lead_brake_cap) output_a_target = max(output_a_target, far_lead_brake_cap) + if follow_control_lead is not None and not panic_bypass and not output_should_stop and not vision_low_speed_stop_active: + tracked_vision_model_brake_cap = self.get_tracked_vision_model_brake_cap( + follow_control_lead, + scene_v_ego, + effective_t_follow, + model_desired_accel, + ) + if tracked_vision_model_brake_cap is not None: + self.a_desired = max(self.a_desired, tracked_vision_model_brake_cap) + output_a_target = max(output_a_target, tracked_vision_model_brake_cap) + output_accel_max = no_throttle_output_max if not self.allow_throttle else accel_limits_turns[1] output_a_target = float(np.clip(output_a_target, output_accel_min, output_accel_max)) + manual_stop_resume_override = self._update_manual_stop_resume_override(sm) + if manual_stop_resume_override: + output_a_target = max(output_a_target, MANUAL_STOP_RESUME_OVERRIDE_MIN_ACCEL) + output_should_stop = False + self.output_a_target = output_a_target self.output_should_stop = bool(output_should_stop or vision_low_speed_stop_active) @@ -1414,7 +1605,13 @@ class LongitudinalPlanner: longitudinalPlan.fcw = self.fcw longitudinalPlan.aTarget = float(self.output_a_target) - longitudinalPlan.shouldStop = bool(self.output_should_stop) or (sm['starpilotPlan'].forcingStop and sm['starpilotPlan'].forcingStopLength < 1) + force_stop_handoff = bool( + sm['starpilotPlan'].forcingStop and ( + sm['starpilotPlan'].forcingStopLength < 1.0 or + sm['starpilotPlan'].vCruise <= 0.0 + ) + ) + longitudinalPlan.shouldStop = bool(self.output_should_stop) or force_stop_handoff longitudinalPlan.allowBrake = True longitudinalPlan.allowThrottle = bool(self.allow_throttle) diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index b437f4221..6daaab515 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -42,6 +42,8 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import ( get_ioniq_5_ff_scale, get_ioniq_5_friction_scale, get_ioniq_5_friction_threshold, + get_ioniq_ev_old_center_taper_scale, + get_ioniq_ev_old_ff_scale, get_ioniq_6_center_taper_scale, get_ioniq_6_directional_taper_scale, get_ioniq_6_output_taper_scale, @@ -296,6 +298,17 @@ class TestLatControl: assert unwind_left_scale < 1.0 assert unwind_right_scale < unwind_left_scale + def test_ioniq_ev_old_ff_scale_curve(self): + assert get_ioniq_ev_old_ff_scale(0.0, 0.0, 20.0) == 1.0 + assert get_ioniq_ev_old_ff_scale(0.35, 0.0, 20.0) > get_ioniq_ev_old_ff_scale(-0.35, 0.0, 20.0) + assert get_ioniq_ev_old_ff_scale(0.35, 0.7, 8.0) > get_ioniq_ev_old_ff_scale(0.35, 0.0, 8.0) + assert get_ioniq_ev_old_ff_scale(0.35, -0.7, 8.0) < get_ioniq_ev_old_ff_scale(0.35, 0.0, 8.0) + assert get_ioniq_ev_old_ff_scale(-0.35, -0.7, 8.0) <= get_ioniq_ev_old_ff_scale(-0.35, 0.0, 8.0) + + def test_ioniq_ev_old_center_taper_curve(self): + assert get_ioniq_ev_old_center_taper_scale(0.0, 30.0) < get_ioniq_ev_old_center_taper_scale(0.0, 15.0) + assert get_ioniq_ev_old_center_taper_scale(0.0, 30.0) < get_ioniq_ev_old_center_taper_scale(0.20, 30.0) <= 1.0 + def test_ioniq_6_ff_scale_curve(self): assert get_ioniq_6_ff_scale(0.0, 0.0, 20.0) == 1.0 assert get_ioniq_6_ff_scale(0.4, 0.0, 20.0) > get_ioniq_6_ff_scale(-0.4, 0.0, 20.0) @@ -574,6 +587,8 @@ class TestLatControl: base_turn_in_right = get_civic_bosch_modified_b_ff_scale(-0.5, -0.8, 12.0) base_unwind_left = get_civic_bosch_modified_b_ff_scale(0.5, -0.8, 12.0) base_unwind_right = get_civic_bosch_modified_b_ff_scale(-0.5, 0.8, 12.0) + base_turn_in_right_friction = get_civic_bosch_modified_b_friction_scale(12.0, -0.5, -0.8) + base_unwind_left_friction = get_civic_bosch_modified_b_friction_scale(12.0, 0.5, -0.8) base_unwind_right_friction = get_civic_bosch_modified_b_friction_scale(12.0, -0.5, 0.8) monkeypatch.setattr(latcontrol_torque, "civic_bosch_modified_a_lateral_testing_ground_active", lambda: True) @@ -584,15 +599,19 @@ class TestLatControl: a_variant_turn_in_right = get_civic_bosch_modified_b_ff_scale(-0.5, -0.8, 12.0) a_variant_unwind_left = get_civic_bosch_modified_b_ff_scale(0.5, -0.8, 12.0) a_variant_unwind_right = get_civic_bosch_modified_b_ff_scale(-0.5, 0.8, 12.0) + a_variant_turn_in_right_friction = get_civic_bosch_modified_b_friction_scale(12.0, -0.5, -0.8) + a_variant_unwind_left_friction = get_civic_bosch_modified_b_friction_scale(12.0, 0.5, -0.8) a_variant_unwind_right_friction = get_civic_bosch_modified_b_friction_scale(12.0, -0.5, 0.8) - assert a_variant_steady_left == pytest.approx(base_steady_left) - assert a_variant_steady_right == pytest.approx(base_steady_right) - assert a_variant_turn_in_left == pytest.approx(base_turn_in_left) - assert a_variant_turn_in_right == pytest.approx(base_turn_in_right) + assert a_variant_steady_left < base_steady_left + assert a_variant_steady_right > base_steady_right + assert a_variant_turn_in_left < base_turn_in_left + assert a_variant_turn_in_right > base_turn_in_right assert a_variant_unwind_left < base_unwind_left - assert a_variant_unwind_right < base_unwind_right - assert a_variant_unwind_right_friction < base_unwind_right_friction + assert a_variant_unwind_right > base_unwind_right + assert a_variant_turn_in_right_friction > base_turn_in_right_friction + assert a_variant_unwind_left_friction < base_unwind_left_friction + assert a_variant_unwind_right_friction >= 0.82 def test_modified_civic_a_variant_center_taper_curve(self): assert get_civic_bosch_modified_a_center_taper_scale(0.0, 25.0) < get_civic_bosch_modified_a_center_taper_scale(0.0, 10.0) diff --git a/selfdrive/controls/tests/test_longcontrol.py b/selfdrive/controls/tests/test_longcontrol.py index b73da55e7..94854f26c 100644 --- a/selfdrive/controls/tests/test_longcontrol.py +++ b/selfdrive/controls/tests/test_longcontrol.py @@ -264,6 +264,32 @@ def test_update_releases_stopping_with_cruise_standstill_latched(): assert output_accel > 0.0 +def test_stopping_state_follows_stronger_moving_stop_target(): + CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5) + CP.longitudinalTuning.kpBP = [0.0] + CP.longitudinalTuning.kpV = [0.1] + CP.longitudinalTuning.kiBP = [0.0] + CP.longitudinalTuning.kiV = [0.03] + + lc = LongControl(CP) + lc.long_control_state = LongCtrlState.stopping + lc.last_output_accel = -1.40 + CS = car.CarState.new_message(vEgo=4.0, aEgo=-1.2, brakePressed=False) + CS.cruiseState.standstill = False + + output_accel = lc.update( + active=True, + CS=CS, + a_target=-3.5, + should_stop=True, + accel_limits=(-3.5, 2.0), + starpilot_toggles=make_toggles(stopAccel=-0.5, stoppingDecelRate=0.8, vEgoStopping=0.5), + ) + + assert lc.long_control_state == LongCtrlState.stopping + assert output_accel < -1.43 + + def test_volt_testing_ground_handoff_freezes_integrator(monkeypatch): CP = car.CarParams.new_message() CP.brand = "gm" @@ -329,6 +355,44 @@ def test_negative_target_unwinds_positive_accel_command_after_sign_flip(): assert output_accel <= 0.01 +def test_negative_target_unwinds_positive_accel_command_at_low_speed(): + CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5) + CP.longitudinalTuning.kpBP = [0.0] + CP.longitudinalTuning.kpV = [0.1] + CP.longitudinalTuning.kiBP = [0.0] + CP.longitudinalTuning.kiV = [0.03] + + lc = LongControl(CP) + lc.long_control_state = LongCtrlState.pid + lc.last_output_accel = 0.9 + lc.pid.i = 0.9 + CS = car.CarState.new_message(vEgo=1.3, aEgo=0.35, brakePressed=False, gasPressed=False) + CS.cruiseState.standstill = False + + output_accel = lc.update( + active=True, + CS=CS, + a_target=-1.2, + should_stop=False, + accel_limits=(-3.0, 2.0), + starpilot_toggles=make_toggles(), + ) + + assert lc.long_control_state == LongCtrlState.pid + assert output_accel <= 0.01 + + +def test_negative_target_creep_guard_keeps_mild_crawl_request(): + capped = LongControl._cap_positive_output_on_negative_target( + output_accel=0.18, + a_target=-0.2, + error=-0.5, + CS=car.CarState.new_message(vEgo=0.2, aEgo=0.0), + ) + + assert capped == pytest.approx(0.18) + + def test_pedal_long_brake_bias_adds_small_negative_nudge_for_strong_decel_request(): CP = car.CarParams.new_message() CP.brand = "gm" diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index eb4020244..56277c17d 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -10,6 +10,7 @@ from opendbc.car.honda.interface import CarInterface from opendbc.car.honda.values import CAR import openpilot.selfdrive.controls.lib.longitudinal_planner as longitudinal_planner_module from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState +from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner, get_coast_accel, get_vehicle_min_accel from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import soften_far_radar_lead_accel, should_trigger_planner_fcw from openpilot.selfdrive.modeld.constants import ModelConstants, Plan @@ -80,6 +81,7 @@ def make_sm(v_ego: float, desired_accel: float, min_accel: float, *, experimenta leadTwo=lead_two if lead_two is not None else make_lead(status=False), ), "selfdriveState": SimpleNamespace(enabled=True, experimentalMode=experimental_mode, personality=0), + "starpilotCarState": SimpleNamespace(accelPressed=False), "starpilotPlan": SimpleNamespace( vCruise=v_ego + 5.0, minAcceleration=min_accel, @@ -91,6 +93,8 @@ def make_sm(v_ego: float, desired_accel: float, min_accel: float, *, experimenta speedJerk=5.0, dangerFactor=1.0, tFollow=1.45, + forcingStop=False, + redLight=False, forcingStopLength=2, ), } @@ -801,6 +805,93 @@ def test_acc_mode_low_speed_vision_stop_buffer_stays_latched_when_closure_soften assert cap_held <= -1.25 +@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"]) +def test_acc_mode_tracked_vision_model_brake_floor_prevents_positive_output_on_slower_lead(model_version): + v_ego = 19.1 + + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + sm = make_sm( + v_ego, + desired_accel=-1.18, + min_accel=-0.5, + experimental_mode=False, + tracking_lead=True, + lead_one=make_lead(status=True, d_rel=36.5, v_lead=17.2, a_lead=-0.53, radar=False, model_prob=0.993), + ) + + for _ in range(8): + planner.update(sm, make_toggles(model_version)) + + assert planner.mode == "acc" + assert planner.output_a_target <= -0.35 + + +@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"]) +def test_tracked_vision_model_brake_cap_relaxes_mild_model_brake_slam_window(model_version): + v_ego = 20.56 + + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead = make_lead(status=True, d_rel=38.1, v_lead=19.07, a_lead=-0.30, radar=False, model_prob=0.999) + + cap = planner.get_tracked_vision_model_brake_cap(lead, v_ego, 1.45, -0.35) + + assert cap is not None + assert cap > -1.2 + + +@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"]) +def test_tracked_vision_model_brake_cap_does_not_relax_strong_model_brake(model_version): + v_ego = 20.56 + + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead = make_lead(status=True, d_rel=38.1, v_lead=19.07, a_lead=-0.30, radar=False, model_prob=0.999) + + cap = planner.get_tracked_vision_model_brake_cap(lead, v_ego, 1.45, -1.2) + + assert cap is None + + +@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"]) +def test_manual_resume_override_clears_no_lead_model_stop_at_standstill(model_version): + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=0.0) + sm = make_sm(0.0, desired_accel=0.0, min_accel=-0.5) + sm["carState"].standstill = True + sm["controlsState"].longControlState = LongCtrlState.stopping + sm["modelV2"].action.shouldStop = True + sm["starpilotPlan"].forcingStop = True + sm["starpilotCarState"] = SimpleNamespace(accelPressed=True) + + planner.update(sm, make_toggles(model_version)) + + assert not planner.output_should_stop + assert planner.output_a_target >= 0.2 + + +@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"]) +def test_manual_resume_override_does_not_clear_stopped_lead_stop(model_version): + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=0.0) + sm = make_sm( + 0.0, + desired_accel=0.0, + min_accel=-0.5, + lead_one=make_lead(status=True, d_rel=4.0, v_lead=0.0, radar=False, model_prob=0.99), + ) + sm["carState"].standstill = True + sm["controlsState"].longControlState = LongCtrlState.stopping + sm["modelV2"].action.shouldStop = True + sm["starpilotPlan"].forcingStop = True + sm["starpilotCarState"] = SimpleNamespace(accelPressed=True) + + planner.update(sm, make_toggles(model_version)) + + assert planner.output_should_stop + + @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"]) def test_standstill_moving_lead_does_not_force_resume_while_should_stop(model_version): v_ego = 0.0 @@ -825,6 +916,32 @@ def test_standstill_moving_lead_does_not_force_resume_while_should_stop(model_ve assert planner.output_a_target < 0.1 +@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"]) +def test_standstill_moving_lead_applies_resume_floor_once_stop_clears(model_version): + v_ego = 0.0 + + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + sm = make_sm( + v_ego, + desired_accel=0.1, + min_accel=-0.5, + experimental_mode=False, + tracking_lead=True, + lead_one=make_lead(status=True, d_rel=15.0, v_lead=2.2, a_lead=0.4, radar=False, model_prob=0.99), + ) + sm["carState"].standstill = True + sm["controlsState"].longControlState = LongCtrlState.stopping + sm["modelV2"].action.shouldStop = False + sm["starpilotPlan"].vCruise = 10.0 + + for _ in range(12): + planner.update(sm, make_toggles(model_version)) + + assert not planner.output_should_stop + assert planner.output_a_target >= 0.2 + + @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"]) def test_acc_mode_damps_far_radar_mild_lead_brake_more_than_close_brake(model_version): far_v_ego = 29.26 @@ -943,6 +1060,43 @@ def test_modeld_action_uses_direct_action_head_for_v14(monkeypatch): assert not action.shouldStop +def test_publish_force_stop_handoff_sets_should_stop_when_vcruise_zero(): + class FakePM: + def __init__(self): + self.sent = {} + + def send(self, name, msg): + self.sent[name] = msg + + class FakeSM(dict): + def all_checks(self, service_list=None): + return True + + logMonoTime = {"modelV2": int(1e9)} + + v_ego = 5.0 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + planner.output_a_target = -0.5 + planner.output_should_stop = False + planner.v_desired_trajectory = np.zeros(CONTROL_N) + planner.a_desired_trajectory = np.zeros(CONTROL_N) + planner.j_desired_trajectory = np.zeros(CONTROL_N) + planner.fcw = False + planner.mpc.source = "cruise" + planner.mpc.solve_time = 0.0 + pm = FakePM() + + sm = FakeSM(make_sm(v_ego, desired_accel=0.0, min_accel=-1.0, experimental_mode=False)) + sm["starpilotPlan"].forcingStop = True + sm["starpilotPlan"].forcingStopLength = 5.0 + sm["starpilotPlan"].vCruise = 0.0 + + planner.publish(sm, pm) + + assert pm.sent["longitudinalPlan"].longitudinalPlan.shouldStop + + def test_allow_throttle_hysteresis_filters_gas_prob_chatter(): v_ego = 10.0 diff --git a/starpilot/common/starpilot_functions.py b/starpilot/common/starpilot_functions.py index 9426fb836..ff94af244 100644 --- a/starpilot/common/starpilot_functions.py +++ b/starpilot/common/starpilot_functions.py @@ -299,4 +299,7 @@ def update_openpilot(thread_manager, params): if not update_available(): break + while params.get_bool("IsOnroad") or thread_manager.is_thread_alive("lock_doors"): + time.sleep(60) + HARDWARE.reboot() diff --git a/system/manager/manager.py b/system/manager/manager.py index 0512b5957..6663c2042 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -751,6 +751,7 @@ def manager_thread() -> None: started_prev = False ignition_prev = False + warned_onroad_reboot = False # StarPilot variables sm = sm.extend(['starpilotPlan']) @@ -809,8 +810,15 @@ def manager_thread() -> None: # Exit main loop when uninstall/shutdown/reboot is needed shutdown = False for param in ("DoUninstall", "DoShutdown", "DoReboot"): + if param == "DoReboot" and started: + if params.get_bool(param): + if not warned_onroad_reboot: + cloudlog.warning("ignoring DoReboot while onroad; deferring until offroad") + warned_onroad_reboot = True + continue if params.get_bool(param): shutdown = True + warned_onroad_reboot = False params.put("LastManagerExitReason", f"{param} {datetime.datetime.now()}") cloudlog.warning(f"Shutting down manager - {param} set") diff --git a/system/updated/tests/test_base.py b/system/updated/tests/test_base.py index c4894f271..f970c3a85 100644 --- a/system/updated/tests/test_base.py +++ b/system/updated/tests/test_base.py @@ -227,6 +227,27 @@ class ParamsBaseUpdateTest(TestBaseUpdate): self._test_params("master", False, True) self._test_finalized_update("master", *self.MOCK_RELEASES["master"]) + def test_download_blocked_onroad(self): + self.setup_remote_release("release3") + self.setup_basedir_release("release3") + + with self.additional_context(), processes_context(["updated"]) as [updated]: + self.wait_for_idle() + + self.MOCK_RELEASES["release3"] = ("0.1.3", "1.2", "0.1.3 release notes") + self.update_remote_release("release3") + + self.send_check_for_updates_signal(updated) + self.wait_for_fetch_available() + self._test_params("release3", True, False) + + self.params.put_bool("IsOffroad", False) + self.send_download_signal(updated) + self.wait_for_idle() + + assert not self.params.get_bool("UpdateAvailable") + assert not get_consistent_flag(str(self.staging_root / "finalized")) + def test_agnos_update(self, mocker): # Start on release3, push an update with an agnos change self.setup_remote_release("release3") diff --git a/system/updated/updated.py b/system/updated/updated.py index 9fbd57802..8e7c8890c 100644 --- a/system/updated/updated.py +++ b/system/updated/updated.py @@ -47,6 +47,11 @@ class UserRequest: CHECK = 1 FETCH = 2 + +class UpdateAborted(Exception): + pass + + class WaitTimeHelper: def __init__(self): self.ready_event = threading.Event() @@ -75,6 +80,38 @@ def run(cmd: list[str], cwd: str = None) -> str: return subprocess.check_output(cmd, cwd=cwd, stderr=subprocess.STDOUT, encoding='utf8') +def run_with_offroad_abort(cmd: list[str], params: Params, cwd: str = None, poll_interval: float = 0.5) -> str: + proc = subprocess.Popen(cmd, cwd=cwd, stdout=subprocess.PIPE, stderr=subprocess.STDOUT, text=True) + output: list[str] = [] + + try: + while True: + try: + stdout, _ = proc.communicate(timeout=poll_interval) + if stdout: + output.append(stdout) + if proc.returncode: + raise subprocess.CalledProcessError(proc.returncode, cmd, ''.join(output)) + return ''.join(output) + except subprocess.TimeoutExpired: + if not params.get_bool("IsOffroad"): + proc.terminate() + try: + stdout, _ = proc.communicate(timeout=5) + if stdout: + output.append(stdout) + except subprocess.TimeoutExpired: + proc.kill() + stdout, _ = proc.communicate() + if stdout: + output.append(stdout) + raise UpdateAborted(f"aborted {' '.join(cmd)} because vehicle went onroad") + finally: + if proc.poll() is None: + proc.kill() + proc.communicate() + + def set_consistent_flag(consistent: bool) -> None: os.sync() consistent_file = Path(os.path.join(FINALIZED, ".overlay_consistent")) @@ -374,7 +411,12 @@ class Updater: else: cloudlog.info(f"up to date on {cur_branch} ({str(cur_commit)[:7]})") + def require_offroad(self, context: str) -> None: + if not self.params.get_bool("IsOffroad"): + raise UpdateAborted(f"{context} blocked because vehicle is onroad") + def fetch_update(self) -> None: + self.require_offroad("update fetch") cloudlog.info("attempting git fetch inside staging overlay") self.params.put("UpdaterState", "downloading...") @@ -388,7 +430,7 @@ class Updater: run(["git", "config", "--replace-all", "remote.origin.fetch", "+refs/heads/*:refs/remotes/origin/*"], OVERLAY_MERGED) branch = self.target_branch - git_fetch_output = run(["git", "fetch", "origin", branch], OVERLAY_MERGED) + git_fetch_output = run_with_offroad_abort(["git", "fetch", "origin", branch], self.params, OVERLAY_MERGED) cloudlog.info("git fetch success: %s", git_fetch_output) cloudlog.info("git reset in progress") @@ -401,14 +443,19 @@ class Updater: ["git", "submodule", "update", "--init", "--recursive"], ["git", "submodule", "foreach", "--recursive", "git", "reset", "--hard"], ] - r = [run(cmd, OVERLAY_MERGED) for cmd in cmds] + r = [] + for cmd in cmds: + self.require_offroad("update apply") + r.append(run_with_offroad_abort(cmd, self.params, OVERLAY_MERGED)) cloudlog.info("git reset success: %s", '\n'.join(r)) # TODO: show agnos download progress if AGNOS: + self.require_offroad("AGNOS update") handle_agnos_update() # Create the finalized, ready-to-swap update + self.require_offroad("update finalization") self.params.put("UpdaterState", "finalizing update...") finalize_update() cloudlog.info("finalize success!") @@ -484,7 +531,8 @@ def main() -> None: update_failed_count += 1 - if manual_update_requested or user_requested_action or (params.get_bool("IsOffroad") and automatic_updates_enabled): + should_check = manual_update_requested or user_requested_action or (params.get_bool("IsOffroad") and automatic_updates_enabled) + if should_check: # check for update params.put("UpdaterState", "checking...") updater.check_for_update() @@ -497,6 +545,8 @@ def main() -> None: cloudlog.info("skipping fetch, connection metered") elif wait_helper.user_request == UserRequest.CHECK: cloudlog.info("skipping fetch, only checking") + elif not params.get_bool("IsOffroad"): + cloudlog.info("skipping fetch, vehicle went onroad") else: updater.fetch_update() write_time_to_param(params, "UpdaterLastFetchTime") @@ -515,6 +565,10 @@ def main() -> None: ) exception = f"command failed: {e.cmd}\n{e.output}" OVERLAY_INIT.unlink(missing_ok=True) + except UpdateAborted as e: + cloudlog.warning(str(e)) + exception = str(e) + OVERLAY_INIT.unlink(missing_ok=True) except Exception as e: cloudlog.exception("uncaught updated exception, shouldn't happen") exception = str(e)