RSA, Distance button and briskspirit longcontrol (#762)

* toyota rsa logic

* no arne messaging and add opedit

* add gas_press, smartspeed and spdval1 to carstate

* more fixes

* new_message

* thats right we dont have anglelater code yet.

* revert to working mapd code? and pylint ignore mapd

* flake8 ignore.

* flake8 e701 fix

* control df with distance button?

* wrong struct

* Update carstate.py

* update missing

* start MessagedArneThread

* Update carstate.py

* pubmaster?

* message

* update carstate.py

* no .status

* pm

* use dp_dynamic_follow to send DF status

* sntax

* encoding='utf8'

* Update carstate.py

* Update carstate.py

* pyopencl pipenv

* change the order so we can display right thing with distance toggle

* doesnt work well with dg

* Revert "change the order so we can display right thing with distance toggle"

This reverts commit ee4522a.

* turn off distance button for now.

* Parametrize MIN_CAN_SPEED in car interfaces

* Parametrize stoppingBrakeRate

* Parametrize startingBrakeRate

* add the parameter to cereal .

* Update dp_common.py

Co-authored-by: Arne Schwarck <arneschwarck@gmail.com>
Co-authored-by: George Hotz <george@comma.ai>
Co-authored-by: Igor <briskspirit@users.noreply.github.com>

Pipfile

@@ -77,6 +77,7 @@ torch = "*"
 torchsummary = "*"
 blosc = "==1.9.2"
 segmentation-models-pytorch = "==0.1.2"
+pyopencl = "*"
 
 [packages]
 atomicwrites = "*"

cereal/car.capnp

@@ -417,6 +417,9 @@ struct CarParams {
   steerRateCost @33 :Float32; # Lateral MPC cost on steering rate
   steerControlType @34 :SteerControlType;
   radarOffCan @35 :Bool; # True when radar objects aren't visible on CAN
+  minSpeedCan @51 :Float32; # Minimum vehicle speed from CAN (below this value drops to 0)
+  stoppingBrakeRate @52 :Float32; # brake_travel/s while trying to stop
+  startingBrakeRate @53 :Float32; # brake_travel/s while releasing on restart
 
   steerActuatorDelay @36 :Float32; # Steering wheel actuator delay in seconds
   openpilotLongitudinalControl @37 :Bool; # is openpilot doing the longitudinal control?

common/op_params.py

@@ -94,15 +94,15 @@ def __init__(self):
                         #'hotspot_on_boot': Param(False, bool, 'Enable Hotspot On Boot'),
                         'keep_openpilot_engaged': Param(True, bool, 'True is stock behavior in this fork. False lets you use the brake and cruise control stalk to disengage as usual'),
                         #'lat_d': Param(9.0, VT.number, 'The lateral derivative gain, default is 9.0 for TSS2 Corolla. This is active at all speeds', live=True),
-                        #'limit_rsa': Param(False, bool, "Switch off RSA above rsa_max_speed"),
+                        'limit_rsa': Param(False, bool, "Switch off RSA above rsa_max_speed"),
                         #'ludicrous_mode': Param(False, bool, 'Double overall acceleration!'),
                         'mpc_offset': Param(0.0, VT.number, 'Offset model braking by how many m/s. Lower numbers equals more model braking', live=True),
                         #'NoctuaMode': Param(False, bool, 'Noctua Fan are super quite and they run at full speed at all time.'),
                         'offset_limit': Param(0, VT.number, 'Speed at which apk percent offset will work in m/s'),
                         'osm': Param(True, bool, 'Whether to use OSM for drives'),
                         'prius_pid': Param(False, bool, 'This enables the PID lateral controller with new a experimental derivative tune\nFalse: stock INDI, True: TSS2-tuned PID'),
                         'rolling_stop': Param(False, bool, 'If you do not want stop signs to go down to 0 kph enable this for 9kph slow down'),
-                        #'rsa_max_speed': Param(24.5, VT.number, 'Speed limit to ignore RSA in m/s'),
+                        'rsa_max_speed': Param(24.5, VT.number, 'Speed limit to ignore RSA in m/s'),
                         'smart_speed': Param(True, bool, 'Whether to use Smart Speed for drives above smart_speed_max_vego'),
                         'smart_speed_max_vego': Param(26.8, VT.number, 'Speed limit to ignore Smartspeed in m/s'),
                         #'spairrowtuning': Param(False, bool, 'INDI Tuning for Corolla Tss2, set steer_up_15 param to True and flash panda'),

selfdrive/car/honda/interface.py

@@ -18,6 +18,7 @@
 ButtonType = car.CarState.ButtonEvent.Type
 EventName = car.CarEvent.EventName
 
+
 def compute_gb_honda(accel, speed):
   creep_brake = 0.0
   creep_speed = 2.3

selfdrive/car/hyundai/interface.py

@@ -104,9 +104,9 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
       ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
     elif candidate in [CAR.IONIQ, CAR.IONIQ_EV_LTD]:
       ret.lateralTuning.pid.kf = 0.00006
-      ret.mass = 1490. + STD_CARGO_KG   #weight per hyundai site https://www.hyundaiusa.com/ioniq-electric/specifications.aspx
+      ret.mass = 1490. + STD_CARGO_KG  # weight per hyundai site https://www.hyundaiusa.com/ioniq-electric/specifications.aspx
       ret.wheelbase = 2.7
-      ret.steerRatio = 13.73   #Spec
+      ret.steerRatio = 13.73  # Spec
       tire_stiffness_factor = 0.385
       ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
       ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
@@ -238,7 +238,7 @@ def update(self, c, can_strings, dragonconf):
     ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
 
     events = self.create_common_events(ret)
-    #TODO: addd abs(self.CS.angle_steers) > 90 to 'steerTempUnavailable' event
+    # TODO: addd abs(self.CS.angle_steers) > 90 to 'steerTempUnavailable' event
 
     if dragonconf.dpAtl:
       if ret.vEgo < self.CP.minSteerSpeed:

selfdrive/car/interfaces.py

@@ -72,6 +72,9 @@ def get_std_params(candidate, fingerprint, has_relay):
     ret.brakeMaxV = [1.]
     ret.openpilotLongitudinalControl = False
     ret.startAccel = 1.2
+    ret.minSpeedCan = 0.3
+    ret.stoppingBrakeRate = 0.2 # brake_travel/s while trying to stop
+    ret.startingBrakeRate = 0.8 # brake_travel/s while releasing on restart
     ret.stoppingControl = False
     ret.longitudinalTuning.deadzoneBP = [0.]
     ret.longitudinalTuning.deadzoneV = [0.]

selfdrive/car/mazda/interface.py

@@ -54,7 +54,6 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
       ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.19], [0.019]]
       ret.lateralTuning.pid.kf = 0.00006
 
