|
|
|
@ -40,7 +40,7 @@ class Controls: |
|
|
|
|
'driverMonitoringState', 'onroadEvents', 'driverAssistance'], poll='selfdriveState') |
|
|
|
|
self.pm = messaging.PubMaster(['carControl', 'controlsState']) |
|
|
|
|
|
|
|
|
|
self.steer_limited = False |
|
|
|
|
self.steer_limited_by_controls = False |
|
|
|
|
self.desired_curvature = 0.0 |
|
|
|
|
|
|
|
|
|
self.pose_calibrator = PoseCalibrator() |
|
|
|
@ -112,7 +112,7 @@ class Controls: |
|
|
|
|
self.desired_curvature = clip_curvature(CS.vEgo, self.desired_curvature, model_v2.action.desiredCurvature) |
|
|
|
|
actuators.curvature = float(self.desired_curvature) |
|
|
|
|
steer, steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp, |
|
|
|
|
self.steer_limited, self.desired_curvature, |
|
|
|
|
self.steer_limited_by_controls, self.desired_curvature, |
|
|
|
|
self.calibrated_pose) # TODO what if not available |
|
|
|
|
actuators.torque = float(steer) |
|
|
|
|
actuators.steeringAngleDeg = float(steeringAngleDeg) |
|
|
|
@ -161,10 +161,10 @@ class Controls: |
|
|
|
|
if self.sm['selfdriveState'].active: |
|
|
|
|
CO = self.sm['carOutput'] |
|
|
|
|
if self.CP.steerControlType == car.CarParams.SteerControlType.angle: |
|
|
|
|
self.steer_limited = abs(CC.actuators.steeringAngleDeg - CO.actuatorsOutput.steeringAngleDeg) > \ |
|
|
|
|
STEER_ANGLE_SATURATION_THRESHOLD |
|
|
|
|
self.steer_limited_by_controls = abs(CC.actuators.steeringAngleDeg - CO.actuatorsOutput.steeringAngleDeg) > \ |
|
|
|
|
STEER_ANGLE_SATURATION_THRESHOLD |
|
|
|
|
else: |
|
|
|
|
self.steer_limited = abs(CC.actuators.torque - CO.actuatorsOutput.torque) > 1e-2 |
|
|
|
|
self.steer_limited_by_controls = abs(CC.actuators.torque - CO.actuatorsOutput.torque) > 1e-2 |
|
|
|
|
|
|
|
|
|
# TODO: both controlsState and carControl valids should be set by |
|
|
|
|
# sm.all_checks(), but this creates a circular dependency |
|
|
|
|