066 updates

cereal/arne182.capnp

@@ -33,7 +33,6 @@ struct PhantomData {
   status @0 :Bool;
   speed @1 :Float32;
   angle @2 :Float32;
-  time @3 :Float32;
 }
 
 struct ManagerData {

panda/VERSION

@@ -1 +1 @@
-v1.6.9
+v1.5.9

panda/board/safety/safety_honda.h

@@ -114,9 +114,9 @@ static int honda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
 
   // disallow actuator commands if gas or brake (with vehicle moving) are pressed
   // and the the latching controls_allowed flag is True
-  int pedal_pressed = honda_gas_prev || (gas_interceptor_prev > HONDA_GAS_INTERCEPTOR_THRESHOLD) ||
-                      (honda_brake_pressed_prev && honda_moving);
-  bool current_controls_allowed = controls_allowed && !(pedal_pressed);
+  //int pedal_pressed = honda_gas_prev || (gas_interceptor_prev > HONDA_GAS_INTERCEPTOR_THRESHOLD) ||
+                      //(honda_brake_pressed_prev && honda_moving);
+  bool current_controls_allowed = controls_allowed;
 
   // BRAKE: safety check
   if ((addr == 0x1FA) && (bus == 0)) {

panda/board/safety/safety_toyota.h

@@ -14,7 +14,7 @@ const uint32_t TOYOTA_RT_INTERVAL = 250000;    // 250ms between real time checks
 
 // longitudinal limits
 const int TOYOTA_MAX_ACCEL = 4000;        // 4.0 m/s2
-const int TOYOTA_MIN_ACCEL = -4000;       // 4.0 m/s2
+const int TOYOTA_MIN_ACCEL = -8000;       // 8.0 m/s2
 
 const int TOYOTA_GAS_INTERCEPTOR_THRESHOLD = 475;  // ratio between offset and gain from dbc file
 

panda/tests/safety/test_toyota.py

@@ -9,7 +9,7 @@
 MAX_TORQUE = 1500
 
 MAX_ACCEL = 4000
-MIN_ACCEL = -4000
+MIN_ACCEL = -8000
 
 MAX_RT_DELTA = 375
 RT_INTERVAL = 250000

selfdrive/can/common.pxd

@@ -17,7 +17,9 @@ cdef extern from "common.h":
     HONDA_COUNTER,
     TOYOTA_CHECKSUM,
     PEDAL_CHECKSUM,
-    PEDAL_COUNTER
+    PEDAL_COUNTER,
+    VOLKSWAGEN_CHECKSUM,
+    VOLKSWAGEN_COUNTER
 
   cdef struct Signal:
     const char* name

selfdrive/can/dbc_template.cc

@@ -25,7 +25,7 @@ const Signal sigs_{{address}}[] = {
       .type = SignalType::HONDA_COUNTER,
       {% elif checksum_type == "toyota" and sig.name == "CHECKSUM" %}
       .type = SignalType::TOYOTA_CHECKSUM,
-	    {% elif checksum_type == "volkswagen" and sig.name == "CHECKSUM" %}
+      {% elif checksum_type == "volkswagen" and sig.name == "CHECKSUM" %}
       .type = SignalType::VOLKSWAGEN_CHECKSUM,
       {% elif checksum_type == "volkswagen" and sig.name == "COUNTER" %}
       .type = SignalType::VOLKSWAGEN_COUNTER,

selfdrive/can/parser.cc

@@ -55,7 +55,6 @@ bool MessageState::parse(uint64_t sec, uint16_t ts_, uint8_t * dat) {
       }
     } else if (sig.type == SignalType::VOLKSWAGEN_COUNTER) {
         if (!update_counter_generic(tmp, sig.b2)) {
-        INFO("0x%X CHECKSUM FAIL\n", address);
         return false;
       }
     } else if (sig.type == SignalType::PEDAL_CHECKSUM) {
@@ -84,7 +83,7 @@ bool MessageState::update_counter_generic(int64_t v, int cnt_size) {
   if (((old_counter+1) & ((1 << cnt_size) -1)) != v) {
     counter_fail += 1;
     if (counter_fail > 1) {
-      INFO("%X COUNTER FAIL %d -- %d vs %d\n", address, counter_fail, old_counter, (int)v);
+      INFO("0x%X COUNTER FAIL %d -- %d vs %d\n", address, counter_fail, old_counter, (int)v);
     }
     if (counter_fail >= MAX_BAD_COUNTER) {
       return false;
@@ -125,7 +124,7 @@ CANParser::CANParser(int abus, const std::string& dbc_name,
       }
     }
     if (!msg) {
-      fprintf(stderr, "CANParser: could not find message 0x%X in dnc %s\n", op.address, dbc_name.c_str());
+      fprintf(stderr, "CANParser: could not find message 0x%X in DBC %s\n", op.address, dbc_name.c_str());
       assert(false);
     }
 
@@ -193,7 +192,7 @@ void CANParser::UpdateValid(uint64_t sec) {
     const auto& state = kv.second;
     if (state.check_threshold > 0 && (sec - state.seen) > state.check_threshold) {
       if (state.seen > 0) {
-        DEBUG("%X TIMEOUT\n", state.address);
+        DEBUG("0x%X TIMEOUT\n", state.address);
       }
       can_valid = false;
     }

selfdrive/car/car_helpers.py

@@ -134,7 +134,7 @@ def fingerprint(logcan, sendcan, has_relay):
 
     frame += 1
 
-  cloudlog.warning("fingerprinted %s", car_fingerprint)
+  cloudlog.warning("fingerprinted {}".format({car_fingerprint: finger[0]}))
   params.put("CachedFingerprint", json.dumps([car_fingerprint, {int(key): value for key, value in finger[0].items()}]))
   return car_fingerprint, finger, vin
 

selfdrive/car/toyota/carcontroller.py

@@ -8,7 +8,7 @@
                                            create_acc_cancel_command, create_fcw_command
 from selfdrive.car.toyota.values import CAR, ECU, STATIC_MSGS, TSS2_CAR, SteerLimitParams
 from selfdrive.can.packer import CANPacker
-from selfdrive.phantom import Phantom
+from selfdrive.phantom.phantom import Phantom
 
 VisualAlert = car.CarControl.HUDControl.VisualAlert
 
@@ -98,7 +98,7 @@ def __init__(self, dbc_name, car_fingerprint, enable_camera, enable_dsu, enable_
     self.standstill_req = False
     self.angle_control = False
     self.phantom = Phantom()
-
+    self.fcw_countdown = 0
     self.steer_angle_enabled = False
     self.ipas_reset_counter = 0
     self.last_fault_frame = -200
@@ -237,7 +237,26 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
                                                  ECU.APGS in self.fake_ecus))
     elif ECU.APGS in self.fake_ecus:
       can_sends.append(create_ipas_steer_command(self.packer, 0, 0, True))
-
+
+    # ui mesg is at 100Hz but we send asap if:
+    # - there is something to display
+    # - there is something to stop displaying
+    alert_out = process_hud_alert(hud_alert)
+    steer, fcw = alert_out
+
+    if fcw:
+      self.fcw_countdown = 200
+    if self.fcw_countdown > 0:
+      apply_accel = -8.0
+      self.fcw_countdown = self.fcw_countdown - 1
+
+    if (any(alert_out) and not self.alert_active) or \
+       (not any(alert_out) and self.alert_active):
+      send_ui = True
+      self.alert_active = not self.alert_active
+    else:
+      send_ui = False
+
     # accel cmd comes from DSU, but we can spam can to cancel the system even if we are using lat only control
     if (frame % 3 == 0 and ECU.DSU in self.fake_ecus) or (pcm_cancel_cmd and ECU.CAM in self.fake_ecus):
       lead = lead or CS.v_ego < 12.    # at low speed we always assume the lead is present do ACC can be engaged
@@ -255,18 +274,7 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
         # This prevents unexpected pedal range rescaling
         can_sends.append(create_gas_command(self.packer, apply_gas, frame//2))
 
-    # ui mesg is at 100Hz but we send asap if:
-    # - there is something to display
-    # - there is something to stop displaying
-    alert_out = process_hud_alert(hud_alert)
-    steer, fcw = alert_out
 
-    if (any(alert_out) and not self.alert_active) or \
-       (not any(alert_out) and self.alert_active):
-      send_ui = True
-      self.alert_active = not self.alert_active
-    else:
-      send_ui = False
 
     # disengage msg causes a bad fault sound so play a good sound instead
     if pcm_cancel_cmd:

selfdrive/car/toyota/carstate.py

@@ -1,8 +1,7 @@
 import numpy as np
 import math
-from selfdrive.services import service_list
-from cereal import arne182
 import selfdrive.messaging as messaging
+import selfdrive.messaging_arne as messaging_arne
 from common.numpy_fast import interp
 from cereal import car
 from common.numpy_fast import mean
@@ -141,8 +140,7 @@ def __init__(self, CP):
     self.Angle = [0, 5, 10, 15,20,25,30,35,60,100,180,270,500]
     self.Angle_Speed = [255,160,100,80,70,60,55,50,40,33,27,17,12]
     if not travis:
-      self.traffic_data_sock = messaging.pub_sock('liveTrafficData')
-      self.arne182Status_sock = messaging.pub_sock('arne182Status')
+      self.arne_pm = messaging_arne.PubMaster(['liveTrafficData', 'arne182Status'])
     # initialize can parser
     self.car_fingerprint = CP.carFingerprint
 
@@ -218,11 +216,12 @@ def update(self, cp, cp_cam):
       self.gasbuttonstatus = 2
     if self.sport_on == 0 and self.econ_on == 0:
       self.gasbuttonstatus = 0
-    msg = arne182.Arne182Status.new_message()
-    msg.gasbuttonstatus = self.gasbuttonstatus
-    msg.readdistancelines = cp.vl["PCM_CRUISE_SM"]['DISTANCE_LINES']
+    msg = messaging_arne.new_message()
+    msg.init('arne182Status')
+    msg.arne182Status.gasbuttonstatus = self.gasbuttonstatus
+    msg.arne182Status.readdistancelines = cp.vl["PCM_CRUISE_SM"]['DISTANCE_LINES']
     if not travis:
-      self.arne182Status_sock.send(msg.to_bytes())
+      self.arne_pm.send('arne182Status', msg)
     if self.CP.carFingerprint == CAR.LEXUS_IS:
       self.main_on = cp.vl["DSU_CRUISE"]['MAIN_ON']
     else:
@@ -313,21 +312,22 @@ def update(self, cp, cp_cam):
     self.splsgn4 = cp_cam.vl["RSA2"]['SPLSGN4']
     self.noovertake = self.tsgn1 == 65 or self.tsgn2 == 65 or self.tsgn3 == 65 or self.tsgn4 == 65 or self.tsgn1 == 66 or self.tsgn2 == 66 or self.tsgn3 == 66 or self.tsgn4 == 66
     if self.spdval1 > 0 or self.spdval2 > 0:
-      dat = arne182.LiveTrafficData.new_message()
+      dat = messaging_arne.new_message()
+      dat.init('liveTrafficData')
       if self.spdval1 > 0:
-        dat.speedLimitValid = True
+        dat.liveTrafficData.speedLimitValid = True
         if self.tsgn1 == 36:
-          dat.speedLimit = self.spdval1 * 1.60934
+          dat.liveTrafficData.speedLimit = self.spdval1 * 1.60934
         elif self.tsgn1 == 1:
-          dat.speedLimit = self.spdval1
+          dat.liveTrafficData.speedLimit = self.spdval1
         else:
-          dat.speedLimit = 0
+          dat.liveTrafficData.speedLimit = 0
       else:
-        dat.speedLimitValid = False
+        dat.liveTrafficData.speedLimitValid = False
       if self.spdval2 > 0:
-        dat.speedAdvisoryValid = True
-        dat.speedAdvisory = self.spdval2
+        dat.liveTrafficData.speedAdvisoryValid = True
+        dat.liveTrafficData.speedAdvisory = self.spdval2
       else:
-        dat.speedAdvisoryValid = False
+        dat.liveTrafficData.speedAdvisoryValid = False
       if not travis:
-        self.traffic_data_sock.send(dat.to_bytes())
+        self.arne_pm.send('liveTrafficData', dat)

selfdrive/car/toyota/interface.py

@@ -98,7 +98,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay
       ret.lateralTuning.init('lqr')
 
       ret.lateralTuning.lqr.scale = 1500.0
-      ret.lateralTuning.lqr.ki = 0.05
+      ret.lateralTuning.lqr.ki = 0.06
 
       ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268]
       ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05]

selfdrive/car/volkswagen/carcontroller.py

@@ -181,7 +181,7 @@ def update(self, enabled, CS, frame, actuators, visual_alert, audible_alert, lef
         idx = (CS.graMsgBusCounter + 1) % 16
         can_sends.append(volkswagencan.create_mqb_acc_buttons_control(self.packer_gw, canbus.extended, self.graButtonStatesToSend, CS, idx))
         self.graMsgSentCount += 1
-        if self.graMsgSentCount >= 16:
+        if self.graMsgSentCount >= P.GRA_VBP_COUNT:
           self.graButtonStatesToSend = None
           self.graMsgSentCount = 0
 

selfdrive/car/volkswagen/values.py

@@ -51,13 +51,6 @@ class CAR:
   }],
 }
 
-class ECU:
-  CAM = 0
-
-ECU_FINGERPRINT = {
-  ECU.CAM: [294, 919],                    # HCA_01 Heading Control Assist, LDW_02 Lane Departure Warning
-}
-
 DBC = {
   CAR.GOLF: dbc_dict('vw_mqb_2010', None),
 }

selfdrive/controls/controlsd.py

@@ -4,8 +4,7 @@
 import capnp
 import zmq
 import selfdrive.messaging_arne as messaging_arne
-from selfdrive.services import service_list
-from cereal import car, log, arne182
+from cereal import car, log
 from common.numpy_fast import clip
 from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper, DT_CTRL
 from common.profiler import Profiler
@@ -220,7 +219,7 @@ def state_transition(frame, CS, CP, state, events, soft_disable_timer, v_cruise_
 
 
 def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last,
-                  AM, rk, driver_status, LaC, LoC, read_only, is_metric, cal_perc, sm, poller, arne182Status):
+                  AM, rk, driver_status, LaC, LoC, read_only, is_metric, cal_perc, radar_state, arne_sm):
   """Given the state, this function returns an actuators packet"""
 
   actuators = car.CarControl.Actuators.new_message()
@@ -263,27 +262,16 @@ def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cr
   a_acc_sol = plan.aStart + (dt / LON_MPC_STEP) * (plan.aTarget - plan.aStart)
   v_acc_sol = plan.vStart + dt * (a_acc_sol + plan.aStart) / 2.0
 
-  try:
-    gasinterceptor = CP.enableGasInterceptor
-  except AttributeError:
-    gasinterceptor = False
+
 
   # Gas/Brake PID loop
-  try:
-    leadOne = sm['radarState'].leadOne
-  except KeyError:
-    leadOne = None
-  gasstatus = None
-  for socket, _ in poller.poll(0):
-    if socket is arne182Status:
-      gasstatus = arne182.Arne182Status.from_bytes(socket.recv())
-  if gasstatus is None:
-    gasbuttonstatus = 0
-  else:
-    gasbuttonstatus = gasstatus.gasbuttonstatus
-  actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.brakePressed, CS.standstill, CS.cruiseState.standstill,
-                                              v_cruise_kph, v_acc_sol, plan.vTargetFuture, a_acc_sol, CP, gasinterceptor, gasbuttonstatus, plan.decelForTurn, plan.longitudinalPlanSource,
-                                              leadOne, CS.gasPressed)
+  arne_sm.update(0)
+  gas_button_status = arne_sm['arne182Status'].gasbuttonstatus
+  actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.brakePressed, CS.standstill,
+                                              CS.cruiseState.standstill,
+                                              v_cruise_kph, v_acc_sol, plan.vTargetFuture, a_acc_sol, CP,
+                                              gas_button_status, plan.decelForTurn, plan.longitudinalPlanSource,
+                                              radar_state.leadOne, CS.gasPressed, plan.fcw)
   # Steering PID loop and lateral MPC
   actuators.steer, actuators.steerAngle, lac_log = LaC.update(active, CS.vEgo, CS.steeringAngle, CS.steeringRate, CS.steeringTorqueEps, CS.steeringPressed, CP, path_plan)
 
