Merge pull request #5 from arne182/release2

Newest ALCA

selfdrive/car/honda/carstate.py

@@ -135,6 +135,42 @@ def get_can_parser(CP):
 
 class CarState(object):
   def __init__(self, CP):
+    #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"]

selfdrive/car/modules/ALCA_module.py

@@ -24,43 +24,69 @@
 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
+HISTORY
+-------
+v3.6 - moved parameters to carstate.py
+v3.5 - changing the start angle to keep turning until we reach MAX_ANGLE_DELTA
+v3.4 - read steerRatio from each car parameters file
 v3.3 - re-entry logic changed for smoothness
 v3.2 - angle adjustment to compesate for road curvature change
 v3.1 - new angle logic for a smoother re-entry
 v3.0 - better lane dettection logic
 v2.0 - detection of lane crossing 
 v1.0 - fixed angle move
-"""
 
-from common.numpy_fast import interp
-from selfdrive.controls.lib.pid import PIController
+
+
+########################################################
+The following parameters need to be added to carstate.py 
+and based on each model you want to define
+########################################################
 
 # max REAL delta angle for correction vs actuator
-CL_MAX_ANGLE_DELTA = 1.2
+CL_MAX_ANGLE_DELTA_BP = [10., 44.]
+CL_MAX_ANGLE_DELTA = [2., .8]
+
+# adjustment factor for merging steer angle to actuator; should be over 4; the higher the smoother
+CL_ADJUST_FACTOR_BP = [10., 44.]
+CL_ADJUST_FACTOR = [16. , 16.]
+ # reenrey angle when to let go
+CL_REENTRY_ANGLE_BP = [10., 44.]
+CL_REENTRY_ANGLE = [5. , 5.]
 
 # a jump in angle above the CL_LANE_DETECT_FACTOR means we crossed the line
 CL_LANE_DETECT_BP = [10., 44.]
-CL_LANE_DETECT_FACTOR = [1.3, .75]
+CL_LANE_DETECT_FACTOR = [1.5, .75]
+
+CL_LANE_PASS_BP = [10., 44.]
+CL_LANE_PASS_TIME = [100., 5.] 
 
 # change lane delta angles and other params
 CL_MAXD_BP = [10., 32., 44.]
-CL_MAXD_A = [.308, 0.084, 0.042] #delta angle based on speed; needs fine tune, based on Tesla steer ratio of 16.75
+CL_MAXD_A = [.358, 0.084, 0.042] #delta angle based on speed; needs fine tune, based on Tesla steer ratio of 16.75
+
 CL_MIN_V = 8.9 # do not turn if speed less than x m/2; 20 mph = 8.9 m/s
 CL_MAX_A = 10. # do not turn if actuator wants more than x deg for going straight; this should be interp based on speed
 
 # 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
-MAX_ACTUATOR_DELTA = 2.
-MIN_ACTUATOR_DELTA = 0. 
-CORRECTION_FACTOR = 1.
+MCL_MAX_ACTUATOR_DELTA = 2.
+CL_MIN_ACTUATOR_DELTA = 0. 
+CL_CORRECTION_FACTOR = 1.
 
 #duration after we cross the line until we release is a factor of speed
 CL_TIMEA_BP = [10., 32., 44.]
-CL_TIMEA_T = [0.9 ,0.35, 0.25]
+CL_TIMEA_T = [0.7 ,0.30, 0.20]
+
+"""
+from common.numpy_fast import interp
+from selfdrive.controls.lib.pid import PIController
+
 
 class ALCAController(object):
   def __init__(self,carcontroller,alcaEnabled,steerByAngle):
+    #import settings
     self.CC = carcontroller  # added to start, will see if we need it actually
     # variables for lane change
     self.alcaEnabled = alcaEnabled
@@ -119,13 +145,25 @@ def update_status(self,alcaEnabled):
     self.alcaEnabled = alcaEnabled
 
   def set_pid(self,CS):
+    self.laneChange_steerr = CS.CP.steerRatio
     self.pid = PIController((CS.CP.steerKpBP, CS.CP.steerKpV),
                             (CS.CP.steerKiBP, CS.CP.steerKiV),
                             k_f=CS.CP.steerKf, pos_limit=1.0)
 
   def update_angle(self,enabled,CS,frame,actuators):
