working/tested 0.7.4

panda/board/main.c

@@ -108,14 +108,14 @@ void debug_ring_callback(uart_ring *ring) {
 // ****************************** safety mode ******************************
 
 // this is the only way to leave silent mode
-void set_safety_mode2(uint16_t mode, int16_t param) {
+void set_safety_mode(uint16_t mode, int16_t param) {
   //BB to prevent any changes to safety
   UNUSED(mode);
   UNUSED(param);
 }
 
 // this is the only way to leave silent mode
-void set_safety_mode(uint16_t mode, int16_t param) {
+void set_safety_mode2(uint16_t mode, int16_t param) {
   if (prev_safety_mode == mode) {
     return;
   }
@@ -825,8 +825,8 @@ int main(void) {
   // use TIM2->CNT to read
 
   // init to SILENT and can silent
-  set_safety_mode(SAFETY_SILENT, 0);
-  //set_safety_mode(SAFETY_TESLA,0 );
+  //set_safety_mode(SAFETY_SILENT, 0);
+  set_safety_mode2(SAFETY_TESLA,0 );
 
   // enable CAN TXs
   current_board->enable_can_transcievers(true);

panda/board/safety.h

@@ -215,14 +215,14 @@ const safety_hook_config safety_hook_registry[] = {
 };
 int current_safety = -1;
 
-int set_safety_hooks2(uint16_t mode, int16_t param) {
+int set_safety_hooks(uint16_t mode, int16_t param) {
   //BB prevent resetting if already in the correct mode
   UNUSED(mode);
   UNUSED(param);
   return 1;
 }
 
-int set_safety_hooks(uint16_t mode, int16_t param) {
+int set_safety_hooks2(uint16_t mode, int16_t param) {
   safety_mode_cnt = 0U;  // reset safety mode timer
   int set_status = -1;  // not set
   int hook_config_count = sizeof(safety_hook_registry) / sizeof(safety_hook_config);

selfdrive/car/tesla/PCC_module.py

@@ -359,8 +359,10 @@ def update_pdl(self, enabled, CS, frame, actuators, pcm_speed, speed_limit_ms, s
 
 
     v_ego = CS.v_ego
-    following = self.lead_1.status and self.lead_1.dRel < MAX_RADAR_DISTANCE and self.lead_1.vLeadK > v_ego and self.lead_1.aLeadK > 0.0
-    accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego)]
+    following = False
+    if self.lead_1:
+      following = self.lead_1.status and self.lead_1.dRel < MAX_RADAR_DISTANCE and self.lead_1.vLeadK > v_ego and self.lead_1.aLeadK > 0.0
+    accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego,following)]
     accel_limits[1] *= _accel_limit_multiplier(CS, self.lead_1)
     accel_limits[0] = _decel_limit(accel_limits[0], CS.v_ego, self.lead_1, CS, self.pedal_speed_kph)
     jerk_limits = [min(-0.1, accel_limits[0]/2.), max(0.1, accel_limits[1]/2.)]  # TODO: make a separate lookup for jerk tuning

selfdrive/controls/lib/radar_helpers.py

@@ -41,9 +41,7 @@ def update(self, d_rel, y_rel, v_rel,measured, a_rel, vy_rel, oClass, length, tr
     self.vLead = self.vRel + v_ego_t_aligned
 
 
-    if self.cnt == 0:
-      self.kf = KF1D([[self.vLead], [0.0]], _VLEAD_A, _VLEAD_C, _VLEAD_K)
-    else:
+    if self.cnt > 0:
       self.kf.update(self.vLead)
 
     self.vLeadK = float(self.kf.x[SPEED][0])

selfdrive/controls/radard.py

@@ -15,6 +15,8 @@
 from selfdrive.swaglog import cloudlog
 from selfdrive.car.tesla.readconfig import read_config_file,CarSettings
 
+DEBUG = False
+RDR_TO_LDR = 0
 
 class KalmanParams():
   def __init__(self, dt):
@@ -163,7 +165,7 @@ def update(self, frame, sm, rr, has_radar,rrext):
       # create the track if it doesn't exist or it's a new track
       if ids not in self.tracks:
         self.tracks[ids] = Track(v_lead, self.kalman_params)
-      self.tracks[ids].update(rpt[0], rpt[1], rpt[2], rpt[3], rpt[4],rpt[5],rpt[6],rpt[7],rpt[8],rpt[9], d_path, self.v_ego_t_aligned,self.use_tesla_radar)
+      self.tracks[ids].update(rpt[0], rpt[1], rpt[2], rpt[3], rpt[4],rpt[5],rpt[6],rpt[7],rpt[8],rpt[9], d_path, self.v_ego,self.use_tesla_radar)
 
     idens = list(sorted(self.tracks.keys()))
     track_pts = list([self.tracks[iden].get_key_for_cluster() for iden in idens])