pull/20092/head
Adeeb Shihadeh 4 years ago
parent c189d15af9
commit 77fd1c65eb
  1. 2
      cereal
  2. 4
      selfdrive/car/chrysler/carstate.py
  3. 2
      selfdrive/car/chrysler/interface.py
  4. 4
      selfdrive/car/ford/carcontroller.py
  5. 2
      selfdrive/car/ford/carstate.py
  6. 2
      selfdrive/car/gm/carstate.py
  7. 2
      selfdrive/car/gm/interface.py
  8. 4
      selfdrive/car/honda/carstate.py
  9. 2
      selfdrive/car/honda/interface.py
  10. 2
      selfdrive/car/hyundai/carcontroller.py
  11. 4
      selfdrive/car/hyundai/carstate.py
  12. 6
      selfdrive/car/hyundai/interface.py
  13. 4
      selfdrive/car/mazda/carstate.py
  14. 2
      selfdrive/car/mock/interface.py
  15. 4
      selfdrive/car/nissan/carcontroller.py
  16. 2
      selfdrive/car/nissan/carstate.py
  17. 2
      selfdrive/car/subaru/carstate.py
  18. 2
      selfdrive/car/subaru/interface.py
  19. 2
      selfdrive/car/toyota/carcontroller.py
  20. 10
      selfdrive/car/toyota/carstate.py
  21. 2
      selfdrive/car/toyota/interface.py
  22. 4
      selfdrive/car/volkswagen/carstate.py
  23. 2
      selfdrive/car/volkswagen/interface.py
  24. 6
      selfdrive/controls/controlsd.py
  25. 6
      selfdrive/controls/lib/latcontrol_indi.py
  26. 6
      selfdrive/controls/lib/latcontrol_lqr.py
  27. 8
      selfdrive/controls/lib/latcontrol_pid.py
  28. 2
      selfdrive/controls/lib/lateral_planner.py
  29. 2
      selfdrive/controls/lib/longitudinal_planner.py
  30. 2
      selfdrive/debug/internal/measure_steering_accuracy.py
  31. 4
      selfdrive/locationd/paramsd.py
  32. 4
      tools/carcontrols/debug_controls.py
  33. 8
      tools/replay/ui.py

@ -1 +1 @@
Subproject commit 39d80be84108820a42fc4b1b1817e8e3b73c89fb Subproject commit 775db20d6eb0112dbae82e49f92dc281f0c6daa7

@ -42,8 +42,8 @@ class CarState(CarStateBase):
ret.leftBlinker = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 1 ret.leftBlinker = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 1
ret.rightBlinker = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 2 ret.rightBlinker = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 2
ret.steeringAngle = cp.vl["STEERING"]['STEER_ANGLE'] ret.steeringAngleDegDegDeg = cp.vl["STEERING"]['STEER_ANGLE']
ret.steeringRate = cp.vl["STEERING"]['STEERING_RATE'] ret.steeringRateDeg = cp.vl["STEERING"]['STEERING_RATE']
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(cp.vl['GEAR']['PRNDL'], None)) 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. ret.cruiseState.enabled = cp.vl["ACC_2"]['ACC_STATUS_2'] == 7 # ACC is green.

