From 819b7a120ac08ca64abda390da7cd36b21da4781 Mon Sep 17 00:00:00 2001 From: Jason Young <46612682+jyoung8607@users.noreply.github.com> Date: Mon, 22 Nov 2021 16:01:42 -0600 Subject: [PATCH 1/2] VW MQB: Longitudinal prereqs (#23004) --- selfdrive/car/volkswagen/carcontroller.py | 45 ++++++++++++----------- selfdrive/car/volkswagen/carstate.py | 18 +++++---- selfdrive/car/volkswagen/interface.py | 4 +- 3 files changed, 36 insertions(+), 31 deletions(-) diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index 97f00008bd..977818dbd5 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -86,27 +86,28 @@ class CarController(): # FIXME: this entire section is in desperate need of refactoring - if frame > self.graMsgStartFramePrev + P.GRA_VBP_STEP: - if not enabled and CS.out.cruiseState.enabled: - # Cancel ACC if it's engaged with OP disengaged. - self.graButtonStatesToSend = BUTTON_STATES.copy() - self.graButtonStatesToSend["cancel"] = True - elif enabled and CS.out.standstill: - # 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() - self.graButtonStatesToSend["resumeCruise"] = True - - if CS.graMsgBusCounter != self.graMsgBusCounterPrev: - self.graMsgBusCounterPrev = CS.graMsgBusCounter - if self.graButtonStatesToSend is not None: - if self.graMsgSentCount == 0: - self.graMsgStartFramePrev = frame - idx = (CS.graMsgBusCounter + 1) % 16 - can_sends.append(volkswagencan.create_mqb_acc_buttons_control(self.packer_pt, ext_bus, self.graButtonStatesToSend, CS, idx)) - self.graMsgSentCount += 1 - if self.graMsgSentCount >= P.GRA_VBP_COUNT: - self.graButtonStatesToSend = None - self.graMsgSentCount = 0 + if CS.CP.pcmCruise: + if frame > self.graMsgStartFramePrev + P.GRA_VBP_STEP: + if not enabled and CS.out.cruiseState.enabled: + # 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: + # 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() + self.graButtonStatesToSend["resumeCruise"] = True + + if CS.graMsgBusCounter != self.graMsgBusCounterPrev: + self.graMsgBusCounterPrev = CS.graMsgBusCounter + if self.graButtonStatesToSend is not None: + if self.graMsgSentCount == 0: + self.graMsgStartFramePrev = frame + idx = (CS.graMsgBusCounter + 1) % 16 + can_sends.append(volkswagencan.create_mqb_acc_buttons_control(self.packer_pt, ext_bus, self.graButtonStatesToSend, CS, idx)) + self.graMsgSentCount += 1 + if self.graMsgSentCount >= P.GRA_VBP_COUNT: + self.graButtonStatesToSend = None + self.graMsgSentCount = 0 return can_sends diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py index f02129a61c..b6a799e36f 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -27,7 +27,7 @@ class CarState(CarStateBase): ret.vEgoRaw = float(np.mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr])) ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) - ret.standstill = bool(pt_cp.vl["ESP_21"]["ESP_Haltebestaetigung"]) + ret.standstill = ret.vEgo < 0.1 # Update steering angle, rate, yaw rate, and driver input torque. VW send # the sign/direction in a separate signal so they must be recombined. @@ -47,6 +47,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"]) + self.esp_hold_confirmation = pt_cp.vl["ESP_21"]["ESP_Haltebestaetigung"] # Update gear and/or clutch position data. if trans_type == TransmissionType.automatic: @@ -94,13 +95,13 @@ class CarState(CarStateBase): ret.stockAeb = bool(ext_cp.vl["ACC_10"]["ANB_Teilbremsung_Freigabe"]) or bool(ext_cp.vl["ACC_10"]["ANB_Zielbremsung_Freigabe"]) # Update ACC radar status. - accStatus = pt_cp.vl["TSK_06"]["TSK_Status"] - if accStatus == 2: + self.tsk_status = pt_cp.vl["TSK_06"]["TSK_Status"] + if self.tsk_status == 2: # ACC okay and enabled, but not currently engaged ret.cruiseState.available = True ret.cruiseState.enabled = False - elif accStatus in [3, 4, 5]: - # ACC okay and enabled, currently engaged and regulating speed (3) or engaged with driver accelerating (4) or overrun (5) + elif self.tsk_status in [3, 4, 5]: + # ACC okay and enabled, currently regulating speed (3) or driver accel override (4) or overrun coast-down (5) ret.cruiseState.available = True ret.cruiseState.enabled = True else: @@ -110,9 +111,10 @@ class CarState(CarStateBase): # Update ACC setpoint. When the setpoint is zero or there's an error, the # radar sends a set-speed of ~90.69 m/s / 203mph. - ret.cruiseState.speed = ext_cp.vl["ACC_02"]["ACC_Wunschgeschw"] * CV.KPH_TO_MS - if ret.cruiseState.speed > 90: - ret.cruiseState.speed = 0 + if self.CP.pcmCruise: + ret.cruiseState.speed = ext_cp.vl["ACC_02"]["ACC_Wunschgeschw"] * CV.KPH_TO_MS + if ret.cruiseState.speed > 90: + ret.cruiseState.speed = 0 # Update control button states for turn signals and ACC controls. self.buttonStates["accelCruise"] = bool(pt_cp.vl["GRA_ACC_01"]["GRA_Tip_Hoch"]) diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index b8c9c5d67b..51b6a9a045 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -43,7 +43,7 @@ class CarInterface(CarInterfaceBase): else: ret.networkLocation = NetworkLocation.fwdCamera - # Global tuning defaults, can be overridden per-vehicle + # Global lateral tuning defaults, can be overridden per-vehicle ret.steerActuatorDelay = 0.05 ret.steerRateCost = 1.0 @@ -190,6 +190,8 @@ class CarInterface(CarInterfaceBase): # 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) # Low speed steer alert hysteresis logic if self.CP.minSteerSpeed > 0. and ret.vEgo < (self.CP.minSteerSpeed + 1.): From b51da61dc4401efadfe15b90ce65ab24c80cf9e0 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Mon, 22 Nov 2021 14:03:42 -0800 Subject: [PATCH 2/2] perfetto (#22998) Co-authored-by: Comma Device --- selfdrive/debug/profiling/perfetto/.gitignore | 7 +++++++ selfdrive/debug/profiling/perfetto/build.sh | 11 +++++++++++ selfdrive/debug/profiling/perfetto/copy.sh | 6 ++++++ selfdrive/debug/profiling/perfetto/record.sh | 8 ++++++++ selfdrive/debug/profiling/perfetto/server.sh | 6 ++++++ selfdrive/debug/profiling/perfetto/traces.sh | 5 +++++ 6 files changed, 43 insertions(+) create mode 100644 selfdrive/debug/profiling/perfetto/.gitignore create mode 100755 selfdrive/debug/profiling/perfetto/build.sh create mode 100755 selfdrive/debug/profiling/perfetto/copy.sh create mode 100755 selfdrive/debug/profiling/perfetto/record.sh create mode 100755 selfdrive/debug/profiling/perfetto/server.sh create mode 100755 selfdrive/debug/profiling/perfetto/traces.sh diff --git a/selfdrive/debug/profiling/perfetto/.gitignore b/selfdrive/debug/profiling/perfetto/.gitignore new file mode 100644 index 0000000000..d86c89d462 --- /dev/null +++ b/selfdrive/debug/profiling/perfetto/.gitignore @@ -0,0 +1,7 @@ +trace_* + +tracebox +trace_processor + +perfetto/ +configs/ diff --git a/selfdrive/debug/profiling/perfetto/build.sh b/selfdrive/debug/profiling/perfetto/build.sh new file mode 100755 index 0000000000..448c1d04ae --- /dev/null +++ b/selfdrive/debug/profiling/perfetto/build.sh @@ -0,0 +1,11 @@ +#!/usr/bin/bash + +if [ ! -d perfetto ]; then + git clone https://android.googlesource.com/platform/external/perfetto/ +fi + +cd perfetto + +tools/install-build-deps --linux-arm +tools/gn gen --args='is_debug=false target_os="linux" target_cpu="arm64"' out/linux +tools/ninja -C out/linux tracebox traced traced_probes perfetto diff --git a/selfdrive/debug/profiling/perfetto/copy.sh b/selfdrive/debug/profiling/perfetto/copy.sh new file mode 100755 index 0000000000..8deca9a6e7 --- /dev/null +++ b/selfdrive/debug/profiling/perfetto/copy.sh @@ -0,0 +1,6 @@ +#!/usr/bin/bash + +DEST=tici:/data/openpilot/selfdrive/debug/profiling/perfetto + +scp -r perfetto/out/linux/tracebox $DEST +scp -r perfetto/test/configs $DEST diff --git a/selfdrive/debug/profiling/perfetto/record.sh b/selfdrive/debug/profiling/perfetto/record.sh new file mode 100755 index 0000000000..99f2168771 --- /dev/null +++ b/selfdrive/debug/profiling/perfetto/record.sh @@ -0,0 +1,8 @@ +#!/usr/bin/bash + +DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)" +cd $DIR + +OUT=trace_ +sudo ./tracebox -o $OUT --txt -c configs/scheduling.cfg +sudo chown $USER:$USER $OUT diff --git a/selfdrive/debug/profiling/perfetto/server.sh b/selfdrive/debug/profiling/perfetto/server.sh new file mode 100755 index 0000000000..19958c653b --- /dev/null +++ b/selfdrive/debug/profiling/perfetto/server.sh @@ -0,0 +1,6 @@ +#!/usr/bin/bash + +curl -LO https://get.perfetto.dev/trace_processor +chmod +x ./trace_processor + +./trace_processor --httpd diff --git a/selfdrive/debug/profiling/perfetto/traces.sh b/selfdrive/debug/profiling/perfetto/traces.sh new file mode 100755 index 0000000000..489b042377 --- /dev/null +++ b/selfdrive/debug/profiling/perfetto/traces.sh @@ -0,0 +1,5 @@ +#!/usr/bin/bash + +DEST=tici:/data/openpilot/selfdrive/debug/profiling/perfetto + +scp tici:/data/openpilot/selfdrive/debug/profiling/perfetto/trace_* .