Merge latest changes from BogGyver for ALCA

selfdrive/car/honda/carstate.py

@@ -135,6 +135,61 @@ def get_can_parser(CP):
 
 class CarState(object):
   def __init__(self, CP):
+    #labels for buttons
+    self.btns_init = [["alca","ALC",["MadMax","Normal","Wifey"]], \
+                      ["","",[""]], \
+                      ["","",[""]], \
+                      ["sound","SND",[""]], \
+                      ["", "",[""]], \
+                      ["", "", [""]]]
+
+    #if (CP.carFingerprint == CAR.MODELS):
+    # ALCA PARAMS
+    # max REAL delta angle for correction vs actuator
+    self.CL_MAX_ANGLE_DELTA_BP = [10., 44.]
+    self.CL_MAX_ANGLE_DELTA = [1.8, .3]
+
+    # adjustment factor for merging steer angle to actuator; should be over 4; the higher the smoother
+    self.CL_ADJUST_FACTOR_BP = [10., 44.]
+    self.CL_ADJUST_FACTOR = [16. , 8.]
+
+
+    # reenrey angle when to let go
+    self.CL_REENTRY_ANGLE_BP = [10., 44.]
+    self.CL_REENTRY_ANGLE = [5. , 5.]
+
+    # a jump in angle above the CL_LANE_DETECT_FACTOR means we crossed the line
+    self.CL_LANE_DETECT_BP = [10., 44.]
+    self.CL_LANE_DETECT_FACTOR = [1.5, 1.5]
+
+    self.CL_LANE_PASS_BP = [10., 20., 44.]
+    self.CL_LANE_PASS_TIME = [40.,10., 3.] 
+
+    # change lane delta angles and other params
+    self.CL_MAXD_BP = [10., 32., 44.]
+    self.CL_MAXD_A = [.358, 0.084, 0.042] #delta angle based on speed; needs fine tune, based on Tesla steer ratio of 16.75
+
+    self.CL_MIN_V = 8.9 # do not turn if speed less than x m/2; 20 mph = 8.9 m/s
+
+    # do not turn if actuator wants more than x deg for going straight; this should be interp based on speed
+    self.CL_MAX_A_BP = [10., 44.]
+    self.CL_MAX_A = [10., 10.] 
+
+    # define limits for angle change every 0.1 s
+    # we need to force correction above 10 deg but less than 20
+    # anything more means we are going to steep or not enough in a turn
+    self.CL_MAX_ACTUATOR_DELTA = 2.
+    self.CL_MIN_ACTUATOR_DELTA = 0. 
+    self.CL_CORRECTION_FACTOR = 1.
+
+    #duration after we cross the line until we release is a factor of speed
+    self.CL_TIMEA_BP = [10., 32., 44.]
+    self.CL_TIMEA_T = [0.7 ,0.30, 0.20]
+
+    #duration to wait (in seconds) with blinkers on before starting to turn
+    self.CL_WAIT_BEFORE_START = 1
+    #END OF ALCA PARAMS
+
     self.CP = CP
     self.can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
     self.shifter_values = self.can_define.dv["GEARBOX"]["GEAR_SHIFTER"]
@@ -176,23 +231,6 @@ def __init__(self, CP):
                          K=[[0.12287673], [0.29666309]])
     self.v_ego = 0.0
 
-  #BB init ui buttons
-  def init_ui_buttons(self):
-    btns = []
-    btns.append(UIButton("alca", "ALC", 0, "", 0))
-    btns.append(UIButton("", "", 0, "", 1))
-    btns.append(UIButton("", "", 0, "", 2))
-    btns.append(UIButton("sound", "SND", 1, "", 3))
-    btns.append(UIButton("", "", 0, "", 4))
-    btns.append(UIButton("", "", 0, "", 5))
-    return btns
-
-  #BB update ui buttons
-  def update_ui_buttons(self,id,btn_status):
-    if self.cstm_btns.btns[id].btn_status > 0:
-        self.cstm_btns.btns[id].btn_status = btn_status * self.cstm_btns.btns[id].btn_status
-    else:
-        self.cstm_btns.btns[id].btn_status = btn_status
 
   def update(self, cp):
 

selfdrive/car/modules/UIBT_module.py

@@ -29,16 +29,33 @@ def read_buttons_labels_from_file(self):
         fi =  open(self.buttons_labels_path, buttons_file_r)
         indata = fi.read()
         fi.close()
+        file_matches = True
         if len(indata) == btn_msg_len * 6 :
-            #we have all the data
-            self.btns = []
+            #check if it matches the current setup
             for i in range(0, len(indata), btn_msg_len):
-                name,label,label2 = struct.unpack(btn_msg_struct, indata[i:i+btn_msg_len])  
-                self.btns.append(UIButton(name.rstrip("\0"),label.rstrip("\0"),0,label2.rstrip("\0"),0))
-            #now read the last saved statuses
+                j = int(i/btn_msg_len)
+                name,label,label2 = struct.unpack(btn_msg_struct, indata[i:i+btn_msg_len]) 
+                if (self.btns[j].btn_name != name.rstrip("\0")):
+                    file_matches = False
+                    print "Btn file does not match"
+            #we have all the data and it matches
+            if file_matches:
+                print "Btn file matches"
+                for i in range(0, len(indata), btn_msg_len):
+                    j = int(i/btn_msg_len)
+                    name,label,label2 = struct.unpack(btn_msg_struct, indata[i:i+btn_msg_len]) 
+                    self.btns[j].btn_label = label.rstrip("\0")
+                    #check if label is actually a valid option
+                    if label2.rstrip("\0") in self.CS.btns_init[j][2]:
+                        self.btns[j].btn_label2 = label2.rstrip("\0")
+                        print "Set label2 from file"
+                    else:
+                        self.btns[j].btn_label2 = self.CS.btns_init[j][2][0]
+            return file_matches
         else:
             #we don't have all the data, ignore
             print "labels file is bad"
