Merge branch 'tesla_0.7.4_rebase2' into Tesla_0.7.4_rebase

selfdrive/boardd/boardd.cc

@@ -635,6 +635,9 @@ void *can_send_thread(void *crap) {
   while (!do_exit) {
     can_send(subscriber);
   }
+
+  delete subscriber;
+  delete context;
   return NULL;
 }
 
@@ -665,6 +668,9 @@ void *can_recv_thread(void *crap) {
 
     next_frame_time += dt;
   }
+
+  delete publisher;
+  delete c;
   return NULL;
 }
 
@@ -680,6 +686,9 @@ void *can_health_thread(void *crap) {
     can_health(publisher);
     usleep(500*1000);
   }
+
+  delete publisher;
+  delete c;
   return NULL;
 }
 
@@ -914,6 +923,8 @@ void *pigeon_thread(void *crap) {
     cnt++;
   }
 
+  delete publisher;
+  delete context;
   return NULL;
 }
 
@@ -946,7 +957,12 @@ int main() {
   // init libusb
   err = libusb_init(&ctx);
   assert(err == 0);
+
+#if LIBUSB_API_VERSION >= 0x01000106
+  libusb_set_option(ctx, LIBUSB_OPTION_LOG_LEVEL, LIBUSB_LOG_LEVEL_INFO);
+#else
   libusb_set_debug(ctx, 3);
+#endif
 
   pthread_t can_health_thread_handle;
   err = pthread_create(&can_health_thread_handle, NULL,

selfdrive/camerad/main.cc

@@ -975,7 +975,6 @@ void init_buffers(VisionState *s) {
     s->rgb_width = s->frame_width;
     s->rgb_height = s->frame_height;
   }
-
   for (int i=0; i<UI_BUF_COUNT; i++) {
     VisionImg img = visionimg_alloc_rgb24(s->rgb_width, s->rgb_height, &s->rgb_bufs[i]);
     s->rgb_bufs_cl[i] = visionbuf_to_cl(&s->rgb_bufs[i], s->device_id, s->context);
@@ -1013,7 +1012,6 @@ void init_buffers(VisionState *s) {
   s->yuv_width = s->rgb_width;
   s->yuv_height = s->rgb_height;
   s->yuv_buf_size = s->rgb_width * s->rgb_height * 3 / 2;
-
   for (int i=0; i<YUV_COUNT; i++) {
     s->yuv_ion[i] = visionbuf_allocate_cl(s->yuv_buf_size, s->device_id, s->context, &s->yuv_cl[i]);
     s->yuv_bufs[i].y = (uint8_t*)s->yuv_ion[i].addr;
@@ -1061,7 +1059,6 @@ void init_buffers(VisionState *s) {
     s->krnl_debayer_front = clCreateKernel(s->prg_debayer_front, "debayer10", &err);
     assert(err == 0);
   }
-
   rgb_to_yuv_init(&s->rgb_to_yuv_state, s->context, s->device_id, s->yuv_width, s->yuv_height, s->rgb_stride);
   rgb_to_yuv_init(&s->front_rgb_to_yuv_state, s->context, s->device_id, s->yuv_front_width, s->yuv_front_height, s->rgb_front_stride);
 }
@@ -1177,9 +1174,7 @@ int main(int argc, char *argv[]) {
   assert(s->front_frame_sock != NULL);
   assert(s->thumbnail_sock != NULL);
 #endif
-
   cameras_open(&s->cameras, &s->camera_bufs[0], &s->focus_bufs[0], &s->stats_bufs[0], &s->front_camera_bufs[0]);
-
   party(s);
 
 #if defined(QCOM) || defined(QCOM2)

selfdrive/car/hyundai/carcontroller.py

@@ -1,47 +1,102 @@
+from cereal import car
 from selfdrive.car import apply_std_steer_torque_limits
-from selfdrive.car.hyundai.hyundaican import create_lkas11, create_clu11
-from selfdrive.car.hyundai.values import Buttons, SteerLimitParams
+from selfdrive.car.hyundai.hyundaican import create_lkas11, create_clu11, create_lfa_mfa
+from selfdrive.car.hyundai.values import Buttons, SteerLimitParams, CAR
 from opendbc.can.packer import CANPacker
 
+VisualAlert = car.CarControl.HUDControl.VisualAlert
+
+
+def process_hud_alert(enabled, fingerprint, visual_alert, left_lane,
+                      right_lane, left_lane_depart, right_lane_depart):
+  sys_warning = (visual_alert == VisualAlert.steerRequired)
+
+  # initialize to no line visible
+  sys_state = 1
+  if left_lane and right_lane or sys_warning:  #HUD alert only display when LKAS status is active
+    if enabled or sys_warning:
+      sys_state = 3
+    else:
+      sys_state = 4
+  elif left_lane:
+    sys_state = 5
+  elif right_lane:
+    sys_state = 6
+
+  # initialize to no warnings
+  left_lane_warning = 0
+  right_lane_warning = 0
+  if left_lane_depart:
+    left_lane_warning = 1 if fingerprint in [CAR.GENESIS_G90, CAR.GENESIS_G80] else 2
+  if right_lane_depart:
+    right_lane_warning = 1 if fingerprint in [CAR.GENESIS_G90, CAR.GENESIS_G80] else 2
+
+  return sys_warning, sys_state, left_lane_warning, right_lane_warning
+
 
 class CarController():
   def __init__(self, dbc_name, CP, VM):
     self.apply_steer_last = 0
     self.car_fingerprint = CP.carFingerprint
-    self.lkas11_cnt = 0
-    self.cnt = 0
-    self.last_resume_cnt = 0
     self.packer = CANPacker(dbc_name)
     self.steer_rate_limited = False
+    self.resume_cnt = 0
+    self.last_resume_frame = 0
+    self.last_lead_distance = 0
 
-  def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert):
-
-    ### Steering Torque
+  def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert,
+             left_lane, right_lane, left_lane_depart, right_lane_depart):
+    # Steering Torque
     new_steer = actuators.steer * SteerLimitParams.STEER_MAX
     apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, SteerLimitParams)
     self.steer_rate_limited = new_steer != apply_steer
 
-    if not enabled:
-      apply_steer = 0
+    # disable if steer angle reach 90 deg, otherwise mdps fault in some models
+    lkas_active = enabled and abs(CS.out.steeringAngle) < 90.
+
+    # fix for Genesis hard fault at low speed
+    if CS.out.vEgo < 16.7 and self.car_fingerprint == CAR.HYUNDAI_GENESIS:
+      lkas_active = 0
 
-    steer_req = 1 if enabled else 0
+    if not lkas_active:
+      apply_steer = 0
 
     self.apply_steer_last = apply_steer
 
+    sys_warning, sys_state, left_lane_warning, right_lane_warning =\
+      process_hud_alert(enabled, self.car_fingerprint, visual_alert,
+                        left_lane, right_lane, left_lane_depart, right_lane_depart)
+
     can_sends = []
+    can_sends.append(create_lkas11(self.packer, frame, self.car_fingerprint, apply_steer, lkas_active,
+                                   CS.lkas11, sys_warning, sys_state, enabled,
+                                   left_lane, right_lane,
+                                   left_lane_warning, right_lane_warning))
 
-    self.lkas11_cnt = self.cnt % 0x10
-    self.clu11_cnt = self.cnt % 0x10
+    if pcm_cancel_cmd:
+      can_sends.append(create_clu11(self.packer, frame, CS.clu11, Buttons.CANCEL))
 
-    can_sends.append(create_lkas11(self.packer, self.car_fingerprint, apply_steer, steer_req, self.lkas11_cnt,
-                                   enabled, CS.lkas11, hud_alert, keep_stock=True))
+    elif CS.out.cruiseState.standstill:
+      # run only first time when the car stopped
+      if self.last_lead_distance == 0:
+        # get the lead distance from the Radar
+        self.last_lead_distance = CS.lead_distance
+        self.resume_cnt = 0
+      # when lead car starts moving, create 6 RES msgs
+      elif CS.lead_distance != self.last_lead_distance and (frame - self.last_resume_frame) > 5:
+        can_sends.append(create_clu11(self.packer, frame, CS.clu11, Buttons.RES_ACCEL))
+        self.resume_cnt += 1
+        # interval after 6 msgs
+        if self.resume_cnt > 5:
+          self.last_resume_frame = frame
+          self.clu11_cnt = 0
+    # reset lead distnce after the car starts moving
+    elif self.last_lead_distance != 0:
+      self.last_lead_distance = 0
 
-    if pcm_cancel_cmd:
-      can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.CANCEL))
-    elif CS.out.cruiseState.standstill and (self.cnt - self.last_resume_cnt) > 5:
-      self.last_resume_cnt = self.cnt
-      can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.RES_ACCEL))
 
-    self.cnt += 1
+    # 20 Hz LFA MFA message
+    if frame % 5 == 0 and self.car_fingerprint in [CAR.SONATA, CAR.PALISADE]:
+      can_sends.append(create_lfa_mfa(self.packer, frame, enabled))
 
     return can_sends