From 77fd1c65ebd01abfa8493ae12c9e6b14f7ada976 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Sat, 13 Feb 2021 13:45:57 -0800 Subject: [PATCH] car stuff --- cereal | 2 +- selfdrive/car/chrysler/carstate.py | 4 ++-- selfdrive/car/chrysler/interface.py | 2 +- selfdrive/car/ford/carcontroller.py | 4 ++-- selfdrive/car/ford/carstate.py | 2 +- selfdrive/car/gm/carstate.py | 2 +- selfdrive/car/gm/interface.py | 2 +- selfdrive/car/honda/carstate.py | 4 ++-- selfdrive/car/honda/interface.py | 2 +- selfdrive/car/hyundai/carcontroller.py | 2 +- selfdrive/car/hyundai/carstate.py | 4 ++-- selfdrive/car/hyundai/interface.py | 6 +++--- selfdrive/car/mazda/carstate.py | 4 ++-- selfdrive/car/mock/interface.py | 2 +- selfdrive/car/nissan/carcontroller.py | 4 ++-- selfdrive/car/nissan/carstate.py | 2 +- selfdrive/car/subaru/carstate.py | 2 +- selfdrive/car/subaru/interface.py | 2 +- selfdrive/car/toyota/carcontroller.py | 2 +- selfdrive/car/toyota/carstate.py | 10 +++++----- selfdrive/car/toyota/interface.py | 2 +- selfdrive/car/volkswagen/carstate.py | 4 ++-- selfdrive/car/volkswagen/interface.py | 2 +- selfdrive/controls/controlsd.py | 6 +++--- selfdrive/controls/lib/latcontrol_indi.py | 6 +++--- selfdrive/controls/lib/latcontrol_lqr.py | 6 +++--- selfdrive/controls/lib/latcontrol_pid.py | 8 ++++---- selfdrive/controls/lib/lateral_planner.py | 2 +- selfdrive/controls/lib/longitudinal_planner.py | 2 +- selfdrive/debug/internal/measure_steering_accuracy.py | 2 +- selfdrive/locationd/paramsd.py | 4 ++-- tools/carcontrols/debug_controls.py | 4 ++-- tools/replay/ui.py | 8 ++++---- 33 files changed, 60 insertions(+), 60 deletions(-) diff --git a/cereal b/cereal index 39d80be841..775db20d6e 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 39d80be84108820a42fc4b1b1817e8e3b73c89fb +Subproject commit 775db20d6eb0112dbae82e49f92dc281f0c6daa7 diff --git a/selfdrive/car/chrysler/carstate.py b/selfdrive/car/chrysler/carstate.py index 5994a813c2..3bc71b5f56 100644 --- a/selfdrive/car/chrysler/carstate.py +++ b/selfdrive/car/chrysler/carstate.py @@ -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. diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index e433665d99..a6d1061bd6 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -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], diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py index 648fa07741..16c4e5c675 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/selfdrive/car/ford/carcontroller.py @@ -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: diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py index 7f34acc9df..768ca768ea 100644 --- a/selfdrive/car/ford/carstate.py +++ b/selfdrive/car/ford/carstate.py @@ -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 diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index 7f45e7ea9e..99dd6dccec 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -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. diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 2f7d8f473b..c98bd9bf1a 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -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 = [] diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index 16d85892a6..ba61456f79 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -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'] diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index f1b71f0763..8855bc493e 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -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 diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index a9001ca8d5..7ad8bd7750 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -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: diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 951b615e07..83143b9ea6 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -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']) diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 5569aa6ff7..6ad626389a 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -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 diff --git a/selfdrive/car/mazda/carstate.py b/selfdrive/car/mazda/carstate.py index bc6c6d0eb9..17389e4338 100644 --- a/selfdrive/car/mazda/carstate.py +++ b/selfdrive/car/mazda/carstate.py @@ -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'] diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py index e497ab96e4..69ab88dd65 100755 --- a/selfdrive/car/mock/interface.py +++ b/selfdrive/car/mock/interface.py @@ -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() diff --git a/selfdrive/car/nissan/carcontroller.py b/selfdrive/car/nissan/carcontroller.py index 2804ec86cb..5641cb38f4 100644 --- a/selfdrive/car/nissan/carcontroller.py +++ b/selfdrive/car/nissan/carcontroller.py @@ -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 diff --git a/selfdrive/car/nissan/carstate.py b/selfdrive/car/nissan/carstate.py index fcaa878a01..7c9afa7d32 100644 --- a/selfdrive/car/nissan/carstate.py +++ b/selfdrive/car/nissan/carstate.py @@ -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"]) diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py index 3be516ed33..a5015388fe 100644 --- a/selfdrive/car/subaru/carstate.py +++ b/selfdrive/car/subaru/carstate.py @@ -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] diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index 0cfb6fb494..f78fd5de3c 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -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() diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index a3a04dcc9a..7152aa73ac 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -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): diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 32474025f3..a7830e824e 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -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)) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 4ba0989bb0..a9be7f8776 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -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) diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py index a994130929..24309b287f 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -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 diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index 24c078d7ad..497130a150 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -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, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 5764265c8e..9377101a91 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -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') diff --git a/selfdrive/controls/lib/latcontrol_indi.py b/selfdrive/controls/lib/latcontrol_indi.py index 277f0e0779..fbfff7652d 100644 --- a/selfdrive/controls/lib/latcontrol_indi.py +++ b/selfdrive/controls/lib/latcontrol_indi.py @@ -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 diff --git a/selfdrive/controls/lib/latcontrol_lqr.py b/selfdrive/controls/lib/latcontrol_lqr.py index cb54e52ebd..9dc1bb0bb6 100644 --- a/selfdrive/controls/lib/latcontrol_lqr.py +++ b/selfdrive/controls/lib/latcontrol_lqr.py @@ -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 diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index 173dcd58f7..befc8fa82a 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -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 diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index b77be714ba..d81d55f216 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/selfdrive/controls/lib/lateral_planner.py @@ -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) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index b812bc48a0..297d7fca56 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -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 diff --git a/selfdrive/debug/internal/measure_steering_accuracy.py b/selfdrive/debug/internal/measure_steering_accuracy.py index 706af73989..a9040a6d4a 100755 --- a/selfdrive/debug/internal/measure_steering_accuracy.py +++ b/selfdrive/debug/internal/measure_steering_accuracy.py @@ -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 diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index c0afe16974..43a7ce28a0 100755 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -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: diff --git a/tools/carcontrols/debug_controls.py b/tools/carcontrols/debug_controls.py index 595147048f..9264b2d768 100755 --- a/tools/carcontrols/debug_controls.py +++ b/tools/carcontrols/debug_controls.py @@ -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 diff --git a/tools/replay/ui.py b/tools/replay/ui.py index d910dd5732..03a9d01744 100755 --- a/tools/replay/ui.py +++ b/tools/replay/ui.py @@ -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