+            return False
 
 
     def write_buttons_out_file(self):
@@ -78,12 +95,18 @@ def __init__(self, carstate,car,folder):
         self.hasChanges = True
         self.last_in_read_time = datetime.min 
         if os.path.exists(self.buttons_labels_path):
+            self.init_ui_buttons()
             #there is a file, load it
-            self.read_buttons_labels_from_file()
-            self.read_buttons_out_file()
+            if self.read_buttons_labels_from_file():
+                self.read_buttons_out_file()
+            else:
+                #no match, so write the new ones
+                self.hasChanges = True
+                self.write_buttons_labels_to_file()
+                self.write_buttons_out_file()
         else:
             #there is no file, create it
-            self.btns = self.CS.init_ui_buttons()
+            self.init_ui_buttons()
             self.hasChanges = True
             self.write_buttons_labels_to_file()
             self.write_buttons_out_file()
@@ -92,6 +115,17 @@ def __init__(self, carstate,car,folder):
         self.send_button_info()
         self.CS.UE.uiSetCarEvent(self.car_folder,self.car_name)
 
+    def init_ui_buttons(self):
+        self.btns = []
+        try:
+            self.CS.init_ui_buttons()
+            print "Buttons iniatlized with custom CS code"  
+        except AttributeError:
+            # no init method
+            print "Buttons iniatlized with just base code"
+        for i in range(0,len(self.CS.btns_init)):
+            self.btns.append(UIButton(self.CS.btns_init[i][0],self.CS.btns_init[i][1],1,self.CS.btns_init[i][2][0],i))
+
     def get_button(self, btn_name):
         for button in self.btns:
             if button.btn_name.strip() == btn_name:
@@ -119,15 +153,18 @@ def set_button_status(self,btn_name,btn_status):
 
     def set_button_status_from_ui(self,id,btn_status):
         old_btn_status = self.btns[id].btn_status
+        old_btn_label2 = self.btns[id].btn_label2
         if old_btn_status * btn_status == 0 and old_btn_status != btn_status:
             self.hasChanges = True
-        self.CS.update_ui_buttons(id,btn_status)
+        self.update_ui_buttons(id,btn_status)
         new_btn_status = self.btns[id].btn_status
         if new_btn_status * btn_status == 0 and new_btn_status != btn_status:
             self.hasChanges = True
         if self.hasChanges:
             self.CS.UE.uiButtonInfoEvent(id,self.btns[id].btn_name, \
                     self.btns[id].btn_label,self.btns[id].btn_status,self.btns[id].btn_label2)
+            if old_btn_label2 != self.btns[id].btn_label2:
+                self.write_buttons_labels_to_file()
             self.write_buttons_out_file()
 
 
@@ -137,3 +174,30 @@ def get_button_label2(self, btn_name):
             return btn.btn_label2
         else:
             return -1    
+
+    def get_button_label2_index(self, btn_name):
+        btn = self.get_button(btn_name)
+        if btn:
+            b_index = -1
+            bpos = self.btns.index(btn)
+            for i in range(0,len(self.CS.btns_init[bpos][2])):
+                if btn.btn_label2 == self.CS.btns_init[bpos][2][i]:
+                    b_index = i
+            return b_index
+        else:
+            return -1
+
+    def update_ui_buttons(self,id,btn_status):
+        if self.CS.cstm_btns.btns[id].btn_status > 0:
+        #if we have more than one Label2 then we toggle
+            if (len(self.CS.btns_init[id][2]) > 1):
+                status2 = 0
+                for i in range(0,len(self.CS.btns_init[id][2])):
+                    if self.CS.cstm_btns.btns[id].btn_label2 == self.CS.btns_init[id][2][i]:
+                        status2 = (i + 1 ) % len(self.CS.btns_init[id][2])
+                self.CS.cstm_btns.btns[id].btn_label2 = self.CS.btns_init[id][2][status2]
+                self.CS.cstm_btns.hasChanges = True
+            else:
+                self.CS.cstm_btns.btns[id].btn_status = btn_status * self.CS.cstm_btns.btns[id].btn_status
+        else:
+            self.CS.cstm_btns.btns[id].btn_status = btn_status

selfdrive/car/tesla/ACC_module.py

@@ -177,7 +177,7 @@ def update_acc(self, enabled, CS, frame, actuators, pcm_speed):
           elif speed_offset > half_press_kph and half_press_kph < available_speed:
             # Send cruise stalk up_1st.
             button_to_press = CruiseButtons.RES_ACCEL
-      if CS.cstm_btns.get_button_label2("acc") == "Mod JJ":
+      if CS.cstm_btns.get_button_label2_index("acc") == 1:
         # Alternative speed decision logic that uses the lead car's distance
         # and speed more directly.
         # Bring in the lead car distance from the Live20 feed