Skip to content

Commit

Permalink
car stuff
Browse files Browse the repository at this point in the history
  • Loading branch information
adeebshihadeh committed Feb 13, 2021
1 parent c189d15 commit 77fd1c6
Show file tree
Hide file tree
Showing 33 changed files with 60 additions and 60 deletions.
2 changes: 1 addition & 1 deletion cereal
4 changes: 2 additions & 2 deletions selfdrive/car/chrysler/carstate.py
Original file line number Diff line number Diff line change
Expand Up @@ -42,8 +42,8 @@ def update(self, cp, cp_cam):

ret.leftBlinker = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 1
ret.rightBlinker = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 2
ret.steeringAngle = cp.vl["STEERING"]['STEER_ANGLE']
ret.steeringRate = cp.vl["STEERING"]['STEERING_RATE']
ret.steeringAngleDegDegDeg = cp.vl["STEERING"]['STEER_ANGLE']
ret.steeringRateDeg = cp.vl["STEERING"]['STEERING_RATE']
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(cp.vl['GEAR']['PRNDL'], None))

ret.cruiseState.enabled = cp.vl["ACC_2"]['ACC_STATUS_2'] == 7 # ACC is green.
Expand Down
2 changes: 1 addition & 1 deletion selfdrive/car/chrysler/interface.py
Original file line number Diff line number Diff line change
Expand Up @@ -67,7 +67,7 @@ def update(self, c, can_strings):
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid

# speeds
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
ret.steeringRateDegLimited = self.CC.steer_rate_limited if self.CC is not None else False

# events
events = self.create_common_events(ret, extra_gears=[car.CarState.GearShifter.low],
Expand Down
4 changes: 2 additions & 2 deletions selfdrive/car/ford/carcontroller.py
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,7 @@ def update(self, enabled, CS, frame, actuators, visual_alert, pcm_cancel):

if (frame % 3) == 0:

curvature = self.vehicle_model.calc_curvature(actuators.steerAngle*3.1415/180., CS.out.vEgo)
curvature = self.vehicle_model.calc_curvature(actuators.steeringAngleDeg*3.1415/180., CS.out.vEgo)

# The use of the toggle below is handy for trying out the various LKAS modes
if TOGGLE_DEBUG:
Expand All @@ -43,7 +43,7 @@ def update(self, enabled, CS, frame, actuators, visual_alert, pcm_cancel):
self.lkas_action = 5 # 4 and 5 seem the best. 8 and 9 seem to aggressive and laggy

can_sends.append(create_steer_command(self.packer, apply_steer, enabled,
CS.lkas_state, CS.out.steeringAngle, curvature, self.lkas_action))
CS.lkas_state, CS.out.steeringAngleDegDegDeg, curvature, self.lkas_action))
self.generic_toggle_last = CS.out.genericToggle

if (frame % 100) == 0:
Expand Down
2 changes: 1 addition & 1 deletion selfdrive/car/ford/carstate.py
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@ def update(self, cp):
ret.vEgoRaw = mean([ret.wheelSpeeds.rr, ret.wheelSpeeds.rl, ret.wheelSpeeds.fr, ret.wheelSpeeds.fl])
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = not ret.vEgoRaw > 0.001
ret.steeringAngle = cp.vl["Steering_Wheel_Data_CG1"]['SteWhlRelInit_An_Sns']
ret.steeringAngleDegDegDeg = cp.vl["Steering_Wheel_Data_CG1"]['SteWhlRelInit_An_Sns']
ret.steeringPressed = not cp.vl["Lane_Keep_Assist_Status"]['LaHandsOff_B_Actl']
ret.steerError = cp.vl["Lane_Keep_Assist_Status"]['LaActDeny_B_Actl'] == 1
ret.cruiseState.speed = cp.vl["Cruise_Status"]['Set_Speed'] * CV.MPH_TO_MS
Expand Down
2 changes: 1 addition & 1 deletion selfdrive/car/gm/carstate.py
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,7 @@ def update(self, pt_cp):
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = ret.vEgoRaw < 0.01

