rename steer_limited (#34763)

rename
pull/34766/head
Shane Smiskol 2 months ago committed by GitHub
parent 159b1c9eb4
commit 68d22b960b
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
  1. 8
      selfdrive/controls/controlsd.py
  2. 6
      selfdrive/controls/lib/latcontrol.py
  3. 2
      selfdrive/controls/lib/latcontrol_angle.py
  4. 4
      selfdrive/controls/lib/latcontrol_pid.py
  5. 6
      selfdrive/controls/lib/latcontrol_torque.py
  6. 4
      tools/tuning/measure_steering_accuracy.py

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

@ -17,14 +17,14 @@ class LatControl(ABC):
self.steer_max = 1.0 self.steer_max = 1.0
@abstractmethod @abstractmethod
def update(self, active, CS, VM, params, steer_limited, desired_curvature, calibrated_pose): def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose):
pass pass
def reset(self): def reset(self):
self.sat_count = 0. self.sat_count = 0.
def _check_saturation(self, saturated, CS, steer_limited): def _check_saturation(self, saturated, CS, steer_limited_by_controls):
if saturated and CS.vEgo > self.sat_check_min_speed and not steer_limited and not CS.steeringPressed: if saturated and CS.vEgo > self.sat_check_min_speed and not steer_limited_by_controls and not CS.steeringPressed:
self.sat_count += self.sat_count_rate self.sat_count += self.sat_count_rate
else: else:
self.sat_count -= self.sat_count_rate self.sat_count -= self.sat_count_rate

@ -11,7 +11,7 @@ class LatControlAngle(LatControl):
super().__init__(CP, CI) super().__init__(CP, CI)
self.sat_check_min_speed = 5. self.sat_check_min_speed = 5.
def update(self, active, CS, VM, params, steer_limited, desired_curvature, calibrated_pose): def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose):
angle_log = log.ControlsState.LateralAngleState.new_message() angle_log = log.ControlsState.LateralAngleState.new_message()
if not active: if not active:

@ -17,7 +17,7 @@ class LatControlPID(LatControl):
super().reset() super().reset()
self.pid.reset() self.pid.reset()
def update(self, active, CS, VM, params, steer_limited, desired_curvature, calibrated_pose): def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose):
pid_log = log.ControlsState.LateralPIDState.new_message() pid_log = log.ControlsState.LateralPIDState.new_message()
pid_log.steeringAngleDeg = float(CS.steeringAngleDeg) pid_log.steeringAngleDeg = float(CS.steeringAngleDeg)
pid_log.steeringRateDeg = float(CS.steeringRateDeg) pid_log.steeringRateDeg = float(CS.steeringRateDeg)
@ -43,6 +43,6 @@ class LatControlPID(LatControl):
pid_log.i = float(self.pid.i) pid_log.i = float(self.pid.i)
pid_log.f = float(self.pid.f) pid_log.f = float(self.pid.f)
pid_log.output = float(output_steer) pid_log.output = float(output_steer)
pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_steer) < 1e-3, CS, steer_limited)) pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_steer) < 1e-3, CS, steer_limited_by_controls))
return output_steer, angle_steers_des, pid_log return output_steer, angle_steers_des, pid_log

@ -37,7 +37,7 @@ class LatControlTorque(LatControl):
self.torque_params.latAccelOffset = latAccelOffset self.torque_params.latAccelOffset = latAccelOffset
self.torque_params.friction = friction self.torque_params.friction = friction
def update(self, active, CS, VM, params, steer_limited, desired_curvature, calibrated_pose): def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose):
pid_log = log.ControlsState.LateralTorqueState.new_message() pid_log = log.ControlsState.LateralTorqueState.new_message()
if not active: if not active:
output_torque = 0.0 output_torque = 0.0
@ -73,7 +73,7 @@ class LatControlTorque(LatControl):
desired_lateral_accel - actual_lateral_accel, lateral_accel_deadzone, friction_compensation=True, desired_lateral_accel - actual_lateral_accel, lateral_accel_deadzone, friction_compensation=True,
gravity_adjusted=True) gravity_adjusted=True)
freeze_integrator = steer_limited or CS.steeringPressed or CS.vEgo < 5 freeze_integrator = steer_limited_by_controls or CS.steeringPressed or CS.vEgo < 5
output_torque = self.pid.update(pid_log.error, output_torque = self.pid.update(pid_log.error,
feedforward=ff, feedforward=ff,
speed=CS.vEgo, speed=CS.vEgo,
@ -87,7 +87,7 @@ class LatControlTorque(LatControl):
pid_log.output = float(-output_torque) pid_log.output = float(-output_torque)
pid_log.actualLateralAccel = float(actual_lateral_accel) pid_log.actualLateralAccel = float(actual_lateral_accel)
pid_log.desiredLateralAccel = float(desired_lateral_accel) pid_log.desiredLateralAccel = float(desired_lateral_accel)
pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited)) pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_controls))
# TODO left is positive in this convention # TODO left is positive in this convention
return -output_torque, 0.0, pid_log return -output_torque, 0.0, pid_log

@ -49,7 +49,7 @@ class SteeringAccuracyTool:
active = sm['controlsState'].active active = sm['controlsState'].active
steer = sm['carOutput'].actuatorsOutput.torque steer = sm['carOutput'].actuatorsOutput.torque
standstill = sm['carState'].standstill standstill = sm['carState'].standstill
steer_limited = abs(sm['carControl'].actuators.torque - sm['carControl'].actuatorsOutput.torque) > 1e-2 steer_limited_by_controls = abs(sm['carControl'].actuators.torque - sm['carControl'].actuatorsOutput.torque) > 1e-2
overriding = sm['carState'].steeringPressed overriding = sm['carState'].steeringPressed
changing_lanes = sm['modelV2'].meta.laneChangeState != 0 changing_lanes = sm['modelV2'].meta.laneChangeState != 0
model_points = sm['modelV2'].position.y model_points = sm['modelV2'].position.y
@ -77,7 +77,7 @@ class SteeringAccuracyTool:
self.speed_group_stats[group][angle_abs]["steer"] += abs(steer) self.speed_group_stats[group][angle_abs]["steer"] += abs(steer)
if len(model_points): if len(model_points):
self.speed_group_stats[group][angle_abs]["dpp"] += abs(model_points[0]) self.speed_group_stats[group][angle_abs]["dpp"] += abs(model_points[0])
if steer_limited: if steer_limited_by_controls:
self.speed_group_stats[group][angle_abs]["limited"] += 1 self.speed_group_stats[group][angle_abs]["limited"] += 1
if control_state.saturated: if control_state.saturated:
self.speed_group_stats[group][angle_abs]["saturated"] += 1 self.speed_group_stats[group][angle_abs]["saturated"] += 1

Loading…
Cancel
Save