From c60ce2a770e32a228f5dc234df7a37d2c77c37d1 Mon Sep 17 00:00:00 2001 From: Daniel <35647712+BadOlives@users.noreply.github.com> Date: Wed, 27 Apr 2022 21:43:36 -0700 Subject: [PATCH 01/21] Update can_replay.py (#24346) Fixed typo from PandaJugnle to PandaJungle --- tools/replay/can_replay.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tools/replay/can_replay.py b/tools/replay/can_replay.py index 7b5949c26..4b0ad21db 100755 --- a/tools/replay/can_replay.py +++ b/tools/replay/can_replay.py @@ -17,7 +17,7 @@ try: panda_jungle_imported = True from panda_jungle import PandaJungle # pylint: disable=import-error # type: ignore except ImportError: - PandaJugnle = None + PandaJungle = None panda_jungle_imported = False From be0f27ac852fe41a904af1bb05b1438d43938896 Mon Sep 17 00:00:00 2001 From: Jason Young <46612682+jyoung8607@users.noreply.github.com> Date: Thu, 28 Apr 2022 00:57:37 -0400 Subject: [PATCH 02/21] Fix PlotJuggler saved tuning layout (#24347) --- tools/plotjuggler/layouts/tuning.xml | 36 ++++++++++++++-------------- 1 file changed, 18 insertions(+), 18 deletions(-) diff --git a/tools/plotjuggler/layouts/tuning.xml b/tools/plotjuggler/layouts/tuning.xml index 877fb82a5..503e726ca 100644 --- a/tools/plotjuggler/layouts/tuning.xml +++ b/tools/plotjuggler/layouts/tuning.xml @@ -161,12 +161,12 @@ if (time > last_bad_time + engage_delay) then else return 0 end - /liveLocationKalman/angularVelocityCalibrated/value/2 - + /liveLocationKalman/angularVelocityCalibrated/value/2 + /carState/steeringPressed /carControl/enabled /liveLocationKalman/velocityCalibrated/value/0 - + engage_delay = 5 @@ -184,11 +184,11 @@ if (time > last_bad_time + engage_delay) then else return 0 end - /controlsState/curvature - + /controlsState/curvature + /carState/steeringPressed /carControl/enabled - + engage_delay = 5 @@ -206,11 +206,11 @@ if (time > last_bad_time + engage_delay) then else return 0 end - /lateralPlan/curvatures/0 - + /lateralPlan/curvatures/0 + /carState/steeringPressed /carControl/enabled - + engage_delay = 5 @@ -229,12 +229,12 @@ if (time > last_bad_time + engage_delay) then else return 0 end - /carState/aEgo - + /carState/aEgo + /carState/brakePressed /carState/gasPressed /carControl/enabled - + engage_delay = 5 @@ -253,12 +253,12 @@ if (time > last_bad_time + engage_delay) then else return 0 end - /longitudinalPlan/accels/0 - + /longitudinalPlan/accels/0 + /carState/brakePressed /carState/gasPressed /carControl/enabled - + engage_delay = 5 @@ -277,12 +277,12 @@ if (time > last_bad_time + engage_delay) then else return 0 end - /carControl/actuators/accel - + /carControl/actuators/accel + /carState/brakePressed /carState/gasPressed /carControl/enabled - + From c9be2f02c3877fe00674ea7fc86912234376a36a Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Thu, 28 Apr 2022 00:42:52 -0700 Subject: [PATCH 03/21] Clean up controllers (#24340) * clean up lat controllers * pass CP once * sort --- selfdrive/controls/controlsd.py | 9 +++---- selfdrive/controls/lib/latcontrol.py | 4 +-- selfdrive/controls/lib/latcontrol_angle.py | 2 +- selfdrive/controls/lib/latcontrol_indi.py | 2 +- selfdrive/controls/lib/latcontrol_pid.py | 13 ++++------ selfdrive/controls/lib/latcontrol_torque.py | 23 +++++++++--------- selfdrive/controls/lib/longcontrol.py | 27 +++++++++++---------- 7 files changed, 38 insertions(+), 42 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index ab23af7b8..ffc52cb3e 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -98,8 +98,7 @@ class Controls: self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration', 'driverMonitoringState', 'longitudinalPlan', 'lateralPlan', 'liveLocationKalman', 'managerState', 'liveParameters', 'radarState'] + self.camera_packets + joystick_packet, - ignore_alive=ignore, ignore_avg_freq=['radarState', 'longitudinalPlan']) - + ignore_alive=ignore, ignore_avg_freq=['radarState', 'longitudinalPlan']) # set alternative experiences from parameters self.disengage_on_accelerator = params.get_bool("DisengageOnAccelerator") @@ -557,15 +556,15 @@ class Controls: # accel PID loop pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, self.v_cruise_kph * CV.KPH_TO_MS) t_since_plan = (self.sm.frame - self.sm.rcv_frame['longitudinalPlan']) * DT_CTRL - actuators.accel = self.LoC.update(CC.longActive, CS, self.CP, long_plan, pid_accel_limits, t_since_plan) + actuators.accel = self.LoC.update(CC.longActive, CS, long_plan, pid_accel_limits, t_since_plan) # Steering PID loop and lateral MPC desired_curvature, desired_curvature_rate = get_lag_adjusted_curvature(self.CP, CS.vEgo, lat_plan.psis, lat_plan.curvatures, lat_plan.curvatureRates) - actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.CP, self.VM, - params, self.last_actuators, desired_curvature, + actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, params, + self.last_actuators, desired_curvature, desired_curvature_rate, self.sm['liveLocationKalman']) else: lac_log = log.ControlsState.LateralDebugState.new_message() diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index 0d137df73..785c8faa8 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -1,7 +1,7 @@ from abc import abstractmethod, ABC -from common.realtime import DT_CTRL from common.numpy_fast import clip +from common.realtime import DT_CTRL MIN_STEER_SPEED = 0.3 @@ -16,7 +16,7 @@ class LatControl(ABC): self.steer_max = 1.0 @abstractmethod - def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk): + def update(self, active, CS, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk): pass def reset(self): diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index 8d09432b6..2eee71c70 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/selfdrive/controls/lib/latcontrol_angle.py @@ -7,7 +7,7 @@ STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees class LatControlAngle(LatControl): - def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk): + def update(self, active, CS, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk): angle_log = log.ControlsState.LateralAngleState.new_message() if CS.vEgo < MIN_STEER_SPEED or not active: diff --git a/selfdrive/controls/lib/latcontrol_indi.py b/selfdrive/controls/lib/latcontrol_indi.py index 4d2a72762..b5041eb17 100644 --- a/selfdrive/controls/lib/latcontrol_indi.py +++ b/selfdrive/controls/lib/latcontrol_indi.py @@ -63,7 +63,7 @@ class LatControlINDI(LatControl): self.steer_filter.x = 0. self.speed = 0. - def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk): + def update(self, active, CS, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk): self.speed = CS.vEgo # Update Kalman filter y = np.array([[math.radians(CS.steeringAngleDeg)], [math.radians(CS.steeringRateDeg)]]) diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index 813ba13ab..5dd1b20ae 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -1,23 +1,23 @@ import math -from selfdrive.controls.lib.pid import PIDController -from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED from cereal import log +from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED +from selfdrive.controls.lib.pid import PIDController class LatControlPID(LatControl): def __init__(self, CP, CI): super().__init__(CP, CI) self.pid = PIDController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV), - (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV), - k_f=CP.lateralTuning.pid.kf, pos_limit=1.0, neg_limit=-1.0) + (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV), + k_f=CP.lateralTuning.pid.kf, pos_limit=self.steer_max, neg_limit=-self.steer_max) self.get_steer_feedforward = CI.get_steer_feedforward_function() def reset(self): super().reset() self.pid.reset() - def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk): + def update(self, active, CS, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk): pid_log = log.ControlsState.LateralPIDState.new_message() pid_log.steeringAngleDeg = float(CS.steeringAngleDeg) pid_log.steeringRateDeg = float(CS.steeringRateDeg) @@ -33,9 +33,6 @@ class LatControlPID(LatControl): pid_log.active = False self.pid.reset() else: - self.pid.pos_limit = self.steer_max - self.pid.neg_limit = -self.steer_max - # offset does not contribute to resistive torque steer_feedforward = self.get_steer_feedforward(angle_steers_des_no_offset, CS.vEgo) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 862e8b384..a73744de7 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -1,9 +1,10 @@ import math -from selfdrive.controls.lib.pid import PIDController + +from cereal import log from common.numpy_fast import interp from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED +from selfdrive.controls.lib.pid import PIDController from selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY -from cereal import log # At higher speeds (25+mph) we can assume: # Lateral acceleration achieved by a specific car correlates to @@ -12,7 +13,7 @@ from cereal import log # This controller applies torque to achieve desired lateral # accelerations. To compensate for the low speed effects we -# use a LOW_SPEED_FACTOR in the error. Additionally there is +# use a LOW_SPEED_FACTOR in the error. Additionally, there is # friction in the steering wheel that needs to be overcome to # move it at all, this is compensated for too. @@ -24,12 +25,10 @@ JERK_THRESHOLD = 0.2 class LatControlTorque(LatControl): def __init__(self, CP, CI): super().__init__(CP, CI) + self.CP = CP self.pid = PIDController(CP.lateralTuning.torque.kp, CP.lateralTuning.torque.ki, - k_f=CP.lateralTuning.torque.kf, pos_limit=1.0, neg_limit=-1.0) + k_f=CP.lateralTuning.torque.kf, pos_limit=self.steer_max, neg_limit=-self.steer_max) self.get_steer_feedforward = CI.get_steer_feedforward_function() - self.steer_max = 1.0 - self.pid.pos_limit = self.steer_max - self.pid.neg_limit = -self.steer_max self.use_steering_angle = CP.lateralTuning.torque.useSteeringAngle self.friction = CP.lateralTuning.torque.friction @@ -37,7 +36,7 @@ class LatControlTorque(LatControl): super().reset() self.pid.reset() - def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk): + def update(self, active, CS, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk): pid_log = log.ControlsState.LateralTorqueState.new_message() if CS.vEgo < MIN_STEER_SPEED or not active: @@ -49,9 +48,9 @@ class LatControlTorque(LatControl): actual_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll) else: actual_curvature = llk.angularVelocityCalibrated.value[2] / CS.vEgo - desired_lateral_accel = desired_curvature * CS.vEgo**2 - desired_lateral_jerk = desired_curvature_rate * CS.vEgo**2 - actual_lateral_accel = actual_curvature * CS.vEgo**2 + desired_lateral_accel = desired_curvature * CS.vEgo ** 2 + desired_lateral_jerk = desired_curvature_rate * CS.vEgo ** 2 + actual_lateral_accel = actual_curvature * CS.vEgo ** 2 setpoint = desired_lateral_accel + LOW_SPEED_FACTOR * desired_curvature measurement = actual_lateral_accel + LOW_SPEED_FACTOR * actual_curvature @@ -61,7 +60,7 @@ class LatControlTorque(LatControl): ff = desired_lateral_accel - params.roll * ACCELERATION_DUE_TO_GRAVITY # convert friction into lateral accel units for feedforward friction_compensation = interp(desired_lateral_jerk, [-JERK_THRESHOLD, JERK_THRESHOLD], [-self.friction, self.friction]) - ff += friction_compensation / CP.lateralTuning.torque.kf + ff += friction_compensation / self.CP.lateralTuning.torque.kf output_torque = self.pid.update(error, override=CS.steeringPressed, feedforward=ff, speed=CS.vEgo, diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 0fda7789a..f2cd28219 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -1,8 +1,8 @@ from cereal import car from common.numpy_fast import clip, interp from common.realtime import DT_CTRL -from selfdrive.controls.lib.pid import PIDController from selfdrive.controls.lib.drive_helpers import CONTROL_N +from selfdrive.controls.lib.pid import PIDController from selfdrive.modeld.constants import T_IDXS LongCtrlState = car.CarControl.Actuators.LongControlState @@ -50,12 +50,13 @@ def long_control_state_trans(CP, active, long_control_state, v_ego, v_target, return long_control_state -class LongControl(): +class LongControl: def __init__(self, CP): + self.CP = CP self.long_control_state = LongCtrlState.off # initialized to off self.pid = PIDController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV), (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV), - k_f = CP.longitudinalTuning.kf, rate=1 / DT_CTRL) + k_f=CP.longitudinalTuning.kf, rate=1 / DT_CTRL) self.v_pid = 0.0 self.last_output_accel = 0.0 @@ -64,7 +65,7 @@ class LongControl(): self.pid.reset() self.v_pid = v_pid - def update(self, active, CS, CP, long_plan, accel_limits, t_since_plan): + def update(self, active, CS, long_plan, accel_limits, t_since_plan): """Update longitudinal control. This updates the state machine and runs a PID loop""" # Interp control trajectory speeds = long_plan.speeds @@ -72,11 +73,11 @@ class LongControl(): v_target = interp(t_since_plan, T_IDXS[:CONTROL_N], speeds) a_target = interp(t_since_plan, T_IDXS[:CONTROL_N], long_plan.accels) - v_target_lower = interp(CP.longitudinalActuatorDelayLowerBound + t_since_plan, T_IDXS[:CONTROL_N], speeds) - a_target_lower = 2 * (v_target_lower - v_target) / CP.longitudinalActuatorDelayLowerBound - a_target + v_target_lower = interp(self.CP.longitudinalActuatorDelayLowerBound + t_since_plan, T_IDXS[:CONTROL_N], speeds) + a_target_lower = 2 * (v_target_lower - v_target) / self.CP.longitudinalActuatorDelayLowerBound - a_target - v_target_upper = interp(CP.longitudinalActuatorDelayUpperBound + t_since_plan, T_IDXS[:CONTROL_N], speeds) - a_target_upper = 2 * (v_target_upper - v_target) / CP.longitudinalActuatorDelayUpperBound - a_target + v_target_upper = interp(self.CP.longitudinalActuatorDelayUpperBound + t_since_plan, T_IDXS[:CONTROL_N], speeds) + a_target_upper = 2 * (v_target_upper - v_target) / self.CP.longitudinalActuatorDelayUpperBound - a_target a_target = min(a_target_lower, a_target_upper) v_target_future = speeds[-1] @@ -93,7 +94,7 @@ class LongControl(): # Update state machine output_accel = self.last_output_accel - self.long_control_state = long_control_state_trans(CP, active, self.long_control_state, CS.vEgo, + self.long_control_state = long_control_state_trans(self.CP, active, self.long_control_state, CS.vEgo, v_target, v_target_future, CS.brakePressed, CS.cruiseState.standstill) @@ -107,8 +108,8 @@ class LongControl(): # Toyota starts braking more when it thinks you want to stop # Freeze the integrator so we don't accelerate to compensate, and don't allow positive acceleration - prevent_overshoot = not CP.stoppingControl and CS.vEgo < 1.5 and v_target_future < 0.7 and v_target_future < self.v_pid - deadzone = interp(CS.vEgo, CP.longitudinalTuning.deadzoneBP, CP.longitudinalTuning.deadzoneV) + prevent_overshoot = not self.CP.stoppingControl and CS.vEgo < 1.5 and v_target_future < 0.7 and v_target_future < self.v_pid + deadzone = interp(CS.vEgo, self.CP.longitudinalTuning.deadzoneBP, self.CP.longitudinalTuning.deadzoneV) freeze_integrator = prevent_overshoot error = self.v_pid - CS.vEgo @@ -121,8 +122,8 @@ class LongControl(): # Intention is to stop, switch to a different brake control until we stop elif self.long_control_state == LongCtrlState.stopping: # Keep applying brakes until the car is stopped - if not CS.standstill or output_accel > CP.stopAccel: - output_accel -= CP.stoppingDecelRate * DT_CTRL + if not CS.standstill or output_accel > self.CP.stopAccel: + output_accel -= self.CP.stoppingDecelRate * DT_CTRL output_accel = clip(output_accel, accel_limits[0], accel_limits[1]) self.reset(CS.vEgo) From bff0db566bfc20a8f2ab936646c67be22b9ade76 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Thu, 28 Apr 2022 14:51:02 +0200 Subject: [PATCH 04/21] ui: only open map pane when route is not empty (#24352) --- selfdrive/ui/qt/maps/map.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/ui/qt/maps/map.cc b/selfdrive/ui/qt/maps/map.cc index 5e5fdb40c..3b8668d0b 100644 --- a/selfdrive/ui/qt/maps/map.cc +++ b/selfdrive/ui/qt/maps/map.cc @@ -131,7 +131,7 @@ void MapWindow::timerUpdate() { } } - if (sm->updated("navRoute")) { + if (sm->updated("navRoute") && (*sm)["navRoute"].getNavRoute().getCoordinates().size()) { qWarning() << "Got new navRoute from navd. Opening map:" << allow_open; // Only open the map on setting destination the first time From fb7d84875bc993beaf75645a7e4b311704742f02 Mon Sep 17 00:00:00 2001 From: George Hotz <72895+geohot@users.noreply.github.com> Date: Thu, 28 Apr 2022 11:32:32 -0700 Subject: [PATCH 05/21] camerad: don't remap everything every time (#24334) * premap the buffers * memory manager * free buffers properly, alignment seems okay * update camerad CPU usage * cam_sync_fd * useless line, and use the define * cheap prereqs for multistream Co-authored-by: Comma Device --- selfdrive/camerad/cameras/camera_qcom2.cc | 160 +++++++++++++--------- selfdrive/camerad/cameras/camera_qcom2.h | 17 ++- selfdrive/test/test_onroad.py | 2 +- 3 files changed, 113 insertions(+), 66 deletions(-) diff --git a/selfdrive/camerad/cameras/camera_qcom2.cc b/selfdrive/camerad/cameras/camera_qcom2.cc index 8986351fc..1d092eb6a 100644 --- a/selfdrive/camerad/cameras/camera_qcom2.cc +++ b/selfdrive/camerad/cameras/camera_qcom2.cc @@ -87,11 +87,11 @@ int do_cam_control(int fd, int op_code, void *handle, int size) { return ret; } -std::optional device_acquire(int fd, int32_t session_handle, void *data) { +std::optional device_acquire(int fd, int32_t session_handle, void *data, uint32_t num_resources=1) { struct cam_acquire_dev_cmd cmd = { .session_handle = session_handle, .handle_type = CAM_HANDLE_USER_POINTER, - .num_resources = (uint32_t)(data ? 1 : 0), + .num_resources = (uint32_t)(data ? num_resources : 0), .resource_hdl = (uint64_t)data, }; int err = do_cam_control(fd, CAM_ACQUIRE_DEV, &cmd, sizeof(cmd)); @@ -158,6 +158,42 @@ void release_fd(int video0_fd, uint32_t handle) { release(video0_fd, handle); } +void *MemoryManager::alloc(int size, uint32_t *handle) { + lock.lock(); + void *ptr; + if (!cached_allocations[size].empty()) { + ptr = cached_allocations[size].front(); + cached_allocations[size].pop(); + *handle = handle_lookup[ptr]; + } else { + ptr = alloc_w_mmu_hdl(video0_fd, size, handle); + handle_lookup[ptr] = *handle; + size_lookup[ptr] = size; + } + lock.unlock(); + return ptr; +} + +void MemoryManager::free(void *ptr) { + lock.lock(); + cached_allocations[size_lookup[ptr]].push(ptr); + lock.unlock(); +} + +MemoryManager::~MemoryManager() { + for (auto& x : cached_allocations) { + while (!x.second.empty()) { + void *ptr = x.second.front(); + x.second.pop(); + LOGD("freeing cached allocation %p with size %d", ptr, size_lookup[ptr]); + munmap(ptr, size_lookup[ptr]); + release_fd(video0_fd, handle_lookup[ptr]); + handle_lookup.erase(ptr); + size_lookup.erase(ptr); + } + } +} + int CameraState::clear_req_queue() { struct cam_req_mgr_flush_info req_mgr_flush_request = {0}; req_mgr_flush_request.session_hdl = session_handle; @@ -186,7 +222,7 @@ void CameraState::sensors_start() { void CameraState::sensors_poke(int request_id) { uint32_t cam_packet_handle = 0; int size = sizeof(struct cam_packet); - struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(multi_cam_state->video0_fd, size, &cam_packet_handle); + struct cam_packet *pkt = (struct cam_packet *)mm.alloc(size, &cam_packet_handle); pkt->num_cmd_buf = 0; pkt->kmd_cmd_buf_index = -1; pkt->header.size = size; @@ -200,15 +236,14 @@ void CameraState::sensors_poke(int request_id) { return; } - munmap(pkt, size); - release_fd(multi_cam_state->video0_fd, cam_packet_handle); + mm.free(pkt); } void CameraState::sensors_i2c(struct i2c_random_wr_payload* dat, int len, int op_code, bool data_word) { // LOGD("sensors_i2c: %d", len); uint32_t cam_packet_handle = 0; int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*1; - struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(multi_cam_state->video0_fd, size, &cam_packet_handle); + struct cam_packet *pkt = (struct cam_packet *)mm.alloc(size, &cam_packet_handle); pkt->num_cmd_buf = 1; pkt->kmd_cmd_buf_index = -1; pkt->header.size = size; @@ -218,7 +253,7 @@ void CameraState::sensors_i2c(struct i2c_random_wr_payload* dat, int len, int op buf_desc[0].size = buf_desc[0].length = sizeof(struct i2c_rdwr_header) + len*sizeof(struct i2c_random_wr_payload); buf_desc[0].type = CAM_CMD_BUF_I2C; - struct cam_cmd_i2c_random_wr *i2c_random_wr = (struct cam_cmd_i2c_random_wr *)alloc_w_mmu_hdl(multi_cam_state->video0_fd, buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle); + struct cam_cmd_i2c_random_wr *i2c_random_wr = (struct cam_cmd_i2c_random_wr *)mm.alloc(buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle); i2c_random_wr->header.count = len; i2c_random_wr->header.op_code = 1; i2c_random_wr->header.cmd_type = CAMERA_SENSOR_CMD_TYPE_I2C_RNDM_WR; @@ -233,10 +268,8 @@ void CameraState::sensors_i2c(struct i2c_random_wr_payload* dat, int len, int op return; } - munmap(i2c_random_wr, buf_desc[0].size); - release_fd(multi_cam_state->video0_fd, buf_desc[0].mem_handle); - munmap(pkt, size); - release_fd(multi_cam_state->video0_fd, cam_packet_handle); + mm.free(i2c_random_wr); + mm.free(pkt); } static cam_cmd_power *power_set_wait(cam_cmd_power *power, int16_t delay_ms) { @@ -248,10 +281,9 @@ static cam_cmd_power *power_set_wait(cam_cmd_power *power, int16_t delay_ms) { }; int CameraState::sensors_init() { - int video0_fd = multi_cam_state->video0_fd; uint32_t cam_packet_handle = 0; int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*2; - struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(video0_fd, size, &cam_packet_handle); + struct cam_packet *pkt = (struct cam_packet *)mm.alloc(size, &cam_packet_handle); pkt->num_cmd_buf = 2; pkt->kmd_cmd_buf_index = -1; pkt->header.op_code = 0x1000000 | CAM_SENSOR_PACKET_OPCODE_SENSOR_PROBE; @@ -260,7 +292,7 @@ int CameraState::sensors_init() { buf_desc[0].size = buf_desc[0].length = sizeof(struct cam_cmd_i2c_info) + sizeof(struct cam_cmd_probe); buf_desc[0].type = CAM_CMD_BUF_LEGACY; - struct cam_cmd_i2c_info *i2c_info = (struct cam_cmd_i2c_info *)alloc_w_mmu_hdl(video0_fd, buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle); + struct cam_cmd_i2c_info *i2c_info = (struct cam_cmd_i2c_info *)mm.alloc(buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle); auto probe = (struct cam_cmd_probe *)(i2c_info + 1); probe->camera_id = camera_num; @@ -302,7 +334,7 @@ int CameraState::sensors_init() { //buf_desc[1].size = buf_desc[1].length = 148; buf_desc[1].size = buf_desc[1].length = 196; buf_desc[1].type = CAM_CMD_BUF_I2C; - struct cam_cmd_power *power_settings = (struct cam_cmd_power *)alloc_w_mmu_hdl(video0_fd, buf_desc[1].size, (uint32_t*)&buf_desc[1].mem_handle); + struct cam_cmd_power *power_settings = (struct cam_cmd_power *)mm.alloc(buf_desc[1].size, (uint32_t*)&buf_desc[1].mem_handle); memset(power_settings, 0, buf_desc[1].size); // power on @@ -363,12 +395,9 @@ int CameraState::sensors_init() { LOGD("probing the sensor"); int ret = do_cam_control(sensor_fd, CAM_SENSOR_PROBE_CMD, (void *)(uintptr_t)cam_packet_handle, 0); - munmap(i2c_info, buf_desc[0].size); - release_fd(video0_fd, buf_desc[0].mem_handle); - munmap(power_settings, buf_desc[1].size); - release_fd(video0_fd, buf_desc[1].mem_handle); - munmap(pkt, size); - release_fd(video0_fd, cam_packet_handle); + mm.free(i2c_info); + mm.free(power_settings); + mm.free(pkt); return ret; } @@ -379,7 +408,7 @@ void CameraState::config_isp(int io_mem_handle, int fence, int request_id, int b if (io_mem_handle != 0) { size += sizeof(struct cam_buf_io_cfg); } - struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(multi_cam_state->video0_fd, size, &cam_packet_handle); + struct cam_packet *pkt = (struct cam_packet *)mm.alloc(size, &cam_packet_handle); pkt->num_cmd_buf = 2; pkt->kmd_cmd_buf_index = 0; // YUV has kmd_cmd_buf_offset = 1780 @@ -387,7 +416,7 @@ void CameraState::config_isp(int io_mem_handle, int fence, int request_id, int b // YUV also has patch_offset = 0x1030 and num_patches = 10 if (io_mem_handle != 0) { - pkt->io_configs_offset = sizeof(struct cam_cmd_buf_desc)*2; + pkt->io_configs_offset = sizeof(struct cam_cmd_buf_desc)*pkt->num_cmd_buf; pkt->num_io_configs = 1; } @@ -474,7 +503,7 @@ void CameraState::config_isp(int io_mem_handle, int fence, int request_id, int b buf_desc[1].length = buf_desc[1].size - buf_desc[1].offset; buf_desc[1].type = CAM_CMD_BUF_GENERIC; buf_desc[1].meta_data = CAM_ISP_PACKET_META_GENERIC_BLOB_COMMON; - uint32_t *buf2 = (uint32_t *)alloc_w_mmu_hdl(multi_cam_state->video0_fd, buf_desc[1].size, (uint32_t*)&buf_desc[1].mem_handle, 0x20); + uint32_t *buf2 = (uint32_t *)mm.alloc(buf_desc[1].size, (uint32_t*)&buf_desc[1].mem_handle); memcpy(buf2, &tmp, sizeof(tmp)); if (io_mem_handle != 0) { @@ -510,24 +539,20 @@ void CameraState::config_isp(int io_mem_handle, int fence, int request_id, int b printf("ISP CONFIG FAILED\n"); } - munmap(buf2, buf_desc[1].size); - release_fd(multi_cam_state->video0_fd, buf_desc[1].mem_handle); - // release_fd(multi_cam_state->video0_fd, buf_desc[0].mem_handle); - munmap(pkt, size); - release_fd(multi_cam_state->video0_fd, cam_packet_handle); + mm.free(buf2); + mm.free(pkt); } void CameraState::enqueue_buffer(int i, bool dp) { int ret; int request_id = request_ids[i]; - if (buf_handle[i]) { - release(multi_cam_state->video0_fd, buf_handle[i]); + if (buf_handle[i] && sync_objs[i]) { // wait struct cam_sync_wait sync_wait = {0}; sync_wait.sync_obj = sync_objs[i]; sync_wait.timeout_ms = 50; // max dt tolerance, typical should be 23 - ret = do_cam_control(multi_cam_state->video1_fd, CAM_SYNC_WAIT, &sync_wait, sizeof(sync_wait)); + ret = do_cam_control(multi_cam_state->cam_sync_fd, CAM_SYNC_WAIT, &sync_wait, sizeof(sync_wait)); // LOGD("fence wait: %d %d", ret, sync_wait.sync_obj); buf.camera_bufs_metadata[i].timestamp_eof = (uint64_t)nanos_since_boot(); // set true eof @@ -535,13 +560,19 @@ void CameraState::enqueue_buffer(int i, bool dp) { // destroy old output fence struct cam_sync_info sync_destroy = {0}; - strcpy(sync_destroy.name, "NodeOutputPortFence"); sync_destroy.sync_obj = sync_objs[i]; - ret = do_cam_control(multi_cam_state->video1_fd, CAM_SYNC_DESTROY, &sync_destroy, sizeof(sync_destroy)); + ret = do_cam_control(multi_cam_state->cam_sync_fd, CAM_SYNC_DESTROY, &sync_destroy, sizeof(sync_destroy)); // LOGD("fence destroy: %d %d", ret, sync_destroy.sync_obj); } - // do stuff + // create output fence + struct cam_sync_info sync_create = {0}; + strcpy(sync_create.name, "NodeOutputPortFence"); + ret = do_cam_control(multi_cam_state->cam_sync_fd, CAM_SYNC_CREATE, &sync_create, sizeof(sync_create)); + // LOGD("fence req: %d %d", ret, sync_create.sync_obj); + sync_objs[i] = sync_create.sync_obj; + + // schedule request with camera request manager struct cam_req_mgr_sched_request req_mgr_sched_request = {0}; req_mgr_sched_request.session_hdl = session_handle; req_mgr_sched_request.link_hdl = link_handle; @@ -549,28 +580,11 @@ void CameraState::enqueue_buffer(int i, bool dp) { ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_SCHED_REQ, &req_mgr_sched_request, sizeof(req_mgr_sched_request)); // LOGD("sched req: %d %d", ret, request_id); - // create output fence - struct cam_sync_info sync_create = {0}; - strcpy(sync_create.name, "NodeOutputPortFence"); - ret = do_cam_control(multi_cam_state->video1_fd, CAM_SYNC_CREATE, &sync_create, sizeof(sync_create)); - // LOGD("fence req: %d %d", ret, sync_create.sync_obj); - sync_objs[i] = sync_create.sync_obj; - - // configure ISP to put the image in place - struct cam_mem_mgr_map_cmd mem_mgr_map_cmd = {0}; - mem_mgr_map_cmd.mmu_hdls[0] = multi_cam_state->device_iommu; - mem_mgr_map_cmd.num_hdl = 1; - mem_mgr_map_cmd.flags = CAM_MEM_FLAG_HW_READ_WRITE; - mem_mgr_map_cmd.fd = buf.camera_bufs[i].fd; - ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_MAP_BUF, &mem_mgr_map_cmd, sizeof(mem_mgr_map_cmd)); - // LOGD("map buf req: (fd: %d) 0x%x %d", bufs[i].fd, mem_mgr_map_cmd.out.buf_handle, ret); - buf_handle[i] = mem_mgr_map_cmd.out.buf_handle; - - // poke sensor + // poke sensor, must happen after schedule sensors_poke(request_id); // LOGD("Poked sensor"); - // push the buffer + // submit request to the ife config_isp(buf_handle[i], sync_objs[i], request_id, buf0_handle, 65632*(i+1)); } @@ -616,6 +630,9 @@ void CameraState::camera_open() { assert(sensor_fd >= 0); LOGD("opened sensor for %d", camera_num); + // init memorymanager for this camera + mm.init(multi_cam_state->video0_fd); + // probe the sensor LOGD("-- Probing sensor %d", camera_num); ret = sensors_init(); @@ -729,7 +746,7 @@ void CameraState::camera_open() { { uint32_t cam_packet_handle = 0; int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*1; - struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(multi_cam_state->video0_fd, size, &cam_packet_handle); + struct cam_packet *pkt = (struct cam_packet *)mm.alloc(size, &cam_packet_handle); pkt->num_cmd_buf = 1; pkt->kmd_cmd_buf_index = -1; pkt->header.size = size; @@ -738,7 +755,7 @@ void CameraState::camera_open() { buf_desc[0].size = buf_desc[0].length = sizeof(struct cam_csiphy_info); buf_desc[0].type = CAM_CMD_BUF_GENERIC; - struct cam_csiphy_info *csiphy_info = (struct cam_csiphy_info *)alloc_w_mmu_hdl(multi_cam_state->video0_fd, buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle); + struct cam_csiphy_info *csiphy_info = (struct cam_csiphy_info *)mm.alloc(buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle); csiphy_info->lane_mask = 0x1f; csiphy_info->lane_assign = 0x3210;// skip clk. How is this 16 bit for 5 channels?? csiphy_info->csiphy_3phase = 0x0; // no 3 phase, only 2 conductors per lane @@ -751,10 +768,8 @@ void CameraState::camera_open() { int ret_ = device_config(csiphy_fd, session_handle, csiphy_dev_handle, cam_packet_handle); assert(ret_ == 0); - munmap(csiphy_info, buf_desc[0].size); - release_fd(multi_cam_state->video0_fd, buf_desc[0].mem_handle); - munmap(pkt, size); - release_fd(multi_cam_state->video0_fd, cam_packet_handle); + mm.free(csiphy_info); + mm.free(pkt); } // link devices @@ -781,6 +796,18 @@ void CameraState::camera_open() { ret = device_control(multi_cam_state->isp_fd, CAM_START_DEV, session_handle, isp_dev_handle); LOGD("start isp: %d", ret); + for (int i = 0; i < FRAME_BUF_COUNT; i++) { + // configure ISP to put the image in place + struct cam_mem_mgr_map_cmd mem_mgr_map_cmd = {0}; + mem_mgr_map_cmd.mmu_hdls[0] = multi_cam_state->device_iommu; + mem_mgr_map_cmd.num_hdl = 1; + mem_mgr_map_cmd.flags = CAM_MEM_FLAG_HW_READ_WRITE; + mem_mgr_map_cmd.fd = buf.camera_bufs[i].fd; + ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_MAP_BUF, &mem_mgr_map_cmd, sizeof(mem_mgr_map_cmd)); + LOGD("map buf req: (fd: %d) 0x%x %d", buf.camera_bufs[i].fd, mem_mgr_map_cmd.out.buf_handle, ret); + buf_handle[i] = mem_mgr_map_cmd.out.buf_handle; + } + // TODO: this is unneeded, should we be doing the start i2c in a different way? //ret = device_control(sensor_fd, CAM_START_DEV, session_handle, sensor_dev_handle); //LOGD("start sensor: %d", ret); @@ -807,9 +834,9 @@ void cameras_open(MultiCameraState *s) { LOGD("opened video0"); // video1 is cam_sync, the target of some ioctls - s->video1_fd = HANDLE_EINTR(open("/dev/v4l/by-path/platform-cam_sync-video-index0", O_RDWR | O_NONBLOCK)); - assert(s->video1_fd >= 0); - LOGD("opened video1"); + s->cam_sync_fd = HANDLE_EINTR(open("/dev/v4l/by-path/platform-cam_sync-video-index0", O_RDWR | O_NONBLOCK)); + assert(s->cam_sync_fd >= 0); + LOGD("opened video1 (cam_sync)"); // looks like there's only one of these s->isp_fd = open_v4l_by_name_and_index("cam-isp"); @@ -834,7 +861,7 @@ void cameras_open(MultiCameraState *s) { LOG("-- Subscribing"); static struct v4l2_event_subscription sub = {0}; sub.type = V4L_EVENT_CAM_REQ_MGR_EVENT; - sub.id = 2; // should use boot time for sof + sub.id = V4L_EVENT_CAM_REQ_MGR_SOF_BOOT_TS; ret = HANDLE_EINTR(ioctl(s->video0_fd, VIDIOC_SUBSCRIBE_EVENT, &sub)); printf("req mgr subscribe: %d\n", ret); @@ -883,6 +910,11 @@ void CameraState::camera_close() { LOGD("release isp: %d", ret); ret = device_control(csiphy_fd, CAM_RELEASE_DEV, session_handle, csiphy_dev_handle); LOGD("release csiphy: %d", ret); + + for (int i = 0; i < FRAME_BUF_COUNT; i++) { + release(multi_cam_state->video0_fd, buf_handle[i]); + } + LOGD("released buffers"); } ret = device_control(sensor_fd, CAM_RELEASE_DEV, session_handle, sensor_dev_handle); diff --git a/selfdrive/camerad/cameras/camera_qcom2.h b/selfdrive/camerad/cameras/camera_qcom2.h index 199aa40e7..a7c64c066 100644 --- a/selfdrive/camerad/cameras/camera_qcom2.h +++ b/selfdrive/camerad/cameras/camera_qcom2.h @@ -9,6 +9,20 @@ #define FRAME_BUF_COUNT 4 +class MemoryManager { + public: + void init(int _video0_fd) { video0_fd = _video0_fd; } + void *alloc(int len, uint32_t *handle); + void free(void *ptr); + ~MemoryManager(); + private: + std::mutex lock; + std::map handle_lookup; + std::map size_lookup; + std::map > cached_allocations; + int video0_fd; +}; + class CameraState { public: MultiCameraState *multi_cam_state; @@ -60,6 +74,7 @@ public: int camera_id; CameraBuf buf; + MemoryManager mm; private: void config_isp(int io_mem_handle, int fence, int request_id, int buf0_mem_handle, int buf0_offset); void enqueue_req_multi(int start, int n, bool dp); @@ -73,7 +88,7 @@ private: typedef struct MultiCameraState { unique_fd video0_fd; - unique_fd video1_fd; + unique_fd cam_sync_fd; unique_fd isp_fd; int device_iommu; int cdm_iommu; diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index c8c1a3437..b845ee0e0 100755 --- a/selfdrive/test/test_onroad.py +++ b/selfdrive/test/test_onroad.py @@ -23,7 +23,7 @@ from tools.lib.logreader import LogReader PROCS = { "selfdrive.controls.controlsd": 31.0, "./loggerd": 70.0, - "./camerad": 41.0, + "./camerad": 26.0, "./locationd": 9.1, "selfdrive.controls.plannerd": 11.7, "./_ui": 18.4, From 2fa5a59e3faf30c2d4bb45056f20ec2408dcb3f4 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Thu, 28 Apr 2022 17:43:26 -0700 Subject: [PATCH 06/21] Fix can replay Jungle warning (#24364) --- tools/replay/can_replay.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tools/replay/can_replay.py b/tools/replay/can_replay.py index 4b0ad21db..b212abc43 100755 --- a/tools/replay/can_replay.py +++ b/tools/replay/can_replay.py @@ -86,7 +86,7 @@ def connect(): if __name__ == "__main__": - if panda_jungle_imported: + if not panda_jungle_imported: print("\33[31m", "WARNING: cannot connect to jungles. Clone the jungle library to enable support:", "\033[0m") print("\033[34m", f"cd {BASEDIR} && git clone https://github.com/commaai/panda_jungle", "\033[0m") From 45f131e0e7d1df168295f180195a3b06aba87736 Mon Sep 17 00:00:00 2001 From: Jason Young <46612682+jyoung8607@users.noreply.github.com> Date: Thu, 28 Apr 2022 20:46:58 -0400 Subject: [PATCH 07/21] Accept resumeCruise for resuming prior speed setpoint (#24348) * accept two button types for resume * retry CI * Update selfdrive/controls/lib/drive_helpers.py Co-authored-by: Shane Smiskol --- selfdrive/controls/lib/drive_helpers.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index ca5c1d332..a9725a475 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -79,7 +79,7 @@ def update_v_cruise(v_cruise_kph, buttonEvents, button_timers, enabled, metric): def initialize_v_cruise(v_ego, buttonEvents, v_cruise_last): for b in buttonEvents: # 250kph or above probably means we never had a set speed - if b.type == car.CarState.ButtonEvent.Type.accelCruise and v_cruise_last < 250: + if b.type in (car.CarState.ButtonEvent.Type.accelCruise, car.CarState.ButtonEvent.Type.resumeCruise) and v_cruise_last < 250: return v_cruise_last return int(round(clip(v_ego * CV.MS_TO_KPH, V_CRUISE_ENABLE_MIN, V_CRUISE_MAX))) From 66dd8f934cc84cdbd3e48e7b49836ec95f82c23a Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Thu, 28 Apr 2022 17:56:34 -0700 Subject: [PATCH 08/21] safety param: make uint16_t (#24362) * only use half of the available bytes * move to set_safety_model * regen and update refs * last two * bump to master --- panda | 2 +- selfdrive/boardd/panda.cc | 4 +++- selfdrive/test/process_replay/ref_commit | 2 +- selfdrive/test/process_replay/test_processes.py | 2 +- 4 files changed, 6 insertions(+), 4 deletions(-) diff --git a/panda b/panda index d031d6e28..cf8fb0b88 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit d031d6e283e2887cb3360c068a3925f9b58b2b12 +Subproject commit cf8fb0b8834f4e63eea27fc92289742d75d1d37d diff --git a/selfdrive/boardd/panda.cc b/selfdrive/boardd/panda.cc index 394437902..1c287fc57 100644 --- a/selfdrive/boardd/panda.cc +++ b/selfdrive/boardd/panda.cc @@ -248,7 +248,9 @@ int Panda::usb_bulk_read(unsigned char endpoint, unsigned char* data, int length } void Panda::set_safety_model(cereal::CarParams::SafetyModel safety_model, uint32_t safety_param) { - usb_write(0xdc, (uint16_t)safety_model, safety_param); + // FIXME: last two bytes must be empty + assert((safety_param >> 16) == 0U); + usb_write(0xdc, (uint16_t)safety_model, (uint16_t)safety_param); } void Panda::set_alternative_experience(uint16_t alternative_experience) { diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index f0a493c3c..cb18cd006 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -8d369bed132b691e1c000a726ab253ce7cbf8e09 \ No newline at end of file +d3c925db251cda03d6661987ff6cec9df256a575 \ No newline at end of file diff --git a/selfdrive/test/process_replay/test_processes.py b/selfdrive/test/process_replay/test_processes.py index 4682bf509..65d72f16f 100755 --- a/selfdrive/test/process_replay/test_processes.py +++ b/selfdrive/test/process_replay/test_processes.py @@ -34,7 +34,7 @@ segments = [ ("BODY", "bd6a637565e91581|2022-04-04--22-05-08--0"), ("HYUNDAI", "fakedata|2022-01-20--17-49-04--0"), ("TOYOTA", "fakedata|2022-04-13--18-53-16--0"), - ("TOYOTA2", "fakedata|2022-04-26--22-58-12--0"), + ("TOYOTA2", "fakedata|2022-04-28--15-52-38--0"), ("TOYOTA3", "fakedata|2022-04-13--19-09-53--0"), ("HONDA", "fakedata|2022-01-20--17-56-40--0"), ("HONDA2", "fakedata|2022-04-13--19-23-30--0"), From 3abbe827deac4954a2af97a191171c8b7d08d0c1 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Thu, 28 Apr 2022 19:51:42 -0700 Subject: [PATCH 09/21] controlsd: add lagging alert (#24360) * controlsd: add lagging alert * move those * just this for now * no camera malfunction --- cereal | 2 +- common/realtime.py | 13 +++++++++++++ selfdrive/controls/controlsd.py | 15 +++++++++------ selfdrive/controls/lib/events.py | 5 +++++ 4 files changed, 28 insertions(+), 7 deletions(-) diff --git a/cereal b/cereal index 29f4fe89e..a057aed16 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 29f4fe89ef710ff86a5aeb998a357187d0619fb8 +Subproject commit a057aed16747d0e414145d83d4861c50315781ad diff --git a/common/realtime.py b/common/realtime.py index 4a37efada..6ef27ed3a 100644 --- a/common/realtime.py +++ b/common/realtime.py @@ -2,6 +2,7 @@ import gc import os import time +from collections import deque from typing import Optional, List, Union from setproctitle import getproctitle # pylint: disable=no-name-in-module @@ -59,6 +60,8 @@ class Ratekeeper: self._frame = 0 self._remaining = 0.0 self._process_name = getproctitle() + self._dts = deque([self._interval], maxlen=100) + self._last_monitor_time = sec_since_boot() @property def frame(self) -> int: @@ -68,6 +71,12 @@ class Ratekeeper: def remaining(self) -> float: return self._remaining + @property + def lagging(self) -> bool: + avg_dt = sum(self._dts) / len(self._dts) + expected_dt = self._interval * (1 / 0.9) + return avg_dt > expected_dt + # Maintain loop rate by calling this at the end of each loop def keep_time(self) -> bool: lagged = self.monitor_time() @@ -77,6 +86,10 @@ class Ratekeeper: # this only monitor the cumulative lag, but does not enforce a rate def monitor_time(self) -> bool: + prev = self._last_monitor_time + self._last_monitor_time = sec_since_boot() + self._dts.append(self._last_monitor_time - prev) + lagged = False remaining = self._next_frame_time - sec_since_boot() self._next_frame_time += self._interval diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index ffc52cb3e..958de4599 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -285,9 +285,11 @@ class Controls: self.events.add(EventName.relayMalfunction) # Check for HW or system issues - if len(self.sm['radarState'].radarErrors): + if self.rk.lagging: + self.events.add(EventName.controlsdLagging) + elif len(self.sm['radarState'].radarErrors): self.events.add(EventName.radarFault) - elif not self.sm.valid["pandaStates"]: + elif not self.sm.valid['pandaStates']: self.events.add(EventName.usbError) elif not self.sm.all_checks() or self.can_rcv_error: @@ -352,10 +354,11 @@ class Controls: # Not show in first 1 km to allow for driving out of garage. This event shows after 5 minutes self.events.add(EventName.noGps) - if not self.sm.all_alive(self.camera_packets): - self.events.add(EventName.cameraMalfunction) - elif not self.sm.all_freq_ok(self.camera_packets): - self.events.add(EventName.cameraFrameRate) + if not self.rk.lagging: + if not self.sm.all_alive(self.camera_packets): + self.events.add(EventName.cameraMalfunction) + elif not self.sm.all_freq_ok(self.camera_packets): + self.events.add(EventName.cameraFrameRate) if self.sm['modelV2'].frameDropPerc > 20: self.events.add(EventName.modeldLagging) diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index 19620cbf4..12a42a7e3 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -695,6 +695,11 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { ET.NO_ENTRY: NoEntryAlert("Low Communication Rate between Processes"), }, + EventName.controlsdLagging: { + ET.SOFT_DISABLE: soft_disable_alert("Controls Lagging"), + ET.NO_ENTRY: NoEntryAlert("Controls Process Lagging: Reboot Your Device"), + }, + # Thrown when manager detects a service exited unexpectedly while driving EventName.processNotRunning: { ET.NO_ENTRY: NoEntryAlert("System Malfunction: Reboot Your Device"), From ef7ece408a932ae724cadd108e523d0e4a7cef94 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Thu, 28 Apr 2022 22:02:32 -0700 Subject: [PATCH 10/21] jenkins: reduce timeout to 20 minutes --- Jenkinsfile | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Jenkinsfile b/Jenkinsfile index aa73ccfc4..c5edda56f 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -30,7 +30,7 @@ END""" def phone_steps(String device_type, steps) { lock(resource: "", label: device_type, inversePrecedence: true, variable: 'device_ip', quantity: 1) { - timeout(time: 60, unit: 'MINUTES') { + timeout(time: 20, unit: 'MINUTES') { phone(device_ip, "git checkout", readFile("selfdrive/test/setup_device_ci.sh"),) steps.each { item -> phone(device_ip, item[0], item[1]) @@ -46,7 +46,7 @@ pipeline { SOURCE_DIR = "/data/openpilot_source/" } options { - timeout(time: 4, unit: 'HOURS') + timeout(time: 4, unit: 'HOURS') } stages { From c1caca104f8553d485334e33446bd7b8ec1fb8ca Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Thu, 28 Apr 2022 22:37:46 -0700 Subject: [PATCH 11/21] Support RAV4 Hybrid 2022 with stock longitudinal (#23969) * add panda flag for toyota stock long with camera * clean up * Add 2022 RAV4 Hybrid from Philly * fix wrong fw in interface, did this ever work? * Must be a hybrid * no radar parsing * fix can error * move to own platform * generate docs * fix * Add 2022 Rav4 XSE Australia fingerprint parameters (#24303) * Update values.py Add 2022 Rav4 XSE Australia * add commas Co-authored-by: Shane Smiskol * bump panda * wait, the camera doesn't even send 0x343, right? * use a set instead, more obvious * don't test without a parser * bump panda * flip panda flag * bump panda * add commas * regen and update refs * set to none by default * revert parenthesis * update comment * bump panda * regen and update refs * add test models and update readme * bump to master Co-authored-by: BrettLynch123 <34538435+BrettLynch123@users.noreply.github.com> --- RELEASES.md | 1 + docs/CARS.md | 1 + panda | 2 +- selfdrive/car/interfaces.py | 1 + selfdrive/car/tests/routes.py | 1 + selfdrive/car/tests/test_car_interfaces.py | 3 +- selfdrive/car/toyota/carstate.py | 23 ++++++++++---- selfdrive/car/toyota/interface.py | 11 ++++--- selfdrive/car/toyota/radar_interface.py | 5 +++- selfdrive/car/toyota/values.py | 30 +++++++++++++++++-- selfdrive/test/process_replay/ref_commit | 2 +- .../test/process_replay/test_processes.py | 2 +- 12 files changed, 65 insertions(+), 17 deletions(-) diff --git a/RELEASES.md b/RELEASES.md index c8f2d73c6..75088c19a 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -18,6 +18,7 @@ Version 0.8.14 (2022-0X-XX) * Hyundai Tucson Diesel 2019 support thanks to sunnyhaibin! * Toyota Alphard Hybrid 2021 support * Toyota Avalon Hybrid 2022 support + * Toyota RAV4 Hybrid 2022 support Version 0.8.13 (2022-02-18) ======================== diff --git a/docs/CARS.md b/docs/CARS.md index 5bdd13874..436728650 100644 --- a/docs/CARS.md +++ b/docs/CARS.md @@ -130,6 +130,7 @@ How We Rate The Cars |Toyota|Highlander 2017-19|All|[3](#footnotes)||||| |Toyota|Highlander Hybrid 2017-19|All|[3](#footnotes)||||| |Toyota|RAV4 Hybrid 2016-18|TSS-P|[3](#footnotes)||||| +|Toyota|RAV4 Hybrid 2022|All|||||| |Toyota|Sienna 2018-20|All|[3](#footnotes)||||| |Volkswagen|Arteon 2018, 2021[8](#footnotes)|Driver Assistance|||||| |Volkswagen|Atlas 2018-19, 2022[8](#footnotes)|Driver Assistance|||||| diff --git a/panda b/panda index cf8fb0b88..326cc2a8d 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit cf8fb0b8834f4e63eea27fc92289742d75d1d37d +Subproject commit 326cc2a8dbabbfe6c442f4b0192b18178a83b6a6 diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index ac15b2e1f..23822fe45 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -187,6 +187,7 @@ class CarInterfaceBase(ABC): class RadarInterfaceBase(ABC): def __init__(self, CP): + self.rcp = None self.pts = {} self.delay = 0 self.radar_ts = CP.radarTimeStep diff --git a/selfdrive/car/tests/routes.py b/selfdrive/car/tests/routes.py index a26d6dcd0..88309e571 100644 --- a/selfdrive/car/tests/routes.py +++ b/selfdrive/car/tests/routes.py @@ -128,6 +128,7 @@ routes = [ TestRoute("32a7df20486b0f70|2020-02-06--16-06-50", TOYOTA.RAV4H), TestRoute("cdf2f7de565d40ae|2019-04-25--03-53-41", TOYOTA.RAV4_TSS2), TestRoute("7e34a988419b5307|2019-12-18--19-13-30", TOYOTA.RAV4H_TSS2), + TestRoute("2475fb3eb2ffcc2e|2022-04-29--12-46-23", TOYOTA.RAV4H_TSS2_2022), TestRoute("e6a24be49a6cd46e|2019-10-29--10-52-42", TOYOTA.LEXUS_ES_TSS2), TestRoute("25057fa6a5a63dfb|2020-03-04--08-44-23", TOYOTA.LEXUS_CTH), TestRoute("f49e8041283f2939|2019-05-30--11-51-51", TOYOTA.LEXUS_ESH_TSS2), diff --git a/selfdrive/car/tests/test_car_interfaces.py b/selfdrive/car/tests/test_car_interfaces.py index d97b16e68..92024ab0c 100755 --- a/selfdrive/car/tests/test_car_interfaces.py +++ b/selfdrive/car/tests/test_car_interfaces.py @@ -64,7 +64,8 @@ class TestCarInterfaces(unittest.TestCase): # Run radar interface once radar_interface.update([]) - if not car_params.radarOffCan and hasattr(radar_interface, '_update') and hasattr(radar_interface, 'trigger_msg'): + if not car_params.radarOffCan and radar_interface.rcp is not None and \ + hasattr(radar_interface, '_update') and hasattr(radar_interface, 'trigger_msg'): radar_interface._update([radar_interface.trigger_msg]) if __name__ == "__main__": diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 614e1a691..54922ac2d 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -6,7 +6,7 @@ from common.realtime import DT_CTRL from opendbc.can.can_define import CANDefine from opendbc.can.parser import CANParser from selfdrive.car.interfaces import CarStateBase -from selfdrive.car.toyota.values import ToyotaFlags, CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, TSS2_CAR, EPS_SCALE +from selfdrive.car.toyota.values import ToyotaFlags, CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE class CarState(CarStateBase): @@ -91,8 +91,12 @@ class CarState(CarStateBase): ret.cruiseState.available = cp.vl["PCM_CRUISE_2"]["MAIN_ON"] != 0 ret.cruiseState.speed = cp.vl["PCM_CRUISE_2"]["SET_SPEED"] * CV.KPH_TO_MS - if self.CP.carFingerprint in TSS2_CAR: + if self.CP.carFingerprint in RADAR_ACC_CAR: + self.acc_type = cp.vl["ACC_CONTROL"]["ACC_TYPE"] + ret.stockFcw = bool(cp.vl["ACC_HUD"]["FCW"]) + elif self.CP.carFingerprint in TSS2_CAR: self.acc_type = cp_cam.vl["ACC_CONTROL"]["ACC_TYPE"] + ret.stockFcw = bool(cp_cam.vl["ACC_HUD"]["FCW"]) # some TSS2 cars have low speed lockout permanently set, so ignore on those cars # these cars are identified by an ACC_TYPE value of 2. @@ -115,9 +119,6 @@ class CarState(CarStateBase): ret.genericToggle = bool(cp.vl["LIGHT_STALK"]["AUTO_HIGH_BEAM"]) ret.stockAeb = bool(cp_cam.vl["PRE_COLLISION"]["PRECOLLISION_ACTIVE"] and cp_cam.vl["PRE_COLLISION"]["FORCE"] < -1e-5) - if self.CP.carFingerprint in TSS2_CAR: - ret.stockFcw = bool(cp_cam.vl["ACC_HUD"]["FCW"]) - ret.espDisabled = cp.vl["ESP_CONTROL"]["TC_DISABLED"] != 0 # 2 is standby, 10 is active. TODO: check that everything else is really a faulty state self.steer_state = cp.vl["EPS_STATUS"]["LKA_STATE"] @@ -207,6 +208,16 @@ class CarState(CarStateBase): ] checks.append(("BSM", 1)) + if CP.carFingerprint in RADAR_ACC_CAR: + signals += [ + ("ACC_TYPE", "ACC_CONTROL"), + ("FCW", "ACC_HUD"), + ] + checks += [ + ("ACC_CONTROL", 33), + ("ACC_HUD", 1), + ] + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0) @staticmethod @@ -222,7 +233,7 @@ class CarState(CarStateBase): ("PRE_COLLISION", 0), # TODO: figure out why freq is inconsistent ] - if CP.carFingerprint in TSS2_CAR: + if CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR): signals += [ ("ACC_TYPE", "ACC_CONTROL"), ("FCW", "ACC_HUD"), diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index b3d0aefa8..872a27c64 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -3,7 +3,7 @@ from cereal import car from common.conversions import Conversions as CV from panda import Panda from selfdrive.car.toyota.tunes import LatTunes, LongTunes, set_long_tune, set_lat_tune -from selfdrive.car.toyota.values import Ecu, CAR, ToyotaFlags, TSS2_CAR, NO_DSU_CAR, MIN_ACC_SPEED, EPS_SCALE, EV_HYBRID_CAR, CarControllerParams +from selfdrive.car.toyota.values import Ecu, CAR, ToyotaFlags, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, MIN_ACC_SPEED, EPS_SCALE, EV_HYBRID_CAR, CarControllerParams from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config from selfdrive.car.interfaces import CarInterfaceBase @@ -115,7 +115,7 @@ class CarInterface(CarInterfaceBase): ret.mass = 3505. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid set_lat_tune(ret.lateralTuning, LatTunes.PID_H) - elif candidate in (CAR.RAV4_TSS2, CAR.RAV4H_TSS2): + elif candidate in (CAR.RAV4_TSS2, CAR.RAV4H_TSS2, CAR.RAV4H_TSS2_2022): stop_and_go = True ret.wheelbase = 2.68986 ret.steerRatio = 14.3 @@ -123,7 +123,7 @@ class CarInterface(CarInterfaceBase): ret.mass = 3585. * CV.LB_TO_KG + STD_CARGO_KG # Average between ICE and Hybrid set_lat_tune(ret.lateralTuning, LatTunes.PID_D) - # 2019+ Rav4 TSS2 uses two different steering racks and specific tuning seems to be necessary. + # 2019+ RAV4 TSS2 uses two different steering racks and specific tuning seems to be necessary. # See https://github.com/commaai/openpilot/pull/21429#issuecomment-873652891 for fw in car_fw: if fw.ecu == "eps" and (fw.fwVersion.startswith(b'\x02') or fw.fwVersion in [b'8965B42181\x00\x00\x00\x00\x00\x00']): @@ -221,7 +221,10 @@ class CarInterface(CarInterfaceBase): ret.enableDsu = (len(found_ecus) > 0) and (Ecu.dsu not in found_ecus) and (candidate not in NO_DSU_CAR) and (not smartDsu) ret.enableGasInterceptor = 0x201 in fingerprint[0] # if the smartDSU is detected, openpilot can send ACC_CMD (and the smartDSU will block it from the DSU) or not (the DSU is "connected") - ret.openpilotLongitudinalControl = smartDsu or ret.enableDsu or candidate in TSS2_CAR + ret.openpilotLongitudinalControl = smartDsu or ret.enableDsu or candidate in (TSS2_CAR - RADAR_ACC_CAR) + + if not ret.openpilotLongitudinalControl: + ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_STOCK_LONGITUDINAL # we can't use the fingerprint to detect this reliably, since # the EV gas pedal signal can take a couple seconds to appear diff --git a/selfdrive/car/toyota/radar_interface.py b/selfdrive/car/toyota/radar_interface.py index 590840851..8c87704ff 100755 --- a/selfdrive/car/toyota/radar_interface.py +++ b/selfdrive/car/toyota/radar_interface.py @@ -6,6 +6,9 @@ from selfdrive.car.interfaces import RadarInterfaceBase def _create_radar_can_parser(car_fingerprint): + if DBC[car_fingerprint]['radar'] is None: + return None + if car_fingerprint in TSS2_CAR: RADAR_A_MSGS = list(range(0x180, 0x190)) RADAR_B_MSGS = list(range(0x190, 0x1a0)) @@ -48,7 +51,7 @@ class RadarInterface(RadarInterfaceBase): self.no_radar = CP.carFingerprint in NO_DSU_CAR and CP.carFingerprint not in TSS2_CAR def update(self, can_strings): - if self.no_radar: + if self.no_radar or self.rcp is None: return super().update(None) vls = self.rcp.update_strings(can_strings) diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 0e74be245..68e8b36f3 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -63,6 +63,7 @@ class CAR: RAV4H = "TOYOTA RAV4 HYBRID 2017" RAV4_TSS2 = "TOYOTA RAV4 2019" RAV4H_TSS2 = "TOYOTA RAV4 HYBRID 2019" + RAV4H_TSS2_2022 = "TOYOTA RAV4 HYBRID 2022" MIRAI = "TOYOTA MIRAI 2021" # TSS 2.5 SIENNA = "TOYOTA SIENNA 2018" @@ -142,6 +143,7 @@ CAR_INFO: Dict[str, Union[ToyotaCarInfo, List[ToyotaCarInfo]]] = { CAR.RAV4H: ToyotaCarInfo("Toyota RAV4 Hybrid 2016-18", "TSS-P", footnotes=[Footnote.DSU]), CAR.RAV4_TSS2: ToyotaCarInfo("Toyota RAV4 2019-21", video_link="https://www.youtube.com/watch?v=wJxjDd42gGA"), CAR.RAV4H_TSS2: ToyotaCarInfo("Toyota RAV4 Hybrid 2019-21"), + CAR.RAV4H_TSS2_2022: ToyotaCarInfo("Toyota RAV4 Hybrid 2022"), CAR.MIRAI: ToyotaCarInfo("Toyota Mirai 2021"), CAR.SIENNA: ToyotaCarInfo("Toyota Sienna 2018-20", video_link="https://www.youtube.com/watch?v=q1UPOo4Sh68", footnotes=[Footnote.DSU]), @@ -1330,6 +1332,26 @@ FW_VERSIONS = { b'\x028646F4203800\x00\x00\x00\x008646G2601500\x00\x00\x00\x00', ], }, + CAR.RAV4H_TSS2_2022: { + (Ecu.esp, 0x7b0, None): [ + b'\x01F15264283100\x00\x00\x00\x00', + b'\x01F15264286200\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7a1, None): [ + b'\x028965B0R01500\x00\x00\x00\x008965B0R02500\x00\x00\x00\x00', + b'8965B42182\x00\x00\x00\x00\x00\x00', + ], + (Ecu.engine, 0x700, None): [ + b'\x01896634A62000\x00\x00\x00\x00', + b'\x01896634A08000\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'\x018821F0R01100\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'\x028646F0R02100\x00\x00\x00\x008646G0R01100\x00\x00\x00\x00', + ], + }, CAR.SIENNA: { (Ecu.engine, 0x700, None): [ b'\x01896630832100\x00\x00\x00\x00', @@ -1840,6 +1862,7 @@ DBC = { CAR.LEXUS_IS: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), CAR.LEXUS_CTH: dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'), CAR.RAV4H_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), + CAR.RAV4H_TSS2_2022: dbc_dict('toyota_nodsu_pt_generated', None), CAR.LEXUS_NXH: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), CAR.LEXUS_NX: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), CAR.LEXUS_NX_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), @@ -1853,14 +1876,17 @@ DBC = { EPS_SCALE = defaultdict(lambda: 73, {CAR.PRIUS: 66, CAR.COROLLA: 88, CAR.LEXUS_IS: 77, CAR.LEXUS_RC: 77, CAR.LEXUS_CTH: 100, CAR.PRIUS_V: 100}) # Toyota/Lexus Safety Sense 2.0 and 2.5 -TSS2_CAR = {CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2, +TSS2_CAR = {CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2, CAR.RAV4H_TSS2_2022, CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2, CAR.PRIUS_TSS2, CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2, CAR.MIRAI, CAR.LEXUS_NX_TSS2, CAR.ALPHARD_TSS2, CAR.AVALON_TSS2, CAR.AVALONH_TSS2, CAR.ALPHARDH_TSS2} NO_DSU_CAR = TSS2_CAR | {CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH} +# these cars have a radar which sends ACC messages instead of the camera +RADAR_ACC_CAR = {CAR.RAV4H_TSS2_2022} + EV_HYBRID_CAR = {CAR.AVALONH_2019, CAR.AVALONH_TSS2, CAR.CAMRYH, CAR.CAMRYH_TSS2, CAR.CHRH, CAR.COROLLAH_TSS2, CAR.HIGHLANDERH, CAR.HIGHLANDERH_TSS2, CAR.PRIUS, - CAR.PRIUS_V, CAR.RAV4H, CAR.RAV4H_TSS2, CAR.LEXUS_CTH, CAR.MIRAI, CAR.LEXUS_ESH, CAR.LEXUS_ESH_TSS2, CAR.LEXUS_NXH, CAR.LEXUS_RXH, + CAR.PRIUS_V, CAR.RAV4H, CAR.RAV4H_TSS2, CAR.RAV4H_TSS2_2022, CAR.LEXUS_CTH, CAR.MIRAI, CAR.LEXUS_ESH, CAR.LEXUS_ESH_TSS2, CAR.LEXUS_NXH, CAR.LEXUS_RXH, CAR.LEXUS_RXH_TSS2, CAR.PRIUS_TSS2, CAR.ALPHARDH_TSS2} # no resume button press required diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index cb18cd006..39c8add0e 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -d3c925db251cda03d6661987ff6cec9df256a575 \ No newline at end of file +e37c2ea1447363af1403e8260450c30e2d862101 \ No newline at end of file diff --git a/selfdrive/test/process_replay/test_processes.py b/selfdrive/test/process_replay/test_processes.py index 65d72f16f..c59fe197a 100755 --- a/selfdrive/test/process_replay/test_processes.py +++ b/selfdrive/test/process_replay/test_processes.py @@ -33,7 +33,7 @@ original_segments = [ segments = [ ("BODY", "bd6a637565e91581|2022-04-04--22-05-08--0"), ("HYUNDAI", "fakedata|2022-01-20--17-49-04--0"), - ("TOYOTA", "fakedata|2022-04-13--18-53-16--0"), + ("TOYOTA", "fakedata|2022-04-28--18-59-34--0"), ("TOYOTA2", "fakedata|2022-04-28--15-52-38--0"), ("TOYOTA3", "fakedata|2022-04-13--19-09-53--0"), ("HONDA", "fakedata|2022-01-20--17-56-40--0"), From 7b7fc92aba522a6bdb7684bc32b3b663372f20a5 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Thu, 28 Apr 2022 23:08:49 -0700 Subject: [PATCH 12/21] Add no disengage on accelerator to release notes (#24365) * add ndog to releases * aligns with latActive * note to split it up and describe what it does * Update RELEASES.md Co-authored-by: Adeeb Shihadeh Co-authored-by: Adeeb Shihadeh --- RELEASES.md | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/RELEASES.md b/RELEASES.md index 75088c19a..6a72f8989 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -10,8 +10,9 @@ Version 0.8.14 (2022-0X-XX) * New lateral controller based on physical wheel torque model * Much smoother control, consistent across the speed range * Effective feedforward that uses road roll - * Simplified tuning, all car specific parameters can be derived from data - * Initially used on TSS2 Corolla and TSS-P Rav4 + * Simplified tuning, all car-specific parameters can be derived from data + * Initially used on TSS2 Corolla and TSS-P RAV4 + * Added toggle to disable disengaging on the accelerator pedal * comma body support * Audi RS3 support thanks to jyoung8607! * Hyundai Ioniq Plug-in Hybrid 2019 support thanks to sunnyhaibin! From ee433dfa57a60364423a222a7f18dc137b77f532 Mon Sep 17 00:00:00 2001 From: Gijs Koning Date: Fri, 29 Apr 2022 07:15:05 -0700 Subject: [PATCH 13/21] Simulator: Fix CI and set low_quality default (#24354) * Change low_quality argument and fix closing carla bridge * Some fixes * Change carla process in test * Change fov to 120, higher doesn't look good * Update readme and remove redundant test * update * Add folder description --- tools/sim/README.md | 60 +++++++++------ tools/sim/bridge.py | 94 +++++++++++++----------- tools/sim/lib/keyboard_ctrl.py | 2 +- tools/sim/start_carla.sh | 2 +- tools/sim/test/test_carla_integration.py | 28 +++---- 5 files changed, 101 insertions(+), 85 deletions(-) diff --git a/tools/sim/README.md b/tools/sim/README.md index 3cc124cdb..c77bd718a 100644 --- a/tools/sim/README.md +++ b/tools/sim/README.md @@ -1,44 +1,56 @@ openpilot in simulator ===================== -openpilot implements a [bridge](bridge.py) that allows it to run in the [CARLA simulator](https://carla.org/). +openpilot implements a [bridge](bridge.py) that allows it to run in the [CARLA simulator](https://carla.org/). ## System Requirements -openpilot doesn't have any extreme hardware requirements, however CARLA requires an NVIDIA graphics card and is very resource-intensive and may not run smoothly on your system. For this case, we have a low quality mode you can activate by running: -``` -./start_openpilot_docker.sh --low_quality -``` +openpilot doesn't have any extreme hardware requirements, however CARLA requires an NVIDIA graphics card and is very resource-intensive and may not run smoothly on your system. +For this case, we have a the simulator in low quality by default. You can also check out the [CARLA python documentation](https://carla.readthedocs.io/en/latest/python_api/) to find more parameters to tune that might increase performance on your system. ## Running the simulator - -First, start the CARLA server in one terminal. -``` +Start Carla simulator, openpilot and bridge processes located in tools/sim: +``` bash +# Terminal 1 ./start_carla.sh + +# Terminal 2 - Run openpilot and bridge in one Docker: +./start_openpilot_docker.sh + +# Running the latest local code execute + # Terminal 2: + ./launch_openpilot.sh + # Terminal 3 + ./bridge.py ``` -Then, start the bridge and openpilot in another terminal. +### Bridge usage +_Same commands hold for start_openpilot_docker_ ``` -./start_openpilot_docker.sh +$ ./bridge.py -h +Usage: bridge.py [options] +Bridge between CARLA and openpilot. + +Options: + -h, --help show this help message and exit + --joystick Use joystick input to control the car + --high_quality Set simulator to higher quality (requires good GPU) + --town TOWN Select map to drive in + --spawn_point NUM Number of the spawn point to start in ``` To engage openpilot press 1 a few times while focused on bridge.py to increase the cruise speed. - -## Controls - -You can control openpilot driving in the simulation with the following keys - -| key | functionality | -| :---: | :---------------: | -| 1 | Cruise up 5 mph | -| 2 | Cruise down 5 mph | -| 3 | Cruise cancel | -| q | Exit all | - -To see the options for changing the environment, such as the town, spawn point or precipitation, you can run `./start_openpilot_docker.sh --help`. -This will print the help output inside the docker container. You need to exit the docker container before running `./start_openpilot_docker.sh` again. +All inputs: + +| key | functionality | +|:----:|:-----------------:| +| 1 | Cruise up 5 mph | +| 2 | Cruise down 5 mph | +| 3 | Cruise cancel | +| q | Exit all | +| wasd | Control manually | ## Further Reading diff --git a/tools/sim/bridge.py b/tools/sim/bridge.py index 0b07c1b0e..368e18fd9 100755 --- a/tools/sim/bridge.py +++ b/tools/sim/bridge.py @@ -32,11 +32,10 @@ STEER_RATIO = 15. pm = messaging.PubMaster(['roadCameraState', 'wideRoadCameraState', 'sensorEvents', 'can', "gpsLocationExternal"]) sm = messaging.SubMaster(['carControl', 'controlsState']) - def parse_args(add_args=None): parser = argparse.ArgumentParser(description='Bridge between CARLA and openpilot.') parser.add_argument('--joystick', action='store_true') - parser.add_argument('--low_quality', action='store_true') + parser.add_argument('--high_quality', action='store_true') parser.add_argument('--town', type=str, default='Town04_Opt') parser.add_argument('--spawn_point', dest='num_selected_spawn_point', type=int, default=16) @@ -86,10 +85,9 @@ class Camerad: # TODO: move rgb_to_yuv.cl to local dir once the frame stream camera is removed kernel_fn = os.path.join(BASEDIR, "selfdrive", "camerad", "transforms", "rgb_to_yuv.cl") - self._kernel_file = open(kernel_fn) - - prg = cl.Program(self.ctx, self._kernel_file.read()).build(cl_arg) - self.krnl = prg.rgb_to_yuv + with open(kernel_fn) as f: + prg = cl.Program(self.ctx, f.read()).build(cl_arg) + self.krnl = prg.rgb_to_yuv self.Wdiv4 = W // 4 if (W % 4 == 0) else (W + (4 - W % 4)) // 4 self.Hdiv4 = H // 4 if (H % 4 == 0) else (H + (4 - H % 4)) // 4 @@ -130,10 +128,6 @@ class Camerad: setattr(dat, pub_type, msg) pm.send(pub_type, dat) - def close(self): - self._kernel_file.close() - - def imu_callback(imu, vehicle_state): vehicle_state.bearing_deg = math.degrees(imu.compass) dat = messaging.new_message('sensorEvents', 2) @@ -240,13 +234,13 @@ def can_function_runner(vs: VehicleState, exit_event: threading.Event): def connect_carla_client(): client = carla.Client("127.0.0.1", 2000) - client.set_timeout(10) + client.set_timeout(5) return client class CarlaBridge: - def __init__(self, args): + def __init__(self, arguments): set_params_enabled() msg = messaging.new_message('liveCalibration') @@ -254,32 +248,48 @@ class CarlaBridge: msg.liveCalibration.rpyCalib = [0.0, 0.0, 0.0] Params().put("CalibrationParams", msg.to_bytes()) - self._args = args + self._args = arguments self._carla_objects = [] self._camerad = None - self._threads_exit_event = threading.Event() + self._exit_event = threading.Event() self._threads = [] - self._shutdown = False + self._keep_alive = True self.started = False signal.signal(signal.SIGTERM, self._on_shutdown) + self._exit = threading.Event() def _on_shutdown(self, signal, frame): - self._shutdown = True + self._keep_alive = False def bridge_keep_alive(self, q: Queue, retries: int): - while not self._shutdown: - try: - self._run(q) - break - except RuntimeError as e: - if retries == 0: - raise - retries -= 1 - print(f"Restarting bridge. Retries left {retries}. Error: {e} ") + try: + while self._keep_alive: + try: + self._run(q) + break + except RuntimeError as e: + self.close() + if retries == 0: + raise + + # Reset for another try + self._carla_objects = [] + self._threads = [] + self._exit_event = threading.Event() + + retries -= 1 + if retries <= -1: + print(f"Restarting bridge. Error: {e} ") + else: + print(f"Restarting bridge. Retries left {retries}. Error: {e} ") + finally: + # Clean up resources in the opposite order they were created. + self.close() def _run(self, q: Queue): client = connect_carla_client() world = client.load_world(self._args.town) + settings = world.get_settings() settings.synchronous_mode = True # Enables synchronous mode settings.fixed_delta_seconds = 0.05 @@ -287,7 +297,7 @@ class CarlaBridge: world.set_weather(carla.WeatherParameters.ClearSunset) - if self._args.low_quality: + if not self._args.high_quality: world.unload_map_layer(carla.MapLayer.Foliage) world.unload_map_layer(carla.MapLayer.Buildings) world.unload_map_layer(carla.MapLayer.ParkedVehicles) @@ -324,7 +334,7 @@ class CarlaBridge: blueprint.set_attribute('image_size_x', str(W)) blueprint.set_attribute('image_size_y', str(H)) blueprint.set_attribute('fov', str(fov)) - if self._args.low_quality: + if not self._args.high_quality: blueprint.set_attribute('enable_postprocess_effects', 'False') camera = world.spawn_actor(blueprint, transform, attach_to=vehicle) camera.listen(callback) @@ -332,7 +342,7 @@ class CarlaBridge: self._camerad = Camerad() road_camera = create_camera(fov=40, callback=self._camerad.cam_callback_road) - road_wide_camera = create_camera(fov=163, callback=self._camerad.cam_callback_wide_road) # fov bigger than 163 shows unwanted artifacts + road_wide_camera = create_camera(fov=120, callback=self._camerad.cam_callback_wide_road) # fov bigger than 120 shows unwanted artifacts self._carla_objects.extend([road_camera, road_wide_camera]) @@ -349,16 +359,13 @@ class CarlaBridge: self._carla_objects.extend([imu, gps]) # launch fake car threads - self._threads.append(threading.Thread(target=panda_state_function, args=(vehicle_state, self._threads_exit_event,))) - self._threads.append(threading.Thread(target=peripheral_state_function, args=(self._threads_exit_event,))) - self._threads.append(threading.Thread(target=fake_driver_monitoring, args=(self._threads_exit_event,))) - self._threads.append(threading.Thread(target=can_function_runner, args=(vehicle_state, self._threads_exit_event,))) + self._threads.append(threading.Thread(target=panda_state_function, args=(vehicle_state, self._exit_event,))) + self._threads.append(threading.Thread(target=peripheral_state_function, args=(self._exit_event,))) + self._threads.append(threading.Thread(target=fake_driver_monitoring, args=(self._exit_event,))) + self._threads.append(threading.Thread(target=can_function_runner, args=(vehicle_state, self._exit_event,))) for t in self._threads: t.start() - # can loop - rk = Ratekeeper(100, print_delay_threshold=0.05) - # init throttle_ease_out_counter = REPEAT_COUNTER brake_ease_out_counter = REPEAT_COUNTER @@ -376,7 +383,14 @@ class CarlaBridge: brake_manual_multiplier = 0.7 # keyboard signal is always 1 steer_manual_multiplier = 45 * STEER_RATIO # keyboard signal is always 1 - while not self._shutdown: + # Simulation tends to be slow in the initial steps. This prevents lagging later + for _ in range(20): + world.tick() + + # loop + rk = Ratekeeper(100, print_delay_threshold=0.05) + + while self._keep_alive: # 1. Read the throttle, steer and brake from op or manual controls # 2. Set instructions in Carla # 3. Send current carstate to op via can @@ -495,13 +509,9 @@ class CarlaBridge: rk.keep_time() self.started = True - # Clean up resources in the opposite order they were created. - self.close() - def close(self): - self._threads_exit_event.set() - if self._camerad is not None: - self._camerad.close() + self.started = False + self._exit_event.set() for s in self._carla_objects: try: s.destroy() diff --git a/tools/sim/lib/keyboard_ctrl.py b/tools/sim/lib/keyboard_ctrl.py index 1c50b9b89..803aa091a 100644 --- a/tools/sim/lib/keyboard_ctrl.py +++ b/tools/sim/lib/keyboard_ctrl.py @@ -38,7 +38,7 @@ def getch() -> str: def keyboard_poll_thread(q: 'Queue[str]'): while True: c = getch() - # print("got %s" % c) + print("got %s" % c) if c == '1': q.put("cruise_up") elif c == '2': diff --git a/tools/sim/start_carla.sh b/tools/sim/start_carla.sh index 8c3b140c8..912c7d6f0 100755 --- a/tools/sim/start_carla.sh +++ b/tools/sim/start_carla.sh @@ -25,4 +25,4 @@ docker run \ -v /tmp/.X11-unix:/tmp/.X11-unix:rw \ -it \ carlasim/carla:0.9.12 \ - /bin/bash ./CarlaUE4.sh -opengl -nosound -RenderOffScreen -benchmark -fps=20 -quality-level=High + /bin/bash ./CarlaUE4.sh -opengl -nosound -RenderOffScreen -benchmark -fps=20 -quality-level=Low diff --git a/tools/sim/test/test_carla_integration.py b/tools/sim/test/test_carla_integration.py index e70e881f0..8f76abb56 100755 --- a/tools/sim/test/test_carla_integration.py +++ b/tools/sim/test/test_carla_integration.py @@ -15,27 +15,18 @@ class TestCarlaIntegration(unittest.TestCase): Tests need Carla simulator to run """ processes = None + carla_process = None def setUp(self): self.processes = [] - # We want to make sure that carla_sim docker is still running. Skip output shell + # We want to make sure that carla_sim docker isn't still running. subprocess.run("docker rm -f carla_sim", shell=True, stderr=subprocess.PIPE, check=False) - self.processes.append(subprocess.Popen(".././start_carla.sh")) + self.carla_process = subprocess.Popen(".././start_carla.sh") # Too many lagging messages in bridge.py can cause a crash. This prevents it. unblock_stdout() - - def test_run_bridge(self): - # Test bridge connect with carla and runs without any errors for 60 seconds - test_duration = 60 - - carla_bridge = CarlaBridge(bridge.parse_args(['--low_quality'])) - p = carla_bridge.run(Queue(), retries=3) - self.processes = [p] - - time.sleep(test_duration) - - self.assertEqual(p.exitcode, None, f"Bridge process should be running, but exited with code {p.exitcode}") + # Wait 10 seconds to startup carla + time.sleep(10) def test_engage(self): # Startup manager and bridge.py. Check processes are running, then engage and verify. @@ -44,7 +35,7 @@ class TestCarlaIntegration(unittest.TestCase): sm = messaging.SubMaster(['controlsState', 'carEvents', 'managerState']) q = Queue() - carla_bridge = CarlaBridge(bridge.parse_args(['--low_quality'])) + carla_bridge = CarlaBridge(bridge.parse_args([])) p_bridge = carla_bridge.run(q, retries=3) self.processes.append(p_bridge) @@ -70,7 +61,7 @@ class TestCarlaIntegration(unittest.TestCase): no_car_events_issues_once = True break - self.assertTrue(no_car_events_issues_once, f"Failed because sm offline, or CarEvents '{car_event_issues}' or processes not running '{not_running}'") + self.assertTrue(no_car_events_issues_once, f"Failed because no messages received, or CarEvents '{car_event_issues}' or processes not running '{not_running}'") start_time = time.monotonic() min_counts_control_active = 100 @@ -99,8 +90,11 @@ class TestCarlaIntegration(unittest.TestCase): p.wait(15) else: p.join(15) - subprocess.run("docker rm -f carla_sim", shell=True, stderr=subprocess.PIPE, check=False) + # Stop carla simulator by removing docker container + subprocess.run("docker rm -f carla_sim", shell=True, stderr=subprocess.PIPE, check=False) + if self.carla_process is not None: + self.carla_process.wait() if __name__ == "__main__": From 5ddd2000b23ca3fc3c67a5a29331a28cffaa2a9e Mon Sep 17 00:00:00 2001 From: George Hotz <72895+geohot@users.noreply.github.com> Date: Fri, 29 Apr 2022 07:59:03 -0700 Subject: [PATCH 14/21] Revert "camerad: don't remap everything every time (#24334)" (#24370) This reverts commit fb7d84875bc993beaf75645a7e4b311704742f02. --- selfdrive/camerad/cameras/camera_qcom2.cc | 160 +++++++++------------- selfdrive/camerad/cameras/camera_qcom2.h | 17 +-- selfdrive/test/test_onroad.py | 2 +- 3 files changed, 66 insertions(+), 113 deletions(-) diff --git a/selfdrive/camerad/cameras/camera_qcom2.cc b/selfdrive/camerad/cameras/camera_qcom2.cc index 1d092eb6a..8986351fc 100644 --- a/selfdrive/camerad/cameras/camera_qcom2.cc +++ b/selfdrive/camerad/cameras/camera_qcom2.cc @@ -87,11 +87,11 @@ int do_cam_control(int fd, int op_code, void *handle, int size) { return ret; } -std::optional device_acquire(int fd, int32_t session_handle, void *data, uint32_t num_resources=1) { +std::optional device_acquire(int fd, int32_t session_handle, void *data) { struct cam_acquire_dev_cmd cmd = { .session_handle = session_handle, .handle_type = CAM_HANDLE_USER_POINTER, - .num_resources = (uint32_t)(data ? num_resources : 0), + .num_resources = (uint32_t)(data ? 1 : 0), .resource_hdl = (uint64_t)data, }; int err = do_cam_control(fd, CAM_ACQUIRE_DEV, &cmd, sizeof(cmd)); @@ -158,42 +158,6 @@ void release_fd(int video0_fd, uint32_t handle) { release(video0_fd, handle); } -void *MemoryManager::alloc(int size, uint32_t *handle) { - lock.lock(); - void *ptr; - if (!cached_allocations[size].empty()) { - ptr = cached_allocations[size].front(); - cached_allocations[size].pop(); - *handle = handle_lookup[ptr]; - } else { - ptr = alloc_w_mmu_hdl(video0_fd, size, handle); - handle_lookup[ptr] = *handle; - size_lookup[ptr] = size; - } - lock.unlock(); - return ptr; -} - -void MemoryManager::free(void *ptr) { - lock.lock(); - cached_allocations[size_lookup[ptr]].push(ptr); - lock.unlock(); -} - -MemoryManager::~MemoryManager() { - for (auto& x : cached_allocations) { - while (!x.second.empty()) { - void *ptr = x.second.front(); - x.second.pop(); - LOGD("freeing cached allocation %p with size %d", ptr, size_lookup[ptr]); - munmap(ptr, size_lookup[ptr]); - release_fd(video0_fd, handle_lookup[ptr]); - handle_lookup.erase(ptr); - size_lookup.erase(ptr); - } - } -} - int CameraState::clear_req_queue() { struct cam_req_mgr_flush_info req_mgr_flush_request = {0}; req_mgr_flush_request.session_hdl = session_handle; @@ -222,7 +186,7 @@ void CameraState::sensors_start() { void CameraState::sensors_poke(int request_id) { uint32_t cam_packet_handle = 0; int size = sizeof(struct cam_packet); - struct cam_packet *pkt = (struct cam_packet *)mm.alloc(size, &cam_packet_handle); + struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(multi_cam_state->video0_fd, size, &cam_packet_handle); pkt->num_cmd_buf = 0; pkt->kmd_cmd_buf_index = -1; pkt->header.size = size; @@ -236,14 +200,15 @@ void CameraState::sensors_poke(int request_id) { return; } - mm.free(pkt); + munmap(pkt, size); + release_fd(multi_cam_state->video0_fd, cam_packet_handle); } void CameraState::sensors_i2c(struct i2c_random_wr_payload* dat, int len, int op_code, bool data_word) { // LOGD("sensors_i2c: %d", len); uint32_t cam_packet_handle = 0; int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*1; - struct cam_packet *pkt = (struct cam_packet *)mm.alloc(size, &cam_packet_handle); + struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(multi_cam_state->video0_fd, size, &cam_packet_handle); pkt->num_cmd_buf = 1; pkt->kmd_cmd_buf_index = -1; pkt->header.size = size; @@ -253,7 +218,7 @@ void CameraState::sensors_i2c(struct i2c_random_wr_payload* dat, int len, int op buf_desc[0].size = buf_desc[0].length = sizeof(struct i2c_rdwr_header) + len*sizeof(struct i2c_random_wr_payload); buf_desc[0].type = CAM_CMD_BUF_I2C; - struct cam_cmd_i2c_random_wr *i2c_random_wr = (struct cam_cmd_i2c_random_wr *)mm.alloc(buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle); + struct cam_cmd_i2c_random_wr *i2c_random_wr = (struct cam_cmd_i2c_random_wr *)alloc_w_mmu_hdl(multi_cam_state->video0_fd, buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle); i2c_random_wr->header.count = len; i2c_random_wr->header.op_code = 1; i2c_random_wr->header.cmd_type = CAMERA_SENSOR_CMD_TYPE_I2C_RNDM_WR; @@ -268,8 +233,10 @@ void CameraState::sensors_i2c(struct i2c_random_wr_payload* dat, int len, int op return; } - mm.free(i2c_random_wr); - mm.free(pkt); + munmap(i2c_random_wr, buf_desc[0].size); + release_fd(multi_cam_state->video0_fd, buf_desc[0].mem_handle); + munmap(pkt, size); + release_fd(multi_cam_state->video0_fd, cam_packet_handle); } static cam_cmd_power *power_set_wait(cam_cmd_power *power, int16_t delay_ms) { @@ -281,9 +248,10 @@ static cam_cmd_power *power_set_wait(cam_cmd_power *power, int16_t delay_ms) { }; int CameraState::sensors_init() { + int video0_fd = multi_cam_state->video0_fd; uint32_t cam_packet_handle = 0; int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*2; - struct cam_packet *pkt = (struct cam_packet *)mm.alloc(size, &cam_packet_handle); + struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(video0_fd, size, &cam_packet_handle); pkt->num_cmd_buf = 2; pkt->kmd_cmd_buf_index = -1; pkt->header.op_code = 0x1000000 | CAM_SENSOR_PACKET_OPCODE_SENSOR_PROBE; @@ -292,7 +260,7 @@ int CameraState::sensors_init() { buf_desc[0].size = buf_desc[0].length = sizeof(struct cam_cmd_i2c_info) + sizeof(struct cam_cmd_probe); buf_desc[0].type = CAM_CMD_BUF_LEGACY; - struct cam_cmd_i2c_info *i2c_info = (struct cam_cmd_i2c_info *)mm.alloc(buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle); + struct cam_cmd_i2c_info *i2c_info = (struct cam_cmd_i2c_info *)alloc_w_mmu_hdl(video0_fd, buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle); auto probe = (struct cam_cmd_probe *)(i2c_info + 1); probe->camera_id = camera_num; @@ -334,7 +302,7 @@ int CameraState::sensors_init() { //buf_desc[1].size = buf_desc[1].length = 148; buf_desc[1].size = buf_desc[1].length = 196; buf_desc[1].type = CAM_CMD_BUF_I2C; - struct cam_cmd_power *power_settings = (struct cam_cmd_power *)mm.alloc(buf_desc[1].size, (uint32_t*)&buf_desc[1].mem_handle); + struct cam_cmd_power *power_settings = (struct cam_cmd_power *)alloc_w_mmu_hdl(video0_fd, buf_desc[1].size, (uint32_t*)&buf_desc[1].mem_handle); memset(power_settings, 0, buf_desc[1].size); // power on @@ -395,9 +363,12 @@ int CameraState::sensors_init() { LOGD("probing the sensor"); int ret = do_cam_control(sensor_fd, CAM_SENSOR_PROBE_CMD, (void *)(uintptr_t)cam_packet_handle, 0); - mm.free(i2c_info); - mm.free(power_settings); - mm.free(pkt); + munmap(i2c_info, buf_desc[0].size); + release_fd(video0_fd, buf_desc[0].mem_handle); + munmap(power_settings, buf_desc[1].size); + release_fd(video0_fd, buf_desc[1].mem_handle); + munmap(pkt, size); + release_fd(video0_fd, cam_packet_handle); return ret; } @@ -408,7 +379,7 @@ void CameraState::config_isp(int io_mem_handle, int fence, int request_id, int b if (io_mem_handle != 0) { size += sizeof(struct cam_buf_io_cfg); } - struct cam_packet *pkt = (struct cam_packet *)mm.alloc(size, &cam_packet_handle); + struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(multi_cam_state->video0_fd, size, &cam_packet_handle); pkt->num_cmd_buf = 2; pkt->kmd_cmd_buf_index = 0; // YUV has kmd_cmd_buf_offset = 1780 @@ -416,7 +387,7 @@ void CameraState::config_isp(int io_mem_handle, int fence, int request_id, int b // YUV also has patch_offset = 0x1030 and num_patches = 10 if (io_mem_handle != 0) { - pkt->io_configs_offset = sizeof(struct cam_cmd_buf_desc)*pkt->num_cmd_buf; + pkt->io_configs_offset = sizeof(struct cam_cmd_buf_desc)*2; pkt->num_io_configs = 1; } @@ -503,7 +474,7 @@ void CameraState::config_isp(int io_mem_handle, int fence, int request_id, int b buf_desc[1].length = buf_desc[1].size - buf_desc[1].offset; buf_desc[1].type = CAM_CMD_BUF_GENERIC; buf_desc[1].meta_data = CAM_ISP_PACKET_META_GENERIC_BLOB_COMMON; - uint32_t *buf2 = (uint32_t *)mm.alloc(buf_desc[1].size, (uint32_t*)&buf_desc[1].mem_handle); + uint32_t *buf2 = (uint32_t *)alloc_w_mmu_hdl(multi_cam_state->video0_fd, buf_desc[1].size, (uint32_t*)&buf_desc[1].mem_handle, 0x20); memcpy(buf2, &tmp, sizeof(tmp)); if (io_mem_handle != 0) { @@ -539,20 +510,24 @@ void CameraState::config_isp(int io_mem_handle, int fence, int request_id, int b printf("ISP CONFIG FAILED\n"); } - mm.free(buf2); - mm.free(pkt); + munmap(buf2, buf_desc[1].size); + release_fd(multi_cam_state->video0_fd, buf_desc[1].mem_handle); + // release_fd(multi_cam_state->video0_fd, buf_desc[0].mem_handle); + munmap(pkt, size); + release_fd(multi_cam_state->video0_fd, cam_packet_handle); } void CameraState::enqueue_buffer(int i, bool dp) { int ret; int request_id = request_ids[i]; - if (buf_handle[i] && sync_objs[i]) { + if (buf_handle[i]) { + release(multi_cam_state->video0_fd, buf_handle[i]); // wait struct cam_sync_wait sync_wait = {0}; sync_wait.sync_obj = sync_objs[i]; sync_wait.timeout_ms = 50; // max dt tolerance, typical should be 23 - ret = do_cam_control(multi_cam_state->cam_sync_fd, CAM_SYNC_WAIT, &sync_wait, sizeof(sync_wait)); + ret = do_cam_control(multi_cam_state->video1_fd, CAM_SYNC_WAIT, &sync_wait, sizeof(sync_wait)); // LOGD("fence wait: %d %d", ret, sync_wait.sync_obj); buf.camera_bufs_metadata[i].timestamp_eof = (uint64_t)nanos_since_boot(); // set true eof @@ -560,19 +535,13 @@ void CameraState::enqueue_buffer(int i, bool dp) { // destroy old output fence struct cam_sync_info sync_destroy = {0}; + strcpy(sync_destroy.name, "NodeOutputPortFence"); sync_destroy.sync_obj = sync_objs[i]; - ret = do_cam_control(multi_cam_state->cam_sync_fd, CAM_SYNC_DESTROY, &sync_destroy, sizeof(sync_destroy)); + ret = do_cam_control(multi_cam_state->video1_fd, CAM_SYNC_DESTROY, &sync_destroy, sizeof(sync_destroy)); // LOGD("fence destroy: %d %d", ret, sync_destroy.sync_obj); } - // create output fence - struct cam_sync_info sync_create = {0}; - strcpy(sync_create.name, "NodeOutputPortFence"); - ret = do_cam_control(multi_cam_state->cam_sync_fd, CAM_SYNC_CREATE, &sync_create, sizeof(sync_create)); - // LOGD("fence req: %d %d", ret, sync_create.sync_obj); - sync_objs[i] = sync_create.sync_obj; - - // schedule request with camera request manager + // do stuff struct cam_req_mgr_sched_request req_mgr_sched_request = {0}; req_mgr_sched_request.session_hdl = session_handle; req_mgr_sched_request.link_hdl = link_handle; @@ -580,11 +549,28 @@ void CameraState::enqueue_buffer(int i, bool dp) { ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_SCHED_REQ, &req_mgr_sched_request, sizeof(req_mgr_sched_request)); // LOGD("sched req: %d %d", ret, request_id); - // poke sensor, must happen after schedule + // create output fence + struct cam_sync_info sync_create = {0}; + strcpy(sync_create.name, "NodeOutputPortFence"); + ret = do_cam_control(multi_cam_state->video1_fd, CAM_SYNC_CREATE, &sync_create, sizeof(sync_create)); + // LOGD("fence req: %d %d", ret, sync_create.sync_obj); + sync_objs[i] = sync_create.sync_obj; + + // configure ISP to put the image in place + struct cam_mem_mgr_map_cmd mem_mgr_map_cmd = {0}; + mem_mgr_map_cmd.mmu_hdls[0] = multi_cam_state->device_iommu; + mem_mgr_map_cmd.num_hdl = 1; + mem_mgr_map_cmd.flags = CAM_MEM_FLAG_HW_READ_WRITE; + mem_mgr_map_cmd.fd = buf.camera_bufs[i].fd; + ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_MAP_BUF, &mem_mgr_map_cmd, sizeof(mem_mgr_map_cmd)); + // LOGD("map buf req: (fd: %d) 0x%x %d", bufs[i].fd, mem_mgr_map_cmd.out.buf_handle, ret); + buf_handle[i] = mem_mgr_map_cmd.out.buf_handle; + + // poke sensor sensors_poke(request_id); // LOGD("Poked sensor"); - // submit request to the ife + // push the buffer config_isp(buf_handle[i], sync_objs[i], request_id, buf0_handle, 65632*(i+1)); } @@ -630,9 +616,6 @@ void CameraState::camera_open() { assert(sensor_fd >= 0); LOGD("opened sensor for %d", camera_num); - // init memorymanager for this camera - mm.init(multi_cam_state->video0_fd); - // probe the sensor LOGD("-- Probing sensor %d", camera_num); ret = sensors_init(); @@ -746,7 +729,7 @@ void CameraState::camera_open() { { uint32_t cam_packet_handle = 0; int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*1; - struct cam_packet *pkt = (struct cam_packet *)mm.alloc(size, &cam_packet_handle); + struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(multi_cam_state->video0_fd, size, &cam_packet_handle); pkt->num_cmd_buf = 1; pkt->kmd_cmd_buf_index = -1; pkt->header.size = size; @@ -755,7 +738,7 @@ void CameraState::camera_open() { buf_desc[0].size = buf_desc[0].length = sizeof(struct cam_csiphy_info); buf_desc[0].type = CAM_CMD_BUF_GENERIC; - struct cam_csiphy_info *csiphy_info = (struct cam_csiphy_info *)mm.alloc(buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle); + struct cam_csiphy_info *csiphy_info = (struct cam_csiphy_info *)alloc_w_mmu_hdl(multi_cam_state->video0_fd, buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle); csiphy_info->lane_mask = 0x1f; csiphy_info->lane_assign = 0x3210;// skip clk. How is this 16 bit for 5 channels?? csiphy_info->csiphy_3phase = 0x0; // no 3 phase, only 2 conductors per lane @@ -768,8 +751,10 @@ void CameraState::camera_open() { int ret_ = device_config(csiphy_fd, session_handle, csiphy_dev_handle, cam_packet_handle); assert(ret_ == 0); - mm.free(csiphy_info); - mm.free(pkt); + munmap(csiphy_info, buf_desc[0].size); + release_fd(multi_cam_state->video0_fd, buf_desc[0].mem_handle); + munmap(pkt, size); + release_fd(multi_cam_state->video0_fd, cam_packet_handle); } // link devices @@ -796,18 +781,6 @@ void CameraState::camera_open() { ret = device_control(multi_cam_state->isp_fd, CAM_START_DEV, session_handle, isp_dev_handle); LOGD("start isp: %d", ret); - for (int i = 0; i < FRAME_BUF_COUNT; i++) { - // configure ISP to put the image in place - struct cam_mem_mgr_map_cmd mem_mgr_map_cmd = {0}; - mem_mgr_map_cmd.mmu_hdls[0] = multi_cam_state->device_iommu; - mem_mgr_map_cmd.num_hdl = 1; - mem_mgr_map_cmd.flags = CAM_MEM_FLAG_HW_READ_WRITE; - mem_mgr_map_cmd.fd = buf.camera_bufs[i].fd; - ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_MAP_BUF, &mem_mgr_map_cmd, sizeof(mem_mgr_map_cmd)); - LOGD("map buf req: (fd: %d) 0x%x %d", buf.camera_bufs[i].fd, mem_mgr_map_cmd.out.buf_handle, ret); - buf_handle[i] = mem_mgr_map_cmd.out.buf_handle; - } - // TODO: this is unneeded, should we be doing the start i2c in a different way? //ret = device_control(sensor_fd, CAM_START_DEV, session_handle, sensor_dev_handle); //LOGD("start sensor: %d", ret); @@ -834,9 +807,9 @@ void cameras_open(MultiCameraState *s) { LOGD("opened video0"); // video1 is cam_sync, the target of some ioctls - s->cam_sync_fd = HANDLE_EINTR(open("/dev/v4l/by-path/platform-cam_sync-video-index0", O_RDWR | O_NONBLOCK)); - assert(s->cam_sync_fd >= 0); - LOGD("opened video1 (cam_sync)"); + s->video1_fd = HANDLE_EINTR(open("/dev/v4l/by-path/platform-cam_sync-video-index0", O_RDWR | O_NONBLOCK)); + assert(s->video1_fd >= 0); + LOGD("opened video1"); // looks like there's only one of these s->isp_fd = open_v4l_by_name_and_index("cam-isp"); @@ -861,7 +834,7 @@ void cameras_open(MultiCameraState *s) { LOG("-- Subscribing"); static struct v4l2_event_subscription sub = {0}; sub.type = V4L_EVENT_CAM_REQ_MGR_EVENT; - sub.id = V4L_EVENT_CAM_REQ_MGR_SOF_BOOT_TS; + sub.id = 2; // should use boot time for sof ret = HANDLE_EINTR(ioctl(s->video0_fd, VIDIOC_SUBSCRIBE_EVENT, &sub)); printf("req mgr subscribe: %d\n", ret); @@ -910,11 +883,6 @@ void CameraState::camera_close() { LOGD("release isp: %d", ret); ret = device_control(csiphy_fd, CAM_RELEASE_DEV, session_handle, csiphy_dev_handle); LOGD("release csiphy: %d", ret); - - for (int i = 0; i < FRAME_BUF_COUNT; i++) { - release(multi_cam_state->video0_fd, buf_handle[i]); - } - LOGD("released buffers"); } ret = device_control(sensor_fd, CAM_RELEASE_DEV, session_handle, sensor_dev_handle); diff --git a/selfdrive/camerad/cameras/camera_qcom2.h b/selfdrive/camerad/cameras/camera_qcom2.h index a7c64c066..199aa40e7 100644 --- a/selfdrive/camerad/cameras/camera_qcom2.h +++ b/selfdrive/camerad/cameras/camera_qcom2.h @@ -9,20 +9,6 @@ #define FRAME_BUF_COUNT 4 -class MemoryManager { - public: - void init(int _video0_fd) { video0_fd = _video0_fd; } - void *alloc(int len, uint32_t *handle); - void free(void *ptr); - ~MemoryManager(); - private: - std::mutex lock; - std::map handle_lookup; - std::map size_lookup; - std::map > cached_allocations; - int video0_fd; -}; - class CameraState { public: MultiCameraState *multi_cam_state; @@ -74,7 +60,6 @@ public: int camera_id; CameraBuf buf; - MemoryManager mm; private: void config_isp(int io_mem_handle, int fence, int request_id, int buf0_mem_handle, int buf0_offset); void enqueue_req_multi(int start, int n, bool dp); @@ -88,7 +73,7 @@ private: typedef struct MultiCameraState { unique_fd video0_fd; - unique_fd cam_sync_fd; + unique_fd video1_fd; unique_fd isp_fd; int device_iommu; int cdm_iommu; diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index b845ee0e0..c8c1a3437 100755 --- a/selfdrive/test/test_onroad.py +++ b/selfdrive/test/test_onroad.py @@ -23,7 +23,7 @@ from tools.lib.logreader import LogReader PROCS = { "selfdrive.controls.controlsd": 31.0, "./loggerd": 70.0, - "./camerad": 26.0, + "./camerad": 41.0, "./locationd": 9.1, "selfdrive.controls.plannerd": 11.7, "./_ui": 18.4, From b7ddce8bacde6de2f708af0b70ea8a20d61396f2 Mon Sep 17 00:00:00 2001 From: George Hotz <72895+geohot@users.noreply.github.com> Date: Fri, 29 Apr 2022 09:09:12 -0700 Subject: [PATCH 15/21] camerad: less ioctls try 2 (#24371) * premap the buffers * memory manager * free buffers properly, alignment seems okay * update camerad CPU usage * cam_sync_fd * useless line, and use the define * cheap prereqs for multistream Co-authored-by: Comma Device --- selfdrive/camerad/cameras/camera_qcom2.cc | 160 +++++++++++++--------- selfdrive/camerad/cameras/camera_qcom2.h | 17 ++- selfdrive/test/test_onroad.py | 2 +- 3 files changed, 113 insertions(+), 66 deletions(-) diff --git a/selfdrive/camerad/cameras/camera_qcom2.cc b/selfdrive/camerad/cameras/camera_qcom2.cc index 8986351fc..1d092eb6a 100644 --- a/selfdrive/camerad/cameras/camera_qcom2.cc +++ b/selfdrive/camerad/cameras/camera_qcom2.cc @@ -87,11 +87,11 @@ int do_cam_control(int fd, int op_code, void *handle, int size) { return ret; } -std::optional device_acquire(int fd, int32_t session_handle, void *data) { +std::optional device_acquire(int fd, int32_t session_handle, void *data, uint32_t num_resources=1) { struct cam_acquire_dev_cmd cmd = { .session_handle = session_handle, .handle_type = CAM_HANDLE_USER_POINTER, - .num_resources = (uint32_t)(data ? 1 : 0), + .num_resources = (uint32_t)(data ? num_resources : 0), .resource_hdl = (uint64_t)data, }; int err = do_cam_control(fd, CAM_ACQUIRE_DEV, &cmd, sizeof(cmd)); @@ -158,6 +158,42 @@ void release_fd(int video0_fd, uint32_t handle) { release(video0_fd, handle); } +void *MemoryManager::alloc(int size, uint32_t *handle) { + lock.lock(); + void *ptr; + if (!cached_allocations[size].empty()) { + ptr = cached_allocations[size].front(); + cached_allocations[size].pop(); + *handle = handle_lookup[ptr]; + } else { + ptr = alloc_w_mmu_hdl(video0_fd, size, handle); + handle_lookup[ptr] = *handle; + size_lookup[ptr] = size; + } + lock.unlock(); + return ptr; +} + +void MemoryManager::free(void *ptr) { + lock.lock(); + cached_allocations[size_lookup[ptr]].push(ptr); + lock.unlock(); +} + +MemoryManager::~MemoryManager() { + for (auto& x : cached_allocations) { + while (!x.second.empty()) { + void *ptr = x.second.front(); + x.second.pop(); + LOGD("freeing cached allocation %p with size %d", ptr, size_lookup[ptr]); + munmap(ptr, size_lookup[ptr]); + release_fd(video0_fd, handle_lookup[ptr]); + handle_lookup.erase(ptr); + size_lookup.erase(ptr); + } + } +} + int CameraState::clear_req_queue() { struct cam_req_mgr_flush_info req_mgr_flush_request = {0}; req_mgr_flush_request.session_hdl = session_handle; @@ -186,7 +222,7 @@ void CameraState::sensors_start() { void CameraState::sensors_poke(int request_id) { uint32_t cam_packet_handle = 0; int size = sizeof(struct cam_packet); - struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(multi_cam_state->video0_fd, size, &cam_packet_handle); + struct cam_packet *pkt = (struct cam_packet *)mm.alloc(size, &cam_packet_handle); pkt->num_cmd_buf = 0; pkt->kmd_cmd_buf_index = -1; pkt->header.size = size; @@ -200,15 +236,14 @@ void CameraState::sensors_poke(int request_id) { return; } - munmap(pkt, size); - release_fd(multi_cam_state->video0_fd, cam_packet_handle); + mm.free(pkt); } void CameraState::sensors_i2c(struct i2c_random_wr_payload* dat, int len, int op_code, bool data_word) { // LOGD("sensors_i2c: %d", len); uint32_t cam_packet_handle = 0; int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*1; - struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(multi_cam_state->video0_fd, size, &cam_packet_handle); + struct cam_packet *pkt = (struct cam_packet *)mm.alloc(size, &cam_packet_handle); pkt->num_cmd_buf = 1; pkt->kmd_cmd_buf_index = -1; pkt->header.size = size; @@ -218,7 +253,7 @@ void CameraState::sensors_i2c(struct i2c_random_wr_payload* dat, int len, int op buf_desc[0].size = buf_desc[0].length = sizeof(struct i2c_rdwr_header) + len*sizeof(struct i2c_random_wr_payload); buf_desc[0].type = CAM_CMD_BUF_I2C; - struct cam_cmd_i2c_random_wr *i2c_random_wr = (struct cam_cmd_i2c_random_wr *)alloc_w_mmu_hdl(multi_cam_state->video0_fd, buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle); + struct cam_cmd_i2c_random_wr *i2c_random_wr = (struct cam_cmd_i2c_random_wr *)mm.alloc(buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle); i2c_random_wr->header.count = len; i2c_random_wr->header.op_code = 1; i2c_random_wr->header.cmd_type = CAMERA_SENSOR_CMD_TYPE_I2C_RNDM_WR; @@ -233,10 +268,8 @@ void CameraState::sensors_i2c(struct i2c_random_wr_payload* dat, int len, int op return; } - munmap(i2c_random_wr, buf_desc[0].size); - release_fd(multi_cam_state->video0_fd, buf_desc[0].mem_handle); - munmap(pkt, size); - release_fd(multi_cam_state->video0_fd, cam_packet_handle); + mm.free(i2c_random_wr); + mm.free(pkt); } static cam_cmd_power *power_set_wait(cam_cmd_power *power, int16_t delay_ms) { @@ -248,10 +281,9 @@ static cam_cmd_power *power_set_wait(cam_cmd_power *power, int16_t delay_ms) { }; int CameraState::sensors_init() { - int video0_fd = multi_cam_state->video0_fd; uint32_t cam_packet_handle = 0; int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*2; - struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(video0_fd, size, &cam_packet_handle); + struct cam_packet *pkt = (struct cam_packet *)mm.alloc(size, &cam_packet_handle); pkt->num_cmd_buf = 2; pkt->kmd_cmd_buf_index = -1; pkt->header.op_code = 0x1000000 | CAM_SENSOR_PACKET_OPCODE_SENSOR_PROBE; @@ -260,7 +292,7 @@ int CameraState::sensors_init() { buf_desc[0].size = buf_desc[0].length = sizeof(struct cam_cmd_i2c_info) + sizeof(struct cam_cmd_probe); buf_desc[0].type = CAM_CMD_BUF_LEGACY; - struct cam_cmd_i2c_info *i2c_info = (struct cam_cmd_i2c_info *)alloc_w_mmu_hdl(video0_fd, buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle); + struct cam_cmd_i2c_info *i2c_info = (struct cam_cmd_i2c_info *)mm.alloc(buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle); auto probe = (struct cam_cmd_probe *)(i2c_info + 1); probe->camera_id = camera_num; @@ -302,7 +334,7 @@ int CameraState::sensors_init() { //buf_desc[1].size = buf_desc[1].length = 148; buf_desc[1].size = buf_desc[1].length = 196; buf_desc[1].type = CAM_CMD_BUF_I2C; - struct cam_cmd_power *power_settings = (struct cam_cmd_power *)alloc_w_mmu_hdl(video0_fd, buf_desc[1].size, (uint32_t*)&buf_desc[1].mem_handle); + struct cam_cmd_power *power_settings = (struct cam_cmd_power *)mm.alloc(buf_desc[1].size, (uint32_t*)&buf_desc[1].mem_handle); memset(power_settings, 0, buf_desc[1].size); // power on @@ -363,12 +395,9 @@ int CameraState::sensors_init() { LOGD("probing the sensor"); int ret = do_cam_control(sensor_fd, CAM_SENSOR_PROBE_CMD, (void *)(uintptr_t)cam_packet_handle, 0); - munmap(i2c_info, buf_desc[0].size); - release_fd(video0_fd, buf_desc[0].mem_handle); - munmap(power_settings, buf_desc[1].size); - release_fd(video0_fd, buf_desc[1].mem_handle); - munmap(pkt, size); - release_fd(video0_fd, cam_packet_handle); + mm.free(i2c_info); + mm.free(power_settings); + mm.free(pkt); return ret; } @@ -379,7 +408,7 @@ void CameraState::config_isp(int io_mem_handle, int fence, int request_id, int b if (io_mem_handle != 0) { size += sizeof(struct cam_buf_io_cfg); } - struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(multi_cam_state->video0_fd, size, &cam_packet_handle); + struct cam_packet *pkt = (struct cam_packet *)mm.alloc(size, &cam_packet_handle); pkt->num_cmd_buf = 2; pkt->kmd_cmd_buf_index = 0; // YUV has kmd_cmd_buf_offset = 1780 @@ -387,7 +416,7 @@ void CameraState::config_isp(int io_mem_handle, int fence, int request_id, int b // YUV also has patch_offset = 0x1030 and num_patches = 10 if (io_mem_handle != 0) { - pkt->io_configs_offset = sizeof(struct cam_cmd_buf_desc)*2; + pkt->io_configs_offset = sizeof(struct cam_cmd_buf_desc)*pkt->num_cmd_buf; pkt->num_io_configs = 1; } @@ -474,7 +503,7 @@ void CameraState::config_isp(int io_mem_handle, int fence, int request_id, int b buf_desc[1].length = buf_desc[1].size - buf_desc[1].offset; buf_desc[1].type = CAM_CMD_BUF_GENERIC; buf_desc[1].meta_data = CAM_ISP_PACKET_META_GENERIC_BLOB_COMMON; - uint32_t *buf2 = (uint32_t *)alloc_w_mmu_hdl(multi_cam_state->video0_fd, buf_desc[1].size, (uint32_t*)&buf_desc[1].mem_handle, 0x20); + uint32_t *buf2 = (uint32_t *)mm.alloc(buf_desc[1].size, (uint32_t*)&buf_desc[1].mem_handle); memcpy(buf2, &tmp, sizeof(tmp)); if (io_mem_handle != 0) { @@ -510,24 +539,20 @@ void CameraState::config_isp(int io_mem_handle, int fence, int request_id, int b printf("ISP CONFIG FAILED\n"); } - munmap(buf2, buf_desc[1].size); - release_fd(multi_cam_state->video0_fd, buf_desc[1].mem_handle); - // release_fd(multi_cam_state->video0_fd, buf_desc[0].mem_handle); - munmap(pkt, size); - release_fd(multi_cam_state->video0_fd, cam_packet_handle); + mm.free(buf2); + mm.free(pkt); } void CameraState::enqueue_buffer(int i, bool dp) { int ret; int request_id = request_ids[i]; - if (buf_handle[i]) { - release(multi_cam_state->video0_fd, buf_handle[i]); + if (buf_handle[i] && sync_objs[i]) { // wait struct cam_sync_wait sync_wait = {0}; sync_wait.sync_obj = sync_objs[i]; sync_wait.timeout_ms = 50; // max dt tolerance, typical should be 23 - ret = do_cam_control(multi_cam_state->video1_fd, CAM_SYNC_WAIT, &sync_wait, sizeof(sync_wait)); + ret = do_cam_control(multi_cam_state->cam_sync_fd, CAM_SYNC_WAIT, &sync_wait, sizeof(sync_wait)); // LOGD("fence wait: %d %d", ret, sync_wait.sync_obj); buf.camera_bufs_metadata[i].timestamp_eof = (uint64_t)nanos_since_boot(); // set true eof @@ -535,13 +560,19 @@ void CameraState::enqueue_buffer(int i, bool dp) { // destroy old output fence struct cam_sync_info sync_destroy = {0}; - strcpy(sync_destroy.name, "NodeOutputPortFence"); sync_destroy.sync_obj = sync_objs[i]; - ret = do_cam_control(multi_cam_state->video1_fd, CAM_SYNC_DESTROY, &sync_destroy, sizeof(sync_destroy)); + ret = do_cam_control(multi_cam_state->cam_sync_fd, CAM_SYNC_DESTROY, &sync_destroy, sizeof(sync_destroy)); // LOGD("fence destroy: %d %d", ret, sync_destroy.sync_obj); } - // do stuff + // create output fence + struct cam_sync_info sync_create = {0}; + strcpy(sync_create.name, "NodeOutputPortFence"); + ret = do_cam_control(multi_cam_state->cam_sync_fd, CAM_SYNC_CREATE, &sync_create, sizeof(sync_create)); + // LOGD("fence req: %d %d", ret, sync_create.sync_obj); + sync_objs[i] = sync_create.sync_obj; + + // schedule request with camera request manager struct cam_req_mgr_sched_request req_mgr_sched_request = {0}; req_mgr_sched_request.session_hdl = session_handle; req_mgr_sched_request.link_hdl = link_handle; @@ -549,28 +580,11 @@ void CameraState::enqueue_buffer(int i, bool dp) { ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_SCHED_REQ, &req_mgr_sched_request, sizeof(req_mgr_sched_request)); // LOGD("sched req: %d %d", ret, request_id); - // create output fence - struct cam_sync_info sync_create = {0}; - strcpy(sync_create.name, "NodeOutputPortFence"); - ret = do_cam_control(multi_cam_state->video1_fd, CAM_SYNC_CREATE, &sync_create, sizeof(sync_create)); - // LOGD("fence req: %d %d", ret, sync_create.sync_obj); - sync_objs[i] = sync_create.sync_obj; - - // configure ISP to put the image in place - struct cam_mem_mgr_map_cmd mem_mgr_map_cmd = {0}; - mem_mgr_map_cmd.mmu_hdls[0] = multi_cam_state->device_iommu; - mem_mgr_map_cmd.num_hdl = 1; - mem_mgr_map_cmd.flags = CAM_MEM_FLAG_HW_READ_WRITE; - mem_mgr_map_cmd.fd = buf.camera_bufs[i].fd; - ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_MAP_BUF, &mem_mgr_map_cmd, sizeof(mem_mgr_map_cmd)); - // LOGD("map buf req: (fd: %d) 0x%x %d", bufs[i].fd, mem_mgr_map_cmd.out.buf_handle, ret); - buf_handle[i] = mem_mgr_map_cmd.out.buf_handle; - - // poke sensor + // poke sensor, must happen after schedule sensors_poke(request_id); // LOGD("Poked sensor"); - // push the buffer + // submit request to the ife config_isp(buf_handle[i], sync_objs[i], request_id, buf0_handle, 65632*(i+1)); } @@ -616,6 +630,9 @@ void CameraState::camera_open() { assert(sensor_fd >= 0); LOGD("opened sensor for %d", camera_num); + // init memorymanager for this camera + mm.init(multi_cam_state->video0_fd); + // probe the sensor LOGD("-- Probing sensor %d", camera_num); ret = sensors_init(); @@ -729,7 +746,7 @@ void CameraState::camera_open() { { uint32_t cam_packet_handle = 0; int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*1; - struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(multi_cam_state->video0_fd, size, &cam_packet_handle); + struct cam_packet *pkt = (struct cam_packet *)mm.alloc(size, &cam_packet_handle); pkt->num_cmd_buf = 1; pkt->kmd_cmd_buf_index = -1; pkt->header.size = size; @@ -738,7 +755,7 @@ void CameraState::camera_open() { buf_desc[0].size = buf_desc[0].length = sizeof(struct cam_csiphy_info); buf_desc[0].type = CAM_CMD_BUF_GENERIC; - struct cam_csiphy_info *csiphy_info = (struct cam_csiphy_info *)alloc_w_mmu_hdl(multi_cam_state->video0_fd, buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle); + struct cam_csiphy_info *csiphy_info = (struct cam_csiphy_info *)mm.alloc(buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle); csiphy_info->lane_mask = 0x1f; csiphy_info->lane_assign = 0x3210;// skip clk. How is this 16 bit for 5 channels?? csiphy_info->csiphy_3phase = 0x0; // no 3 phase, only 2 conductors per lane @@ -751,10 +768,8 @@ void CameraState::camera_open() { int ret_ = device_config(csiphy_fd, session_handle, csiphy_dev_handle, cam_packet_handle); assert(ret_ == 0); - munmap(csiphy_info, buf_desc[0].size); - release_fd(multi_cam_state->video0_fd, buf_desc[0].mem_handle); - munmap(pkt, size); - release_fd(multi_cam_state->video0_fd, cam_packet_handle); + mm.free(csiphy_info); + mm.free(pkt); } // link devices @@ -781,6 +796,18 @@ void CameraState::camera_open() { ret = device_control(multi_cam_state->isp_fd, CAM_START_DEV, session_handle, isp_dev_handle); LOGD("start isp: %d", ret); + for (int i = 0; i < FRAME_BUF_COUNT; i++) { + // configure ISP to put the image in place + struct cam_mem_mgr_map_cmd mem_mgr_map_cmd = {0}; + mem_mgr_map_cmd.mmu_hdls[0] = multi_cam_state->device_iommu; + mem_mgr_map_cmd.num_hdl = 1; + mem_mgr_map_cmd.flags = CAM_MEM_FLAG_HW_READ_WRITE; + mem_mgr_map_cmd.fd = buf.camera_bufs[i].fd; + ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_MAP_BUF, &mem_mgr_map_cmd, sizeof(mem_mgr_map_cmd)); + LOGD("map buf req: (fd: %d) 0x%x %d", buf.camera_bufs[i].fd, mem_mgr_map_cmd.out.buf_handle, ret); + buf_handle[i] = mem_mgr_map_cmd.out.buf_handle; + } + // TODO: this is unneeded, should we be doing the start i2c in a different way? //ret = device_control(sensor_fd, CAM_START_DEV, session_handle, sensor_dev_handle); //LOGD("start sensor: %d", ret); @@ -807,9 +834,9 @@ void cameras_open(MultiCameraState *s) { LOGD("opened video0"); // video1 is cam_sync, the target of some ioctls - s->video1_fd = HANDLE_EINTR(open("/dev/v4l/by-path/platform-cam_sync-video-index0", O_RDWR | O_NONBLOCK)); - assert(s->video1_fd >= 0); - LOGD("opened video1"); + s->cam_sync_fd = HANDLE_EINTR(open("/dev/v4l/by-path/platform-cam_sync-video-index0", O_RDWR | O_NONBLOCK)); + assert(s->cam_sync_fd >= 0); + LOGD("opened video1 (cam_sync)"); // looks like there's only one of these s->isp_fd = open_v4l_by_name_and_index("cam-isp"); @@ -834,7 +861,7 @@ void cameras_open(MultiCameraState *s) { LOG("-- Subscribing"); static struct v4l2_event_subscription sub = {0}; sub.type = V4L_EVENT_CAM_REQ_MGR_EVENT; - sub.id = 2; // should use boot time for sof + sub.id = V4L_EVENT_CAM_REQ_MGR_SOF_BOOT_TS; ret = HANDLE_EINTR(ioctl(s->video0_fd, VIDIOC_SUBSCRIBE_EVENT, &sub)); printf("req mgr subscribe: %d\n", ret); @@ -883,6 +910,11 @@ void CameraState::camera_close() { LOGD("release isp: %d", ret); ret = device_control(csiphy_fd, CAM_RELEASE_DEV, session_handle, csiphy_dev_handle); LOGD("release csiphy: %d", ret); + + for (int i = 0; i < FRAME_BUF_COUNT; i++) { + release(multi_cam_state->video0_fd, buf_handle[i]); + } + LOGD("released buffers"); } ret = device_control(sensor_fd, CAM_RELEASE_DEV, session_handle, sensor_dev_handle); diff --git a/selfdrive/camerad/cameras/camera_qcom2.h b/selfdrive/camerad/cameras/camera_qcom2.h index 199aa40e7..a7c64c066 100644 --- a/selfdrive/camerad/cameras/camera_qcom2.h +++ b/selfdrive/camerad/cameras/camera_qcom2.h @@ -9,6 +9,20 @@ #define FRAME_BUF_COUNT 4 +class MemoryManager { + public: + void init(int _video0_fd) { video0_fd = _video0_fd; } + void *alloc(int len, uint32_t *handle); + void free(void *ptr); + ~MemoryManager(); + private: + std::mutex lock; + std::map handle_lookup; + std::map size_lookup; + std::map > cached_allocations; + int video0_fd; +}; + class CameraState { public: MultiCameraState *multi_cam_state; @@ -60,6 +74,7 @@ public: int camera_id; CameraBuf buf; + MemoryManager mm; private: void config_isp(int io_mem_handle, int fence, int request_id, int buf0_mem_handle, int buf0_offset); void enqueue_req_multi(int start, int n, bool dp); @@ -73,7 +88,7 @@ private: typedef struct MultiCameraState { unique_fd video0_fd; - unique_fd video1_fd; + unique_fd cam_sync_fd; unique_fd isp_fd; int device_iommu; int cdm_iommu; diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index c8c1a3437..b845ee0e0 100755 --- a/selfdrive/test/test_onroad.py +++ b/selfdrive/test/test_onroad.py @@ -23,7 +23,7 @@ from tools.lib.logreader import LogReader PROCS = { "selfdrive.controls.controlsd": 31.0, "./loggerd": 70.0, - "./camerad": 41.0, + "./camerad": 26.0, "./locationd": 9.1, "selfdrive.controls.plannerd": 11.7, "./_ui": 18.4, From 1810909195c3b49467fb158cf972c054ac2bc42c Mon Sep 17 00:00:00 2001 From: Jason Young <46612682+jyoung8607@users.noreply.github.com> Date: Fri, 29 Apr 2022 13:25:34 -0400 Subject: [PATCH 16/21] VW MQB: Add FW for 2019 Volkswagen Tiguan (#24373) --- docs/CARS.md | 2 +- selfdrive/car/volkswagen/values.py | 5 ++++- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/docs/CARS.md b/docs/CARS.md index 436728650..14664d480 100644 --- a/docs/CARS.md +++ b/docs/CARS.md @@ -149,7 +149,7 @@ How We Rate The Cars |Volkswagen|T-Cross 2021[8](#footnotes)|Driver Assistance|||||| |Volkswagen|T-Roc 2021[8](#footnotes)|Driver Assistance|||||| |Volkswagen|Taos 2022[8](#footnotes)|Driver Assistance|||||| -|Volkswagen|Tiguan 2020-22[8](#footnotes)|Driver Assistance|||||| +|Volkswagen|Tiguan 2019-22[8](#footnotes)|Driver Assistance|||||| |Volkswagen|Touran 2017|Driver Assistance|||||| ## Bronze Cars diff --git a/selfdrive/car/volkswagen/values.py b/selfdrive/car/volkswagen/values.py index 89637c184..3555b2908 100755 --- a/selfdrive/car/volkswagen/values.py +++ b/selfdrive/car/volkswagen/values.py @@ -133,7 +133,7 @@ CAR_INFO: Dict[str, Union[VWCarInfo, List[VWCarInfo]]] = { CAR.POLO_MK6: VWCarInfo("Volkswagen Polo 2020"), CAR.TAOS_MK1: VWCarInfo("Volkswagen Taos 2022", footnotes=[Footnote.VW_HARNESS]), CAR.TCROSS_MK1: VWCarInfo("Volkswagen T-Cross 2021", footnotes=[Footnote.VW_HARNESS]), - CAR.TIGUAN_MK2: VWCarInfo("Volkswagen Tiguan 2020-22", footnotes=[Footnote.VW_HARNESS]), + CAR.TIGUAN_MK2: VWCarInfo("Volkswagen Tiguan 2019-22", footnotes=[Footnote.VW_HARNESS]), CAR.TOURAN_MK2: VWCarInfo("Volkswagen Touran 2017"), CAR.TRANSPORTER_T61: [ VWCarInfo("Volkswagen Caravelle 2020", footnotes=[Footnote.VW_HARNESS]), @@ -517,6 +517,7 @@ FW_VERSIONS = { b'\xf1\x8704L906026EJ\xf1\x893661', b'\xf1\x8704L906027G \xf1\x899893', b'\xf1\x875N0906259 \xf1\x890002', + b'\xf1\x875NA907115E \xf1\x890005', b'\xf1\x8783A907115B \xf1\x890005', b'\xf1\x8783A907115G \xf1\x890001', ], @@ -527,6 +528,7 @@ FW_VERSIONS = { b'\xf1\x870DL300011N \xf1\x892001', b'\xf1\x870DL300011N \xf1\x892012', b'\xf1\x870DL300013A \xf1\x893005', + b'\xf1\x870DL300013G \xf1\x892119', b'\xf1\x870DL300013G \xf1\x892120', ], (Ecu.srs, 0x715, None): [ @@ -534,6 +536,7 @@ FW_VERSIONS = { b'\xf1\x875Q0959655BM\xf1\x890403\xf1\x82\02316143231313500314641011750179333423100', b'\xf1\x875Q0959655BT\xf1\x890403\xf1\x82\02312110031333300314240583752379333423100', b'\xf1\x875Q0959655BT\xf1\x890403\xf1\x82\02331310031333336313140013950399333423100', + b'\xf1\x875Q0959655BT\xf1\x890403\xf1\x82\x1331310031333334313140573752379333423100', b'\xf1\x875Q0959655CB\xf1\x890421\xf1\x82\x1316143231313500314647021750179333613100', ], (Ecu.eps, 0x712, None): [ From a3cfa444d37bacdac5278335ac7a49b6cb5b81f0 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Fri, 29 Apr 2022 19:55:44 +0200 Subject: [PATCH 17/21] ui: combine OnroadHud into NvgWindow (#24369) * ui: combine OnroadHUD and NVGWindow * draw hud first * cleanup * removed commented line * fix text rendering * increase cpu usage --- selfdrive/test/test_onroad.py | 2 +- selfdrive/ui/qt/onroad.cc | 42 +++++++----------- selfdrive/ui/qt/onroad.h | 64 +++++++++++---------------- selfdrive/ui/qt/widgets/cameraview.cc | 1 - 4 files changed, 44 insertions(+), 65 deletions(-) diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index b845ee0e0..0fea95d0f 100755 --- a/selfdrive/test/test_onroad.py +++ b/selfdrive/test/test_onroad.py @@ -26,7 +26,7 @@ PROCS = { "./camerad": 26.0, "./locationd": 9.1, "selfdrive.controls.plannerd": 11.7, - "./_ui": 18.4, + "./_ui": 21.0, "selfdrive.locationd.paramsd": 9.0, "./_sensord": 6.17, "selfdrive.controls.radard": 4.5, diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 844095a11..cee347daa 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -18,18 +18,13 @@ OnroadWindow::OnroadWindow(QWidget *parent) : QWidget(parent) { stacked_layout->setStackingMode(QStackedLayout::StackAll); main_layout->addLayout(stacked_layout); - QStackedLayout *road_view_layout = new QStackedLayout; - road_view_layout->setStackingMode(QStackedLayout::StackAll); nvg = new NvgWindow(VISION_STREAM_ROAD, this); - road_view_layout->addWidget(nvg); - hud = new OnroadHud(this); - road_view_layout->addWidget(hud); QWidget * split_wrapper = new QWidget; split = new QHBoxLayout(split_wrapper); split->setContentsMargins(0, 0, 0, 0); split->setSpacing(0); - split->addLayout(road_view_layout); + split->addWidget(nvg); stacked_layout->addWidget(split_wrapper); @@ -57,7 +52,7 @@ void OnroadWindow::updateState(const UIState &s) { alerts->updateAlert(alert, bgColor); } - hud->updateState(s); + nvg->updateState(s); if (bg != bgColor) { // repaint border @@ -167,15 +162,14 @@ void OnroadAlerts::paintEvent(QPaintEvent *event) { } } -// OnroadHud -OnroadHud::OnroadHud(QWidget *parent) : QWidget(parent) { +// NvgWindow + +NvgWindow::NvgWindow(VisionStreamType type, QWidget* parent) : fps_filter(UI_FREQ, 3, 1. / UI_FREQ), CameraViewWidget("camerad", type, true, parent) { engage_img = loadPixmap("../assets/img_chffr_wheel.png", {img_size, img_size}); dm_img = loadPixmap("../assets/img_driver_face.png", {img_size, img_size}); - - connect(this, &OnroadHud::valueChanged, [=] { update(); }); } -void OnroadHud::updateState(const UIState &s) { +void NvgWindow::updateState(const UIState &s) { const int SET_SPEED_NA = 255; const SubMaster &sm = *(s.sm); const auto cs = sm["controlsState"].getControlsState(); @@ -202,9 +196,8 @@ void OnroadHud::updateState(const UIState &s) { } } -void OnroadHud::paintEvent(QPaintEvent *event) { - QPainter p(this); - p.setRenderHint(QPainter::Antialiasing); +void NvgWindow::drawHud(QPainter &p) { + p.save(); // Header gradient QLinearGradient bg(0, header_h - (header_h / 2.5), 0, header_h); @@ -246,9 +239,10 @@ void OnroadHud::paintEvent(QPaintEvent *event) { drawIcon(p, radius / 2 + (bdr_s * 2), rect().bottom() - footer_h / 2, dm_img, QColor(0, 0, 0, 70), dmActive ? 1.0 : 0.2); } + p.restore(); } -void OnroadHud::drawText(QPainter &p, int x, int y, const QString &text, int alpha) { +void NvgWindow::drawText(QPainter &p, int x, int y, const QString &text, int alpha) { QFontMetrics fm(p.font()); QRect init_rect = fm.boundingRect(text); QRect real_rect = fm.boundingRect(init_rect, 0, text); @@ -258,7 +252,7 @@ void OnroadHud::drawText(QPainter &p, int x, int y, const QString &text, int alp p.drawText(real_rect.x(), real_rect.bottom(), text); } -void OnroadHud::drawIcon(QPainter &p, int x, int y, QPixmap &img, QBrush bg, float opacity) { +void NvgWindow::drawIcon(QPainter &p, int x, int y, QPixmap &img, QBrush bg, float opacity) { p.setPen(Qt::NoPen); p.setBrush(bg); p.drawEllipse(x - radius / 2, y - radius / 2, radius, radius); @@ -266,11 +260,6 @@ void OnroadHud::drawIcon(QPainter &p, int x, int y, QPixmap &img, QBrush bg, flo p.drawPixmap(x - img_size / 2, y - img_size / 2, img); } -// NvgWindow - -NvgWindow::NvgWindow(VisionStreamType type, QWidget* parent) : fps_filter(UI_FREQ, 3, 1. / UI_FREQ), CameraViewWidget("camerad", type, true, parent) { - -} void NvgWindow::initializeGL() { CameraViewWidget::initializeGL(); @@ -377,11 +366,14 @@ void NvgWindow::drawLead(QPainter &painter, const cereal::ModelDataV2::LeadDataV void NvgWindow::paintGL() { CameraViewWidget::paintGL(); + QPainter painter(this); + painter.setRenderHint(QPainter::Antialiasing); + painter.setPen(Qt::NoPen); + + drawHud(painter); + UIState *s = uiState(); if (s->worldObjectsVisible()) { - QPainter painter(this); - painter.setRenderHint(QPainter::Antialiasing); - painter.setPen(Qt::NoPen); drawLaneLines(painter, s); diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index 1d95fa49f..f4c62d1c7 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -9,26 +9,40 @@ // ***** onroad widgets ***** +class OnroadAlerts : public QWidget { + Q_OBJECT + +public: + OnroadAlerts(QWidget *parent = 0) : QWidget(parent) {}; + void updateAlert(const Alert &a, const QColor &color); -class OnroadHud : public QWidget { +protected: + void paintEvent(QPaintEvent*) override; + +private: + QColor bg; + Alert alert = {}; +}; + +// container window for the NVG UI +class NvgWindow : public CameraViewWidget { Q_OBJECT - Q_PROPERTY(QString speed MEMBER speed NOTIFY valueChanged); - Q_PROPERTY(QString speedUnit MEMBER speedUnit NOTIFY valueChanged); - Q_PROPERTY(QString maxSpeed MEMBER maxSpeed NOTIFY valueChanged); - Q_PROPERTY(bool is_cruise_set MEMBER is_cruise_set NOTIFY valueChanged); - Q_PROPERTY(bool engageable MEMBER engageable NOTIFY valueChanged); - Q_PROPERTY(bool dmActive MEMBER dmActive NOTIFY valueChanged); - Q_PROPERTY(bool hideDM MEMBER hideDM NOTIFY valueChanged); - Q_PROPERTY(int status MEMBER status NOTIFY valueChanged); + Q_PROPERTY(QString speed MEMBER speed); + Q_PROPERTY(QString speedUnit MEMBER speedUnit); + Q_PROPERTY(QString maxSpeed MEMBER maxSpeed); + Q_PROPERTY(bool is_cruise_set MEMBER is_cruise_set); + Q_PROPERTY(bool engageable MEMBER engageable); + Q_PROPERTY(bool dmActive MEMBER dmActive); + Q_PROPERTY(bool hideDM MEMBER hideDM); + Q_PROPERTY(int status MEMBER status); public: - explicit OnroadHud(QWidget *parent); + explicit NvgWindow(VisionStreamType type, QWidget* parent = 0); void updateState(const UIState &s); private: void drawIcon(QPainter &p, int x, int y, QPixmap &img, QBrush bg, float opacity); void drawText(QPainter &p, int x, int y, const QString &text, int alpha = 255); - void paintEvent(QPaintEvent *event) override; QPixmap engage_img; QPixmap dm_img; @@ -43,32 +57,6 @@ private: bool hideDM = false; int status = STATUS_DISENGAGED; -signals: - void valueChanged(); -}; - -class OnroadAlerts : public QWidget { - Q_OBJECT - -public: - OnroadAlerts(QWidget *parent = 0) : QWidget(parent) {}; - void updateAlert(const Alert &a, const QColor &color); - -protected: - void paintEvent(QPaintEvent*) override; - -private: - QColor bg; - Alert alert = {}; -}; - -// container window for the NVG UI -class NvgWindow : public CameraViewWidget { - Q_OBJECT - -public: - explicit NvgWindow(VisionStreamType type, QWidget* parent = 0); - protected: void paintGL() override; void initializeGL() override; @@ -76,6 +64,7 @@ protected: void updateFrameMat(int w, int h) override; void drawLaneLines(QPainter &painter, const UIState *s); void drawLead(QPainter &painter, const cereal::ModelDataV2::LeadDataV3::Reader &lead_data, const QPointF &vd); + void drawHud(QPainter &p); inline QColor redColor(int alpha = 255) { return QColor(201, 34, 49, alpha); } inline QColor whiteColor(int alpha = 255) { return QColor(255, 255, 255, alpha); } @@ -94,7 +83,6 @@ public: private: void paintEvent(QPaintEvent *event); void mousePressEvent(QMouseEvent* e) override; - OnroadHud *hud; OnroadAlerts *alerts; NvgWindow *nvg; QColor bg = bg_colors[STATUS_DISENGAGED]; diff --git a/selfdrive/ui/qt/widgets/cameraview.cc b/selfdrive/ui/qt/widgets/cameraview.cc index 4ef9ef137..d817d5556 100644 --- a/selfdrive/ui/qt/widgets/cameraview.cc +++ b/selfdrive/ui/qt/widgets/cameraview.cc @@ -253,7 +253,6 @@ void CameraViewWidget::vipcConnected(VisionIpcClient *vipc_client) { stream_width = vipc_client->buffers[0].width; stream_height = vipc_client->buffers[0].height; - glPixelStorei(GL_UNPACK_ALIGNMENT, 1); for (int i = 0; i < 3; ++i) { glBindTexture(GL_TEXTURE_2D, textures[i]); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); From c971061859e5f9ff571afb5b6ae7d8e9f2b0ef63 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Fri, 29 Apr 2022 10:56:39 -0700 Subject: [PATCH 18/21] power draw test improvements (#24368) Co-authored-by: Comma Device --- selfdrive/hardware/tici/test_power_draw.py | 43 +++++++++++++--------- 1 file changed, 26 insertions(+), 17 deletions(-) diff --git a/selfdrive/hardware/tici/test_power_draw.py b/selfdrive/hardware/tici/test_power_draw.py index fee799503..4261e1044 100755 --- a/selfdrive/hardware/tici/test_power_draw.py +++ b/selfdrive/hardware/tici/test_power_draw.py @@ -2,19 +2,28 @@ import unittest import time import math -from collections import OrderedDict +from dataclasses import dataclass from selfdrive.hardware import HARDWARE, TICI from selfdrive.hardware.tici.power_monitor import get_power from selfdrive.manager.process_config import managed_processes from selfdrive.manager.manager import manager_cleanup -POWER = OrderedDict( - camerad=2.58, - modeld=0.90, - dmonitoringmodeld=0.25, - loggerd=0.45, -) + +@dataclass +class Proc: + name: str + power: float + rtol: float = 0.05 + atol: float = 0.1 + warmup: float = 3. + +PROCS = [ + Proc('camerad', 2.5), + Proc('modeld', 0.95), + Proc('dmonitoringmodeld', 0.25), + Proc('loggerd', 0.45, warmup=10.), +] class TestPowerDraw(unittest.TestCase): @@ -36,24 +45,24 @@ class TestPowerDraw(unittest.TestCase): prev = baseline used = {} - for proc in POWER.keys(): - managed_processes[proc].start() - time.sleep(6) + for proc in PROCS: + managed_processes[proc.name].start() + time.sleep(proc.warmup) now = get_power(8) - used[proc] = now - prev + used[proc.name] = now - prev prev = now manager_cleanup() print("-"*35) print(f"Baseline {baseline:.2f}W\n") - for proc in POWER.keys(): - cur = used[proc] - expected = POWER[proc] - print(f"{proc.ljust(20)} {expected:.2f}W {cur:.2f}W") - with self.subTest(proc=proc): - self.assertTrue(math.isclose(cur, expected, rel_tol=0.10, abs_tol=0.1)) + for proc in PROCS: + cur = used[proc.name] + expected = proc.power + print(f"{proc.name.ljust(20)} {expected:.2f}W {cur:.2f}W") + with self.subTest(proc=proc.name): + self.assertTrue(math.isclose(cur, expected, rel_tol=proc.rtol, abs_tol=proc.atol)) print("-"*35) From 1439867d16295a289af753c581617e55e684188e Mon Sep 17 00:00:00 2001 From: Lukas Petersson Date: Fri, 29 Apr 2022 13:32:21 -0700 Subject: [PATCH 19/21] Latency logger bad data robustness (#24343) * robustness against bad data * sort by time logreader * loop until last frameid --- tools/latencylogger/latency_logger.py | 35 +++++++++++++++++++-------- tools/lib/logreader.py | 4 +-- 2 files changed, 27 insertions(+), 12 deletions(-) diff --git a/tools/latencylogger/latency_logger.py b/tools/latencylogger/latency_logger.py index 47c434716..b03dc84f6 100644 --- a/tools/latencylogger/latency_logger.py +++ b/tools/latencylogger/latency_logger.py @@ -8,7 +8,7 @@ from collections import defaultdict from tools.lib.logreader import logreader_from_route_or_segment -DEMO_ROUTE = "9f583b1d93915c31|2022-04-06--11-34-03" +DEMO_ROUTE = "9f583b1d93915c31|2022-04-26--18-49-49" SERVICES = ['camerad', 'modeld', 'plannerd', 'controlsd', 'boardd'] # Retrive controlsd frameId from lateralPlan, mismatch with longitudinalPlan will be ignored @@ -32,6 +32,7 @@ def read_logs(lr): data = defaultdict(lambda: defaultdict(lambda: defaultdict(list))) mono_to_frame = {} frame_mismatches = [] + frame_id_fails = 0 latest_sendcan_monotime = 0 for msg in lr: if msg.which() == 'sendcan': @@ -56,6 +57,9 @@ def read_logs(lr): frame_id = mono_to_frame[getattr(msg_obj, key)] if continue_outer: continue + if frame_id == -1: + frame_id_fails += 1 + continue mono_to_frame[msg.logMonoTime] = frame_id data['timestamp'][frame_id][service].append((msg.which()+" published", msg.logMonoTime)) @@ -79,9 +83,7 @@ def read_logs(lr): if msg_obj.frameIdExtra != frame_id: frame_mismatches.append(frame_id) - # Failed frameId fetches are stored in -1 - assert sum([len(data['timestamp'][-1][service]) for service in data['timestamp'][-1].keys()]) < 20, "Too many frameId fetch fails" - del data['timestamp'][-1] + assert frame_id_fails < 20, "Too many frameId fetch fails" assert len(frame_mismatches) < 20, "Too many frame mismatches" return (data, frame_mismatches) @@ -91,9 +93,20 @@ def find_frame_id(time, service, start_times, end_times): if start_times[frame_id][service] <= time <= end_times[frame_id][service]: yield frame_id +def find_t0(start_times, frame_id=-1): + frame_id = frame_id if frame_id > -1 else min(start_times.keys()) + m = max(start_times.keys()) + while frame_id <= m: + for service in SERVICES: + if service in start_times[frame_id]: + return start_times[frame_id][service] + frame_id += 1 + raise Exception('No start time has been set') + + ## ASSUMES THAT AT LEAST ONE CLOUDLOG IS MADE IN CONTROLSD def insert_cloudlogs(lr, timestamps, start_times, end_times): - t0 = start_times[min(start_times.keys())][SERVICES[0]] + t0 = find_t0(start_times) failed_inserts = 0 latest_controls_frameid = 0 for msg in lr: @@ -129,12 +142,13 @@ def insert_cloudlogs(lr, timestamps, start_times, end_times): assert failed_inserts < len(timestamps), "Too many failed cloudlog inserts" def print_timestamps(timestamps, durations, start_times, relative): - t0 = start_times[min(start_times.keys())][SERVICES[0]] + t0 = find_t0(start_times) for frame_id in timestamps.keys(): print('='*80) print("Frame ID:", frame_id) if relative: - t0 = start_times[frame_id][SERVICES[0]] + t0 = find_t0(start_times, frame_id) + for service in SERVICES: print(" "+service) events = timestamps[frame_id][service] @@ -144,10 +158,11 @@ def print_timestamps(timestamps, durations, start_times, relative): print(" "+'%-53s%-53s' %(event, str(time*1000))) def graph_timestamps(timestamps, start_times, end_times, relative): - t0 = start_times[min(start_times.keys())][SERVICES[0]] + t0 = find_t0(start_times) + y0 = min(start_times.keys()) fig, ax = plt.subplots() ax.set_xlim(0, 150 if relative else 750) - ax.set_ylim(0, 15) + ax.set_ylim(y0, y0+15) ax.set_xlabel('milliseconds') ax.set_ylabel('Frame ID') colors = ['blue', 'green', 'red', 'yellow', 'purple'] @@ -156,7 +171,7 @@ def graph_timestamps(timestamps, start_times, end_times, relative): points = {"x": [], "y": [], "labels": []} for frame_id, services in timestamps.items(): if relative: - t0 = start_times[frame_id][SERVICES[0]] + t0 = find_t0(start_times, frame_id) service_bars = [] for service, events in services.items(): if start_times[frame_id][service] and end_times[frame_id][service]: diff --git a/tools/lib/logreader.py b/tools/lib/logreader.py index 06dd248bf..1ad947470 100755 --- a/tools/lib/logreader.py +++ b/tools/lib/logreader.py @@ -107,9 +107,9 @@ def logreader_from_route_or_segment(r, sort_by_time=False): sn = SegmentName(r, allow_route_name=True) route = Route(sn.route_name.canonical_name) if sn.segment_num < 0: - return MultiLogIterator(route.log_paths(), sort_by_time) + return MultiLogIterator(route.log_paths(), sort_by_time=sort_by_time) else: - return LogReader(route.log_paths()[sn.segment_num], sort_by_time) + return LogReader(route.log_paths()[sn.segment_num], sort_by_time=sort_by_time) if __name__ == "__main__": From be748c0d6a1a931a26b4ec741bbba71d4269b821 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Fri, 29 Apr 2022 13:36:25 -0700 Subject: [PATCH 20/21] controlsd: better alert precedence for system malfunctions (#24366) * controlsd: better alert precedence for system malfunctions * down to 20 * move that * check can flags * update refs * update refs --- selfdrive/controls/controlsd.py | 75 +++++++++++++----------- selfdrive/test/process_replay/ref_commit | 2 +- 2 files changed, 42 insertions(+), 35 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 958de4599..6dff3bb2e 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -59,7 +59,7 @@ ENABLED_STATES = (State.preEnabled, *ACTIVE_STATES) class Controls: def __init__(self, sm=None, pm=None, can_sock=None, CI=None): - config_realtime_process(4 if TICI else 3, Priority.CTRL_HIGH) + config_realtime_process(4, Priority.CTRL_HIGH) # Setup sockets self.pm = pm @@ -73,7 +73,7 @@ class Controls: self.can_sock = can_sock if can_sock is None: - can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100 + can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 20 self.can_sock = messaging.sub_sock('can', timeout=can_timeout) if TICI: @@ -162,7 +162,7 @@ class Controls: self.last_functional_fan_frame = 0 self.events_prev = [] self.current_alert_types = [ET.PERMANENT] - self.logged_comm_issue = False + self.logged_comm_issue = None self.button_timers = {ButtonEvent.Type.decelCruise: 0, ButtonEvent.Type.accelCruise: 0} self.last_actuators = car.CarControl.Actuators.new_message() @@ -213,11 +213,17 @@ class Controls: self.events.add(EventName.pedalPressedPreEnable if self.disengage_on_accelerator else EventName.gasPressedOverride) - self.events.add_from_msg(CS.events) - if not self.CP.notCar: self.events.add_from_msg(self.sm['driverMonitoringState'].events) + # Handle car events. Ignore when CAN is invalid + if CS.canTimeout: + self.events.add(EventName.canBusMissing) + elif not CS.canValid: + self.events.add(EventName.canError) + else: + self.events.add_from_msg(CS.events) + # Create events for temperature, disk space, and memory if self.sm['deviceState'].thermalStatus >= ThermalStatus.red: self.events.add(EventName.overheat) @@ -225,7 +231,7 @@ class Controls: # under 7% of space free no enable allowed self.events.add(EventName.outOfSpace) # TODO: make tici threshold the same - if self.sm['deviceState'].memoryUsagePercent > (90 if TICI else 65) and not SIMULATION: + if self.sm['deviceState'].memoryUsagePercent > 90 and not SIMULATION: self.events.add(EventName.lowMemory) # TODO: enable this once loggerd CPU usage is more reasonable @@ -234,7 +240,7 @@ class Controls: # self.events.add(EventName.highCpuUsage) # Alert if fan isn't spinning for 5 seconds - if self.sm['peripheralState'].pandaType in (PandaType.uno, PandaType.dos): + if self.sm['peripheralState'].pandaType == PandaType.dos: if self.sm['peripheralState'].fanSpeedRpm == 0 and self.sm['deviceState'].fanSpeedPercentDesired > 50: if (self.sm.frame - self.last_functional_fan_frame) * DT_CTRL > 5.0: self.events.add(EventName.fanMalfunction) @@ -264,11 +270,6 @@ class Controls: LaneChangeState.laneChangeFinishing): self.events.add(EventName.laneChange) - if CS.canTimeout: - self.events.add(EventName.canBusMissing) - elif not CS.canValid: - self.events.add(EventName.canError) - for i, pandaState in enumerate(self.sm['pandaStates']): # All pandas must match the list of safetyConfigs, and if outside this list, must be silent or noOutput if i < len(self.CP.safetyConfigs): @@ -284,15 +285,29 @@ class Controls: if log.PandaState.FaultType.relayMalfunction in pandaState.faults: self.events.add(EventName.relayMalfunction) - # Check for HW or system issues + # Handle HW and system malfunctions + # Order is very intentional here. Be careful when modifying this. + num_events = len(self.events) + + not_running = {p.name for p in self.sm['managerState'].processes if not p.running and p.shouldBeRunning} + if self.sm.rcv_frame['managerState'] and (not_running - IGNORE_PROCESSES): + self.events.add(EventName.processNotRunning) + else: + if not SIMULATION and not self.rk.lagging: + if not self.sm.all_alive(self.camera_packets): + self.events.add(EventName.cameraMalfunction) + elif not self.sm.all_freq_ok(self.camera_packets): + self.events.add(EventName.cameraFrameRate) if self.rk.lagging: self.events.add(EventName.controlsdLagging) - elif len(self.sm['radarState'].radarErrors): + if len(self.sm['radarState'].radarErrors): self.events.add(EventName.radarFault) - elif not self.sm.valid['pandaStates']: + if not self.sm.valid['pandaStates']: self.events.add(EventName.usbError) - elif not self.sm.all_checks() or self.can_rcv_error: + # generic catch-all. ideally, a more specific event should be added above instead + no_system_errors = len(self.events) != num_events + if (not self.sm.all_checks() or self.can_rcv_error) and no_system_errors and CS.canValid and not CS.canTimeout: if not self.sm.all_alive(): self.events.add(EventName.commIssue) elif not self.sm.all_freq_ok(): @@ -300,14 +315,17 @@ class Controls: else: # invalid or can_rcv_error. self.events.add(EventName.commIssue) - if not self.logged_comm_issue: - invalid = [s for s, valid in self.sm.valid.items() if not valid] - not_alive = [s for s, alive in self.sm.alive.items() if not alive] - not_freq_ok = [s for s, freq_ok in self.sm.freq_ok.items() if not freq_ok] - cloudlog.event("commIssue", invalid=invalid, not_alive=not_alive, not_freq_ok=not_freq_ok, can_error=self.can_rcv_error, error=True) - self.logged_comm_issue = True + logs = { + 'invalid': [s for s, valid in self.sm.valid.items() if not valid], + 'not_alive': [s for s, alive in self.sm.alive.items() if not alive], + 'not_freq_ok': [s for s, freq_ok in self.sm.freq_ok.items() if not freq_ok], + 'can_error': self.can_rcv_error, + } + if logs != self.logged_comm_issue: + cloudlog.event("commIssue", error=True, **logs) + self.logged_comm_issue = logs else: - self.logged_comm_issue = False + self.logged_comm_issue = None if not self.sm['liveParameters'].valid: self.events.add(EventName.vehicleModelInvalid) @@ -354,22 +372,11 @@ class Controls: # Not show in first 1 km to allow for driving out of garage. This event shows after 5 minutes self.events.add(EventName.noGps) - if not self.rk.lagging: - if not self.sm.all_alive(self.camera_packets): - self.events.add(EventName.cameraMalfunction) - elif not self.sm.all_freq_ok(self.camera_packets): - self.events.add(EventName.cameraFrameRate) - if self.sm['modelV2'].frameDropPerc > 20: self.events.add(EventName.modeldLagging) if self.sm['liveLocationKalman'].excessiveResets: self.events.add(EventName.localizerMalfunction) - # Check if all manager processes are running - not_running = {p.name for p in self.sm['managerState'].processes if not p.running and p.shouldBeRunning} - if self.sm.rcv_frame['managerState'] and (not_running - IGNORE_PROCESSES): - self.events.add(EventName.processNotRunning) - # Only allow engagement with brake pressed when stopped behind another stopped car speeds = self.sm['longitudinalPlan'].speeds if len(speeds) > 1: diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 39c8add0e..b870f9846 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -e37c2ea1447363af1403e8260450c30e2d862101 \ No newline at end of file +d17ecea1f071007193a8a1e1ececf78c96b9deac \ No newline at end of file From 41f62f7604f70ff77742d70f4089a38112b71a70 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Fri, 29 Apr 2022 14:40:29 -0700 Subject: [PATCH 21/21] Toyota: Add FW versions for RAV4 Hybrid 2022 (#24374) --- selfdrive/car/toyota/values.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 68e8b36f3..2d9948b9a 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -1336,14 +1336,17 @@ FW_VERSIONS = { (Ecu.esp, 0x7b0, None): [ b'\x01F15264283100\x00\x00\x00\x00', b'\x01F15264286200\x00\x00\x00\x00', + b'\x01F15264286100\x00\x00\x00\x00', ], (Ecu.eps, 0x7a1, None): [ b'\x028965B0R01500\x00\x00\x00\x008965B0R02500\x00\x00\x00\x00', b'8965B42182\x00\x00\x00\x00\x00\x00', + b'8965B42172\x00\x00\x00\x00\x00\x00', ], (Ecu.engine, 0x700, None): [ b'\x01896634A62000\x00\x00\x00\x00', b'\x01896634A08000\x00\x00\x00\x00', + b'\x01896634A61000\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ b'\x018821F0R01100\x00\x00\x00\x00',