spacecruft
/
nopenpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

car stuff

pull/20092/head
Adeeb Shihadeh 2021-02-13 13:45:57 -08:00
parent c189d15af9
commit 77fd1c65eb
33 changed files with 60 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cereal

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

4
selfdrive/car/chrysler/carstate.py
View File

@ -42,8 +42,8 @@ class CarState(CarStateBase):
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.

2
selfdrive/car/chrysler/interface.py
View File

@ -67,7 +67,7 @@ class CarInterface(CarInterfaceBase):
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],

4
selfdrive/car/ford/carcontroller.py
View File

@ -33,7 +33,7 @@ class CarController():
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:
@ -43,7 +43,7 @@ class CarController():
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:

2
selfdrive/car/ford/carstate.py
View File

@ -17,7 +17,7 @@ class CarState(CarStateBase):
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

2
selfdrive/car/gm/carstate.py
View File

@ -28,7 +28,7 @@ class CarState(CarStateBase):
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.

2
selfdrive/car/gm/interface.py
View File

@ -121,7 +121,7 @@ class CarInterface(CarInterfaceBase):
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 = []

4
selfdrive/car/honda/carstate.py
View File

@ -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.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']

2
selfdrive/car/honda/interface.py
View File

@ -447,7 +447,7 @@ class CarInterface(CarInterfaceBase):
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

2
selfdrive/car/hyundai/carcontroller.py
View File

@ -50,7 +50,7 @@ class CarController():
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:

4
selfdrive/car/hyundai/carstate.py
View File

@ -26,8 +26,8 @@ class CarState(CarStateBase):
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'])

6
selfdrive/car/hyundai/interface.py
View File

@ -27,7 +27,7 @@ class CarInterface(CarInterfaceBase):
ret.steerLimitTimer = 0.4
tire_stiffness_factor = 1.
ret.maxSteerAngle = 90.
ret.maxSteerAngleDeg = 90.
eps_modified = False
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.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
@ -222,7 +222,7 @@ class CarInterface(CarInterfaceBase):
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

4
selfdrive/car/mazda/carstate.py
View File

@ -38,12 +38,12 @@ class CarState(CarStateBase):
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']

2
selfdrive/car/mock/interface.py
View File

@ -81,7 +81,7 @@ class CarInterface(CarInterfaceBase):
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()

4
selfdrive/car/nissan/carcontroller.py
View File

@ -29,7 +29,7 @@ class CarController():
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
@ -55,7 +55,7 @@ class CarController():
)
else:
apply_angle = CS.out.steeringAngle
apply_angle = CS.out.steeringAngleDegDegDeg
self.lkas_max_torque = 0
self.last_angle = apply_angle

2
selfdrive/car/nissan/carstate.py
View File

@ -70,7 +70,7 @@ class CarState(CarStateBase):
# 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"])

2
selfdrive/car/subaru/carstate.py
View File

@ -43,7 +43,7 @@ class CarState(CarStateBase):
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]

2
selfdrive/car/subaru/interface.py
View File

@ -110,7 +110,7 @@ class CarInterface(CarInterfaceBase):
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()

2
selfdrive/car/toyota/carcontroller.py
View File

@ -103,7 +103,7 @@ class CarController():
# 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):

10
selfdrive/car/toyota/carstate.py
View File

@ -52,17 +52,17 @@ class CarState(CarStateBase):
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))

2
selfdrive/car/toyota/interface.py
View File

@ -347,7 +347,7 @@ class CarInterface(CarInterfaceBase):
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)

4
selfdrive/car/volkswagen/carstate.py
View File

@ -28,8 +28,8 @@ class CarState(CarStateBase):
# 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

2
selfdrive/car/volkswagen/interface.py
View File

@ -74,7 +74,7 @@ class CarInterface(CarInterfaceBase):
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,

6
selfdrive/controls/controlsd.py
View File

@ -389,11 +389,11 @@ class Controls:
# 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
@ -470,7 +470,7 @@ class Controls:
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')

6
selfdrive/controls/lib/latcontrol_indi.py
View File

@ -83,11 +83,11 @@ class LatControlINDI():
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])
@ -136,7 +136,7 @@ class LatControlINDI():
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
selfdrive/controls/lib/latcontrol_lqr.py
View File

@ -49,7 +49,7 @@ class LatControlLQR():
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
@ -86,10 +86,10 @@ class LatControlLQR():
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

8
selfdrive/controls/lib/latcontrol_pid.py
View File

@ -17,8 +17,8 @@ class LatControlPID():
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
@ -37,8 +37,8 @@ class LatControlPID():
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

2
selfdrive/controls/lib/lateral_planner.py
View File

@ -83,7 +83,7 @@ class LateralPlanner():
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)

2
selfdrive/controls/lib/longitudinal_planner.py
View File

@ -132,7 +132,7 @@ class Planner():
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)]
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 required so, force a smooth deceleration

2
selfdrive/debug/internal/measure_steering_accuracy.py
View File

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

4
selfdrive/locationd/paramsd.py
View File

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

4
tools/carcontrols/debug_controls.py
View File

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

8
tools/replay/ui.py
View File

@ -134,15 +134,15 @@ def ui_thread(addr, frame_address):
w = sm['controlsState'].lateralControlState.which()
if w == 'lqrState':
angle_steers_k = sm['controlsState'].lateralControlState.lqrState.steerAngle
angle_steers_k = sm['controlsState'].lateralControlState.lqrState.steeringAngleDeg
elif w == 'indiState':
angle_steers_k = sm['controlsState'].lateralControlState.indiState.steerAngle
angle_steers_k = sm['controlsState'].lateralControlState.indiState.steeringAngleDeg
else:
angle_steers_k = np.inf
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_des']] = sm['carControl'].actuators.steerAngle
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.steeringAngleDeg
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['computer_gas']] = sm['carControl'].actuators.gas