ret.steeringAngle = pt_cp.vl["PSCMSteeringAngle"]['SteeringWheelAngle']
ret.steeringAngleDegDegDeg = pt_cp.vl["PSCMSteeringAngle"]['SteeringWheelAngle']
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(pt_cp.vl["ECMPRDNL"]['PRNDL'], None))
ret.brake = pt_cp.vl["EBCMBrakePedalPosition"]['BrakePedalPosition'] / 0xd0
# Brake pedal's potentiometer returns near-zero reading even when pedal is not pressed.
Expand Down
2 changes: 1 addition & 1 deletion selfdrive/car/gm/interface.py
Original file line number Diff line number Diff line change
Expand Up @@ -121,7 +121,7 @@ def update(self, c, can_strings):
ret = self.CS.update(self.cp)

ret.canValid = self.cp.can_valid
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
ret.steeringRateDegLimited = self.CC.steer_rate_limited if self.CC is not None else False

buttonEvents = []

Expand Down
4 changes: 2 additions & 2 deletions selfdrive/car/honda/carstate.py
Original file line number Diff line number Diff line change
Expand Up @@ -227,8 +227,8 @@ def update(self, cp, cp_cam, cp_body):
ret.vEgoRaw = (1. - v_weight) * cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] * CV.KPH_TO_MS * speed_factor + v_weight * v_wheel
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)

ret.steeringAngle = cp.vl["STEERING_SENSORS"]['STEER_ANGLE']
ret.steeringRate = cp.vl["STEERING_SENSORS"]['STEER_ANGLE_RATE']
ret.steeringAngleDegDegDeg = cp.vl["STEERING_SENSORS"]['STEER_ANGLE']
ret.steeringRateDeg = cp.vl["STEERING_SENSORS"]['STEER_ANGLE_RATE']

self.cruise_setting = cp.vl["SCM_BUTTONS"]['CRUISE_SETTING']
self.cruise_buttons = cp.vl["SCM_BUTTONS"]['CRUISE_BUTTONS']
Expand Down
2 changes: 1 addition & 1 deletion selfdrive/car/honda/interface.py
Original file line number Diff line number Diff line change
Expand Up @@ -447,7 +447,7 @@ def update(self, c, can_strings):
ret = self.CS.update(self.cp, self.cp_cam, self.cp_body)

ret.canValid = self.cp.can_valid and self.cp_cam.can_valid and (self.cp_body is None or self.cp_body.can_valid)
ret.yawRate = self.VM.yaw_rate(ret.steeringAngle * CV.DEG_TO_RAD, ret.vEgo)
ret.yawRate = self.VM.yaw_rate(ret.steeringAngleDegDegDeg * CV.DEG_TO_RAD, ret.vEgo)
# FIXME: read sendcan for brakelights
brakelights_threshold = 0.02 if self.CS.CP.carFingerprint == CAR.CIVIC else 0.1
ret.brakeLights = bool(self.CS.brake_switch or
Expand Down
2 changes: 1 addition & 1 deletion selfdrive/car/hyundai/carcontroller.py
Original file line number Diff line number Diff line change
Expand Up @@ -50,7 +50,7 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert,
self.steer_rate_limited = new_steer != apply_steer

# disable if steer angle reach 90 deg, otherwise mdps fault in some models
lkas_active = enabled and abs(CS.out.steeringAngle) < CS.CP.maxSteerAngle
lkas_active = enabled and abs(CS.out.steeringAngleDegDegDeg) < CS.CP.maxSteerAngleDeg

# fix for Genesis hard fault at low speed
if CS.out.vEgo < 16.7 and self.car_fingerprint == CAR.HYUNDAI_GENESIS:
Expand Down
4 changes: 2 additions & 2 deletions selfdrive/car/hyundai/carstate.py
Original file line number Diff line number Diff line change
Expand Up @@ -26,8 +26,8 @@ def update(self, cp, cp_cam):

ret.standstill = ret.vEgoRaw < 0.1

ret.steeringAngle = cp.vl["SAS11"]['SAS_Angle']
ret.steeringRate = cp.vl["SAS11"]['SAS_Speed']
ret.steeringAngleDegDegDeg = cp.vl["SAS11"]['SAS_Angle']
ret.steeringRateDeg = cp.vl["SAS11"]['SAS_Speed']
ret.yawRate = cp.vl["ESP12"]['YAW_RATE']
ret.leftBlinker, ret.rightBlinker = self.update_blinker(50, cp.vl["CGW1"]['CF_Gway_TurnSigLh'],
cp.vl["CGW1"]['CF_Gway_TurnSigRh'])
Expand Down
6 changes: 3 additions & 3 deletions selfdrive/car/hyundai/interface.py
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # py
ret.steerLimitTimer = 0.4
tire_stiffness_factor = 1.

ret.maxSteerAngle = 90.
ret.maxSteerAngleDeg = 90.

eps_modified = False
for fw in car_fw:
Expand Down Expand Up @@ -66,7 +66,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # py
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.05]]
if eps_modified:
ret.maxSteerAngle = 1000.
ret.maxSteerAngleDeg = 1000.
elif candidate in [CAR.ELANTRA, CAR.ELANTRA_GT_I30]:
ret.lateralTuning.pid.kf = 0.00006
ret.mass = 1275. + STD_CARGO_KG
Expand Down Expand Up @@ -222,7 +222,7 @@ def update(self, c, can_strings):

