Merge pull request #3 from arne182/release2

Rec button

selfdrive/car/gm/.gitignore

@@ -0,0 +1,2 @@
+buttons.msg
+buttons.cc.msg

selfdrive/car/gm/carcontroller.py

@@ -4,7 +4,7 @@
 from selfdrive.boardd.boardd import can_list_to_can_capnp
 from selfdrive.car import apply_std_steer_torque_limits
 from selfdrive.car.gm import gmcan
-from selfdrive.car.gm.values import CAR, DBC
+from selfdrive.car.gm.values import CAR, DBC, AccState
 from selfdrive.can.packer import CANPacker
 
 
@@ -24,16 +24,20 @@ def __init__(self, car_fingerprint):
     self.STEER_DRIVER_ALLOWANCE = 50   # allowed driver torque before start limiting
     self.STEER_DRIVER_MULTIPLIER = 4   # weight driver torque heavily
     self.STEER_DRIVER_FACTOR = 100     # from dbc
-    self.NEAR_STOP_BRAKE_PHASE = 0.5 # m/s, more aggressive braking near full stop
+    self.NEAR_STOP_BRAKE_PHASE = 1.5 # m/s, more aggressive braking near full stop
+
+    # Takes case of "Service Adaptive Cruise" and "Service Front Camera"
+    # dashboard messages.
+    self.ADAS_KEEPALIVE_STEP = 100
+    self.CAMERA_KEEPALIVE_STEP = 100
 
-    self.ADAS_KEEPALIVE_STEP = 10
     # pedal lookups, only for Volt
     MAX_GAS = 3072              # Only a safety limit
-    ZERO_GAS = 2048
+    self.ZERO_GAS = 2048
     MAX_BRAKE = 350             # Should be around 3.5m/s^2, including regen
     self.MAX_ACC_REGEN = 1404  # ACC Regen braking is slightly less powerful than max regen paddle
     self.GAS_LOOKUP_BP = [-0.25, 0., 0.5]
-    self.GAS_LOOKUP_V = [self.MAX_ACC_REGEN, ZERO_GAS, MAX_GAS]
+    self.GAS_LOOKUP_V = [self.MAX_ACC_REGEN, self.ZERO_GAS, MAX_GAS]
     self.BRAKE_LOOKUP_BP = [-1., -0.25]
     self.BRAKE_LOOKUP_V = [MAX_BRAKE, 0]
 
@@ -59,12 +63,11 @@ def __init__(self, canbus, car_fingerprint, allow_controls):
     self.pedal_steady = 0.
     self.start_time = sec_since_boot()
     self.chime = 0
-    self.lkas_active = False
-    self.inhibit_steer_for = 0
     self.steer_idx = 0
     self.apply_steer_last = 0
     self.car_fingerprint = car_fingerprint
     self.allow_controls = allow_controls
+    self.lka_icon_status_last = 0
 
     # Setup detection helper. Routes commands to
     # an appropriate CAN bus number.
@@ -77,13 +80,17 @@ def __init__(self, canbus, car_fingerprint, allow_controls):
   def update(self, sendcan, enabled, CS, frame, actuators, \
              hud_v_cruise, hud_show_lanes, hud_show_car, chime, chime_cnt):
     """ Controls thread """
-
+#update custom UI buttons and alerts
+    CS.UE.update_custom_ui()
+    if (frame % 1000 == 0):
+      CS.cstm_btns.send_button_info()
+      CS.UE.uiSetCarEvent(CS.cstm_btns.car_folder,CS.cstm_btns.car_name)
+
     # Sanity check.
     if not self.allow_controls:
       return
 
     P = self.params
-
     # Send CAN commands.
     can_sends = []
     canbus = self.canbus
