From 00ebd10b87d6d96f4b801879c281927cab82d27c Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Mon, 4 Apr 2022 19:37:21 -0700 Subject: [PATCH] body: only control while engaged (#24114) * body: only control while engaged * bump panda * remove that * bump panda * update refs * update refs --- panda | 2 +- selfdrive/car/body/carcontroller.py | 121 +++++++++--------- selfdrive/car/body/interface.py | 9 ++ selfdrive/controls/controlsd.py | 4 +- selfdrive/test/process_replay/ref_commit | 2 +- .../test/process_replay/test_processes.py | 4 +- 6 files changed, 75 insertions(+), 67 deletions(-) diff --git a/panda b/panda index ddf1eb68ba..ba5cbd5ee1 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit ddf1eb68ba6324022b95103994b11bedb615f608 +Subproject commit ba5cbd5ee1ece245471933a65604f3e7f9520121 diff --git a/selfdrive/car/body/carcontroller.py b/selfdrive/car/body/carcontroller.py index 57910746ee..39e27ff0d9 100644 --- a/selfdrive/car/body/carcontroller.py +++ b/selfdrive/car/body/carcontroller.py @@ -29,7 +29,6 @@ class CarController(): self.torque_r_filtered = 0. self.torque_l_filtered = 0. - self.wait_counter = int(5. / DT_CTRL) @staticmethod def deadband_filter(torque, deadband): @@ -40,67 +39,65 @@ class CarController(): return torque def update(self, CC, CS): - if self.wait_counter > 0: - self.wait_counter -= 1 - return CC.actuators.copy(), [] - if len(CC.orientationNED) == 0 or len(CC.angularVelocity) == 0: - return CC.actuators.copy(), [] - - # /////////////////////////////////////// - # Steer and accel mixin. Speed should be used as a target? (speed should be in m/s! now it is in RPM) - # Mix steer into torque_diff - # self.steerRatio = 0.5 - # torque_r = int(np.clip((CC.actuators.accel*1000) - (CC.actuators.steer*1000) * self.steerRatio, -1000, 1000)) - # torque_l = int(np.clip((CC.actuators.accel*1000) + (CC.actuators.steer*1000) * self.steerRatio, -1000, 1000)) - # //// - - # Setpoint speed PID - kp_speed = 0.001 / SPEED_FROM_RPM - ki_speed = 0.001 / SPEED_FROM_RPM - alpha_speed = 1.0 - - self.speed_measured = SPEED_FROM_RPM * (CS.out.wheelSpeeds.fl + CS.out.wheelSpeeds.fr) / 2. - self.speed_desired = (1. - alpha_speed) * self.speed_desired - speed_error = self.speed_desired - self.speed_measured - self.i_speed += speed_error * DT_CTRL - self.i_speed = np.clip(self.i_speed, -MAX_POS_INTEGRATOR, MAX_POS_INTEGRATOR) - set_point = kp_speed * speed_error + ki_speed * self.i_speed - - # Balancing PID - kp_balance = 1300 - ki_balance = 0 - kd_balance = 280 - - # Clip angle error, this is enough to get up from stands - p_balance = np.clip((-CC.orientationNED[1]) - set_point, np.radians(-MAX_ANGLE_ERROR), np.radians(MAX_ANGLE_ERROR)) - self.i_balance += CS.out.wheelSpeeds.fl + CS.out.wheelSpeeds.fr - self.d_balance = np.clip(-CC.angularVelocity[1], -1., 1.) - torque = int(np.clip((p_balance*kp_balance + self.i_balance*ki_balance + self.d_balance*kd_balance), -1000, 1000)) - - # yaw recovery PID - kp_turn = 0.1 / SPEED_FROM_RPM - ki_turn = 0.1 / SPEED_FROM_RPM - - speed_diff_measured = SPEED_FROM_RPM * (CS.out.wheelSpeeds.fl - CS.out.wheelSpeeds.fr) - self.i_speed_diff += speed_diff_measured * DT_CTRL - self.i_speed_diff = np.clip(self.i_speed_diff, -MAX_TURN_INTEGRATOR, MAX_TURN_INTEGRATOR) - torque_diff = int(np.clip(kp_turn * speed_diff_measured + ki_turn * self.i_speed_diff, -100, 100)) - - # Combine 2 PIDs outputs - torque_r = torque + torque_diff - torque_l = torque - torque_diff - - # Torque rate limits - self.torque_r_filtered = np.clip(self.deadband_filter(torque_r, 10), - self.torque_r_filtered - MAX_TORQUE_RATE, - self.torque_r_filtered + MAX_TORQUE_RATE) - self.torque_l_filtered = np.clip(self.deadband_filter(torque_l, 10), - self.torque_l_filtered - MAX_TORQUE_RATE, - self.torque_l_filtered + MAX_TORQUE_RATE) - torque_r = int(np.clip(self.torque_r_filtered, -MAX_TORQUE, MAX_TORQUE)) - torque_l = int(np.clip(self.torque_l_filtered, -MAX_TORQUE, MAX_TORQUE)) - - # /////////////////////////////////////// + + torque_l = 0 + torque_r = 0 + + llk_valid = len(CC.orientationNED) > 0 and len(CC.angularVelocity) > 0 + if CC.enabled and llk_valid: + + # Steer and accel mixin. Speed should be used as a target? (speed should be in m/s! now it is in RPM) + # Mix steer into torque_diff + # self.steerRatio = 0.5 + # torque_r = int(np.clip((CC.actuators.accel*1000) - (CC.actuators.steer*1000) * self.steerRatio, -1000, 1000)) + # torque_l = int(np.clip((CC.actuators.accel*1000) + (CC.actuators.steer*1000) * self.steerRatio, -1000, 1000)) + # //// + + # Setpoint speed PID + kp_speed = 0.001 / SPEED_FROM_RPM + ki_speed = 0.001 / SPEED_FROM_RPM + alpha_speed = 1.0 + + self.speed_measured = SPEED_FROM_RPM * (CS.out.wheelSpeeds.fl + CS.out.wheelSpeeds.fr) / 2. + self.speed_desired = (1. - alpha_speed) * self.speed_desired + speed_error = self.speed_desired - self.speed_measured + self.i_speed += speed_error * DT_CTRL + self.i_speed = np.clip(self.i_speed, -MAX_POS_INTEGRATOR, MAX_POS_INTEGRATOR) + set_point = kp_speed * speed_error + ki_speed * self.i_speed + + # Balancing PID + kp_balance = 1300 + ki_balance = 0 + kd_balance = 280 + + # Clip angle error, this is enough to get up from stands + p_balance = np.clip((-CC.orientationNED[1]) - set_point, np.radians(-MAX_ANGLE_ERROR), np.radians(MAX_ANGLE_ERROR)) + self.i_balance += CS.out.wheelSpeeds.fl + CS.out.wheelSpeeds.fr + self.d_balance = np.clip(-CC.angularVelocity[1], -1., 1.) + torque = int(np.clip((p_balance*kp_balance + self.i_balance*ki_balance + self.d_balance*kd_balance), -1000, 1000)) + + # yaw recovery PID + kp_turn = 0.1 / SPEED_FROM_RPM + ki_turn = 0.1 / SPEED_FROM_RPM + + speed_diff_measured = SPEED_FROM_RPM * (CS.out.wheelSpeeds.fl - CS.out.wheelSpeeds.fr) + self.i_speed_diff += speed_diff_measured * DT_CTRL + self.i_speed_diff = np.clip(self.i_speed_diff, -MAX_TURN_INTEGRATOR, MAX_TURN_INTEGRATOR) + torque_diff = int(np.clip(kp_turn * speed_diff_measured + ki_turn * self.i_speed_diff, -100, 100)) + + # Combine 2 PIDs outputs + torque_r = torque + torque_diff + torque_l = torque - torque_diff + + # Torque rate limits + self.torque_r_filtered = np.clip(self.deadband_filter(torque_r, 10), + self.torque_r_filtered - MAX_TORQUE_RATE, + self.torque_r_filtered + MAX_TORQUE_RATE) + self.torque_l_filtered = np.clip(self.deadband_filter(torque_l, 10), + self.torque_l_filtered - MAX_TORQUE_RATE, + self.torque_l_filtered + MAX_TORQUE_RATE) + torque_r = int(np.clip(self.torque_r_filtered, -MAX_TORQUE, MAX_TORQUE)) + torque_l = int(np.clip(self.torque_l_filtered, -MAX_TORQUE, MAX_TORQUE)) can_sends = [] can_sends.append(bodycan.create_control(self.packer, torque_l, torque_r, self.frame // 2)) diff --git a/selfdrive/car/body/interface.py b/selfdrive/car/body/interface.py index 8f36e1ed0d..d2e097fea8 100644 --- a/selfdrive/car/body/interface.py +++ b/selfdrive/car/body/interface.py @@ -1,5 +1,6 @@ #!/usr/bin/env python3 from cereal import car +from common.realtime import DT_CTRL from selfdrive.car import scale_rot_inertia, scale_tire_stiffness, get_safety_config from selfdrive.car.interfaces import CarInterfaceBase from selfdrive.car.body.values import SPEED_FROM_RPM @@ -41,6 +42,14 @@ class CarInterface(CarInterfaceBase): ret.canValid = self.cp.can_valid + # wait for everything to init first + if self.frame > int(5. / DT_CTRL): + # body always wants to enable + ret.init('events', 1) + ret.events[0].name = car.CarEvent.EventName.pcmEnable + ret.events[0].enable = True + self.frame += 1 + self.CS.out = ret.as_reader() return self.CS.out diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 3525e70abb..3ad8ce166f 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -206,7 +206,9 @@ class Controls: self.events.add(EventName.pedalPressed) self.events.add_from_msg(CS.events) - self.events.add_from_msg(self.sm['driverMonitoringState'].events) + + if not self.CP.notCar: + self.events.add_from_msg(self.sm['driverMonitoringState'].events) # Create events for battery, temperature, disk space, and memory if EON and (self.sm['peripheralState'].pandaType != PandaType.uno) and \ diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index e835264cad..a0cebb0961 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -594f0e291d2a9720c854f6d0f074b7734ed3c741 \ No newline at end of file +ad17c3c1d11691bc35efe3de78a7cef3daf5dbdf \ No newline at end of file diff --git a/selfdrive/test/process_replay/test_processes.py b/selfdrive/test/process_replay/test_processes.py index a8352ef26b..9c64c00dc3 100755 --- a/selfdrive/test/process_replay/test_processes.py +++ b/selfdrive/test/process_replay/test_processes.py @@ -12,7 +12,7 @@ from tools.lib.logreader import LogReader original_segments = [ - ("BODY", "d6ac8ebdb47bc549|2022-03-31--13-10-06--4"), # COMMA.BODY + ("BODY", "bd6a637565e91581|2022-04-04--19-14-27--0"), # COMMA.BODY ("HYUNDAI", "02c45f73a2e5c6e9|2021-01-01--19-08-22--1"), # HYUNDAI.SONATA ("TOYOTA", "0982d79ebb0de295|2021-01-04--17-13-21--13"), # TOYOTA.PRIUS (INDI) ("TOYOTA2", "0982d79ebb0de295|2021-01-03--20-03-36--6"), # TOYOTA.RAV4 (LQR) @@ -31,7 +31,7 @@ original_segments = [ ] segments = [ - ("BODY", "d6ac8ebdb47bc549|2022-03-31--13-10-06--4"), + ("BODY", "bd6a637565e91581|2022-04-04--19-14-27--0"), ("HYUNDAI", "fakedata|2022-01-20--17-49-04--0"), ("TOYOTA", "fakedata|2022-01-20--17-50-51--0"), ("TOYOTA2", "fakedata|2022-01-20--17-52-36--0"),