ret = self.CS.update(self.cp, self.cp_cam)
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
ret.steeringRateDegLimited = self.CC.steer_rate_limited if self.CC is not None else False

events = self.create_common_events(ret)
# TODO: addd abs(self.CS.angle_steers) > 90 to 'steerTempUnavailable' event
Expand Down
4 changes: 2 additions & 2 deletions selfdrive/car/mazda/carstate.py
Original file line number Diff line number Diff line change
Expand Up @@ -38,12 +38,12 @@ def update(self, cp, cp_cam):
ret.leftBlinker = cp.vl["BLINK_INFO"]['LEFT_BLINK'] == 1
ret.rightBlinker = cp.vl["BLINK_INFO"]['RIGHT_BLINK'] == 1

ret.steeringAngle = cp.vl["STEER"]['STEER_ANGLE']
ret.steeringAngleDegDegDeg = cp.vl["STEER"]['STEER_ANGLE']
ret.steeringTorque = cp.vl["STEER_TORQUE"]['STEER_TORQUE_SENSOR']
ret.steeringPressed = abs(ret.steeringTorque) > LKAS_LIMITS.STEER_THRESHOLD

ret.steeringTorqueEps = cp.vl["STEER_TORQUE"]['STEER_TORQUE_MOTOR']
ret.steeringRate = cp.vl["STEER_RATE"]['STEER_ANGLE_RATE']
ret.steeringRateDeg = cp.vl["STEER_RATE"]['STEER_ANGLE_RATE']

ret.brakePressed = cp.vl["PEDALS"]['BRAKE_ON'] == 1
ret.brake = cp.vl["BRAKE"]['BRAKE_PRESSURE']
Expand Down
2 changes: 1 addition & 1 deletion selfdrive/car/mock/interface.py
Original file line number Diff line number Diff line change
Expand Up @@ -81,7 +81,7 @@ def update(self, c, can_strings):

self.yawRate = LPG * self.yaw_rate_meas + (1. - LPG) * self.yaw_rate
curvature = self.yaw_rate / max(self.speed, 1.)
ret.steeringAngle = curvature * self.CP.steerRatio * self.CP.wheelbase * CV.RAD_TO_DEG
ret.steeringAngleDegDegDeg = curvature * self.CP.steerRatio * self.CP.wheelbase * CV.RAD_TO_DEG

return ret.as_reader()

Expand Down
4 changes: 2 additions & 2 deletions selfdrive/car/nissan/carcontroller.py
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,7 @@ def update(self, enabled, CS, frame, actuators, cruise_cancel, hud_alert,
acc_active = bool(CS.out.cruiseState.enabled)
lkas_hud_msg = CS.lkas_hud_msg
lkas_hud_info_msg = CS.lkas_hud_info_msg
apply_angle = actuators.steerAngle
apply_angle = actuators.steeringAngleDeg

steer_hud_alert = 1 if hud_alert == VisualAlert.steerRequired else 0

Expand All @@ -55,7 +55,7 @@ def update(self, enabled, CS, frame, actuators, cruise_cancel, hud_alert,
)

else:
apply_angle = CS.out.steeringAngle
apply_angle = CS.out.steeringAngleDegDegDeg
self.lkas_max_torque = 0

self.last_angle = apply_angle
Expand Down
2 changes: 1 addition & 1 deletion selfdrive/car/nissan/carstate.py
Original file line number Diff line number Diff line change
Expand Up @@ -70,7 +70,7 @@ def update(self, cp, cp_adas, cp_cam):
# Filtering driver torque to prevent steeringPressed false positives
ret.steeringPressed = bool(abs(sum(self.steeringTorqueSamples) / TORQUE_SAMPLES) > CarControllerParams.STEER_THRESHOLD)

ret.steeringAngle = cp.vl["STEER_ANGLE_SENSOR"]["STEER_ANGLE"]
ret.steeringAngleDegDegDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_ANGLE"]

ret.leftBlinker = bool(cp.vl["LIGHTS"]["LEFT_BLINKER"])
ret.rightBlinker = bool(cp.vl["LIGHTS"]["RIGHT_BLINKER"])
Expand Down
2 changes: 1 addition & 1 deletion selfdrive/car/subaru/carstate.py
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,7 @@ def update(self, cp, cp_cam):
can_gear = int(cp.vl["Transmission"]['Gear'])
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))