-
     # No steer below disable speed
     ret.minSteerSpeed = LKAS_LIMITS.DISABLE_SPEED * CV.KPH_TO_MS
 

selfdrive/car/nissan/interface.py

@@ -5,6 +5,7 @@
 from selfdrive.car.interfaces import CarInterfaceBase
 from common.dp_common import common_interface_atl, common_interface_get_params_lqr
 
+
 class CarInterface(CarInterfaceBase):
   def __init__(self, CP, CarController, CarState):
     super().__init__(CP, CarController, CarState)

selfdrive/car/subaru/interface.py

@@ -66,7 +66,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
       ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.01, 0.065, 0.2], [0.001, 0.015, 0.025]]
 
     if candidate in [CAR.FORESTER_PREGLOBAL, CAR.OUTBACK_PREGLOBAL_2018]:
-      ret.safetyParam = 1 # Outback 2018-2019 and Forester have reversed driver torque signal
+      ret.safetyParam = 1  # Outback 2018-2019 and Forester have reversed driver torque signal
       ret.mass = 1568 + STD_CARGO_KG
       ret.wheelbase = 2.67
       ret.centerToFront = ret.wheelbase * 0.5

selfdrive/car/toyota/carcontroller.py

@@ -71,15 +71,15 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
       apply_accel = actuators.gas - actuators.brake
 
     # dynamic acceleration
-    dynamic_accel_max = ACCEL_MAX
-    if CS.out.vEgo > 5.5:
-      if CS.out.vEgo >= 20:
-        dynamic_accel_max = 0.5
-      else:
-        dynamic_accel_max = ACCEL_MAX - (((CS.out.vEgo - 5.5)/ 14.5))
+    #dynamic_accel_max = ACCEL_MAX
+    #if CS.out.vEgo > 5.5:
+      #if CS.out.vEgo >= 20:
+        #dynamic_accel_max = 0.5
+      #else:
+        #dynamic_accel_max = ACCEL_MAX - (((CS.out.vEgo - 5.5)/ 14.5))
 
     apply_accel, self.accel_steady = accel_hysteresis(apply_accel, self.accel_steady, enabled)