+    # speed variable parameters
+    cl_max_angle_delta = interp(CS.v_ego,CS.CL_MAX_ANGLE_DELTA_BP,CS.CL_MAX_ANGLE_DELTA)
+    cl_maxd_a =  interp(CS.v_ego, CS.CL_MAXD_BP, CS.CL_MAXD_A)
+    cl_lane_pass_time = interp(CS.v_ego,CS.CL_LANE_PASS_BP,CS.CL_LANE_PASS_TIME)
+    cl_timea_t = interp(CS.v_ego,CS.CL_TIMEA_BP,CS.CL_TIMEA_T)
+    cl_lane_detect_factor = interp(CS.v_ego, CS.CL_LANE_DETECT_BP, CS.CL_LANE_DETECT_FACTOR)
+    cl_max_a = interp(CS.v_ego, CS.CL_MAX_A, CS.CL_MAX_A_BP)
+    cl_adjust_factor = interp(CS.v_ego, CS.CL_ADJUST_FACTOR_BP, CS.CL_ADJUST_FACTOR)
+    cl_reentry_angle = interp(CS.v_ego, CS.CL_REENTRY_ANGLE_BP, CS.CL_REENTRY_ANGLE)
+    cl_min_v = CS.CL_MIN_V
+    self.laneChange_wait = CS.CL_WAIT_BEFORE_START
+
     # Basic highway lane change logic
-    changing_lanes = CS.right_blinker_on or CS.left_blinker_on
     blindspot = CS.blind_spot_on
     #if changing_lanes:
       #if blindspot:
@@ -168,23 +206,23 @@ def update_angle(self,enabled,CS,frame,actuators):
 
 
     if (CS.cstm_btns.get_button_status("alca") > 0) and self.alcaEnabled and (self.laneChange_enabled == 1):
-      if ((CS.v_ego < CL_MIN_V) or (abs(actuators.steerAngle) >= CL_MAX_A) or \
-      (abs(CS.angle_steers)>= CL_MAX_A)  or (not enabled)): 
+      if ((CS.v_ego < cl_min_v) or (abs(actuators.steerAngle) >= cl_max_a) or \
+      (abs(CS.angle_steers)>= cl_max_a)  or (not enabled)): 
         CS.cstm_btns.set_button_status("alca",9)
       else:
         CS.cstm_btns.set_button_status("alca",1)
 
     if self.alcaEnabled and enabled and (((not self.prev_right_blinker_on) and CS.right_blinker_on) or \
       ((not self.prev_left_blinker_on) and CS.left_blinker_on)) and \
-      ((CS.v_ego < CL_MIN_V) or (abs(actuators.steerAngle) >= CL_MAX_A) or (abs(CS.angle_steers) >=CL_MAX_A)):
+      ((CS.v_ego < cl_min_v) or (abs(actuators.steerAngle) >= cl_max_a) or (abs(CS.angle_steers) >=cl_max_a)):
       # something is not right, the speed or angle is limitting
       CS.UE.custom_alert_message(3,"Auto Lane Change Unavailable!",500,3)
       CS.cstm_btns.set_button_status("alca",9)
 
 
     if self.alcaEnabled and enabled and (((not self.prev_right_blinker_on) and CS.right_blinker_on) or \
       ((not self.prev_left_blinker_on) and CS.left_blinker_on))  and \
-      (CS.v_ego >= CL_MIN_V) and (abs(actuators.steerAngle) < CL_MAX_A):
+      (CS.v_ego >= cl_min_v) and (abs(actuators.steerAngle) < cl_max_a):
       # start blinker, speed and angle is within limits, let's go
       laneChange_direction = 1
       # changing lanes
@@ -206,7 +244,7 @@ def update_angle(self,enabled,CS,frame,actuators):
         self.laneChange_direction = laneChange_direction
         self.laneChange_avg_angle = 0.
         self.laneChange_avg_count = 0.
-        self.laneChange_angled = self.laneChange_direction * self.laneChange_steerr *  interp(CS.v_ego, CL_MAXD_BP, CL_MAXD_A)
+        self.laneChange_angled = self.laneChange_direction * self.laneChange_steerr * cl_maxd_a
         self.laneChange_last_actuator_angle = 0.
         self.laneChange_last_actuator_delta = 0.
         self.laneChange_over_the_line = 0 
