diff --git a/SA_RELEASES.md b/SA_RELEASES.md index abcd5713d60799..e7a24a96c53c3f 100644 --- a/SA_RELEASES.md +++ b/SA_RELEASES.md @@ -1,6 +1,7 @@ Stock Additions Update 2 - 2021-08-15 (0.8.8) === * Update SA to 0.8.8 with new model from upstream! + * Experimental model cruise control button is back with a new model Stock Additions Update 1 - 2021-08-15 (0.8.7) === diff --git a/models/supercombo.dlc b/models/supercombo.dlc index 29c282c03e3860..e339a0c12861f8 100644 --- a/models/supercombo.dlc +++ b/models/supercombo.dlc @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:1c53859f4d15a172811e0af815f192c272072005366c1cb9d05b819f19a6c48d +oid sha256:033b03a182d6befef35bb8b7066ab400691da1b26856d76ff5b4c517db29131a size 56720671 diff --git a/models/supercombo.onnx b/models/supercombo.onnx index 6d2a81b5b1a57a..36ee6dfe91d6f9 100644 --- a/models/supercombo.onnx +++ b/models/supercombo.onnx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:69c1f8f71fd815c9d30361b99b1dfd39df5460176c628c038d3f7d91e4801704 +oid sha256:70fb3f18b77a5e6f4c2efaba4f6287e42b9e15d0428fe9ab161d2d8dd10d85a2 size 56742706 diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 9a64c44838a358..0d663936cf84df 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -92,11 +92,12 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # py ret.minSpeedCan = 0.1 * CV.KPH_TO_MS tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 2860. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid - - ret.lateralTuning.init('model') - ret.lateralTuning.model.name = "corolla_model_v5" - ret.lateralTuning.model.useRates = False # TODO: makes model sluggish, see comments in latcontrol_model.py - ret.lateralTuning.model.multiplier = 1. + ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kpV = [[20, 31], [0.06, 0.12]] # 45 to 70 mph + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kiV = [[20, 31], [0.001, 0.02]] + ret.lateralTuning.pid.kdBP, ret.lateralTuning.pid.kdV = [[20, 31], [0.1, 0.2]] + ret.lateralTuning.pid.kf = 0.00003 # full torque for 20 deg at 80mph means 0.00007818594 + # ret.lateralTuning.pid.kf = 0.00006908923778520113 # full torque for 20 deg at 80mph means 0.00007818594 + ret.lateralTuning.pid.newKfTuned = True elif candidate == CAR.LEXUS_RX: stop_and_go = True diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index 3db6781674ea5f..d6206864a8d568 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -1,5 +1,8 @@ +import importlib import math +from common.filter_simple import FirstOrderFilter +from common.realtime import DT_CTRL from selfdrive.controls.lib.pid import LatPIDController from selfdrive.controls.lib.drive_helpers import get_steer_max from cereal import log @@ -11,7 +14,12 @@ def __init__(self, CP): (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV), (CP.lateralTuning.pid.kdBP, CP.lateralTuning.pid.kdV), k_f=CP.lateralTuning.pid.kf, pos_limit=1.0, sat_limit=CP.steerLimitTimer) - self.new_kf_tuned = CP.lateralTuning.pid.newKfTuned + self.new_kf_tuned = True # CP.lateralTuning.pid.newKfTuned + self.kf_filter = FirstOrderFilter(CP.lateralTuning.pid.kf, 10, DT_CTRL) + + self.CarControllerParams = importlib.import_module('selfdrive.car.{}.values'.format(CP.carName)).CarControllerParams + assert self.CarControllerParams, 'Missing CarControllerParams!' + assert self.CarControllerParams.STEER_MAX != 0, 'Can\'t be 0' def reset(self): self.pid.reset() @@ -28,6 +36,13 @@ def update(self, active, CS, CP, VM, params, desired_curvature, desired_curvatur output_steer = 0.0 pid_log.active = False self.pid.reset() + + if abs(CS.steeringRateDeg) < 20 and 5 < abs(CS.steeringAngleDeg) < 90 and CS.vEgo > 5: + torque = CS.steeringTorqueEps / self.CarControllerParams.STEER_MAX + predicted_kf = torque / (CS.steeringAngleDeg * CS.vEgo ** 2) + self.pid.k_f = self.kf_filter.update(predicted_kf) + + print('PREDICTED KF: {}'.format(self.kf_filter.x)) else: steers_max = get_steer_max(CP, CS.vEgo) self.pid.pos_limit = steers_max diff --git a/selfdrive/controls/lib/lead_mpc.py b/selfdrive/controls/lib/lead_mpc.py index d94966c5184dc3..d9f38076660bd1 100644 --- a/selfdrive/controls/lib/lead_mpc.py +++ b/selfdrive/controls/lib/lead_mpc.py @@ -49,7 +49,7 @@ def set_cur_state(self, v, a): self.cur_state[0].v_ego = v_safe self.cur_state[0].a_ego = a_safe - def update(self, CS, radarstate, v_cruise): + def update(self, CS, radarstate, modelstate, v_cruise): v_ego = CS.vEgo if self.lead_id == 0: lead = radarstate.leadOne diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py index 4d6f3846d05271..29e4c21f251d85 100644 --- a/selfdrive/controls/lib/long_mpc.py +++ b/selfdrive/controls/lib/long_mpc.py @@ -9,18 +9,23 @@ class LongitudinalMpc(): - def __init__(self): + def __init__(self, mpc_id): + self.mpc_id = mpc_id + self.reset_mpc() self.last_cloudlog_t = 0.0 self.ts = list(range(10)) self.status = True - self.min_a = -1.2 + self.min_a = -1.2 if self.mpc_id == 0 else -3.5 self.max_a = 1.2 def reset_mpc(self): self.libmpc = libmpc_py.libmpc - self.libmpc.init(0.0, 1.0, 0.0, 50.0, 10000.0) + if self.mpc_id == 0: + self.libmpc.init(0.0, 1.0, 0.0, 50.0, 10000.0) + else: + self.libmpc.init(1.0, 1.0, 0.0, 5.0, 10000.0) self.mpc_solution = libmpc_py.ffi.new("log_t *") self.cur_state = libmpc_py.ffi.new("state_t *") @@ -45,10 +50,14 @@ def set_cur_state(self, v, a): self.cur_state[0].v_ego = v_safe self.cur_state[0].a_ego = a_safe - def update(self, carstate, radarstate, v_cruise): + def update(self, carstate, radarstate, modelstate, v_cruise): v_cruise_clipped = np.clip(v_cruise, self.cur_state[0].v_ego - 10., self.cur_state[0].v_ego + 10.0) - poss = v_cruise_clipped * np.array(T_IDXS[:LON_MPC_N+1]) - speeds = v_cruise_clipped * np.ones(LON_MPC_N+1) + if self.mpc_id == 0: + poss = v_cruise_clipped * np.array(T_IDXS[:LON_MPC_N+1]) + speeds = v_cruise_clipped * np.ones(LON_MPC_N+1) + else: + poss = np.minimum(np.array(modelstate.position.x)[:LON_MPC_N+1], v_cruise_clipped * np.array(T_IDXS[:LON_MPC_N+1])) + speeds = np.minimum(np.array(modelstate.velocity.x)[:LON_MPC_N+1], v_cruise_clipped) accels = np.zeros(LON_MPC_N+1) self.update_with_xva(poss, speeds, accels) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index a4c10d6db84f7e..a9daf18698b07a 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -15,8 +15,6 @@ from selfdrive.controls.lib.long_mpc import LongitudinalMpc from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, CONTROL_N from selfdrive.swaglog import cloudlog -# from selfdrive.controls.lib.long_mpc_model import LongitudinalMpcModel -# from common.op_params import opParams LON_MPC_STEP = 0.2 # first step is 0.2s AWARENESS_DECEL = -0.2 # car smoothly decel at .2m/s^2 when user is distracted @@ -46,44 +44,17 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP): return [a_target[0], min(a_target[1], a_x_allowed)] -class ModelMpcHelper: - def __init__(self): - self.model_t = [i ** 2 / 102.4 for i in range(33)] # the timesteps of the model predictions - self.mpc_t = list(range(10)) # the timesteps of what the LongMpcModel class takes in, 1 sec intervels to 10 - self.model_t_idx = [sorted(range(len(self.model_t)), key=[abs(idx - t) for t in self.model_t].__getitem__)[0] for idx in self.mpc_t] # matches 0 to 9 interval to idx from t - assert len(self.model_t_idx) == 10, 'Needs to be length 10 for mpc' - - def convert_data(self, sm): - modelV2 = sm['modelV2'] - distances, speeds, accelerations = [], [], [] - if not sm.alive['modelV2'] or len(modelV2.position.x) == 0: - return distances, speeds, accelerations - - speeds = [modelV2.velocity.x[t] for t in self.model_t_idx] - distances = [modelV2.position.x[t] for t in self.model_t_idx] - for t in self.mpc_t: # todo these three in one loop - if 0 < t < 9: - accelerations.append((speeds[t + 1] - speeds[t - 1]) / 2) - - # Extrapolate forward and backward at edges - accelerations.append(accelerations[-1] - (accelerations[-2] - accelerations[-1])) - accelerations.insert(0, accelerations[0] - (accelerations[1] - accelerations[0])) - return distances, speeds, accelerations - - class Planner(): def __init__(self, CP): self.CP = CP self.mpcs = {} self.mpcs['lead0'] = LeadMpc(0) self.mpcs['lead1'] = LeadMpc(1) - self.mpcs['cruise'] = LongitudinalMpc() - # self.mpcs['model'] = LongitudinalMpcModel() + self.mpcs['cruise'] = LongitudinalMpc(0) + self.mpcs['e2e'] = LongitudinalMpc(1) self.fcw = False self.fcw_checker = FCWChecker() - # self.op_params = opParams() - self.model_mpc_helper = ModelMpcHelper() self.v_desired = 0.0 self.a_desired = 0.0 @@ -134,9 +105,9 @@ def update(self, sm, CP): next_a = np.inf for key in self.mpcs: self.mpcs[key].set_cur_state(self.v_desired, self.a_desired) - self.mpcs[key].update(sm['carState'], sm['radarState'], v_cruise) - # TODO: handle model long enabled check - if self.mpcs[key].status and self.mpcs[key].a_solution[5] < next_a: # picks slowest solution from accel in ~0.2 seconds + self.mpcs[key].update(sm['carState'], sm['radarState'], sm['modelV2'], v_cruise) + if (self.mpcs[key].status and self.mpcs[key].a_solution[5] < next_a and # picks slowest solution from accel in ~0.2 seconds + ((key == 'e2e' and sm['modelLongButton'].enabled) or key != 'e2e')): self.longitudinalPlanSource = key self.v_desired_trajectory = self.mpcs[key].v_solution[:CONTROL_N] self.a_desired_trajectory = self.mpcs[key].a_solution[:CONTROL_N] diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 6d4ba43fe9fdc7..b28962da50cbe8 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -38,7 +38,6 @@ def plannerd_thread(sm=None, pm=None): if sm.updated['modelV2']: lateral_planner.update(sm, CP) lateral_planner.publish(sm, pm) - if sm.updated['radarState']: longitudinal_planner.update(sm, CP) longitudinal_planner.publish(sm, pm) diff --git a/selfdrive/controls/tests/test_following_distance.py b/selfdrive/controls/tests/test_following_distance.py index b27ca605458c28..7f70fd63012bcf 100644 --- a/selfdrive/controls/tests/test_following_distance.py +++ b/selfdrive/controls/tests/test_following_distance.py @@ -6,6 +6,8 @@ import cereal.messaging as messaging from selfdrive.config import Conversions as CV from selfdrive.controls.lib.lead_mpc import LeadMpc +from selfdrive.controls.lib.drive_helpers import LON_MPC_N +from selfdrive.modeld.constants import T_IDXS def RW(v_ego, v_l): @@ -39,7 +41,7 @@ def run_following_distance_simulation(v_lead, t_end=200.0): CS.carState.vEgo = v_ego CS.carState.aEgo = a_ego - # Setup model packet + # Setup radarstate packet radarstate = messaging.new_message('radarState') lead = log.RadarState.LeadData.new_message() lead.modelProb = .75 @@ -49,12 +51,21 @@ def run_following_distance_simulation(v_lead, t_end=200.0): lead.status = True radarstate.radarState.leadOne = lead + # Setup model packet + modelstate = messaging.new_message('modelV2') + positions = log.ModelDataV2.XYZTData.new_message() + velocities = log.ModelDataV2.XYZTData.new_message() + positions.x = (v_ego * np.array(T_IDXS[:LON_MPC_N+1])).tolist() + velocities.x = (v_ego * np.ones(LON_MPC_N+1)).tolist() + modelstate.modelV2.position = positions + modelstate.modelV2.velocity = velocities + # Run MPC mpc.set_cur_state(v_ego, a_ego) if first: # Make sure MPC is converged on first timestep for _ in range(20): - mpc.update(CS.carState, radarstate.radarState, 0) - mpc.update(CS.carState, radarstate.radarState, 0) + mpc.update(CS.carState, radarstate.radarState, modelstate.modelV2, 0) + mpc.update(CS.carState, radarstate.radarState, modelstate.modelV2, 0) # Choose slowest of two solutions v_ego, a_ego = mpc.mpc_solution.v_ego[5], mpc.mpc_solution.a_ego[5] diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py index a967a8ed88642c..ea2b693ecbec5e 100755 --- a/selfdrive/manager/manager.py +++ b/selfdrive/manager/manager.py @@ -150,7 +150,7 @@ def manager_thread(): running_list = ["%s%s\u001b[0m" % ("\u001b[32m" if p.proc.is_alive() else "\u001b[31m", p.name) for p in managed_processes.values() if p.proc] - cloudlog.debug(' '.join(running_list)) + # cloudlog.debug(' '.join(running_list)) # send managerState msg = messaging.new_message('managerState') diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 6846a4aa0fd3a1..c42e389115b24d 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -264,8 +264,8 @@ def ublox_rcv_callback(msg): ProcessConfig( proc_name="plannerd", pub_sub={ - "modelV2": ["lateralPlan"], "radarState": ["longitudinalPlan"], - "carState": [], "controlsState": [], 'modelLongButton': [], + "modelV2": ["lateralPlan", "longitudinalPlan"], + "carState": [], "controlsState": [], "radarState": [], }, ignore=["logMonoTime", "valid", "longitudinalPlan.processingDelay"], init_callback=get_car_params, diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index e92f2a1986e2c8..7e0ad555613e38 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -113,19 +113,19 @@ ButtonsWindow::ButtonsWindow(QWidget *parent) : QWidget(parent) { QWidget *btns_wrapper = new QWidget; QHBoxLayout *btns_layout = new QHBoxLayout(btns_wrapper); btns_layout->setSpacing(0); - btns_layout->setContentsMargins(0, 0, 30, 30); + btns_layout->setContentsMargins(30, 0, 30, 30); main_layout->addWidget(btns_wrapper, 0, Qt::AlignBottom); -// mlButton = new QPushButton("Toggle Model Long"); -// mlButton->setStyleSheet("font-size: 50px; border-radius: 25px; border-color: #b83737;"); -// QObject::connect(mlButton, &QPushButton::clicked, [=]() { -// mlButton->setStyleSheet("font-size: 50px; border-radius: 25px; border-color: #37b868;"); -// }); -// mlButton->setFixedWidth(525); -// mlButton->setFixedHeight(150); -// btns_layout->addStretch(); -// btns_layout->addWidget(mlButton, 0, Qt::AlignCenter); + mlButton = new QPushButton("Model Cruise Control"); + QObject::connect(mlButton, &QPushButton::clicked, [=]() { + QUIState::ui_state.scene.mlButtonEnabled = !mlEnabled; + }); + mlButton->setFixedWidth(575); + mlButton->setFixedHeight(150); + btns_layout->addStretch(4); + btns_layout->addWidget(mlButton, 0, Qt::AlignHCenter | Qt::AlignBottom); + btns_layout->addStretch(3); dfButton = new QPushButton("DF\nprofile"); QObject::connect(dfButton, &QPushButton::clicked, [=]() { @@ -133,7 +133,6 @@ ButtonsWindow::ButtonsWindow(QWidget *parent) : QWidget(parent) { }); dfButton->setFixedWidth(200); dfButton->setFixedHeight(200); -// btns_layout->addStretch(); btns_layout->addWidget(dfButton, 0, Qt::AlignRight); setStyleSheet(R"( @@ -149,13 +148,8 @@ ButtonsWindow::ButtonsWindow(QWidget *parent) : QWidget(parent) { } void ButtonsWindow::updateState(const UIState &s) { - updateDfButton(s.scene.dfButtonStatus); // update dynamic follow profile button -// updateMlButton(s.scene.dfButtonStatus); // update model longitudinal button // TODO: add model long back first -} - -void ButtonsWindow::updateDfButton(int status) { - if (dfStatus != status) { // updates (resets) on car start, or on button press - dfStatus = status; + if (dfStatus != s.scene.dfButtonStatus) { // update dynamic follow profile button + dfStatus = s.scene.dfButtonStatus; dfButton->setStyleSheet(QString("font-size: 45px; border-radius: 100px; border-color: %1").arg(dfButtonColors.at(dfStatus))); MessageBuilder msg; @@ -163,6 +157,16 @@ void ButtonsWindow::updateDfButton(int status) { dfButtonStatus.setStatus(dfStatus); QUIState::ui_state.pm->send("dynamicFollowButton", msg); } + + if (mlEnabled != s.scene.mlButtonEnabled) { // update model longitudinal button + mlEnabled = s.scene.mlButtonEnabled; + mlButton->setStyleSheet(QString("font-size: 50px; border-radius: 25px; border-color: %1").arg(mlButtonColors.at(mlEnabled))); + + MessageBuilder msg; + auto mlButtonEnabled = msg.initEvent().initModelLongButton(); + mlButtonEnabled.setEnabled(mlEnabled); + QUIState::ui_state.pm->send("modelLongButton", msg); + } } void OnroadAlerts::updateAlert(const Alert &a, const QColor &color) { diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index 17292bd40c2909..03d392a2882778 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -21,11 +21,15 @@ class ButtonsWindow : public QWidget { private: QPushButton *dfButton; -// QPushButton *mlButton; + QPushButton *mlButton; + // dynamic follow button int dfStatus = -1; // always initialize style sheet and send msg const QStringList dfButtonColors = {"#044389", "#24a8bc", "#fcff4b", "#37b868"}; - void updateDfButton(int status); + + // model long button + bool mlEnabled = true; // triggers initialization + const QStringList mlButtonColors = {"#b83737", "#37b868"}; public slots: void updateState(const UIState &s); diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 965e4aafab4f4b..7a7fa046010df6 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -102,7 +102,7 @@ typedef struct UIScene { int dfButtonStatus = 0; int lsButtonStatus = 0; - bool mlButtonEnabled; + bool mlButtonEnabled = false; mat3 view_from_calib; bool world_objects_visible;