diff --git a/selfdrive/car/body/carcontroller.py b/selfdrive/car/body/carcontroller.py index 39e27ff0d9..32c2bdf2d6 100644 --- a/selfdrive/car/body/carcontroller.py +++ b/selfdrive/car/body/carcontroller.py @@ -23,10 +23,6 @@ class CarController(): self.i_balance = 0 self.d_balance = 0 - - self.speed_measured = 0. - self.speed_desired = 0. - self.torque_r_filtered = 0. self.torque_l_filtered = 0. @@ -45,22 +41,18 @@ class CarController(): llk_valid = len(CC.orientationNED) > 0 and len(CC.angularVelocity) > 0 if CC.enabled and llk_valid: - - # Steer and accel mixin. Speed should be used as a target? (speed should be in m/s! now it is in RPM) - # Mix steer into torque_diff - # self.steerRatio = 0.5 - # torque_r = int(np.clip((CC.actuators.accel*1000) - (CC.actuators.steer*1000) * self.steerRatio, -1000, 1000)) - # torque_l = int(np.clip((CC.actuators.accel*1000) + (CC.actuators.steer*1000) * self.steerRatio, -1000, 1000)) - # //// + # Read these from the joystick + # TODO: this isn't acceleration, okay? + speed_desired = CC.actuators.accel / 5. + speed_diff_desired = -CC.actuators.steer # Setpoint speed PID kp_speed = 0.001 / SPEED_FROM_RPM - ki_speed = 0.001 / SPEED_FROM_RPM - alpha_speed = 1.0 + ki_speed = 0.002 / SPEED_FROM_RPM + + speed_measured = SPEED_FROM_RPM * (CS.out.wheelSpeeds.fl + CS.out.wheelSpeeds.fr) / 2. + speed_error = speed_desired - speed_measured - self.speed_measured = SPEED_FROM_RPM * (CS.out.wheelSpeeds.fl + CS.out.wheelSpeeds.fr) / 2. - self.speed_desired = (1. - alpha_speed) * self.speed_desired - speed_error = self.speed_desired - self.speed_measured self.i_speed += speed_error * DT_CTRL self.i_speed = np.clip(self.i_speed, -MAX_POS_INTEGRATOR, MAX_POS_INTEGRATOR) set_point = kp_speed * speed_error + ki_speed * self.i_speed @@ -77,13 +69,14 @@ class CarController(): torque = int(np.clip((p_balance*kp_balance + self.i_balance*ki_balance + self.d_balance*kd_balance), -1000, 1000)) # yaw recovery PID - kp_turn = 0.1 / SPEED_FROM_RPM + kp_turn = 0.95 / SPEED_FROM_RPM ki_turn = 0.1 / SPEED_FROM_RPM speed_diff_measured = SPEED_FROM_RPM * (CS.out.wheelSpeeds.fl - CS.out.wheelSpeeds.fr) - self.i_speed_diff += speed_diff_measured * DT_CTRL + speed_diff_error = speed_diff_measured - speed_diff_desired + self.i_speed_diff += speed_diff_error * DT_CTRL self.i_speed_diff = np.clip(self.i_speed_diff, -MAX_TURN_INTEGRATOR, MAX_TURN_INTEGRATOR) - torque_diff = int(np.clip(kp_turn * speed_diff_measured + ki_turn * self.i_speed_diff, -100, 100)) + torque_diff = int(np.clip(kp_turn * speed_diff_error + ki_turn * self.i_speed_diff, -100, 100)) # Combine 2 PIDs outputs torque_r = torque + torque_diff diff --git a/selfdrive/car/body/interface.py b/selfdrive/car/body/interface.py index d2e097fea8..379e89f624 100644 --- a/selfdrive/car/body/interface.py +++ b/selfdrive/car/body/interface.py @@ -1,4 +1,5 @@ #!/usr/bin/env python3 +import math from cereal import car from common.realtime import DT_CTRL from selfdrive.car import scale_rot_inertia, scale_tire_stiffness, get_safety_config @@ -15,6 +16,7 @@ class CarInterface(CarInterfaceBase): ret.carName = "body" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.body)] + ret.minSteerSpeed = -math.inf ret.steerRatio = 0.5 ret.steerRateCost = 0.5 ret.steerLimitTimer = 1.0 diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 0b8be1e9ea..54db6976d6 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -72,18 +72,6 @@ class Controls: if TICI: self.camera_packets.append("wideRoadCameraState") - params = Params() - self.joystick_mode = params.get_bool("JoystickDebugMode") - joystick_packet = ['testJoystick'] if self.joystick_mode else [] - - self.sm = sm - if self.sm is None: - ignore = ['driverCameraState', 'managerState'] if SIMULATION else None - self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration', - 'driverMonitoringState', 'longitudinalPlan', 'lateralPlan', 'liveLocationKalman', - 'managerState', 'liveParameters', 'radarState'] + self.camera_packets + joystick_packet, - ignore_alive=ignore, ignore_avg_freq=['radarState', 'longitudinalPlan']) - self.can_sock = can_sock if can_sock is None: can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100 @@ -101,6 +89,19 @@ class Controls: else: self.CI, self.CP = CI, CI.CP + params = Params() + self.joystick_mode = params.get_bool("JoystickDebugMode") or (self.CP.notCar and sm is None) + joystick_packet = ['testJoystick'] if self.joystick_mode else [] + + self.sm = sm + if self.sm is None: + ignore = ['driverCameraState', 'managerState'] if SIMULATION else None + self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration', + 'driverMonitoringState', 'longitudinalPlan', 'lateralPlan', 'liveLocationKalman', + 'managerState', 'liveParameters', 'radarState'] + self.camera_packets + joystick_packet, + ignore_alive=ignore, ignore_avg_freq=['radarState', 'longitudinalPlan']) + + # set alternative experiences from parameters self.disengage_on_accelerator = params.get_bool("DisengageOnAccelerator") self.CP.alternativeExperience = 0 diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 87b1281b1b..fa06c2891f 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -65fca83abed98f32993286dc5a66e3e583f06172 \ No newline at end of file +d71efcf28d2216aa53eb4f272514d28c9f96d433 \ No newline at end of file