From 8252134e62440ae2d19e6dce20ff9475389cdd06 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Sun, 13 Mar 2022 16:18:01 -0700 Subject: [PATCH 01/17] install pre-commit hooks for all submodules --- update_requirements.sh | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/update_requirements.sh b/update_requirements.sh index ac9472dca2..dd5d933526 100755 --- a/update_requirements.sh +++ b/update_requirements.sh @@ -1,5 +1,4 @@ #!/bin/bash - set -e DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )" @@ -45,10 +44,8 @@ echo "pip packages install..." pipenv install --dev --deploy --clear pyenv rehash -if [ -f "$DIR/.pre-commit-config.yaml" ]; then - echo "precommit install ..." +echo "pre-commit hooks install..." +for f in .pre-commit-config.yaml */.pre-commit-config.yaml; do + cd $DIR/$(dirname $f) $RUN pre-commit install - [ -d "./xx" ] && (cd xx && $RUN pre-commit install) - [ -d "./notebooks" ] && (cd notebooks && $RUN pre-commit install) - echo "pre-commit hooks installed" -fi +done From d6f6e024b3fca004dd93b07892a31470ce2c8cf3 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Sun, 13 Mar 2022 16:37:53 -0700 Subject: [PATCH 02/17] no git in CI --- update_requirements.sh | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/update_requirements.sh b/update_requirements.sh index dd5d933526..4925e48bb0 100755 --- a/update_requirements.sh +++ b/update_requirements.sh @@ -47,5 +47,7 @@ pyenv rehash echo "pre-commit hooks install..." for f in .pre-commit-config.yaml */.pre-commit-config.yaml; do cd $DIR/$(dirname $f) - $RUN pre-commit install + if [ -e ".git" ]; then + $RUN pre-commit install + fi done From a958213ec75bf7f4020ea786e903156a678abd43 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Sun, 13 Mar 2022 20:00:23 -0700 Subject: [PATCH 03/17] fix pre-commit install when subdirectories don't have a config --- update_requirements.sh | 1 + 1 file changed, 1 insertion(+) diff --git a/update_requirements.sh b/update_requirements.sh index 4925e48bb0..94b14496f1 100755 --- a/update_requirements.sh +++ b/update_requirements.sh @@ -45,6 +45,7 @@ pipenv install --dev --deploy --clear pyenv rehash echo "pre-commit hooks install..." +shopt -s nullglob for f in .pre-commit-config.yaml */.pre-commit-config.yaml; do cd $DIR/$(dirname $f) if [ -e ".git" ]; then From b9484a6a99b4abe3a0c60d83a0ad1b43926cba45 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Sun, 13 Mar 2022 20:22:49 -0700 Subject: [PATCH 04/17] cars: remove duplicated cancel logic (#23956) --- selfdrive/car/mazda/carcontroller.py | 2 +- selfdrive/car/nissan/carcontroller.py | 5 ----- selfdrive/car/tesla/carcontroller.py | 4 ---- selfdrive/car/volkswagen/carcontroller.py | 2 +- 4 files changed, 2 insertions(+), 11 deletions(-) diff --git a/selfdrive/car/mazda/carcontroller.py b/selfdrive/car/mazda/carcontroller.py index 60bb620377..b6e4726c7f 100644 --- a/selfdrive/car/mazda/carcontroller.py +++ b/selfdrive/car/mazda/carcontroller.py @@ -32,7 +32,7 @@ class CarController(): # TODO: improve the resume trigger logic by looking at actual radar data can_sends.append(mazdacan.create_button_cmd(self.packer, CS.CP.carFingerprint, CS.crz_btns_counter, Buttons.RESUME)) - if c.cruiseControl.cancel or (CS.out.cruiseState.enabled and not c.enabled): + if c.cruiseControl.cancel: # If brake is pressed, let us wait >70ms before trying to disable crz to avoid # a race condition with the stock system, where the second cancel from openpilot # will disable the crz 'main on'. crz ctrl msg runs at 50hz. 70ms allows us to diff --git a/selfdrive/car/nissan/carcontroller.py b/selfdrive/car/nissan/carcontroller.py index 8b30c11249..eceb0cc75a 100644 --- a/selfdrive/car/nissan/carcontroller.py +++ b/selfdrive/car/nissan/carcontroller.py @@ -24,7 +24,6 @@ class CarController(): can_sends = [] ### STEER ### - acc_active = CS.out.cruiseState.enabled lkas_hud_msg = CS.lkas_hud_msg lkas_hud_info_msg = CS.lkas_hud_info_msg apply_angle = actuators.steeringAngleDeg @@ -58,10 +57,6 @@ class CarController(): self.last_angle = apply_angle - if not enabled and acc_active: - # send acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated - cruise_cancel = 1 - if self.CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA) and cruise_cancel: can_sends.append(nissancan.create_acc_cancel_cmd(self.packer, self.car_fingerprint, CS.cruise_throttle_msg, frame)) diff --git a/selfdrive/car/tesla/carcontroller.py b/selfdrive/car/tesla/carcontroller.py index 03f09f2407..69caf5876e 100644 --- a/selfdrive/car/tesla/carcontroller.py +++ b/selfdrive/car/tesla/carcontroller.py @@ -50,10 +50,6 @@ class CarController(): if hands_on_fault: cruise_cancel = True - # Cancel when openpilot is not enabled anymore - if not enabled and bool(CS.out.cruiseState.enabled): - cruise_cancel = True - if ((frame % 10) == 0 and cruise_cancel): # Spam every possible counter value, otherwise it might not be accepted for counter in range(16): diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index 4400d050ce..63d79aaf05 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -88,7 +88,7 @@ class CarController(): if CS.CP.pcmCruise: if frame > self.graMsgStartFramePrev + P.GRA_VBP_STEP: - if not enabled and CS.out.cruiseState.enabled: + if c.cruiseControl.cancel: # Cancel ACC if it's engaged with OP disengaged. self.graButtonStatesToSend = BUTTON_STATES.copy() self.graButtonStatesToSend["cancel"] = True From 629399a44925080b2c35b6dde35c093cc2e32035 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sun, 13 Mar 2022 20:58:44 -0700 Subject: [PATCH 05/17] carControl: add long and lat active fields (#23859) * proof of concept * actuators packet describes which actuators are active * bump cereal * fixes * not needed for this PR * Do Toyota * add back controlsState.active * bump cereal * rest of cars * in actuators * add active back * which * use controlsState.active for now * will make an issue * Update selfdrive/controls/controlsd.py Co-authored-by: Adeeb Shihadeh * move local lat_active checks into CC.latActive * remove redundant checks * move comment move comment * fix joystick mode * get enabled from carcontrol * do standstill check in controlsd * make sure we consider the gas press case for GM * use CC.actuators * fix * capitalization * Bump cereal Bump cereal * make intermediate actuators * similar convention to before * clean that up * update refs Co-authored-by: Adeeb Shihadeh --- cereal | 2 +- selfdrive/car/ford/interface.py | 2 +- selfdrive/car/gm/carcontroller.py | 15 +++--- selfdrive/car/gm/interface.py | 7 +-- selfdrive/car/honda/carcontroller.py | 14 +++--- selfdrive/car/honda/interface.py | 8 ++-- selfdrive/car/hyundai/carcontroller.py | 19 ++++---- selfdrive/car/hyundai/interface.py | 6 +-- selfdrive/car/mazda/carcontroller.py | 2 +- selfdrive/car/nissan/carcontroller.py | 8 ++-- selfdrive/car/nissan/interface.py | 2 +- selfdrive/car/subaru/carcontroller.py | 6 +-- selfdrive/car/subaru/interface.py | 2 +- selfdrive/car/tesla/carcontroller.py | 4 +- selfdrive/car/tesla/interface.py | 2 +- selfdrive/car/toyota/carcontroller.py | 10 ++-- selfdrive/car/toyota/interface.py | 2 +- selfdrive/car/volkswagen/carcontroller.py | 8 ++-- selfdrive/car/volkswagen/interface.py | 2 +- selfdrive/controls/controlsd.py | 57 ++++++++++++----------- selfdrive/test/process_replay/ref_commit | 2 +- 21 files changed, 86 insertions(+), 94 deletions(-) diff --git a/cereal b/cereal index 279311d0bd..84a1793eb4 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 279311d0bde60e814a697da22c55c6abe8a3b03c +Subproject commit 84a1793eb4d09821f23613b98d80e2ce4ee57b9c diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index d792e5fd55..fc6843c826 100755 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -66,7 +66,7 @@ class CarInterface(CarInterfaceBase): def apply(self, c): ret = self.CC.update(c.enabled, self.CS, self.frame, c.actuators, - c.hudControl.visualAlert, c.cruiseControl.cancel) + c.hudControl.visualAlert, c.cruiseControl.cancel) self.frame += 1 return ret diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index 93c28a1cfc..39e348ff29 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -27,8 +27,7 @@ class CarController(): self.packer_obj = CANPacker(DBC[CP.carFingerprint]['radar']) self.packer_ch = CANPacker(DBC[CP.carFingerprint]['chassis']) - def update(self, c, enabled, CS, frame, actuators, - hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert): + def update(self, c, CS, frame, actuators, hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert): P = self.params @@ -41,7 +40,7 @@ class CarController(): if CS.lka_steering_cmd_counter != self.lka_steering_cmd_counter_last: self.lka_steering_cmd_counter_last = CS.lka_steering_cmd_counter elif (frame % P.STEER_STEP) == 0: - lkas_enabled = c.active and not (CS.out.steerFaultTemporary or CS.out.steerFaultPermanent) and CS.out.vEgo > P.MIN_STEER_SPEED + lkas_enabled = c.latActive and CS.out.vEgo > P.MIN_STEER_SPEED if lkas_enabled: new_steer = int(round(actuators.steer * P.STEER_MAX)) apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, P) @@ -58,7 +57,7 @@ class CarController(): # Gas/regen and brakes - all at 25Hz if (frame % 4) == 0: - if not c.active: + if not c.longActive: # Stock ECU sends max regen when not enabled. self.apply_gas = P.MAX_ACC_REGEN self.apply_brake = 0 @@ -68,15 +67,15 @@ class CarController(): idx = (frame // 4) % 4 - at_full_stop = enabled and CS.out.standstill - near_stop = enabled and (CS.out.vEgo < P.NEAR_STOP_BRAKE_PHASE) + at_full_stop = c.longActive and CS.out.standstill + near_stop = c.longActive and (CS.out.vEgo < P.NEAR_STOP_BRAKE_PHASE) can_sends.append(gmcan.create_friction_brake_command(self.packer_ch, CanBus.CHASSIS, self.apply_brake, idx, near_stop, at_full_stop)) - can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, self.apply_gas, idx, enabled, at_full_stop)) + can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, self.apply_gas, idx, c.longActive, at_full_stop)) # Send dashboard UI commands (ACC status), 25hz if (frame % 4) == 0: send_fcw = hud_alert == VisualAlert.fcw - can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, CanBus.POWERTRAIN, enabled, hud_v_cruise * CV.MS_TO_KPH, hud_show_car, send_fcw)) + can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, CanBus.POWERTRAIN, c.longActive, hud_v_cruise * CV.MS_TO_KPH, hud_show_car, send_fcw)) # Radar needs to know current speed and yaw rate (50hz), # and that ADAS is alive (10hz) diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index d6a2d3cfee..0aad0a7f04 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -222,12 +222,7 @@ class CarInterface(CarInterfaceBase): if hud_v_cruise > 70: hud_v_cruise = 0 - # For Openpilot, "enabled" includes pre-enable. - # In GM, PCM faults out if ACC command overlaps user gas. - enabled = c.enabled and not self.CS.out.gasPressed - - ret = self.CC.update(c, enabled, self.CS, self.frame, - c.actuators, + ret = self.CC.update(c, self.CS, self.frame, c.actuators, hud_v_cruise, hud_control.lanesVisible, hud_control.leadVisible, hud_control.visualAlert) diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 3ba8b64dd3..bf5456bc4b 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -112,12 +112,12 @@ class CarController(): self.params = CarControllerParams(CP) - def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd, + def update(self, c, CS, frame, actuators, pcm_cancel_cmd, hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert): P = self.params - if active: + if c.longActive: accel = actuators.accel gas, brake = compute_gas_brake(actuators.accel, CS.out.vEgo, CS.CP.carFingerprint) else: @@ -136,7 +136,7 @@ class CarController(): else: hud_lanes = 0 - if enabled: + if c.enabled: if hud_show_car: hud_car = 2 else: @@ -152,8 +152,6 @@ class CarController(): # steer torque is converted back to CAN reference (positive when steering right) apply_steer = int(interp(-actuators.steer * P.STEER_MAX, P.STEER_LOOKUP_BP, P.STEER_LOOKUP_V)) - lkas_active = active - # Send CAN commands. can_sends = [] @@ -165,7 +163,7 @@ class CarController(): # Send steering command. idx = frame % 4 can_sends.append(hondacan.create_steering_control(self.packer, apply_steer, - lkas_active, CS.CP.carFingerprint, idx, CS.CP.openpilotLongitudinalControl)) + c.latActive, CS.CP.carFingerprint, idx, CS.CP.openpilotLongitudinalControl)) stopping = actuators.longControlState == LongCtrlState.stopping @@ -217,7 +215,7 @@ class CarController(): if CS.CP.carFingerprint in HONDA_BOSCH: self.accel = clip(accel, P.BOSCH_ACCEL_MIN, P.BOSCH_ACCEL_MAX) self.gas = interp(accel, P.BOSCH_GAS_LOOKUP_BP, P.BOSCH_GAS_LOOKUP_V) - can_sends.extend(hondacan.create_acc_commands(self.packer, enabled, active, accel, self.gas, idx, stopping, CS.CP.carFingerprint)) + can_sends.extend(hondacan.create_acc_commands(self.packer, c.enabled, c.longActive, accel, self.gas, idx, stopping, CS.CP.carFingerprint)) else: apply_brake = clip(self.brake_last - wind_brake, 0.0, 1.0) apply_brake = int(clip(apply_brake * P.NIDEC_BRAKE_MAX, 0, P.NIDEC_BRAKE_MAX - 1)) @@ -236,7 +234,7 @@ class CarController(): # This prevents unexpected pedal range rescaling # Sending non-zero gas when OP is not enabled will cause the PCM not to respond to throttle as expected # when you do enable. - if active: + if c.longActive: self.gas = clip(gas_mult * (gas - brake + wind_brake*3/4), 0., 1.) else: self.gas = 0.0 diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 94e4305909..9f09f8a719 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -427,11 +427,9 @@ class CarInterface(CarInterfaceBase): else: hud_v_cruise = 255 - ret = self.CC.update(c.enabled, c.active, self.CS, self.frame, - c.actuators, - c.cruiseControl.cancel, - hud_v_cruise, - hud_control.lanesVisible, + ret = self.CC.update(c, self.CS, self.frame, + c.actuators, c.cruiseControl.cancel, + hud_v_cruise, hud_control.lanesVisible, hud_show_car=hud_control.leadVisible, hud_alert=hud_control.visualAlert) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 8572465b6b..55fc442f6d 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -46,23 +46,20 @@ class CarController(): self.last_resume_frame = 0 self.accel = 0 - def update(self, c, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, hud_speed, + def update(self, c, CS, frame, actuators, pcm_cancel_cmd, visual_alert, hud_speed, left_lane, right_lane, left_lane_depart, right_lane_depart): # Steering Torque new_steer = int(round(actuators.steer * self.p.STEER_MAX)) apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.p) self.steer_rate_limited = new_steer != apply_steer - # disable when temp fault is active, or below LKA minimum speed - lkas_active = c.active and not CS.out.steerFaultTemporary and CS.out.vEgo >= CS.CP.minSteerSpeed - - if not lkas_active: + if not c.latActive: apply_steer = 0 self.apply_steer_last = apply_steer sys_warning, sys_state, left_lane_warning, right_lane_warning = \ - process_hud_alert(enabled, self.car_fingerprint, visual_alert, + process_hud_alert(c.enabled, self.car_fingerprint, visual_alert, left_lane, right_lane, left_lane_depart, right_lane_depart) can_sends = [] @@ -72,8 +69,8 @@ class CarController(): if (frame % 100) == 0: can_sends.append([0x7D0, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", 0]) - can_sends.append(create_lkas11(self.packer, frame, self.car_fingerprint, apply_steer, lkas_active, - CS.lkas11, sys_warning, sys_state, enabled, + can_sends.append(create_lkas11(self.packer, frame, self.car_fingerprint, apply_steer, c.latActive, + CS.lkas11, sys_warning, sys_state, c.enabled, left_lane, right_lane, left_lane_warning, right_lane_warning)) @@ -89,7 +86,7 @@ class CarController(): if frame % 2 == 0 and CS.CP.openpilotLongitudinalControl: lead_visible = False - accel = actuators.accel if c.active else 0 + accel = actuators.accel if c.longActive else 0 jerk = clip(2.0 * (accel - CS.out.aEgo), -12.7, 12.7) @@ -100,7 +97,7 @@ class CarController(): stopping = (actuators.longControlState == LongCtrlState.stopping) set_speed_in_units = hud_speed * (CV.MS_TO_MPH if CS.clu11["CF_Clu_SPEED_UNIT"] == 1 else CV.MS_TO_KPH) - can_sends.extend(create_acc_commands(self.packer, enabled, accel, jerk, int(frame / 2), lead_visible, set_speed_in_units, stopping)) + can_sends.extend(create_acc_commands(self.packer, c.enabled, accel, jerk, int(frame / 2), lead_visible, set_speed_in_units, stopping)) self.accel = accel # 20 Hz LFA MFA message @@ -108,7 +105,7 @@ class CarController(): CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_CEED, CAR.KIA_SELTOS, CAR.KONA_EV, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.GENESIS_G70_2020, CAR.SANTA_FE_PHEV_2022): - can_sends.append(create_lfahda_mfc(self.packer, enabled)) + can_sends.append(create_lfahda_mfc(self.packer, c.enabled)) # 5 Hz ACC options if frame % 20 == 0 and CS.CP.openpilotLongitudinalControl: diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 379e6937ae..cda94e1346 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -352,8 +352,8 @@ class CarInterface(CarInterfaceBase): def apply(self, c): hud_control = c.hudControl - ret = self.CC.update(c, c.enabled, self.CS, self.frame, c.actuators, - c.cruiseControl.cancel, hud_control.visualAlert, hud_control.setSpeed, hud_control.leftLaneVisible, - hud_control.rightLaneVisible, hud_control.leftLaneDepart, hud_control.rightLaneDepart) + ret = self.CC.update(c, self.CS, self.frame, c.actuators, c.cruiseControl.cancel, hud_control.visualAlert, + hud_control.setSpeed, hud_control.leftLaneVisible, hud_control.rightLaneVisible, + hud_control.leftLaneDepart, hud_control.rightLaneDepart) self.frame += 1 return ret diff --git a/selfdrive/car/mazda/carcontroller.py b/selfdrive/car/mazda/carcontroller.py index b6e4726c7f..5f3d41ae34 100644 --- a/selfdrive/car/mazda/carcontroller.py +++ b/selfdrive/car/mazda/carcontroller.py @@ -19,7 +19,7 @@ class CarController(): apply_steer = 0 self.steer_rate_limited = False - if c.active: + if c.latActive: # calculate steer and also set limits due to driver torque new_steer = int(round(c.actuators.steer * CarControllerParams.STEER_MAX)) apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, diff --git a/selfdrive/car/nissan/carcontroller.py b/selfdrive/car/nissan/carcontroller.py index eceb0cc75a..15cd44be92 100644 --- a/selfdrive/car/nissan/carcontroller.py +++ b/selfdrive/car/nissan/carcontroller.py @@ -18,7 +18,7 @@ class CarController(): self.packer = CANPacker(dbc_name) - def update(self, c, enabled, CS, frame, actuators, cruise_cancel, hud_alert, + def update(self, c, CS, frame, actuators, cruise_cancel, hud_alert, left_line, right_line, left_lane_depart, right_lane_depart): can_sends = [] @@ -30,7 +30,7 @@ class CarController(): steer_hud_alert = 1 if hud_alert in (VisualAlert.steerRequired, VisualAlert.ldw) else 0 - if c.active: + if c.latActive: # # windup slower if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle): angle_rate_lim = interp(CS.out.vEgo, CarControllerParams.ANGLE_DELTA_BP, CarControllerParams.ANGLE_DELTA_V) @@ -68,12 +68,12 @@ class CarController(): can_sends.append(nissancan.create_cancel_msg(self.packer, CS.cancel_msg, cruise_cancel)) can_sends.append(nissancan.create_steering_control( - self.packer, apply_angle, frame, enabled, self.lkas_max_torque)) + self.packer, apply_angle, frame, c.enabled, self.lkas_max_torque)) if lkas_hud_msg and lkas_hud_info_msg: if frame % 2 == 0: can_sends.append(nissancan.create_lkas_hud_msg( - self.packer, lkas_hud_msg, enabled, left_line, right_line, left_lane_depart, right_lane_depart)) + self.packer, lkas_hud_msg, c.enabled, left_line, right_line, left_lane_depart, right_lane_depart)) if frame % 50 == 0: can_sends.append(nissancan.create_lkas_hud_info_msg( diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py index c32fb13780..63b4dad7fd 100644 --- a/selfdrive/car/nissan/interface.py +++ b/selfdrive/car/nissan/interface.py @@ -79,7 +79,7 @@ class CarInterface(CarInterfaceBase): def apply(self, c): hud_control = c.hudControl - ret = self.CC.update(c, c.enabled, self.CS, self.frame, c.actuators, + ret = self.CC.update(c, self.CS, self.frame, c.actuators, c.cruiseControl.cancel, hud_control.visualAlert, hud_control.leftLaneVisible, hud_control.rightLaneVisible, hud_control.leftLaneDepart, hud_control.rightLaneDepart) diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index afc91f4755..fd439356e9 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -15,7 +15,7 @@ class CarController(): self.p = CarControllerParams(CP) self.packer = CANPacker(DBC[CP.carFingerprint]['pt']) - def update(self, c, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart): + def update(self, c, CS, frame, actuators, pcm_cancel_cmd, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart): can_sends = [] @@ -30,7 +30,7 @@ class CarController(): apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.p) self.steer_rate_limited = new_steer != apply_steer - if not c.active: + if not c.latActive: apply_steer = 0 if CS.CP.carFingerprint in PREGLOBAL_CARS: @@ -69,7 +69,7 @@ class CarController(): self.es_distance_cnt = CS.es_distance_msg["Counter"] if self.es_lkas_cnt != CS.es_lkas_msg["Counter"]: - can_sends.append(subarucan.create_es_lkas(self.packer, CS.es_lkas_msg, enabled, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart)) + can_sends.append(subarucan.create_es_lkas(self.packer, CS.es_lkas_msg, c.enabled, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart)) self.es_lkas_cnt = CS.es_lkas_msg["Counter"] new_actuators = actuators.copy() diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index 8c6d188643..fdd077ac49 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -123,7 +123,7 @@ class CarInterface(CarInterfaceBase): def apply(self, c): hud_control = c.hudControl - ret = self.CC.update(c, c.enabled, self.CS, self.frame, c.actuators, + ret = self.CC.update(c, self.CS, self.frame, c.actuators, c.cruiseControl.cancel, hud_control.visualAlert, hud_control.leftLaneVisible, hud_control.rightLaneVisible, hud_control.leftLaneDepart, hud_control.rightLaneDepart) self.frame += 1 diff --git a/selfdrive/car/tesla/carcontroller.py b/selfdrive/car/tesla/carcontroller.py index 69caf5876e..d5a5bb8629 100644 --- a/selfdrive/car/tesla/carcontroller.py +++ b/selfdrive/car/tesla/carcontroller.py @@ -12,12 +12,12 @@ class CarController(): self.pt_packer = CANPacker(DBC[CP.carFingerprint]['pt']) self.tesla_can = TeslaCAN(self.packer, self.pt_packer) - def update(self, c, enabled, CS, frame, actuators, cruise_cancel): + def update(self, c, CS, frame, actuators, cruise_cancel): can_sends = [] # Temp disable steering on a hands_on_fault, and allow for user override hands_on_fault = (CS.steer_warning == "EAC_ERROR_HANDS_ON" and CS.hands_on_level >= 3) - lkas_enabled = c.active and (not hands_on_fault) + lkas_enabled = c.latActive and (not hands_on_fault) if lkas_enabled: apply_angle = actuators.steeringAngleDeg diff --git a/selfdrive/car/tesla/interface.py b/selfdrive/car/tesla/interface.py index 746cd356ea..3bd1a0efca 100755 --- a/selfdrive/car/tesla/interface.py +++ b/selfdrive/car/tesla/interface.py @@ -71,6 +71,6 @@ class CarInterface(CarInterfaceBase): return self.CS.out def apply(self, c): - ret = self.CC.update(c, c.enabled, self.CS, self.frame, c.actuators, c.cruiseControl.cancel) + ret = self.CC.update(c, self.CS, self.frame, c.actuators, c.cruiseControl.cancel) self.frame += 1 return ret diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 87ba0055f0..5ec69488d5 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -22,11 +22,11 @@ class CarController(): self.gas = 0 self.accel = 0 - def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd, hud_alert, + def update(self, c, CS, frame, actuators, pcm_cancel_cmd, hud_alert, left_line, right_line, lead, left_lane_depart, right_lane_depart): # gas and brake - if CS.CP.enableGasInterceptor and active: + if CS.CP.enableGasInterceptor and c.longActive: MAX_INTERCEPTOR_GAS = 0.5 # RAV4 has very sensitive gas pedal if CS.CP.carFingerprint in (CAR.RAV4, CAR.RAV4H, CAR.HIGHLANDER, CAR.HIGHLANDERH): @@ -49,7 +49,7 @@ class CarController(): self.steer_rate_limited = new_steer != apply_steer # Cut steering while we're in a known fault state (2s) - if not active or CS.steer_state in (9, 25): + if not c.latActive or CS.steer_state in (9, 25): apply_steer = 0 apply_steer_req = 0 else: @@ -57,7 +57,7 @@ class CarController(): # TODO: probably can delete this. CS.pcm_acc_status uses a different signal # than CS.cruiseState.enabled. confirm they're not meaningfully different - if not enabled and CS.pcm_acc_status: + if not c.enabled and CS.pcm_acc_status: pcm_cancel_cmd = 1 # on entering standstill, send standstill request @@ -122,7 +122,7 @@ class CarController(): send_ui = True if (frame % 100 == 0 or send_ui): - can_sends.append(create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, left_line, right_line, left_lane_depart, right_lane_depart, enabled)) + can_sends.append(create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, left_line, right_line, left_lane_depart, right_lane_depart, c.enabled)) if frame % 100 == 0 and CS.CP.enableDsu: can_sends.append(create_fcw_command(self.packer, fcw_alert)) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 18e15bfefd..b6a01bcc46 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -273,7 +273,7 @@ class CarInterface(CarInterfaceBase): # to be called @ 100hz def apply(self, c): hud_control = c.hudControl - ret = self.CC.update(c.enabled, c.active, self.CS, self.frame, + ret = self.CC.update(c, self.CS, self.frame, c.actuators, c.cruiseControl.cancel, hud_control.visualAlert, hud_control.leftLaneVisible, hud_control.rightLaneVisible, hud_control.leadVisible, diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index 63d79aaf05..5726c98285 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -21,7 +21,7 @@ class CarController(): self.steer_rate_limited = False - def update(self, c, enabled, CS, frame, ext_bus, actuators, visual_alert, left_lane_visible, right_lane_visible, left_lane_depart, right_lane_depart): + def update(self, c, CS, frame, ext_bus, actuators, visual_alert, left_lane_visible, right_lane_visible, left_lane_depart, right_lane_depart): """ Controls thread """ can_sends = [] @@ -39,7 +39,7 @@ class CarController(): # torque value. Do that anytime we happen to have 0 torque, or failing that, # when exceeding ~1/3 the 360 second timer. - if c.active and CS.out.vEgo > CS.CP.minSteerSpeed and not (CS.out.standstill or CS.out.steerFaultPermanent or CS.out.steerFaultTemporary): + if c.latActive: new_steer = int(round(actuators.steer * P.STEER_MAX)) apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, P) self.steer_rate_limited = new_steer != apply_steer @@ -77,7 +77,7 @@ class CarController(): else: hud_alert = MQB_LDW_MESSAGES["none"] - can_sends.append(volkswagencan.create_mqb_hud_control(self.packer_pt, CANBUS.pt, enabled, + can_sends.append(volkswagencan.create_mqb_hud_control(self.packer_pt, CANBUS.pt, c.enabled, CS.out.steeringPressed, hud_alert, left_lane_visible, right_lane_visible, CS.ldw_stock_values, left_lane_depart, right_lane_depart)) @@ -92,7 +92,7 @@ class CarController(): # Cancel ACC if it's engaged with OP disengaged. self.graButtonStatesToSend = BUTTON_STATES.copy() self.graButtonStatesToSend["cancel"] = True - elif enabled and CS.esp_hold_confirmation: + elif c.enabled and CS.esp_hold_confirmation: # Blip the Resume button if we're engaged at standstill. # FIXME: This is a naive implementation, improve with visiond or radar input. self.graButtonStatesToSend = BUTTON_STATES.copy() diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index 961cfed7fe..976c4459e3 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -211,7 +211,7 @@ class CarInterface(CarInterfaceBase): def apply(self, c): hud_control = c.hudControl - ret = self.CC.update(c, c.enabled, self.CS, self.frame, self.ext_bus, c.actuators, + ret = self.CC.update(c, self.CS, self.frame, self.ext_bus, c.actuators, hud_control.visualAlert, hud_control.leftLaneVisible, hud_control.rightLaneVisible, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 7887c2428b..cf403b50b2 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -459,7 +459,7 @@ class Controls: self.v_cruise_kph = initialize_v_cruise(CS.vEgo, CS.buttonEvents, self.v_cruise_kph_last) # Check if actuators are enabled - self.active = self.state == State.enabled or self.state == State.softDisabling + self.active = self.state in (State.enabled, State.softDisabling) if self.active: self.current_alert_types.append(ET.WARNING) @@ -467,7 +467,7 @@ class Controls: self.enabled = self.active or self.state == State.preEnabled def state_control(self, CS): - """Given the state, this function returns an actuators packet""" + """Given the state, this function returns a CarControl packet""" # Update VehicleModel params = self.sm['liveParameters'] @@ -478,7 +478,14 @@ class Controls: lat_plan = self.sm['lateralPlan'] long_plan = self.sm['longitudinalPlan'] - actuators = car.CarControl.Actuators.new_message() + CC = car.CarControl.new_message() + CC.enabled = self.enabled + # Check which actuators can be enabled + CC.latActive = self.active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \ + CS.vEgo > self.CP.minSteerSpeed and not CS.standstill + CC.longActive = self.active + + actuators = CC.actuators actuators.longControlState = self.LoC.long_control_state if CS.leftBlinker or CS.rightBlinker: @@ -486,37 +493,40 @@ class Controls: # State specific actions - if not self.active: + if not CC.latActive: self.LaC.reset() + if not CC.longActive: self.LoC.reset(v_pid=CS.vEgo) if not self.joystick_mode: # 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(self.active, CS, self.CP, long_plan, pid_accel_limits, t_since_plan) + actuators.accel = self.LoC.update(CC.longActive, CS, self.CP, long_plan, pid_accel_limits, t_since_plan) # Steering PID loop and lateral MPC - lat_active = self.active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and CS.vEgo > self.CP.minSteerSpeed 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(lat_active, CS, self.CP, self.VM, params, self.last_actuators, - desired_curvature, desired_curvature_rate) + actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.CP, self.VM, + params, self.last_actuators, desired_curvature, + desired_curvature_rate) else: lac_log = log.ControlsState.LateralDebugState.new_message() - if self.sm.rcv_frame['testJoystick'] > 0 and self.active: - actuators.accel = 4.0*clip(self.sm['testJoystick'].axes[0], -1, 1) + if self.sm.rcv_frame['testJoystick'] > 0: + if CC.longActive: + actuators.accel = 4.0*clip(self.sm['testJoystick'].axes[0], -1, 1) - steer = clip(self.sm['testJoystick'].axes[1], -1, 1) - # max angle is 45 for angle-based cars - actuators.steer, actuators.steeringAngleDeg = steer, steer * 45. + if CC.latActive: + steer = clip(self.sm['testJoystick'].axes[1], -1, 1) + # max angle is 45 for angle-based cars + actuators.steer, actuators.steeringAngleDeg = steer, steer * 45. - lac_log.active = True + lac_log.active = self.active lac_log.steeringAngleDeg = CS.steeringAngleDeg - lac_log.output = steer - lac_log.saturated = abs(steer) >= 0.9 + lac_log.output = actuators.steer + lac_log.saturated = abs(actuators.steer) >= 0.9 # Send a "steering required alert" if saturation count has reached the limit if lac_log.active and lac_log.saturated and not CS.steeringPressed: @@ -540,7 +550,7 @@ class Controls: cloudlog.error(f"actuators.{p} not finite {actuators.to_dict()}") setattr(actuators, p, 0.0) - return actuators, lac_log + return CC, lac_log def update_button_timers(self, buttonEvents): # increment timer for buttons still pressed @@ -552,14 +562,9 @@ class Controls: if b.type.raw in self.button_timers: self.button_timers[b.type.raw] = 1 if b.pressed else 0 - def publish_logs(self, CS, start_time, actuators, lac_log): + def publish_logs(self, CS, start_time, CC, lac_log): """Send actuators and hud commands to the car, send controlsstate and MPC logging""" - CC = car.CarControl.new_message() - CC.enabled = self.enabled - CC.active = self.active - CC.actuators = actuators - orientation_value = self.sm['liveLocationKalman'].orientationNED.value if len(orientation_value) > 2: CC.roll = orientation_value[0] @@ -580,7 +585,7 @@ class Controls: recent_blinker = (self.sm.frame - self.last_blinker_frame) * DT_CTRL < 5.0 # 5s blinker cooldown ldw_allowed = self.is_ldw_enabled and CS.vEgo > LDW_MIN_SPEED and not recent_blinker \ - and not self.active and self.sm['liveCalibration'].calStatus == Calibration.CALIBRATED + and not CC.latActive and self.sm['liveCalibration'].calStatus == Calibration.CALIBRATED model_v2 = self.sm['modelV2'] desire_prediction = model_v2.meta.desirePrediction @@ -719,12 +724,12 @@ class Controls: self.prof.checkpoint("State transition") # Compute actuators (runs PID loops and lateral MPC) - actuators, lac_log = self.state_control(CS) + CC, lac_log = self.state_control(CS) self.prof.checkpoint("State Control") # Publish data - self.publish_logs(CS, start_time, actuators, lac_log) + self.publish_logs(CS, start_time, CC, lac_log) self.prof.checkpoint("Sent") self.update_button_timers(CS.buttonEvents) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 1993b05fb7..d81fe18ac1 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -595f118aa4f6eebacaef4db77179c108ec725483 \ No newline at end of file +c6d7afb13c183852f8d61d5f79495b828ed401ed \ No newline at end of file From 8ac30fea24e3816622d854615b55a24440c4e53f Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Sun, 13 Mar 2022 21:21:30 -0700 Subject: [PATCH 06/17] move pandad to boardd/ (#23958) --- release/files_common | 2 +- selfdrive/{ => boardd}/pandad.py | 0 selfdrive/manager/process_config.py | 2 +- 3 files changed, 2 insertions(+), 2 deletions(-) rename selfdrive/{ => boardd}/pandad.py (100%) diff --git a/release/files_common b/release/files_common index 7073625cfb..e69d58520d 100644 --- a/release/files_common +++ b/release/files_common @@ -74,7 +74,6 @@ selfdrive/sentry.py selfdrive/swaglog.py selfdrive/logmessaged.py selfdrive/tombstoned.py -selfdrive/pandad.py selfdrive/updated.py selfdrive/rtshield.py selfdrive/statsd.py @@ -98,6 +97,7 @@ selfdrive/boardd/panda.h selfdrive/boardd/pigeon.cc selfdrive/boardd/pigeon.h selfdrive/boardd/set_time.py +selfdrive/boardd/pandad.py selfdrive/car/__init__.py selfdrive/car/car_helpers.py diff --git a/selfdrive/pandad.py b/selfdrive/boardd/pandad.py similarity index 100% rename from selfdrive/pandad.py rename to selfdrive/boardd/pandad.py diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py index c017e48fab..bc88154c2c 100644 --- a/selfdrive/manager/process_config.py +++ b/selfdrive/manager/process_config.py @@ -27,7 +27,7 @@ procs = [ PythonProcess("deleter", "selfdrive.loggerd.deleter", persistent=True), PythonProcess("dmonitoringd", "selfdrive.monitoring.dmonitoringd", enabled=(not PC or WEBCAM), driverview=True), PythonProcess("logmessaged", "selfdrive.logmessaged", persistent=True), - PythonProcess("pandad", "selfdrive.pandad", persistent=True), + PythonProcess("pandad", "selfdrive.boardd.pandad", persistent=True), PythonProcess("paramsd", "selfdrive.locationd.paramsd"), PythonProcess("plannerd", "selfdrive.controls.plannerd"), PythonProcess("radard", "selfdrive.controls.radard"), From 8f99e59c926e172bc2528af03aeca8d1a30eb0db Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Sun, 13 Mar 2022 21:26:24 -0700 Subject: [PATCH 07/17] bump panda --- panda | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panda b/panda index 234e436e93..5a7af82f06 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 234e436e93f4dbd084fd912fe9b10d504ca9882e +Subproject commit 5a7af82f06bf5f8039df8f58f9b1114f7d3436ee From 5c3f3e44bc7b25c9ce502419bcd0261c12ec2b0d Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Sun, 13 Mar 2022 21:42:39 -0700 Subject: [PATCH 08/17] break up selfdrive.config (#23959) * break up selfdrive.config * add to release files * fix import order --- common/conversions.py | 19 ++++++++++++ release/files_common | 2 +- selfdrive/car/chrysler/carstate.py | 2 +- selfdrive/car/ford/carstate.py | 4 +-- selfdrive/car/ford/interface.py | 2 +- selfdrive/car/ford/radar_interface.py | 2 +- selfdrive/car/gm/carcontroller.py | 4 +-- selfdrive/car/gm/interface.py | 5 ++-- selfdrive/car/gm/radar_interface.py | 2 +- selfdrive/car/honda/carstate.py | 5 ++-- selfdrive/car/honda/hondacan.py | 2 +- selfdrive/car/honda/interface.py | 2 +- selfdrive/car/hyundai/carcontroller.py | 2 +- selfdrive/car/hyundai/carstate.py | 6 ++-- selfdrive/car/hyundai/interface.py | 2 +- selfdrive/car/interfaces.py | 2 +- selfdrive/car/mazda/carstate.py | 2 +- selfdrive/car/mazda/interface.py | 2 +- selfdrive/car/mock/interface.py | 2 +- selfdrive/car/nissan/carstate.py | 2 +- selfdrive/car/subaru/carstate.py | 2 +- selfdrive/car/tesla/carstate.py | 2 +- selfdrive/car/tesla/teslacan.py | 2 +- selfdrive/car/toyota/carstate.py | 4 +-- selfdrive/car/toyota/interface.py | 2 +- selfdrive/car/toyota/values.py | 2 +- selfdrive/car/volkswagen/carstate.py | 2 +- selfdrive/config.py | 29 ------------------- selfdrive/controls/controlsd.py | 2 +- selfdrive/controls/lib/desire_helper.py | 2 +- selfdrive/controls/lib/drive_helpers.py | 2 +- selfdrive/controls/lib/events.py | 2 +- .../controls/lib/longitudinal_planner.py | 2 +- selfdrive/controls/lib/radar_helpers.py | 3 +- selfdrive/controls/radard.py | 3 +- selfdrive/locationd/calibrationd.py | 2 +- tools/replay/lib/ui_helpers.py | 12 ++++++-- tools/replay/ui.py | 3 +- 38 files changed, 74 insertions(+), 75 deletions(-) create mode 100644 common/conversions.py delete mode 100644 selfdrive/config.py diff --git a/common/conversions.py b/common/conversions.py new file mode 100644 index 0000000000..b02b33c625 --- /dev/null +++ b/common/conversions.py @@ -0,0 +1,19 @@ +import numpy as np + +class Conversions: + # Speed + MPH_TO_KPH = 1.609344 + KPH_TO_MPH = 1. / MPH_TO_KPH + MS_TO_KPH = 3.6 + KPH_TO_MS = 1. / MS_TO_KPH + MS_TO_MPH = MS_TO_KPH * KPH_TO_MPH + MPH_TO_MS = MPH_TO_KPH * KPH_TO_MS + MS_TO_KNOTS = 1.9438 + KNOTS_TO_MS = 1. / MS_TO_KNOTS + + # Angle + DEG_TO_RAD = np.pi / 180. + RAD_TO_DEG = 1. / DEG_TO_RAD + + # Mass + LB_TO_KG = 0.453592 diff --git a/release/files_common b/release/files_common index e69d58520d..30029ba0d9 100644 --- a/release/files_common +++ b/release/files_common @@ -17,6 +17,7 @@ site_scons/site_tools/cython.py common/.gitignore common/__init__.py +common/conversions.py common/gpio.py common/realtime.py common/clock.pyx @@ -69,7 +70,6 @@ installer/updater/updater selfdrive/version.py selfdrive/__init__.py -selfdrive/config.py selfdrive/sentry.py selfdrive/swaglog.py selfdrive/logmessaged.py diff --git a/selfdrive/car/chrysler/carstate.py b/selfdrive/car/chrysler/carstate.py index f7dc140e2e..5f83bbde8b 100644 --- a/selfdrive/car/chrysler/carstate.py +++ b/selfdrive/car/chrysler/carstate.py @@ -1,7 +1,7 @@ from cereal import car +from common.conversions import Conversions as CV from opendbc.can.parser import CANParser from opendbc.can.can_define import CANDefine -from selfdrive.config import Conversions as CV from selfdrive.car.interfaces import CarStateBase from selfdrive.car.chrysler.values import DBC, STEER_THRESHOLD diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py index 6324791740..ff82e585b4 100644 --- a/selfdrive/car/ford/carstate.py +++ b/selfdrive/car/ford/carstate.py @@ -1,7 +1,7 @@ from cereal import car -from opendbc.can.parser import CANParser +from common.conversions import Conversions as CV from common.numpy_fast import mean -from selfdrive.config import Conversions as CV +from opendbc.can.parser import CANParser from selfdrive.car.interfaces import CarStateBase from selfdrive.car.ford.values import DBC diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index fc6843c826..5d89950816 100755 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 from cereal import car -from selfdrive.config import Conversions as CV +from common.conversions import Conversions as CV from selfdrive.car.ford.values import MAX_ANGLE 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 diff --git a/selfdrive/car/ford/radar_interface.py b/selfdrive/car/ford/radar_interface.py index 94eb8bb0cc..f8477951cd 100755 --- a/selfdrive/car/ford/radar_interface.py +++ b/selfdrive/car/ford/radar_interface.py @@ -1,8 +1,8 @@ #!/usr/bin/env python3 from cereal import car +from common.conversions import Conversions as CV from opendbc.can.parser import CANParser from selfdrive.car.ford.values import DBC -from selfdrive.config import Conversions as CV from selfdrive.car.interfaces import RadarInterfaceBase RADAR_MSGS = list(range(0x500, 0x540)) diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index 39e348ff29..11c2d367cb 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -1,11 +1,11 @@ from cereal import car +from common.conversions import Conversions as CV from common.realtime import DT_CTRL from common.numpy_fast import interp -from selfdrive.config import Conversions as CV +from opendbc.can.packer import CANPacker from selfdrive.car import apply_std_steer_torque_limits from selfdrive.car.gm import gmcan from selfdrive.car.gm.values import DBC, CanBus, CarControllerParams -from opendbc.can.packer import CANPacker VisualAlert = car.CarControl.HUDControl.VisualAlert diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 0aad0a7f04..7125b4a913 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -1,10 +1,11 @@ #!/usr/bin/env python3 from cereal import car from math import fabs -from selfdrive.config import Conversions as CV + +from common.conversions import Conversions as CV +from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config from selfdrive.car.gm.values import CAR, CruiseButtons, \ AccState, 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 ButtonType = car.CarState.ButtonEvent.Type diff --git a/selfdrive/car/gm/radar_interface.py b/selfdrive/car/gm/radar_interface.py index 66fac54748..6904e6f899 100755 --- a/selfdrive/car/gm/radar_interface.py +++ b/selfdrive/car/gm/radar_interface.py @@ -1,9 +1,9 @@ #!/usr/bin/env python3 import math from cereal import car +from common.conversions import Conversions as CV from opendbc.can.parser import CANParser from selfdrive.car.gm.values import DBC, CAR, CanBus -from selfdrive.config import Conversions as CV from selfdrive.car.interfaces import RadarInterfaceBase RADAR_HEADER_MSG = 1120 diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index 4d09fdcacb..332290373a 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -1,9 +1,10 @@ -from cereal import car from collections import defaultdict + +from cereal import car +from common.conversions import Conversions as CV from common.numpy_fast import interp from opendbc.can.can_define import CANDefine from opendbc.can.parser import CANParser -from selfdrive.config import Conversions as CV from selfdrive.car.interfaces import CarStateBase from selfdrive.car.honda.values import CAR, DBC, STEER_THRESHOLD, HONDA_BOSCH, HONDA_NIDEC_ALT_SCM_MESSAGES, HONDA_BOSCH_ALT_BRAKE_SIGNAL diff --git a/selfdrive/car/honda/hondacan.py b/selfdrive/car/honda/hondacan.py index db7104cd4f..513babf3d1 100644 --- a/selfdrive/car/honda/hondacan.py +++ b/selfdrive/car/honda/hondacan.py @@ -1,5 +1,5 @@ +from common.conversions import Conversions as CV from selfdrive.car.honda.values import HondaFlags, HONDA_BOSCH, CAR, CarControllerParams -from selfdrive.config import Conversions as CV # CAN bus layout with relay # 0 = ACC-CAN - radar side diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 9f09f8a719..2c801f7f15 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -1,13 +1,13 @@ #!/usr/bin/env python3 from cereal import car from panda import Panda +from common.conversions import Conversions as CV from common.numpy_fast import interp from common.params import Params from selfdrive.car.honda.values import CarControllerParams, CruiseButtons, HondaFlags, CAR, HONDA_BOSCH, HONDA_NIDEC_ALT_SCM_MESSAGES, HONDA_BOSCH_ALT_BRAKE_SIGNAL from selfdrive.car import STD_CARGO_KG, CivicParams, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config from selfdrive.car.interfaces import CarInterfaceBase from selfdrive.car.disable_ecu import disable_ecu -from selfdrive.config import Conversions as CV ButtonType = car.CarState.ButtonEvent.Type diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 55fc442f6d..6b2cbd422d 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -1,7 +1,7 @@ from cereal import car from common.realtime import DT_CTRL from common.numpy_fast import clip, interp -from selfdrive.config import Conversions as CV +from common.conversions import Conversions as CV from selfdrive.car import apply_std_steer_torque_limits from selfdrive.car.hyundai.hyundaican import create_lkas11, create_clu11, create_lfahda_mfc, create_acc_commands, create_acc_opt, create_frt_radar_opt from selfdrive.car.hyundai.values import Buttons, CarControllerParams, CAR diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 2356782dde..95c1878e3d 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -1,10 +1,10 @@ import copy from cereal import car -from selfdrive.car.hyundai.values import DBC, STEER_THRESHOLD, FEATURES, EV_CAR, HYBRID_CAR -from selfdrive.car.interfaces import CarStateBase +from common.conversions import Conversions as CV from opendbc.can.parser import CANParser from opendbc.can.can_define import CANDefine -from selfdrive.config import Conversions as CV +from selfdrive.car.hyundai.values import DBC, STEER_THRESHOLD, FEATURES, EV_CAR, HYBRID_CAR +from selfdrive.car.interfaces import CarStateBase class CarState(CarStateBase): diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index cda94e1346..89308c7897 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -2,7 +2,7 @@ from cereal import car from panda import Panda from common.params import Params -from selfdrive.config import Conversions as CV +from common.conversions import Conversions as CV from selfdrive.car.hyundai.values import CAR, EV_CAR, HYBRID_CAR, LEGACY_SAFETY_MODE_CAR, Buttons, CarControllerParams from selfdrive.car.hyundai.radar_interface import RADAR_START_ADDR from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index e6f546acc3..80d6b1db3e 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -7,7 +7,7 @@ from cereal import car from common.kalman.simple_kalman import KF1D from common.realtime import DT_CTRL from selfdrive.car import gen_empty_fingerprint -from selfdrive.config import Conversions as CV +from common.conversions import Conversions as CV from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX from selfdrive.controls.lib.events import Events from selfdrive.controls.lib.vehicle_model import VehicleModel diff --git a/selfdrive/car/mazda/carstate.py b/selfdrive/car/mazda/carstate.py index ea3d420e5d..fa15f2453c 100644 --- a/selfdrive/car/mazda/carstate.py +++ b/selfdrive/car/mazda/carstate.py @@ -1,5 +1,5 @@ from cereal import car -from selfdrive.config import Conversions as CV +from common.conversions import Conversions as CV from opendbc.can.can_define import CANDefine from opendbc.can.parser import CANParser from selfdrive.car.interfaces import CarStateBase diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py index fb8edd6f42..c910d7e148 100755 --- a/selfdrive/car/mazda/interface.py +++ b/selfdrive/car/mazda/interface.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 from cereal import car -from selfdrive.config import Conversions as CV +from common.conversions import Conversions as CV from selfdrive.car.mazda.values import CAR, LKAS_LIMITS 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 diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py index b2e315a5f9..d7fcad0748 100755 --- a/selfdrive/car/mock/interface.py +++ b/selfdrive/car/mock/interface.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 import math from cereal import car -from selfdrive.config import Conversions as CV +from common.conversions import Conversions as CV from selfdrive.swaglog import cloudlog import cereal.messaging as messaging from selfdrive.car import gen_empty_fingerprint, get_safety_config diff --git a/selfdrive/car/nissan/carstate.py b/selfdrive/car/nissan/carstate.py index 6b030e9b45..3c5d7dc24b 100644 --- a/selfdrive/car/nissan/carstate.py +++ b/selfdrive/car/nissan/carstate.py @@ -3,7 +3,7 @@ from collections import deque from cereal import car from opendbc.can.can_define import CANDefine from selfdrive.car.interfaces import CarStateBase -from selfdrive.config import Conversions as CV +from common.conversions import Conversions as CV from opendbc.can.parser import CANParser from selfdrive.car.nissan.values import CAR, DBC, CarControllerParams diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py index ad86131d74..45ea66fb27 100644 --- a/selfdrive/car/subaru/carstate.py +++ b/selfdrive/car/subaru/carstate.py @@ -1,7 +1,7 @@ import copy from cereal import car from opendbc.can.can_define import CANDefine -from selfdrive.config import Conversions as CV +from common.conversions import Conversions as CV from selfdrive.car.interfaces import CarStateBase from opendbc.can.parser import CANParser from selfdrive.car.subaru.values import DBC, STEER_THRESHOLD, CAR, PREGLOBAL_CARS diff --git a/selfdrive/car/tesla/carstate.py b/selfdrive/car/tesla/carstate.py index 484f5d3630..42a7983505 100644 --- a/selfdrive/car/tesla/carstate.py +++ b/selfdrive/car/tesla/carstate.py @@ -1,10 +1,10 @@ import copy from cereal import car +from common.conversions import Conversions as CV from selfdrive.car.tesla.values import DBC, CANBUS, GEAR_MAP, DOORS, BUTTONS from selfdrive.car.interfaces import CarStateBase from opendbc.can.parser import CANParser from opendbc.can.can_define import CANDefine -from selfdrive.config import Conversions as CV class CarState(CarStateBase): def __init__(self, CP): diff --git a/selfdrive/car/tesla/teslacan.py b/selfdrive/car/tesla/teslacan.py index 1301802860..d48cd284d9 100644 --- a/selfdrive/car/tesla/teslacan.py +++ b/selfdrive/car/tesla/teslacan.py @@ -1,6 +1,6 @@ import copy import crcmod -from selfdrive.config import Conversions as CV +from common.conversions import Conversions as CV from selfdrive.car.tesla.values import CANBUS, CarControllerParams diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 8c7aa34c5c..8a6f91324a 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -1,11 +1,11 @@ from cereal import car +from common.conversions import Conversions as CV from common.numpy_fast import mean from common.filter_simple import FirstOrderFilter from common.realtime import DT_CTRL from opendbc.can.can_define import CANDefine -from selfdrive.car.interfaces import CarStateBase from opendbc.can.parser import CANParser -from selfdrive.config import Conversions as CV +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 diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index b6a01bcc46..4ac13d3d97 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 from cereal import car -from selfdrive.config import Conversions as CV +from common.conversions import Conversions as CV 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 import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 31a40a71f8..acf3f42c38 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -2,8 +2,8 @@ from collections import defaultdict from enum import IntFlag from cereal import car +from common.conversions import Conversions as CV from selfdrive.car import dbc_dict -from selfdrive.config import Conversions as CV Ecu = car.CarParams.Ecu MIN_ACC_SPEED = 19. * CV.MPH_TO_MS diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py index d7ec587106..83dbb956f7 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -1,6 +1,6 @@ import numpy as np from cereal import car -from selfdrive.config import Conversions as CV +from common.conversions import Conversions as CV from selfdrive.car.interfaces import CarStateBase from opendbc.can.parser import CANParser from opendbc.can.can_define import CANDefine diff --git a/selfdrive/config.py b/selfdrive/config.py deleted file mode 100644 index 511f6126c4..0000000000 --- a/selfdrive/config.py +++ /dev/null @@ -1,29 +0,0 @@ -import numpy as np - -class Conversions: - #Speed - MPH_TO_KPH = 1.609344 - KPH_TO_MPH = 1. / MPH_TO_KPH - MS_TO_KPH = 3.6 - KPH_TO_MS = 1. / MS_TO_KPH - MS_TO_MPH = MS_TO_KPH * KPH_TO_MPH - MPH_TO_MS = MPH_TO_KPH * KPH_TO_MS - MS_TO_KNOTS = 1.9438 - KNOTS_TO_MS = 1. / MS_TO_KNOTS - #Angle - DEG_TO_RAD = np.pi / 180. - RAD_TO_DEG = 1. / DEG_TO_RAD - #Mass - LB_TO_KG = 0.453592 - - -RADAR_TO_CENTER = 2.7 # (deprecated) RADAR is ~ 2.7m ahead from center of car -RADAR_TO_CAMERA = 1.52 # RADAR is ~ 1.5m ahead from center of mesh frame - -class UIParams: - lidar_x, lidar_y, lidar_zoom = 384, 960, 6 - lidar_car_x, lidar_car_y = lidar_x / 2., lidar_y / 1.1 - car_hwidth = 1.7272 / 2 * lidar_zoom - car_front = 2.6924 * lidar_zoom - car_back = 1.8796 * lidar_zoom - car_color = 110 diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index cf403b50b2..fcb1a4974a 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -9,7 +9,7 @@ from common.realtime import sec_since_boot, config_realtime_process, Priority, R from common.profiler import Profiler from common.params import Params, put_nonblocking import cereal.messaging as messaging -from selfdrive.config import Conversions as CV +from common.conversions import Conversions as CV from selfdrive.swaglog import cloudlog from selfdrive.boardd.boardd import can_list_to_can_capnp from selfdrive.car.car_helpers import get_car, get_startup_event, get_one_can diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index c34d143a5a..978c386530 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -1,6 +1,6 @@ from cereal import log +from common.conversions import Conversions as CV from common.realtime import DT_MDL -from selfdrive.config import Conversions as CV LaneChangeState = log.LateralPlan.LaneChangeState LaneChangeDirection = log.LateralPlan.LaneChangeDirection diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 01d7600345..dba6c190d7 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -2,7 +2,7 @@ import math from cereal import car from common.numpy_fast import clip, interp from common.realtime import DT_MDL -from selfdrive.config import Conversions as CV +from common.conversions import Conversions as CV from selfdrive.modeld.constants import T_IDXS # WARNING: this value was determined based on the model's training distribution, diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index 931932cc0d..e04ec262de 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -4,8 +4,8 @@ from typing import Dict, Union, Callable, List, Optional from cereal import log, car import cereal.messaging as messaging +from common.conversions import Conversions as CV from common.realtime import DT_CTRL -from selfdrive.config import Conversions as CV from selfdrive.locationd.calibrationd import MIN_SPEED_FILTER from selfdrive.version import get_short_branch diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index ad44953e94..fd76142366 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -4,10 +4,10 @@ import numpy as np from common.numpy_fast import interp import cereal.messaging as messaging +from common.conversions import Conversions as CV from common.filter_simple import FirstOrderFilter from common.realtime import DT_MDL from selfdrive.modeld.constants import T_IDXS -from selfdrive.config import Conversions as CV from selfdrive.controls.lib.longcontrol import LongCtrlState from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC diff --git a/selfdrive/controls/lib/radar_helpers.py b/selfdrive/controls/lib/radar_helpers.py index 4f87fdf09b..85699866b0 100644 --- a/selfdrive/controls/lib/radar_helpers.py +++ b/selfdrive/controls/lib/radar_helpers.py @@ -1,6 +1,5 @@ from common.numpy_fast import mean from common.kalman.simple_kalman import KF1D -from selfdrive.config import RADAR_TO_CAMERA # the longer lead decels, the more likely it will keep decelerating @@ -13,6 +12,8 @@ SPEED, ACCEL = 0, 1 # Kalman filter states enum # stationary qualification parameters v_ego_stationary = 4. # no stationary object flag below this speed +RADAR_TO_CENTER = 2.7 # (deprecated) RADAR is ~ 2.7m ahead from center of car +RADAR_TO_CAMERA = 1.52 # RADAR is ~ 1.5m ahead from center of mesh frame class Track(): def __init__(self, v_lead, kalman_params): diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 65f8480c7c..ddba0920b8 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -8,9 +8,8 @@ from cereal import car from common.numpy_fast import interp from common.params import Params from common.realtime import Ratekeeper, Priority, config_realtime_process -from selfdrive.config import RADAR_TO_CAMERA from selfdrive.controls.lib.cluster.fastcluster_py import cluster_points_centroid -from selfdrive.controls.lib.radar_helpers import Cluster, Track +from selfdrive.controls.lib.radar_helpers import Cluster, Track, RADAR_TO_CAMERA from selfdrive.swaglog import cloudlog from selfdrive.hardware import TICI diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py index ae314e38c4..c8c3912528 100755 --- a/selfdrive/locationd/calibrationd.py +++ b/selfdrive/locationd/calibrationd.py @@ -13,12 +13,12 @@ from typing import NoReturn from cereal import log import cereal.messaging as messaging +from common.conversions import Conversions as CV from common.params import Params, put_nonblocking from common.realtime import set_realtime_priority from common.transformations.model import model_height from common.transformations.camera import get_view_frame_from_road_frame from common.transformations.orientation import rot_from_euler, euler_from_rot -from selfdrive.config import Conversions as CV from selfdrive.hardware import TICI from selfdrive.swaglog import cloudlog diff --git a/tools/replay/lib/ui_helpers.py b/tools/replay/lib/ui_helpers.py index 33a1e77e05..d66fe79306 100644 --- a/tools/replay/lib/ui_helpers.py +++ b/tools/replay/lib/ui_helpers.py @@ -10,8 +10,7 @@ from matplotlib.backends.backend_agg import FigureCanvasAgg from common.transformations.camera import (eon_f_frame_size, eon_f_focal_length, tici_f_frame_size, tici_f_focal_length, get_view_frame_from_calib_frame) -from selfdrive.config import UIParams as UP -from selfdrive.config import RADAR_TO_CAMERA +from selfdrive.controls.lib.radar_helpers import RADAR_TO_CAMERA RED = (255, 0, 0) @@ -24,6 +23,15 @@ WHITE = (255, 255, 255) _FULL_FRAME_SIZE = { } +class UIParams: + lidar_x, lidar_y, lidar_zoom = 384, 960, 6 + lidar_car_x, lidar_car_y = lidar_x / 2., lidar_y / 1.1 + car_hwidth = 1.7272 / 2 * lidar_zoom + car_front = 2.6924 * lidar_zoom + car_back = 1.8796 * lidar_zoom + car_color = 110 +UP = UIParams + _BB_TO_FULL_FRAME = {} _CALIB_BB_TO_FULL = {} _FULL_FRAME_TO_BB = {} diff --git a/tools/replay/ui.py b/tools/replay/ui.py index b39377ffdc..dcbcef1016 100755 --- a/tools/replay/ui.py +++ b/tools/replay/ui.py @@ -10,8 +10,7 @@ import pygame # pylint: disable=import-error import cereal.messaging as messaging from common.numpy_fast import clip from common.basedir import BASEDIR -from selfdrive.config import UIParams as UP -from tools.replay.lib.ui_helpers import (_BB_TO_FULL_FRAME, +from tools.replay.lib.ui_helpers import (_BB_TO_FULL_FRAME, UP, _INTRINSICS, BLACK, GREEN, YELLOW, Calibration, get_blank_lid_overlay, init_plots, From a9b7f3bdd8a1a3137c8a04570b6ec5cc03dc6476 Mon Sep 17 00:00:00 2001 From: George Hotz <72895+geohot@users.noreply.github.com> Date: Sun, 13 Mar 2022 21:43:38 -0700 Subject: [PATCH 09/17] 12bit only (#23953) Co-authored-by: Comma Device --- selfdrive/camerad/cameras/camera_qcom2.cc | 13 ++++++------- selfdrive/camerad/cameras/real_debayer.cl | 12 ++++++------ selfdrive/camerad/cameras/sensor2_i2c.h | 12 ++++++------ 3 files changed, 18 insertions(+), 19 deletions(-) diff --git a/selfdrive/camerad/cameras/camera_qcom2.cc b/selfdrive/camerad/cameras/camera_qcom2.cc index 1f1be208d3..ebfffab6de 100644 --- a/selfdrive/camerad/cameras/camera_qcom2.cc +++ b/selfdrive/camerad/cameras/camera_qcom2.cc @@ -26,7 +26,7 @@ extern ExitHandler do_exit; const size_t FRAME_WIDTH = 1928; const size_t FRAME_HEIGHT = 1208; -const size_t FRAME_STRIDE = 2416; // for 10 bit output +const size_t FRAME_STRIDE = 2896; // for 12 bit output. 1928 * 12 / 8 + 4 (alignment) const int MIPI_SETTLE_CNT = 33; // Calculated by camera_freqs.py @@ -471,10 +471,10 @@ void CameraState::config_isp(int io_mem_handle, int fence, int request_id, int b .h_init = 0x0, .v_init = 0x0, }; - io_cfg[0].format = CAM_FORMAT_MIPI_RAW_10; // CAM_FORMAT_UBWC_TP10 for YUV + io_cfg[0].format = CAM_FORMAT_MIPI_RAW_12; // CAM_FORMAT_UBWC_TP10 for YUV io_cfg[0].color_space = CAM_COLOR_SPACE_BASE; // CAM_COLOR_SPACE_BT601_FULL for YUV io_cfg[0].color_pattern = 0x5; // 0x0 for YUV - io_cfg[0].bpp = 0xa; + io_cfg[0].bpp = 0xc; io_cfg[0].resource_type = CAM_ISP_IFE_OUT_RES_RDI_0; // CAM_ISP_IFE_OUT_RES_FULL for YUV io_cfg[0].fence = fence; io_cfg[0].direction = CAM_BUF_OUTPUT; @@ -615,9 +615,8 @@ void CameraState::camera_open() { .lane_cfg = 0x3210, .vc = 0x0, - // .dt = 0x2C; //CSI_RAW12 - .dt = 0x2B, //CSI_RAW10 - .format = CAM_FORMAT_MIPI_RAW_10, + .dt = 0x2C, // CSI_RAW12 + .format = CAM_FORMAT_MIPI_RAW_12, .test_pattern = 0x2, // 0x3? .usage_type = 0x0, @@ -643,7 +642,7 @@ void CameraState::camera_open() { .num_out_res = 0x1, .data[0] = (struct cam_isp_out_port_info){ .res_type = CAM_ISP_IFE_OUT_RES_RDI_0, - .format = CAM_FORMAT_MIPI_RAW_10, + .format = CAM_FORMAT_MIPI_RAW_12, .width = FRAME_WIDTH, .height = FRAME_HEIGHT, .comp_grp_id = 0x0, .split_point = 0x0, .secure_mode = 0x0, diff --git a/selfdrive/camerad/cameras/real_debayer.cl b/selfdrive/camerad/cameras/real_debayer.cl index fe6a99f373..4553036ee3 100644 --- a/selfdrive/camerad/cameras/real_debayer.cl +++ b/selfdrive/camerad/cameras/real_debayer.cl @@ -40,12 +40,12 @@ half3 color_correct(half3 rgb) { } half val_from_10(const uchar * source, int gx, int gy) { - // parse 10bit - int start = gy * FRAME_STRIDE + (5 * (gx / 4)); - int offset = gx % 4; - uint major = (uint)source[start + offset] << 2; - uint minor = (source[start + 4] >> (2 * offset)) & 3; - half pv = (half)(major + minor); + // parse 12bit + int start = gy * FRAME_STRIDE + (3 * (gx / 2)); + int offset = gx % 2; + uint major = (uint)source[start + offset] << 4; + uint minor = (source[start + 2] >> (4 * offset)) & 0xf; + half pv = (half)((major + minor)/4); // normalize pv = max(0.0h, pv - black_level); diff --git a/selfdrive/camerad/cameras/sensor2_i2c.h b/selfdrive/camerad/cameras/sensor2_i2c.h index c3d8861a97..0f91503be0 100644 --- a/selfdrive/camerad/cameras/sensor2_i2c.h +++ b/selfdrive/camerad/cameras/sensor2_i2c.h @@ -9,7 +9,7 @@ struct i2c_random_wr_payload init_array_ar0231[] = { {0x302C, 0x0001}, // VT_SYS_CLK_DIV {0x302E, 0x0002}, // PRE_PLL_CLK_DIV {0x3030, 0x0032}, // PLL_MULTIPLIER - {0x3036, 0x000A}, // OP_WORD_CLK_DIV + {0x3036, 0x000C}, // OP_WORD_CLK_DIV {0x3038, 0x0001}, // OP_SYS_CLK_DIV // FORMAT @@ -46,11 +46,11 @@ struct i2c_random_wr_payload init_array_ar0231[] = { // Readout Settings {0x31AE, 0x0204}, // SERIAL_FORMAT, 4-lane MIPI - {0x31AC, 0x0C0A}, // DATA_FORMAT_BITS, 12 -> 10 - {0x3342, 0x122B}, // MIPI_F1_PDT_EDT - {0x3346, 0x122B}, // MIPI_F2_PDT_EDT - {0x334A, 0x122B}, // MIPI_F3_PDT_EDT - {0x334E, 0x122B}, // MIPI_F4_PDT_EDT + {0x31AC, 0x0C0C}, // DATA_FORMAT_BITS, 12 -> 12 + {0x3342, 0x122C}, // MIPI_F1_PDT_EDT + {0x3346, 0x122C}, // MIPI_F2_PDT_EDT + {0x334A, 0x122C}, // MIPI_F3_PDT_EDT + {0x334E, 0x122C}, // MIPI_F4_PDT_EDT {0x3344, 0x0011}, // MIPI_F1_VDT_VC {0x3348, 0x0111}, // MIPI_F2_VDT_VC {0x334C, 0x0211}, // MIPI_F3_VDT_VC From f1aab78d085c416f9dc56f4fdfd2e1f5eb9d4b4b Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Sun, 13 Mar 2022 21:45:23 -0700 Subject: [PATCH 10/17] longitudinal MPC: add MAX_T in T_IDXS_LST (#23960) * longitudinal MPC: add MAX_T in T_IDXS_LST * update refs Co-authored-by: KexianShen --- selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py | 2 +- selfdrive/test/process_replay/ref_commit | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 94d7ae80ea..69bf081ee3 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -45,7 +45,7 @@ ACADOS_SOLVER_TYPE = 'SQP_RTI' # much better convergence of the MPC with low iterations N = 12 MAX_T = 10.0 -T_IDXS_LST = [index_function(idx, max_val=MAX_T, max_idx=N+1) for idx in range(N+1)] +T_IDXS_LST = [index_function(idx, max_val=MAX_T, max_idx=N) for idx in range(N+1)] T_IDXS = np.array(T_IDXS_LST) T_DIFFS = np.diff(T_IDXS, prepend=[0.]) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index d81fe18ac1..3226b8614b 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -c6d7afb13c183852f8d61d5f79495b828ed401ed \ No newline at end of file +37aa2e3afedc8364d2bbecc5f1b7ed4efec52e30 \ No newline at end of file From 08c2d066cf076134d2ec8528dd2cb245c627a7cf Mon Sep 17 00:00:00 2001 From: Gregor Kikelj <96022003+GregorKikelj@users.noreply.github.com> Date: Mon, 14 Mar 2022 22:03:28 +0100 Subject: [PATCH 11/17] Improve style in drive_helpers (#23962) --- selfdrive/controls/lib/drive_helpers.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index dba6c190d7..f34467081b 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -54,7 +54,7 @@ def update_v_cruise(v_cruise_kph, buttonEvents, button_timers, enabled, metric): long_press = False button_type = None - v_cruise_delta = 1 if metric else 1.6 + v_cruise_delta = 1. if metric else CV.MPH_TO_KPH for b in buttonEvents: if b.type.raw in button_timers and not b.pressed: @@ -91,9 +91,9 @@ def initialize_v_cruise(v_ego, buttonEvents, v_cruise_last): def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, curvature_rates): if len(psis) != CONTROL_N: - psis = [0.0 for i in range(CONTROL_N)] - curvatures = [0.0 for i in range(CONTROL_N)] - curvature_rates = [0.0 for i in range(CONTROL_N)] + psis = [0.0]*CONTROL_N + curvatures = [0.0]*CONTROL_N + curvature_rates = [0.0]*CONTROL_N # TODO this needs more thought, use .2s extra for now to estimate other delays delay = CP.steerActuatorDelay + .2 From 57b6fdc17a1f84ec922647c044da5df3cafc0ddc Mon Sep 17 00:00:00 2001 From: Gijs Koning Date: Mon, 14 Mar 2022 22:03:54 +0100 Subject: [PATCH 12/17] Rename RGB vision streams to match YUV streams (#23961) * Renaming VISION_STREAM_RGB_.. to match yuv names like VISION_STREAM_ROAD VISION_STREAM_RGB_BACK -> VISION_STREAM_RGB_ROAD VISION_STREAM_RGB_FRONT -> VISION_STREAM_RGB_DRIVER * little more Co-authored-by: Adeeb Shihadeh --- cereal | 2 +- selfdrive/camerad/cameras/camera_qcom.cc | 4 ++-- selfdrive/camerad/cameras/camera_qcom2.cc | 6 +++--- selfdrive/camerad/cameras/camera_replay.cc | 4 ++-- selfdrive/camerad/cameras/camera_webcam.cc | 4 ++-- selfdrive/camerad/snapshot/snapshot.py | 6 +++--- selfdrive/test/process_replay/regen.py | 2 +- selfdrive/ui/qt/offroad/driverview.cc | 2 +- selfdrive/ui/qt/onroad.cc | 4 ++-- selfdrive/ui/qt/widgets/cameraview.cc | 8 ++++---- selfdrive/ui/replay/camera.h | 6 +++--- selfdrive/ui/watch3.cc | 6 +++--- tools/replay/ui.py | 2 +- tools/sim/bridge.py | 4 ++-- 14 files changed, 30 insertions(+), 30 deletions(-) diff --git a/cereal b/cereal index 84a1793eb4..e2a813144f 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 84a1793eb4d09821f23613b98d80e2ce4ee57b9c +Subproject commit e2a813144f93dc58a6cc15627915b23fbfa04d0d diff --git a/selfdrive/camerad/cameras/camera_qcom.cc b/selfdrive/camerad/cameras/camera_qcom.cc index da01b94177..411ff0aec4 100644 --- a/selfdrive/camerad/cameras/camera_qcom.cc +++ b/selfdrive/camerad/cameras/camera_qcom.cc @@ -211,12 +211,12 @@ void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_i /*fps*/ 20, #endif device_id, ctx, - VISION_STREAM_RGB_BACK, VISION_STREAM_ROAD); + VISION_STREAM_RGB_ROAD, VISION_STREAM_ROAD); camera_init(v, &s->driver_cam, CAMERA_ID_OV8865, 1, /*pixel_clock=*/72000000, /*line_length_pclk=*/1602, /*max_gain=*/510, 10, device_id, ctx, - VISION_STREAM_RGB_FRONT, VISION_STREAM_DRIVER); + VISION_STREAM_RGB_DRIVER, VISION_STREAM_DRIVER); s->sm = new SubMaster({"driverState"}); s->pm = new PubMaster({"roadCameraState", "driverCameraState", "thumbnail"}); diff --git a/selfdrive/camerad/cameras/camera_qcom2.cc b/selfdrive/camerad/cameras/camera_qcom2.cc index ebfffab6de..2e810e4202 100644 --- a/selfdrive/camerad/cameras/camera_qcom2.cc +++ b/selfdrive/camerad/cameras/camera_qcom2.cc @@ -743,12 +743,12 @@ void CameraState::camera_open() { } void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx) { - s->driver_cam.camera_init(s, v, CAMERA_ID_AR0231, 2, 20, device_id, ctx, VISION_STREAM_RGB_FRONT, VISION_STREAM_DRIVER); + s->driver_cam.camera_init(s, v, CAMERA_ID_AR0231, 2, 20, device_id, ctx, VISION_STREAM_RGB_DRIVER, VISION_STREAM_DRIVER); printf("driver camera initted \n"); if (!env_only_driver) { - s->road_cam.camera_init(s, v, CAMERA_ID_AR0231, 1, 20, device_id, ctx, VISION_STREAM_RGB_BACK, VISION_STREAM_ROAD); // swap left/right + s->road_cam.camera_init(s, v, CAMERA_ID_AR0231, 1, 20, device_id, ctx, VISION_STREAM_RGB_ROAD, VISION_STREAM_ROAD); // swap left/right printf("road camera initted \n"); - s->wide_road_cam.camera_init(s, v, CAMERA_ID_AR0231, 0, 20, device_id, ctx, VISION_STREAM_RGB_WIDE, VISION_STREAM_WIDE_ROAD); + s->wide_road_cam.camera_init(s, v, CAMERA_ID_AR0231, 0, 20, device_id, ctx, VISION_STREAM_RGB_WIDE_ROAD, VISION_STREAM_WIDE_ROAD); printf("wide road camera initted \n"); } diff --git a/selfdrive/camerad/cameras/camera_replay.cc b/selfdrive/camerad/cameras/camera_replay.cc index b5b2e6ad29..a9983fe23e 100644 --- a/selfdrive/camerad/cameras/camera_replay.cc +++ b/selfdrive/camerad/cameras/camera_replay.cc @@ -98,9 +98,9 @@ void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) { void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx) { camera_init(v, &s->road_cam, CAMERA_ID_LGC920, 20, device_id, ctx, - VISION_STREAM_RGB_BACK, VISION_STREAM_ROAD, get_url(road_camera_route, "fcamera", 0)); + VISION_STREAM_RGB_ROAD, VISION_STREAM_ROAD, get_url(road_camera_route, "fcamera", 0)); // camera_init(v, &s->driver_cam, CAMERA_ID_LGC615, 10, device_id, ctx, - // VISION_STREAM_RGB_FRONT, VISION_STREAM_DRIVER, get_url(driver_camera_route, "dcamera", 0)); + // VISION_STREAM_RGB_DRIVER, VISION_STREAM_DRIVER, get_url(driver_camera_route, "dcamera", 0)); s->pm = new PubMaster({"roadCameraState", "driverCameraState", "thumbnail"}); } diff --git a/selfdrive/camerad/cameras/camera_webcam.cc b/selfdrive/camerad/cameras/camera_webcam.cc index 956f2dc88f..6001f9fd3b 100644 --- a/selfdrive/camerad/cameras/camera_webcam.cc +++ b/selfdrive/camerad/cameras/camera_webcam.cc @@ -141,9 +141,9 @@ void driver_camera_thread(CameraState *s) { void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx) { camera_init(v, &s->road_cam, CAMERA_ID_LGC920, 20, device_id, ctx, - VISION_STREAM_RGB_BACK, VISION_STREAM_ROAD); + VISION_STREAM_RGB_ROAD, VISION_STREAM_ROAD); camera_init(v, &s->driver_cam, CAMERA_ID_LGC615, 10, device_id, ctx, - VISION_STREAM_RGB_FRONT, VISION_STREAM_DRIVER); + VISION_STREAM_RGB_DRIVER, VISION_STREAM_DRIVER); s->pm = new PubMaster({"roadCameraState", "driverCameraState", "thumbnail"}); } diff --git a/selfdrive/camerad/snapshot/snapshot.py b/selfdrive/camerad/snapshot/snapshot.py index 506064de3d..1ec7677d31 100755 --- a/selfdrive/camerad/snapshot/snapshot.py +++ b/selfdrive/camerad/snapshot/snapshot.py @@ -17,9 +17,9 @@ from selfdrive.manager.process_config import managed_processes LM_THRESH = 120 # defined in selfdrive/camerad/imgproc/utils.h VISION_STREAMS = { - "roadCameraState": VisionStreamType.VISION_STREAM_RGB_BACK, - "driverCameraState": VisionStreamType.VISION_STREAM_RGB_FRONT, - "wideRoadCameraState": VisionStreamType.VISION_STREAM_RGB_WIDE, + "roadCameraState": VisionStreamType.VISION_STREAM_RGB_ROAD, + "driverCameraState": VisionStreamType.VISION_STREAM_RGB_DRIVER, + "wideRoadCameraState": VisionStreamType.VISION_STREAM_RGB_WIDE_ROAD, } diff --git a/selfdrive/test/process_replay/regen.py b/selfdrive/test/process_replay/regen.py index 16ee5e9a28..e2493d048f 100755 --- a/selfdrive/test/process_replay/regen.py +++ b/selfdrive/test/process_replay/regen.py @@ -159,7 +159,7 @@ def replay_cameras(lr, frs): args=(s, stream, dt, vs, frames, size))) # hack to make UI work - vs.create_buffers(VisionStreamType.VISION_STREAM_RGB_BACK, 4, True, eon_f_frame_size[0], eon_f_frame_size[1]) + vs.create_buffers(VisionStreamType.VISION_STREAM_RGB_ROAD, 4, True, eon_f_frame_size[0], eon_f_frame_size[1]) vs.start_listener() return vs, p diff --git a/selfdrive/ui/qt/offroad/driverview.cc b/selfdrive/ui/qt/offroad/driverview.cc index 3ab5b999b2..c17d2e2573 100644 --- a/selfdrive/ui/qt/offroad/driverview.cc +++ b/selfdrive/ui/qt/offroad/driverview.cc @@ -12,7 +12,7 @@ DriverViewWindow::DriverViewWindow(QWidget* parent) : QWidget(parent) { layout = new QStackedLayout(this); layout->setStackingMode(QStackedLayout::StackAll); - cameraView = new CameraViewWidget("camerad", VISION_STREAM_RGB_FRONT, true, this); + cameraView = new CameraViewWidget("camerad", VISION_STREAM_RGB_DRIVER, true, this); layout->addWidget(cameraView); scene = new DriverViewScene(this); diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 5cd9552a27..e8d871aae1 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -20,7 +20,7 @@ OnroadWindow::OnroadWindow(QWidget *parent) : QWidget(parent) { QStackedLayout *road_view_layout = new QStackedLayout; road_view_layout->setStackingMode(QStackedLayout::StackAll); - nvg = new NvgWindow(VISION_STREAM_RGB_BACK, this); + nvg = new NvgWindow(VISION_STREAM_RGB_ROAD, this); road_view_layout->addWidget(nvg); hud = new OnroadHud(this); road_view_layout->addWidget(hud); @@ -97,7 +97,7 @@ void OnroadWindow::offroadTransition(bool offroad) { // update stream type bool wide_cam = Hardware::TICI() && Params().getBool("EnableWideCamera"); - nvg->setStreamType(wide_cam ? VISION_STREAM_RGB_WIDE : VISION_STREAM_RGB_BACK); + nvg->setStreamType(wide_cam ? VISION_STREAM_RGB_WIDE_ROAD : VISION_STREAM_RGB_ROAD); } void OnroadWindow::paintEvent(QPaintEvent *event) { diff --git a/selfdrive/ui/qt/widgets/cameraview.cc b/selfdrive/ui/qt/widgets/cameraview.cc index fbd425b025..ed40094f73 100644 --- a/selfdrive/ui/qt/widgets/cameraview.cc +++ b/selfdrive/ui/qt/widgets/cameraview.cc @@ -123,7 +123,7 @@ void CameraViewWidget::initializeGL() { GLint frame_pos_loc = program->attributeLocation("aPosition"); GLint frame_texcoord_loc = program->attributeLocation("aTexCoord"); - auto [x1, x2, y1, y2] = stream_type == VISION_STREAM_RGB_FRONT ? std::tuple(0.f, 1.f, 1.f, 0.f) : std::tuple(1.f, 0.f, 1.f, 0.f); + auto [x1, x2, y1, y2] = stream_type == VISION_STREAM_RGB_DRIVER ? std::tuple(0.f, 1.f, 1.f, 0.f) : std::tuple(1.f, 0.f, 1.f, 0.f); const uint8_t frame_indicies[] = {0, 1, 2, 0, 2, 3}; const float frame_coords[4][4] = { {-1.0, -1.0, x2, y1}, // bl @@ -171,12 +171,12 @@ void CameraViewWidget::hideEvent(QHideEvent *event) { void CameraViewWidget::updateFrameMat(int w, int h) { if (zoomed_view) { - if (stream_type == VISION_STREAM_RGB_FRONT) { + if (stream_type == VISION_STREAM_RGB_DRIVER) { frame_mat = matmul(device_transform, get_driver_view_transform(w, h, stream_width, stream_height)); } else { - auto intrinsic_matrix = stream_type == VISION_STREAM_RGB_WIDE ? ecam_intrinsic_matrix : fcam_intrinsic_matrix; + auto intrinsic_matrix = stream_type == VISION_STREAM_RGB_WIDE_ROAD ? ecam_intrinsic_matrix : fcam_intrinsic_matrix; float zoom = ZOOM / intrinsic_matrix.v[0]; - if (stream_type == VISION_STREAM_RGB_WIDE) { + if (stream_type == VISION_STREAM_RGB_WIDE_ROAD) { zoom *= 0.5; } float zx = zoom * 2 * intrinsic_matrix.v[2] / width(); diff --git a/selfdrive/ui/replay/camera.h b/selfdrive/ui/replay/camera.h index 340a120e8c..9d72094d4c 100644 --- a/selfdrive/ui/replay/camera.h +++ b/selfdrive/ui/replay/camera.h @@ -32,9 +32,9 @@ protected: void cameraThread(Camera &cam); Camera cameras_[MAX_CAMERAS] = { - {.type = RoadCam, .rgb_type = VISION_STREAM_RGB_BACK, .yuv_type = VISION_STREAM_ROAD}, - {.type = DriverCam, .rgb_type = VISION_STREAM_RGB_FRONT, .yuv_type = VISION_STREAM_DRIVER}, - {.type = WideRoadCam, .rgb_type = VISION_STREAM_RGB_WIDE, .yuv_type = VISION_STREAM_WIDE_ROAD}, + {.type = RoadCam, .rgb_type = VISION_STREAM_RGB_ROAD, .yuv_type = VISION_STREAM_ROAD}, + {.type = DriverCam, .rgb_type = VISION_STREAM_RGB_DRIVER, .yuv_type = VISION_STREAM_DRIVER}, + {.type = WideRoadCam, .rgb_type = VISION_STREAM_RGB_WIDE_ROAD, .yuv_type = VISION_STREAM_WIDE_ROAD}, }; std::atomic publishing_ = 0; std::unique_ptr vipc_server_; diff --git a/selfdrive/ui/watch3.cc b/selfdrive/ui/watch3.cc index 1752ce9b6d..74c00fe18e 100644 --- a/selfdrive/ui/watch3.cc +++ b/selfdrive/ui/watch3.cc @@ -20,14 +20,14 @@ int main(int argc, char *argv[]) { QHBoxLayout *hlayout = new QHBoxLayout(); layout->addLayout(hlayout); hlayout->addWidget(new CameraViewWidget("navd", VISION_STREAM_RGB_MAP, false)); - hlayout->addWidget(new CameraViewWidget("camerad", VISION_STREAM_RGB_BACK, false)); + hlayout->addWidget(new CameraViewWidget("camerad", VISION_STREAM_RGB_ROAD, false)); } { QHBoxLayout *hlayout = new QHBoxLayout(); layout->addLayout(hlayout); - hlayout->addWidget(new CameraViewWidget("camerad", VISION_STREAM_RGB_FRONT, false)); - hlayout->addWidget(new CameraViewWidget("camerad", VISION_STREAM_RGB_WIDE, false)); + hlayout->addWidget(new CameraViewWidget("camerad", VISION_STREAM_RGB_DRIVER, false)); + hlayout->addWidget(new CameraViewWidget("camerad", VISION_STREAM_RGB_WIDE_ROAD, false)); } return a.exec(); diff --git a/tools/replay/ui.py b/tools/replay/ui.py index dcbcef1016..729d4cd0d8 100755 --- a/tools/replay/ui.py +++ b/tools/replay/ui.py @@ -99,7 +99,7 @@ def ui_thread(addr): draw_plots = init_plots(plot_arr, name_to_arr_idx, plot_xlims, plot_ylims, plot_names, plot_colors, plot_styles) - vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_RGB_BACK, True) + vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_RGB_ROAD, True) while 1: list(pygame.event.get()) diff --git a/tools/sim/bridge.py b/tools/sim/bridge.py index 2e19faefcd..ae6483fd28 100755 --- a/tools/sim/bridge.py +++ b/tools/sim/bridge.py @@ -67,7 +67,7 @@ class Camerad: self.vipc_server = VisionIpcServer("camerad") # TODO: remove RGB buffers once the last RGB vipc subscriber is removed - self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_RGB_BACK, 4, True, W, H) + self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_RGB_ROAD, 4, True, W, H) self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_ROAD, 40, False, W, H) self.vipc_server.start_listener() @@ -97,7 +97,7 @@ class Camerad: eof = self.frame_id * 0.05 # TODO: remove RGB send once the last RGB vipc subscriber is removed - self.vipc_server.send(VisionStreamType.VISION_STREAM_RGB_BACK, img.tobytes(), self.frame_id, eof, eof) + self.vipc_server.send(VisionStreamType.VISION_STREAM_RGB_ROAD, img.tobytes(), self.frame_id, eof, eof) self.vipc_server.send(VisionStreamType.VISION_STREAM_ROAD, yuv.data.tobytes(), self.frame_id, eof, eof) dat = messaging.new_message('roadCameraState') From 814741dafe6a591b87d23d3b0aa9662a01f61634 Mon Sep 17 00:00:00 2001 From: George Hotz <72895+geohot@users.noreply.github.com> Date: Mon, 14 Mar 2022 14:45:50 -0700 Subject: [PATCH 13/17] thermald: use named thermal zones (#23936) * thermald: use named thermal zones * remove print * and for c2 Co-authored-by: Comma Device --- selfdrive/hardware/eon/hardware.py | 4 +++- selfdrive/hardware/tici/hardware.py | 8 +++++++- selfdrive/thermald/thermald.py | 15 +++++++++++++++ 3 files changed, 25 insertions(+), 2 deletions(-) diff --git a/selfdrive/hardware/eon/hardware.py b/selfdrive/hardware/eon/hardware.py index 4ab9f81fcf..dcc29d5919 100644 --- a/selfdrive/hardware/eon/hardware.py +++ b/selfdrive/hardware/eon/hardware.py @@ -380,7 +380,9 @@ class Android(HardwareBase): os.system('LD_LIBRARY_PATH="" svc power shutdown') def get_thermal_config(self): - return ThermalConfig(cpu=((5, 7, 10, 12), 10), gpu=((16,), 10), mem=(2, 10), bat=(29, 1000), ambient=(25, 1), pmic=((22,), 1000)) + # the thermal sensors on the 820 don't have meaningful names + return ThermalConfig(cpu=((5, 7, 10, 12), 10), gpu=((16,), 10), mem=(2, 10), + bat=("battery", 1000), ambient=("pa_therm0", 1), pmic=(("pm8994_tz",), 1000)) def set_screen_brightness(self, percentage): with open("/sys/class/leds/lcd-backlight/brightness", "w") as f: diff --git a/selfdrive/hardware/tici/hardware.py b/selfdrive/hardware/tici/hardware.py index ede0c3e004..7a0d5204c1 100644 --- a/selfdrive/hardware/tici/hardware.py +++ b/selfdrive/hardware/tici/hardware.py @@ -362,7 +362,13 @@ class Tici(HardwareBase): os.system("sudo poweroff") def get_thermal_config(self): - return ThermalConfig(cpu=((1, 2, 3, 4, 5, 6, 7, 8), 1000), gpu=((48,49), 1000), mem=(15, 1000), bat=(None, 1), ambient=(65, 1000), pmic=((35, 36), 1000)) + return ThermalConfig(cpu=(["cpu%d-silver-usr" % i for i in range(4)] + + ["cpu%d-gold-usr" % i for i in range(4)], 1000), + gpu=(("gpu0-usr", "gpu1-usr"), 1000), + mem=("ddr-usr", 1000), + bat=(None, 1), + ambient=("xo-therm-adc", 1000), + pmic=(("pm8998_tz", "pm8005_tz"), 1000)) def set_screen_brightness(self, percentage): try: diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py index f5e818acf2..1f62738ed7 100755 --- a/selfdrive/thermald/thermald.py +++ b/selfdrive/thermald/thermald.py @@ -50,10 +50,25 @@ OFFROAD_DANGER_TEMP = 79.5 if TICI else 70.0 prev_offroad_states: Dict[str, Tuple[bool, Optional[str]]] = {} +tz_by_type: Optional[Dict[str, int]] = None +def populate_tz_by_type(): + global tz_by_type + tz_by_type = {} + for n in os.listdir("/sys/devices/virtual/thermal"): + if not n.startswith("thermal_zone"): + continue + with open(os.path.join("/sys/devices/virtual/thermal", n, "type")) as f: + tz_by_type[f.read().strip()] = int(n.lstrip("thermal_zone")) + def read_tz(x): if x is None: return 0 + if isinstance(x, str): + if tz_by_type is None: + populate_tz_by_type() + x = tz_by_type[x] + try: with open(f"/sys/devices/virtual/thermal/thermal_zone{x}/temp") as f: return int(f.read()) From fa4b434e432f68dfc8944592c63f3594a9fad0b3 Mon Sep 17 00:00:00 2001 From: George Hotz Date: Mon, 14 Mar 2022 15:02:42 -0700 Subject: [PATCH 14/17] camerastream: update receive.py with some options --- tools/camerastream/receive.py | 23 ++++++++++++++++------- 1 file changed, 16 insertions(+), 7 deletions(-) diff --git a/tools/camerastream/receive.py b/tools/camerastream/receive.py index be034816ac..6f8d67c78f 100755 --- a/tools/camerastream/receive.py +++ b/tools/camerastream/receive.py @@ -11,23 +11,32 @@ import cereal.messaging as messaging # also start bridge # then run this "./receive.py " -SCALE = 1 -XMIN = 771 -XMAX = 1156 -YMIN = 483 -YMAX = 724 +if "FULL" in os.environ: + SCALE = 2 + XMIN, XMAX = 0, 1927 + YMIN, YMAX = 0, 1207 +else: + SCALE = 1 + XMIN = 771 + XMAX = 1156 + YMIN = 483 + YMAX = 724 H, W = ((YMAX-YMIN+1)//SCALE, (XMAX-XMIN+1)//SCALE) if __name__ == '__main__': cameras = ['roadCameraState', 'wideRoadCameraState', 'driverCameraState'] + if "CAM" in os.environ: + cam = int(os.environ['CAM']) + cameras = cameras[cam:cam+1] sm = messaging.SubMaster(cameras, addr=sys.argv[1]) - win = Window(W*3, H) - bdat = np.zeros((H, W*3, 3), dtype=np.uint8) + win = Window(W*len(cameras), H) + bdat = np.zeros((H, W*len(cameras), 3), dtype=np.uint8) while 1: sm.update() for i,k in enumerate(cameras): if sm.updated[k]: + #print("update", k) bgr_raw = sm[k].image dat = np.frombuffer(bgr_raw, dtype=np.uint8).reshape(H, W, 3)[:, :, [2,1,0]] bdat[:, W*i:W*(i+1)] = dat From 8718a5933049d5934cc53ef31af9b5ded3223a7c Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Mon, 14 Mar 2022 15:46:16 -0700 Subject: [PATCH 15/17] cleanup selfdrive/test (#23967) * move test routes and test_models * move fingerprints * little more * remove that --- .github/PULL_REQUEST_TEMPLATE/car_port.md | 2 +- .github/workflows/selfdrive_tests.yaml | 5 ++--- release/files_common | 1 - selfdrive/car/gm/interface.py | 2 +- selfdrive/car/hyundai/interface.py | 2 +- selfdrive/{test/test_routes.py => car/tests/routes.py} | 0 selfdrive/{test => car/tests}/test_fingerprints.py | 0 selfdrive/{test => car/tests}/test_models.py | 2 +- selfdrive/test/update_ci_routes.py | 2 +- 9 files changed, 7 insertions(+), 9 deletions(-) rename selfdrive/{test/test_routes.py => car/tests/routes.py} (100%) rename selfdrive/{test => car/tests}/test_fingerprints.py (100%) rename selfdrive/{test => car/tests}/test_models.py (99%) diff --git a/.github/PULL_REQUEST_TEMPLATE/car_port.md b/.github/PULL_REQUEST_TEMPLATE/car_port.md index d275468395..4264363ba2 100644 --- a/.github/PULL_REQUEST_TEMPLATE/car_port.md +++ b/.github/PULL_REQUEST_TEMPLATE/car_port.md @@ -9,7 +9,7 @@ assignees: '' **Checklist** - [ ] added to README -- [ ] test route added to [test_routes.py](https://github.com/commaai/openpilot/blob/master/selfdrive/test/test_models.py) +- [ ] test route added to [routes.py](https://github.com/commaai/openpilot/blob/master/selfdrive/car/tests/routes.py) - [ ] route with openpilot: - [ ] route with stock system: - [ ] car harness used (if comma doesn't sell it, put N/A): diff --git a/.github/workflows/selfdrive_tests.yaml b/.github/workflows/selfdrive_tests.yaml index 9870637003..0083d39c3a 100644 --- a/.github/workflows/selfdrive_tests.yaml +++ b/.github/workflows/selfdrive_tests.yaml @@ -245,7 +245,6 @@ jobs: - name: Run unit tests run: | ${{ env.RUN }} "scons -j$(nproc) --test && \ - coverage run selfdrive/test/test_fingerprints.py && \ $UNIT_TEST common && \ $UNIT_TEST opendbc/can && \ $UNIT_TEST selfdrive/boardd && \ @@ -384,7 +383,7 @@ jobs: uses: actions/cache@v2 with: path: /tmp/comma_download_cache - key: car_models-${{ hashFiles('selfdrive/test/test_models.py', 'selfdrive/test/test_routes.py') }}-${{ matrix.job }} + key: car_models-${{ hashFiles('selfdrive/car/tests/test_models.py', 'selfdrive/test/test_routes.py') }}-${{ matrix.job }} - name: Cache scons id: scons-cache # TODO: Change the version to the released version when https://github.com/actions/cache/pull/489 (or 571) is merged. @@ -402,7 +401,7 @@ jobs: - name: Test car models run: | ${{ env.RUN }} "scons -j$(nproc) --test && \ - FILEREADER_CACHE=1 coverage run -m pytest selfdrive/test/test_models.py && \ + FILEREADER_CACHE=1 coverage run -m pytest selfdrive/car/tests/test_models.py && \ coverage xml && \ chmod -R 777 /tmp/comma_download_cache" env: diff --git a/release/files_common b/release/files_common index 30029ba0d9..d2d5f14ae4 100644 --- a/release/files_common +++ b/release/files_common @@ -341,7 +341,6 @@ selfdrive/thermald/fan_controller.py selfdrive/test/__init__.py selfdrive/test/helpers.py selfdrive/test/setup_device_ci.sh -selfdrive/test/test_fingerprints.py selfdrive/test/test_onroad.py selfdrive/ui/.gitignore diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 7125b4a913..0fa3ec8418 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -47,7 +47,7 @@ class CarInterface(CarInterfaceBase): # These cars have been put into dashcam only due to both a lack of users and test coverage. # These cars likely still work fine. Once a user confirms each car works and a test route is - # added to selfdrive/test/test_routes, we can remove it from this list. + # added to selfdrive/car/tests/routes.py, we can remove it from this list. ret.dashcamOnly = candidate in {CAR.CADILLAC_ATS, CAR.HOLDEN_ASTRA, CAR.MALIBU, CAR.BUICK_REGAL} # Presence of a camera on the object bus is ok. diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 89308c7897..6bb7bc361c 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -32,7 +32,7 @@ class CarInterface(CarInterfaceBase): # These cars have been put into dashcam only due to both a lack of users and test coverage. # These cars likely still work fine. Once a user confirms each car works and a test route is - # added to selfdrive/test/test_routes, we can remove it from this list. + # added to selfdrive/car/tests/routes.py, we can remove it from this list. ret.dashcamOnly = candidate in {CAR.KIA_OPTIMA_H, CAR.ELANTRA_GT_I30} ret.steerActuatorDelay = 0.1 # Default delay diff --git a/selfdrive/test/test_routes.py b/selfdrive/car/tests/routes.py similarity index 100% rename from selfdrive/test/test_routes.py rename to selfdrive/car/tests/routes.py diff --git a/selfdrive/test/test_fingerprints.py b/selfdrive/car/tests/test_fingerprints.py similarity index 100% rename from selfdrive/test/test_fingerprints.py rename to selfdrive/car/tests/test_fingerprints.py diff --git a/selfdrive/test/test_models.py b/selfdrive/car/tests/test_models.py similarity index 99% rename from selfdrive/test/test_models.py rename to selfdrive/car/tests/test_models.py index 5afe8b6c11..09d17462e6 100755 --- a/selfdrive/test/test_models.py +++ b/selfdrive/car/tests/test_models.py @@ -17,7 +17,7 @@ from selfdrive.car.gm.values import CAR as GM from selfdrive.car.honda.values import CAR as HONDA, HONDA_BOSCH from selfdrive.car.hyundai.values import CAR as HYUNDAI from selfdrive.car.toyota.values import CAR as TOYOTA -from selfdrive.test.test_routes import routes, non_tested_cars +from selfdrive.car.tests.routes import routes, non_tested_cars from selfdrive.test.openpilotci import get_url from tools.lib.logreader import LogReader diff --git a/selfdrive/test/update_ci_routes.py b/selfdrive/test/update_ci_routes.py index 3a392b4b36..fe6d815ebf 100755 --- a/selfdrive/test/update_ci_routes.py +++ b/selfdrive/test/update_ci_routes.py @@ -3,7 +3,7 @@ import sys import subprocess from azure.storage.blob import BlockBlobService # pylint: disable=import-error -from selfdrive.test.test_routes import routes as test_car_models_routes +from selfdrive.car.tests.routes import routes as test_car_models_routes from selfdrive.test.process_replay.test_processes import original_segments as replay_segments from xx.chffr.lib import azureutil # pylint: disable=import-error from xx.chffr.lib.storage import _DATA_ACCOUNT_PRODUCTION, _DATA_ACCOUNT_CI, _DATA_BUCKET_PRODUCTION # pylint: disable=import-error From ae19308e83991c4809a785bb698d300cf7afe7e1 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Mon, 14 Mar 2022 16:32:20 -0700 Subject: [PATCH 16/17] add parkingBrake to carState (#23968) * add parkingBrake to carState * fix gm --- cereal | 2 +- selfdrive/car/gm/carstate.py | 2 +- selfdrive/car/gm/interface.py | 2 -- selfdrive/car/honda/carstate.py | 5 ++--- selfdrive/car/honda/interface.py | 2 -- selfdrive/car/hyundai/carstate.py | 2 +- selfdrive/car/hyundai/interface.py | 2 -- selfdrive/car/interfaces.py | 2 ++ selfdrive/car/tests/test_car_interfaces.py | 1 - selfdrive/car/volkswagen/carstate.py | 2 +- selfdrive/car/volkswagen/interface.py | 2 -- 11 files changed, 8 insertions(+), 16 deletions(-) diff --git a/cereal b/cereal index e2a813144f..ad2fe885da 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit e2a813144f93dc58a6cc15627915b23fbfa04d0d +Subproject commit ad2fe885dab99896908b88e765a5f720bfd79b3b diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index 59960a4dc0..b94fd0f2ab 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -59,7 +59,7 @@ class CarState(CarStateBase): ret.leftBlinker = pt_cp.vl["BCMTurnSignals"]["TurnSignals"] == 1 ret.rightBlinker = pt_cp.vl["BCMTurnSignals"]["TurnSignals"] == 2 - self.park_brake = pt_cp.vl["EPBStatus"]["EPBClosed"] + ret.parkingBrake = pt_cp.vl["EPBStatus"]["EPBClosed"] == 1 ret.cruiseState.available = pt_cp.vl["ECMEngineStatus"]["CruiseMainOn"] != 0 ret.espDisabled = pt_cp.vl["ESPStatus"]["TractionControlOn"] != 1 self.pcm_acc_status = pt_cp.vl["AcceleratorPedal2"]["CruiseState"] diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 0fa3ec8418..0473abfc74 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -192,8 +192,6 @@ class CarInterface(CarInterfaceBase): if ret.vEgo < self.CP.minEnableSpeed: events.add(EventName.belowEngageSpeed) - if self.CS.park_brake: - events.add(EventName.parkBrake) if ret.cruiseState.standstill: events.add(EventName.resumeRequired) if self.CS.pcm_acc_status == AccState.FAULTED: diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index 332290373a..fc262ba42c 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -230,11 +230,10 @@ class CarState(CarStateBase): 250, cp.vl["SCM_FEEDBACK"]["LEFT_BLINKER"], cp.vl["SCM_FEEDBACK"]["RIGHT_BLINKER"]) ret.brakeHoldActive = cp.vl["VSA_STATUS"]["BRAKE_HOLD_ACTIVE"] == 1 + # TODO: set for all cars if self.CP.carFingerprint in (CAR.CIVIC, CAR.ODYSSEY, CAR.ODYSSEY_CHN, CAR.CRV_5G, CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E): - self.park_brake = cp.vl["EPB_STATUS"]["EPB_STATE"] != 0 - else: - self.park_brake = 0 # TODO + ret.parkingBrake = cp.vl["EPB_STATUS"]["EPB_STATE"] != 0 gear = int(cp.vl[self.gearbox_msg]["GEAR_SHIFTER"]) ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(gear, None)) diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 2c801f7f15..040587be6f 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -380,8 +380,6 @@ class CarInterface(CarInterfaceBase): events = self.create_common_events(ret, pcm_enable=False) if self.CS.brake_error: events.add(EventName.brakeUnavailable) - if self.CS.park_brake: - events.add(EventName.parkBrake) if self.CP.pcmCruise and ret.vEgo < self.CP.minEnableSpeed: events.add(EventName.belowEngageSpeed) diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 95c1878e3d..b47fed75df 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -70,6 +70,7 @@ class CarState(CarStateBase): ret.brake = 0 ret.brakePressed = cp.vl["TCS13"]["DriverBraking"] != 0 ret.brakeHoldActive = cp.vl["TCS15"]["AVH_LAMP"] == 2 # 0 OFF, 1 ERROR, 2 ACTIVE, 3 READY + ret.parkingBrake = cp.vl["TCS13"]["PBRAKE_ACT"] == 1 if self.CP.carFingerprint in (HYBRID_CAR | EV_CAR): if self.CP.carFingerprint in HYBRID_CAR: @@ -109,7 +110,6 @@ class CarState(CarStateBase): # save the entire LKAS11 and CLU11 self.lkas11 = copy.copy(cp_cam.vl["LKAS11"]) self.clu11 = copy.copy(cp.vl["CLU11"]) - self.park_brake = cp.vl["TCS13"]["PBRAKE_ACT"] == 1 self.steer_state = cp.vl["MDPS12"]["CF_Mdps_ToiActive"] # 0 NOT ACTIVE, 1 ACTIVE self.brake_error = cp.vl["TCS13"]["ACCEnable"] != 0 # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED self.prev_cruise_buttons = self.cruise_buttons diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 6bb7bc361c..5d75be0aa4 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -302,8 +302,6 @@ class CarInterface(CarInterfaceBase): if self.CS.brake_error: events.add(EventName.brakeUnavailable) - if self.CS.park_brake: - events.add(EventName.parkBrake) if self.CS.CP.openpilotLongitudinalControl: buttonEvents = [] diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 80d6b1db3e..ab8a179bf5 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -139,6 +139,8 @@ class CarInterfaceBase(ABC): events.add(EventName.wrongCruiseMode) if cs_out.brakeHoldActive and self.CP.openpilotLongitudinalControl: events.add(EventName.brakeHold) + if cs_out.parkingBrake: + events.add(EventName.parkBrake) # Handle permanent and temporary steering faults diff --git a/selfdrive/car/tests/test_car_interfaces.py b/selfdrive/car/tests/test_car_interfaces.py index 930619ee67..2f4984c3d9 100755 --- a/selfdrive/car/tests/test_car_interfaces.py +++ b/selfdrive/car/tests/test_car_interfaces.py @@ -12,7 +12,6 @@ class TestCarInterfaces(unittest.TestCase): @parameterized.expand([(car,) for car in all_known_cars()]) def test_car_interfaces(self, car_name): - print(car_name) if car_name in FINGERPRINTS: fingerprint = FINGERPRINTS[car_name][0] else: diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py index 83dbb956f7..858b5e1440 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -49,6 +49,7 @@ class CarState(CarStateBase): ret.gasPressed = ret.gas > 0 ret.brake = pt_cp.vl["ESP_05"]["ESP_Bremsdruck"] / 250.0 # FIXME: this is pressure in Bar, not sure what OP expects ret.brakePressed = bool(pt_cp.vl["ESP_05"]["ESP_Fahrer_bremst"]) + ret.parkingBrake = bool(pt_cp.vl["Kombi_01"]["KBI_Handbremse"]) # FIXME: need to include an EPB check as well self.esp_hold_confirmation = pt_cp.vl["ESP_21"]["ESP_Haltebestaetigung"] # Update gear and/or clutch position data. @@ -140,7 +141,6 @@ class CarState(CarStateBase): self.graMsgBusCounter = pt_cp.vl["GRA_ACC_01"]["COUNTER"] # Additional safety checks performed in CarInterface. - self.parkingBrakeSet = bool(pt_cp.vl["Kombi_01"]["KBI_Handbremse"]) # FIXME: need to include an EPB check as well ret.espDisabled = pt_cp.vl["ESP_21"]["ESP_Tastung_passiv"] != 0 return ret diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index 976c4459e3..f96f1bae5a 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -186,8 +186,6 @@ class CarInterface(CarInterfaceBase): events = self.create_common_events(ret, extra_gears=[GearShifter.eco, GearShifter.sport, GearShifter.manumatic]) # Vehicle health and operation safety checks - if self.CS.parkingBrakeSet: - events.add(EventName.parkBrake) if self.CS.tsk_status in (6, 7): events.add(EventName.accFaulted) From d7c758d4bfa9ac6fc70971f8f9a2b5626d971e0a Mon Sep 17 00:00:00 2001 From: cydia2020 <12470297+cydia2020@users.noreply.github.com> Date: Tue, 15 Mar 2022 13:19:31 +1100 Subject: [PATCH 17/17] Toyota: parking brake state (#23970) --- selfdrive/car/toyota/carstate.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 8a6f91324a..a951955505 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -31,6 +31,7 @@ class CarState(CarStateBase): ret.doorOpen = any([cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_FL"], cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_FR"], cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_RL"], cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_RR"]]) ret.seatbeltUnlatched = cp.vl["BODY_CONTROL_STATE"]["SEATBELT_DRIVER_UNLATCHED"] != 0 + ret.parkingBrake = cp.vl["BODY_CONTROL_STATE"]["PARKING_BRAKE"] == 1 ret.brakePressed = cp.vl["BRAKE_MODULE"]["BRAKE_PRESSED"] != 0 ret.brakeHoldActive = cp.vl["ESP_CONTROL"]["BRAKE_HOLD_ACTIVE"] == 1 @@ -140,6 +141,7 @@ class CarState(CarStateBase): ("DOOR_OPEN_RL", "BODY_CONTROL_STATE"), ("DOOR_OPEN_RR", "BODY_CONTROL_STATE"), ("SEATBELT_DRIVER_UNLATCHED", "BODY_CONTROL_STATE"), + ("PARKING_BRAKE", "BODY_CONTROL_STATE"), ("TC_DISABLED", "ESP_CONTROL"), ("BRAKE_HOLD_ACTIVE", "ESP_CONTROL"), ("STEER_FRACTION", "STEER_ANGLE_SENSOR"),