Merge pull request #5 from infiniteCable2/lw_offset

Manual Angle Offset

common/params_keys.h

@@ -129,8 +129,10 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> 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}},

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)
 

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<std::tuple<QString, QString, QString, QString, bool>> 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 &param) {

selfdrive/ui/qt/offroad/settings.h

@@ -81,8 +81,10 @@ public slots:
   void scrollToToggle(const QString &param);
 
 protected:
+  void showEvent(QShowEvent *event) override;
   Params params;
   std::map<std::string, ParamControl*> toggles;
+  OptionControlSP *steer_offset_control = nullptr;
 };
 
 class TogglesPanel : public ListWidget {