diff --git a/common/params_keys.h b/common/params_keys.h index 35c9b3568..8d32689f2 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -129,8 +129,10 @@ inline static std::unordered_map keys = { {"Version", {PERSISTENT, STRING}}, // --- infiniteCable params --- // + {"AngleOffsetDegree", {PERSISTENT, FLOAT, "0.0"}}, {"DisableScreenTimer", {PERSISTENT, BOOL}}, {"DarkMode", {PERSISTENT, BOOL}}, + {"EnableAngleOffset", {PERSISTENT, BOOL}}, {"EnableCurvatureController", {PERSISTENT, BOOL, "1"}}, {"EnableScreenEvent", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, BOOL}}, {"EnableSmoothSteer", {PERSISTENT, BOOL}}, diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index b4084fe5b..e6e315fae 100755 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -19,6 +19,7 @@ ROLL_LOWERED_MAX = np.radians(8) ROLL_STD_MAX = np.radians(1.5) LATERAL_ACC_SENSOR_THRESHOLD = 4.0 OFFSET_MAX = 10.0 +OFFSET_HIGHER_MAX = 18.0 OFFSET_LOWERED_MAX = 8.0 MIN_ACTIVE_SPEED = 1.0 LOW_ACTIVE_SPEED = 10.0 @@ -26,12 +27,16 @@ LOW_ACTIVE_SPEED = 10.0 class VehicleParamsLearner: def __init__(self, CP: car.CarParams, steer_ratio: float, stiffness_factor: float, angle_offset: float, P_initial: np.ndarray | None = None): + params = Params() self.kf = CarKalman(GENERATED_DIR) self.x_initial = CarKalman.initial_x.copy() self.x_initial[States.STEER_RATIO] = steer_ratio self.x_initial[States.STIFFNESS] = stiffness_factor - self.x_initial[States.ANGLE_OFFSET] = angle_offset + set_manual_angle_offset = params.get_bool("EnableAngleOffset") + self.allow_higher_angle_offset = set_manual_angle_offset + manual_angle_offset_deg = float(params.get("AngleOffsetDegree") or 0.0) + self.x_initial[States.ANGLE_OFFSET] = np.radians(manual_angle_offset_deg) if set_manual_angle_offset else angle_offset self.P_initial = P_initial if P_initial is not None else CarKalman.P_initial self.kf.set_globals( @@ -150,8 +155,9 @@ class VehicleParamsLearner: sensors_valid = bool(abs(self.observed_speed * (x[States.YAW_RATE].item() + self.observed_yaw_rate)) < LATERAL_ACC_SENSOR_THRESHOLD) else: sensors_valid = True - self.avg_offset_valid = check_valid_with_hysteresis(self.avg_offset_valid, self.avg_angle_offset, OFFSET_MAX, OFFSET_LOWERED_MAX) - self.total_offset_valid = check_valid_with_hysteresis(self.total_offset_valid, self.angle_offset, OFFSET_MAX, OFFSET_LOWERED_MAX) + avg_angle_offset_max = OFFSET_HIGHER_MAX if self.allow_higher_angle_offset else OFFSET_MAX + self.avg_offset_valid = check_valid_with_hysteresis(self.avg_offset_valid, self.avg_angle_offset, avg_angle_offset_max, OFFSET_LOWERED_MAX) + self.total_offset_valid = check_valid_with_hysteresis(self.total_offset_valid, self.angle_offset, avg_angle_offset_max, OFFSET_LOWERED_MAX) self.roll_valid = check_valid_with_hysteresis(self.roll_valid, self.roll, ROLL_MAX, ROLL_LOWERED_MAX) msg = messaging.new_message('liveParameters') @@ -290,6 +296,7 @@ def main(): msg_dat = msg.to_bytes() if sm.frame % 1200 == 0: # once a minute params.put_nonblocking("LiveParametersV2", msg_dat) + params.put_nonblocking("AngleOffsetDegree", msg.liveParameters.angleOffsetDeg) pm.send('liveParameters', msg_dat) diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index 9d58f57f9..7b36e1e0f 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -15,6 +15,7 @@ #include "selfdrive/ui/qt/widgets/scrollview.h" #include "selfdrive/ui/qt/offroad/developer_panel.h" #include "selfdrive/ui/qt/offroad/firehose.h" +#include "selfdrive/ui/sunnypilot/qt/widgets/controls.h" InfiniteCableTogglesPanel::InfiniteCableTogglesPanel(SettingsWindow *parent) : ListWidget(parent) { std::vector> toggle_defs{ @@ -74,6 +75,13 @@ InfiniteCableTogglesPanel::InfiniteCableTogglesPanel(SettingsWindow *parent) : L "../assets/icons/eye_closed.png", false, }, + { + "EnableAngleOffset", + tr("Enable Steer Angle Offset"), + tr("Enables the use of manual steer angle offset selection as start value for params detection."), + "../assets/icons/eye_closed.png", + false, + }, }; for (auto &[param, title, desc, icon, needs_restart] : toggle_defs) { @@ -97,6 +105,38 @@ InfiniteCableTogglesPanel::InfiniteCableTogglesPanel(SettingsWindow *parent) : L addItem(toggle); toggles[param.toStdString()] = toggle; } + + steer_offset_control = new OptionControlSP( + "AngleOffsetDegree", + tr("Steering Angle Offset"), + tr("Adjust steering angle offset params learner start value manually (in degrees). Taking effect only for onroad transition. Is refreshed with the actual learned value and saved."), + "", {-1800, 1800}, 10, false, nullptr, true, true); + + float stored_val = QString::fromStdString(params.get("AngleOffsetDegree")).toFloat(); + steer_offset_control->setLabel(QString::number(stored_val, 'f', 1) + "°"); + steer_offset_control->showDescription(); + addItem(steer_offset_control); + + bool enable_angle_offset = params.getBool("EnableAngleOffset"); + steer_offset_control->setVisible(enable_angle_offset); + + connect(uiStateSP(), &UIStateSP::uiUpdate, this, [=]() { + bool enable_steer_offset_control = params.getBool("EnableAngleOffset"); + steer_offset_control->setVisible(enable_steer_offset_control); + }); + + QObject::connect(steer_offset_control, &OptionControlSP::updateLabels, [=]() { + float val = QString::fromStdString(params.get("AngleOffsetDegree")).toFloat(); + steer_offset_control->setLabel(QString::number(val, 'f', 1) + "°"); + }); +} + +void InfiniteCableTogglesPanel::showEvent(QShowEvent *event) { + ListWidget::showEvent(event); + if (steer_offset_control) { + float val = QString::fromStdString(params.get("AngleOffsetDegree")).toFloat(); + steer_offset_control->setLabel(QString::number(val, 'f', 1) + "°"); + } } void InfiniteCableTogglesPanel::expandToggleDescription(const QString ¶m) { diff --git a/selfdrive/ui/qt/offroad/settings.h b/selfdrive/ui/qt/offroad/settings.h index 501a9732c..049e0d05b 100644 --- a/selfdrive/ui/qt/offroad/settings.h +++ b/selfdrive/ui/qt/offroad/settings.h @@ -81,8 +81,10 @@ public slots: void scrollToToggle(const QString ¶m); protected: + void showEvent(QShowEvent *event) override; Params params; std::map toggles; + OptionControlSP *steer_offset_control = nullptr; }; class TogglesPanel : public ListWidget {