ret.steeringAngle = cp.vl["Steering_Torque"]['Steering_Angle']
ret.steeringAngleDegDegDeg = cp.vl["Steering_Torque"]['Steering_Angle']
ret.steeringTorque = cp.vl["Steering_Torque"]['Steer_Torque_Sensor']
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD[self.car_fingerprint]

Expand Down
2 changes: 1 addition & 1 deletion selfdrive/car/subaru/interface.py
Original file line number Diff line number Diff line change
Expand Up @@ -110,7 +110,7 @@ def update(self, c, can_strings):
ret = self.CS.update(self.cp, self.cp_cam)

ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
ret.steeringRateDegLimited = self.CC.steer_rate_limited if self.CC is not None else False

ret.events = self.create_common_events(ret).to_msg()

Expand Down
2 changes: 1 addition & 1 deletion selfdrive/car/toyota/carcontroller.py
Original file line number Diff line number Diff line change
Expand Up @@ -103,7 +103,7 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
# LTA mode. Set ret.steerControlType = car.CarParams.SteerControlType.angle and whitelist 0x191 in the panda
# if frame % 2 == 0:
# can_sends.append(create_steer_command(self.packer, 0, 0, frame // 2))
# can_sends.append(create_lta_steer_command(self.packer, actuators.steerAngle, apply_steer_req, frame // 2))
# can_sends.append(create_lta_steer_command(self.packer, actuators.steeringAngleDeg, apply_steer_req, frame // 2))

# we can spam can to cancel the system even if we are using lat only control
if (frame % 3 == 0 and CS.CP.openpilotLongitudinalControl) or (pcm_cancel_cmd and Ecu.fwdCamera in self.fake_ecus):
Expand Down
10 changes: 5 additions & 5 deletions selfdrive/car/toyota/carstate.py
Original file line number Diff line number Diff line change
Expand Up @@ -52,17 +52,17 @@ def update(self, cp, cp_cam):
self.accurate_steer_angle_seen = True

if self.accurate_steer_angle_seen:
ret.steeringAngle = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] - self.angle_offset
ret.steeringAngleDegDegDeg = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] - self.angle_offset

if self.needs_angle_offset:
angle_wheel = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
if abs(angle_wheel) > 1e-3 and abs(ret.steeringAngle) > 1e-3:
if abs(angle_wheel) > 1e-3 and abs(ret.steeringAngleDegDegDeg) > 1e-3:
self.needs_angle_offset = False
self.angle_offset = ret.steeringAngle - angle_wheel
self.angle_offset = ret.steeringAngleDegDegDeg - angle_wheel
else:
ret.steeringAngle = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
ret.steeringAngleDegDegDeg = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']

ret.steeringRate = cp.vl["STEER_ANGLE_SENSOR"]['STEER_RATE']
ret.steeringRateDeg = cp.vl["STEER_ANGLE_SENSOR"]['STEER_RATE']

can_gear = int(cp.vl["GEAR_PACKET"]['GEAR'])
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))
Expand Down
2 changes: 1 addition & 1 deletion selfdrive/car/toyota/interface.py
Original file line number Diff line number Diff line change
Expand Up @@ -347,7 +347,7 @@ def update(self, c, can_strings):
ret = self.CS.update(self.cp, self.cp_cam)

ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
ret.steeringRateDegLimited = self.CC.steer_rate_limited if self.CC is not None else False

# events
events = self.create_common_events(ret)
Expand Down
4 changes: 2 additions & 2 deletions selfdrive/car/volkswagen/carstate.py
Original file line number Diff line number Diff line change
Expand Up @@ -28,8 +28,8 @@ def update(self, pt_cp):

