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

Cleanup cray cray control

craycraycontrol2
Harald Schafer 2022-03-25 16:57:33 -07:00
parent 1838ad9852
commit fa0fb63ad6
1 changed files with 14 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
selfdrive/controls/lib/latcontrol_craycray.py
View File

@ -1,5 +1,4 @@
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
from cereal import log
@ -11,6 +10,9 @@ class LatControlCrayCray(LatControl):
(CP.lateralTuning.craycray.kiBP, CP.lateralTuning.craycray.kiV),
k_f=CP.lateralTuning.craycray.kf, pos_limit=1.0, neg_limit=-1.0)
self.get_steer_feedforward = CI.get_steer_feedforward_function()
self.steer_max = 1.0
self.pid.pos_limit = self.steer_max
self.pid.neg_limit = -self.steer_max
def reset(self):
super().reset()
@ -18,38 +20,31 @@ class LatControlCrayCray(LatControl):
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)
#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.steeringAngleDeg = float(CS.steeringAngleDeg)
pid_log.steeringRateDeg = float(CS.steeringRateDeg)
#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
output_torque = 0.0
pid_log.active = False
self.pid.reset()
else:
steers_max = get_steer_max(CP, CS.vEgo)
self.pid.pos_limit = steers_max
self.pid.neg_limit = -steers_max
# TODO lateral acceleration works great at high speed, not so much at low speed
lateral_accel_desired = desired_curvature * CS.vEgo**2
lateral_accel_actual = llk.angularVelocityCalibrated.value[2] * CS.vEgo
# offset does not contribute to resistive torque
steer_feedforward = -(lateral_accel_desired - 9.81 * llk.orientationNED.value[0])/2.0
deadzone = 0.0
output_steer = self.pid.update(-lateral_accel_desired, -lateral_accel_actual, override=CS.steeringPressed,
feedforward=steer_feedforward, speed=CS.vEgo, deadzone=deadzone)
output_torque = self.pid.update(lateral_accel_desired, lateral_accel_actual, override=CS.steeringPressed,
feedforward=lateral_accel_desired, speed=CS.vEgo, deadzone=deadzone)
pid_log.active = True
pid_log.p = self.pid.p
pid_log.i = self.pid.i
pid_log.f = self.pid.f
pid_log.output = output_steer
pid_log.saturated = self._check_saturation(steers_max - abs(output_steer) < 1e-3, CS)
#TODO left is positive in this log
pid_log.output = -output_torque
pid_log.saturated = self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS)
return output_steer, 0.0, pid_log
#TODO left is positive in this convention
return -output_torque, 0.0, pid_log