-    apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, dynamic_accel_max)
+    apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX)
 
     if CS.CP.enableGasInterceptor:
       if CS.out.gasPressed:

selfdrive/car/toyota/carstate.py

@@ -6,6 +6,13 @@
 from selfdrive.config import Conversions as CV
 from selfdrive.car.toyota.values import CAR, DBC, STEER_THRESHOLD, TSS2_CAR, NO_STOP_TIMER_CAR
 from common.params import Params
+import cereal.messaging as messaging
+from common.travis_checker import travis
+from common.op_params import opParams
+
+op_params = opParams()
+rsa_max_speed = op_params.get('rsa_max_speed')
+limit_rsa = op_params.get('limit_rsa')
 
 
 class CarState(CarStateBase):
@@ -22,8 +29,14 @@ def __init__(self, CP):
     self.setspeedcounter = 0
     self.pcm_acc_active = False
     self.main_on = False
+    self.gas_pressed = False
+    self.smartspeed = 0
+    self.spdval1 = 0
     self.distance = 0
-
+    #self.read_distance_lines = 0
+    if not travis:
+      self.pm = messaging.PubMaster(['liveTrafficData'])
+      self.sm = messaging.SubMaster(['liveMapData'])#',latControl',])
     # On NO_DSU cars but not TSS2 cars the cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE']
     # is zeroed to where the steering angle is at start.
     # Need to apply an offset as soon as the steering angle measurements are both received
@@ -79,6 +92,15 @@ def update(self, cp, cp_cam):
     ret.steeringRate = cp.vl["STEER_ANGLE_SENSOR"]['STEER_RATE']
     can_gear = int(cp.vl["GEAR_PACKET"]['GEAR'])
     ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))
+
+    #if self.read_distance_lines != cp.vl["PCM_CRUISE_SM"]['DISTANCE_LINES']:
+      #self.read_distance_lines = cp.vl["PCM_CRUISE_SM"]['DISTANCE_LINES']
+      #Params().put('dp_dynamic_follow', str(int(max(self.read_distance_lines - 1, 0))))
+
+    if not travis:
+      self.sm.update(0)
+      self.smartspeed = self.sm['liveMapData'].speedLimit
+
     ret.leftBlinker = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 1
     ret.rightBlinker = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 2
 
@@ -131,6 +153,54 @@ def update(self, cp, cp_cam):
       ret.leftBlindspot = (cp.vl["BSM"]['L_ADJACENT'] == 1) or (cp.vl["BSM"]['L_APPROACHING'] == 1)
       ret.rightBlindspot = (cp.vl["BSM"]['R_ADJACENT'] == 1) or (cp.vl["BSM"]['R_APPROACHING'] == 1)
 
+
+    self.tsgn1 = cp_cam.vl["RSA1"]['TSGN1']
+    if self.spdval1 != cp_cam.vl["RSA1"]['SPDVAL1']:
+      self.rsa_ignored_speed = 0
+    self.spdval1 = cp_cam.vl["RSA1"]['SPDVAL1']
+
+    self.splsgn1 = cp_cam.vl["RSA1"]['SPLSGN1']
+    self.tsgn2 = cp_cam.vl["RSA1"]['TSGN2']
+    #self.spdval2 = cp_cam.vl["RSA1"]['SPDVAL2']
+
+    self.splsgn2 = cp_cam.vl["RSA1"]['SPLSGN2']
+    self.tsgn3 = cp_cam.vl["RSA2"]['TSGN3']
+    self.splsgn3 = cp_cam.vl["RSA2"]['SPLSGN3']
+    self.tsgn4 = cp_cam.vl["RSA2"]['TSGN4']
+    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) and not (self.spdval1 == 35 and self.tsgn1 == 1) and self.rsa_ignored_speed != self.spdval1:
+      dat =  messaging.new_message('liveTrafficData')
+      if self.spdval1 > 0:
+        dat.liveTrafficData.speedLimitValid = True
+        if self.tsgn1 == 36:
+          dat.liveTrafficData.speedLimit = self.spdval1 * 1.60934
+        elif self.tsgn1 == 1:
+          dat.liveTrafficData.speedLimit = self.spdval1
+        else:
+          dat.liveTrafficData.speedLimit = 0
+      else:
+        dat.liveTrafficData.speedLimitValid = False
+      #if self.spdval2 > 0:
+      #  dat.liveTrafficData.speedAdvisoryValid = True
+      #  dat.liveTrafficData.speedAdvisory = self.spdval2
+      #else:
+      dat.liveTrafficData.speedAdvisoryValid = False
+      if limit_rsa and rsa_max_speed < ret.vEgo:
+        dat.liveTrafficData.speedLimitValid = False
+      if not travis:
+        self.pm.send('liveTrafficData', dat)
+    if ret.gasPressed and not self.gas_pressed:
+      self.engaged_when_gas_was_pressed = self.pcm_acc_active
+    if ((ret.gasPressed) or (self.gas_pressed and not ret.gasPressed)) and self.engaged_when_gas_was_pressed and ret.vEgo > self.smartspeed:
+      self.rsa_ignored_speed = self.spdval1
+      dat = messaging.new_message('liveTrafficData')
+      dat.liveTrafficData.speedLimitValid = True
+      dat.liveTrafficData.speedLimit = ret.vEgo * 3.6
+      if not travis:
+        self.pm.send('liveTrafficData', dat)
+    self.gas_pressed = ret.gasPressed
+
     return ret
 
   @staticmethod