@ -67,7 +67,7 @@ class CarInterface(CarInterfaceBase):
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
# speeds # 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
events = self.create_common_events(ret, extra_gears=[car.CarState.GearShifter.low], events = self.create_common_events(ret, extra_gears=[car.CarState.GearShifter.low],

@ -33,7 +33,7 @@ class CarController():
if (frame % 3) == 0: 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 # The use of the toggle below is handy for trying out the various LKAS modes
if TOGGLE_DEBUG: if TOGGLE_DEBUG:
@ -43,7 +43,7 @@ class CarController():
self.lkas_action = 5 # 4 and 5 seem the best. 8 and 9 seem to aggressive and laggy 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, 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 self.generic_toggle_last = CS.out.genericToggle
if (frame % 100) == 0: if (frame % 100) == 0:

@ -17,7 +17,7 @@ class CarState(CarStateBase):
ret.vEgoRaw = mean([ret.wheelSpeeds.rr, ret.wheelSpeeds.rl, ret.wheelSpeeds.fr, ret.wheelSpeeds.fl]) 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.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = not ret.vEgoRaw > 0.001 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.steeringPressed = not cp.vl["Lane_Keep_Assist_Status"]['LaHandsOff_B_Actl']
ret.steerError = cp.vl["Lane_Keep_Assist_Status"]['LaActDeny_B_Actl'] == 1 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 ret.cruiseState.speed = cp.vl["Cruise_Status"]['Set_Speed'] * CV.MPH_TO_MS

@ -28,7 +28,7 @@ class CarState(CarStateBase):
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = ret.vEgoRaw < 0.01 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.gearShifter = self.parse_gear_shifter(self.shifter_values.get(pt_cp.vl["ECMPRDNL"]['PRNDL'], None))
ret.brake = pt_cp.vl["EBCMBrakePedalPosition"]['BrakePedalPosition'] / 0xd0 ret.brake = pt_cp.vl["EBCMBrakePedalPosition"]['BrakePedalPosition'] / 0xd0
# Brake pedal's potentiometer returns near-zero reading even when pedal is not pressed. # Brake pedal's potentiometer returns near-zero reading even when pedal is not pressed.

@ -121,7 +121,7 @@ class CarInterface(CarInterfaceBase):
ret = self.CS.update(self.cp) ret = self.CS.update(self.cp)
ret.canValid = self.cp.can_valid 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 = [] buttonEvents = []

@ -227,8 +227,8 @@ class CarState(CarStateBase):
ret.vEgoRaw = (1. - v_weight) * cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] * CV.KPH_TO_MS * speed_factor + v_weight * v_wheel 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.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.steeringAngle = cp.vl["STEERING_SENSORS"]['STEER_ANGLE'] ret.steeringAngleDegDegDeg = cp.vl["STEERING_SENSORS"]['STEER_ANGLE']
ret.steeringRate = cp.vl["STEERING_SENSORS"]['STEER_ANGLE_RATE'] ret.steeringRateDeg = cp.vl["STEERING_SENSORS"]['STEER_ANGLE_RATE']
self.cruise_setting = cp.vl["SCM_BUTTONS"]['CRUISE_SETTING'] self.cruise_setting = cp.vl["SCM_BUTTONS"]['CRUISE_SETTING']
self.cruise_buttons = cp.vl["SCM_BUTTONS"]['CRUISE_BUTTONS'] self.cruise_buttons = cp.vl["SCM_BUTTONS"]['CRUISE_BUTTONS']

@ -447,7 +447,7 @@ class CarInterface(CarInterfaceBase):
ret = self.CS.update(self.cp, self.cp_cam, self.cp_body) 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.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 # FIXME: read sendcan for brakelights
brakelights_threshold = 0.02 if self.CS.CP.carFingerprint == CAR.CIVIC else 0.1 brakelights_threshold = 0.02 if self.CS.CP.carFingerprint == CAR.CIVIC else 0.1
ret.brakeLights = bool(self.CS.brake_switch or ret.brakeLights = bool(self.CS.brake_switch or

@ -50,7 +50,7 @@ class CarController():
self.steer_rate_limited = new_steer != apply_steer self.steer_rate_limited = new_steer != apply_steer
# disable if steer angle reach 90 deg, otherwise mdps fault in some models # 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 # fix for Genesis hard fault at low speed
if CS.out.vEgo < 16.7 and self.car_fingerprint == CAR.HYUNDAI_GENESIS: if CS.out.vEgo < 16.7 and self.car_fingerprint == CAR.HYUNDAI_GENESIS:

@ -26,8 +26,8 @@ class CarState(CarStateBase):
ret.standstill = ret.vEgoRaw < 0.1 ret.standstill = ret.vEgoRaw < 0.1
ret.steeringAngle = cp.vl["SAS11"]['SAS_Angle'] ret.steeringAngleDegDegDeg = cp.vl["SAS11"]['SAS_Angle']
ret.steeringRate = cp.vl["SAS11"]['SAS_Speed'] ret.steeringRateDeg = cp.vl["SAS11"]['SAS_Speed']
ret.yawRate = cp.vl["ESP12"]['YAW_RATE'] ret.yawRate = cp.vl["ESP12"]['YAW_RATE']
ret.leftBlinker, ret.rightBlinker = self.update_blinker(50, cp.vl["CGW1"]['CF_Gway_TurnSigLh'], ret.leftBlinker, ret.rightBlinker = self.update_blinker(50, cp.vl["CGW1"]['CF_Gway_TurnSigLh'],
cp.vl["CGW1"]['CF_Gway_TurnSigRh']) cp.vl["CGW1"]['CF_Gway_TurnSigRh'])

@ -27,7 +27,7 @@ class CarInterface(CarInterfaceBase):
ret.steerLimitTimer = 0.4 ret.steerLimitTimer = 0.4
tire_stiffness_factor = 1. tire_stiffness_factor = 1.
ret.maxSteerAngle = 90. ret.maxSteerAngleDeg = 90.
eps_modified = False eps_modified = False
for fw in car_fw: for fw in car_fw:
@ -66,7 +66,7 @@ class CarInterface(CarInterfaceBase):
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.05]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.05]]
if eps_modified: if eps_modified:
ret.maxSteerAngle = 1000. ret.maxSteerAngleDeg = 1000.
elif candidate in [CAR.ELANTRA, CAR.ELANTRA_GT_I30]: elif candidate in [CAR.ELANTRA, CAR.ELANTRA_GT_I30]:
ret.lateralTuning.pid.kf = 0.00006 ret.lateralTuning.pid.kf = 0.00006
ret.mass = 1275. + STD_CARGO_KG ret.mass = 1275. + STD_CARGO_KG
@ -222,7 +222,7 @@ class CarInterface(CarInterfaceBase):
ret = self.CS.update(self.cp, self.cp_cam) ret = self.CS.update(self.cp, self.cp_cam)
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid 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) events = self.create_common_events(ret)
# TODO: addd abs(self.CS.angle_steers) > 90 to 'steerTempUnavailable' event # TODO: addd abs(self.CS.angle_steers) > 90 to 'steerTempUnavailable' event