@@ -221,7 +259,7 @@ def update_angle(self,enabled,CS,frame,actuators):
 
     # lane change in process
     if self.laneChange_enabled > 1:
-      if (CS.steer_override or (CS.v_ego < CL_MIN_V) or (not changing_lanes)):
+      if (CS.steer_override or (CS.v_ego < cl_min_v) or (not changing_lanes)):
         CS.UE.custom_alert_message(4,"Auto Lane Change Canceled! (u)",200,3)
         # if any steer override cancel process or if speed less than min speed
         self.laneChange_counter = 0
@@ -241,25 +279,24 @@ def update_angle(self,enabled,CS,frame,actuators):
       #     3. we check to see if the delta between our steering and the actuator is less than 5 deg
       #     4. we disengage after 0.5s and let OP take over
       if self.laneChange_enabled ==2:
+        laneChange_angle = self.laneChange_angled
         if self.laneChange_counter ==1:
           CS.UE.custom_alert_message(2,"Auto Lane Change Engaged! (5)",800)
         self.laneChange_counter += 1
         # check if angle continues to decrease
         current_delta = abs(self.laneChange_angle + laneChange_angle + (-actuators.steerAngle))
         previous_delta = abs(self.laneChange_last_sent_angle  - self.laneChange_last_actuator_angle)
-        if (self.laneChange_counter > 4):
+        if (self.laneChange_counter > cl_lane_pass_time):
           # continue to half the angle between our angle and actuator
-          laneChange_angle = (-actuators.steerAngle - self.laneChange_angle)/2 #self.laneChange_angled
-          self.laneChange_angle += laneChange_angle
-        else:
-          laneChange_angle = self.laneChange_angled
-        # wait 0.05 sec before starting to check if angle increases or if we are within 5 deg of actuator.angleSteer
-        if ((current_delta > previous_delta) or  (current_delta <= 5.)) and (self.laneChange_counter > 5):
+          delta_angle = (-actuators.steerAngle - self.laneChange_angle - self.laneChange_angled)/cl_adjust_factor 
+          self.laneChange_angle += delta_angle
+        # wait 0.05 sec before starting to check if angle increases or if we are within X deg of actuator.angleSteer
+        if ((current_delta > previous_delta) or  (current_delta <= cl_reentry_angle)) and (self.laneChange_counter > cl_lane_pass_time):
           self.laneChange_enabled = 7
           self.laneChange_counter = 1
           self.laneChange_direction = 0
         # we crossed the line, so  x sec later give control back to OP
-        laneChange_after_lane_duration = interp(CS.v_ego,CL_TIMEA_BP,CL_TIMEA_T) * self.laneChange_after_lane_duration_mult
+        laneChange_after_lane_duration = cl_timea_t * self.laneChange_after_lane_duration_mult
         if (self.laneChange_counter > laneChange_after_lane_duration * 100):
           self.laneChange_enabled = 7
           self.laneChange_counter = 1
@@ -294,14 +331,26 @@ def update_angle(self,enabled,CS,frame,actuators):
           self.laneChange_counter = 1
 
           # didn't change the lane yet, check that we are not eversteering or understeering based on road curvature
-
+
+         """
+         # code for v3.1 designed to turn more if lane moves away
+         n,a = self.last10delta_add(actuator_delta)
+        c = 5.
+        if a == 0:
+          a = 0.00001
+        if (abs(a) < CL_MIN_ACTUATOR_DELTA) and (self.keep_angle == False):
+          c = (a/abs(a)) * (CL_MIN_ACTUATOR_DELTA - abs(a)) / 10
+          self.laneChange_angle = self.laneChange_angle -self.laneChange_direction * CL_CORRECTION_FACTOR * c * 10 
+          self.last10delta_correct(c)
+        #print a, c, actuator_delta, self.laneChange_angle
+        """
           #print a, c, actuator_delta, self.laneChange_angle
         # code for v3.2 and above
         a_delta = self.laneChange_direction * (self.laneChange_angle + laneChange_angle - (-actuators.steerAngle))
