Merge pull request commaai#207 from xx979xx/mad-patch

Mad kegman

README.md

@@ -1,5 +1,7 @@
+
 Many people have asked if they could donate some cash for my efforts...  I've setup a patreon page here if you feel like buying me a beer or 3:  https://www.patreon.com/kegman
 
+
 This is a fork of comma's openpilot: https://github.com/commaai/openpilot, and contains tweaks for Hondas and GM vehicles.  It is open source and inherits MIT license.  By installing this software you accept all responsibility for anything that might occur while you use it.  All contributors to this fork are not liable.  <b>Use at your own risk.</b>
 
 <b>NOTE:  If you upgrade to 0.6 you cannot go back to 0.5.xx without reflashing your NEOS!</b>

cereal/car.capnp

@@ -86,6 +86,7 @@ struct CarEvent @0x9b1657f34caf3ad3 {
     internetConnectivityNeeded @61;
     manualSteeringRequired @62;
     manualSteeringRequiredBlinkersOn @63;
+    turningIndicatorOn @64;
   }
 }
 

opendbc/hyundai_kia_generic.dbc

@@ -1444,3 +1444,6 @@ BO_ 905 SCC14: 8 SCC
  SG_ JerkLowerLimit : 19|7@1+ (0.1,0) [0|12.7] "m/s^3"  ESC
  SG_ SCCMode : 32|3@1+ (1,0) [0|7] ""  ESC
 
+BO_ 882 NEW11: 8 XXX
+ SG_ Gear_Signal : 16|3@1+ (1,0) [0|255] ""  CLU
+

panda/board/main.c

@@ -120,7 +120,7 @@ void set_safety_mode(uint16_t mode, int16_t param) {
           if(hw_type == HW_TYPE_BLACK_PANDA){
             current_board->set_can_mode(CAN_MODE_NORMAL);
           }
-          can_silent = ALL_CAN_SILENT;
+          can_silent = ALL_CAN_LIVE;
           break;
         case SAFETY_ELM327:
           set_intercept_relay(false);
@@ -701,7 +701,7 @@ int main(void) {
       // if SAFETY_NOOUTPUT isn't succesfully set, we can't continue
     }
   }
-  can_silent = ALL_CAN_SILENT;
+  can_silent = ALL_CAN_LIVE;
   can_init_all();
 
 #ifndef EON

panda/board/safety/safety_defaults.h

@@ -1,5 +1,32 @@
+bool HKG_forwarding_enabled = 1;
+int HKG_MDPS12_checksum = -1;
+int HKG_MDPS12_cnt = 0;   
+int HKG_last_StrColT = 0;
+
 void default_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
