From 7da6576afdff4205db4755799620a57ecffdbfd6 Mon Sep 17 00:00:00 2001
From: Jafar Al-Gharaibeh <to.jafar@gmail.com>
Date: Wed, 27 May 2020 16:31:06 -0500
Subject: [PATCH] Sync stock ACC state with OP

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

diff --git a/selfdrive/car/mazda/carcontroller.py b/selfdrive/car/mazda/carcontroller.py
index 61ad78ca3b..2270e83fe1 100644
--- a/selfdrive/car/mazda/carcontroller.py
+++ b/selfdrive/car/mazda/carcontroller.py
@@ -25,10 +25,12 @@ class CarController():
     else:
       apply_steer = 0
       self.steer_rate_limited = False
+      if CS.acc_active and frame % 10 == 0:
+        can_sends.append(mazdacan.create_cancel_acc(self.packer, CS.CP.carFingerprint))
+
 
     self.apply_steer_last = apply_steer
   
-
     can_sends.append(mazdacan.create_steering_control(self.packer, CS.CP.carFingerprint,
                                                       frame, apply_steer, CS.cam_lkas))
     return can_sends
diff --git a/selfdrive/car/mazda/carstate.py b/selfdrive/car/mazda/carstate.py
index 30a4a97cea..25d2bc0f53 100644
--- a/selfdrive/car/mazda/carstate.py
+++ b/selfdrive/car/mazda/carstate.py
@@ -73,20 +73,16 @@ class CarState(CarStateBase):
     if any([cp.vl["CRZ_BTNS"]['RES'],
                 cp.vl["CRZ_BTNS"]['SET_P'],
                 cp.vl["CRZ_BTNS"]['SET_M']]):
-      self.acc_active = True
       self.cruise_speed = ret.vEgoRaw
 
     ret.cruiseState.available = cp.vl["CRZ_CTRL"]['CRZ_ACTIVE'] == 1
-    if not ret.cruiseState.available or ret.gasPressed or ret.brakePressed:
-      self.acc_active = False
-
+    self.acc_active = ret.cruiseState.available
     ret.cruiseState.enabled = self.acc_active
     ret.cruiseState.speed = self.cruise_speed
 
     if self.acc_active:
       if not self.lkas_on:
         if not self.acc_active_last:
-          self.acc_active = False
           self.low_speed_lockout = True
         else:
           self.low_speed_alert = True
@@ -149,6 +145,7 @@ class CarState(CarStateBase):
         ("RES", "CRZ_BTNS", 0),
         ("SET_P", "CRZ_BTNS", 0),
         ("SET_M", "CRZ_BTNS", 0),
+        ("CTR", "CRZ_BTNS", 0),
       ]
 
       checks += [
diff --git a/selfdrive/car/mazda/mazdacan.py b/selfdrive/car/mazda/mazdacan.py
index 931673423a..a768f9f15f 100644
--- a/selfdrive/car/mazda/mazdacan.py
+++ b/selfdrive/car/mazda/mazdacan.py
@@ -55,3 +55,39 @@ def create_steering_control(packer, car_fingerprint, frame, apply_steer, lkas):
     }
 
   return packer.make_can_msg("CAM_LKAS", 0, values)
+
+
+def create_cancel_acc(packer, car_fingerprint):
+  if car_fingerprint == CAR.CX5:
+    values = {
+      "CAN_OFF"           : 1,
+      "CAN_OFF_INV"       : 0,
+
+      "SET_P"             : 0,
+      "SET_P_INV"         : 1,
+
+      "RES"               : 0,
+      "RES_INV"           : 1,
+
+      "SET_M"             : 0,
+      "SET_M_INV"         : 1,
+
+      "DISTANCE_LESS"     : 0,
+      "DISTANCE_LESS_INV" : 1,
+
+      "DISTANCE_MORE"     : 0,
+      "DISTANCE_MORE_INV" : 1,
+
+      "MODE_X"            : 0,
+      "MODE_X_INV"        : 1,
+
+      "MODE_Y"            : 0,
+      "MODE_Y_INV"        : 1,
+
+      "BIT1"              : 1,
+      "BIT2"              : 1,
+      "BIT3"              : 1,
+      "CTR"               : 0
+    }
+
+    return packer.make_can_msg("CRZ_BTNS", 0, values)