@ -38,12 +38,12 @@ class CarState(CarStateBase):
ret.leftBlinker = cp.vl["BLINK_INFO"]['LEFT_BLINK'] == 1 ret.leftBlinker = cp.vl["BLINK_INFO"]['LEFT_BLINK'] == 1
ret.rightBlinker = cp.vl["BLINK_INFO"]['RIGHT_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.steeringTorque = cp.vl["STEER_TORQUE"]['STEER_TORQUE_SENSOR']
ret.steeringPressed = abs(ret.steeringTorque) > LKAS_LIMITS.STEER_THRESHOLD ret.steeringPressed = abs(ret.steeringTorque) > LKAS_LIMITS.STEER_THRESHOLD
ret.steeringTorqueEps = cp.vl["STEER_TORQUE"]['STEER_TORQUE_MOTOR'] 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.brakePressed = cp.vl["PEDALS"]['BRAKE_ON'] == 1
ret.brake = cp.vl["BRAKE"]['BRAKE_PRESSURE'] ret.brake = cp.vl["BRAKE"]['BRAKE_PRESSURE']

@ -81,7 +81,7 @@ class CarInterface(CarInterfaceBase):
self.yawRate = LPG * self.yaw_rate_meas + (1. - LPG) * self.yaw_rate self.yawRate = LPG * self.yaw_rate_meas + (1. - LPG) * self.yaw_rate
curvature = self.yaw_rate / max(self.speed, 1.) 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() return ret.as_reader()

@ -29,7 +29,7 @@ class CarController():
acc_active = bool(CS.out.cruiseState.enabled) acc_active = bool(CS.out.cruiseState.enabled)
lkas_hud_msg = CS.lkas_hud_msg lkas_hud_msg = CS.lkas_hud_msg
lkas_hud_info_msg = CS.lkas_hud_info_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 steer_hud_alert = 1 if hud_alert == VisualAlert.steerRequired else 0
@ -55,7 +55,7 @@ class CarController():
) )
else: else:
apply_angle = CS.out.steeringAngle apply_angle = CS.out.steeringAngleDegDegDeg
self.lkas_max_torque = 0 self.lkas_max_torque = 0
self.last_angle = apply_angle self.last_angle = apply_angle

@ -70,7 +70,7 @@ class CarState(CarStateBase):
# Filtering driver torque to prevent steeringPressed false positives # Filtering driver torque to prevent steeringPressed false positives
ret.steeringPressed = bool(abs(sum(self.steeringTorqueSamples) / TORQUE_SAMPLES) > CarControllerParams.STEER_THRESHOLD) 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.leftBlinker = bool(cp.vl["LIGHTS"]["LEFT_BLINKER"])
ret.rightBlinker = bool(cp.vl["LIGHTS"]["RIGHT_BLINKER"]) ret.rightBlinker = bool(cp.vl["LIGHTS"]["RIGHT_BLINKER"])

