diff --git a/.github/workflows/selfdrive_tests.yaml b/.github/workflows/selfdrive_tests.yaml index 67cbc5f097..a91e9862ac 100644 --- a/.github/workflows/selfdrive_tests.yaml +++ b/.github/workflows/selfdrive_tests.yaml @@ -315,7 +315,7 @@ jobs: run: | ${{ env.RUN }} "source selfdrive/test/setup_xvfb.sh && \ source selfdrive/test/setup_vsound.sh && \ - CI=1 pytest tools/sim/tests/test_metadrive_bridge.py" + CI=1 pytest -s tools/sim/tests/test_metadrive_bridge.py" create_ui_report: # This job name needs to be the same as UI_JOB_NAME in ui_preview.yaml diff --git a/common/realtime.py b/common/realtime.py index 854c3ca592..0178692415 100644 --- a/common/realtime.py +++ b/common/realtime.py @@ -27,11 +27,6 @@ class Priority: CTRL_HIGH = 53 -def set_realtime_priority(level: int) -> None: - if not PC: - os.sched_setscheduler(0, os.SCHED_FIFO, os.sched_param(level)) - - def set_core_affinity(cores: list[int]) -> None: if not PC: os.sched_setaffinity(0, cores) @@ -39,7 +34,8 @@ def set_core_affinity(cores: list[int]) -> None: def config_realtime_process(cores: int | list[int], priority: int) -> None: gc.disable() - set_realtime_priority(priority) + if not PC: + os.sched_setscheduler(0, os.SCHED_FIFO, os.sched_param(priority)) c = cores if isinstance(cores, list) else [cores, ] set_core_affinity(c) diff --git a/opendbc_repo b/opendbc_repo index 4bfb262fd5..130bf3183f 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 4bfb262fd52bc6946de1c8280efc424910bd9d60 +Subproject commit 130bf3183f4ab871ee8dd56b122cfbb038542606 diff --git a/panda b/panda index 3ff97305c5..978ee19005 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 3ff97305c565d5e43931052b9ec4747b2f617401 +Subproject commit 978ee190051806f8e5b7b8fede6b6be4e4db4aae diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index b8735359e1..f00cbe55e1 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -7,8 +7,6 @@ import cereal.messaging as messaging from cereal import car, log -from panda import ALTERNATIVE_EXPERIENCE - from openpilot.common.params import Params from openpilot.common.realtime import config_realtime_process, Priority, Ratekeeper from openpilot.common.swaglog import cloudlog, ForwardingHandler @@ -19,6 +17,7 @@ from opendbc.car.carlog import carlog from opendbc.car.fw_versions import ObdCallback from opendbc.car.car_helpers import get_car, get_radar_interface from opendbc.car.interfaces import CarInterfaceBase, RadarInterfaceBase +from opendbc.safety import ALTERNATIVE_EXPERIENCE from openpilot.selfdrive.pandad import can_capnp_to_list, can_list_to_can_capnp from openpilot.selfdrive.car.cruise import VCruiseHelper from openpilot.selfdrive.car.car_specific import MockCarState diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 579ee85fd4..3f9d8245bd 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -3,7 +3,7 @@ import os import time import numpy as np from cereal import log -from opendbc.car.interfaces import ACCEL_MIN +from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX from openpilot.common.realtime import DT_MDL from openpilot.common.swaglog import cloudlog # WARNING: imports outside of constants will not trigger a rebuild @@ -55,6 +55,8 @@ FCW_IDXS = T_IDXS < 5.0 T_DIFFS = np.diff(T_IDXS, prepend=[0.]) COMFORT_BRAKE = 2.5 STOP_DISTANCE = 6.0 +CRUISE_MIN_ACCEL = -1.2 +CRUISE_MAX_ACCEL = 1.6 def get_jerk_factor(personality=log.LongitudinalPersonality.standard): if personality==log.LongitudinalPersonality.relaxed: @@ -281,7 +283,7 @@ class LongitudinalMpc: elif self.mode == 'blended': a_change_cost = 40.0 if prev_accel_constraint else 0 cost_weights = [0., 0.1, 0.2, 5.0, a_change_cost, 1.0] - constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, 50.0] + constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST] else: raise NotImplementedError(f'Planner mode {self.mode} not recognized in planner cost set') self.set_cost_weights(cost_weights, constraint_cost_weights) @@ -325,12 +327,6 @@ class LongitudinalMpc: lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau) return lead_xv - def set_accel_limits(self, min_a, max_a): - # TODO this sets a max accel limit, but the minimum limit is only for cruise decel - # needs refactor - self.cruise_min_a = min_a - self.max_a = max_a - def update(self, radarstate, v_cruise, x, v, a, j, personality=log.LongitudinalPersonality.standard): t_follow = get_T_FOLLOW(personality) v_ego = self.x0[1] @@ -346,8 +342,7 @@ class LongitudinalMpc: lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1]) self.params[:,0] = ACCEL_MIN - # negative accel constraint causes problems because negative speed is not allowed - self.params[:,1] = max(0.0, self.max_a) + self.params[:,1] = ACCEL_MAX # Update in ACC mode or ACC/e2e blend if self.mode == 'acc': @@ -355,9 +350,9 @@ class LongitudinalMpc: # Fake an obstacle for cruise, this ensures smooth acceleration to set speed # when the leads are no factor. - v_lower = v_ego + (T_IDXS * self.cruise_min_a * 1.05) + v_lower = v_ego + (T_IDXS * CRUISE_MIN_ACCEL * 1.05) # TODO does this make sense when max_a is negative? - v_upper = v_ego + (T_IDXS * self.max_a * 1.05) + v_upper = v_ego + (T_IDXS * CRUISE_MAX_ACCEL * 1.05) v_cruise_clipped = np.clip(v_cruise * np.ones(N+1), v_lower, v_upper) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 5560dfbb09..c7a2295a5f 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -16,7 +16,6 @@ from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET from openpilot.common.swaglog import cloudlog LON_MPC_STEP = 0.2 # first step is 0.2s -A_CRUISE_MIN = -1.2 A_CRUISE_MAX_VALS = [1.6, 1.2, 0.8, 0.6] A_CRUISE_MAX_BP = [0., 10.0, 25., 40.] CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N] @@ -76,7 +75,10 @@ class LongitudinalPlanner: self.a_desired = init_a self.v_desired_filter = FirstOrderFilter(init_v, 2.0, self.dt) + self.prev_accel_clip = [ACCEL_MIN, ACCEL_MAX] self.v_model_error = 0.0 + self.output_a_target = 0.0 + self.output_should_stop = False self.v_desired_trajectory = np.zeros(CONTROL_N) self.a_desired_trajectory = np.zeros(CONTROL_N) @@ -128,17 +130,16 @@ class LongitudinalPlanner: prev_accel_constraint = not (reset_state or sm['carState'].standstill) if self.mpc.mode == 'acc': - accel_limits = [A_CRUISE_MIN, get_max_accel(v_ego)] + accel_clip = [ACCEL_MIN, get_max_accel(v_ego)] steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg - accel_limits_turns = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_limits, self.CP) + accel_clip = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_clip, self.CP) else: - accel_limits = [ACCEL_MIN, ACCEL_MAX] - accel_limits_turns = [ACCEL_MIN, ACCEL_MAX] + accel_clip = [ACCEL_MIN, ACCEL_MAX] if reset_state: self.v_desired_filter.x = v_ego # Clip aEgo to cruise limits to prevent large accelerations when becoming active - self.a_desired = np.clip(sm['carState'].aEgo, accel_limits[0], accel_limits[1]) + self.a_desired = np.clip(sm['carState'].aEgo, accel_clip[0], accel_clip[1]) # Prevent divergence, smooth in current v_ego self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego)) @@ -149,18 +150,14 @@ class LongitudinalPlanner: self.allow_throttle = throttle_prob > ALLOW_THROTTLE_THRESHOLD or v_ego <= MIN_ALLOW_THROTTLE_SPEED if not self.allow_throttle: - clipped_accel_coast = max(accel_coast, accel_limits_turns[0]) - clipped_accel_coast_interp = np.interp(v_ego, [MIN_ALLOW_THROTTLE_SPEED, MIN_ALLOW_THROTTLE_SPEED*2], [accel_limits_turns[1], clipped_accel_coast]) - accel_limits_turns[1] = min(accel_limits_turns[1], clipped_accel_coast_interp) + clipped_accel_coast = max(accel_coast, accel_clip[0]) + clipped_accel_coast_interp = np.interp(v_ego, [MIN_ALLOW_THROTTLE_SPEED, MIN_ALLOW_THROTTLE_SPEED*2], [accel_clip[1], clipped_accel_coast]) + accel_clip[1] = min(accel_clip[1], clipped_accel_coast_interp) if force_slow_decel: v_cruise = 0.0 - # clip limits, cannot init MPC outside of bounds - accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05) - accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05) self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality) - self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1]) self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired) self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=sm['selfdriveState'].personality) @@ -178,6 +175,15 @@ class LongitudinalPlanner: self.a_desired = float(np.interp(self.dt, CONTROL_N_T_IDX, self.a_desired_trajectory)) self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.a_desired + a_prev) / 2.0 + action_t = self.CP.longitudinalActuatorDelay + DT_MDL + output_a_target, self.output_should_stop = get_accel_from_plan(self.v_desired_trajectory, self.a_desired_trajectory, + action_t=action_t, vEgoStopping=self.CP.vEgoStopping) + + for idx in range(2): + accel_clip[idx] = np.clip(accel_clip[idx], self.prev_accel_clip[idx] - 0.05, self.prev_accel_clip[idx] + 0.05) + self.output_a_target = np.clip(output_a_target, accel_clip[0], accel_clip[1]) + self.prev_accel_clip = accel_clip + def publish(self, sm, pm): plan_send = messaging.new_message('longitudinalPlan') @@ -196,11 +202,8 @@ class LongitudinalPlanner: longitudinalPlan.longitudinalPlanSource = self.mpc.source longitudinalPlan.fcw = self.fcw - action_t = self.CP.longitudinalActuatorDelay + DT_MDL - a_target, should_stop = get_accel_from_plan(longitudinalPlan.speeds, longitudinalPlan.accels, - action_t=action_t, vEgoStopping=self.CP.vEgoStopping) - longitudinalPlan.aTarget = float(a_target) - longitudinalPlan.shouldStop = bool(should_stop) + longitudinalPlan.aTarget = float(self.output_a_target) + longitudinalPlan.shouldStop = bool(self.output_should_stop) longitudinalPlan.allowBrake = True longitudinalPlan.allowThrottle = bool(self.allow_throttle) diff --git a/selfdrive/debug/clear_dtc.py b/selfdrive/debug/clear_dtc.py index 2c7f525d80..3578930388 100755 --- a/selfdrive/debug/clear_dtc.py +++ b/selfdrive/debug/clear_dtc.py @@ -4,6 +4,7 @@ import argparse from subprocess import check_output, CalledProcessError from opendbc.car.carlog import carlog from opendbc.car.uds import UdsClient, MessageTimeoutError, SESSION_TYPE, DTC_GROUP_TYPE +from opendbc.safety import Safety from panda import Panda parser = argparse.ArgumentParser(description="clear DTC status") @@ -24,7 +25,7 @@ except CalledProcessError as e: raise e panda = Panda() -panda.set_safety_mode(Panda.SAFETY_ELM327) +panda.set_safety_mode(Safety.SAFETY_ELM327) uds_client = UdsClient(panda, args.addr, bus=args.bus) print("extended diagnostic session ...") try: diff --git a/selfdrive/debug/hyundai_enable_radar_points.py b/selfdrive/debug/hyundai_enable_radar_points.py index 27e6c68c0e..8612018b08 100755 --- a/selfdrive/debug/hyundai_enable_radar_points.py +++ b/selfdrive/debug/hyundai_enable_radar_points.py @@ -18,6 +18,7 @@ from subprocess import check_output, CalledProcessError from opendbc.car.carlog import carlog from opendbc.car.uds import UdsClient, SESSION_TYPE, DATA_IDENTIFIER_TYPE +from opendbc.safety import Safety from panda.python import Panda class ConfigValues(NamedTuple): @@ -96,7 +97,7 @@ if __name__ == "__main__": sys.exit(0) panda = Panda() - panda.set_safety_mode(Panda.SAFETY_ELM327) + panda.set_safety_mode(Safety.SAFETY_ELM327) uds_client = UdsClient(panda, 0x7D0, bus=args.bus) print("\n[START DIAGNOSTIC SESSION]") diff --git a/selfdrive/debug/read_dtc_status.py b/selfdrive/debug/read_dtc_status.py index 4ae471aefa..56f32abf46 100755 --- a/selfdrive/debug/read_dtc_status.py +++ b/selfdrive/debug/read_dtc_status.py @@ -4,6 +4,7 @@ import argparse from subprocess import check_output, CalledProcessError from opendbc.car.carlog import carlog from opendbc.car.uds import UdsClient, SESSION_TYPE, DTC_REPORT_TYPE, DTC_STATUS_MASK_TYPE, get_dtc_num_as_str, get_dtc_status_names +from opendbc.safety import Safety from panda import Panda parser = argparse.ArgumentParser(description="read DTC status") @@ -24,7 +25,7 @@ except CalledProcessError as e: raise e panda = Panda() -panda.set_safety_mode(Panda.SAFETY_ELM327) +panda.set_safety_mode(Safety.SAFETY_ELM327) uds_client = UdsClient(panda, args.addr, bus=args.bus) print("extended diagnostic session ...") uds_client.diagnostic_session_control(SESSION_TYPE.EXTENDED_DIAGNOSTIC) diff --git a/selfdrive/debug/vw_mqb_config.py b/selfdrive/debug/vw_mqb_config.py index 69d2951941..bc92d3a4c4 100755 --- a/selfdrive/debug/vw_mqb_config.py +++ b/selfdrive/debug/vw_mqb_config.py @@ -6,6 +6,7 @@ from enum import IntEnum from opendbc.car.carlog import carlog from opendbc.car.uds import UdsClient, MessageTimeoutError, NegativeResponseError, SESSION_TYPE,\ DATA_IDENTIFIER_TYPE, ACCESS_TYPE +from opendbc.safety import Safety from panda import Panda from datetime import date @@ -38,7 +39,7 @@ if __name__ == "__main__": carlog.setLevel('DEBUG') panda = Panda() - panda.set_safety_mode(Panda.SAFETY_ELM327) + panda.set_safety_mode(Safety.SAFETY_ELM327) bus = 1 if panda.has_obd() else 0 uds_client = UdsClient(panda, MQB_EPS_CAN_ADDR, MQB_EPS_CAN_ADDR + RX_OFFSET, bus, timeout=0.2) diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py index 6e154bf07c..8ce884ae4f 100755 --- a/selfdrive/locationd/calibrationd.py +++ b/selfdrive/locationd/calibrationd.py @@ -6,7 +6,6 @@ While the roll calibration is a real value that can be estimated, here we assume and the image input into the neural network is not corrected for roll. ''' -import gc import os import capnp import numpy as np @@ -16,7 +15,7 @@ from cereal import log import cereal.messaging as messaging from openpilot.common.conversions import Conversions as CV from openpilot.common.params import Params -from openpilot.common.realtime import set_realtime_priority +from openpilot.common.realtime import config_realtime_process from openpilot.common.transformations.orientation import rot_from_euler, euler_from_rot from openpilot.common.swaglog import cloudlog @@ -256,8 +255,7 @@ class Calibrator: def main() -> NoReturn: - gc.disable() - set_realtime_priority(1) + config_realtime_process([0, 1, 2, 3], 5) pm = messaging.PubMaster(['liveCalibration']) sm = messaging.SubMaster(['cameraOdometry', 'carState', 'carParams'], poll='cameraOdometry') diff --git a/selfdrive/modeld/dmonitoringmodeld.py b/selfdrive/modeld/dmonitoringmodeld.py index e2dd6f71ca..a20155e3ec 100755 --- a/selfdrive/modeld/dmonitoringmodeld.py +++ b/selfdrive/modeld/dmonitoringmodeld.py @@ -8,7 +8,6 @@ if TICI: os.environ['QCOM'] = '1' else: from openpilot.selfdrive.modeld.runners.ort_helpers import make_onnx_cpu_runner -import gc import math import time import pickle @@ -21,7 +20,7 @@ from cereal import messaging from cereal.messaging import PubMaster, SubMaster from msgq.visionipc import VisionIpcClient, VisionStreamType, VisionBuf from openpilot.common.swaglog import cloudlog -from openpilot.common.realtime import set_realtime_priority +from openpilot.common.realtime import config_realtime_process from openpilot.common.transformations.model import dmonitoringmodel_intrinsics, DM_INPUT_SIZE from openpilot.common.transformations.camera import _ar_ox_fisheye, _os_fisheye from openpilot.selfdrive.modeld.models.commonmodel_pyx import CLContext, MonitoringModelFrame @@ -140,9 +139,8 @@ def get_driverstate_packet(model_output: np.ndarray, frame_id: int, location_ts: def main(): - gc.disable() setproctitle(PROCESS_NAME) - set_realtime_priority(1) + config_realtime_process([0, 1, 2, 3], 5) sentry.set_tag("daemon", PROCESS_NAME) cloudlog.bind(daemon=PROCESS_NAME) diff --git a/selfdrive/monitoring/dmonitoringd.py b/selfdrive/monitoring/dmonitoringd.py index 54d22c1243..f137b406b4 100755 --- a/selfdrive/monitoring/dmonitoringd.py +++ b/selfdrive/monitoring/dmonitoringd.py @@ -1,15 +1,12 @@ #!/usr/bin/env python3 -import gc - import cereal.messaging as messaging from openpilot.common.params import Params -from openpilot.common.realtime import set_realtime_priority +from openpilot.common.realtime import config_realtime_process from openpilot.selfdrive.monitoring.helpers import DriverMonitoring def dmonitoringd_thread(): - gc.disable() - set_realtime_priority(2) + config_realtime_process([0, 1, 2, 3], 5) params = Params() pm = messaging.PubMaster(['driverMonitoringState']) diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index cf189df57b..9d30090b8b 100755 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -7,7 +7,7 @@ import cereal.messaging as messaging from cereal import car, log from msgq.visionipc import VisionIpcClient, VisionStreamType -from panda import ALTERNATIVE_EXPERIENCE +from opendbc.safety import ALTERNATIVE_EXPERIENCE from openpilot.common.params import Params diff --git a/selfdrive/test/longitudinal_maneuvers/maneuver.py b/selfdrive/test/longitudinal_maneuvers/maneuver.py index 301f99dd56..dfd5b3e109 100644 --- a/selfdrive/test/longitudinal_maneuvers/maneuver.py +++ b/selfdrive/test/longitudinal_maneuvers/maneuver.py @@ -66,7 +66,7 @@ class Maneuver: print("Crashed!!!!") valid = False - if self.ensure_start and log['v_rel'] > 0 and log['speeds'][-1] <= 0.1: + if self.ensure_start and log['v_rel'] > 0 and log['acceleration'] < 1e-3: print('LongitudinalPlanner not starting!') valid = False diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index 026c8ce22a..989b84dee3 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -30,8 +30,8 @@ class Plant: self.distance = 0. self.speed = speed + self.should_stop = False self.acceleration = 0.0 - self.speeds = [] # lead car self.lead_relevancy = lead_relevancy @@ -134,9 +134,9 @@ class Plant: 'liveParameters': lp.liveParameters, 'modelV2': model.modelV2} self.planner.update(sm) - self.speed = self.planner.v_desired_filter.x - self.acceleration = self.planner.a_desired - self.speeds = self.planner.v_desired_trajectory.tolist() + self.acceleration = self.planner.output_a_target + self.speed = self.speed + self.acceleration * self.ts + self.should_stop = self.planner.output_should_stop fcw = self.planner.fcw self.distance_lead = self.distance_lead + v_lead * self.ts @@ -168,7 +168,7 @@ class Plant: "distance": self.distance, "speed": self.speed, "acceleration": self.acceleration, - "speeds": self.speeds, + "should_stop": self.should_stop, "distance_lead": self.distance_lead, "fcw": fcw, } diff --git a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py index 4bc1ebba8f..ab1800b4fb 100644 --- a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py +++ b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py @@ -150,10 +150,7 @@ def create_maneuvers(kwargs): enabled=False, **kwargs, ), - ] - if not kwargs['e2e']: - # allow_throttle won't trigger with e2e - maneuvers.append(Maneuver( + Maneuver( "slow to 5m/s with allow_throttle = False and pitch = +0.1", duration=30., initial_speed=20., @@ -164,7 +161,7 @@ def create_maneuvers(kwargs): breakpoints=[0.0, 2., 2.01], ensure_slowdown=True, **kwargs, - )) + )] if not kwargs['force_decel']: # controls relies on planner commanding to move for stock-ACC resume spamming maneuvers.append(Maneuver( diff --git a/selfdrive/test/process_replay/migration.py b/selfdrive/test/process_replay/migration.py index 14934f8521..fbe891abea 100644 --- a/selfdrive/test/process_replay/migration.py +++ b/selfdrive/test/process_replay/migration.py @@ -6,15 +6,15 @@ import traceback from cereal import messaging, car, log from opendbc.car.fingerprints import MIGRATION -from opendbc.car.toyota.values import EPS_SCALE -from opendbc.car.ford.values import CAR as FORD, FordFlags +from opendbc.car.toyota.values import EPS_SCALE, ToyotaSafetyFlags +from opendbc.car.ford.values import CAR as FORD, FordFlags, FordSafetyFlags +from opendbc.car.hyundai.values import HyundaiSafetyFlags from openpilot.selfdrive.modeld.constants import ModelConstants from openpilot.selfdrive.modeld.fill_model_msg import fill_xyz_poly, fill_lane_line_meta from openpilot.selfdrive.test.process_replay.vision_meta import meta_from_encode_index from openpilot.selfdrive.controls.lib.longitudinal_planner import get_accel_from_plan from openpilot.system.manager.process_config import managed_processes from openpilot.tools.lib.logreader import LogIterable -from panda import Panda MessageWithIndex = tuple[int, capnp.lib.capnp._DynamicStructReader] MigrationOps = tuple[list[tuple[int, capnp.lib.capnp._DynamicStructReader]], list[capnp.lib.capnp._DynamicStructReader], list[int]] @@ -269,19 +269,19 @@ def migrate_carOutput(msgs): def migrate_pandaStates(msgs): # TODO: safety param migration should be handled automatically safety_param_migration = { - "TOYOTA_PRIUS": EPS_SCALE["TOYOTA_PRIUS"] | Panda.FLAG_TOYOTA_STOCK_LONGITUDINAL, - "TOYOTA_RAV4": EPS_SCALE["TOYOTA_RAV4"] | Panda.FLAG_TOYOTA_ALT_BRAKE, - "KIA_EV6": Panda.FLAG_HYUNDAI_EV_GAS | Panda.FLAG_HYUNDAI_CANFD_HDA2, + "TOYOTA_PRIUS": EPS_SCALE["TOYOTA_PRIUS"] | ToyotaSafetyFlags.FLAG_TOYOTA_STOCK_LONGITUDINAL, + "TOYOTA_RAV4": EPS_SCALE["TOYOTA_RAV4"] | ToyotaSafetyFlags.FLAG_TOYOTA_ALT_BRAKE, + "KIA_EV6": HyundaiSafetyFlags.FLAG_HYUNDAI_EV_GAS | HyundaiSafetyFlags.FLAG_HYUNDAI_CANFD_HDA2, } # TODO: get new Ford route - safety_param_migration |= {car: Panda.FLAG_FORD_LONG_CONTROL for car in (set(FORD) - FORD.with_flags(FordFlags.CANFD))} + safety_param_migration |= {car: FordSafetyFlags.FLAG_FORD_LONG_CONTROL for car in (set(FORD) - FORD.with_flags(FordFlags.CANFD))} # Migrate safety param base on carParams CP = next((m.carParams for _, m in msgs if m.which() == 'carParams'), None) assert CP is not None, "carParams message not found" fingerprint = MIGRATION.get(CP.carFingerprint, CP.carFingerprint) if fingerprint in safety_param_migration: - safety_param = safety_param_migration[fingerprint] + safety_param = safety_param_migration[fingerprint].value elif len(CP.safetyConfigs): safety_param = CP.safetyConfigs[0].safetyParam if CP.safetyConfigs[0].safetyParamDEPRECATED != 0: diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index e4b3ac88e2..ad21c5e40a 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -18,11 +18,11 @@ from cereal import car from cereal.services import SERVICE_LIST from msgq.visionipc import VisionIpcServer, get_endpoint_name as vipc_get_endpoint_name from opendbc.car.car_helpers import get_car, interfaces +from opendbc.safety import ALTERNATIVE_EXPERIENCE from openpilot.common.params import Params from openpilot.common.prefix import OpenpilotPrefix from openpilot.common.timeout import Timeout from openpilot.common.realtime import DT_CTRL -from panda.python import ALTERNATIVE_EXPERIENCE from openpilot.selfdrive.car.card import can_comm_callbacks from openpilot.system.manager.process_config import managed_processes from openpilot.selfdrive.test.process_replay.vision_meta import meta_from_camera_state, available_streams diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 3b5f5f3466..1cdd551dd0 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -95f1384edc0dc00959b0e804de1aaafc35d2f15f \ No newline at end of file +b8595cc8351043a7c49b41e7c36f93ffd2e3dc6d diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index 72f0b10a97..98a800e9bd 100644 --- a/selfdrive/test/test_onroad.py +++ b/selfdrive/test/test_onroad.py @@ -34,7 +34,7 @@ CPU usage budget TEST_DURATION = 25 LOG_OFFSET = 8 -MAX_TOTAL_CPU = 275. # total for all 8 cores +MAX_TOTAL_CPU = 280. # total for all 8 cores PROCS = { # Baseline CPU usage by process "selfdrive.controls.controlsd": 16.0, @@ -42,7 +42,7 @@ PROCS = { "selfdrive.car.card": 26.0, "./loggerd": 14.0, "./encoderd": 17.0, - "./camerad": 14.5, + "./camerad": 10.0, "selfdrive.controls.plannerd": 9.0, "./ui": 18.0, "selfdrive.locationd.paramsd": 9.0, @@ -99,12 +99,12 @@ TIMINGS = { "wideRoadCameraState": [1.5, 0.35], } -LOGS_SIZE_RATE = { - "qlog.zst": 0.0083, - "rlog.zst": 0.135, - "qcamera.ts": 0.03828, +LOGS_SIZE = { # MB per segment + "qlog.zst": 0.5, + "rlog.zst": 8.1, + "qcamera.ts": 2.3, } -LOGS_SIZE_RATE.update(dict.fromkeys(['ecamera.hevc', 'fcamera.hevc'], 1.2740)) +LOGS_SIZE.update(dict.fromkeys(['ecamera.hevc', 'fcamera.hevc', 'dcamera.hevc'], 76.5)) def cputime_total(ct): @@ -126,6 +126,7 @@ class TestOnroad: # setup env params = Params() params.remove("CurrentRoute") + params.put_bool("RecordFront", True) set_params_enabled() os.environ['REPLAY'] = '1' os.environ['TESTING_CLOSET'] = '1' @@ -212,12 +213,13 @@ class TestOnroad: big_logs = [f for f, n in cnt.most_common(3) if n / sum(cnt.values()) > 30.] assert len(big_logs) == 0, f"Log spam: {big_logs}" - def test_log_sizes(self): + def test_log_sizes(self, subtests): for f, sz in self.log_sizes.items(): - rate = LOGS_SIZE_RATE[f.name] - minn = rate * TEST_DURATION * 0.8 - maxx = rate * TEST_DURATION * 1.2 - assert minn < sz < maxx + rate = LOGS_SIZE[f.name]/60. + minn = rate * TEST_DURATION * 0.5 + maxx = rate * TEST_DURATION * 1.5 + with subtests.test(file=f.name): + assert minn < sz < maxx def test_ui_timings(self): result = "\n" @@ -311,24 +313,8 @@ class TestOnroad: def test_gpu_usage(self): assert self.gpu_procs == {"weston", "ui", "camerad", "selfdrive.modeld.modeld", "selfdrive.modeld.dmonitoringmodeld"} - def test_camera_processing_time(self): - result = "\n" - result += "------------------------------------------------\n" - result += "-------------- ImgProc Timing ------------------\n" - result += "------------------------------------------------\n" - - ts = [] - for s in ['roadCameraState', 'driverCameraState', 'wideCameraState']: - ts.extend(getattr(m, s).processingTime for m in self.msgs[s]) - assert min(ts) < 0.025, f"high execution time: {min(ts)}" - result += f"execution time: min {min(ts):.5f}s\n" - result += f"execution time: max {max(ts):.5f}s\n" - result += f"execution time: mean {np.mean(ts):.5f}s\n" - result += "------------------------------------------------\n" - print(result) - @pytest.mark.skip("TODO: enable once timings are fixed") - def test_camera_frame_timings(self): + def test_camera_frame_timings(self, subtests): result = "\n" result += "------------------------------------------------\n" result += "----------------- SoF Timing ------------------\n" @@ -337,11 +323,12 @@ class TestOnroad: ts = [getattr(m, m.which()).timestampSof for m in self.lr if name in m.which()] d_ms = np.diff(ts) / 1e6 d50 = np.abs(d_ms-50) - assert max(d50) < 1.0, f"high sof delta vs 50ms: {max(d50)}" result += f"{name} sof delta vs 50ms: min {min(d50):.5f}s\n" result += f"{name} sof delta vs 50ms: max {max(d50):.5f}s\n" result += f"{name} sof delta vs 50ms: mean {d50.mean():.5f}s\n" - result += "------------------------------------------------\n" + with subtests.test(camera=name): + assert max(d50) < 1.0, f"high SOF delta vs 50ms: {max(d50)}" + result += "------------------------------------------------\n" print(result) def test_mpc_execution_timings(self): diff --git a/system/camerad/cameras/camera_qcom2.cc b/system/camerad/cameras/camera_qcom2.cc index 0aa9f1443d..e5c9f548ab 100644 --- a/system/camerad/cameras/camera_qcom2.cc +++ b/system/camerad/cameras/camera_qcom2.cc @@ -55,7 +55,7 @@ public: float fl_pix = 0; - CameraState(SpectraMaster *master, const CameraConfig &config) : camera(master, config, config.stream_type == VISION_STREAM_DRIVER ? ISP_BPS_PROCESSED : ISP_IFE_PROCESSED) {}; + CameraState(SpectraMaster *master, const CameraConfig &config) : camera(master, config, config.stream_type == VISION_STREAM_ROAD ? ISP_RAW_OUTPUT : ISP_IFE_PROCESSED) {}; ~CameraState(); void init(VisionIpcServer *v, cl_device_id device_id, cl_context ctx); void update_exposure_score(float desired_ev, int exp_t, int exp_g_idx, float exp_gain); @@ -283,7 +283,7 @@ void camerad_thread() { // *** per-cam init *** std::vector> cams; - for (const auto &config : {WIDE_ROAD_CAMERA_CONFIG, ROAD_CAMERA_CONFIG, DRIVER_CAMERA_CONFIG}) { + for (const auto &config : {WIDE_ROAD_CAMERA_CONFIG, DRIVER_CAMERA_CONFIG, ROAD_CAMERA_CONFIG}) { auto cam = std::make_unique(&m, config); cam->init(&v, device_id, ctx); cams.emplace_back(std::move(cam)); diff --git a/system/hardware/tici/tests/test_power_draw.py b/system/hardware/tici/tests/test_power_draw.py index 0a5737574b..e1b9845c4c 100644 --- a/system/hardware/tici/tests/test_power_draw.py +++ b/system/hardware/tici/tests/test_power_draw.py @@ -31,7 +31,7 @@ class Proc: PROCS = [ - Proc(['camerad'], 1.6, msgs=['roadCameraState', 'wideRoadCameraState', 'driverCameraState']), + Proc(['camerad'], 1.75, msgs=['roadCameraState', 'wideRoadCameraState', 'driverCameraState']), Proc(['modeld'], 1.12, atol=0.2, msgs=['modelV2']), Proc(['dmonitoringmodeld'], 0.6, msgs=['driverStateV2']), Proc(['encoderd'], 0.23, msgs=[]), diff --git a/tools/sim/lib/simulated_car.py b/tools/sim/lib/simulated_car.py index 253623ded7..402ac1a21e 100644 --- a/tools/sim/lib/simulated_car.py +++ b/tools/sim/lib/simulated_car.py @@ -1,11 +1,12 @@ +import traceback import cereal.messaging as messaging from opendbc.can.packer import CANPacker from opendbc.can.parser import CANParser +from opendbc.car.honda.values import HondaSafetyFlags from openpilot.common.params import Params from openpilot.selfdrive.pandad.pandad_api_impl import can_list_to_can_capnp from openpilot.tools.sim.lib.common import SimulatorState -from panda.python import Panda class SimulatedCar: @@ -94,14 +95,18 @@ class SimulatedCar: 'controlsAllowed': True, 'safetyModel': 'hondaBosch', 'alternativeExperience': self.sm["carParams"].alternativeExperience, - 'safetyParam': Panda.FLAG_HONDA_RADARLESS | Panda.FLAG_HONDA_BOSCH_LONG, + 'safetyParam': HondaSafetyFlags.FLAG_HONDA_RADARLESS.value | HondaSafetyFlags.FLAG_HONDA_BOSCH_LONG.value, } self.pm.send('pandaStates', dat) def update(self, simulator_state: SimulatorState): - self.send_can_messages(simulator_state) + try: + self.send_can_messages(simulator_state) - if self.idx % 50 == 0: # only send panda states at 2hz - self.send_panda_state(simulator_state) + if self.idx % 50 == 0: # only send panda states at 2hz + self.send_panda_state(simulator_state) - self.idx += 1 + self.idx += 1 + except Exception: + traceback.print_exc() + raise