-        if (self.laneChange_over_the_line == 0) and ((abs(a_delta) > CL_MAX_ANGLE_DELTA * self.laneChange_steerr) or (self.keep_angle)):
+        if (self.laneChange_over_the_line == 0) and ((abs(a_delta) > cl_max_angle_delta * self.laneChange_steerr) or (self.keep_angle)):
           #steering more than what we wanted, need to adjust
           self.keep_angle = True
-          self.laneChange_angle  = -actuators.steerAngle + self.laneChange_direction * CL_MAX_ANGLE_DELTA * self.laneChange_steerr - laneChange_angle
+          self.laneChange_angle  = -actuators.steerAngle + self.laneChange_direction * cl_max_angle_delta * self.laneChange_steerr - laneChange_angle
         if self.laneChange_counter >  (self.laneChange_duration) * 100:
           self.laneChange_enabled = 1
           self.laneChange_counter = 0
@@ -320,21 +369,28 @@ def update_angle(self,enabled,CS,frame,actuators):
         if self.laneChange_counter == 1:
           self.laneChange_angle = -actuators.steerAngle
           CS.UE.custom_alert_message(2,"Auto Lane Change Engaged! (3)",100)
-          self.laneChange_angled = self.laneChange_direction * self.laneChange_steerr *  interp(CS.v_ego, CL_MAXD_BP, CL_MAXD_A)
+          self.laneChange_angled = self.laneChange_direction * self.laneChange_steerr * cl_maxd_a
           # if angle more than max angle allowed cancel; last chance to cancel on road curvature
-          if (abs(self.laneChange_angle) > CL_MAX_A):
+        if self.laneChange_counter == 2:
+          CS.UE.custom_alert_message(2,"Auto Lane Change     Engaged! (3)",100)
+        if (abs(self.laneChange_angle) > cl_max_a) and (self.laneChange_counter == 1):
             CS.UE.custom_alert_message(4,"Auto Lane Change Canceled! (a)",200,5)
             self.laneChange_enabled = 1
             self.laneChange_counter = 0
             self.laneChange_direction = 0
             CS.cstm_btns.set_button_status("alca",1)
         laneChange_angle = self.laneChange_strStartFactor * self.laneChange_angled *  self.laneChange_counter / 50
         self.laneChange_counter += 1
-        delta_change = abs(self.laneChange_angle+ laneChange_angle + actuators.steerAngle) - self.laneChange_strStartMultiplier * abs(self.laneChange_angled)
+        #laneChange_angle = self.laneChange_strStartFactor * self.laneChange_angled *  self.laneChange_counter / 50
+        #delta_change = abs(self.laneChange_angle+ laneChange_angle + actuators.steerAngle) - self.laneChange_strStartMultiplier * abs(self.laneChange_angled)
+        laneChange_angle = self.laneChange_strStartFactor * self.laneChange_angled *  self.laneChange_counter / 100
+        delta_change = abs(self.laneChange_angle + laneChange_angle + actuators.steerAngle) - cl_max_angle_delta * self.laneChange_steerr
         if (self.laneChange_counter == 100) or (delta_change >= 0):
           if (delta_change < 0):
             # didn't achieve desired angle yet, so repeat
-            self.laneChange_counter = 1
+            self.laneChange_counter = 2
+            self.laneChange_angle += laneChange_angle
+            laneChange_angle = 0.
           else:
             self.laneChange_enabled = 3
             self.laneChange_counter = 1

selfdrive/car/modules/UIBT_module.py

@@ -29,16 +29,26 @@ 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
+            #we have all the da;ta and it matches
+            if 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")
+                    self.btns[j].btn_label2 = label2.rstrip("\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,9 +88,15 @@ 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.btns = self.CS.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()

selfdrive/car/modules/snd/playsound.py

@@ -24,4 +24,4 @@
   env = dict(os.environ)
   env['LD_LIBRARY_PATH'] = base_fld_code + "."
   args = [base_fld_code + "mediaplayer", snd_command]
-subprocess.Popen(args, shell = False, stdin=None, stdout=None, stderr=None, env = env, close_fds=True)
+  subprocess.Popen(args, shell = False, stdin=None, stdout=None, stderr=None, env = env, close_fds=True)