Body cleanup + 100Hz locationd (#24168)

* use PID

* 100hz on the branch

* Better defaults

* fix int clip

* More cleanup

* Fix pid comments

* only notcar gets 100hz

* cleanup

* fix tests

* ignore

* update refs

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>

selfdrive/car/body/carcontroller.py

@@ -4,10 +4,12 @@ from common.realtime import DT_CTRL
 from selfdrive.car.body import bodycan
 from opendbc.can.packer import CANPacker
 from selfdrive.car.body.values import SPEED_FROM_RPM
+from selfdrive.controls.lib.pid import PIDController
+
 
 MAX_TORQUE = 500
 MAX_TORQUE_RATE = 50
-MAX_ANGLE_ERROR = 7
+MAX_ANGLE_ERROR = np.radians(7)
 MAX_POS_INTEGRATOR = 0.2   # meters
 MAX_TURN_INTEGRATOR = 0.1  # meters
 
@@ -17,11 +19,10 @@ class CarController():
     self.frame = 0
     self.packer = CANPacker(dbc_name)
 
-    self.i_speed = 0
-    self.i_speed_diff = 0
-
-    self.i_balance = 0
-    self.d_balance = 0
+    # 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)
+    self.turn_pid = PIDController(110, k_i=11.5, rate=1/DT_CTRL)
 
     self.torque_r_filtered = 0.
     self.torque_l_filtered = 0.
@@ -46,37 +47,23 @@ class CarController():
       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.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.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
-
-      # Balancing PID
-      kp_balance = 1300
-      ki_balance = 0
-      kd_balance = 280
+      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
-      p_balance = np.clip((-CC.orientationNED[1]) - set_point, np.radians(-MAX_ANGLE_ERROR), np.radians(MAX_ANGLE_ERROR))
-      self.i_balance += CS.out.wheelSpeeds.fl + CS.out.wheelSpeeds.fr
-      self.d_balance = np.clip(-CC.angularVelocity[1], -1., 1.)
-      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.95 / SPEED_FROM_RPM
-      ki_turn = 0.1 / SPEED_FROM_RPM
+      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)
 
       speed_diff_measured = SPEED_FROM_RPM * (CS.out.wheelSpeeds.fl - CS.out.wheelSpeeds.fr)
-      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_error + ki_turn * self.i_speed_diff, -100, 100))
+      turn_error = speed_diff_measured - speed_diff_desired
+      freeze_integrator = ((turn_error < 0 and self.turn_pid.error_integral <= -MAX_POS_INTEGRATOR) or
+                           (turn_error > 0 and self.turn_pid.error_integral >= MAX_POS_INTEGRATOR))
+      torque_diff = self.turn_pid.update(turn_error, freeze_integrator=freeze_integrator)
 
       # Combine 2 PIDs outputs
       torque_r = torque + torque_diff

selfdrive/controls/lib/pid.py

@@ -5,10 +5,10 @@ from common.numpy_fast import clip, interp
 
 
 class PIDController():
-  def __init__(self, k_p, k_i, k_f=0., k_d=0., pos_limit=None, neg_limit=None, rate=100):
-    self._k_p = k_p  # proportional gain
-    self._k_i = k_i  # integral gain
-    self._k_d = k_d   # feedforward gain
+  def __init__(self, k_p, k_i, k_f=0., k_d=0., pos_limit=1e308, neg_limit=-1e308, rate=100):
+    self._k_p = k_p
+    self._k_i = k_i
+    self._k_d = k_d
     self.k_f = k_f   # feedforward gain
     if isinstance(self._k_p, Number):
       self._k_p = [[0], [self._k_p]]
@@ -22,6 +22,7 @@ class PIDController():
 
     self.i_unwind_rate = 0.3 / rate
     self.i_rate = 1.0 / rate
+    self.speed = 0.0
 
     self.reset()
 
@@ -37,6 +38,10 @@ class PIDController():
   def k_d(self):
     return interp(self.speed, self._k_d[0], self._k_d[1])
 
+  @property
+  def error_integral(self):
+    return self.i/self.k_i
+
   def reset(self):
     self.p = 0.0
     self.i = 0.0

selfdrive/locationd/liblocationd.cc

@@ -7,11 +7,10 @@ extern "C" {
     return new Localizer();
   }
 