-  UNUSED(to_push);
+  int addr = GET_ADDR(to_push);
+
+  if (addr == 593) {
+    if (HKG_MDPS12_checksum == -1) {
+      int New_Chksum2 = 0;
+      uint8_t dat[8];
+      for (int i=0; i<8; i++) {
+        dat[i] = GET_BYTE(to_push, i);
+      }
+      int Chksum2 = dat[3];
+      dat[3] = 0;
+      for (int i=0; i<8; i++) {
+        New_Chksum2 += dat[i];
+      }
+      New_Chksum2 %= 256;
+      if (Chksum2 == New_Chksum2) {
+        HKG_MDPS12_checksum = 0;
+      }
+      else {
+        HKG_MDPS12_checksum = 1;
+      }
+    }
+  }
 }
 
 int default_ign_hook(void) {
@@ -15,7 +42,7 @@ static void nooutput_init(int16_t param) {
 
 static int nooutput_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
   UNUSED(to_send);
-  return false;
+  return 1;
 }
 
 static int nooutput_tx_lin_hook(int lin_num, uint8_t *data, int len) {
@@ -25,10 +52,87 @@ static int nooutput_tx_lin_hook(int lin_num, uint8_t *data, int len) {
   return false;
 }
 
-static int default_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
-  UNUSED(bus_num);
-  UNUSED(to_fwd);
-  return -1;
+ static int default_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
+  int addr = GET_ADDR(to_fwd);
+  int bus_fwd = -1;
+
+  if ((bus_num == 0) && (addr == 832)) {
+    HKG_forwarding_enabled = 0;
+  }
+
+  if (HKG_forwarding_enabled) {
+    if (bus_num == 0) {
+      if (addr == 593) {
+        uint8_t dat[8];
+        int New_Chksum2 = 0;
+        for (int i=0; i<8; i++) {
+          dat[i] = GET_BYTE(to_fwd, i);
+        }
+        if (HKG_MDPS12_cnt > 330) {
+          int StrColTq = dat[0] | (dat[1] & 0x7) << 8;
+          int OutTq = dat[6] >> 4 | dat[7] << 4;
+          if (HKG_MDPS12_cnt == 331) {
+            StrColTq -= 164;
+          }
+          else {
+            StrColTq = HKG_last_StrColT + 34;
+          }
+          OutTq = 2058;
+
+          dat[0] = StrColTq & 0xFF;
+          dat[1] &= 0xF8;
+          dat[1] |= StrColTq >> 8;
+          dat[6] &= 0xF;
+          dat[6] |= (OutTq & 0xF) << 4;
+          dat[7] = OutTq >> 4;
+
+
+          to_fwd->RDLR &= 0xFFF800;
+          to_fwd->RDLR |= StrColTq;
+          to_fwd->RDHR &= 0xFFFFF;
+          to_fwd->RDHR |= OutTq << 20;
+          HKG_last_StrColT = StrColTq;
+
+          dat[3] = 0;
+          if (HKG_MDPS12_checksum == 0) { 
+            for (int i=0; i<8; i++) {
+              New_Chksum2 += dat[i];
+            }
+            New_Chksum2 %= 256;
+          }
+          else if (HKG_MDPS12_checksum == 1) { //we need CRC8 checksum
+            uint8_t crc = 0xFF;
+            uint8_t poly = 0x1D;
+            int i, j;
+            for (i=0; i<8; i++){
+              if (i!=3){ //don't include CRC byte
+                crc ^= dat[i];
+                for (j=0; j<8; j++) {
+                  if ((crc & 0x80) != 0U) {
+                    crc = (crc << 1) ^ poly;
+                  } else
+                  {
+                  crc <<= 1;
+                  }
+                }
+              }
+            }
+            crc ^= 0xFF;
+            crc %= 256;
+            New_Chksum2 = crc;
+          }
+          to_fwd->RDLR |= New_Chksum2 << 24;
+        }
+        HKG_MDPS12_cnt += 1;
+        HKG_MDPS12_cnt %= 345;
+      }
+      bus_fwd = 2;
+    }
+    if (bus_num == 2) {
+      bus_fwd = 0;
+    }
+  }
+  return bus_fwd;
 }
 
 const safety_hooks nooutput_hooks = {

panda/board/safety/safety_elm327.h

@@ -15,6 +15,8 @@ static int elm327_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
       ((addr != 0x7DF) && ((addr & 0x7F8) != 0x7E0))) {
     tx = 0;
   }
+  // needed for forwarding
+  tx = 1;
   return tx;
 }
 

panda/board/safety/safety_hyundai.h

@@ -14,6 +14,9 @@ int hyundai_desired_torque_last = 0;
 int hyundai_cruise_engaged_last = 0;
 uint32_t hyundai_ts_last = 0;
 struct sample_t hyundai_torque_driver;         // last few driver torques measured
+int OP_LKAS_live = 0;
+bool hyundai_LKAS_forwarded = 0;
+bool hyundai_has_scc = 0;
 
 static void hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
   int bus = GET_BUS(to_push);