@@ -100,7 +107,8 @@ def update(self, sendcan, enabled, CS, frame, actuators, \
 
       self.apply_steer_last = apply_steer
       idx = (frame / P.STEER_STEP) % 4
-
+      if CS.cstm_btns.get_button_status("lka") == 0:
+        apply_steer = 0
       if self.car_fingerprint == CAR.VOLT:
         can_sends.append(gmcan.create_steering_control(self.packer_pt,
           canbus.powertrain, apply_steer, idx, lkas_enabled))
@@ -131,12 +139,18 @@ def update(self, sendcan, enabled, CS, frame, actuators, \
       if (frame % 4) == 0:
         idx = (frame / 4) % 4
 
-        at_full_stop = enabled and CS.standstill
-        near_stop = enabled and (CS.v_ego < P.NEAR_STOP_BRAKE_PHASE)
+        car_stopping = apply_gas < P.ZERO_GAS
+        standstill = CS.pcm_acc_status == AccState.STANDSTILL
+        at_full_stop = enabled and standstill and car_stopping
+        near_stop = enabled and (CS.v_ego < P.NEAR_STOP_BRAKE_PHASE) and car_stopping
         can_sends.append(gmcan.create_friction_brake_command(self.packer_ch, canbus.chassis, apply_brake, idx, near_stop, at_full_stop))
 
-        at_full_stop = enabled and CS.standstill
-        can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, canbus.powertrain, apply_gas, idx, enabled, at_full_stop))
+        # Auto-resume from full stop by resetting ACC control
+        acc_enabled = enabled
+        if standstill and not car_stopping:
+          acc_enabled = False
+
+        can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, canbus.powertrain, apply_gas, idx, acc_enabled, at_full_stop))
 
       # Send dashboard UI commands (ACC status), 25hz
       if (frame % 4) == 0:
