Master pedal

installer/Makefile

@@ -92,7 +92,7 @@ installer_openpilot.o: installer.c
 	$(CC) $(CFLAGS) -MMD \
           -I.. -I../selfdrive \
           -DBRAND=openpilot \
-					-DBRANCH=release2 \
+					-DBRANCH="master-ci" \
           -c -o '$@' '$<'
 
 installer_dashcam.o: installer.c

installer/installer.c

@@ -31,7 +31,7 @@
 #define BRANCH_S STR2(BRANCH)
 
 #define PRE_CHECKOUT_FOLDER "/system/comma/openpilot"
-#define GIT_CLONE_COMMAND "git clone https://github.com/commaai/openpilot.git "
+#define GIT_CLONE_COMMAND "git clone https://github.com/jasonjshuler/openpilot.git "
 
 
 extern const uint8_t str_continue[] asm("_binary_continue_" BRAND_S "_sh_start");

release/build_devel.sh

@@ -7,17 +7,17 @@ chmod 777 /dev/shm
 add_subtree() {
   echo "[-] adding $2 subtree T=$SECONDS"
   if [ -d "$2" ]; then
-    if git subtree pull --prefix "$2" https://github.com/commaai/"$1".git "$3" --squash -m "Merge $2 subtree"; then
+    if git subtree pull --prefix "$2" https://github.com/jasonjshuler/"$1".git "$3" --squash -m "Merge $2 subtree"; then
       echo "git subtree pull succeeds"
     else
       echo "git subtree pull failed, fixing"
       git merge --abort || true
       git rm -r $2
       git commit -m "Remove old $2 subtree"
-      git subtree add --prefix "$2" https://github.com/commaai/"$1".git "$3" --squash
+      git subtree add --prefix "$2" https://github.com/jasonjshuler/"$1".git "$3" --squash
     fi
   else
-    git subtree add --prefix "$2" https://github.com/commaai/"$1".git "$3" --squash
+    git subtree add --prefix "$2" https://github.com/jasonjshuler/"$1".git "$3" --squash
   fi
 }
 
@@ -26,18 +26,18 @@ TARGET_DIR=/data/openpilot
 
 ln -sf $TARGET_DIR /data/pythonpath
 
-export GIT_COMMITTER_NAME="Vehicle Researcher"
-export GIT_COMMITTER_EMAIL="user@comma.ai"
-export GIT_AUTHOR_NAME="Vehicle Researcher"
-export GIT_AUTHOR_EMAIL="user@comma.ai"
+export GIT_COMMITTER_NAME="Jason Shuler"
+export GIT_COMMITTER_EMAIL="jshuler@gmail.com"
+export GIT_AUTHOR_NAME="Jason Shuler"
+export GIT_AUTHOR_EMAIL="jshuler@gmail.com"
 export GIT_SSH_COMMAND="ssh -i /tmp/deploy_key"
 
 echo "[-] Setting up repo T=$SECONDS"
 if [ ! -d "$TARGET_DIR" ]; then
     mkdir -p $TARGET_DIR
     cd $TARGET_DIR
     git init
-    git remote add origin git@github.com:commaai/openpilot.git
+    git remote add origin jshuler@gmail.com:jasonjshuler/openpilot.git
 fi
 
 
@@ -58,8 +58,8 @@ git clean -xdf
 
 # subtrees to make updates more reliable. updating them needs a clean tree
 add_subtree "cereal" "cereal" master
-add_subtree "panda" "panda" master
-add_subtree "opendbc" "opendbc" master
+add_subtree "panda" "panda" master-pedal
+add_subtree "opendbc" "opendbc" master-pedal
 add_subtree "openpilot-pyextra" "pyextra" master
 
 # leave .git alone

selfdrive/car/gm/carcontroller.py

@@ -1,8 +1,8 @@
 from cereal import car
 from common.realtime import DT_CTRL
-from common.numpy_fast import interp
+from common.numpy_fast import interp, clip
 from selfdrive.config import Conversions as CV
-from selfdrive.car import apply_std_steer_torque_limits
+from selfdrive.car import apply_std_steer_torque_limits, create_gas_command
 from selfdrive.car.gm import gmcan
 from selfdrive.car.gm.values import DBC, SUPERCRUISE_CARS, NO_ASCM_CARS, CanBus
 from opendbc.can.packer import CANPacker
@@ -20,9 +20,9 @@ def __init__(self, car_fingerprint):
       self.MIN_STEER_SPEED = -1.       # can steer down to zero
     elif car_fingerprint in NO_ASCM_CARS:
       self.STEER_MAX = 300