@@ -460,8 +448,8 @@ def controlsd_thread(sm=None, pm=None, can_sock=None):
     sm = messaging.SubMaster(['thermal', 'health', 'liveCalibration', 'driverMonitoring', 'plan', 'pathPlan', \
                               'gpsLocation', 'radarState'], ignore_alive=['gpsLocation'])
 
-  poller = zmq.Poller()
-  arne182Status = messaging_arne.sub_sock(service_list['arne182Status'].port, poller, conflate=True)  # todo: can we use messaging_arne here?
+  arne_sm = messaging_arne.SubMaster('arne182Status')
+
   if can_sock is None:
     can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100
     can_sock = messaging.sub_sock('can', timeout=can_timeout)
@@ -576,7 +564,7 @@ def controlsd_thread(sm=None, pm=None, can_sock=None):
     # Compute actuators (runs PID loops and lateral MPC)
     actuators, v_cruise_kph, driver_status, v_acc, a_acc, lac_log = \
       state_control(sm.frame, sm.rcv_frame, sm['plan'], sm['pathPlan'], CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk,
-                    driver_status, LaC, LoC, read_only, is_metric, cal_perc, sm, poller, arne182Status)
+                    driver_status, LaC, LoC, read_only, is_metric, cal_perc, sm['radarState'], arne_sm)
 
     prof.checkpoint("State Control")
 

selfdrive/controls/lib/drive_helpers.py

@@ -17,7 +17,7 @@ class MPC_COST_LAT:
 
 
 class MPC_COST_LONG:
-  TTC = 10.0
+  TTC = 17.0
   DISTANCE = 1.0
   ACCELERATION = 10.0
   JERK = 20.0