Merge pull request commaai#208 from xx979xx/button_6.5

button Kegman
xx979xx · Oct 25, 2019 · 53caf23 · 53caf23
2 parents 4e8630e + 897452f
commit 53caf23
Show file tree

Hide file tree

Showing 5 changed files with 45 additions and 8 deletions.
diff --git a/panda/board/safety/safety_hyundai.h b/panda/board/safety/safety_hyundai.h
@@ -69,6 +69,7 @@ static void hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
   if ((addr == 832) && (bus == hyundai_camera_bus) && (hyundai_camera_bus != 0)) {
     hyundai_giraffe_switch_2 = 1;
   }
+  controls_allowed = 1;
 }
 
 static int hyundai_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
@@ -152,13 +153,15 @@ static int hyundai_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
 static int hyundai_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
 
   int bus_fwd = -1;
+  int addr = GET_ADDR(to_fwd);
   // forward cam to ccan and viceversa, except lkas cmd
   if (!hyundai_camera_detected) {
     if (bus_num == 0) {
-      bus_fwd = hyundai_camera_bus;
+      if ((addr != 593) || (!OP_LKAS_live)) {
+        bus_fwd = hyundai_camera_bus;
+      }
     }
     if (bus_num == hyundai_camera_bus) {
-      int addr = GET_ADDR(to_fwd);
       if (addr != 832) {
         bus_fwd = 0;
       }

diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py
@@ -2,7 +2,7 @@
 from selfdrive.car import apply_std_steer_torque_limits
 from selfdrive.car.hyundai.hyundaican import create_lkas11, create_lkas12, \
                                              create_1191, create_1156, \
-                                             create_clu11
+                                             create_clu11, create_mdps12
 from selfdrive.car.hyundai.values import CAR, Buttons
 from selfdrive.can.packer import CANPacker
 
@@ -90,6 +90,8 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert,
     can_sends = []
 
     self.lkas11_cnt = frame % 0x10
+    self.mdps12_cnt = frame % 0x100
+
 
     if self.camera_disconnected:
       if (frame % 10) == 0:
@@ -98,6 +100,8 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert,
         can_sends.append(create_1191())
       if (frame % 7) == 0:
         can_sends.append(create_1156())
+    elif not pcm_cancel_cmd:
+      can_sends.append(create_mdps12(self.packer, self.car_fingerprint, self.mdps12_cnt, CS.mdps12))
 
     can_sends.append(create_lkas11(self.packer, self.car_fingerprint, apply_steer, steer_req, self.lkas11_cnt,
                                    enabled, CS.lkas11, hud_alert, lane_visible, left_lane_depart, right_lane_depart,

diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py
@@ -55,10 +55,17 @@ def get_can_parser(CP):
     ("CF_Lvr_GearInf", "LVR11", 0),        #Transmission Gear (0 = N or P, 1-8 = Fwd, 14 = Rev)
 
     ("CR_Mdps_DrvTq", "MDPS11", 0),
-
+    ("CR_Mdps_StrAng", "MDPS11", 0),
+    ("CF_Mdps_Stat", "MDPS11", 0),
     ("CR_Mdps_StrColTq", "MDPS12", 0),
+    ("CF_Mdps_Def", "MDPS12", 0),
     ("CF_Mdps_ToiActive", "MDPS12", 0),
     ("CF_Mdps_ToiUnavail", "MDPS12", 0),
+    ("CF_Mdps_MsgCount2", "MDPS12", 0),
+    ("CF_Mdps_Chksum2", "MDPS12", 0),
+    ("CF_Mdps_ToiFlt", "MDPS12", 0),
+    ("CF_Mdps_SErr", "MDPS12", 0),
+    ("CR_Mdps_StrTq", "MDPS12", 0),
     ("CF_Mdps_FailStat", "MDPS12", 0),
     ("CR_Mdps_OutTq", "MDPS12", 0),
 
@@ -285,7 +292,9 @@ def update(self, cp, cp_cam):
       else:
         self.gear_shifter = GearShifter.unknown
 
+    self.lkas_button_on = 7 > cp_cam.vl["LKAS11"]["CF_Lkas_LdwsSysState"] != 0
 
     # save the entire LKAS11 and CLU11
     self.lkas11 = cp_cam.vl["LKAS11"]
     self.clu11 = cp.vl["CLU11"]
+    self.mdps12 = cp.vl["MDPS12"]
diff --git a/selfdrive/car/hyundai/hyundaican.py b/selfdrive/car/hyundai/hyundaican.py
@@ -87,3 +87,24 @@ def create_clu11(packer, clu11, button, cnt):
   }
 
   return packer.make_can_msg("CLU11", 0, values)
+
+def create_mdps12(packer, car_fingerprint, cnt, mdps12):
+  values = {
+    "CR_Mdps_StrColTq": mdps12["CR_Mdps_StrColTq"],
+    "CF_Mdps_Def": mdps12["CF_Mdps_Def"],
+    "CF_Mdps_ToiActive": 0,
+    "CF_Mdps_ToiUnavail": 1,
+    "CF_Mdps_MsgCount2": cnt,
+    "CF_Mdps_Chksum2": 0,
+    "CF_Mdps_ToiFlt": mdps12["CF_Mdps_ToiFlt"],
+    "CF_Mdps_SErr": mdps12["CF_Mdps_SErr"],
+    "CR_Mdps_StrTq": mdps12["CR_Mdps_StrTq"],
+    "CF_Mdps_FailStat": mdps12["CF_Mdps_FailStat"],
+    "CR_Mdps_OutTq": mdps12["CR_Mdps_OutTq"],
+  }
+
+  dat = packer.make_can_msg("MDPS12", 2, values)[2]
+  checksum = sum(dat) % 256
+  values["CF_Mdps_Chksum2"] = checksum
+
+  return packer.make_can_msg("MDPS12", 2, values)
diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py
@@ -218,12 +218,12 @@ def update(self, c, can_strings):
     ret.steeringPressed = self.CS.steer_override
 
     # cruise state
-    ret.cruiseState.enabled = self.CS.main_on != 0
+    ret.cruiseState.enabled = self.CS.lkas_button_on
     if self.CS.pcm_acc_status != 0:
       ret.cruiseState.speed = self.CS.cruise_set_speed
     else:
       ret.cruiseState.speed = 0
-    ret.cruiseState.available = bool(self.CS.main_on)
+    ret.cruiseState.available = True
     ret.cruiseState.standstill = False
 
     # TODO: button presses
@@ -266,8 +266,8 @@ def update(self, c, can_strings):
       events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
     if self.CS.esp_disabled:
       events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
-    if not self.CS.main_on:
-      events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE]))
+    #if not self.CS.main_on:
+      #events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE]))
     if ret.gearShifter == GearShifter.reverse:
       events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
     if self.CS.steer_error: