From 65adf79be645b00c0a78de6ede4e68ed4b775f6c Mon Sep 17 00:00:00 2001
From: dragonpilot <dgnpilot@gmail.com>
Date: Wed, 27 Nov 2019 10:09:41 +1000
Subject: [PATCH] add ability to disable steering ratio learner

---
 common/params.py                             |  1 +
 selfdrive/dragonpilot/dragonconf/__init__.py |  1 +
 selfdrive/locationd/params_learner.cc        | 21 +++++++++++++-------
 selfdrive/locationd/params_learner.h         |  2 +-
 selfdrive/locationd/paramsd.cc               | 14 ++++++++++++-
 5 files changed, 30 insertions(+), 9 deletions(-)

diff --git a/common/params.py b/common/params.py
index f054b1c6f..5905327cd 100755
--- a/common/params.py
+++ b/common/params.py
@@ -149,6 +149,7 @@ keys = {
   "DragonToyotaSnGMod": [TxType.PERSISTENT],
   "DragonIsEON": [TxType.PERSISTENT],
   "DragonHWChecked": [TxType.PERSISTENT],
+  "DragonEnableSRLearner": [TxType.PERSISTENT],
 }
 
 
diff --git a/selfdrive/dragonpilot/dragonconf/__init__.py b/selfdrive/dragonpilot/dragonconf/__init__.py
index 9e0325102..521ac1914 100644
--- a/selfdrive/dragonpilot/dragonconf/__init__.py
+++ b/selfdrive/dragonpilot/dragonconf/__init__.py
@@ -56,6 +56,7 @@ default_conf = {
   'DragonToyotaSnGMod': '0',
   'DragonIsEON': '1',
   'DragonHWChecked': '0',
+  'DragonEnableSRLearner': '1',
 }
 
 deprecated_conf = {
diff --git a/selfdrive/locationd/params_learner.cc b/selfdrive/locationd/params_learner.cc
index 66f378a1d..fbeaf9f25 100644
--- a/selfdrive/locationd/params_learner.cc
+++ b/selfdrive/locationd/params_learner.cc
@@ -44,7 +44,7 @@ ParamsLearner::ParamsLearner(cereal::CarParams::Reader car_params,
   alpha4 = 1.0 * learning_rate;
 }
 
-bool ParamsLearner::update(double psi, double u, double sa) {
+bool ParamsLearner::update(double psi, double u, double sa, bool enable_sr_learner) {
   if (u > 10.0 && fabs(sa) < (DEGREES_TO_RADIANS * 90.)) {
     double ao_diff = 2.0*cF0*cR0*l*u*x*(1.0*cF0*cR0*l*u*x*(ao - sa) + psi*sR*(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0)))/(pow(sR, 2)*pow(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0), 2));
     double new_ao = ao - alpha1 * ao_diff;
@@ -54,6 +54,9 @@ bool ParamsLearner::update(double psi, double u, double sa) {
 
     double new_x = x - alpha3 * (-2.0*cF0*cR0*l*m*pow(u, 3)*(slow_ao - sa)*(aF*cF0 - aR*cR0)*(1.0*cF0*cR0*l*u*x*(slow_ao - sa) + psi*sR*(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0)))/(pow(sR, 2)*pow(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0), 3)));
     double new_sR = sR - alpha4 * (-2.0*cF0*cR0*l*u*x*(slow_ao - sa)*(1.0*cF0*cR0*l*u*x*(slow_ao - sa) + psi*sR*(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0)))/(pow(sR, 3)*pow(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0), 2)));
+    if (!enable_sr_learner) {
+      new_sR = sR;
+    }
 
     ao = new_ao;
     slow_ao = new_slow_ao;
@@ -69,12 +72,16 @@ bool ParamsLearner::update(double psi, double u, double sa) {
   ao = clip(ao, -MAX_ANGLE_OFFSET, MAX_ANGLE_OFFSET);
   slow_ao = clip(slow_ao, -MAX_ANGLE_OFFSET, MAX_ANGLE_OFFSET);
   x = clip(x, MIN_STIFFNESS, MAX_STIFFNESS);
-  sR = clip(sR, min_sr, max_sr);
+  if (enable_sr_learner) {
+    sR = clip(sR, min_sr, max_sr);
 
-  bool valid = fabs(slow_ao) < MAX_ANGLE_OFFSET_TH;
-  valid = valid && sR > min_sr_th;
-  valid = valid && sR < max_sr_th;
-  return valid;
+    bool valid = fabs(slow_ao) < MAX_ANGLE_OFFSET_TH;
+    valid = valid && sR > min_sr_th;
+    valid = valid && sR < max_sr_th;
+    return valid;
+  } else {
+    return true;
+  }
 }
 
 
@@ -93,7 +100,7 @@ extern "C" {
 
   bool params_learner_update(void * params_learner, double psi, double u, double sa) {
     ParamsLearner * p = (ParamsLearner*) params_learner;
-    return p->update(psi, u, sa);
+    return p->update(psi, u, sa, true);
   }
 
   double params_learner_get_ao(void * params_learner){
diff --git a/selfdrive/locationd/params_learner.h b/selfdrive/locationd/params_learner.h
index 4d97551b3..ed3971683 100644
--- a/selfdrive/locationd/params_learner.h
+++ b/selfdrive/locationd/params_learner.h
@@ -31,5 +31,5 @@ public:
                 double steer_ratio,
                 double learning_rate);
 
-  bool update(double psi, double u, double sa);
+  bool update(double psi, double u, double sa, bool enable_sr_learner);
 };
diff --git a/selfdrive/locationd/paramsd.cc b/selfdrive/locationd/paramsd.cc
index a38fc403a..a759f5e8a 100644
--- a/selfdrive/locationd/paramsd.cc
+++ b/selfdrive/locationd/paramsd.cc
@@ -96,6 +96,18 @@ int main(int argc, char *argv[]) {
 
   // Main loop
   int save_counter = 0;
+
+  // dragonpilot
+  // Steer Ratio Learner
+  char* enable_sr_learner_val = NULL;
+  read_db_value(NULL, "DragonEnableSRLearner", &enable_sr_learner_val, NULL);
+  bool enable_sr_learner = true;
+  if (enable_sr_learner_val && strlen(enable_sr_learner_val) && enable_sr_learner_val[0] == '1') {
+    enable_sr_learner = true;
+  } else {
+    enable_sr_learner = false;
+  }
+
   while (true){
     for (auto s : poller->poll(-1)){
       Message * msg = s->receive();
@@ -120,7 +132,7 @@ int main(int argc, char *argv[]) {
         save_counter++;
 
         double yaw_rate = -localizer.x[0];
-        bool valid = learner.update(yaw_rate, localizer.car_speed, localizer.steering_angle);
+        bool valid = learner.update(yaw_rate, localizer.car_speed, localizer.steering_angle, enable_sr_learner);
 
         // TODO: Fix in replay
         double sensor_data_age = localizer.controls_state_time - localizer.sensor_data_time;