op engages

selfdrive/car/ocelot/carstate.py

@@ -20,30 +20,31 @@ class CarState(CarStateBase):
     self.oldSpeedDn = False
     self.setSpeed = 10
     self.buttonStates = BUTTON_STATES.copy()
+    self.leverPressOld = False
 
   def update(self, cp, cp_body):
     ret = car.CarState.new_message()
 
     #Car specific information
-    ret.doorOpen =any([cp_body.vl["DOORS"]['FL_DOOR'], cp_body.vl["DOORS"]['FR_DOOR'],
-                        cp_body.vl["DOORS"]['RL_DOOR'], cp_body.vl["DOORS"]['RR_DOOR']]) != 0
-    ret.seatbeltUnlatched = bool(cp_body.vl["SEATBELTS"]["DRIVER_SEATBELT"])
+    ret.doorOpen = False #any([cp_body.vl["DOORS"]['FL_DOOR'], cp_body.vl["DOORS"]['FR_DOOR'],
+                        #cp_body.vl["DOORS"]['RL_DOOR'], cp_body.vl["DOORS"]['RR_DOOR']]) != 0
+    ret.seatbeltUnlatched = False #bool(cp_body.vl["SEATBELTS"]["DRIVER_SEATBELT"])
     ret.leftBlinker = (cp_body.vl["SIGNAL_STALK"]['TURN_SIGNALS'] == 1)
     ret.rightBlinker = (cp_body.vl["SIGNAL_STALK"]['TURN_SIGNALS'] == 3)
     ret.espDisabled = False #cp_body.vl["ABS"]['ESP_STATUS']
-    ret.wheelSpeeds.fl = cp_body.vl["WHEEL_SPEEDS"]['WHEEL_FL'] * CV.MPH_TO_MS
-    ret.wheelSpeeds.fr = cp_body.vl["WHEEL_SPEEDS"]['WHEEL_FR'] * CV.MPH_TO_MS
-    ret.wheelSpeeds.rl = cp_body.vl["WHEEL_SPEEDS"]['WHEEL_RL'] * CV.MPH_TO_MS
-    ret.wheelSpeeds.rr = cp_body.vl["WHEEL_SPEEDS"]['WHEEL_RR'] * CV.MPH_TO_MS
+    ret.wheelSpeeds.fl = cp_body.vl["WHEEL_SPEEDS"]['WHEEL_FL'] * 0.33
+    ret.wheelSpeeds.fr = cp_body.vl["WHEEL_SPEEDS"]['WHEEL_FR'] * 0.33
+    ret.wheelSpeeds.rl = cp_body.vl["WHEEL_SPEEDS"]['WHEEL_RL'] * 0.33
+    ret.wheelSpeeds.rr = cp_body.vl["WHEEL_SPEEDS"]['WHEEL_RR'] * 0.33
     ret.brakeLights = bool(cp_body.vl["GAS_BRAKE"]['BRAKE_PRESSED'])
     can_gear = int(cp_body.vl["GEAR_PACKET"]['GEAR'])
     ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))
 
     #Ibooster data
-    ret.brakePressed = bool(cp.vl["OCELOT_BRAKE_STATUS"]['DRIVER_BRAKE_APPLIED'])
+    ret.brakePressed = False #bool(cp.vl["OCELOT_BRAKE_STATUS"]['DRIVER_BRAKE_APPLIED'])
 
     ret.gas = ((cp.vl["PEDAL_GAS_SENSOR"]['PED_GAS'] / 2) + cp.vl["PEDAL_GAS_SENSOR"]['PED_GAS2']) / 2.
-    ret.gasPressed = ret.gas > 260
+    ret.gasPressed = False #ret.gas > 260
 
     # ret.gas = 0
     # ret.gasPressed = False
@@ -68,10 +69,13 @@ class CarState(CarStateBase):
 
     # TODO: handle steering actuator
     ret.steeringTorqueEps = raw_torque * 10 #cp.vl["STEERING_STATUS"]['STEER_TORQUE_EPS']
-    ret.steeringPressed = False #abs(ret.steeringTorque) > STEER_THRESHOLD
+
+    ret.steeringPressed =  (ret.leftBlinker or ret.rightBlinker) and not self.leverPressOld #abs(ret.steeringTorque) > 
+    self.leverPressOld = (ret.leftBlinker or ret.rightBlinker)
+
     ret.steerWarning = False #cp.vl["STEERING_STATUS"]['STEERING_OK'] != 0
 
-    main_on = bool(cp_body.vl["CRUISE_BUTTONS"]['MAIN_ON'])
+    main_on = True #bool(cp_body.vl["CRUISE_BUTTONS"]['MAIN_ON'])
 
     ret.cruiseState.available = main_on
     ret.cruiseState.standstill = False
