Wheeled body (#31614)

* Wheeled body

* 100hz only for balance

* No carparams in locationd no more

* Update ref
old-commit-hash: c05b37979d

common/params.cc

@@ -207,7 +207,6 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"UpdaterLastFetchTime", PERSISTENT},
     {"Version", PERSISTENT},
     {"VisionRadarToggle", PERSISTENT},
-    {"WheeledBody", PERSISTENT},
 };
 
 } // namespace

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

selfdrive/locationd/locationd.cc

@@ -691,10 +691,9 @@ int Localizer::locationd_thread() {
 
   this->configure_gnss_source(source);
   const std::initializer_list<const char *> 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();

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"],

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-d0cdea7eb15f3cac8a921f7ace3eaa6baebb4fd5
+47609e372bf616932c4dca74d2616c3d97fa2443