@ -43,7 +43,7 @@ class CarState(CarStateBase):
can_gear = int(cp.vl["Transmission"]['Gear']) can_gear = int(cp.vl["Transmission"]['Gear'])
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None)) 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.steeringTorque = cp.vl["Steering_Torque"]['Steer_Torque_Sensor']
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD[self.car_fingerprint] ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD[self.car_fingerprint]

@ -110,7 +110,7 @@ class CarInterface(CarInterfaceBase):
ret = self.CS.update(self.cp, self.cp_cam) ret = self.CS.update(self.cp, self.cp_cam)
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid 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() ret.events = self.create_common_events(ret).to_msg()

@ -103,7 +103,7 @@ class CarController():
# LTA mode. Set ret.steerControlType = car.CarParams.SteerControlType.angle and whitelist 0x191 in the panda # LTA mode. Set ret.steerControlType = car.CarParams.SteerControlType.angle and whitelist 0x191 in the panda
# if frame % 2 == 0: # if frame % 2 == 0:
# can_sends.append(create_steer_command(self.packer, 0, 0, frame // 2)) # 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 # 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): if (frame % 3 == 0 and CS.CP.openpilotLongitudinalControl) or (pcm_cancel_cmd and Ecu.fwdCamera in self.fake_ecus):

@ -52,17 +52,17 @@ class CarState(CarStateBase):
self.accurate_steer_angle_seen = True self.accurate_steer_angle_seen = True
if self.accurate_steer_angle_seen: 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: if self.needs_angle_offset:
angle_wheel = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION'] 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.needs_angle_offset = False
self.angle_offset = ret.steeringAngle - angle_wheel self.angle_offset = ret.steeringAngleDegDegDeg - angle_wheel
else: 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']) can_gear = int(cp.vl["GEAR_PACKET"]['GEAR'])
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None)) ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))

@ -347,7 +347,7 @@ class CarInterface(CarInterfaceBase):
ret = self.CS.update(self.cp, self.cp_cam) ret = self.CS.update(self.cp, self.cp_cam)
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid 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
events = self.create_common_events(ret) events = self.create_common_events(ret)

@ -28,8 +28,8 @@ class CarState(CarStateBase):
# Update steering angle, rate, yaw rate, and driver input torque. VW send # Update steering angle, rate, yaw rate, and driver input torque. VW send
# the sign/direction in a separate signal so they must be recombined. # 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.steeringAngleDegDegDeg = 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.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.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.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 ret.yawRate = pt_cp.vl["ESP_02"]['ESP_Gierrate'] * (1, -1)[int(pt_cp.vl["ESP_02"]['ESP_VZ_Gierrate'])] * CV.DEG_TO_RAD

@ -74,7 +74,7 @@ class CarInterface(CarInterfaceBase):
ret = self.CS.update(self.cp) ret = self.CS.update(self.cp)
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid 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 # 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, # Update the device metric configuration to match the car at first startup,

@ -389,11 +389,11 @@ class Controls:
# Gas/Brake PID loop # Gas/Brake PID loop
actuators.gas, actuators.brake = self.LoC.update(self.active, CS, v_acc_sol, plan.vTargetFuture, a_acc_sol, self.CP) 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 # 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 # Check for difference between desired angle and angle for angle based control
angle_control_saturated = self.CP.steerControlType == car.CarParams.SteerControlType.angle and \ 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: if angle_control_saturated and not CS.steeringPressed and self.active:
self.saturated_count += 1 self.saturated_count += 1
@ -470,7 +470,7 @@ class Controls:
force_decel = (self.sm['driverMonitoringState'].awarenessStatus < 0.) or \ force_decel = (self.sm['driverMonitoringState'].awarenessStatus < 0.) or \
(self.state == State.softDisabling) (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 # controlsState
dat = messaging.new_message('controlsState') dat = messaging.new_message('controlsState')

@ -83,11 +83,11 @@ class LatControlINDI():
def update(self, active, CS, CP, path_plan): def update(self, active, CS, CP, path_plan):
self.speed = CS.vEgo self.speed = CS.vEgo
# Update Kalman filter # 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) self.x = np.dot(self.A_K, self.x) + np.dot(self.K, y)
indi_log = log.ControlsState.LateralINDIState.new_message() 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.steerRate = math.degrees(self.x[1])
indi_log.steerAccel = math.degrees(self.x[2]) indi_log.steerAccel = math.degrees(self.x[2])
@ -136,7 +136,7 @@ class LatControlINDI():
indi_log.delta = float(delta_u) indi_log.delta = float(delta_u)
indi_log.output = float(self.output_steer) 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) 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 return float(self.output_steer), float(self.angle_steers_des), indi_log