-  void localizer_get_message_bytes(Localizer *localizer, uint64_t logMonoTime,
-    bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid, char *buff, size_t buff_size)
-  {
+  void localizer_get_message_bytes(Localizer *localizer, bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid,
+                                   char *buff, size_t buff_size) {
     MessageBuilder msg_builder;
-    kj::ArrayPtr<char> arr = localizer->get_message_bytes(msg_builder, logMonoTime, inputsOK, sensorsOK, gpsOK, msgValid).asChars();
+    kj::ArrayPtr<char> arr = localizer->get_message_bytes(msg_builder, inputsOK, sensorsOK, gpsOK, msgValid).asChars();
     assert(buff_size >= arr.size());
     memcpy(buff, arr.begin(), arr.size());
   }

selfdrive/locationd/locationd.cc

@@ -456,11 +456,9 @@ void Localizer::handle_msg(const cereal::Event::Reader& log) {
   this->update_reset_tracker();
 }
 
-kj::ArrayPtr<capnp::byte> Localizer::get_message_bytes(MessageBuilder& msg_builder, uint64_t logMonoTime,
-  bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid)
-{
+kj::ArrayPtr<capnp::byte> Localizer::get_message_bytes(MessageBuilder& msg_builder, bool inputsOK,
+                                                       bool sensorsOK, bool gpsOK, bool msgValid) {
   cereal::Event::Builder evt = msg_builder.initEvent();
-  evt.setLogMonoTime(logMonoTime);
   evt.setValid(msgValid);
   cereal::LiveLocationKalman::Builder liveLoc = evt.initLiveLocationKalman();
   this->build_live_location(liveLoc);
@@ -492,10 +490,11 @@ void Localizer::determine_gps_mode(double current_time) {
 }
 
 int Localizer::locationd_thread() {
-  const std::initializer_list<const char *> service_list =
-      { "gpsLocationExternal", "sensorEvents", "cameraOdometry", "liveCalibration", "carState" };
-  PubMaster pm({ "liveLocationKalman" });
-  SubMaster sm(service_list, nullptr, { "gpsLocationExternal" });
+  const std::initializer_list<const char *> service_list = {"gpsLocationExternal", "sensorEvents", "cameraOdometry", "liveCalibration", "carState", "carParams"};
+  PubMaster pm({"liveLocationKalman"});
+
+  // TODO: remove carParams once we're always sending at 100Hz
+  SubMaster sm(service_list, nullptr, {"gpsLocationExternal", "carParams"});
 
   uint64_t cnt = 0;
   bool filterInitialized = false;
@@ -513,14 +512,15 @@ int Localizer::locationd_thread() {
       filterInitialized = sm.allAliveAndValid();
     }
 
-    if (sm.updated("cameraOdometry")) {
-      uint64_t logMonoTime = sm["cameraOdometry"].getLogMonoTime();
+    // 100Hz publish for notcars, 20Hz for cars
+    const char* trigger_msg = sm["carParams"].getCarParams().getNotCar() ? "sensorEvents" : "cameraOdometry";
+    if (sm.updated(trigger_msg)) {
       bool inputsOK = sm.allAliveAndValid();
       bool sensorsOK = sm.alive("sensorEvents") && sm.valid("sensorEvents");
       bool gpsOK = this->isGpsOK();
 
       MessageBuilder msg_builder;
-      kj::ArrayPtr<capnp::byte> bytes = this->get_message_bytes(msg_builder, logMonoTime, inputsOK, sensorsOK, gpsOK, filterInitialized);
+      kj::ArrayPtr<capnp::byte> bytes = this->get_message_bytes(msg_builder, inputsOK, sensorsOK, gpsOK, filterInitialized);
       pm.send("liveLocationKalman", bytes.begin(), bytes.size());
 
       if (cnt % 1200 == 0 && gpsOK) {  // once a minute

selfdrive/locationd/locationd.h

@@ -35,7 +35,7 @@ public:
   bool isGpsOK();
   void determine_gps_mode(double current_time);
 
-  kj::ArrayPtr<capnp::byte> get_message_bytes(MessageBuilder& msg_builder, uint64_t logMonoTime,
+  kj::ArrayPtr<capnp::byte> get_message_bytes(MessageBuilder& msg_builder,
     bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid);
   void build_live_location(cereal::LiveLocationKalman::Builder& fix);
 

selfdrive/locationd/test/test_locationd.py

@@ -24,7 +24,7 @@ class TestLocationdLib(unittest.TestCase):
   def setUp(self):
     header = '''typedef ...* Localizer_t;
 Localizer_t localizer_init();
-void localizer_get_message_bytes(Localizer_t localizer, uint64_t logMonoTime, bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid, char *buff, size_t buff_size);
+void localizer_get_message_bytes(Localizer_t localizer, bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid, char *buff, size_t buff_size);
 void localizer_handle_msg_bytes(Localizer_t localizer, const char *data, size_t size);'''
 
     self.ffi = FFI()
@@ -41,7 +41,7 @@ void localizer_handle_msg_bytes(Localizer_t localizer, const char *data, size_t
     self.lib.localizer_handle_msg_bytes(self.localizer, self.ffi.from_buffer(bytstr), len(bytstr))
 
   def localizer_get_msg(self, t=0, inputsOK=True, sensorsOK=True, gpsOK=True, msgValid=True):
-    self.lib.localizer_get_message_bytes(self.localizer, t, inputsOK, sensorsOK, gpsOK, msgValid, self.ffi.addressof(self.msg_buff, 0), self.buff_size)
+    self.lib.localizer_get_message_bytes(self.localizer, inputsOK, sensorsOK, gpsOK, msgValid, self.ffi.addressof(self.msg_buff, 0), self.buff_size)
     return log.Event.from_bytes(self.ffi.buffer(self.msg_buff), nesting_limit=self.buff_size // 8)
 
   def test_liblocalizer(self):

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-c93e3735143a88f0088eb23e6fde56660d4a53e2
+de01f1544a1441f89c24c5ea587f3f99632f2b5a