@@ -84,7 +88,7 @@ class CarState(CarStateBase):
     if not main_on:
       self.enabled = False
 
-    if bool(cp_body.vl["CRUISE_BUTTONS"]['MAIN_ON']):
+    if main_on: #bool(cp_body.vl["CRUISE_BUTTONS"]['MAIN_ON']):
       if bool(self.buttonStates["setCruise"]) and not self.oldEnabled:
         self.enabled = not self.enabled
         if self.enabled:
@@ -93,10 +97,10 @@ class CarState(CarStateBase):
               self.setSpeed = 10
 
     #increase or decrease speed in 5mph increments
-    if cp_body.vl["CRUISE_BUTTONS"]['SET_PLUS']:
+    if cp_body.vl["CRUISE_BUTTONS"]['SET_PLUS'] and not self.oldSpeedUp:
       self.setSpeed = self.setSpeed + 5
 
-    if cp_body.vl["CRUISE_BUTTONS"]['SET_MINUS']:
+    if cp_body.vl["CRUISE_BUTTONS"]['SET_MINUS'] and not self.oldSpeedDn:
       self.setSpeed = self.setSpeed - 5
 
     ret.cruiseState.speed = self.setSpeed * CV.MPH_TO_MS

selfdrive/car/ocelot/interface.py

@@ -38,8 +38,8 @@ class CarInterface(CarInterfaceBase):
     ret.steerRatio = 21
     tire_stiffness_factor = 0.444
     ret.mass = 810 + STD_CARGO_KG
-    ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.05]]
-    ret.lateralTuning.pid.kf = 0.00007   # full torque for 20 deg at 80mph means 0.00007818594
+    ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.1], [0.05]]
+    ret.lateralTuning.pid.kf = 0.00001   # full torque for 20 deg at 80mph means 0.00007818594
 
 
     ret.steerRateCost = 1.
@@ -94,18 +94,18 @@ class CarInterface(CarInterfaceBase):
 
     # events
     events = self.create_common_events(ret)
-
-    if ret.vEgo < self.CP.minEnableSpeed and self.CP.openpilotLongitudinalControl:
-      events.add(EventName.belowEngageSpeed)
-      if c.actuators.gas > 0.1:
-        # some margin on the actuator to not false trigger cancellation while stopping
-        events.add(EventName.speedTooLow)
-      if ret.vEgo < 0.001:
-        # while in standstill, send a user alert
-        events.add(EventName.manualRestart)
+    if not ret.cruiseState.enabled:
+      events.add(EventName.pcmDisable)
+    # Attempt OP engagement only on rising edge of stock ACC engagement.
+    elif not self.cruise_enabled_prev:
+      events.add(EventName.pcmEnable)
 
     ret.events = events.to_msg()
 
+    # update previous car states
+
+    self.cruise_enabled_prev = ret.cruiseState.enabled
+
     self.CS.out = ret.as_reader()
     return self.CS.out
 

selfdrive/car/ocelot/ocelotcan.py

@@ -1,12 +1,20 @@
+#def create_steer_command(packer, steer, mode, raw_cnt):
+#  """Creates a CAN message for the Seb Smith EPAS Steer Command."""
+#
+#  values = {
+#    "STEER_MODE": mode,
+#    "REQUESTED_STEER_TORQUE": steer,
+#    "COUNTER": raw_cnt,
+#  }
+#  return packer.make_can_msg("OCELOT_STEERING_COMMAND", 2, values)
+
 def create_steer_command(packer, steer, mode, raw_cnt):
   """Creates a CAN message for the Seb Smith EPAS Steer Command."""
 
   values = {
-    "STEER_MODE": mode,
-    "REQUESTED_STEER_TORQUE": steer,
-    "COUNTER": raw_cnt,
+    "STEER_TORQUE_CMD": -steer * 18,
   }
-  return packer.make_can_msg("OCELOT_STEERING_COMMAND", 2, values)
+  return packer.make_can_msg("STEER_COMMAND", 2, values)
 
 def create_gas_command(packer, gas_amount, idx):
   # Common gas pedal msg generator
@@ -25,9 +33,9 @@ def create_gas_command(packer, gas_amount, idx):
 
 def create_brake_cmd(packer, enabled, brake, raw_cnt):
   values = {
-    "BRAKE_POSITION_COMMAND" : brake * 47,
+    "BRAKE_POSITION_COMMAND" : brake * 27,
     "BRAKE_RELATIVE_COMMAND": 0,
     "BRAKE_MODE": 2 if enabled else 0,
     "COUNTER" : raw_cnt,
   }
-  return packer.make_can_msg("OCELOT_BRAKE_COMMAND", 2, values)
+  return packer.make_can_msg("OCELOT_BRAKE_COMMAND", 2, values)