@@ -236,6 +306,16 @@ def get_cam_can_parser(CP):
     signals = [
       ("FORCE", "PRE_COLLISION", 0),
       ("PRECOLLISION_ACTIVE", "PRE_COLLISION", 0),
+      ("TSGN1", "RSA1", 0),
+      ("SPDVAL1", "RSA1", 0),
+      ("SPLSGN1", "RSA1", 0),
+      ("TSGN2", "RSA1", 0),
+      #("SPDVAL2", "RSA1", 0),
+      ("SPLSGN2", "RSA1", 0),
+      ("TSGN3", "RSA2", 0),
+      ("SPLSGN3", "RSA2", 0),
+      ("TSGN4", "RSA2", 0),
+      ("SPLSGN4", "RSA2", 0),
       ("DISTANCE", "ACC_CONTROL", 0),
     ]
 

selfdrive/car/toyota/toyotacan.py

@@ -76,8 +76,8 @@ def create_ui_command(packer, steer, chime, left_line, right_line, left_lane_dep
     "SET_ME_X02": 0x02,
     "SET_ME_X01": 1,
     "SET_ME_X01_2": 1,
-    "REPEATED_BEEPS": 1 if steer else 0,
-    "TWO_BEEPS": 1 if chime else 0,
-    "LDA_ALERT": 1 if left_lane_depart or right_lane_depart else 0,
+    "REPEATED_BEEPS": 0,
+    "TWO_BEEPS": chime,
+    "LDA_ALERT": steer,
   }
   return packer.make_can_msg("LKAS_HUD", 0, values)

selfdrive/car/volkswagen/interface.py

@@ -6,6 +6,7 @@
 
 EventName = car.CarEvent.EventName
 
+
 class CarInterface(CarInterfaceBase):
   def __init__(self, CP, CarController, CarState):
     super().__init__(CP, CarController, CarState)

selfdrive/controls/lib/longcontrol.py

@@ -7,25 +7,23 @@
 LongCtrlState = log.ControlsState.LongControlState
 
 STOPPING_EGO_SPEED = 0.5
-MIN_CAN_SPEED = 0.250  # TODO: parametrize this in car interface
-STOPPING_TARGET_SPEED = MIN_CAN_SPEED + 0.01
+STOPPING_TARGET_SPEED_OFFSET = 0.01
 STARTING_TARGET_SPEED = 0.5
 BRAKE_THRESHOLD_TO_PID = 0.2
 
-STOPPING_BRAKE_RATE = 0.2  # brake_travel/s while trying to stop
-STARTING_BRAKE_RATE = 0.8  # brake_travel/s while releasing on restart
 BRAKE_STOPPING_TARGET = 0.8  # apply at least this amount of brake to maintain the vehicle stationary
 
 RATE = 100.0
 
 
 def long_control_state_trans(active, long_control_state, v_ego, v_target, v_pid,
-                             output_gb, brake_pressed, cruise_standstill, stop):
+                             output_gb, brake_pressed, cruise_standstill, min_speed_can, stop):
   """Update longitudinal control state machine"""
