diff --git a/common/params.cc b/common/params.cc index eb75705ca3..aef2cbdecd 100644 --- a/common/params.cc +++ b/common/params.cc @@ -207,7 +207,6 @@ std::unordered_map keys = { {"UpdaterLastFetchTime", PERSISTENT}, {"Version", PERSISTENT}, {"VisionRadarToggle", PERSISTENT}, - {"WheeledBody", PERSISTENT}, }; } // namespace diff --git a/selfdrive/car/body/carcontroller.py b/selfdrive/car/body/carcontroller.py index 1dad8e796a..d15f11d3a4 100644 --- a/selfdrive/car/body/carcontroller.py +++ b/selfdrive/car/body/carcontroller.py @@ -1,6 +1,5 @@ import numpy as np -from openpilot.common.params import Params from openpilot.common.realtime import DT_CTRL from opendbc.can.packer import CANPacker from openpilot.selfdrive.car.body import bodycan @@ -20,18 +19,13 @@ class CarController: self.frame = 0 self.packer = CANPacker(dbc_name) - # Speed, balance and turn PIDs - 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) + # PIDs 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: @@ -55,17 +49,7 @@ class CarController: speed_measured = SPEED_FROM_RPM * (CS.out.wheelSpeeds.fl + CS.out.wheelSpeeds.fr) / 2. speed_error = speed_desired - speed_measured - 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) + 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/locationd/locationd.cc b/selfdrive/locationd/locationd.cc index e32ed78a3e..1050d68b9b 100644 --- a/selfdrive/locationd/locationd.cc +++ b/selfdrive/locationd/locationd.cc @@ -691,10 +691,9 @@ int Localizer::locationd_thread() { this->configure_gnss_source(source); const std::initializer_list service_list = {gps_location_socket, "cameraOdometry", "liveCalibration", - "carState", "carParams", "accelerometer", "gyroscope"}; + "carState", "accelerometer", "gyroscope"}; - // TODO: remove carParams once we're always sending at 100Hz - SubMaster sm(service_list, {}, nullptr, {gps_location_socket, "carParams"}); + SubMaster sm(service_list, {}, nullptr, {gps_location_socket}); PubMaster pm({"liveLocationKalman"}); uint64_t cnt = 0; @@ -718,8 +717,7 @@ int Localizer::locationd_thread() { filterInitialized = sm.allAliveAndValid(); } - // 100Hz publish for notcars, 20Hz for cars - const char* trigger_msg = sm["carParams"].getCarParams().getNotCar() ? "accelerometer" : "cameraOdometry"; + const char* trigger_msg = "cameraOdometry"; if (sm.updated(trigger_msg)) { bool inputsOK = sm.allValid() && this->are_inputs_ok(); bool gpsOK = this->is_gps_ok(); diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 4fb3e3c4de..5119be0a8c 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -512,7 +512,7 @@ CONFIGS = [ proc_name="locationd", pubs=[ "cameraOdometry", "accelerometer", "gyroscope", "gpsLocationExternal", - "liveCalibration", "carState", "carParams", "gpsLocation" + "liveCalibration", "carState", "gpsLocation" ], subs=["liveLocationKalman"], ignore=["logMonoTime"], diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index fc38f87310..9c627fb7c9 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -d0cdea7eb15f3cac8a921f7ace3eaa6baebb4fd5 +47609e372bf616932c4dca74d2616c3d97fa2443