Merge pull request #2 from arne182/release2

Alca 3.3 change button order

selfdrive/car/modules/ALCA_module.py

@@ -23,6 +23,8 @@
 THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+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
@@ -36,7 +38,9 @@
 # max REAL delta angle for correction vs actuator
 CL_MAX_ANGLE_DELTA = 1.2
 
-CL_LANE_DETECT_FACTOR = .75
+# 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]
 
 # change lane delta angles and other params
 CL_MAXD_BP = [10., 32., 44.]
@@ -243,16 +247,14 @@ def update_angle(self,enabled,CS,frame,actuators):
         # 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)
-        # 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
-        # wait 0.05 sec before starting to check if angle increases
-        if (current_delta > previous_delta) and (self.laneChange_counter > 5):
-          self.laneChange_enabled = 7
-          self.laneChange_counter = 1
-          self.laneChange_direction = 0
-        # wait 0.10 sec before looking if we are within 5 deg of actuator.angleSteer
-        if (current_delta <= 5.) and (self.laneChange_counter > 10):
+        if (self.laneChange_counter > 4):
+          # 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):
           self.laneChange_enabled = 7
           self.laneChange_counter = 1
           self.laneChange_direction = 0
@@ -280,11 +282,12 @@ def update_angle(self,enabled,CS,frame,actuators):
           self.keep_angle = False
         self.laneChange_counter += 1
         laneChange_angle = self.laneChange_angled
+        cl_lane_detect_factor = interp(CS.v_ego, CL_LANE_DETECT_BP, CL_LANE_DETECT_FACTOR)
         if (self.laneChange_over_the_line == 0):
           # we didn't cross the line, so keep computing the actuator delta until it flips
           actuator_delta = self.laneChange_direction * (-actuators.steerAngle - self.laneChange_last_actuator_angle)
           actuator_ratio = (-actuators.steerAngle)/self.laneChange_last_actuator_angle
-        if (actuator_ratio < 1) and (abs(actuator_delta) > 0.5 * CL_LANE_DETECT_FACTOR):
+        if (actuator_ratio < 1) and (abs(actuator_delta) > 0.5 * cl_lane_detect_factor):
           # sudden change in actuator angle or sign means we are on the other side of the line
           self.laneChange_over_the_line = 1
           self.laneChange_enabled = 2

selfdrive/car/toyota/carstate.py

@@ -16,12 +16,12 @@ def gps_distance(gpsLat, gpsLon, gpsAlt, gpsAcc):
   #x y z alt altacc includeradius approachradius speedlimit
   B = np.array([[-4190726.5,-723704.4,4744593.25,575.5,15.000001,22.000001,100.000001,25.000001],[-4182729.45,-730269.75,4750656.55,587.7,15.000001,22.000001,100.000001,25.000001]])
   dist = 999.000001
-  #lat=48.12893908
+  #lat=48.128939
   #lon=9.797879048
   #lonlatacc=1.00001
   #alt=575.5
   includeradius = 22.000001
-  approachradius = 100.000001
+  approachradius = 100.0001
   speedlimit = 25.000001
   minindex = np.argmin((np.sum((B[:,[0,1,2]] - A)**2,axis=1))**0.5, axis=0)
   altacc = B[minindex,4]
@@ -157,11 +157,11 @@ def __init__(self, CP):
  #BB init ui buttons
   def init_ui_buttons(self):
     btns = []
-    btns.append(UIButton("alca", "ALC", 0, "", 0))
-    btns.append(UIButton("tr", "TR", 0, "", 1))
-    btns.append(UIButton("lka", "LKA", 0, "", 2))
-    btns.append(UIButton("sound", "SND", 1, "", 3))
-    btns.append(UIButton("slow", "SLO", 0, "", 4))
+    btns.append(UIButton("sound", "SND", 1, "", 0))
+    btns.append(UIButton("alca", "ALC", 0, "", 1))
+    btns.append(UIButton("slow", "SLO", 0, "", 2))
+    btns.append(UIButton("lka", "LKA", 0, "", 3))
+    btns.append(UIButton("tr", "TR", 0, "", 4))
     btns.append(UIButton("gas", "GAS", 0, "", 5))
     return btns