-  stopping_condition = stop or (v_ego < 2.0 and cruise_standstill) or \
+  stopping_target_speed = min_speed_can + STOPPING_TARGET_SPEED_OFFSET
+  stopping_condition = (v_ego < 2.0 and cruise_standstill) or \
                        (v_ego < STOPPING_EGO_SPEED and
-                        ((v_pid < STOPPING_TARGET_SPEED and v_target < STOPPING_TARGET_SPEED) or
-                        brake_pressed))
+                        ((v_pid < stopping_target_speed and v_target < stopping_target_speed) or
+                         brake_pressed))
 
   starting_condition = v_target > STARTING_TARGET_SPEED and not cruise_standstill
 
@@ -98,9 +96,9 @@ def update(self, active, CS, v_target, v_target_future, a_target, CP, sm, hasLea
       stop = False
     self.long_control_state = long_control_state_trans(active, self.long_control_state, CS.vEgo,
                                                        v_target_future, self.v_pid, output_gb,
-                                                       CS.brakePressed, CS.cruiseState.standstill, stop)
+                                                       CS.brakePressed, CS.cruiseState.standstill, CP.minSpeedCan, stop)
 
-    v_ego_pid = max(CS.vEgo, MIN_CAN_SPEED)  # Without this we get jumps, CAN bus reports 0 when speed < 0.3
+    v_ego_pid = max(CS.vEgo, CP.minSpeedCan)  # Without this we get jumps, CAN bus reports 0 when speed < 0.3
 
     if self.long_control_state == LongCtrlState.off or CS.gasPressed or CS.brakePressed:
       self.v_pid = v_ego_pid
@@ -148,7 +146,7 @@ def update(self, active, CS, v_target, v_target_future, a_target, CP, sm, hasLea
       if hasLead:
         factor = interp(dRel,[2.0,3.0,4.0,5.0,6.0,7.0,8.0], [3.0,2.1,1.5,1.0,0.6,0.29,0.0])
       if not CS.standstill or output_gb > -BRAKE_STOPPING_TARGET:
-        output_gb -= STOPPING_BRAKE_RATE / RATE * factor
+        output_gb -= CP.stoppingBrakeRate / RATE * factor
       output_gb = clip(output_gb, -brake_max, gas_max)
 
       self.reset(CS.vEgo)
@@ -159,7 +157,7 @@ def update(self, active, CS, v_target, v_target_future, a_target, CP, sm, hasLea
       if hasLead:
         factor = interp(dRel,[0.0,2.0,4.0,6.0], [0.0,0.5,1.0,2.0])
       if output_gb < -0.2:
-        output_gb += STARTING_BRAKE_RATE / RATE * factor
+        output_gb += CP.startingBrakeRate / RATE * factor
       self.v_pid = CS.vEgo
       self.pid.reset()
 

selfdrive/controls/lib/planner.py

@@ -13,7 +13,7 @@
 from selfdrive.swaglog import cloudlog
 from selfdrive.config import Conversions as CV
 from selfdrive.controls.lib.speed_smoother import speed_smoother
-from selfdrive.controls.lib.longcontrol import LongCtrlState, MIN_CAN_SPEED
+from selfdrive.controls.lib.longcontrol import LongCtrlState
 from selfdrive.controls.lib.fcw import FCWChecker
 from selfdrive.controls.lib.long_mpc import LongitudinalMpc
 from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX
@@ -35,7 +35,7 @@
 # lookup tables VS speed to determine min and max accels in cruise
 # make sure these accelerations are smaller than mpc limits
 _A_CRUISE_MIN_V = [-1.0, -.8, -.67, -.5, -.30]
-_A_CRUISE_MIN_BP = [   0., 5.,  10., 20.,  40.]
+_A_CRUISE_MIN_BP = [  0.,  5.,  10., 20.,  40.]
 
 # need fast accel at very low speed for stop and go
 # make sure these accelerations are smaller than mpc limits
@@ -349,7 +349,7 @@ def update(self, sm, pm, CP, VM, PP):
     else:
       starting = long_control_state == LongCtrlState.starting
       a_ego = min(sm['carState'].aEgo, 0.0)
-      reset_speed = MIN_CAN_SPEED if starting else v_ego
+      reset_speed = self.CP.minSpeedCan if starting else v_ego
       reset_accel = self.CP.startAccel if starting else a_ego
       self.v_acc = reset_speed
       self.a_acc = reset_accel