-      self.STEER_STEP = 2              # how often we update the steer cmd
-      self.STEER_DELTA_UP = 7          # ~0.75s time to peak torque (255/50hz/0.75s)
-      self.STEER_DELTA_DOWN = 17       # ~0.3s from peak torque to zero
+      self.STEER_STEP = 1              # how often we update the steer cmd
+      self.STEER_DELTA_UP = 3          # ~0.75s time to peak torque (255/50hz/0.75s)
+      self.STEER_DELTA_DOWN = 7       # ~0.3s from peak torque to zero
       self.MIN_STEER_SPEED = 3.
     else:
       self.STEER_MAX = 300
@@ -121,7 +121,6 @@ def update(self, enabled, CS, frame, actuators, \
           CanBus.POWERTRAIN, apply_steer, idx, lkas_enabled))
 
     ### GAS/BRAKE ###
-
     if self.car_fingerprint not in SUPERCRUISE_CARS:
       # no output if not enabled, but keep sending keepalive messages
       # treat pedals as one
@@ -130,7 +129,7 @@ def update(self, enabled, CS, frame, actuators, \
       # *** apply pedal hysteresis ***
       final_brake, self.brake_steady = actuator_hystereses(
         final_pedal, self.pedal_steady)
-
+      
       if not enabled:
         # Stock ECU sends max regen when not enabled.
         apply_gas = P.MAX_ACC_REGEN
@@ -142,13 +141,30 @@ def update(self, enabled, CS, frame, actuators, \
         # Gas/regen and brakes - all at 25Hz
         if (frame % 4) == 0:
           idx = (frame // 4) % 4
-
           at_full_stop = enabled and CS.out.standstill
           near_stop = enabled and (CS.out.vEgo < P.NEAR_STOP_BRAKE_PHASE)
           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.out.standstill
           can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, apply_gas, idx, enabled, at_full_stop))
+      elif CS.CP.enableGasInterceptor:
+        #It seems in L mode, accel / decel point is around 1/5
+        #0----decel-------0.2-------accel----------1
+        new_gas = 0.8 * actuators.gas + 0.2
+        new_brake = 0.2 * actuators.brake
+        #I am assuming we should not get both a gas and a break value...
+        final_pedal2 = new_gas - new_brake
+        #TODO: Hysteresis
+        #TODO: Use friction brake via AEB for harder braking
+
+        #JJS - no adjust yet - scaling needs to be -1 <-> +1
+        pedal_gas = clip(final_pedal, 0., 1.)
+        #This would be more appropriate
+        #pedal_gas = clip(actuators.gas, 0., 1.)
+        if (frame % 4) == 0:
+          idx = (frame // 4) % 4
+          # send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
+          # This prevents unexpected pedal range rescaling
+          can_sends.append(create_gas_command(self.packer_pt, pedal_gas, idx))
 
       # Send dashboard UI commands (ACC status), 25hz
       if (frame % 4) == 0:

selfdrive/car/gm/carstate.py

@@ -14,6 +14,7 @@ def __init__(self, CP):
     super().__init__(CP)
     can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
     self.shifter_values = can_define.dv["ECMPRDNL"]["PRNDL"]
+    self.user_gas, self.user_gas_pressed = 0., 0
 
   def update(self, pt_cp):
     ret = car.CarState.new_message()
@@ -36,8 +37,27 @@ def update(self, pt_cp):
     if ret.brake < 10/0xd0:
       ret.brake = 0.
 
+
+    # # TODO: need a better way to identify cars without ACC
+    # # TODO: this assumes the Pedal is present. If it isn't, this won't work...
+    # if self.CP.carFingerprint in (CAR.BOLT):
+    #   ret.gas = self.pedal_gas / 256.
+    # else:
     ret.gas = pt_cp.vl["AcceleratorPedal"]['AcceleratorPedal'] / 254.
-    ret.gasPressed = ret.gas > 1e-5
+
+    # this is a hack for the interceptor. This is now only used in the simulation
+    # TODO: Replace tests by toyota so this can go away
+    if self.CP.enableGasInterceptor:
+      self.user_gas = (pt_cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS'] + pt_cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS2']) / 2.
+      self.user_gas_pressed = self.user_gas > 20 # this works because interceptor read < 0 when pedal position is 0. Once calibrated, this will change
+      ret.gasPressed = self.user_gas_pressed
+      #workaround for insta-disengage
+      #ret.gasPressed = False
+    else:
+      ret.gasPressed = ret.gas > 1e-5
+
+
+    #ret.gasPressed = ret.gas > 1e-5
 
     ret.steeringTorque = pt_cp.vl["PSCMStatus"]['LKADriverAppldTrq']
     ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
@@ -122,4 +142,12 @@ def get_can_parser(CP):
         ("CruiseState", "AcceleratorPedal2", 0),
       ]
 
+    # add gas interceptor reading if we are using it
+    if CP.enableGasInterceptor:
+      signals += [
+        ("INTERCEPTOR_GAS", "GAS_SENSOR", 0),
+        ("INTERCEPTOR_GAS2", "GAS_SENSOR", 0)
+      ]
+      #checks.append(("GAS_SENSOR", 50))
+
     return CANParser(DBC[CP.carFingerprint]['pt'], signals, [], CanBus.POWERTRAIN)

