066 clean

selfdrive/boardd/boardd.cc

@@ -57,6 +57,8 @@ bool loopback_can = false;
 cereal::HealthData::HwType hw_type = cereal::HealthData::HwType::UNKNOWN;
 bool is_pigeon = false;
 const uint32_t NO_IGNITION_CNT_MAX = 2 * 60 * 60 * 24 * 1;  // turn off charge after 1 days
+const uint32_t VBATT_START_CHARGING = 12000;
+const uint32_t VBATT_PAUSE_CHARGING = 10500;
 uint32_t no_ignition_cnt = 0;
 bool connected_once = false;
 bool ignition_last = false;
@@ -353,11 +355,20 @@ void can_health(PubSocket *publisher) {
   }
 
 #ifndef __x86_64__
-  if ((no_ignition_cnt > NO_IGNITION_CNT_MAX) && (health.usb_power_mode == (uint8_t)(cereal::HealthData::UsbPowerMode::CDP)) || ((health.voltage < 10500) && !ignition)){
-    printf("TURN OFF CHARGING!\n");
-    pthread_mutex_lock(&usb_lock);
-    libusb_control_transfer(dev_handle, 0xc0, 0xe6, (uint16_t)(cereal::HealthData::UsbPowerMode::CLIENT), 0, NULL, 0, TIMEOUT);
-    pthread_mutex_unlock(&usb_lock);
+  bool cdp_mode = health.usb_power_mode == (uint8_t)(cereal::HealthData::UsbPowerMode::CDP);
+  bool no_ignition_exp = no_ignition_cnt > NO_IGNITION_CNT_MAX;
+  if ((no_ignition_exp || (health.voltage <  VBATT_PAUSE_CHARGING)) && cdp_mode && !ignition) {
+     printf("TURN OFF CHARGING!\n");
+     pthread_mutex_lock(&usb_lock);
+     libusb_control_transfer(dev_handle, 0xc0, 0xe6, (uint16_t)(cereal::HealthData::UsbPowerMode::CLIENT), 0, NULL, 0, TIMEOUT);
+     pthread_mutex_unlock(&usb_lock);
+   }
+  if (!no_ignition_exp && (health.voltage >  VBATT_START_CHARGING) && !cdp_mode) {
+
+     printf("TURN ON CHARGING!\n");
+     pthread_mutex_lock(&usb_lock);
+     libusb_control_transfer(dev_handle, 0xc0, 0xe6, (uint16_t)(cereal::HealthData::UsbPowerMode::CDP), 0, NULL, 0, TIMEOUT);
+     pthread_mutex_unlock(&usb_lock);
   }
 #endif
 

selfdrive/car/toyota/interface.py

@@ -424,10 +424,11 @@ def update(self, c, can_strings):
       events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
 
     # disable on pedals rising edge or when brake is pressed and speed isn't zero
-    if ((ret.gasPressed and not self.gas_pressed_prev) or \
-       (ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001))) and disengage_event:
+    if (((ret.gasPressed and not self.gas_pressed_prev) or \
+       (ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001))) and disengage_event) or (ret.brakePressed and not self.brake_pressed_prev and ret.vEgo < 0.1):
       events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
-
+
+
     if ret.gasPressed and disengage_event:
       events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
 

selfdrive/car/toyota/values.py

