Compare commits
10 Commits
spacecruft
...
endeavr
Author | SHA1 | Date |
---|---|---|
Comma Device | e8b14c7836 | |
Comma Device | 12809b85e3 | |
Comma Device | 5fde5a451c | |
Kevin Roscom | b06cd6b74a | |
Kevin Roscom | 38c16ed242 | |
Kevin Roscom | de810ba292 | |
Kevin Roscom | 5ffa766bdf | |
Kevin Roscom | 0f133c61a1 | |
Kevin Roscom | a60ce1f939 | |
Kevin Roscom | 5b1ffd2c44 |
|
@ -2409,47 +2409,7 @@
|
|||
"index": "pypi",
|
||||
"version": "==7.43.0.6"
|
||||
},
|
||||
"pygame": {
|
||||
"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": {
|
||||
"pygments": {
|
||||
"hashes": [
|
||||
"sha256:a18f47b506a429f6f4b9df81bb02beab9ca21d0a5fee38ed15aef65f0545519f",
|
||||
"sha256:d66e804411278594d764fc69ec36ec13d9ae9147193a1740cd34d272ca383b8e"
|
||||
|
|
|
@ -0,0 +1,7 @@
|
|||
#! /bin/bash
|
||||
|
||||
export USE_WEBCAM=1
|
||||
export ROADCAM_ID=0
|
||||
export DRIVERCAM_ID=2
|
||||
|
||||
./launch_openpilot.sh
|
2
opendbc
2
opendbc
|
@ -1 +1 @@
|
|||
Subproject commit b1d3a96355d830c21d7106ea8465ea2957badbbd
|
||||
Subproject commit 4925c2a9993b0587a052856626614a976d8e616b
|
2
panda
2
panda
|
@ -1 +1 @@
|
|||
Subproject commit b8ce1c9686da19cac3094e7d754dc6a642b87cef
|
||||
Subproject commit 4522320fdc6ac5990aae191082f1e1f132778113
|
|
@ -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 DRIVER_CAMERA_ID = getenv("DRIVERCAM_ID") ? atoi(getenv("DRIVERCAM_ID")) : 2;
|
||||
const int ROAD_CAMERA_ID = 0; //getenv("ROADCAM_ID") ? atoi(getenv("ROADCAM_ID")) : 1;
|
||||
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_HEIGHT, 480);
|
||||
cap_road.set(cv::CAP_PROP_FRAME_WIDTH, 1280);
|
||||
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,
|
||||
0.0, 1.50330396, 76.20704846,
|
||||
// float ts[9] = {1.50330396, 0.0, -59.40969163,
|
||||
// 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,
|
||||
|
|
|
@ -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 ***
|
||||
|
||||
|
|
|
@ -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,38 @@ 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()
|
||||
self.leverPressOld = False
|
||||
|
||||
def update(self, cp, cp_body):
|
||||
ret = car.CarState.new_message()
|
||||
|
||||
#Car specific information
|
||||
if self.CP.carFingerprint == CAR.SMART_ROADSTER_COUPE:
|
||||
ret.doorOpen = False #any([cp_body.vl["BODYCONTROL"]['RIGHT_DOOR'], cp_body.vl["BODYCONTROL"]['LEFT_DOOR']]) != 0
|
||||
ret.seatbeltUnlatched = False
|
||||
ret.leftBlinker = False #cp_body.vl["BODYCONTROL"]['LEFT_SIGNAL']
|
||||
ret.rightBlinker = False #cp_body.vl["BODYCONTROL"]['RIGHT_SIGNAL']
|
||||
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.fr = 0 #cp_body.vl["SMARTROADSTERWHEELSPEEDS"]['WHEELSPEED_FR'] * CV.MPH_TO_MS
|
||||
ret.wheelSpeeds.rl = 0 #cp_body.vl["SMARTROADSTERWHEELSPEEDS"]['WHEELSPEED_RL'] * CV.MPH_TO_MS
|
||||
ret.wheelSpeeds.rr = 0 #cp_body.vl["SMARTROADSTERWHEELSPEEDS"]['WHEELSPEED_RR'] * CV.MPH_TO_MS
|
||||
ret.brakeLights = False #cp_body.vl["ABS"]['BRAKEPEDAL']
|
||||
can_gear = 0 #int(cp_body.vl["GEARBOX"]['GEARPOSITION'])
|
||||
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))
|
||||
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'] * 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 = False #cp.vl["BRAKE_STATUS"]['IBOOSTER_BRAKE_APPLIED']
|
||||
ret.brakePressed = False #bool(cp.vl["OCELOT_BRAKE_STATUS"]['DRIVER_BRAKE_APPLIED'])
|
||||
|
||||
# if CP.enableGasInterceptor:
|
||||
# ret.gas = (cp_body.vl["GAS_SENSOR"]['PED_GAS'] + cp_body.vl["GAS_SENSOR"]['PED_GAS2']) / 2.
|
||||
# ret.gasPressed = ret.gas > 15
|
||||
ret.gas = ((cp.vl["PEDAL_GAS_SENSOR"]['PED_GAS'] / 2) + cp.vl["PEDAL_GAS_SENSOR"]['PED_GAS2']) / 2.
|
||||
ret.gasPressed = False #ret.gas > 260
|
||||
|
||||
ret.gas = 0
|
||||
ret.gasPressed = False
|
||||
# ret.gas = 0
|
||||
# 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 +55,64 @@ 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.steeringRateDeg = 0 #cp.vl["TOYOTA_STEERING_ANGLE_SENSOR1"]['TOYOTA_STEER_RATE']
|
||||
ret.steeringAngleDeg = cp_body.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE']
|
||||
ret.steeringRateDeg = cp_body.vl["VSC"]['STEER_RATE']
|
||||
|
||||
|
||||
#Steering information from smart standin ECU
|
||||
ret.steeringTorque = 0 #cp.vl["STEERING_STATUS"]['STEER_TORQUE_DRIVER']
|
||||
ret.steeringTorqueEps = 0 #cp.vl["STEERING_STATUS"]['STEER_TORQUE_EPS']
|
||||
ret.steeringPressed = False #abs(ret.steeringTorque) > STEER_THRESHOLD
|
||||
raw_torque = 0
|
||||
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 = (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
|
||||
|
||||
ret.cruiseState.available = True
|
||||
main_on = True #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.oldEnabled = self.enabled
|
||||
self.buttonStates["accelCruise"] = bool(cp_body.vl["CRUISE_BUTTONS"]['SET_PLUS'])
|
||||
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 enabled from off (rising edge) set the speed to the current speed rounded to 5mph
|
||||
if self.enabled and not(self.oldEnabled):
|
||||
ret.cruiseState.speed = (self.myround((ret.vEgo * CV.MS_TO_MPH), 5)) * CV.MPH_TO_MS
|
||||
if not main_on:
|
||||
self.enabled = False
|
||||
|
||||
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:
|
||||
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']:
|
||||
# ret.cruiseState.speed = self.setSpeed + 5*CV.MPH_TO_MS
|
||||
if cp_body.vl["CRUISE_BUTTONS"]['SET_PLUS'] and not self.oldSpeedUp:
|
||||
self.setSpeed = self.setSpeed + 5
|
||||
|
||||
# if cp.vl["HIM_CTRLS"]['SPEEDDN_BTN']:
|
||||
# ret.cruiseState.speed = self.setSpeed - 5*CV.MPH_TO_MS
|
||||
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
|
||||
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 +123,50 @@ class CarState(CarStateBase):
|
|||
|
||||
signals = [
|
||||
# sig_name, sig_address, default
|
||||
("BRAKE_OK", "ACTUATOR_BRAKE_STATUS", 0),
|
||||
("STEERING_OK", "ACTUATOR_STEERING_STATUS", 0),
|
||||
("BRAKE_OK", "OCELOT_BRAKE_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 +182,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)
|
||||
|
|
|
@ -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
|
||||
|
@ -36,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.03], [0.01]]
|
||||
ret.lateralTuning.pid.kf = 0.000005 # full torque for 20 deg at 80mph means 0.00007818594
|
||||
|
||||
|
||||
ret.steerRateCost = 1.
|
||||
|
@ -92,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
|
||||
|
||||
|
|
|
@ -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", 0, values)
|
||||
return packer.make_can_msg("STEER_COMMAND", 2, values)
|
||||
|
||||
def create_gas_command(packer, gas_amount, idx):
|
||||
# Common gas pedal msg generator
|
||||
|
@ -18,16 +26,16 @@ def create_gas_command(packer, gas_amount, idx):
|
|||
}
|
||||
|
||||
if enable:
|
||||
values["GAS_COMMAND"] = gas_amount * 255.
|
||||
values["GAS_COMMAND2"] = gas_amount * 255.
|
||||
values["GAS_COMMAND"] = gas_amount * 2500
|
||||
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_RELATIVE_COMMAND": brake * 252,
|
||||
"BRAKE_MODE": enabled,
|
||||
"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", 0, values)
|
||||
return packer.make_can_msg("OCELOT_BRAKE_COMMAND", 2, values)
|
||||
|
|
|
@ -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.validCnt = {key: 0 for key in RADAR_MSGS}
|
||||
self.track_id = 0
|
||||
|
||||
self.rcp = _create_radar_can_parser(CP.carFingerprint)
|
||||
self.trigger_msg = 0x15F
|
||||
self.updated_messages = set()
|
||||
self.pts = {}
|
||||
self.delay = 0
|
||||
self.no_radar_sleep = 0
|
||||
self.radar_ts = 0
|
||||
|
||||
def update(self, can_strings):
|
||||
vls = self.rcp.update_strings(can_strings)
|
||||
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
|
||||
return car.RadarData.new_message()
|
|
@ -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.ALBATROSS: dbc_dict('ocelot_controls', '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', None, 'ford_transit_connect_2015'),
|
||||
}
|
||||
|
|
|
@ -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]:
|
||||
|
|
|
@ -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.
|
||||
|
|
|
@ -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],
|
||||
[ 0., webcam_focal_length, 720/2/1.5],
|
||||
[webcam_focal_length, 0., 1280/2],
|
||||
[ 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_front, (1164, 874), borderMode=cv2.BORDER_CONSTANT, borderValue=0)
|
||||
print(img.shape, end='\r')
|
||||
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)
|
||||
cv2.imshow('preview', img)
|
||||
cv2.waitKey(10)
|
||||
|
|
Loading…
Reference in New Issue