# Update steering angle, rate, yaw rate, and driver input torque. VW send
# the sign/direction in a separate signal so they must be recombined.
ret.steeringAngle = pt_cp.vl["LWI_01"]['LWI_Lenkradwinkel'] * (1, -1)[int(pt_cp.vl["LWI_01"]['LWI_VZ_Lenkradwinkel'])]
ret.steeringRate = pt_cp.vl["LWI_01"]['LWI_Lenkradw_Geschw'] * (1, -1)[int(pt_cp.vl["LWI_01"]['LWI_VZ_Lenkradwinkel'])]
ret.steeringAngleDegDegDeg = pt_cp.vl["LWI_01"]['LWI_Lenkradwinkel'] * (1, -1)[int(pt_cp.vl["LWI_01"]['LWI_VZ_Lenkradwinkel'])]
ret.steeringRateDeg = pt_cp.vl["LWI_01"]['LWI_Lenkradw_Geschw'] * (1, -1)[int(pt_cp.vl["LWI_01"]['LWI_VZ_Lenkradwinkel'])]
ret.steeringTorque = pt_cp.vl["EPS_01"]['Driver_Strain'] * (1, -1)[int(pt_cp.vl["EPS_01"]['Driver_Strain_VZ'])]
ret.steeringPressed = abs(ret.steeringTorque) > CarControllerParams.STEER_DRIVER_ALLOWANCE
ret.yawRate = pt_cp.vl["ESP_02"]['ESP_Gierrate'] * (1, -1)[int(pt_cp.vl["ESP_02"]['ESP_VZ_Gierrate'])] * CV.DEG_TO_RAD
Expand Down
2 changes: 1 addition & 1 deletion selfdrive/car/volkswagen/interface.py
Original file line number Diff line number Diff line change
Expand Up @@ -74,7 +74,7 @@ def update(self, c, can_strings):

ret = self.CS.update(self.cp)
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
ret.steeringRateDegLimited = self.CC.steer_rate_limited if self.CC is not None else False

# TODO: add a field for this to carState, car interface code shouldn't write params
# Update the device metric configuration to match the car at first startup,
Expand Down
6 changes: 3 additions & 3 deletions selfdrive/controls/controlsd.py
Original file line number Diff line number Diff line change
Expand Up @@ -389,11 +389,11 @@ def state_control(self, CS):
# Gas/Brake PID loop
actuators.gas, actuators.brake = self.LoC.update(self.active, CS, v_acc_sol, plan.vTargetFuture, a_acc_sol, self.CP)
# Steering PID loop and lateral MPC
actuators.steer, actuators.steerAngle, lac_log = self.LaC.update(self.active, CS, self.CP, path_plan)
actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(self.active, CS, self.CP, path_plan)

# Check for difference between desired angle and angle for angle based control
angle_control_saturated = self.CP.steerControlType == car.CarParams.SteerControlType.angle and \
abs(actuators.steerAngle - CS.steeringAngle) > STEER_ANGLE_SATURATION_THRESHOLD
abs(actuators.steeringAngleDeg - CS.steeringAngleDegDegDeg) > STEER_ANGLE_SATURATION_THRESHOLD

if angle_control_saturated and not CS.steeringPressed and self.active:
self.saturated_count += 1
Expand Down Expand Up @@ -470,7 +470,7 @@ def publish_logs(self, CS, start_time, actuators, v_acc, a_acc, lac_log):
force_decel = (self.sm['driverMonitoringState'].awarenessStatus < 0.) or \
(self.state == State.softDisabling)

steer_angle_rad = (CS.steeringAngle - self.sm['lateralPlan'].angleOffset) * CV.DEG_TO_RAD
steer_angle_rad = (CS.steeringAngleDegDegDeg - self.sm['lateralPlan'].angleOffset) * CV.DEG_TO_RAD

# controlsState
dat = messaging.new_message('controlsState')
Expand Down
6 changes: 3 additions & 3 deletions selfdrive/controls/lib/latcontrol_indi.py
Original file line number Diff line number Diff line change
Expand Up @@ -83,11 +83,11 @@ def _check_saturation(self, control, check_saturation, limit):
def update(self, active, CS, CP, path_plan):
self.speed = CS.vEgo
# Update Kalman filter
y = np.array([[math.radians(CS.steeringAngle)], [math.radians(CS.steeringRate)]])
y = np.array([[math.radians(CS.steeringAngleDegDegDeg)], [math.radians(CS.steeringRateDeg)]])
self.x = np.dot(self.A_K, self.x) + np.dot(self.K, y)

indi_log = log.ControlsState.LateralINDIState.new_message()
indi_log.steerAngle = math.degrees(self.x[0])
indi_log.steeringAngleDeg = math.degrees(self.x[0])
indi_log.steerRate = math.degrees(self.x[1])
indi_log.steerAccel = math.degrees(self.x[2])

