From dcece099fff5def6c6d8feea6883ab0e0ee5b661 Mon Sep 17 00:00:00 2001 From: HaraldSchafer Date: Wed, 16 Mar 2022 21:12:11 -0700 Subject: [PATCH] Add angular rates carcontroller packet (#23980) * Controlsd: give carcontroller angular rates * Update cereal * Needs to be list * Update refs --- cereal | 2 +- selfdrive/controls/controlsd.py | 10 +++++++--- selfdrive/test/process_replay/ref_commit | 2 +- 3 files changed, 9 insertions(+), 5 deletions(-) diff --git a/cereal b/cereal index 64b601419..85406f667 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 64b6014193a69fe2e2b3b7a17f9e228162081ddf +Subproject commit 85406f66760b30700c27aace228f5e480f3a5bbe diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 1a911ef98..fdc0efb77 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -572,10 +572,14 @@ class Controls: def publish_logs(self, CS, start_time, CC, lac_log): """Send actuators and hud commands to the car, send controlsstate and MPC logging""" - orientation_value = self.sm['liveLocationKalman'].orientationNED.value + # Orientation and angle rates can be useful for carcontroller + # Only calibrated (car) frame is relevant for the carcontroller + orientation_value = list(self.sm['liveLocationKalman'].calibratedOrientationNED.value) if len(orientation_value) > 2: - CC.roll = orientation_value[0] - CC.pitch = orientation_value[1] + CC.orientationNED = orientation_value + angular_rate_value = list(self.sm['liveLocationKalman'].angularVelocityCalibrated.value) + if len(angular_rate_value) > 2: + CC.angularVelocity = angular_rate_value CC.cruiseControl.cancel = CS.cruiseState.enabled and (not self.enabled or not self.CP.pcmCruise) if self.joystick_mode and self.sm.rcv_frame['testJoystick'] > 0 and self.sm['testJoystick'].buttons[0]: diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index f9edf2f88..4802acb9f 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -927918307b45657df0a4ac0255c3e8e3dc62d7cd \ No newline at end of file +61bac237e228da19e59d456c883f99a92172d217 \ No newline at end of file