fix CC for ACC

SippieCup · Nov 24, 2019 · de34cbe · de34cbe
1 parent 637297f
commit de34cbe
Show file tree

Hide file tree

Showing 4 changed files with 13 additions and 8 deletions.
diff --git a/panda/board/safety/safety_tesla.h b/panda/board/safety/safety_tesla.h
@@ -1549,7 +1549,6 @@ static int tesla_tx_hook(CAN_FIFOMailBox_TypeDef *to_send)
     DAS_high_low_beam_request = b0;
     DAS_high_low_beam_reason = b1;
     DAS_ahb_is_enabled = b2 & 0x01;
-    DAS_plain_cc_enabled = (b2 >> 1) & 0x01;
     //intercept and do not forward
     return false;
   }
@@ -1575,6 +1574,11 @@ static int tesla_tx_hook(CAN_FIFOMailBox_TypeDef *to_send)
     DAS_turn_signal_request = ((b2 & 0xC0) >> 6);
     DAS_forward_collision_warning = ((b2 & 0x10) >> 4);
     DAS_units_included = ((b2 & 0x20) >> 5);
+    if (((b2 >> 3) & 0x01) == 0) {
+      DAS_plain_cc_enabled = 1;
+    } else {
+      DAS_plain_cc_enabled = 0;
+    }
     DAS_hands_on_state = (b2 & 0x07);
     DAS_cc_state = ((b3 & 0xC0)>>6);
     DAS_usingPedal = ((b3 & 0x20) >> 5);

diff --git a/selfdrive/car/tesla/ACC_module.py b/selfdrive/car/tesla/ACC_module.py
@@ -223,7 +223,7 @@ def update_acc(self, enabled, CS, frame, actuators, pcm_speed, speed_limit_kph,
       button_to_press = CruiseButtons.MAIN
 
     #if plain cc, not adaptive, then just return None or Cancel
-    if not self.adaptive and self.enable_adaptive_cruise:
+    if (not self.adaptive) and self.enable_adaptive_cruise:
       self.acc_speed_kph = CS.v_cruise_actual #if not CS.imperial_speed_units else CS.v_cruise_actual * CV.MPH_TO_KPH
       return button_to_press
 

diff --git a/selfdrive/car/tesla/carcontroller.py b/selfdrive/car/tesla/carcontroller.py
@@ -598,13 +598,14 @@ def update(self, enabled, CS, frame, actuators, \
     if CS.useTeslaRadar and CS.hasTeslaIcIntegration:
       highLowBeamStatus,highLowBeamReason,ahbIsEnabled = self.AHB.update(CS,frame,self.ahbLead1)
       if frame % 5 == 0:
-        can_sends.append(teslacan.create_fake_DAS_msg2(highLowBeamStatus,highLowBeamReason,ahbIsEnabled,(not CS.pedal_interceptor_available) and (not self.ACC.adaptive)))
+        can_sends.append(teslacan.create_fake_DAS_msg2(highLowBeamStatus,highLowBeamReason,ahbIsEnabled))
     if send_fake_msg:
       if enable_steer_control and op_status == 3:
         op_status = 0x5
+      adaptive_cruise = 1 if   (not CS.pedal_interceptor_available and self.ACC.adaptive) or CS.pedal_interceptor_available else 0
       can_sends.append(teslacan.create_fake_DAS_msg(speed_control_enabled,speed_override,self.DAS_206_apUnavailable, collision_warning, op_status, \
             acc_speed_kph, \
-            turn_signal_needed,forward_collision_warning,  hands_on_state, \
+            turn_signal_needed,forward_collision_warning, adaptive_cruise,  hands_on_state, \
             cc_state, 1 if (CS.pedal_interceptor_available) else 0,alca_state, \
             CS.v_cruise_pcm, #acc_speed_limit_mph,
             CS.speedLimitToIc, #speed_limit_to_car,

diff --git a/selfdrive/car/tesla/teslacan.py b/selfdrive/car/tesla/teslacan.py
@@ -130,17 +130,17 @@ def create_DAS_LR_object_msg(lane,v1Class,v1Id,v1Dx,v1Dy,v1V,v2Class,v2Id,v2Dx,v
     int((v2y >> 5) & 0x03) + ((v2Id << 2) & 0xFC))
   return [msg_id, 0, msg.raw, 0]
 
-def create_fake_DAS_msg2(hiLoBeamStatus,hiLoBeamReason,ahbIsEnabled,plainccenabled):
+def create_fake_DAS_msg2(hiLoBeamStatus,hiLoBeamReason,ahbIsEnabled):
   msg_id = 0x65A
   msg_len = 3
   msg = create_string_buffer(msg_len)
-  struct.pack_into('BBB', msg, 0, hiLoBeamStatus, hiLoBeamReason,(1 if ahbIsEnabled else 0) + ((1 if plainccenabled else 0)<<1))
+  struct.pack_into('BBB', msg, 0, hiLoBeamStatus, hiLoBeamReason,(1 if ahbIsEnabled else 0))
   return [msg_id, 0, msg.raw, 0]
 
 
 def create_fake_DAS_msg(speed_control_enabled,speed_override,apUnavailable, collision_warning, op_status, \
                  acc_speed_kph, \
-                 turn_signal_needed,forward_collission_warning,hands_on_state, \
+                 turn_signal_needed,forward_collission_warning,adaptive_cruise, hands_on_state, \
                  cc_state, pedal_state, alca_state, \
                  acc_speed_limit_mph,
                  legal_speed_limit,
@@ -153,7 +153,7 @@ def create_fake_DAS_msg(speed_control_enabled,speed_override,apUnavailable, coll
   c_apply_steer = int(((int( apply_angle * 10 + 0x4000 )) & 0x7FFF) + (enable_steer_control << 15))
   struct.pack_into('BBBBBBBB', msg, 0,int((speed_control_enabled << 7) + (speed_override << 6) + (apUnavailable << 5) + (collision_warning << 4) + op_status), \
       int(acc_speed_kph), \
-      int((turn_signal_needed << 6) + (units_included << 5) + (forward_collission_warning << 4)  + hands_on_state), \
+      int((turn_signal_needed << 6) + (units_included << 5) + (forward_collission_warning << 4)  + (adaptive_cruise << 3) + hands_on_state), \
       int((cc_state << 6) + (pedal_state << 5) + alca_state), \
       int(acc_speed_limit_mph),
       int(legal_speed_limit),