Expand Down Expand Up @@ -136,7 +136,7 @@ def update(self, active, CS, CP, path_plan):
indi_log.delta = float(delta_u)
indi_log.output = float(self.output_steer)

check_saturation = (CS.vEgo > 10.) and not CS.steeringRateLimited and not CS.steeringPressed
check_saturation = (CS.vEgo > 10.) and not CS.steeringRateDegLimited and not CS.steeringPressed
indi_log.saturated = self._check_saturation(self.output_steer, check_saturation, steers_max)

return float(self.output_steer), float(self.angle_steers_des), indi_log
6 changes: 3 additions & 3 deletions selfdrive/controls/lib/latcontrol_lqr.py
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,7 @@ def update(self, active, CS, CP, path_plan):
steers_max = get_steer_max(CP, CS.vEgo)
torque_scale = (0.45 + CS.vEgo / 60.0)**2 # Scale actuator model with speed

steering_angle = CS.steeringAngle
steering_angle = CS.steeringAngleDegDegDeg

# Subtract offset. Zero angle should correspond to zero torque
self.angle_steers_des = path_plan.angleSteers - path_plan.angleOffset
Expand Down Expand Up @@ -86,10 +86,10 @@ def update(self, active, CS, CP, path_plan):
self.output_steer = lqr_output + self.i_lqr
self.output_steer = clip(self.output_steer, -steers_max, steers_max)

check_saturation = (CS.vEgo > 10) and not CS.steeringRateLimited and not CS.steeringPressed
check_saturation = (CS.vEgo > 10) and not CS.steeringRateDegLimited and not CS.steeringPressed
saturated = self._check_saturation(self.output_steer, check_saturation, steers_max)

lqr_log.steerAngle = angle_steers_k + path_plan.angleOffset
lqr_log.steeringAngleDeg = angle_steers_k + path_plan.angleOffset
lqr_log.i = self.i_lqr
lqr_log.output = self.output_steer
lqr_log.lqrOutput = lqr_output
Expand Down
8 changes: 4 additions & 4 deletions selfdrive/controls/lib/latcontrol_pid.py
Original file line number Diff line number Diff line change
Expand Up @@ -17,8 +17,8 @@ def reset(self):

def update(self, active, CS, CP, path_plan):
pid_log = log.ControlsState.LateralPIDState.new_message()
pid_log.steerAngle = float(CS.steeringAngle)
pid_log.steerRate = float(CS.steeringRate)
pid_log.steeringAngleDeg = float(CS.steeringAngleDegDegDeg)
pid_log.steerRate = float(CS.steeringRateDeg)

if CS.vEgo < 0.3 or not active:
output_steer = 0.0
Expand All @@ -37,8 +37,8 @@ def update(self, active, CS, CP, path_plan):
steer_feedforward *= CS.vEgo**2 # proportional to realigning tire momentum (~ lateral accel)
deadzone = 0.0

check_saturation = (CS.vEgo > 10) and not CS.steeringRateLimited and not CS.steeringPressed
output_steer = self.pid.update(self.angle_steers_des, CS.steeringAngle, check_saturation=check_saturation, override=CS.steeringPressed,
check_saturation = (CS.vEgo > 10) and not CS.steeringRateDegLimited and not CS.steeringPressed
output_steer = self.pid.update(self.angle_steers_des, CS.steeringAngleDegDegDeg, check_saturation=check_saturation, override=CS.steeringPressed,
feedforward=steer_feedforward, speed=CS.vEgo, deadzone=deadzone)
pid_log.active = True
pid_log.p = self.pid.p
Expand Down
2 changes: 1 addition & 1 deletion selfdrive/controls/lib/lateral_planner.py
Original file line number Diff line number Diff line change
Expand Up @@ -83,7 +83,7 @@ def update(self, sm, CP, VM):
v_ego = sm['carState'].vEgo
active = sm['controlsState'].active
steering_wheel_angle_offset_deg = sm['liveParameters'].angleOffset
steering_wheel_angle_deg = sm['carState'].steeringAngle
steering_wheel_angle_deg = sm['carState'].steeringAngleDegDegDeg

# Update vehicle model
x = max(sm['liveParameters'].stiffnessFactor, 0.1)
Expand Down
Loading

0 comments on commit 77fd1c6

Please sign in to comment.