diff --git a/common/params.cc b/common/params.cc index 509d419d64..e8ab42c0b0 100644 --- a/common/params.cc +++ b/common/params.cc @@ -205,6 +205,7 @@ std::unordered_map keys = { {"UpdaterTargetBranch", CLEAR_ON_MANAGER_START}, {"Version", PERSISTENT}, {"VisionRadarToggle", PERSISTENT}, + {"WheeledBody", PERSISTENT}, }; } // namespace diff --git a/selfdrive/car/body/carcontroller.py b/selfdrive/car/body/carcontroller.py index 64c5617ef3..ee5d3f6886 100644 --- a/selfdrive/car/body/carcontroller.py +++ b/selfdrive/car/body/carcontroller.py @@ -1,5 +1,6 @@ import numpy as np +from common.params import Params from common.realtime import DT_CTRL from opendbc.can.packer import CANPacker from selfdrive.car.body import bodycan @@ -23,10 +24,14 @@ class CarController: self.speed_pid = PIDController(0.115, k_i=0.23, rate=1/DT_CTRL) self.balance_pid = PIDController(1300, k_i=0, k_d=280, rate=1/DT_CTRL) self.turn_pid = PIDController(110, k_i=11.5, rate=1/DT_CTRL) + self.wheeled_speed_pid = PIDController(110, k_i=11.5, rate=1/DT_CTRL) self.torque_r_filtered = 0. self.torque_l_filtered = 0. + params = Params() + self.wheeled_body = params.get("WheeledBody") + @staticmethod def deadband_filter(torque, deadband): if torque > 0: @@ -45,19 +50,22 @@ class CarController: # Read these from the joystick # TODO: this isn't acceleration, okay? speed_desired = CC.actuators.accel / 5. - speed_diff_desired = -CC.actuators.steer + speed_diff_desired = -CC.actuators.steer / 2. speed_measured = SPEED_FROM_RPM * (CS.out.wheelSpeeds.fl + CS.out.wheelSpeeds.fr) / 2. speed_error = speed_desired - speed_measured - freeze_integrator = ((speed_error < 0 and self.speed_pid.error_integral <= -MAX_POS_INTEGRATOR) or - (speed_error > 0 and self.speed_pid.error_integral >= MAX_POS_INTEGRATOR)) - angle_setpoint = self.speed_pid.update(speed_error, freeze_integrator=freeze_integrator) - - # Clip angle error, this is enough to get up from stands - angle_error = np.clip((-CC.orientationNED[1]) - angle_setpoint, -MAX_ANGLE_ERROR, MAX_ANGLE_ERROR) - angle_error_rate = np.clip(-CC.angularVelocity[1], -1., 1.) - torque = self.balance_pid.update(angle_error, error_rate=angle_error_rate) + if self.wheeled_body is None: + freeze_integrator = ((speed_error < 0 and self.speed_pid.error_integral <= -MAX_POS_INTEGRATOR) or + (speed_error > 0 and self.speed_pid.error_integral >= MAX_POS_INTEGRATOR)) + angle_setpoint = self.speed_pid.update(speed_error, freeze_integrator=freeze_integrator) + + # Clip angle error, this is enough to get up from stands + angle_error = np.clip((-CC.orientationNED[1]) - angle_setpoint, -MAX_ANGLE_ERROR, MAX_ANGLE_ERROR) + angle_error_rate = np.clip(-CC.angularVelocity[1], -1., 1.) + torque = self.balance_pid.update(angle_error, error_rate=angle_error_rate) + else: + torque = self.wheeled_speed_pid.update(speed_error, freeze_integrator=False) speed_diff_measured = SPEED_FROM_RPM * (CS.out.wheelSpeeds.fl - CS.out.wheelSpeeds.fr) turn_error = speed_diff_measured - speed_diff_desired diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 601d34e948..fe5bb36714 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -640,7 +640,7 @@ class Controls: recent_steer_pressed = (self.sm.frame - self.last_steering_pressed_frame)*DT_CTRL < 2.0 # Send a "steering required alert" if saturation count has reached the limit - if lac_log.active and not recent_steer_pressed: + if lac_log.active and not recent_steer_pressed and not self.CP.notCar: if self.CP.lateralTuning.which() == 'torque' and not self.joystick_mode: undershooting = abs(lac_log.desiredLateralAccel) / abs(1e-3 + lac_log.actualLateralAccel) > 1.2 turning = abs(lac_log.desiredLateralAccel) > 1.0