@ -49,7 +49,7 @@ class LatControlLQR():
steers_max = get_steer_max(CP, CS.vEgo) steers_max = get_steer_max(CP, CS.vEgo)
torque_scale = (0.45 + CS.vEgo / 60.0)**2 # Scale actuator model with speed 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 # Subtract offset. Zero angle should correspond to zero torque
self.angle_steers_des = path_plan.angleSteers - path_plan.angleOffset self.angle_steers_des = path_plan.angleSteers - path_plan.angleOffset
@ -86,10 +86,10 @@ class LatControlLQR():
self.output_steer = lqr_output + self.i_lqr self.output_steer = lqr_output + self.i_lqr
self.output_steer = clip(self.output_steer, -steers_max, steers_max) 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) 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.i = self.i_lqr
lqr_log.output = self.output_steer lqr_log.output = self.output_steer
lqr_log.lqrOutput = lqr_output lqr_log.lqrOutput = lqr_output

@ -17,8 +17,8 @@ class LatControlPID():
def update(self, active, CS, CP, path_plan): def update(self, active, CS, CP, path_plan):
pid_log = log.ControlsState.LateralPIDState.new_message() pid_log = log.ControlsState.LateralPIDState.new_message()
pid_log.steerAngle = float(CS.steeringAngle) pid_log.steeringAngleDeg = float(CS.steeringAngleDegDegDeg)
pid_log.steerRate = float(CS.steeringRate) pid_log.steerRate = float(CS.steeringRateDeg)
if CS.vEgo < 0.3 or not active: if CS.vEgo < 0.3 or not active:
output_steer = 0.0 output_steer = 0.0
@ -37,8 +37,8 @@ class LatControlPID():
steer_feedforward *= CS.vEgo**2 # proportional to realigning tire momentum (~ lateral accel) steer_feedforward *= CS.vEgo**2 # proportional to realigning tire momentum (~ lateral accel)
deadzone = 0.0 deadzone = 0.0
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
output_steer = self.pid.update(self.angle_steers_des, CS.steeringAngle, check_saturation=check_saturation, override=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) feedforward=steer_feedforward, speed=CS.vEgo, deadzone=deadzone)
pid_log.active = True pid_log.active = True
pid_log.p = self.pid.p pid_log.p = self.pid.p

@ -83,7 +83,7 @@ class LateralPlanner():
v_ego = sm['carState'].vEgo v_ego = sm['carState'].vEgo
active = sm['controlsState'].active active = sm['controlsState'].active
steering_wheel_angle_offset_deg = sm['liveParameters'].angleOffset 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 # Update vehicle model
x = max(sm['liveParameters'].stiffnessFactor, 0.1) x = max(sm['liveParameters'].stiffnessFactor, 0.1)

@ -132,7 +132,7 @@ class Planner():
if enabled and not self.first_loop and not sm['carState'].gasPressed: if enabled and not self.first_loop and not sm['carState'].gasPressed:
accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego, following)] accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego, following)]
jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])] # TODO: make a separate lookup for jerk tuning jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])] # TODO: make a separate lookup for jerk tuning
accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngle, accel_limits, self.CP) accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngleDegDegDeg, accel_limits, self.CP)
if force_slow_decel: if force_slow_decel:
# if required so, force a smooth deceleration # if required so, force a smooth deceleration

@ -47,7 +47,7 @@ if __name__ == "__main__":
if cnt >= 500: if cnt >= 500:
# calculate error before rounding # calculate error before rounding
actual_angle = sm['controlsState'].angleSteers actual_angle = sm['controlsState'].angleSteers
desired_angle = sm['carControl'].actuators.steerAngle desired_angle = sm['carControl'].actuators.steeringAngleDeg
angle_error = abs(desired_angle - actual_angle) angle_error = abs(desired_angle - actual_angle)
# round numbers # round numbers

