From 340907221b65b2ac54a22deea188560739cc5a21 Mon Sep 17 00:00:00 2001
From: Jafar Al-Gharaibeh <to.jafar@gmail.com>
Date: Mon, 27 Apr 2020 00:25:03 -0500
Subject: [PATCH] Update checksum to account for steer angle signal used in
 some cars

Signed-off-by: Jafar Al-Gharaibeh <to.jafar@gmail.com>
---
 selfdrive/car/mazda/carcontroller.py |  4 ++--
 selfdrive/car/mazda/carstate.py      |  9 +++----
 selfdrive/car/mazda/mazdacan.py      | 36 +++++++++++++++++++++-------
 3 files changed, 35 insertions(+), 14 deletions(-)

diff --git a/selfdrive/car/mazda/carcontroller.py b/selfdrive/car/mazda/carcontroller.py
index 2f78a45583..0694c58827 100644
--- a/selfdrive/car/mazda/carcontroller.py
+++ b/selfdrive/car/mazda/carcontroller.py
@@ -17,11 +17,11 @@ class CarController():
 
     ### STEER ###
 
-    if enabled and not CS.steer_not_allowed:
+    if enabled:
       # calculate steer and also set limits due to driver torque
       new_steer = int(round(actuators.steer * SteerLimitParams.STEER_MAX))
       apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last,
-                                                  CS.out.steer_torque_driver, SteerLimitParams)
+                                                  CS.out.steeringTorque, SteerLimitParams)
       self.steer_rate_limited = new_steer != apply_steer
     else:
       apply_steer = 0
diff --git a/selfdrive/car/mazda/carstate.py b/selfdrive/car/mazda/carstate.py
index 285442da25..cd98ce96d1 100644
--- a/selfdrive/car/mazda/carstate.py
+++ b/selfdrive/car/mazda/carstate.py
@@ -177,14 +177,15 @@ class CarState(CarStateBase):
     signals = [
       # sig_name, sig_address, default
 
-      ("LKAS_REQUEST",     "CAM_LKAS", 0),
+      ("LKAS_REQUEST",     "CAM_LKAS", 2048),
       ("CTR",              "CAM_LKAS", 0),
       ("ERR_BIT_1",        "CAM_LKAS", 0),
-      ("LDW",              "CAM_LKAS", 0),
       ("LINE_NOT_VISIBLE", "CAM_LKAS", 0),
-      ("BIT_1",            "CAM_LKAS", 0),
+      ("LDW",              "CAM_LKAS", 0),
+      ("BIT_1",            "CAM_LKAS", 1),
       ("ERR_BIT_2",        "CAM_LKAS", 0),
-      ("BIT_2",            "CAM_LKAS", 0),
+      ("LKAS_ANGLE",       "CAM_LKAS", 2048),
+      ("BIT2",             "CAM_LKAS", 0),
       ("CHKSUM",           "CAM_LKAS", 0),
     ]
 
diff --git a/selfdrive/car/mazda/mazdacan.py b/selfdrive/car/mazda/mazdacan.py
index 51c42c277b..540c77934f 100644
--- a/selfdrive/car/mazda/mazdacan.py
+++ b/selfdrive/car/mazda/mazdacan.py
@@ -8,29 +8,49 @@ def create_steering_control(packer, car_fingerprint, frame, apply_steer, lkas):
   hi = tmp >> 8
 
   b1 = int(lkas["BIT_1"])
-  b2 = int(lkas[ "BIT_2"])
   ldw = int(lkas["LDW"])
+  er1= int(lkas["ERR_BIT_1"])
   lnv = 0 #int(lkas["LINE_NOT_VISIBLE"])
+  er2= int(lkas["ERR_BIT_2"])
+
+  lkas_angle = lkas["LKAS_ANGLE"]
+  b2 = int(lkas["BIT2"])
+
+  tmp = int((lkas_angle + 45.06 ) / 0.022)
+  ahi =  tmp >> 10
+  amd =  (tmp  & 0x3FF) >> 2
+  amd =  (amd >> 4) | (( amd & 0xF) << 4)
+  alo =  (tmp & 0x3) << 2
 
   ctr = frame % 16
+  # bytes:     [    1  ] [ 2 ] [             3               ]  [           4         ]
+  csum = 249 - ctr - hi - lo - (lnv << 3) - er1  - (ldw << 7) - ( er2 << 4) - (b1 << 5)
+
+  #bytes       [ 5 ] [ 6 ] [    7   ]
+  csum = csum - ahi - amd - alo - b2
 
-  csum = 241 - ctr - (hi - 8) - lo - (lnv << 3) - (b1 << 5)  - (b2 << 1) - (ldw << 7)
+  if ahi == 1:
+    csum = csum + 15
 
   if csum < 0:
+    if csum < -256:
+      csum = csum + 512
+    else:
       csum = csum + 256
 
   csum = csum % 256
 
   if car_fingerprint == CAR.CX5:
     values = {
-      "CTR"              : ctr,
       "LKAS_REQUEST"     : apply_steer,
-      "BIT_1"            : b1,
-      "BIT_2"            : b2,
-      "LDW"              : ldw,
+      "CTR"              : ctr,
+      "ERR_BIT_1"        : er1,
       "LINE_NOT_VISIBLE" : lnv,
-      "ERR_BIT_1"        : 0,
-      "ERR_BIT_2"        : 0,
+      "LDW"              : ldw,
+      "BIT_1"            : b1,
+      "ERR_BIT_2"        : er2,
+      "LKAS_ANGLE"       : lkas_angle,
+      "BIT2"             : b2,
       "CHKSUM"           : csum
     }