mawei/MoreTore
1
0
Fork 0
mirror of https://github.com/MoreTore/openpilot.git synced 2026-08-05 16:26:14 +08:00
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
This commit is contained in:
Adeeb Shihadeh
2022-04-04 19:37:21 -07:00
committed by GitHub
parent 7df1c025ae
commit 00ebd10b87
6 changed files with 65 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files
panda
+1 -1
Submodule panda updated: ddf1eb68ba...ba5cbd5ee1
selfdrive/car/body/carcontroller.py
+49 -52
View File
@@ -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))
# ////
torque_l = 0
torque_r = 0
# Setpoint speed PID
kp_speed = 0.001 / SPEED_FROM_RPM
ki_speed = 0.001 / SPEED_FROM_RPM
alpha_speed = 1.0
llk_valid = len(CC.orientationNED) > 0 and len(CC.angularVelocity) > 0
if CC.enabled and llk_valid:
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
# 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))
# ////
# Balancing PID
kp_balance = 1300
ki_balance = 0
kd_balance = 280
# Setpoint speed PID
kp_speed = 0.001 / SPEED_FROM_RPM
ki_speed = 0.001 / SPEED_FROM_RPM
alpha_speed = 1.0
# 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))
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
# yaw recovery PID
kp_turn = 0.1 / SPEED_FROM_RPM
ki_turn = 0.1 / SPEED_FROM_RPM
# Balancing PID
kp_balance = 1300
ki_balance = 0
kd_balance = 280
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))
# 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))
# Combine 2 PIDs outputs
torque_r = torque + torque_diff
torque_l = torque - torque_diff
# yaw recovery PID
kp_turn = 0.1 / SPEED_FROM_RPM
ki_turn = 0.1 / SPEED_FROM_RPM
# 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))
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))
selfdrive/car/body/interface.py
+9
View File
@@ -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
selfdrive/controls/controlsd.py
+3 -1
View File
@@ -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 \
selfdrive/test/process_replay/ref_commit
+1 -1
View File
@@ -1 +1 @@
594f0e291d2a9720c854f6d0f074b7734ed3c741
ad17c3c1d11691bc35efe3de78a7cef3daf5dbdf
selfdrive/test/process_replay/test_processes.py
+2 -2
View File
@@ -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"),