diff --git a/selfdrive/car/body/carcontroller.py b/selfdrive/car/body/carcontroller.py index 32c2bdf2d6..5714445f67 100644 --- a/selfdrive/car/body/carcontroller.py +++ b/selfdrive/car/body/carcontroller.py @@ -4,12 +4,14 @@ 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_POS_INTEGRATOR = 0.2 # meters -MAX_TURN_INTEGRATOR = 0.1 # meters +MAX_ANGLE_ERROR = np.radians(7) +MAX_POS_INTEGRATOR = 0.2 # meters +MAX_TURN_INTEGRATOR = 0.1 # meters class CarController(): @@ -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 diff --git a/selfdrive/controls/lib/pid.py b/selfdrive/controls/lib/pid.py index b5ef735680..965158131b 100644 --- a/selfdrive/controls/lib/pid.py +++ b/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 diff --git a/selfdrive/locationd/liblocationd.cc b/selfdrive/locationd/liblocationd.cc index 07216365b3..49404668a4 100755 --- a/selfdrive/locationd/liblocationd.cc +++ b/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 arr = localizer->get_message_bytes(msg_builder, logMonoTime, inputsOK, sensorsOK, gpsOK, msgValid).asChars(); + kj::ArrayPtr arr = localizer->get_message_bytes(msg_builder, inputsOK, sensorsOK, gpsOK, msgValid).asChars(); assert(buff_size >= arr.size()); memcpy(buff, arr.begin(), arr.size()); } diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc index 6864ada0a4..3875d969c5 100755 --- a/selfdrive/locationd/locationd.cc +++ b/selfdrive/locationd/locationd.cc @@ -456,11 +456,9 @@ void Localizer::handle_msg(const cereal::Event::Reader& log) { this->update_reset_tracker(); } -kj::ArrayPtr Localizer::get_message_bytes(MessageBuilder& msg_builder, uint64_t logMonoTime, - bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid) -{ +kj::ArrayPtr 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 service_list = - { "gpsLocationExternal", "sensorEvents", "cameraOdometry", "liveCalibration", "carState" }; - PubMaster pm({ "liveLocationKalman" }); - SubMaster sm(service_list, nullptr, { "gpsLocationExternal" }); + const std::initializer_list 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; @@ -512,15 +511,16 @@ int Localizer::locationd_thread() { } else { 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 bytes = this->get_message_bytes(msg_builder, logMonoTime, inputsOK, sensorsOK, gpsOK, filterInitialized); + kj::ArrayPtr 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 diff --git a/selfdrive/locationd/locationd.h b/selfdrive/locationd/locationd.h index f75e334425..2ab3ba56ad 100755 --- a/selfdrive/locationd/locationd.h +++ b/selfdrive/locationd/locationd.h @@ -35,7 +35,7 @@ public: bool isGpsOK(); void determine_gps_mode(double current_time); - kj::ArrayPtr get_message_bytes(MessageBuilder& msg_builder, uint64_t logMonoTime, + kj::ArrayPtr get_message_bytes(MessageBuilder& msg_builder, bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid); void build_live_location(cereal::LiveLocationKalman::Builder& fix); diff --git a/selfdrive/locationd/test/test_locationd.py b/selfdrive/locationd/test/test_locationd.py index e804917a0b..5c0143bdbf 100755 --- a/selfdrive/locationd/test/test_locationd.py +++ b/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): diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 638b70531a..79349868dd 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -c93e3735143a88f0088eb23e6fde56660d4a53e2 \ No newline at end of file +de01f1544a1441f89c24c5ea587f3f99632f2b5a \ No newline at end of file