@@ -38,13 +41,26 @@ static void hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
 
   // enter controls on rising edge of ACC, exit controls on ACC off
   if (addr == 1057) {
+    hyundai_has_scc = 1;
     // 2 bits: 13-14
     int cruise_engaged = (GET_BYTES_04(to_push) >> 13) & 0x3;
-    if (cruise_engaged && !hyundai_cruise_engaged_last) {
+    //if (cruise_engaged && !hyundai_cruise_engaged_last) {
       controls_allowed = 1;
+    //}
+    if (!cruise_engaged) {
+      //controls_allowed = 0;
     }
+    hyundai_cruise_engaged_last = cruise_engaged;
+  }
+  // cruise control for car without SCC
+  if ((addr == 871) && (!hyundai_has_scc)) {
+    // first byte
+    int cruise_engaged = (GET_BYTES_04(to_push) & 0xFF);
+    //if (cruise_engaged && !hyundai_cruise_engaged_last) {
+      controls_allowed = 1;
+    //}
     if (!cruise_engaged) {
-      controls_allowed = 0;
+      //controls_allowed = 0;
     }
     hyundai_cruise_engaged_last = cruise_engaged;
   }
@@ -71,6 +87,13 @@ static int hyundai_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
     uint32_t ts = TIM2->CNT;
     bool violation = 0;
 
+    if (!hyundai_LKAS_forwarded) {
+      OP_LKAS_live = 20;
+    }
+    if ((hyundai_LKAS_forwarded) && (!OP_LKAS_live)) {
+      hyundai_LKAS_forwarded = 0;
+      return 1;
+    }
     if (controls_allowed) {
 
       // *** global torque limit check ***
@@ -130,7 +153,7 @@ static int hyundai_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
 
   int bus_fwd = -1;
   // forward cam to ccan and viceversa, except lkas cmd
-  if (hyundai_giraffe_switch_2) {
+  if (!hyundai_camera_detected) {
     if (bus_num == 0) {
       bus_fwd = hyundai_camera_bus;
     }
@@ -139,6 +162,13 @@ static int hyundai_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
       if (addr != 832) {
         bus_fwd = 0;
       }
+      else if (!OP_LKAS_live) {
+        hyundai_LKAS_forwarded = 1;
+        bus_fwd = 0;
+      }
+      else {
+        OP_LKAS_live -= 1;
+      }
     }
   }
   return bus_fwd;

selfdrive/can/tests/test_packer_hyundai.py

@@ -39,11 +39,14 @@ def test_correctness(self):
         "CF_Lkas_LdwsOpt_USM": random.randint(0,65536)
       }
       hud_alert = random.randint(0, 65536)
+      lane_visible = random.randint(0, 65536)
+      left_lane_depart = (random.randint(0, 2) % 2 == 0)
+      right_lane_depart = (random.randint(0, 2) % 2 == 0)
       keep_stock = (random.randint(0, 2) % 2 == 0)
       m_old = hyundaican.create_lkas11(self.hyundai_cp_old, car_fingerprint, apply_steer, steer_req, cnt, enabled,
-                                       lkas11, hud_alert, keep_stock)
+                                       lkas11, hud_alert, lane_visible, left_lane_depart, right_lane_depart, keep_stock)
       m = hyundaican.create_lkas11(self.hyundai_cp, car_fingerprint, apply_steer, steer_req, cnt, enabled,
-                                  lkas11, hud_alert, keep_stock)
+                                  lkas11, hud_alert, lane_visible, left_lane_depart, right_lane_depart, keep_stock)
       self.assertEqual(m_old, m)
 
       clu11 = {
@@ -61,8 +64,8 @@ def test_correctness(self):
         "CF_Clu_AliveCnt1": random.randint(0,65536),
       }
       button = random.randint(0, 65536)
-      m_old = hyundaican.create_clu11(self.hyundai_cp_old, clu11, button)
-      m = hyundaican.create_clu11(self.hyundai_cp, clu11, button)
+      m_old = hyundaican.create_clu11(self.hyundai_cp_old, clu11, button, cnt)
+      m = hyundaican.create_clu11(self.hyundai_cp, clu11, button, cnt)
       self.assertEqual(m_old, m)