sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

body: only control while engaged (#24114)

Browse Source 
* body: only control while engaged

* bump panda

* remove that

* bump panda

* update refs

* update refs
pull/24129/head
Adeeb Shihadeh 3 years ago committed by GitHub
parent 7df1c025ae
commit 00ebd10b87
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
6 changed files with 75 additions and 67 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      panda
  2. 121
      selfdrive/car/body/carcontroller.py
  3. 9
      selfdrive/car/body/interface.py
  4. 4
      selfdrive/controls/controlsd.py
  5. 2
      selfdrive/test/process_replay/ref_commit
  6. 4
      selfdrive/test/process_replay/test_processes.py

2
panda

@ -1 +1 @@
Subproject commit ddf1eb68ba6324022b95103994b11bedb615f608 Subproject commit ba5cbd5ee1ece245471933a65604f3e7f9520121

121
selfdrive/car/body/carcontroller.py
Unescape View File

@ -29,7 +29,6 @@ class CarController():
self.torque_r_filtered = 0. self.torque_r_filtered = 0.
self.torque_l_filtered = 0. self.torque_l_filtered = 0.
self.wait_counter = int(5. / DT_CTRL)
@staticmethod @staticmethod
def deadband_filter(torque, deadband): def deadband_filter(torque, deadband):
@ -40,67 +39,65 @@ class CarController():
return torque return torque
def update(self, CC, CS): def update(self, CC, CS):
if self.wait_counter > 0:
self.wait_counter -= 1 torque_l = 0
return CC.actuators.copy(), [] torque_r = 0
if len(CC.orientationNED) == 0 or len(CC.angularVelocity) == 0:
return CC.actuators.copy(), [] 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) # 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 # Mix steer into torque_diff
# self.steerRatio = 0.5 # self.steerRatio = 0.5
# torque_r = int(np.clip((CC.actuators.accel*1000) - (CC.actuators.steer*1000) * self.steerRatio, -1000, 1000)) # 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)) # torque_l = int(np.clip((CC.actuators.accel*1000) + (CC.actuators.steer*1000) * self.steerRatio, -1000, 1000))
# //// # ////
# Setpoint speed PID # Setpoint speed PID
kp_speed = 0.001 / SPEED_FROM_RPM kp_speed = 0.001 / SPEED_FROM_RPM
ki_speed = 0.001 / SPEED_FROM_RPM ki_speed = 0.001 / SPEED_FROM_RPM
alpha_speed = 1.0 alpha_speed = 1.0
self.speed_measured = SPEED_FROM_RPM * (CS.out.wheelSpeeds.fl + CS.out.wheelSpeeds.fr) / 2. self.speed_measured = SPEED_FROM_RPM * (CS.out.wheelSpeeds.fl + CS.out.wheelSpeeds.fr) / 2.
self.speed_desired = (1. - alpha_speed) * self.speed_desired self.speed_desired = (1. - alpha_speed) * self.speed_desired
speed_error = self.speed_desired - self.speed_measured speed_error = self.speed_desired - self.speed_measured
self.i_speed += speed_error * DT_CTRL self.i_speed += speed_error * DT_CTRL
self.i_speed = np.clip(self.i_speed, -MAX_POS_INTEGRATOR, MAX_POS_INTEGRATOR) 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 set_point = kp_speed * speed_error + ki_speed * self.i_speed
# Balancing PID # Balancing PID
kp_balance = 1300 kp_balance = 1300
ki_balance = 0 ki_balance = 0
kd_balance = 280 kd_balance = 280
# Clip angle error, this is enough to get up from stands # 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)) 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.i_balance += CS.out.wheelSpeeds.fl + CS.out.wheelSpeeds.fr
self.d_balance = np.clip(-CC.angularVelocity[1], -1., 1.) 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)) torque = int(np.clip((p_balance*kp_balance + self.i_balance*ki_balance + self.d_balance*kd_balance), -1000, 1000))
# yaw recovery PID # yaw recovery PID
kp_turn = 0.1 / SPEED_FROM_RPM kp_turn = 0.1 / SPEED_FROM_RPM
ki_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) 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 += speed_diff_measured * DT_CTRL
self.i_speed_diff = np.clip(self.i_speed_diff, -MAX_TURN_INTEGRATOR, MAX_TURN_INTEGRATOR) 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)) torque_diff = int(np.clip(kp_turn * speed_diff_measured + ki_turn * self.i_speed_diff, -100, 100))
# Combine 2 PIDs outputs # Combine 2 PIDs outputs
torque_r = torque + torque_diff torque_r = torque + torque_diff
torque_l = torque - torque_diff torque_l = torque - torque_diff
# Torque rate limits # Torque rate limits
self.torque_r_filtered = np.clip(self.deadband_filter(torque_r, 10), 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_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 = np.clip(self.deadband_filter(torque_l, 10),
self.torque_l_filtered - MAX_TORQUE_RATE, self.torque_l_filtered - MAX_TORQUE_RATE,
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_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 = int(np.clip(self.torque_l_filtered, -MAX_TORQUE, MAX_TORQUE))
# ///////////////////////////////////////
can_sends = [] can_sends = []
can_sends.append(bodycan.create_control(self.packer, torque_l, torque_r, self.frame // 2)) can_sends.append(bodycan.create_control(self.packer, torque_l, torque_r, self.frame // 2))

9
selfdrive/car/body/interface.py
Unescape View File

@ -1,5 +1,6 @@
#!/usr/bin/env python3 #!/usr/bin/env python3
from cereal import car 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 import scale_rot_inertia, scale_tire_stiffness, get_safety_config
from selfdrive.car.interfaces import CarInterfaceBase from selfdrive.car.interfaces import CarInterfaceBase
from selfdrive.car.body.values import SPEED_FROM_RPM from selfdrive.car.body.values import SPEED_FROM_RPM
@ -41,6 +42,14 @@ class CarInterface(CarInterfaceBase):
ret.canValid = self.cp.can_valid 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() self.CS.out = ret.as_reader()
return self.CS.out return self.CS.out

4
selfdrive/controls/controlsd.py
Unescape View File

@ -206,7 +206,9 @@ class Controls:
self.events.add(EventName.pedalPressed) self.events.add(EventName.pedalPressed)
self.events.add_from_msg(CS.events) 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 # Create events for battery, temperature, disk space, and memory
if EON and (self.sm['peripheralState'].pandaType != PandaType.uno) and \ if EON and (self.sm['peripheralState'].pandaType != PandaType.uno) and \

2
selfdrive/test/process_replay/ref_commit
Unescape View File

@ -1 +1 @@
594f0e291d2a9720c854f6d0f074b7734ed3c741 ad17c3c1d11691bc35efe3de78a7cef3daf5dbdf

4
selfdrive/test/process_replay/test_processes.py
Unescape View File

@ -12,7 +12,7 @@ from tools.lib.logreader import LogReader
original_segments = [ 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 ("HYUNDAI", "02c45f73a2e5c6e9|2021-01-01--19-08-22--1"), # HYUNDAI.SONATA
("TOYOTA", "0982d79ebb0de295|2021-01-04--17-13-21--13"), # TOYOTA.PRIUS (INDI) ("TOYOTA", "0982d79ebb0de295|2021-01-04--17-13-21--13"), # TOYOTA.PRIUS (INDI)
("TOYOTA2", "0982d79ebb0de295|2021-01-03--20-03-36--6"), # TOYOTA.RAV4 (LQR) ("TOYOTA2", "0982d79ebb0de295|2021-01-03--20-03-36--6"), # TOYOTA.RAV4 (LQR)
@ -31,7 +31,7 @@ original_segments = [
] ]
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"), ("HYUNDAI", "fakedata|2022-01-20--17-49-04--0"),
("TOYOTA", "fakedata|2022-01-20--17-50-51--0"), ("TOYOTA", "fakedata|2022-01-20--17-50-51--0"),
("TOYOTA2", "fakedata|2022-01-20--17-52-36--0"), ("TOYOTA2", "fakedata|2022-01-20--17-52-36--0"),

Write Preview
Loading…
Cancel
Save