From e7b06d99ffa54dac6c4f9fc95e31e011e90158f1 Mon Sep 17 00:00:00 2001
From: Erich Moraga <33645296+ErichMoraga@users.noreply.github.com>
Date: Fri, 7 Feb 2020 21:12:44 -0600
Subject: [PATCH] Basic curvature learner based on @zorrobyte's work

Works best when paired w/ `interface.py` changes and ZSS.  Without ZSS, `interface.py` changes will make it feel twitchy, so leaving that for the ZSS branches only.
---
 selfdrive/controls/lib/pathplanner.py | 26 +++++++++++++++++++++++++-
 1 file changed, 25 insertions(+), 1 deletion(-)

diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py
index de43c041805990..25634c27102d52 100644
--- a/selfdrive/controls/lib/pathplanner.py
+++ b/selfdrive/controls/lib/pathplanner.py
@@ -1,5 +1,7 @@
 import os
 import math
+import numpy as np
+from common.numpy_fast import clip
 from common.realtime import sec_since_boot, DT_MDL
 from selfdrive.swaglog import cloudlog
 from selfdrive.controls.lib.lateral_mpc import libmpc_py
@@ -8,6 +10,8 @@
 from selfdrive.config import Conversions as CV
 import cereal.messaging as messaging
 from cereal import log
+import os.path
+import json
 
 LaneChangeState = log.PathPlan.LaneChangeState
 LaneChangeDirection = log.PathPlan.LaneChangeDirection
@@ -53,6 +57,12 @@ def __init__(self, CP):
 
     self.setup_mpc()
     self.solution_invalid_cnt = 0
+    self.frame = 0
+    if os.path.exists('/data/curvature.json'):
+        with open("/data/curvature.json", "r") as f:
+            self.curvature_offset_i = json.load(f)	
+    else:
+        self.curvature_offset_i = 0.0
     self.lane_change_state = LaneChangeState.off
     self.lane_change_direction = LaneChangeDirection.none
     self.lane_change_timer = 0.0
@@ -84,7 +94,7 @@ def update(self, sm, pm, CP, VM):
     # Run MPC
     self.angle_steers_des_prev = self.angle_steers_des_mpc
     VM.update_params(sm['liveParameters'].stiffnessFactor, sm['liveParameters'].steerRatio)
-    curvature_factor = VM.curvature_factor(v_ego)
+    curvature_factor = VM.curvature_factor(v_ego) + self.curvature_offset_i
 
     self.LP.parse_model(sm['model'])
 
@@ -149,6 +159,20 @@ def update(self, sm, pm, CP, VM):
 
     self.LP.update_d_poly(v_ego)
 
+    if active and angle_steers - angle_offset > 0.5:
+      self.curvature_offset_i -= self.LP.d_poly[3] / 12000
+      #self.LP.d_poly[3] += self.curvature_offset_i
+    elif active and angle_steers - angle_offset < -0.5:
+      self.curvature_offset_i += self.LP.d_poly[3] / 12000
+
+    self.curvature_offset_i = clip(self.curvature_offset_i, -0.3, 0.3)
+    self.frame += 1
+    if self.frame == 12000: #every 2 mins
+      with open("/data/curvature.json", "w") as f:
+          json.dump(self.curvature_offset_i, f)
+      os.chmod("/data/curvature.json", 0o777)
+      self.frame = 0
+
     # account for actuation delay
     self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers - angle_offset, curvature_factor, VM.sR, CP.steerActuatorDelay)