selfdrive/car/gm/interface.py

@@ -41,6 +41,10 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
     ret.lateralTuning.pid.kf = 0.00004   # full torque for 20 deg at 80mph means 0.00007818594
     ret.steerRateCost = 1.0
     ret.steerActuatorDelay = 0.1  # Default delay, not measured yet
+    ret.enableGasInterceptor = 0x201 in fingerprint[0]
+    #TODO: this should be case based
+    if ret.enableGasInterceptor:
+      ret.radarOffCan = False
 
     if candidate == CAR.VOLT:
       # supports stop and go, but initial engage must be above 18mph (which include conservatism)
@@ -52,18 +56,32 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
       ret.centerToFront = ret.wheelbase * 0.4 # wild guess
 
     elif candidate == CAR.BOLT:
-      # initial engage unkown - copied from Volt. Stop and go unknown.
       ret.minEnableSpeed = 25 * CV.MPH_TO_MS
+      if ret.enableGasInterceptor:
+        ret.minEnableSpeed = 5 * CV.MPH_TO_MS #steering works down to 5mph; pedal to 0
       ret.mass = 1616. + STD_CARGO_KG
       ret.safetyModel = car.CarParams.SafetyModel.gm
       ret.wheelbase = 2.60096
       ret.steerRatio = 16.8
       ret.steerRatioRear = 0.
-      ret.centerToFront = ret.wheelbase * 0.4 # wild guess
-      #Thanks Kish
-      ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
-      ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.01]]
-      ret.lateralTuning.pid.kf = 0.000045
+      ret.centerToFront = 2.0828 #ret.wheelbase * 0.4 # wild guess
+
+      #-----------------------------------------------------------------------------
+      # INDI
+      #-----------------------------------------------------------------------------
+      # timeconstant is smoothing. Higher values == more smoothing
+      # actuatoreffectiveness is how much it steers. Lower values == more steering
+      # outer and inner are gains. Higher values = more steering
+      #
+      # JJS - removing tuning as it was causing lane crossing
+      #ret.steerActuatorDelay = 0.15
+      #ret.lateralTuning.init('indi')
+      #ret.lateralTuning.indi.innerLoopGain = 4.57 # rate error gain
+      #ret.lateralTuning.indi.outerLoopGain = 13.1 # error gain
+      #ret.lateralTuning.indi.timeConstant = 5.5
+      #ret.lateralTuning.indi.actuatorEffectiveness = 6.79
+
+      tire_stiffness_factor = 1.0
 
     elif candidate == CAR.MALIBU:
       # supports stop and go, but initial engage must be above 18mph (which include conservatism)
@@ -159,7 +177,7 @@ def update(self, c, can_strings):
     ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
 
     buttonEvents = []
-
+    
     if self.CS.cruise_buttons != self.CS.prev_cruise_buttons and self.CS.prev_cruise_buttons != CruiseButtons.INIT:
       be = car.CarState.ButtonEvent.new_message()
       be.type = ButtonType.unknown
@@ -175,7 +193,8 @@ def update(self, c, can_strings):
       elif but == CruiseButtons.DECEL_SET:
         be.type = ButtonType.decelCruise
       elif but == CruiseButtons.CANCEL:
-        be.type = ButtonType.cancel
+        if not self.CP.enableGasInterceptor: #need to use cancel to disable cc with Pedal
+          be.type = ButtonType.cancel
       elif but == CruiseButtons.MAIN:
         be.type = ButtonType.altButton3
       buttonEvents.append(be)
@@ -197,8 +216,8 @@ def update(self, c, can_strings):
       if self.CS.park_brake:
         events.append(create_event('parkBrake', [ET.NO_ENTRY, ET.USER_DISABLE]))
       # disable on pedals rising edge or when brake is pressed and speed isn't zero
-      if (ret.gasPressed and not self.gas_pressed_prev) or \
-        (ret.brakePressed): # and (not self.brake_pressed_prev or ret.vEgo > 0.001)):
+      if (ret.gasPressed and not self.gas_pressed_prev and not self.CP.enableGasInterceptor) or \
+        (ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)):
         events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
       if ret.cruiseState.standstill:
         events.append(create_event('resumeRequired', [ET.WARNING]))
@@ -234,7 +253,11 @@ def apply(self, c):
 
     # For Openpilot, "enabled" includes pre-enable.
     # In GM, PCM faults out if ACC command overlaps user gas.
-    enabled = c.enabled and not self.CS.out.gasPressed
+    # jjs disabling for now as we have no ACC
+
+    #todo: see if we can make conditional on pedal
+    # enabled = c.enabled and not self.CS.out.gasPressed
+    enabled = c.enabled
 
     can_sends = self.CC.update(enabled, self.CS, self.frame, \
                                c.actuators,