@@ -197,6 +197,6 @@ class ECU:
 NO_STOP_TIMER_CAR = [CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.LEXUS_ESH_TSS2, CAR.SIENNA, CAR.RAV4H_TSS2]  # no resume button press required
 '''
 # todo: I'm using stock 066 below, above this is from 065-clean. Arne, check this out and delete the above if it looks good
-NO_DSU_CAR = [CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2]
-TSS2_CAR = [CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2]
-NO_STOP_TIMER_CAR = [CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.SIENNA]  # no resume button press required
+NO_DSU_CAR = [CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2]
+TSS2_CAR = [CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2]
+NO_STOP_TIMER_CAR = [CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.SIENNA, CAR.RAV4H_TSS2]  # no resume button press required

selfdrive/controls/lib/long_mpc.py

@@ -120,6 +120,7 @@ def dynamic_follow(self):  # in m/s
       x = [4.4704, 6.7056]  # smoothly ramp TR between 10 and 15 mph from 1.8s to defined TR above at 15mph
       y = [1.8, interp(x[1], x_vel, y_mod)]
       TR = interp(self.v_ego, x, y)
+      return round(TR, 3)
 
     if self.lead_data['v_lead'] is not None:  # if lead
       x = [-15.6464, -9.8422, -6.0, -4.0, -2.68, -2.3, -1.8, -1.26, -0.61, 0, 0.61, 1.26, 2.1, 2.68]  # relative velocity values

selfdrive/controls/lib/longcontrol.py

@@ -184,10 +184,10 @@ def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_
         self.pid.k_f=1.0
       if gas_pressed or brake_pressed:
         if not self.freeze:
-          self.pid.i = 0.0
           self.freeze = True
       else:
         if self.freeze:
+          self.pid.i = 0.0
           self.freeze = False
 
       output_gb = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, deadzone=deadzone, feedforward=a_target, freeze_integrator=(prevent_overshoot or gas_pressed or brake_pressed))

selfdrive/controls/lib/planner.py

@@ -117,18 +117,28 @@ def __init__(self, CP):
 
     self.params = Params()
 
-  def choose_solution(self, v_cruise_setpoint, enabled, yRel, dRel, steeringAngle):
+  def choose_solution(self, v_cruise_setpoint, enabled, lead_1, lead_2, steeringAngle):
+    center_x = -2.5 # Wheel base 2.5m
+    lead1_check = True
+    lead2_check = True
+    if steeringAngle > 100: # only at high angles
+      center_y = -1+2.5/math.tan(steeringAngle/1800.*math.pi) # Car Width 2m. Left side considered in left hand turn
+      lead1_check = math.sqrt((lead_1.dRel-center_x)**2+(lead_1.yRel-center_y)**2) < abs(2.5/math.sin(steeringAngle/1800.*math.pi))+1.
+      lead2_check = math.sqrt((lead_2.dRel-center_x)**2+(lead_2.yRel-center_y)**2) < abs(2.5/math.sin(steeringAngle/1800.*math.pi))+1.
+    elif steeringAngle < -100: # only at high angles
+      center_y = +1-2.5/math.tan(steeringAngle/1800.*math.pi) # Car Width 2m. Right side considered in right hand turn
+      lead1_check = math.sqrt((lead_1.dRel-center_x)**2+(lead_1.yRel-center_y)**2) < abs(2.5/math.sin(steeringAngle/1800.*math.pi))+1.
+      lead2_check = math.sqrt((lead_2.dRel-center_x)**2+(lead_2.yRel-center_y)**2) < abs(2.5/math.sin(steeringAngle/1800.*math.pi))+1.
     if enabled:
       solutions = {'model': self.v_model, 'cruise': self.v_cruise}
-      if self.mpc1.prev_lead_status and (abs(math.atan2(yRel,dRel)*180/math.pi - steeringAngle/10) < yRel + max(math.atan2(1,dRel)*180/math.pi, 5.0)):
+      if self.mpc1.prev_lead_status and lead1_check:
         solutions['mpc1'] = self.mpc1.v_mpc
-      if self.mpc2.prev_lead_status:
+      if self.mpc2.prev_lead_status and lead2_check:
         solutions['mpc2'] = self.mpc2.v_mpc
-
+        
       slowest = min(solutions, key=solutions.get)
 
       self.longitudinalPlanSource = slowest
-      # Choose lowest of MPC and cruise
       if slowest == 'mpc1':
         self.v_acc = self.mpc1.v_mpc
         self.a_acc = self.mpc1.a_mpc
@@ -142,7 +152,11 @@ def choose_solution(self, v_cruise_setpoint, enabled, yRel, dRel, steeringAngle)
         self.v_acc = self.v_model
         self.a_acc = self.a_model
 
-    self.v_acc_future = min([self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint])
+    self.v_acc_future = v_cruise_setpoint
+    if lead1_check:
+      self.v_acc_future = min([self.mpc1.v_mpc_future, self.v_acc_future])
+    if lead2_check:
+      self.v_acc_future = min([self.mpc2.v_mpc_future, self.v_acc_future])
 
   def update(self, sm, pm, CP, VM, PP):
     self.arne_sm.update(0)
@@ -177,7 +191,7 @@ def update(self, sm, pm, CP, VM, PP):
     lead_2 = sm['radarState'].leadTwo
 
     enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping)
-    following = lead_1.status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0
+    following = self.mpc1.prev_lead_status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0
 
     v_speedlimit = NO_CURVATURE_SPEED
     v_curvature_map = NO_CURVATURE_SPEED
@@ -302,7 +316,7 @@ def update(self, sm, pm, CP, VM, PP):
     self.mpc1.update(pm, sm['carState'], lead_1, v_cruise_setpoint)
     self.mpc2.update(pm, sm['carState'], lead_2, v_cruise_setpoint)
 
-    self.choose_solution(v_cruise_setpoint, enabled, lead_1.yRel, lead_1.dRel, sm['carState'].steeringAngle)
+    self.choose_solution(v_cruise_setpoint, enabled, lead_1, lead_2, sm['carState'].steeringAngle)
 
     # determine fcw
     if self.mpc1.new_lead:
@@ -340,10 +354,7 @@ def update(self, sm, pm, CP, VM, PP):
     plan_send.plan.aStart = float(self.a_acc_start)
     plan_send.plan.vTarget = float(self.v_acc)
     plan_send.plan.aTarget = float(self.a_acc)
-    if (lead_1.status and lead_1.dRel < 4.0) or (lead_2.status and lead_2.dRel) < 4.0:
-      plan_send.plan.vTargetFuture = float(0.0)
-    else:
-      plan_send.plan.vTargetFuture = float(self.v_acc_future)
+    plan_send.plan.vTargetFuture = float(self.v_acc_future)
     plan_send.plan.hasLead = self.mpc1.prev_lead_status
     plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource
 

selfdrive/manager.py

@@ -97,6 +97,7 @@ def unblock_stdout():
   "gpsd": ("selfdrive/sensord", ["./start_gpsd.py"]),
   "updated": "selfdrive.updated",
   "mapd": ("selfdrive/mapd", ["./mapd.py"]),
+  #"traffic": ("selfdrive/traffic", ["./traffic.py"]),
 }
 daemon_processes = {
   "manage_athenad": ("selfdrive.athena.manage_athenad", "AthenadPid"),
@@ -140,6 +141,7 @@ def get_running():
   'gpsd',
   'deleter',
   'mapd',
+  #'traffic',
 ]
 
 def register_managed_process(name, desc, car_started=False):

selfdrive/mapd/mapd.py

@@ -245,7 +245,7 @@ def mapsd_thread():
             circles = [circle_through_points(*p, direction=True) for p in zip(pnts, pnts[1:], pnts[2:])]
             circles = np.asarray(circles)
             radii = np.nan_to_num(circles[:, 2])
-            radii[radii < 15.] = 10000
+            radii[abs(radii) < 15.] = 10000
 
             if cur_way.way.tags['highway'] == 'trunk':
               radii = radii*1.6 # https://media.springernature.com/lw785/springer-static/image/chp%3A10.1007%2F978-3-658-01689-0_21/MediaObjects/298553_35_De_21_Fig65_HTML.gif

selfdrive/mapd/mapd_helpers.py

@@ -428,7 +428,7 @@ def max_speed_ahead(self, current_speed_limit, lat, lon, heading, lookahead):
               if backwards and (n.tags['traffic_signals:direction']=='backward' or n.tags['traffic_signals:direction']=='both'):
                 print("backward")
                 if way_pts[count, 0] > 0:
-                  speed_ahead_dist = max(0. , way_pts[count, 0] - 1.0)
+                  speed_ahead_dist = max(0. , way_pts[count, 0] - 2.0)
                   print(speed_ahead_dist)
                   speed_ahead = 5/3.6
                   if n.tags['highway']=='traffic_signals':
@@ -438,7 +438,7 @@ def max_speed_ahead(self, current_speed_limit, lat, lon, heading, lookahead):
               elif not backwards and (n.tags['traffic_signals:direction']=='forward' or n.tags['traffic_signals:direction']=='both'):
                 print("forward")
                 if way_pts[count, 0] > 0:
-                  speed_ahead_dist = max(0. , way_pts[count, 0] - 1.0)
+                  speed_ahead_dist = max(0. , way_pts[count, 0] - 2.0)
                   print(speed_ahead_dist)
                   speed_ahead = 5/3.6
                   if n.tags['highway']=='traffic_signals':
@@ -454,7 +454,7 @@ def max_speed_ahead(self, current_speed_limit, lat, lon, heading, lookahead):
                   if direction > 180:
                     direction = direction - 360
                   if abs(direction) > 135:
-                    speed_ahead_dist = max(0. , way_pts[count, 0] - 1.0)
+                    speed_ahead_dist = max(0. , way_pts[count, 0] - 2.0)
                     print(speed_ahead_dist)
                     speed_ahead = 5/3.6
                     if n.tags['highway']=='traffic_signals':
@@ -604,11 +604,11 @@ def next_way(self, heading):
         except (KeyError, IndexError):
           pass
         try:
-          if ways[0].tags['oneway'] == 'yes':
-            if ways[0].nodes[0].id == node.id and ways[1].nodes[0].id != node.id:
+          if (ways[0].tags['oneway'] == 'yes') and (ways[1].tags['oneway'] == 'yes'):
+            if (ways[0].nodes[0].id == node.id and ways[1].nodes[0].id != node.id) and not (ways[0].nodes[0].id != node.id and ways[1].nodes[0].id == node.id):
               way = Way(ways[0], self.query_results)
               return way
-            elif ways[0].nodes[0].id != node.id and ways[1].nodes[0].id == node.id:
+            elif (ways[0].nodes[0].id != node.id and ways[1].nodes[0].id == node.id) and not (ways[0].nodes[0].id == node.id and ways[1].nodes[0].id != node.id):
               way = Way(ways[1], self.query_results)
               return way
         except (KeyError, IndexError):
@@ -640,11 +640,11 @@ def next_way(self, heading):
         except (KeyError, IndexError):
           pass
         try:
-          if ways[0].tags['oneway'] == 'yes':
-            if ways[0].nodes[0].id == node.id and ways[1].nodes[0].id != node.id:
+          if (ways[0].tags['oneway'] == 'yes') and (ways[1].tags['oneway'] == 'yes'):
+            if (ways[0].nodes[0].id == node.id and ways[1].nodes[0].id != node.id) and not (ways[0].nodes[0].id != node.id and ways[1].nodes[0].id == node.id):
               way = Way(ways[0], self.query_results)
               return way
-            elif ways[0].nodes[0].id != node.id and ways[1].nodes[0].id == node.id:
+            elif (ways[0].nodes[0].id != node.id and ways[1].nodes[0].id == node.id) and not (ways[0].nodes[0].id == node.id and ways[1].nodes[0].id != node.id):
               way = Way(ways[1], self.query_results)
               return way
         except (KeyError, IndexError):

selfdrive/thermald.py

@@ -327,13 +327,6 @@ def thermald_thread():
     elif usb_power and not usb_power_prev:
       params.delete("Offroad_ChargeDisabled")
 
-    if msg.thermal.batteryVoltage < 11500:
-      alert_connectivity_prompt = copy.copy(OFFROAD_ALERTS["Offroad_VoltageLow"])
-      alert_connectivity_prompt["text"] += str(msg.thermal.batteryVoltage) + " Volts."
-      params.delete("Offroad_VoltageLow")
-      params.put("Offroad_VoltageLow", json.dumps(alert_connectivity_prompt))
-    else:
-      params.delete("Offroad_VoltageLow")
 
     thermal_status_prev = thermal_status
     usb_power_prev = usb_power
@@ -343,6 +336,13 @@ def thermald_thread():
 
     # report to server once per minute
     if (count % int(60. / DT_TRML)) == 0:
+      if msg.thermal.batteryVoltage < 11500:
+        alert_connectivity_prompt = copy.copy(OFFROAD_ALERTS["Offroad_VoltageLow"])
+        alert_connectivity_prompt["text"] += str(msg.thermal.batteryVoltage) + " Volts."
+        params.delete("Offroad_VoltageLow")
+        params.put("Offroad_VoltageLow", json.dumps(alert_connectivity_prompt))
+      else:
+        params.delete("Offroad_VoltageLow")
       cloudlog.event("STATUS_PACKET",
         count=count,
         health=(health.to_dict() if health else None),

selfdrive/traffic/__init__.py

@@ -0,0 +1 @@
+

selfdrive/traffic/traffic.py

@@ -0,0 +1,121 @@
+#!/usr/bin/env python2
+import os
+import cv2
+import numpy as np
+import zmq
+
+def detect(source):
+
+    font = cv2.FONT_HERSHEY_SIMPLEX
+    img = cv2.imread(source)
+
+    cimg = img
+    hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
+
+
+    lower_red1 = np.array([0,100,100])
+    upper_red1 = np.array([10,255,255])
+    lower_red2 = np.array([160,100,100])
+    upper_red2 = np.array([180,255,255])
+    lower_green = np.array([40,50,50])
+    upper_green = np.array([90,255,255])
+    lower_yellow = np.array([15,150,150])
+    upper_yellow = np.array([35,255,255])
+    mask1 = cv2.inRange(hsv, lower_red1, upper_red1)
+    mask2 = cv2.inRange(hsv, lower_red2, upper_red2)
+    maskg = cv2.inRange(hsv, lower_green, upper_green)
+    masky = cv2.inRange(hsv, lower_yellow, upper_yellow)
+    maskr = cv2.add(mask1, mask2)
+
+    size = img.shape
+
+    r_circles = cv2.HoughCircles(maskr, cv2.HOUGH_GRADIENT, 1, 80,
+                               param1=50, param2=10, minRadius=0, maxRadius=30)
+
+    g_circles = cv2.HoughCircles(maskg, cv2.HOUGH_GRADIENT, 1, 60,
+                                 param1=50, param2=10, minRadius=0, maxRadius=30)
+
+    y_circles = cv2.HoughCircles(masky, cv2.HOUGH_GRADIENT, 1, 30,
+                                 param1=50, param2=5, minRadius=0, maxRadius=30)
+
+    # traffic light detect
+    r = 5
+    bound = 4.0 / 10
+    if r_circles is not None:
+        r_circles = np.uint16(np.around(r_circles))
+
+        for i in r_circles[0, :]:
+            if i[0] > size[1] or i[1] > size[0]or i[1] > size[0]*bound:
+                continue
+
+            h, s = 0.0, 0.0
+            for m in range(-r, r):
+                for n in range(-r, r):
+
+                    if (i[1]+m) >= size[0] or (i[0]+n) >= size[1]:
+                        continue
+                    h += maskr[i[1]+m, i[0]+n]
+                    s += 1
+            if h / s > 50:
+                cv2.circle(cimg, (i[0], i[1]), i[2]+10, (0, 255, 0), 2)
+                cv2.circle(maskr, (i[0], i[1]), i[2]+30, (255, 255, 255), 2)
+                #cv2.putText(cimg,'RED',(i[0], i[1]), font, 1,(255,0,0),2,cv2.LINE_AA)
+                print("RED")
+    if g_circles is not None:
+        g_circles = np.uint16(np.around(g_circles))
+
+        for i in g_circles[0, :]:
+            if i[0] > size[1] or i[1] > size[0] or i[1] > size[0]*bound:
+                continue
+
+            h, s = 0.0, 0.0
+            for m in range(-r, r):
+                for n in range(-r, r):
+
+                    if (i[1]+m) >= size[0] or (i[0]+n) >= size[1]:
+                        continue
+                    h += maskg[i[1]+m, i[0]+n]
+                    s += 1
+            if h / s > 100:
+                cv2.circle(cimg, (i[0], i[1]), i[2]+10, (0, 255, 0), 2)
+                cv2.circle(maskg, (i[0], i[1]), i[2]+30, (255, 255, 255), 2)
+                #cv2.putText(cimg,'GREEN',(i[0], i[1]), font, 1,(255,0,0),2,cv2.LINE_AA)
+                print("GREEN")
+    if y_circles is not None:
+        y_circles = np.uint16(np.around(y_circles))
+
+        for i in y_circles[0, :]:
+            if i[0] > size[1] or i[1] > size[0] or i[1] > size[0]*bound:
+                continue
+
+            h, s = 0.0, 0.0
+            for m in range(-r, r):
+                for n in range(-r, r):
+
+                    if (i[1]+m) >= size[0] or (i[0]+n) >= size[1]:
+                        continue
+                    h += masky[i[1]+m, i[0]+n]
+                    s += 1
+            if h / s > 50:
+                cv2.circle(cimg, (i[0], i[1]), i[2]+10, (0, 255, 0), 2)
+                cv2.circle(masky, (i[0], i[1]), i[2]+30, (255, 255, 255), 2)
+                #cv2.putText(cimg,'YELLOW',(i[0], i[1]), font, 1,(255,0,0),2,cv2.LINE_AA)
+                print("YELLOW")
+
+
+    return cimg
+if __name__ == '__main__':
+
+
+    while True:
+        context = zmq.Context()
+        socket = context.socket(zmq.REP)
+        socket.bind("tcp://*:9000")
+        message = socket.recv()
+        print("Message is of type {}".format(type(message)))
+        print(message)
+        detectimg =detect(message)
+        sleep(0.05)
+    # This function either gives out "RED" "YELLOW" or "GREEN"
+
+