@ -48,7 +48,7 @@ class ParamsLearner:
self.kf.predict_and_observe(t, ObservationKind.ANGLE_OFFSET_FAST, np.array([[[0]]])) self.kf.predict_and_observe(t, ObservationKind.ANGLE_OFFSET_FAST, np.array([[[0]]]))
elif which == 'carState': elif which == 'carState':
self.steering_angle = msg.steeringAngle self.steering_angle = msg.steeringAngleDegDegDeg
self.steering_pressed = msg.steeringPressed self.steering_pressed = msg.steeringPressed
self.speed = msg.vEgo self.speed = msg.vEgo
@ -56,7 +56,7 @@ class ParamsLearner:
self.active = self.speed > 5 and in_linear_region self.active = self.speed > 5 and in_linear_region
if self.active: if self.active:
self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, np.array([[[math.radians(msg.steeringAngle)]]])) self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, np.array([[[math.radians(msg.steeringAngleDegDegDeg)]]]))
self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_X_SPEED, np.array([[[self.speed]]])) self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_X_SPEED, np.array([[[self.speed]]]))
if not self.active: if not self.active:

@ -46,7 +46,7 @@ def steer_thread():
if joystick is not None: if joystick is not None:
axis_3 = clip(-joystick.testJoystick.axes[3] * 1.05, -1., 1.) # -1 to 1 axis_3 = clip(-joystick.testJoystick.axes[3] * 1.05, -1., 1.) # -1 to 1
actuators.steer = axis_3 actuators.steer = axis_3
actuators.steerAngle = axis_3 * 43. # deg actuators.steeringAngleDeg = axis_3 * 43. # deg
axis_1 = clip(-joystick.testJoystick.axes[1] * 1.05, -1., 1.) # -1 to 1 axis_1 = clip(-joystick.testJoystick.axes[1] * 1.05, -1., 1.) # -1 to 1
actuators.gas = max(axis_1, 0.) actuators.gas = max(axis_1, 0.)
actuators.brake = max(-axis_1, 0.) actuators.brake = max(-axis_1, 0.)
@ -67,7 +67,7 @@ def steer_thread():
CC.actuators.gas = actuators.gas CC.actuators.gas = actuators.gas
CC.actuators.brake = actuators.brake CC.actuators.brake = actuators.brake
CC.actuators.steer = actuators.steer CC.actuators.steer = actuators.steer
CC.actuators.steerAngle = actuators.steerAngle CC.actuators.steeringAngleDeg = actuators.steeringAngleDeg
CC.hudControl.visualAlert = hud_alert CC.hudControl.visualAlert = hud_alert
CC.hudControl.setSpeed = 20 CC.hudControl.setSpeed = 20
CC.cruiseControl.cancel = pcm_cancel_cmd CC.cruiseControl.cancel = pcm_cancel_cmd

@ -134,15 +134,15 @@ def ui_thread(addr, frame_address):
w = sm['controlsState'].lateralControlState.which() w = sm['controlsState'].lateralControlState.which()
if w == 'lqrState': if w == 'lqrState':
angle_steers_k = sm['controlsState'].lateralControlState.lqrState.steerAngle angle_steers_k = sm['controlsState'].lateralControlState.lqrState.steeringAngleDeg
elif w == 'indiState': elif w == 'indiState':
angle_steers_k = sm['controlsState'].lateralControlState.indiState.steerAngle angle_steers_k = sm['controlsState'].lateralControlState.indiState.steeringAngleDeg
else: else:
angle_steers_k = np.inf angle_steers_k = np.inf
plot_arr[:-1] = plot_arr[1:] plot_arr[:-1] = plot_arr[1:]
plot_arr[-1, name_to_arr_idx['angle_steers']] = sm['carState'].steeringAngle plot_arr[-1, name_to_arr_idx['angle_steers']] = sm['carState'].steeringAngleDegDeg
plot_arr[-1, name_to_arr_idx['angle_steers_des']] = sm['carControl'].actuators.steerAngle plot_arr[-1, name_to_arr_idx['angle_steers_des']] = sm['carControl'].actuators.steeringAngleDeg
plot_arr[-1, name_to_arr_idx['angle_steers_k']] = angle_steers_k plot_arr[-1, name_to_arr_idx['angle_steers_k']] = angle_steers_k
plot_arr[-1, name_to_arr_idx['gas']] = sm['carState'].gas plot_arr[-1, name_to_arr_idx['gas']] = sm['carState'].gas
plot_arr[-1, name_to_arr_idx['computer_gas']] = sm['carControl'].actuators.gas plot_arr[-1, name_to_arr_idx['computer_gas']] = sm['carControl'].actuators.gas

Loading…
Cancel
Save