@@ -158,10 +172,21 @@ def update(self, sendcan, enabled, CS, frame, actuators, \
         can_sends.append(gmcan.create_adas_steering_status(canbus.obstacle, idx))
         can_sends.append(gmcan.create_adas_accelerometer_speed_status(canbus.obstacle, CS.v_ego, idx))
 
-      # Send ADAS keepalive, 10hz
       if frame % P.ADAS_KEEPALIVE_STEP == 0:
         can_sends += gmcan.create_adas_keepalive(canbus.powertrain)
 
+    # Show green icon when LKA torque is applied, and
+    # alarming orange icon when approaching torque limit.
+    # If not sent periodically, LKA icon disappears in about 5 seconds.
+    # Conveniently, sending camera message periodically also works as a keepalive.
+    lka_active = CS.lkas_status == 1
+    lka_critical = abs(actuators.steer) > 0.9
+    lka_icon_status = lka_active + (int(lka_critical) << 1)
+    if frame % P.CAMERA_KEEPALIVE_STEP == 0 \
+        or lka_icon_status != self.lka_icon_status_last:
+      can_sends.append(gmcan.create_lka_icon_command(canbus.sw_gmlan, lka_active, lka_critical))
+      self.lka_icon_status_last = lka_icon_status
+
     # Send chimes
     if self.chime != chime:
       duration = 0x3c

selfdrive/car/gm/carstate.py

@@ -3,6 +3,8 @@
 from common.kalman.simple_kalman import KF1D
 from selfdrive.config import Conversions as CV
 from selfdrive.can.parser import CANParser
+from selfdrive.car.modules.UIBT_module import UIButtons,UIButton
+from selfdrive.car.modules.UIEV_module import UIEvents
 from selfdrive.car.gm.values import DBC, CAR, parse_gear_shifter, \
                                     CruiseButtons, is_eps_status_ok, \
                                     STEER_THRESHOLD
@@ -56,15 +58,43 @@ def __init__(self, CP, canbus):
     self.prev_left_blinker_on = False
     self.right_blinker_on = False
     self.prev_right_blinker_on = False
-
+
+    #BB UIEvents
+    self.UE = UIEvents(self)
+
+    #BB variable for custom buttons
+    self.cstm_btns = UIButtons(self,"Gm","gm")
+
+    #BB pid holder for ALCA
+    self.pid = None
+
+    #BB custom message counter
+    self.custom_alert_counter = -1 #set to 100 for 1 second display; carcontroller will take down to zero
+
     # vEgo kalman filter
     dt = 0.01
     self.v_ego_kf = KF1D(x0=np.matrix([[0.], [0.]]),
                          A=np.matrix([[1., dt], [0., 1.]]),
                          C=np.matrix([1., 0.]),
                          K=np.matrix([[0.12287673], [0.29666309]]))
     self.v_ego = 0.
-
+    #BB init ui buttons
+  def init_ui_buttons(self):
+    btns = []
+    btns.append(UIButton("","",0,"",0))
+    btns.append(UIButton("","",0,"",1))
+    btns.append(UIButton("","",0,"",2))
+    btns.append(UIButton("","",0,"",3))
+    btns.append(UIButton("gas","GAS",0,"",4))
+    btns.append(UIButton("lka","LKA",0,"",5))
+    return btns
+  #BB update ui buttons
+  def update_ui_buttons(self,id,btn_status):
+    if self.cstm_btns.btns[id].btn_status > 0:
+        self.cstm_btns.btns[id].btn_status = btn_status * self.cstm_btns.btns[id].btn_status
+    else:
+        self.cstm_btns.btns[id].btn_status = btn_status
+
   def update(self, pt_cp):
 
     self.can_valid = pt_cp.can_valid

selfdrive/car/gm/gmcan.py

@@ -59,19 +59,16 @@ def create_gas_regen_command(packer, bus, throttle, idx, acc_engaged, at_full_st
   return packer.make_can_msg("ASCMGasRegenCmd", bus, values)
 
 def create_friction_brake_command(packer, bus, apply_brake, idx, near_stop, at_full_stop):
-
-  if apply_brake == 0:
-    mode = 0x1
-  else:
+  mode = 0x1
+  if apply_brake > 0:
     mode = 0xa
 
-    if at_full_stop:
-      mode = 0xd
-    # TODO: this is to have GM bringing the car to complete stop,
-    # but currently it conflicts with OP controls, so turned off.
-    #elif near_stop:
-    #  mode = 0xb
+  if near_stop:
+    mode = 0xb
 
+  if at_full_stop:
+    mode = 0xd
+
   brake = (0x1000 - apply_brake) & 0xfff
   checksum = (0x10000 - (mode << 12) - brake - idx) & 0xffff
 
@@ -134,6 +131,16 @@ def create_chime_command(bus, chime_type, duration, repeat_cnt):
   dat = [chime_type, duration, repeat_cnt, 0xff, 0]
   return [0x10400060, 0, "".join(map(chr, dat)), bus]
 
+def create_lka_icon_command(bus, active, critical):
+  if active:
+    if critical:
+      dat = "\x40\xc0\x14"
+    else:
+      dat = "\x40\x40\x18"
+  else:
+    dat = "\x00\x00\x00"
+  return [0x104c006c, 0, dat, bus]
+
 # TODO: WIP
 '''
 def create_friction_brake_command_ct6(packer, bus, apply_brake, idx, near_stop, at_full_stop):

selfdrive/car/gm/interface.py

@@ -52,7 +52,13 @@ def __init__(self, CP, sendcan=None):
 
   @staticmethod
   def compute_gb(accel, speed):
-    return float(accel) / 4.0
+  	# Ripped from compute_gb_honda in Honda's interface.py. Works well off shelf but may need more tuning
+    creep_brake = 0.0
+    creep_speed = 2.68
+    creep_brake_value = 0.10
+    if speed < creep_speed:
+      creep_brake = (creep_speed - speed) / creep_speed * creep_brake_value
+    return float(accel) / 4.8 - creep_brake
 
   @staticmethod
   def calc_accel_override(a_ego, a_target, v_ego, v_target):
@@ -76,7 +82,7 @@ def get_params(candidate, fingerprint):
 
     if candidate == CAR.VOLT:
       # supports stop and go, but initial engage must be above 18mph (which include conservatism)
-      ret.minEnableSpeed = 18 * CV.MPH_TO_MS
+      ret.minEnableSpeed = 7 * CV.MPH_TO_MS
       # kg of standard extra cargo to count for drive, gas, etc...
       ret.mass = 1607 + std_cargo
       ret.safetyModel = car.CarParams.SafetyModels.gm
@@ -137,15 +143,15 @@ def get_params(candidate, fingerprint):
     ret.longPidDeadzoneBP = [0.]
     ret.longPidDeadzoneV = [0.]
 
-    ret.longitudinalKpBP = [5., 35.]
-    ret.longitudinalKpV = [2.4, 1.5]
-    ret.longitudinalKiBP = [0.]
-    ret.longitudinalKiV = [0.36]
+    ret.longitudinalKpBP = [0., 5., 35.]
+    ret.longitudinalKpV = [1.8, 2.425, 2.2]
+    ret.longitudinalKiBP = [0., 35.]
+    ret.longitudinalKiV = [0.26, 0.36]
 
     ret.steerLimitAlert = True
 
     ret.stoppingControl = True
-    ret.startAccel = 0.8
+    ret.startAccel = 0.5
 
     ret.steerActuatorDelay = 0.1  # Default delay, not measured yet
     ret.steerRateCost = 1.0
@@ -193,7 +199,7 @@ def update(self, c):
     ret.cruiseState.available = bool(self.CS.main_on)
     cruiseEnabled = self.CS.pcm_acc_status != 0
     ret.cruiseState.enabled = cruiseEnabled
-    ret.cruiseState.standstill = self.CS.pcm_acc_status == 4
+    ret.cruiseState.standstill = False
 
     ret.leftBlinker = self.CS.left_blinker_on
     ret.rightBlinker = self.CS.right_blinker_on
@@ -237,7 +243,7 @@ def update(self, c):
       buttonEvents.append(be)
 
     ret.buttonEvents = buttonEvents
-
+    ret.gasbuttonstatus = self.CS.cstm_btns.get_button_status("gas")
     events = []
     if not self.CS.can_valid:
       self.can_invalid_count += 1
@@ -276,8 +282,6 @@ def update(self, c):
         events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
       if ret.gasPressed:
         events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
-      if ret.cruiseState.standstill:
-        events.append(create_event('resumeRequired', [ET.WARNING]))
 
       # handle button presses
       for b in ret.buttonEvents:

selfdrive/car/gm/values.py

@@ -12,6 +12,12 @@ class CruiseButtons:
   MAIN        = 5
   CANCEL      = 6
 
+class AccState:
+  OFF        = 0
+  ACTIVE     = 1
+  FAULTED    = 3
+  STANDSTILL = 4
+
 def is_eps_status_ok(eps_status, car_fingerprint):
   valid_eps_status = []
   if car_fingerprint == CAR.VOLT:
@@ -49,6 +55,7 @@ def parse_gear_shifter(can_gear):
 
 STEER_THRESHOLD = 1.0
 
+
 STOCK_CONTROL_MSGS = {
   CAR.VOLT: [384, 715], # 384 = "ASCMLKASteeringCmd", 715 = "ASCMGasRegenCmd"
   CAR.CADILLAC_CT6: [], # Cadillac does not require ASCMs to be disconnected

selfdrive/car/honda/carstate.py

@@ -183,7 +183,7 @@ def init_ui_buttons(self):
     btns.append(UIButton("","",0,"",2))
     btns.append(UIButton("sound","SND",1,"",3))
     btns.append(UIButton("","",0,"",4))
-    btns.append(UIButton("","",0,"",5))
+    btns.append(UIButton("gas","Gas",0,"",5))
     return btns
 
   #BB update ui buttons

selfdrive/car/honda/interface.py

@@ -472,7 +472,7 @@ def update(self, c):
       # TODO: more buttons?
       buttonEvents.append(be)
     ret.buttonEvents = buttonEvents
-
+    ret.gasbuttonstatus = self.CS.cstm_btns.get_button_status("gas")
     # events
     # TODO: I don't like the way capnp does enums
     # These strings aren't checked at compile time