fleet speed indicator

panda/board/build.mk

@@ -43,7 +43,7 @@ bin: obj/$(PROJ_NAME).bin
 # this flashes everything
 recover: obj/bootstub.$(PROJ_NAME).bin obj/$(PROJ_NAME).bin
 	-PYTHONPATH=../ python -c "from python import Panda; Panda().reset(enter_bootloader=True)"
-	sleep 1.0
+	sleep 2.0
 	$(DFU_UTIL) -d 0483:df11 -a 0 -s 0x08004000 -D obj/$(PROJ_NAME).bin
 	$(DFU_UTIL) -d 0483:df11 -a 0 -s 0x08000000:leave -D obj/bootstub.$(PROJ_NAME).bin
 

panda/board/safety/safety_tesla.h

@@ -791,7 +791,7 @@ static void do_fake_DAS(uint32_t RIR, uint32_t RDTR) {
   if (fake_DAS_counter % 50 == 0) {
     //send DAS_status - 0x399
     MLB = DAS_op_status + 0xF0 + (DAS_speed_limit_kph << 8) + (((DAS_collision_warning << 6) + DAS_speed_limit_kph) << 16);
-    MHB = ((DAS_cc_state & 0x03) << 3) + (DAS_ldwStatus << 5) + 
+    MHB = ((DAS_fleet_speed_state & 0x03) << 3) + (DAS_ldwStatus << 5) + 
         (((DAS_hands_on_state << 2) + ((DAS_alca_state & 0x03) << 6) + DAS_fleet_speed_state) << 8) +
        ((( DAS_status_idx << 4) + (DAS_alca_state >> 2)) << 16);
     int cksm = add_tesla_cksm2(MLB, MHB, 0x399, 7);
@@ -819,10 +819,15 @@ static void do_fake_DAS(uint32_t RIR, uint32_t RDTR) {
     if (DAS_cc_state > 1) { //enabled or hold
         b4 = 0x84;
         b5 = 0x25; //BB lssState 0x 0x03 should be LSS_STATE_ELK enhanced LK
-        //DAS_RobState active 0x02
-        //DAS_radarTelemetry normal 0x01
-        //DAS_lssState 0x03
-        //DAS_acc_report ACC_report_target_CIPV 0x01
+        int _DAS_RobState = 0x02; //active
+        int _DAS_radarTelemetry = 0x01; // normal 
+        int _DAS_lssState = 0x03;
+        int _DAS_acc_report = 0x01; //ACC_report_target_CIPV
+        if (DAS_fleet_speed_state == 2) {
+          _DAS_acc_report = 0x12; //ACC_report_fleet_speed
+        }
+        b4 = (_DAS_acc_report << 2) + ((_DAS_lssState & 0x01) << 7);
+        b5 =((_DAS_lssState >> 1) & 0x01) + (_DAS_radarTelemetry << 2) + (_DAS_RobState <<4);
     }
     MLB = MLB + (b4 << 24);
     MHB = 0x8000 + b5 + (DAS_status2_idx << 20) + (lcw << 16);

selfdrive/car/tesla/AHB_module.py

@@ -1,5 +1,5 @@
 from selfdrive.config import Conversions as CV
-from cereal import tesla
+from cereal import tesla,log
 import selfdrive.messaging as messaging
 import time
 
@@ -40,6 +40,9 @@ def __init__(self,carcontroller):
     self.ahbInfo = messaging.sub_sock('ahbInfo', conflate=True)
     self.ahbInfoData = None
     self.ahbIsEnabled = False
+    self.frameInfo = messaging.sub_sock('frame', conflate=True)
+    self.frameInfoData = None
+    self.frameInfoGain = 0
 
   def set_and_return(self,CS,frame,highLowBeamStatus,highLowBeamReason):
     self.prev_light_stalk_position = CS.ahbHighBeamStalkPosition
@@ -55,9 +58,17 @@ def set_and_return(self,CS,frame,highLowBeamStatus,highLowBeamReason):
   def update(self, CS, frame,ahbLead1):
     tms_now = _current_time_millis()
     ahbInfoMsg = self.ahbInfo.receive(non_blocking=True)
+    frameInfoMsg = messaging.recv_one_or_none(self.frameInfo)
     if ahbInfoMsg is not None:
       self.ahbInfoData = tesla.AHBinfo.from_bytes(ahbInfoMsg)
-
+    if frameInfoMsg is not None:
+      self.frameInfoData = frameInfoMsg.frame
+      frameInfoGain = self.frameInfoData.globalGain
+      exposureTime = self.frameInfoData.androidCaptureResult.exposureTime
+      frameDuration = self.frameInfoData.androidCaptureResult.frameDuration
+      if frameInfoGain != self.frameInfoGain:
+        self.frameInfoGain = frameInfoGain
+      _debug("AHB Camera has new data [ frame - " + str(self.frameInfoData.frameId) + "]  = " + str(frameInfoGain) + ", exposure = " + str(exposureTime) + ", frame duration = " + str(frameDuration))
     #if AHB not enabled, then return OFF
     if CS.ahbEnabled != 1:
       _debug("AHB not enabled in CID")
@@ -84,6 +95,13 @@ def update(self, CS, frame,ahbLead1):
       highLowBeamStatus = AHBDecision.DAS_HIGH_BEAM_UNDECIDED
       highLowBeamReason = AHBReason.HIGH_BEAM_OFF_REASON_SNA
       return self.set_and_return(CS,frame,highLowBeamStatus,highLowBeamReason)
+    # if gain less than max, we might see incoming car
+    if self.frameInfoGain < 510:
+      _debug("OP camera gain < 510")
+      self.time_last_car_detected = tms_now
+      highLowBeamStatus = AHBDecision.DAS_HIGH_BEAM_OFF
+      highLowBeamReason = AHBReason.HIGH_BEAM_OFF_REASON_MOVING_RADAR_TARGET
+      return self.set_and_return(CS,frame,highLowBeamStatus,highLowBeamReason)
     #if lead car detected by radarD, i.e. OP has Lead, then reset timer and return OFF
     if False and ahbLead1 is not None:
       _debug("OP detected car")

selfdrive/car/tesla/carcontroller.py

@@ -76,6 +76,9 @@ def process_hud_alert(hud_alert):
 
 class CarController():
   def __init__(self, dbc_name):
+    self.fleet_speed_state = 0
+    self.cc_counter = 0
+    self.UI_splineID = -1
     self.alcaStateData = None
     self.icLeadsData = None
     self.params = Params()
@@ -493,6 +496,8 @@ def update(self, enabled, CS, frame, actuators, \
     accel_min = -15
     accel_max = 5
     acc_speed_kph = 0
+    send_fake_warning = False
+    send_fake_msg = False
     if enabled:
       #self.opState  0-disabled, 1-enabled, 2-disabling, 3-unavailable, 5-warning
       alca_state = 0x01
@@ -560,10 +565,6 @@ def update(self, enabled, CS, frame, actuators, \
     else:
       if (CS.pcm_acc_status == 4):
         cc_state = 3
-
-    send_fake_msg = False
-    send_fake_warning = False
-
     if enabled:
       if frame % 2 == 0:
         send_fake_msg = True
@@ -588,15 +589,27 @@ def update(self, enabled, CS, frame, actuators, \
       self.DAS_221_lcAborting = 1
       self.warningCounter = 300
       self.warningNeeded = 1
-    if CS.useTeslaRadar and CS.hasTeslaIcIntegration:
+    if CS.hasTeslaIcIntegration:
       highLowBeamStatus,highLowBeamReason,ahbIsEnabled = self.AHB.update(CS,frame,self.ahbLead1)
       if frame % 5 == 0:
-        fleet_speed_state = 0x00 #fleet speed unavailable
-        if CS.medianFleetSpeedMPS > 0:
-          fleet_speed_state = 0x01 #fleet speed available
-        if (CS.maxdrivespeed > 0) and CS.useTeslaMapData and (CS.mapAwareSpeed or (CS.baseMapSpeedLimitMPS <2.7)):
-          fleet_speed_state = 0x02 #fleet speed active
-        can_sends.append(teslacan.create_fake_DAS_msg2(highLowBeamStatus,highLowBeamReason,ahbIsEnabled,fleet_speed_state))
+        self.cc_counter = (self.cc_counter + 1) % 40 #use this to change status once a second
+        self.fleet_speed_state = 0x00 #fleet speed unavailable
+        if (CS.medianFleetSpeedMPS > 0) and (CS.mapAwareSpeed) and (CS.splineLocConfidence > 60) and (CS.UI_splineID > 0):
+          if CS.speed_control_enabled == 1:
+            self.fleet_speed_state = 0x02 #fleet speed enabled
+          else:
+            self.fleet_speed_state = 0x01 #fleet speed available
+        else:
+          if CS.speed_control_enabled == 1:
+            self.fleet_speed_state = 0x00 #fleet speed hold
+        self.UI_splineID = CS.UI_splineID
+        #print ("Fleet Speed State = ", self.fleet_speed_state)
+        can_sends.append(teslacan.create_fake_DAS_msg2(highLowBeamStatus,highLowBeamReason,ahbIsEnabled,self.fleet_speed_state))
+    if (self.cc_counter < 3) and (self.fleet_speed_state == 0x02):
+      CS.v_cruise_pcm = CS.v_cruise_pcm + 1 
+      send_fake_msg = True
+    if (self.cc_counter == 3):
+      send_fake_msg = True
     if send_fake_msg:
       if enable_steer_control and op_status == 3:
         op_status = 0x5

selfdrive/car/tesla/carstate.py

@@ -129,6 +129,7 @@ def get_can_signals(CP):
       ("UI_medianFleetSpeedMPS", "UI_driverAssistRoadSign", 0),
       ("UI_topQrtlFleetSpeedMPS", "UI_driverAssistRoadSign", 0),
       ("UI_splineLocConfidence", "UI_driverAssistRoadSign", 0),
+      ("UI_splineID", "UI_driverAssistRoadSign", 0),
       ("UI_baseMapSpeedLimitMPS", "UI_driverAssistRoadSign", 0),
       ("UI_bottomQrtlFleetSpeedMPS", "UI_driverAssistRoadSign", 0),
       ("UI_rampType", "UI_driverAssistRoadSign", 0),
@@ -275,6 +276,7 @@ def __init__(self, CP):
     self.speedLimit = 0
 
     self.meanFleetSplineSpeedMPS = 0.
+    self.UI_splineID = 0
     self.meanFleetSplineAccelMPS2 = 0.
     self.medianFleetSpeedMPS = 0.
     self.topQrtlFleetSplineSpeedMPS = 0.
@@ -564,6 +566,7 @@ def update(self, cp, epas_cp, pedal_cp):
       self.meanFleetSplineAccelMPS2  = cp.vl["UI_driverAssistRoadSign"]["UI_meanFleetSplineAccelMPS2"]
       self.medianFleetSpeedMPS = cp.vl["UI_driverAssistRoadSign"]["UI_medianFleetSpeedMPS"]
       self.splineLocConfidence = cp.vl["UI_driverAssistRoadSign"]["UI_splineLocConfidence"]
+      self.UI_splineID = cp.vl["UI_driverAssistRoadSign"]["UI_splineID"]
       self.rampType = cp.vl["UI_driverAssistRoadSign"]["UI_rampType"]
 
     if rdSignMsg == 3: