get to OP finally

Pipfile.lock

@@ -2409,47 +2409,7 @@
             "index": "pypi",
             "version": "==7.43.0.6"
         },
-        "pygame": {
+       "pygments": {
-            "hashes": [
-                "sha256:010c2e49f15de8ca24e15ce51df6911f5d0804f2c484bd9c67ffc8ed78e452f0",
-                "sha256:0267b4def5c07e3b08329f80cb51c8c457d514221f6a2d5ec175569bd02280c9",
-                "sha256:071f0e4d50a5af008a86d43236fc3dc0a903fc6dab1872b4282e1022e8b888b6",
-                "sha256:0b3b26f6ad21a651301acf48c48fdc541bd3dbbdeb214d82bbad5c9403ed71e8",
-                "sha256:10743ba4d4d07fb2437f7399fe65b99b99202b92899b0b8f085d3f97bdd408e0",
-                "sha256:10ea85a1a2a06de7e9ee4b52c391f574104017bdc8a5569d92a95d4b6e4df9cc",
-                "sha256:1162be5304d72e84d173c1c0cfa68ba742a6d20f166095e945ee8f3952a25a25",
-                "sha256:1361dbc180786f17fe399f632a0809881bbaa588c094a613a5d139f0a9f5bb17",
-                "sha256:1f2d9df9374677a963d41e3247eea9a9da768a8556d055ffbd55fff0a48d8a1a",
-                "sha256:1f648ee199a7359b8bdd9534fbbb6d046910ed821ca565d23083eb4e128b38ce",
-                "sha256:370202bbc33b5747986eee24f44e2e84d5fb18a48398ec944018c288e9298874",
-                "sha256:56eefff1c9c4338533673f4cb00c975b1a807d41b429a66ecd6e0cae274b99b2",
-                "sha256:660ab9e8dd960d312f0bd1b0b7b62d22891fb6e188ebd980cc78d57336d067d2",
-                "sha256:68c3a9224bb9e878f244aad9cd842ccef10e9c074c6284b61ec2fd9931e67def",
-                "sha256:71d629fae5b20a88cdbf541abc4f2025b823731ee73b8b2380d9c86160640d11",
-                "sha256:7ae141466b6783abeaf66e10884a2824b3e47ab681f411e83b5c9884e15ba81b",
-                "sha256:7df846da4d24e85a38240e1bcc1d5579d72e4a8b675b4bc77aad899cb8d53feb",
-                "sha256:7fd14b068e784b118b51eda271a8fc3bfe2b0c54b6146ac6e34f66858ac27834",
-                "sha256:8068af05b2a2fc9984e55ad05f9131ec767b2fc312df0f50972e88c4485f5d8e",
-                "sha256:83029fa1ad72bf07aa6a6c8e05cfc5d1a07c08c648d21dfb3176d38c8f0db3b4",
-                "sha256:842f8991ae60c93493d548b34da4d9c9523eaa066d5dc849273cec080f9b46b8",
-                "sha256:8a80cff2fd8818daec3b084b4caa5c5ecbe2a2460d9c10f96682350032c3b5fe",
-                "sha256:921d6565b40ff3da1e505efa1f75f0438214ca0d6b413e19a481fb52bff1ca7a",
-                "sha256:96bc63d8e56998f079d1d92e077213935f3dea800300912e4176c2a300135e13",
-                "sha256:a0fe0dcdbba303cd5dd01d46231cbeabf0bbe2f9c88e6066458c8eeb584352de",
-                "sha256:b47875c58b507b74361cecd0083411e0b19ae15dff7da45e104da86d8a1fefcb",
-                "sha256:bdeb5399d90d352a393c2461ff19f2d627c602706be7ea1415021b4862ce720d",
-                "sha256:d4e0cd3392ebc88c321175b205ebeb0c3247ceb2468d940d90fcb849075b468f",
-                "sha256:d72e3a583b081058107e5157b706cc409b3afdae892f560481ef298fd4154a29",
-                "sha256:d76183fa2a6070006d0f8e95fb707edface752564248f63267bdd98464b55feb",
-                "sha256:da1d869fb24e2f852c259b79e80f47c6f38a1176f872bb79aa866ca79b052867",
-                "sha256:e63c0d05c384337b3354e6368769204f0b965b092d167f55eea26e13e97208d8",
-                "sha256:ecad810029f8058712ae649588fc0b2faaf0746be84aa225de285b2a000aba8f",
-                "sha256:f6d0ea74b97a487e1f491c75386d494ecd573851af4e08090eacfa707ba43428"
-            ],
-            "index": "pypi",
-            "version": "==2.0.0.dev8"
-        },
-        "pygments": {
             "hashes": [
                 "sha256:a18f47b506a429f6f4b9df81bb02beab9ca21d0a5fee38ed15aef65f0545519f",
                 "sha256:d66e804411278594d764fc69ec36ec13d9ae9147193a1740cd34d272ca383b8e"

launch_x86.sh

@@ -0,0 +1,7 @@
+#! /bin/bash
+
+export USE_WEBCAM=1
+export ROADCAM_ID=0
+export DRIVERCAM_ID=2
+
+./launch_openpilot.sh

opendbc

@@ -1 +1 @@
-Subproject commit b1d3a96355d830c21d7106ea8465ea2957badbbd
+Subproject commit 4925c2a9993b0587a052856626614a976d8e616b

panda

@@ -1 +1 @@
-Subproject commit b8ce1c9686da19cac3094e7d754dc6a642b87cef
+Subproject commit 4522320fdc6ac5990aae191082f1e1f132778113

selfdrive/camerad/cameras/camera_webcam.cc

@@ -18,8 +18,8 @@
 #include "selfdrive/common/util.h"
 
 // id of the video capturing device
-const int ROAD_CAMERA_ID = getenv("ROADCAM_ID") ? atoi(getenv("ROADCAM_ID")) : 1;
+const int ROAD_CAMERA_ID = 0; //getenv("ROADCAM_ID") ? atoi(getenv("ROADCAM_ID")) : 1;
-const int DRIVER_CAMERA_ID = getenv("DRIVERCAM_ID") ? atoi(getenv("DRIVERCAM_ID")) : 2;
+const int DRIVER_CAMERA_ID = 2; //getenv("DRIVERCAM_ID") ? atoi(getenv("DRIVERCAM_ID")) : 2;
 
 #define FRAME_WIDTH  1164
 #define FRAME_HEIGHT 874
@@ -97,16 +97,19 @@ static void road_camera_thread(CameraState *s) {
   set_thread_name("webcam_road_camera_thread");
 
   cv::VideoCapture cap_road(ROAD_CAMERA_ID, cv::CAP_V4L2); // road
-  cap_road.set(cv::CAP_PROP_FRAME_WIDTH, 853);
+  cap_road.set(cv::CAP_PROP_FRAME_WIDTH, 1280);
-  cap_road.set(cv::CAP_PROP_FRAME_HEIGHT, 480);
+  cap_road.set(cv::CAP_PROP_FRAME_HEIGHT, 800);
   cap_road.set(cv::CAP_PROP_FPS, s->fps);
   cap_road.set(cv::CAP_PROP_AUTOFOCUS, 0); // off
   cap_road.set(cv::CAP_PROP_FOCUS, 0); // 0 - 255?
   // cv::Rect roi_rear(160, 0, 960, 720);
 
   // transforms calculation see tools/webcam/warp_vis.py
-  float ts[9] = {1.50330396, 0.0, -59.40969163,
+  // float ts[9] = {1.50330396, 0.0, -59.40969163,
-                  0.0, 1.50330396, 76.20704846,
+  //                 0.0, 1.50330396, 76.20704846,
+  //                 0.0, 0.0, 1.0};
+  float ts[9] = {0.99560759, 0.0, -55.18885977,
+                  0.0, 0.99560759, 38.75696264,
                   0.0, 0.0, 1.0};
   // if camera upside down:
   // float ts[9] = {-1.50330396, 0.0, 1223.4,

selfdrive/car/ocelot/carcontroller.py

@@ -1,8 +1,8 @@
 from cereal import car
 from common.numpy_fast import clip
-from selfdrive.car import apply_toyota_steer_torque_limits, make_can_msg
+from selfdrive.car import apply_toyota_steer_torque_limits
 from selfdrive.car.ocelot.ocelotcan import create_steer_command, create_gas_command, create_brake_cmd
-from selfdrive.car.ocelot.values import CAR, SteerLimitParams
+from selfdrive.car.ocelot.values import SteerLimitParams
 from opendbc.can.packer import CANPacker
 
 VisualAlert = car.CarControl.HUDControl.VisualAlert
@@ -103,7 +103,7 @@ class CarController():
       # This prevents unexpected pedal range rescaling
       can_sends.append(create_gas_command(self.packer, apply_gas, frame//2))
     
-    can_sends.append(create_brake_cmd(self.packer, enabled, actuators.brake, frame//2))
+    can_sends.append(create_brake_cmd(self.packer, enabled, actuators.brake, frame))
 
     # ui mesg is at 100Hz but we send asap if:
     # - there is something to display
@@ -111,14 +111,9 @@ class CarController():
     fcw_alert = hud_alert == VisualAlert.fcw
     steer_alert = hud_alert == VisualAlert.steerRequired
 
-    send_ui = False
     if ((fcw_alert or steer_alert) and not self.alert_active) or \
        (not (fcw_alert or steer_alert) and self.alert_active):
-      send_ui = True
       self.alert_active = not self.alert_active
-    elif pcm_cancel_cmd:
-      # forcing the pcm to disengage causes a bad fault sound so play a good sound instead
-      send_ui = True
 
     # #*** static msgs ***
 

selfdrive/car/ocelot/carstate.py

@@ -4,7 +4,7 @@ from opendbc.can.can_define import CANDefine
 from selfdrive.car.interfaces import CarStateBase
 from opendbc.can.parser import CANParser
 from selfdrive.config import Conversions as CV
-from selfdrive.car.ocelot.values import CAR, DBC, STEER_THRESHOLD
+from selfdrive.car.ocelot.values import DBC, BUTTON_STATES
 
 
 class CarState(CarStateBase):
@@ -16,34 +16,37 @@ class CarState(CarStateBase):
     self.setSpeed = 0
     self.enabled = False
     self.oldEnabled = False
+    self.oldSpeedUp = False
+    self.oldSpeedDn = False
+    self.setSpeed = 10
+    self.buttonStates = BUTTON_STATES.copy()
 
   def update(self, cp, cp_body):
     ret = car.CarState.new_message()
 
     #Car specific information
-    if self.CP.carFingerprint == CAR.SMART_ROADSTER_COUPE:
+    ret.doorOpen =any([cp_body.vl["DOORS"]['FL_DOOR'], cp_body.vl["DOORS"]['FR_DOOR'],
-        ret.doorOpen = False #any([cp_body.vl["BODYCONTROL"]['RIGHT_DOOR'], cp_body.vl["BODYCONTROL"]['LEFT_DOOR']]) != 0
+                        cp_body.vl["DOORS"]['RL_DOOR'], cp_body.vl["DOORS"]['RR_DOOR']]) != 0
-        ret.seatbeltUnlatched = False
+    ret.seatbeltUnlatched = bool(cp_body.vl["SEATBELTS"]["DRIVER_SEATBELT"])
-        ret.leftBlinker = False #cp_body.vl["BODYCONTROL"]['LEFT_SIGNAL']
+    ret.leftBlinker = (cp_body.vl["SIGNAL_STALK"]['TURN_SIGNALS'] == 1)
-        ret.rightBlinker = False #cp_body.vl["BODYCONTROL"]['RIGHT_SIGNAL']
+    ret.rightBlinker = (cp_body.vl["SIGNAL_STALK"]['TURN_SIGNALS'] == 3)
-        ret.espDisabled = False #cp_body.vl["ABS"]['ESP_STATUS']
+    ret.espDisabled = False #cp_body.vl["ABS"]['ESP_STATUS']
-        ret.wheelSpeeds.fl = 0 #cp_body.vl["SMARTROADSTERWHEELSPEEDS"]['WHEELSPEED_FL'] * CV.MPH_TO_MS
+    ret.wheelSpeeds.fl = cp_body.vl["WHEEL_SPEEDS"]['WHEEL_FL'] * CV.MPH_TO_MS
-        ret.wheelSpeeds.fr = 0 #cp_body.vl["SMARTROADSTERWHEELSPEEDS"]['WHEELSPEED_FR'] * CV.MPH_TO_MS
+    ret.wheelSpeeds.fr = cp_body.vl["WHEEL_SPEEDS"]['WHEEL_FR'] * CV.MPH_TO_MS
-        ret.wheelSpeeds.rl = 0 #cp_body.vl["SMARTROADSTERWHEELSPEEDS"]['WHEELSPEED_RL'] * CV.MPH_TO_MS
+    ret.wheelSpeeds.rl = cp_body.vl["WHEEL_SPEEDS"]['WHEEL_RL'] * CV.MPH_TO_MS
-        ret.wheelSpeeds.rr = 0 #cp_body.vl["SMARTROADSTERWHEELSPEEDS"]['WHEELSPEED_RR'] * CV.MPH_TO_MS
+    ret.wheelSpeeds.rr = cp_body.vl["WHEEL_SPEEDS"]['WHEEL_RR'] * CV.MPH_TO_MS
-        ret.brakeLights = False #cp_body.vl["ABS"]['BRAKEPEDAL']
+    ret.brakeLights = bool(cp_body.vl["GAS_BRAKE"]['BRAKE_PRESSED'])
-        can_gear = 0 #int(cp_body.vl["GEARBOX"]['GEARPOSITION'])
+    can_gear = int(cp_body.vl["GEAR_PACKET"]['GEAR'])
-        ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))
+    ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))
 
     #Ibooster data
-    ret.brakePressed = False #cp.vl["BRAKE_STATUS"]['IBOOSTER_BRAKE_APPLIED']
+    ret.brakePressed = bool(cp.vl["OCELOT_BRAKE_STATUS"]['DRIVER_BRAKE_APPLIED'])
 
-    # if CP.enableGasInterceptor:
+    ret.gas = ((cp.vl["PEDAL_GAS_SENSOR"]['PED_GAS'] / 2) + cp.vl["PEDAL_GAS_SENSOR"]['PED_GAS2']) / 2.
-    #   ret.gas = (cp_body.vl["GAS_SENSOR"]['PED_GAS'] + cp_body.vl["GAS_SENSOR"]['PED_GAS2']) / 2.
+    ret.gasPressed = ret.gas > 260
-    #   ret.gasPressed = ret.gas > 15
 
-    ret.gas = 0
+    # ret.gas = 0
-    ret.gasPressed = False
+    # ret.gasPressed = False
 
     #calculate speed from wheel speeds
     ret.vEgoRaw = mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr])
@@ -51,37 +54,61 @@ class CarState(CarStateBase):
     ret.standstill = ret.vEgoRaw < 0.001
 
     #Toyota SAS
-    ret.steeringAngleDeg = 0 #cp.vl["TOYOTA_STEERING_ANGLE_SENSOR1"]['TOYOTA_STEER_ANGLE'] + cp.vl["TOYOTA_STEERING_ANGLE_SENSOR1"]['TOYOTA_STEER_FRACTION']
+    ret.steeringAngleDeg = cp_body.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE']
-    ret.steeringRateDeg = 0 #cp.vl["TOYOTA_STEERING_ANGLE_SENSOR1"]['TOYOTA_STEER_RATE']
+    ret.steeringRateDeg = cp_body.vl["VSC"]['STEER_RATE']
 
 
     #Steering information from smart standin ECU
-    ret.steeringTorque = 0 #cp.vl["STEERING_STATUS"]['STEER_TORQUE_DRIVER']
+    raw_torque = 0
-    ret.steeringTorqueEps = 0 #cp.vl["STEERING_STATUS"]['STEER_TORQUE_EPS']
+    if (cp_body.vl["STEER_TORQUE"]['DIRECTION'] == 1):
+      raw_torque = cp_body.vl["STEER_TORQUE"]['DRIVER_TORQUE'] * -1
+    else:
+      raw_torque = cp_body.vl["STEER_TORQUE"]['DRIVER_TORQUE']
+    ret.steeringTorque = raw_torque
+
+    # 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.steerWarning = False #cp.vl["STEERING_STATUS"]['STEERING_OK'] != 0
 
-    ret.cruiseState.available = True
+    main_on = bool(cp_body.vl["CRUISE_BUTTONS"]['MAIN_ON'])
+
+    ret.cruiseState.available = main_on
     ret.cruiseState.standstill = False
     ret.cruiseState.nonAdaptive = False
 
-    #Logic for OP to manage whether it's enabled or not as controls board only sends button inputs
+    self.buttonStates["accelCruise"] = bool(cp_body.vl["CRUISE_BUTTONS"]['SET_PLUS'])
-    self.oldEnabled = self.enabled
+    self.buttonStates["decelCruise"] = bool(cp_body.vl["CRUISE_BUTTONS"]['SET_MINUS'])
+    self.buttonStates["setCruise"] = bool(cp_body.vl["CRUISE_BUTTONS"]['CANCEL'])
 
-    self.setSpeed = ret.cruiseState.speed
+    if not main_on:
-    #if enabled from off (rising edge) set the speed to the current speed rounded to 5mph
+      self.enabled = False
-    if self.enabled and not(self.oldEnabled):
+
-        ret.cruiseState.speed = (self.myround((ret.vEgo * CV.MS_TO_MPH), 5)) * CV.MPH_TO_MS
+    if 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:
+            self.setSpeed = (int(round((ret.vEgo * CV.MS_TO_MPH)/5)) * 5)
+            if ret.standstill:
+              self.setSpeed = 10
 
     #increase or decrease speed in 5mph increments
-    # if cp.vl["HIM_CTRLS"]['SPEEDUP_BTN']:
+    if cp_body.vl["CRUISE_BUTTONS"]['SET_PLUS']:
-    #     ret.cruiseState.speed = self.setSpeed + 5*CV.MPH_TO_MS
+      self.setSpeed = self.setSpeed + 5
 
-    # if cp.vl["HIM_CTRLS"]['SPEEDDN_BTN']:
+    if cp_body.vl["CRUISE_BUTTONS"]['SET_MINUS']:
-    #     ret.cruiseState.speed = self.setSpeed - 5*CV.MPH_TO_MS
+      self.setSpeed = self.setSpeed - 5
 
+    ret.cruiseState.speed = self.setSpeed * CV.MPH_TO_MS
     ret.cruiseState.enabled = self.enabled
 
+    ret.stockAeb = False
+    ret.leftBlindspot = False
+    ret.rightBlindspot = False
+
+    self.oldEnabled = bool(self.buttonStates["setCruise"])
+    self.oldSpeedDn = bool(self.buttonStates["decelCruise"])
+    self.oldSpeedUp = bool(self.buttonStates["accelCruise"])
 
     return ret
 
@@ -92,24 +119,50 @@ class CarState(CarStateBase):
 
     signals = [
       # sig_name, sig_address, default
-      ("BRAKE_OK", "ACTUATOR_BRAKE_STATUS", 0),
+      ("BRAKE_OK", "OCELOT_BRAKE_STATUS", 0),
-      ("STEERING_OK", "ACTUATOR_STEERING_STATUS", 0),
+      ("DRIVER_BRAKE_APPLIED", "OCELOT_BRAKE_STATUS", 0),
+      ("PED_GAS", "PEDAL_GAS_SENSOR", 0),
+      ("PED_GAS2", "PEDAL_GAS_SENSOR", 0),
     ]
 
     checks = [
-      ("ACTUATOR_BRAKE_STATUS", 80),
+      ("OCELOT_BRAKE_STATUS", 80),
+      ("PEDAL_GAS_SENSOR", 50),
     ]
 
-    return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
+    return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)
 
   @staticmethod
   def get_body_can_parser(CP):
 
     signals = [
+      ("MAIN_ON", "CRUISE_BUTTONS", 0),
+      ("SET_MINUS", "CRUISE_BUTTONS", 0),
+      ("SET_PLUS", "CRUISE_BUTTONS", 0),
+      ("CANCEL", "CRUISE_BUTTONS", 0),
+      ("STEER_ANGLE", "STEER_ANGLE_SENSOR", 0),  
+      ("DIRECTION", "STEER_TORQUE", 0),
+      ("DRIVER_TORQUE", "STEER_TORQUE", 0),
+      ("BRAKE_PRESSED", "GAS_BRAKE", 0),
+      ("FL_DOOR", "DOORS", 0),
+      ("FR_DOOR", "DOORS", 0),
+      ("RL_DOOR", "DOORS", 0),
+      ("RR_DOOR", "DOORS", 0),
+      ("DRIVER_SEATBELT", "SEATBELTS", 0),
+      ("WHEEL_FL", "WHEEL_SPEEDS", 0),
+      ("WHEEL_FR", "WHEEL_SPEEDS", 0),
+      ("WHEEL_RL", "WHEEL_SPEEDS", 0),
+      ("WHEEL_RR", "WHEEL_SPEEDS", 0),
+      ("STEER_RATE", "VSC", 0),
+      ("GEAR", "GEAR_PACKET", 0),
+      ("TURN_SIGNALS", "SIGNAL_STALK", 0),
     ]
 
     # use steering message to check if panda is connected to frc
     checks = [
+      ("STEER_ANGLE_SENSOR", 50),
+      ("WHEEL_SPEEDS", 50),
+      ("CRUISE_BUTTONS", 50),
     ]
 
     # if CP.carFingerprint == CAR.SMART_ROADSTER_COUPE:
@@ -125,4 +178,4 @@ class CarState(CarStateBase):
     #     signals.append(("BRAKEPEDAL", "ABS",0))
     #     signals.append(("GEARPOSITION","GEARBOX", 0))
 
-    return CANParser(DBC[CP.carFingerprint]['chassis'], signals, checks, 1)
+    return CANParser(DBC[CP.carFingerprint]['chassis'], signals, checks, 0)

selfdrive/car/ocelot/interface.py

@@ -20,6 +20,8 @@ class CarInterface(CarInterfaceBase):
   @staticmethod
   def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]):  # pylint: disable=dangerous-default-value
     ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
+    
+    ret.enableCamera = True
 
     ret.carName = "ocelot"
     ret.safetyModel = car.CarParams.SafetyModel.allOutput

selfdrive/car/ocelot/ocelotcan.py

@@ -6,7 +6,7 @@ def create_steer_command(packer, steer, mode, raw_cnt):
     "REQUESTED_STEER_TORQUE": steer,
     "COUNTER": raw_cnt,
   }
-  return packer.make_can_msg("OCELOT_STEERING_COMMAND", 0, values)
+  return packer.make_can_msg("OCELOT_STEERING_COMMAND", 2, values)
 
 def create_gas_command(packer, gas_amount, idx):
   # Common gas pedal msg generator
@@ -18,16 +18,16 @@ def create_gas_command(packer, gas_amount, idx):
   }
 
   if enable:
-    values["GAS_COMMAND"] = gas_amount * 255.
+    values["GAS_COMMAND"] = gas_amount * 2500
-    values["GAS_COMMAND2"] = gas_amount * 255.
+    values["GAS_COMMAND2"] = gas_amount * 1250
 
-  return packer.make_can_msg("PEDAL_GAS_COMMAND", 0, values)
+  return packer.make_can_msg("PEDAL_GAS_COMMAND", 2, values)
 
 def create_brake_cmd(packer, enabled, brake, raw_cnt):
   values = {
-    "BRAKE_POSITION_COMMAND" : 0,
+    "BRAKE_POSITION_COMMAND" : brake * 47,
-    "BRAKE_RELATIVE_COMMAND": brake * 252,
+    "BRAKE_RELATIVE_COMMAND": 0,
-    "BRAKE_MODE": enabled,
+    "BRAKE_MODE": 2 if enabled else 0,
     "COUNTER" : raw_cnt,
   }
-  return packer.make_can_msg("OCELOT_BRAKE_COMMAND", 0, values)
+  return packer.make_can_msg("OCELOT_BRAKE_COMMAND", 2, values)

selfdrive/car/ocelot/radar_interface.py

@@ -1,62 +1,14 @@
 #!/usr/bin/env python3
+import time
 from cereal import car
-from opendbc.can.parser import CANParser
-from selfdrive.car.ocelot.values import DBC
 from selfdrive.car.interfaces import RadarInterfaceBase
 
-RADAR_MSGS = list(range(0x120, 0x160))
-
-def _create_radar_can_parser(car_fingerprint):
-  msg_n = len(RADAR_MSGS)
-  signals = list(zip(['CAN_DET_RANGE'] * msg_n + ['CAN_DET_AZIMUTH'] * msg_n + ['CAN_DET_RANGE_RATE'] * msg_n + ['CAN_DET_VALID_LEVEL'] * msg_n,
-                     RADAR_MSGS * 4,
-                     [0] * msg_n + [0] * msg_n + [0] * msg_n + [0] * msg_n))
-  checks = list(zip(RADAR_MSGS, [20]*msg_n))
-
-  return CANParser(DBC[car_fingerprint]['radar'], signals, checks, 2)
-
 class RadarInterface(RadarInterfaceBase):
   def __init__(self, CP):
-    super().__init__(CP)
+    self.pts = {}
-    self.validCnt = {key: 0 for key in RADAR_MSGS}
+    self.delay = 0
-    self.track_id = 0
+    self.no_radar_sleep = 0
-
+    self.radar_ts = 0
-    self.rcp = _create_radar_can_parser(CP.carFingerprint)
-    self.trigger_msg = 0x15F
-    self.updated_messages = set()
 
   def update(self, can_strings):
-    vls = self.rcp.update_strings(can_strings)
+    return car.RadarData.new_message()
-    self.updated_messages.update(vls)
-
-    if self.trigger_msg not in self.updated_messages:
-      return None
-
-    ret = car.RadarData.new_message()
-    errors = []
-    if not self.rcp.can_valid:
-      errors.append("canError")
-    ret.errors = errors
-
-    for ii in sorted(self.updated_messages):
-      cpt = self.rcp.vl[ii]
-
-      # radar point only valid if valid signal asserted
-      if cpt['CAN_DET_VALID_LEVEL'] > 0:
-        if ii not in self.pts:
-          self.pts[ii] = car.RadarData.RadarPoint.new_message()
-          self.pts[ii].trackId = self.track_id
-          self.track_id += 1
-        self.pts[ii].dRel = cpt['CAN_DET_RANGE']  # from front of car
-        self.pts[ii].yRel = cpt['CAN_DET_RANGE'] * cpt['CAN_DET_AZIMUTH']   # in car frame's y axis, left is positive
-        self.pts[ii].vRel = cpt['CAN_DET_RANGE_RATE']
-        self.pts[ii].aRel = float('nan')
-        self.pts[ii].yvRel = float('nan')
-        self.pts[ii].measured = True
-      else:
-        if ii in self.pts:
-          del self.pts[ii]
-
-    ret.points = list(self.pts.values())
-    self.updated_messages.clear()
-    return ret

selfdrive/car/ocelot/values.py

@@ -15,6 +15,14 @@ class CAR:
   SMART_ROADSTER_COUPE = "SMART ROADSTER COUPE 2003-2006"
   ALBATROSS = "ALBATROSS"
 
+BUTTON_STATES = {
+  "accelCruise": False,
+  "decelCruise": False,
+  "cancel": False,
+  "setCruise": False,
+  "resumeCruise": False,
+  "gapAdjustCruise": False
+}
 
 FINGERPRINTS = {
   CAR.SMART_ROADSTER_COUPE: [{
@@ -31,6 +39,6 @@ FINGERPRINTS = {
 STEER_THRESHOLD = 100
 
 DBC = {
-    CAR.SMART_ROADSTER_COUPE: dbc_dict('ocelot_can', 'ford_focus_adas', 'ocelot_smart_roadster_pt'),
+    CAR.SMART_ROADSTER_COUPE: dbc_dict('ocelot_can', None, 'ocelot_smart_roadster_pt'),
-    CAR.ALBATROSS: dbc_dict('ocelot_controls', 'ford_focus_adas', 'ocelot_smart_roadster_pt'),
+    CAR.ALBATROSS: dbc_dict('ocelot_controls', None, 'ford_transit_connect_2015'),
 }

tools/carcontrols/debug_controls.py

@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
 from common.numpy_fast import clip
 from common.params import Params
 from copy import copy
@@ -58,7 +58,7 @@ def steer_thread():
 
       button_1_last = button_1
 
-      #print "enable", enabled, "steer", actuators.steer, "accel", actuators.gas - actuators.brake
+      print("enable", enabled, "steer", actuators.steer, "accel", actuators.gas - actuators.brake)
 
       hud_alert = 0
       if joystick.testJoystick.buttons[3]:

tools/carcontrols/joystickd.py

@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
 
 # This process publishes joystick events. Such events can be suscribed by
 # mocked car controller scripts.

tools/webcam/warp_vis.py

@@ -1,11 +1,11 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
 import numpy as np
 
 # copied from common.transformations/camera.py
 eon_focal_length = 910.0  # pixels
 eon_dcam_focal_length = 860.0  # pixels
 
-webcam_focal_length = -908.0/1.5  # pixels
+webcam_focal_length = 914.014724315  # pixels
 
 eon_intrinsics = np.array([
   [eon_focal_length,   0.,   1164/2.],
@@ -18,8 +18,8 @@ eon_dcam_intrinsics = np.array([
   [  0,    0,     1]])
 
 webcam_intrinsics = np.array([
-  [webcam_focal_length,   0.,   1280/2/1.5],
+  [webcam_focal_length,   0.,   1280/2],
-  [  0.,  webcam_focal_length,  720/2/1.5],
+  [  0.,  webcam_focal_length,  800/2],
   [  0.,    0.,     1.]])
 
 if __name__ == "__main__":
@@ -29,15 +29,14 @@ if __name__ == "__main__":
   print("trans_webcam_to_eon_rear:\n", trans_webcam_to_eon_rear)
   print("trans_webcam_to_eon_front:\n", trans_webcam_to_eon_front)
 
-  cap = cv2.VideoCapture(1)
+  cap = cv2.VideoCapture(0)
   cap.set(cv2.CAP_PROP_FRAME_WIDTH, 853)
   cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)
 
   while (True):
     ret, img = cap.read()
     if ret:
-      # img = cv2.warpPerspective(img, trans_webcam_to_eon_rear, (1164,874), borderMode=cv2.BORDER_CONSTANT, borderValue=0)
+      img = cv2.warpPerspective(img, trans_webcam_to_eon_rear, (1164,874), borderMode=cv2.BORDER_CONSTANT, borderValue=0)
-      img = cv2.warpPerspective(img, trans_webcam_to_eon_front, (1164, 874), borderMode=cv2.BORDER_CONSTANT, borderValue=0)
+      # img = cv2.warpPerspective(img, trans_webcam_to_eon_front, (1164, 874), borderMode=cv2.BORDER_CONSTANT, borderValue=0)
-      print(img.shape, end='\r')
       cv2.imshow('preview', img)
       cv2.waitKey(10)