From 9c4b0fa4a0bdd9632619036bc14c72cb6c9a10dc Mon Sep 17 00:00:00 2001
From: tb205gti <tb@2200kbhn.dk>
Date: Tue, 5 Nov 2019 16:10:38 +0100
Subject: [PATCH] Saving P,I,F parameters from pid controller (#116)

Added separate save/load functions, try catch - and saving the right constants to the params file.
---
 selfdrive/car/tesla/PCC_module.py | 35 ++++++++++++++++++++++++++++++-
 1 file changed, 34 insertions(+), 1 deletion(-)

diff --git a/selfdrive/car/tesla/PCC_module.py b/selfdrive/car/tesla/PCC_module.py
index aec40a03f..573fb19cd 100644
--- a/selfdrive/car/tesla/PCC_module.py
+++ b/selfdrive/car/tesla/PCC_module.py
@@ -177,6 +177,31 @@ def __init__(self,carcontroller):
     self.params = Params()
     self.average_speed_over_x_suggestions = 3 #10x a second
     self.maxsuggestedspeed_avg = MovingAverage(self.average_speed_over_x_suggestions)
+    
+  def load_pid(self):
+    try:
+      v_pid_json = open(V_PID_FILE)
+      data = json.load(v_pid_json)
+      if (self.LoC):
+        self.LoC.pid.p = data['p']
+        self.LoC.pid.i = data['i']
+        self.LoC.pid.f = data['f']
+      else:
+        print("self.LoC not initialized!")
+    except IOError:
+      print("file not present, creating at next reset")
+
+    #Helper function for saving the PCC pid constants across drives
+  def save_pid(self, pid):
+    data = {}
+    data['p'] = pid.p
+    data['i'] = pid.i
+    data['f'] = pid.f
+    try:
+      with open(V_PID_FILE , 'w') as outfile :
+        json.dump(data, outfile)
+    except IOError:
+      print("PDD pid parameters could not be saved to file")
 
   def max_v_by_speed_limit(self,pedal_set_speed_ms ,speed_limit_ms, CS):
     # if more than 10 kph / 2.78 ms, consider we have speed limit
@@ -195,6 +220,9 @@ def max_v_by_speed_limit(self,pedal_set_speed_ms ,speed_limit_ms, CS):
       return pedal_set_speed_ms
 
   def reset(self, v_pid):
+    #save the pid parameters to params file
+    self.save_pid(self.LoC.pid)
+    
     if self.LoC and RESET_PID_ON_DISENGAGE:
       self.LoC.reset(v_pid)
 
@@ -378,7 +406,12 @@ def update_pdl(self, enabled, CS, frame, actuators, pcm_speed, speed_limit_ms, s
     # how much accel and break we have to do
     ####################################################################
     if PCCModes.is_selected(FollowMode(), CS.cstm_btns):
-      self.v_pid = self.calc_follow_speed_ms(CS,alca_enabled)
+      # Get v_id from the stored file, only the first activation where v_pid eq 0
+      if (not RESET_PID_ON_DISENGAGE and self.v_pid == 0.): 
+        self.load_pid()
+      else:
+        self.v_pid = self.calc_follow_speed_ms(CS,alca_enabled)
+      
       if mapd is not None:
         v_curve = max_v_in_mapped_curve_ms(mapd.liveMapData, self.pedal_speed_kph)
         if v_curve: