spacecruft
/
nopenpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Add lateral accel based control

retune_corolla
Harald Schafer 2022-03-19 16:51:17 -07:00
parent e46d4c9f95
commit fd3fc02359
6 changed files with 18 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
selfdrive/controls/controlsd.py
View File

@ -518,7 +518,7 @@ class Controls:
lat_plan.curvatureRates)
actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.CP, self.VM,
params, self.last_actuators, desired_curvature,
desired_curvature_rate)
desired_curvature_rate, self.sm['liveLocationKalman'])
else:
lac_log = log.ControlsState.LateralDebugState.new_message()
if self.sm.rcv_frame['testJoystick'] > 0:

2
selfdrive/controls/lib/latcontrol.py
View File

@ -13,7 +13,7 @@ class LatControl(ABC):
self.sat_count = 0.
@abstractmethod
def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk):
pass
def reset(self):

2
selfdrive/controls/lib/latcontrol_angle.py
View File

@ -7,7 +7,7 @@ STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees
class LatControlAngle(LatControl):
def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk):
angle_log = log.ControlsState.LateralAngleState.new_message()
if CS.vEgo < MIN_STEER_SPEED or not active:

2
selfdrive/controls/lib/latcontrol_indi.py
View File

@ -65,7 +65,7 @@ class LatControlINDI(LatControl):
self.steer_filter.x = 0.
self.speed = 0.
def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk):
self.speed = CS.vEgo
# Update Kalman filter
y = np.array([[math.radians(CS.steeringAngleDeg)], [math.radians(CS.steeringRateDeg)]])

2
selfdrive/controls/lib/latcontrol_lqr.py
View File

@ -31,7 +31,7 @@ class LatControlLQR(LatControl):
super().reset()
self.i_lqr = 0.0
def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk):
lqr_log = log.ControlsState.LateralLQRState.new_message()
steers_max = get_steer_max(CP, CS.vEgo)

25
selfdrive/controls/lib/latcontrol_pid.py
View File

@ -1,5 +1,3 @@
import math
from selfdrive.controls.lib.pid import PIController
from selfdrive.controls.lib.drive_helpers import get_steer_max
from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED
@ -18,16 +16,16 @@ class LatControlPID(LatControl):
super().reset()
self.pid.reset()
def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk):
pid_log = log.ControlsState.LateralPIDState.new_message()
pid_log.steeringAngleDeg = float(CS.steeringAngleDeg)
pid_log.steeringRateDeg = float(CS.steeringRateDeg)
#pid_log.steeringAngleDeg = float(CS.steeringAngleDeg)
#pid_log.steeringRateDeg = float(CS.steeringRateDeg)
angle_steers_des_no_offset = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll))
angle_steers_des = angle_steers_des_no_offset + params.angleOffsetDeg
#angle_steers_des_no_offset = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll))
#angle_steers_des = angle_steers_des_no_offset + params.angleOffsetDeg
pid_log.steeringAngleDesiredDeg = angle_steers_des
pid_log.angleError = angle_steers_des - CS.steeringAngleDeg
#pid_log.steeringAngleDesiredDeg = angle_steers_des
#pid_log.angleError = angle_steers_des - CS.steeringAngleDeg
if CS.vEgo < MIN_STEER_SPEED or not active:
output_steer = 0.0
pid_log.active = False
@ -37,12 +35,15 @@ class LatControlPID(LatControl):
self.pid.pos_limit = steers_max
self.pid.neg_limit = -steers_max
lateral_accel_desired = desired_curvature * CS.vEgo**2
lateral_accel_actual = llk.angularVelocityCalibrated.value[0] * CS.vEgo
# offset does not contribute to resistive torque
steer_feedforward = self.get_steer_feedforward(angle_steers_des_no_offset, CS.vEgo)
steer_feedforward = (lateral_accel_desired - 9.81 * llk.orientationNED.value[0])/2.0
deadzone = 0.0
output_steer = self.pid.update(angle_steers_des, CS.steeringAngleDeg, override=CS.steeringPressed,
output_steer = self.pid.update(lateral_accel_desired, lateral_accel_actual, override=CS.steeringPressed,
feedforward=steer_feedforward, speed=CS.vEgo, deadzone=deadzone)
pid_log.active = True
pid_log.p = self.pid.p
@ -51,4 +52,4 @@ class LatControlPID(LatControl):
pid_log.output = output_steer
pid_log.saturated = self._check_saturation(steers_max - abs(output_steer) < 1e-3, CS)
return output_steer, angle_steers_des, pid_log
return output_steer, 0.0, pid_log