Compare commits
6 Commits
master
...
craycrayco
Author | SHA1 | Date |
---|---|---|
Harald Schafer | 3872110323 | |
Harald Schafer | df5cad47c1 | |
Harald Schafer | fa0fb63ad6 | |
Harald Schafer | 1838ad9852 | |
Harald Schafer | c5d125fd39 | |
Harald Schafer | 37f0283f65 |
2
cereal
2
cereal
|
@ -1 +1 @@
|
|||
Subproject commit e94649e0ed1f70fe5c74c3700a9330b878606101
|
||||
Subproject commit b54fa0f921eb809b4550703eb7a34597934da8a1
|
|
@ -35,7 +35,7 @@ class CarInterface(CarInterfaceBase):
|
|||
tire_stiffness_factor = 0.6371 # hand-tune
|
||||
ret.mass = 3045. * CV.LB_TO_KG + STD_CARGO_KG
|
||||
|
||||
set_lat_tune(ret.lateralTuning, LatTunes.INDI_PRIUS)
|
||||
set_lat_tune(ret.lateralTuning, LatTunes.CRAYCRAY_PRIUS)
|
||||
ret.steerActuatorDelay = 0.3
|
||||
|
||||
elif candidate == CAR.PRIUS_V:
|
||||
|
|
|
@ -8,7 +8,7 @@ class LongTunes(Enum):
|
|||
TSS = 2
|
||||
|
||||
class LatTunes(Enum):
|
||||
INDI_PRIUS = 0
|
||||
CRAYCRAY_PRIUS = 0
|
||||
LQR_RAV4 = 1
|
||||
PID_A = 2
|
||||
PID_B = 3
|
||||
|
@ -50,16 +50,14 @@ def set_long_tune(tune, name):
|
|||
|
||||
###### LAT ######
|
||||
def set_lat_tune(tune, name):
|
||||
if name == LatTunes.INDI_PRIUS:
|
||||
tune.init('indi')
|
||||
tune.indi.innerLoopGainBP = [0.]
|
||||
tune.indi.innerLoopGainV = [4.0]
|
||||
tune.indi.outerLoopGainBP = [0.]
|
||||
tune.indi.outerLoopGainV = [3.0]
|
||||
tune.indi.timeConstantBP = [0.]
|
||||
tune.indi.timeConstantV = [1.0]
|
||||
tune.indi.actuatorEffectivenessBP = [0.]
|
||||
tune.indi.actuatorEffectivenessV = [1.0]
|
||||
if name == LatTunes.CRAYCRAY_PRIUS:
|
||||
tune.init('craycray')
|
||||
tune.craycray.kiBP = [0.0]
|
||||
tune.craycray.kpBP = [0.0]
|
||||
MAX_TORQUE = 1.5
|
||||
tune.craycray.kpV = [2.0/MAX_TORQUE]
|
||||
tune.craycray.kiV = [0.025/MAX_TORQUE]
|
||||
tune.craycray.kf = 0.5 / MAX_TORQUE
|
||||
|
||||
elif name == LatTunes.LQR_RAV4:
|
||||
tune.init('lqr')
|
||||
|
|
|
@ -18,7 +18,7 @@ class CarControllerParams:
|
|||
ACCEL_MIN = -3.5 # m/s2
|
||||
|
||||
STEER_MAX = 1500
|
||||
STEER_DELTA_UP = 10 # 1.5s time to peak torque
|
||||
STEER_DELTA_UP = 15 # 1.0s time to peak torque
|
||||
STEER_DELTA_DOWN = 25 # always lower than 45 otherwise the Rav4 faults (Prius seems ok with 50)
|
||||
STEER_ERROR_MAX = 350 # max delta between torque cmd and torque motor
|
||||
|
||||
|
|
|
@ -21,6 +21,7 @@ from selfdrive.controls.lib.latcontrol_pid import LatControlPID
|
|||
from selfdrive.controls.lib.latcontrol_indi import LatControlINDI
|
||||
from selfdrive.controls.lib.latcontrol_lqr import LatControlLQR
|
||||
from selfdrive.controls.lib.latcontrol_angle import LatControlAngle
|
||||
from selfdrive.controls.lib.latcontrol_craycray import LatControlCrayCray
|
||||
from selfdrive.controls.lib.events import Events, ET
|
||||
from selfdrive.controls.lib.alertmanager import AlertManager, set_offroad_alert
|
||||
from selfdrive.controls.lib.vehicle_model import VehicleModel
|
||||
|
@ -135,6 +136,8 @@ class Controls:
|
|||
self.LaC = LatControlINDI(self.CP, self.CI)
|
||||
elif self.CP.lateralTuning.which() == 'lqr':
|
||||
self.LaC = LatControlLQR(self.CP, self.CI)
|
||||
elif self.CP.lateralTuning.which() == 'craycray':
|
||||
self.LaC = LatControlCrayCray(self.CP, self.CI)
|
||||
|
||||
self.initialized = False
|
||||
self.state = State.disabled
|
||||
|
@ -518,7 +521,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:
|
||||
|
|
|
@ -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):
|
||||
|
|
|
@ -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:
|
||||
|
|
|
@ -0,0 +1,58 @@
|
|||
from selfdrive.controls.lib.pid import PIController
|
||||
from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED
|
||||
from cereal import log
|
||||
|
||||
CURVATURE_SCALE = 200
|
||||
|
||||
|
||||
class LatControlCrayCray(LatControl):
|
||||
def __init__(self, CP, CI):
|
||||
super().__init__(CP, CI)
|
||||
self.pid = PIController((CP.lateralTuning.craycray.kpBP, CP.lateralTuning.craycray.kpV),
|
||||
(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
|
||||
self.use_steering_angle = False
|
||||
|
||||
def reset(self):
|
||||
super().reset()
|
||||
self.pid.reset()
|
||||
|
||||
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.steeringAngleDesiredDeg = angle_steers_des
|
||||
#pid_log.angleError = angle_steers_des - CS.steeringAngleDeg
|
||||
if CS.vEgo < MIN_STEER_SPEED or not active:
|
||||
output_torque = 0.0
|
||||
pid_log.active = False
|
||||
self.pid.reset()
|
||||
else:
|
||||
# TODO lateral acceleration works great at high speed, not so much at low speed
|
||||
if self.use_steering_angle:
|
||||
actual_curvature = VM.calc_curvature(-(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll)
|
||||
else:
|
||||
actual_curvature = llk.angularVelocityCalibrated.value[2] / CS.vEgo
|
||||
desired_lateral_accel = desired_curvature * CS.vEgo**2
|
||||
actual_lateral_accel = actual_curvature * CS.vEgo**2
|
||||
|
||||
setpoint = desired_lateral_accel + CURVATURE_SCALE * desired_curvature
|
||||
measurement = actual_lateral_accel + CURVATURE_SCALE * actual_curvature
|
||||
|
||||
output_torque = self.pid.update(setpoint, measurement, override=CS.steeringPressed,
|
||||
feedforward=desired_lateral_accel, speed=CS.vEgo)
|
||||
pid_log.active = True
|
||||
pid_log.p = self.pid.p
|
||||
pid_log.i = self.pid.i
|
||||
pid_log.f = self.pid.f
|
||||
#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)
|
||||
|
||||
#TODO left is positive in this convention
|
||||
return -output_torque, 0.0, pid_log
|
|
@ -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)]])
|
||||
|
|
|
@ -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)
|
||||
|
|
|
@ -18,7 +18,7 @@ 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)
|
||||
|
|
Loading…
Reference in New Issue