enable E261 in flake8: two spaces before inline comment
parent
02c130731c
commit
27754a277c
|
@ -17,7 +17,7 @@ repos:
|
||||||
- id: flake8
|
- id: flake8
|
||||||
exclude: '^(pyextra)|(external)|(cereal)|(rednose)|(panda)|(laika)|(laika_repo)|(rednose_repo)/'
|
exclude: '^(pyextra)|(external)|(cereal)|(rednose)|(panda)|(laika)|(laika_repo)|(rednose_repo)/'
|
||||||
args:
|
args:
|
||||||
- --ignore=E111,E114,E121,E122,E123,E124,E126,E127,E128,E201,E202,E203,E226,E241,E251,E261,E265,E266,E302,E303,E305,E402,E501,E502,E722,E741,W504
|
- --ignore=E111,E114,E121,E122,E123,E124,E126,E127,E128,E201,E202,E203,E226,E241,E251,E265,E266,E302,E303,E305,E402,E501,E502,E722,E741,W504
|
||||||
- --statistics
|
- --statistics
|
||||||
- repo: local
|
- repo: local
|
||||||
hooks:
|
hooks:
|
||||||
|
|
2
cereal
2
cereal
|
@ -1 +1 @@
|
||||||
Subproject commit 0adfc7e77efbf1ebc21bf2629a85d165b319b23e
|
Subproject commit 76eb23e062679058025369204f4e9a81ea7d479b
|
|
@ -47,10 +47,10 @@ def reboot(reason=None):
|
||||||
reason_args = ["s16", reason]
|
reason_args = ["s16", reason]
|
||||||
|
|
||||||
subprocess.check_output([
|
subprocess.check_output([
|
||||||
"service", "call", "power", "16", # IPowerManager.reboot
|
"service", "call", "power", "16", # IPowerManager.reboot
|
||||||
"i32", "0", # no confirmation,
|
"i32", "0", # no confirmation,
|
||||||
*reason_args,
|
*reason_args,
|
||||||
"i32", "1" # wait
|
"i32", "1" # wait
|
||||||
])
|
])
|
||||||
|
|
||||||
def service_call(call):
|
def service_call(call):
|
||||||
|
@ -71,7 +71,7 @@ def parse_service_call_unpack(r, fmt):
|
||||||
|
|
||||||
def parse_service_call_string(r):
|
def parse_service_call_string(r):
|
||||||
try:
|
try:
|
||||||
r = r[8:] # Cut off length field
|
r = r[8:] # Cut off length field
|
||||||
r = r.decode('utf_16_be')
|
r = r.decode('utf_16_be')
|
||||||
|
|
||||||
# All pairs of two characters seem to be swapped. Not sure why
|
# All pairs of two characters seem to be swapped. Not sure why
|
||||||
|
|
|
@ -100,7 +100,7 @@ def get_camera_frame_from_model_frame(camera_frame_from_road_frame, height=model
|
||||||
|
|
||||||
# This function is super slow, so skip it if height is very close to canonical
|
# This function is super slow, so skip it if height is very close to canonical
|
||||||
# TODO: speed it up!
|
# TODO: speed it up!
|
||||||
if abs(height - model_height) > 0.001: #
|
if abs(height - model_height) > 0.001:
|
||||||
camera_from_model_camera = get_model_height_transform(camera_frame_from_road_frame, height)
|
camera_from_model_camera = get_model_height_transform(camera_frame_from_road_frame, height)
|
||||||
else:
|
else:
|
||||||
camera_from_model_camera = np.eye(3)
|
camera_from_model_camera = np.eye(3)
|
||||||
|
|
|
@ -101,7 +101,7 @@ def get_host_binary_path(binary_name):
|
||||||
elif '.' not in binary_name:
|
elif '.' not in binary_name:
|
||||||
binary_name += '.exe'
|
binary_name += '.exe'
|
||||||
dir = os.path.join(dir, 'windows')
|
dir = os.path.join(dir, 'windows')
|
||||||
elif sys.platform == 'darwin': # OSX
|
elif sys.platform == 'darwin': # OSX
|
||||||
if binary_name.endswith('.so'):
|
if binary_name.endswith('.so'):
|
||||||
binary_name = binary_name[0:-3] + '.dylib'
|
binary_name = binary_name[0:-3] + '.dylib'
|
||||||
dir = os.path.join(dir, 'darwin')
|
dir = os.path.join(dir, 'darwin')
|
||||||
|
|
|
@ -1 +1 @@
|
||||||
Subproject commit 90b4c98502ade83df62fb215ee44375eee46b3d5
|
Subproject commit d0872aa16155e8f73961d52952c8cef41d5702d4
|
2
opendbc
2
opendbc
|
@ -1 +1 @@
|
||||||
Subproject commit e92e74311a7ed66922629bec4b8cee7c8db1b9f0
|
Subproject commit 4c59163aa31b58436fad2f8cbadeacd564887276
|
2
panda
2
panda
|
@ -1 +1 @@
|
||||||
Subproject commit 49ffbe99f65e64d23d27d9d3e37f68bc2368dccd
|
Subproject commit 9102c16118171597abf6cb7b9dee99b557f7eee7
|
|
@ -1,5 +1,5 @@
|
||||||
#!/usr/bin/env python2
|
#!/usr/bin/env python2
|
||||||
import paramiko # pylint: disable=import-error
|
import paramiko # pylint: disable=import-error
|
||||||
import os
|
import os
|
||||||
import sys
|
import sys
|
||||||
import re
|
import re
|
||||||
|
|
|
@ -10,7 +10,7 @@ from multiprocessing import Pool
|
||||||
|
|
||||||
jungle = "JUNGLE" in os.environ
|
jungle = "JUNGLE" in os.environ
|
||||||
if jungle:
|
if jungle:
|
||||||
from panda_jungle import PandaJungle # pylint: disable=import-error
|
from panda_jungle import PandaJungle # pylint: disable=import-error
|
||||||
|
|
||||||
import cereal.messaging as messaging
|
import cereal.messaging as messaging
|
||||||
from selfdrive.boardd.boardd import can_capnp_to_can_list
|
from selfdrive.boardd.boardd import can_capnp_to_can_list
|
||||||
|
|
|
@ -14,9 +14,9 @@ def get_test_string():
|
||||||
BUS = 0
|
BUS = 0
|
||||||
|
|
||||||
def main():
|
def main():
|
||||||
rcv = sub_sock('can') # port 8006
|
rcv = sub_sock('can') # port 8006
|
||||||
snd = pub_sock('sendcan') # port 8017
|
snd = pub_sock('sendcan') # port 8017
|
||||||
time.sleep(0.3) # wait to bind before send/recv
|
time.sleep(0.3) # wait to bind before send/recv
|
||||||
|
|
||||||
for i in range(10):
|
for i in range(10):
|
||||||
print("Loop %d" % i)
|
print("Loop %d" % i)
|
||||||
|
|
|
@ -24,7 +24,7 @@ class CarState(CarStateBase):
|
||||||
cp.vl["DOORS"]['DOOR_OPEN_RR']])
|
cp.vl["DOORS"]['DOOR_OPEN_RR']])
|
||||||
ret.seatbeltUnlatched = cp.vl["SEATBELT_STATUS"]['SEATBELT_DRIVER_UNLATCHED'] == 1
|
ret.seatbeltUnlatched = cp.vl["SEATBELT_STATUS"]['SEATBELT_DRIVER_UNLATCHED'] == 1
|
||||||
|
|
||||||
ret.brakePressed = cp.vl["BRAKE_2"]['BRAKE_PRESSED_2'] == 5 # human-only
|
ret.brakePressed = cp.vl["BRAKE_2"]['BRAKE_PRESSED_2'] == 5 # human-only
|
||||||
ret.brake = 0
|
ret.brake = 0
|
||||||
ret.brakeLights = ret.brakePressed
|
ret.brakeLights = ret.brakePressed
|
||||||
ret.gas = cp.vl["ACCEL_GAS_134"]['ACCEL_134']
|
ret.gas = cp.vl["ACCEL_GAS_134"]['ACCEL_134']
|
||||||
|
|
|
@ -21,7 +21,7 @@ class CarInterface(CarInterfaceBase):
|
||||||
|
|
||||||
# Speed conversion: 20, 45 mph
|
# Speed conversion: 20, 45 mph
|
||||||
ret.wheelbase = 3.089 # in meters for Pacifica Hybrid 2017
|
ret.wheelbase = 3.089 # in meters for Pacifica Hybrid 2017
|
||||||
ret.steerRatio = 16.2 # Pacifica Hybrid 2017
|
ret.steerRatio = 16.2 # Pacifica Hybrid 2017
|
||||||
ret.mass = 2858. + STD_CARGO_KG # kg curb weight Pacifica Hybrid 2017
|
ret.mass = 2858. + STD_CARGO_KG # kg curb weight Pacifica Hybrid 2017
|
||||||
ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kiBP = [[9., 20.], [9., 20.]]
|
ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kiBP = [[9., 20.], [9., 20.]]
|
||||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.15, 0.30], [0.03, 0.05]]
|
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.15, 0.30], [0.03, 0.05]]
|
||||||
|
@ -88,7 +88,7 @@ class CarInterface(CarInterfaceBase):
|
||||||
def apply(self, c):
|
def apply(self, c):
|
||||||
|
|
||||||
if (self.CS.frame == -1):
|
if (self.CS.frame == -1):
|
||||||
return [] # if we haven't seen a frame 220, then do not update.
|
return [] # if we haven't seen a frame 220, then do not update.
|
||||||
|
|
||||||
can_sends = self.CC.update(c.enabled, self.CS, c.actuators, c.cruiseControl.cancel, c.hudControl.visualAlert)
|
can_sends = self.CC.update(c.enabled, self.CS, c.actuators, c.cruiseControl.cancel, c.hudControl.visualAlert)
|
||||||
|
|
||||||
|
|
|
@ -42,9 +42,9 @@ UDS_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40])
|
||||||
|
|
||||||
|
|
||||||
HYUNDAI_VERSION_REQUEST_SHORT = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
|
HYUNDAI_VERSION_REQUEST_SHORT = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
|
||||||
p16(0xf1a0) # 4 Byte version number
|
p16(0xf1a0) # 4 Byte version number
|
||||||
HYUNDAI_VERSION_REQUEST_LONG = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
|
HYUNDAI_VERSION_REQUEST_LONG = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
|
||||||
p16(0xf100) # Long description
|
p16(0xf100) # Long description
|
||||||
HYUNDAI_VERSION_REQUEST_MULTI = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
|
HYUNDAI_VERSION_REQUEST_MULTI = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
|
||||||
p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_SPARE_PART_NUMBER) + \
|
p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_SPARE_PART_NUMBER) + \
|
||||||
p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_IDENTIFICATION) + \
|
p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_IDENTIFICATION) + \
|
||||||
|
|
|
@ -20,7 +20,7 @@ class CarControllerParams():
|
||||||
self.STEER_DRIVER_ALLOWANCE = 50 # allowed driver torque before start limiting
|
self.STEER_DRIVER_ALLOWANCE = 50 # allowed driver torque before start limiting
|
||||||
self.STEER_DRIVER_MULTIPLIER = 4 # weight driver torque heavily
|
self.STEER_DRIVER_MULTIPLIER = 4 # weight driver torque heavily
|
||||||
self.STEER_DRIVER_FACTOR = 100 # from dbc
|
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 = 0.5 # m/s, more aggressive braking near full stop
|
||||||
|
|
||||||
# Takes case of "Service Adaptive Cruise" and "Service Front Camera"
|
# Takes case of "Service Adaptive Cruise" and "Service Front Camera"
|
||||||
# dashboard messages.
|
# dashboard messages.
|
||||||
|
|
|
@ -68,7 +68,7 @@ def create_acc_dashboard_command(packer, bus, acc_engaged, target_speed_kph, lea
|
||||||
"ACCAlwaysOne" : 1,
|
"ACCAlwaysOne" : 1,
|
||||||
"ACCResumeButton" : 0,
|
"ACCResumeButton" : 0,
|
||||||
"ACCSpeedSetpoint" : target_speed,
|
"ACCSpeedSetpoint" : target_speed,
|
||||||
"ACCGapLevel" : 3 * acc_engaged, # 3 "far", 0 "inactive"
|
"ACCGapLevel" : 3 * acc_engaged, # 3 "far", 0 "inactive"
|
||||||
"ACCCmdActive" : acc_engaged,
|
"ACCCmdActive" : acc_engaged,
|
||||||
"ACCAlwaysOne2" : 1,
|
"ACCAlwaysOne2" : 1,
|
||||||
"ACCLeadCar" : lead_car_in_sight,
|
"ACCLeadCar" : lead_car_in_sight,
|
||||||
|
|
|
@ -47,7 +47,7 @@ class CarInterface(CarInterfaceBase):
|
||||||
ret.wheelbase = 2.69
|
ret.wheelbase = 2.69
|
||||||
ret.steerRatio = 15.7
|
ret.steerRatio = 15.7
|
||||||
ret.steerRatioRear = 0.
|
ret.steerRatioRear = 0.
|
||||||
ret.centerToFront = ret.wheelbase * 0.4 # wild guess
|
ret.centerToFront = ret.wheelbase * 0.4 # wild guess
|
||||||
|
|
||||||
elif candidate == CAR.MALIBU:
|
elif candidate == CAR.MALIBU:
|
||||||
# supports stop and go, but initial engage must be above 18mph (which include conservatism)
|
# supports stop and go, but initial engage must be above 18mph (which include conservatism)
|
||||||
|
@ -56,7 +56,7 @@ class CarInterface(CarInterfaceBase):
|
||||||
ret.wheelbase = 2.83
|
ret.wheelbase = 2.83
|
||||||
ret.steerRatio = 15.8
|
ret.steerRatio = 15.8
|
||||||
ret.steerRatioRear = 0.
|
ret.steerRatioRear = 0.
|
||||||
ret.centerToFront = ret.wheelbase * 0.4 # wild guess
|
ret.centerToFront = ret.wheelbase * 0.4 # wild guess
|
||||||
|
|
||||||
elif candidate == CAR.HOLDEN_ASTRA:
|
elif candidate == CAR.HOLDEN_ASTRA:
|
||||||
ret.mass = 1363. + STD_CARGO_KG
|
ret.mass = 1363. + STD_CARGO_KG
|
||||||
|
@ -68,7 +68,7 @@ class CarInterface(CarInterfaceBase):
|
||||||
ret.steerRatioRear = 0.
|
ret.steerRatioRear = 0.
|
||||||
|
|
||||||
elif candidate == CAR.ACADIA:
|
elif candidate == CAR.ACADIA:
|
||||||
ret.minEnableSpeed = -1. # engage speed is decided by pcm
|
ret.minEnableSpeed = -1. # engage speed is decided by pcm
|
||||||
ret.mass = 4353. * CV.LB_TO_KG + STD_CARGO_KG
|
ret.mass = 4353. * CV.LB_TO_KG + STD_CARGO_KG
|
||||||
ret.wheelbase = 2.86
|
ret.wheelbase = 2.86
|
||||||
ret.steerRatio = 14.4 # end to end is 13.46
|
ret.steerRatio = 14.4 # end to end is 13.46
|
||||||
|
@ -77,11 +77,11 @@ class CarInterface(CarInterfaceBase):
|
||||||
|
|
||||||
elif candidate == CAR.BUICK_REGAL:
|
elif candidate == CAR.BUICK_REGAL:
|
||||||
ret.minEnableSpeed = 18 * CV.MPH_TO_MS
|
ret.minEnableSpeed = 18 * CV.MPH_TO_MS
|
||||||
ret.mass = 3779. * CV.LB_TO_KG + STD_CARGO_KG # (3849+3708)/2
|
ret.mass = 3779. * CV.LB_TO_KG + STD_CARGO_KG # (3849+3708)/2
|
||||||
ret.wheelbase = 2.83 # 111.4 inches in meters
|
ret.wheelbase = 2.83 # 111.4 inches in meters
|
||||||
ret.steerRatio = 14.4 # guess for tourx
|
ret.steerRatio = 14.4 # guess for tourx
|
||||||
ret.steerRatioRear = 0.
|
ret.steerRatioRear = 0.
|
||||||
ret.centerToFront = ret.wheelbase * 0.4 # guess for tourx
|
ret.centerToFront = ret.wheelbase * 0.4 # guess for tourx
|
||||||
|
|
||||||
elif candidate == CAR.CADILLAC_ATS:
|
elif candidate == CAR.CADILLAC_ATS:
|
||||||
ret.minEnableSpeed = 18 * CV.MPH_TO_MS
|
ret.minEnableSpeed = 18 * CV.MPH_TO_MS
|
||||||
|
@ -135,7 +135,7 @@ class CarInterface(CarInterfaceBase):
|
||||||
but = self.CS.prev_cruise_buttons
|
but = self.CS.prev_cruise_buttons
|
||||||
if but == CruiseButtons.RES_ACCEL:
|
if but == CruiseButtons.RES_ACCEL:
|
||||||
if not (ret.cruiseState.enabled and ret.standstill):
|
if not (ret.cruiseState.enabled and ret.standstill):
|
||||||
be.type = ButtonType.accelCruise # Suppress resume button if we're resuming from stop so we don't adjust speed.
|
be.type = ButtonType.accelCruise # Suppress resume button if we're resuming from stop so we don't adjust speed.
|
||||||
elif but == CruiseButtons.DECEL_SET:
|
elif but == CruiseButtons.DECEL_SET:
|
||||||
be.type = ButtonType.decelCruise
|
be.type = ButtonType.decelCruise
|
||||||
elif but == CruiseButtons.CANCEL:
|
elif but == CruiseButtons.CANCEL:
|
||||||
|
|
|
@ -95,7 +95,7 @@ class RadarInterface(RadarInterfaceBase):
|
||||||
self.pts[targetId] = car.RadarData.RadarPoint.new_message()
|
self.pts[targetId] = car.RadarData.RadarPoint.new_message()
|
||||||
self.pts[targetId].trackId = targetId
|
self.pts[targetId].trackId = targetId
|
||||||
distance = cpt['TrkRange']
|
distance = cpt['TrkRange']
|
||||||
self.pts[targetId].dRel = distance # from front of car
|
self.pts[targetId].dRel = distance # from front of car
|
||||||
# From driver's pov, left is positive
|
# From driver's pov, left is positive
|
||||||
self.pts[targetId].yRel = math.sin(cpt['TrkAzimuth'] * CV.DEG_TO_RAD) * distance
|
self.pts[targetId].yRel = math.sin(cpt['TrkAzimuth'] * CV.DEG_TO_RAD) * distance
|
||||||
self.pts[targetId].vRel = cpt['TrkRangeRate']
|
self.pts[targetId].vRel = cpt['TrkRangeRate']
|
||||||
|
|
|
@ -179,7 +179,7 @@ class CarState(CarStateBase):
|
||||||
self.prev_cruise_setting = self.cruise_setting
|
self.prev_cruise_setting = self.cruise_setting
|
||||||
|
|
||||||
# ******************* parse out can *******************
|
# ******************* parse out can *******************
|
||||||
if self.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT): # TODO: find wheels moving bit in dbc
|
if self.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT): # TODO: find wheels moving bit in dbc
|
||||||
ret.standstill = cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] < 0.1
|
ret.standstill = cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] < 0.1
|
||||||
ret.doorOpen = bool(cp.vl["SCM_FEEDBACK"]['DRIVERS_DOOR_OPEN'])
|
ret.doorOpen = bool(cp.vl["SCM_FEEDBACK"]['DRIVERS_DOOR_OPEN'])
|
||||||
elif self.CP.carFingerprint == CAR.ODYSSEY_CHN:
|
elif self.CP.carFingerprint == CAR.ODYSSEY_CHN:
|
||||||
|
@ -252,7 +252,7 @@ class CarState(CarStateBase):
|
||||||
# TODO: Replace tests by toyota so this can go away
|
# TODO: Replace tests by toyota so this can go away
|
||||||
if self.CP.enableGasInterceptor:
|
if self.CP.enableGasInterceptor:
|
||||||
self.user_gas = (cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS'] + cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS2']) / 2.
|
self.user_gas = (cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS'] + cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS2']) / 2.
|
||||||
self.user_gas_pressed = self.user_gas > 1e-5 # this works because interceptor read < 0 when pedal position is 0. Once calibrated, this will change
|
self.user_gas_pressed = self.user_gas > 1e-5 # this works because interceptor read < 0 when pedal position is 0. Once calibrated, this will change
|
||||||
ret.gasPressed = self.user_gas_pressed
|
ret.gasPressed = self.user_gas_pressed
|
||||||
else:
|
else:
|
||||||
ret.gasPressed = self.pedal_gas > 1e-5
|
ret.gasPressed = self.pedal_gas > 1e-5
|
||||||
|
|
|
@ -87,7 +87,7 @@ class CarInterface(CarInterfaceBase):
|
||||||
|
|
||||||
# normalized max accel. Allowing max accel at low speed causes speed overshoots
|
# normalized max accel. Allowing max accel at low speed causes speed overshoots
|
||||||
max_accel_bp = [10, 20] # m/s
|
max_accel_bp = [10, 20] # m/s
|
||||||
max_accel_v = [0.714, 1.0] # unit of max accel
|
max_accel_v = [0.714, 1.0] # unit of max accel
|
||||||
max_accel = interp(v_ego, max_accel_bp, max_accel_v)
|
max_accel = interp(v_ego, max_accel_bp, max_accel_v)
|
||||||
|
|
||||||
# limit the pcm accel cmd if:
|
# limit the pcm accel cmd if:
|
||||||
|
@ -144,7 +144,7 @@ class CarInterface(CarInterfaceBase):
|
||||||
# For modeling details, see p.198-200 in "The Science of Vehicle Dynamics (2014), M. Guiggiani"
|
# For modeling details, see p.198-200 in "The Science of Vehicle Dynamics (2014), M. Guiggiani"
|
||||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0], [0]]
|
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0], [0]]
|
||||||
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
|
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
|
||||||
ret.lateralTuning.pid.kf = 0.00006 # conservative feed-forward
|
ret.lateralTuning.pid.kf = 0.00006 # conservative feed-forward
|
||||||
|
|
||||||
eps_modified = False
|
eps_modified = False
|
||||||
for fw in car_fw:
|
for fw in car_fw:
|
||||||
|
@ -182,7 +182,7 @@ class CarInterface(CarInterfaceBase):
|
||||||
ret.wheelbase = CivicParams.WHEELBASE
|
ret.wheelbase = CivicParams.WHEELBASE
|
||||||
ret.centerToFront = CivicParams.CENTER_TO_FRONT
|
ret.centerToFront = CivicParams.CENTER_TO_FRONT
|
||||||
ret.steerRatio = 15.38 # 10.93 is end-to-end spec
|
ret.steerRatio = 15.38 # 10.93 is end-to-end spec
|
||||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
|
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
|
||||||
tire_stiffness_factor = 1.
|
tire_stiffness_factor = 1.
|
||||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]]
|
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]]
|
||||||
ret.longitudinalTuning.kpBP = [0., 5., 35.]
|
ret.longitudinalTuning.kpBP = [0., 5., 35.]
|
||||||
|
@ -192,13 +192,13 @@ class CarInterface(CarInterfaceBase):
|
||||||
|
|
||||||
elif candidate in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH):
|
elif candidate in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH):
|
||||||
stop_and_go = True
|
stop_and_go = True
|
||||||
if not candidate == CAR.ACCORDH: # Hybrid uses same brake msg as hatch
|
if not candidate == CAR.ACCORDH: # Hybrid uses same brake msg as hatch
|
||||||
ret.safetyParam = 1 # Accord and CRV 5G use an alternate user brake msg
|
ret.safetyParam = 1 # Accord and CRV 5G use an alternate user brake msg
|
||||||
ret.mass = 3279. * CV.LB_TO_KG + STD_CARGO_KG
|
ret.mass = 3279. * CV.LB_TO_KG + STD_CARGO_KG
|
||||||
ret.wheelbase = 2.83
|
ret.wheelbase = 2.83
|
||||||
ret.centerToFront = ret.wheelbase * 0.39
|
ret.centerToFront = ret.wheelbase * 0.39
|
||||||
ret.steerRatio = 16.33 # 11.82 is spec end-to-end
|
ret.steerRatio = 16.33 # 11.82 is spec end-to-end
|
||||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
|
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
|
||||||
tire_stiffness_factor = 0.8467
|
tire_stiffness_factor = 0.8467
|
||||||
ret.longitudinalTuning.kpBP = [0., 5., 35.]
|
ret.longitudinalTuning.kpBP = [0., 5., 35.]
|
||||||
ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5]
|
ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5]
|
||||||
|
@ -216,7 +216,7 @@ class CarInterface(CarInterfaceBase):
|
||||||
ret.wheelbase = 2.67
|
ret.wheelbase = 2.67
|
||||||
ret.centerToFront = ret.wheelbase * 0.37
|
ret.centerToFront = ret.wheelbase * 0.37
|
||||||
ret.steerRatio = 18.61 # 15.3 is spec end-to-end
|
ret.steerRatio = 18.61 # 15.3 is spec end-to-end
|
||||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 3840], [0, 3840]] # TODO: determine if there is a dead zone at the top end
|
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 3840], [0, 3840]] # TODO: determine if there is a dead zone at the top end
|
||||||
tire_stiffness_factor = 0.72
|
tire_stiffness_factor = 0.72
|
||||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]]
|
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]]
|
||||||
ret.longitudinalTuning.kpBP = [0., 5., 35.]
|
ret.longitudinalTuning.kpBP = [0., 5., 35.]
|
||||||
|
@ -230,7 +230,7 @@ class CarInterface(CarInterfaceBase):
|
||||||
ret.wheelbase = 2.62
|
ret.wheelbase = 2.62
|
||||||
ret.centerToFront = ret.wheelbase * 0.41
|
ret.centerToFront = ret.wheelbase * 0.41
|
||||||
ret.steerRatio = 16.89 # as spec
|
ret.steerRatio = 16.89 # as spec
|
||||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 1000], [0, 1000]] # TODO: determine if there is a dead zone at the top end
|
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 1000], [0, 1000]] # TODO: determine if there is a dead zone at the top end
|
||||||
tire_stiffness_factor = 0.444
|
tire_stiffness_factor = 0.444
|
||||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]]
|
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]]
|
||||||
ret.longitudinalTuning.kpBP = [0., 5., 35.]
|
ret.longitudinalTuning.kpBP = [0., 5., 35.]
|
||||||
|
@ -263,11 +263,11 @@ class CarInterface(CarInterfaceBase):
|
||||||
elif candidate == CAR.CRV_HYBRID:
|
elif candidate == CAR.CRV_HYBRID:
|
||||||
stop_and_go = True
|
stop_and_go = True
|
||||||
ret.safetyParam = 1 # Accord and CRV 5G use an alternate user brake msg
|
ret.safetyParam = 1 # Accord and CRV 5G use an alternate user brake msg
|
||||||
ret.mass = 1667. + STD_CARGO_KG # mean of 4 models in kg
|
ret.mass = 1667. + STD_CARGO_KG # mean of 4 models in kg
|
||||||
ret.wheelbase = 2.66
|
ret.wheelbase = 2.66
|
||||||
ret.centerToFront = ret.wheelbase * 0.41
|
ret.centerToFront = ret.wheelbase * 0.41
|
||||||
ret.steerRatio = 16.0 # 12.3 is spec end-to-end
|
ret.steerRatio = 16.0 # 12.3 is spec end-to-end
|
||||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
|
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
|
||||||
tire_stiffness_factor = 0.677
|
tire_stiffness_factor = 0.677
|
||||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]]
|
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]]
|
||||||
ret.longitudinalTuning.kpBP = [0., 5., 35.]
|
ret.longitudinalTuning.kpBP = [0., 5., 35.]
|
||||||
|
@ -281,7 +281,7 @@ class CarInterface(CarInterfaceBase):
|
||||||
ret.wheelbase = 2.53
|
ret.wheelbase = 2.53
|
||||||
ret.centerToFront = ret.wheelbase * 0.39
|
ret.centerToFront = ret.wheelbase * 0.39
|
||||||
ret.steerRatio = 13.06
|
ret.steerRatio = 13.06
|
||||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
|
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
|
||||||
tire_stiffness_factor = 0.75
|
tire_stiffness_factor = 0.75
|
||||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.06]]
|
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.06]]
|
||||||
ret.longitudinalTuning.kpBP = [0., 5., 35.]
|
ret.longitudinalTuning.kpBP = [0., 5., 35.]
|
||||||
|
@ -309,7 +309,7 @@ class CarInterface(CarInterfaceBase):
|
||||||
ret.wheelbase = 2.68
|
ret.wheelbase = 2.68
|
||||||
ret.centerToFront = ret.wheelbase * 0.38
|
ret.centerToFront = ret.wheelbase * 0.38
|
||||||
ret.steerRatio = 15.0 # as spec
|
ret.steerRatio = 15.0 # as spec
|
||||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 1000], [0, 1000]] # TODO: determine if there is a dead zone at the top end
|
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 1000], [0, 1000]] # TODO: determine if there is a dead zone at the top end
|
||||||
tire_stiffness_factor = 0.444
|
tire_stiffness_factor = 0.444
|
||||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]]
|
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]]
|
||||||
ret.longitudinalTuning.kpBP = [0., 5., 35.]
|
ret.longitudinalTuning.kpBP = [0., 5., 35.]
|
||||||
|
@ -323,7 +323,7 @@ class CarInterface(CarInterfaceBase):
|
||||||
ret.wheelbase = 3.00
|
ret.wheelbase = 3.00
|
||||||
ret.centerToFront = ret.wheelbase * 0.41
|
ret.centerToFront = ret.wheelbase * 0.41
|
||||||
ret.steerRatio = 14.35 # as spec
|
ret.steerRatio = 14.35 # as spec
|
||||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
|
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
|
||||||
tire_stiffness_factor = 0.82
|
tire_stiffness_factor = 0.82
|
||||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.45], [0.135]]
|
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.45], [0.135]]
|
||||||
ret.longitudinalTuning.kpBP = [0., 5., 35.]
|
ret.longitudinalTuning.kpBP = [0., 5., 35.]
|
||||||
|
@ -337,7 +337,7 @@ class CarInterface(CarInterfaceBase):
|
||||||
ret.wheelbase = 2.90
|
ret.wheelbase = 2.90
|
||||||
ret.centerToFront = ret.wheelbase * 0.41 # from CAR.ODYSSEY
|
ret.centerToFront = ret.wheelbase * 0.41 # from CAR.ODYSSEY
|
||||||
ret.steerRatio = 14.35
|
ret.steerRatio = 14.35
|
||||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 32767], [0, 32767]] # TODO: determine if there is a dead zone at the top end
|
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 32767], [0, 32767]] # TODO: determine if there is a dead zone at the top end
|
||||||
tire_stiffness_factor = 0.82
|
tire_stiffness_factor = 0.82
|
||||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.45], [0.135]]
|
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.45], [0.135]]
|
||||||
ret.longitudinalTuning.kpBP = [0., 5., 35.]
|
ret.longitudinalTuning.kpBP = [0., 5., 35.]
|
||||||
|
@ -347,11 +347,11 @@ class CarInterface(CarInterfaceBase):
|
||||||
|
|
||||||
elif candidate in (CAR.PILOT, CAR.PILOT_2019):
|
elif candidate in (CAR.PILOT, CAR.PILOT_2019):
|
||||||
stop_and_go = False
|
stop_and_go = False
|
||||||
ret.mass = 4204. * CV.LB_TO_KG + STD_CARGO_KG # average weight
|
ret.mass = 4204. * CV.LB_TO_KG + STD_CARGO_KG # average weight
|
||||||
ret.wheelbase = 2.82
|
ret.wheelbase = 2.82
|
||||||
ret.centerToFront = ret.wheelbase * 0.428
|
ret.centerToFront = ret.wheelbase * 0.428
|
||||||
ret.steerRatio = 17.25 # as spec
|
ret.steerRatio = 17.25 # as spec
|
||||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
|
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
|
||||||
tire_stiffness_factor = 0.444
|
tire_stiffness_factor = 0.444
|
||||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.38], [0.11]]
|
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.38], [0.11]]
|
||||||
ret.longitudinalTuning.kpBP = [0., 5., 35.]
|
ret.longitudinalTuning.kpBP = [0., 5., 35.]
|
||||||
|
@ -365,7 +365,7 @@ class CarInterface(CarInterfaceBase):
|
||||||
ret.wheelbase = 3.18
|
ret.wheelbase = 3.18
|
||||||
ret.centerToFront = ret.wheelbase * 0.41
|
ret.centerToFront = ret.wheelbase * 0.41
|
||||||
ret.steerRatio = 15.59 # as spec
|
ret.steerRatio = 15.59 # as spec
|
||||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
|
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
|
||||||
tire_stiffness_factor = 0.444
|
tire_stiffness_factor = 0.444
|
||||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.38], [0.11]]
|
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.38], [0.11]]
|
||||||
ret.longitudinalTuning.kpBP = [0., 5., 35.]
|
ret.longitudinalTuning.kpBP = [0., 5., 35.]
|
||||||
|
@ -379,7 +379,7 @@ class CarInterface(CarInterfaceBase):
|
||||||
ret.wheelbase = 2.7
|
ret.wheelbase = 2.7
|
||||||
ret.centerToFront = ret.wheelbase * 0.39
|
ret.centerToFront = ret.wheelbase * 0.39
|
||||||
ret.steerRatio = 15.0 # 12.58 is spec end-to-end
|
ret.steerRatio = 15.0 # 12.58 is spec end-to-end
|
||||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
|
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
|
||||||
tire_stiffness_factor = 0.82
|
tire_stiffness_factor = 0.82
|
||||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]]
|
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]]
|
||||||
ret.longitudinalTuning.kpBP = [0., 5., 35.]
|
ret.longitudinalTuning.kpBP = [0., 5., 35.]
|
||||||
|
@ -406,7 +406,7 @@ class CarInterface(CarInterfaceBase):
|
||||||
tire_stiffness_factor=tire_stiffness_factor)
|
tire_stiffness_factor=tire_stiffness_factor)
|
||||||
|
|
||||||
ret.gasMaxBP = [0.] # m/s
|
ret.gasMaxBP = [0.] # m/s
|
||||||
ret.gasMaxV = [0.6] if ret.enableGasInterceptor else [0.] # max gas allowed
|
ret.gasMaxV = [0.6] if ret.enableGasInterceptor else [0.] # max gas allowed
|
||||||
ret.brakeMaxBP = [5., 20.] # m/s
|
ret.brakeMaxBP = [5., 20.] # m/s
|
||||||
ret.brakeMaxV = [1., 0.8] # max brake allowed
|
ret.brakeMaxV = [1., 0.8] # max brake allowed
|
||||||
|
|
||||||
|
|
|
@ -85,7 +85,7 @@ class CarState(CarStateBase):
|
||||||
# Gear Selecton - This is only compatible with optima hybrid 2017
|
# Gear Selecton - This is only compatible with optima hybrid 2017
|
||||||
elif self.CP.carFingerprint in FEATURES["use_elect_gears"]:
|
elif self.CP.carFingerprint in FEATURES["use_elect_gears"]:
|
||||||
gear = cp.vl["ELECT_GEAR"]["Elect_Gear_Shifter"]
|
gear = cp.vl["ELECT_GEAR"]["Elect_Gear_Shifter"]
|
||||||
if gear in (5, 8): # 5: D, 8: sport mode
|
if gear in (5, 8): # 5: D, 8: sport mode
|
||||||
ret.gearShifter = GearShifter.drive
|
ret.gearShifter = GearShifter.drive
|
||||||
elif gear == 6:
|
elif gear == 6:
|
||||||
ret.gearShifter = GearShifter.neutral
|
ret.gearShifter = GearShifter.neutral
|
||||||
|
@ -98,7 +98,7 @@ class CarState(CarStateBase):
|
||||||
# Gear Selecton - This is not compatible with all Kia/Hyundai's, But is the best way for those it is compatible with
|
# Gear Selecton - This is not compatible with all Kia/Hyundai's, But is the best way for those it is compatible with
|
||||||
else:
|
else:
|
||||||
gear = cp.vl["LVR12"]["CF_Lvr_Gear"]
|
gear = cp.vl["LVR12"]["CF_Lvr_Gear"]
|
||||||
if gear in (5, 8): # 5: D, 8: sport mode
|
if gear in (5, 8): # 5: D, 8: sport mode
|
||||||
ret.gearShifter = GearShifter.drive
|
ret.gearShifter = GearShifter.drive
|
||||||
elif gear == 6:
|
elif gear == 6:
|
||||||
ret.gearShifter = GearShifter.neutral
|
ret.gearShifter = GearShifter.neutral
|
||||||
|
@ -113,7 +113,7 @@ class CarState(CarStateBase):
|
||||||
self.lkas11 = cp_cam.vl["LKAS11"]
|
self.lkas11 = cp_cam.vl["LKAS11"]
|
||||||
self.clu11 = cp.vl["CLU11"]
|
self.clu11 = cp.vl["CLU11"]
|
||||||
self.park_brake = cp.vl["CGW1"]['CF_Gway_ParkBrakeSw']
|
self.park_brake = cp.vl["CGW1"]['CF_Gway_ParkBrakeSw']
|
||||||
self.steer_state = cp.vl["MDPS12"]['CF_Mdps_ToiActive'] # 0 NOT ACTIVE, 1 ACTIVE
|
self.steer_state = cp.vl["MDPS12"]['CF_Mdps_ToiActive'] # 0 NOT ACTIVE, 1 ACTIVE
|
||||||
self.lead_distance = cp.vl["SCC11"]['ACC_ObjDist']
|
self.lead_distance = cp.vl["SCC11"]['ACC_ObjDist']
|
||||||
|
|
||||||
return ret
|
return ret
|
||||||
|
|
|
@ -150,7 +150,7 @@ CHECKSUM = {
|
||||||
FEATURES = {
|
FEATURES = {
|
||||||
"use_cluster_gears": [CAR.ELANTRA, CAR.KONA, CAR.ELANTRA_GT_I30], # Use Cluster for Gear Selection, rather than Transmission
|
"use_cluster_gears": [CAR.ELANTRA, CAR.KONA, CAR.ELANTRA_GT_I30], # Use Cluster for Gear Selection, rather than Transmission
|
||||||
"use_tcu_gears": [CAR.KIA_OPTIMA, CAR.SONATA_2019], # Use TCU Message for Gear Selection
|
"use_tcu_gears": [CAR.KIA_OPTIMA, CAR.SONATA_2019], # Use TCU Message for Gear Selection
|
||||||
"use_elect_gears": [CAR.KIA_OPTIMA_H, CAR.KONA_EV], # Use TCU Message for Gear Selection
|
"use_elect_gears": [CAR.KIA_OPTIMA_H, CAR.KONA_EV], # Use TCU Message for Gear Selection
|
||||||
}
|
}
|
||||||
|
|
||||||
DBC = {
|
DBC = {
|
||||||
|
|
|
@ -11,7 +11,7 @@ from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX
|
||||||
|
|
||||||
GearShifter = car.CarState.GearShifter
|
GearShifter = car.CarState.GearShifter
|
||||||
EventName = car.CarEvent.EventName
|
EventName = car.CarEvent.EventName
|
||||||
MAX_CTRL_SPEED = (V_CRUISE_MAX + 4) * CV.KPH_TO_MS # 144 + 4 = 92 mph
|
MAX_CTRL_SPEED = (V_CRUISE_MAX + 4) * CV.KPH_TO_MS # 144 + 4 = 92 mph
|
||||||
|
|
||||||
# generic car and radar interfaces
|
# generic car and radar interfaces
|
||||||
|
|
||||||
|
|
|
@ -8,7 +8,7 @@ from selfdrive.car.interfaces import CarInterfaceBase
|
||||||
|
|
||||||
# mocked car interface to work with chffrplus
|
# mocked car interface to work with chffrplus
|
||||||
TS = 0.01 # 100Hz
|
TS = 0.01 # 100Hz
|
||||||
YAW_FR = 0.2 # ~0.8s time constant on yaw rate filter
|
YAW_FR = 0.2 # ~0.8s time constant on yaw rate filter
|
||||||
# low pass gain
|
# low pass gain
|
||||||
LPG = 2 * 3.1415 * YAW_FR * TS / (1 + 2 * 3.1415 * YAW_FR * TS)
|
LPG = 2 * 3.1415 * YAW_FR * TS / (1 + 2 * 3.1415 * YAW_FR * TS)
|
||||||
|
|
||||||
|
@ -41,7 +41,7 @@ class CarInterface(CarInterfaceBase):
|
||||||
ret.rotationalInertia = 2500.
|
ret.rotationalInertia = 2500.
|
||||||
ret.wheelbase = 2.70
|
ret.wheelbase = 2.70
|
||||||
ret.centerToFront = ret.wheelbase * 0.5
|
ret.centerToFront = ret.wheelbase * 0.5
|
||||||
ret.steerRatio = 13. # reasonable
|
ret.steerRatio = 13. # reasonable
|
||||||
ret.tireStiffnessFront = 1e6 # very stiff to neglect slip
|
ret.tireStiffnessFront = 1e6 # very stiff to neglect slip
|
||||||
ret.tireStiffnessRear = 1e6 # very stiff to neglect slip
|
ret.tireStiffnessRear = 1e6 # very stiff to neglect slip
|
||||||
|
|
||||||
|
|
|
@ -31,7 +31,7 @@ class CarInterface(CarInterfaceBase):
|
||||||
ret.lateralTuning.pid.kf = 0.00006
|
ret.lateralTuning.pid.kf = 0.00006
|
||||||
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.0], [0.0]]
|
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.0], [0.0]]
|
||||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.01], [0.005]]
|
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.01], [0.005]]
|
||||||
ret.steerMaxBP = [0.] # m/s
|
ret.steerMaxBP = [0.] # m/s
|
||||||
ret.steerMaxV = [1.]
|
ret.steerMaxV = [1.]
|
||||||
|
|
||||||
if candidate in [CAR.ROGUE, CAR.XTRAIL]:
|
if candidate in [CAR.ROGUE, CAR.XTRAIL]:
|
||||||
|
|
|
@ -12,7 +12,7 @@ VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||||
# Accel limits
|
# Accel limits
|
||||||
ACCEL_HYST_GAP = 0.02 # don't change accel command for small oscilalitons within this value
|
ACCEL_HYST_GAP = 0.02 # don't change accel command for small oscilalitons within this value
|
||||||
ACCEL_MAX = 1.5 # 1.5 m/s2
|
ACCEL_MAX = 1.5 # 1.5 m/s2
|
||||||
ACCEL_MIN = -3.0 # 3 m/s2
|
ACCEL_MIN = -3.0 # 3 m/s2
|
||||||
ACCEL_SCALE = max(ACCEL_MAX, -ACCEL_MIN)
|
ACCEL_SCALE = max(ACCEL_MAX, -ACCEL_MIN)
|
||||||
|
|
||||||
def accel_hysteresis(accel, accel_steady, enabled):
|
def accel_hysteresis(accel, accel_steady, enabled):
|
||||||
|
|
|
@ -23,7 +23,7 @@ class CarInterface(CarInterfaceBase):
|
||||||
ret.steerActuatorDelay = 0.12 # Default delay, Prius has larger delay
|
ret.steerActuatorDelay = 0.12 # Default delay, Prius has larger delay
|
||||||
ret.steerLimitTimer = 0.4
|
ret.steerLimitTimer = 0.4
|
||||||
|
|
||||||
if candidate not in [CAR.PRIUS, CAR.RAV4, CAR.RAV4H]: # These cars use LQR/INDI
|
if candidate not in [CAR.PRIUS, CAR.RAV4, CAR.RAV4H]: # These cars use LQR/INDI
|
||||||
ret.lateralTuning.init('pid')
|
ret.lateralTuning.init('pid')
|
||||||
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
|
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
|
||||||
|
|
||||||
|
@ -207,7 +207,7 @@ class CarInterface(CarInterfaceBase):
|
||||||
stop_and_go = True
|
stop_and_go = True
|
||||||
ret.safetyParam = 73
|
ret.safetyParam = 73
|
||||||
ret.wheelbase = 2.8702
|
ret.wheelbase = 2.8702
|
||||||
ret.steerRatio = 16.0 # not optimized
|
ret.steerRatio = 16.0 # not optimized
|
||||||
tire_stiffness_factor = 0.444 # not optimized yet
|
tire_stiffness_factor = 0.444 # not optimized yet
|
||||||
ret.mass = 3704. * CV.LB_TO_KG + STD_CARGO_KG
|
ret.mass = 3704. * CV.LB_TO_KG + STD_CARGO_KG
|
||||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]]
|
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]]
|
||||||
|
@ -248,7 +248,7 @@ class CarInterface(CarInterfaceBase):
|
||||||
ret.safetyParam = 73
|
ret.safetyParam = 73
|
||||||
ret.wheelbase = 2.66
|
ret.wheelbase = 2.66
|
||||||
ret.steerRatio = 14.7
|
ret.steerRatio = 14.7
|
||||||
tire_stiffness_factor = 0.444 # not optimized yet
|
tire_stiffness_factor = 0.444 # not optimized yet
|
||||||
ret.mass = 4070 * CV.LB_TO_KG + STD_CARGO_KG
|
ret.mass = 4070 * CV.LB_TO_KG + STD_CARGO_KG
|
||||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]]
|
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]]
|
||||||
ret.lateralTuning.pid.kf = 0.00006
|
ret.lateralTuning.pid.kf = 0.00006
|
||||||
|
|
|
@ -37,7 +37,7 @@ class CarState(CarStateBase):
|
||||||
# Update gas, brakes, and gearshift.
|
# Update gas, brakes, and gearshift.
|
||||||
ret.gas = pt_cp.vl["Motor_20"]['MO_Fahrpedalrohwert_01'] / 100.0
|
ret.gas = pt_cp.vl["Motor_20"]['MO_Fahrpedalrohwert_01'] / 100.0
|
||||||
ret.gasPressed = ret.gas > 0
|
ret.gasPressed = ret.gas > 0
|
||||||
ret.brake = pt_cp.vl["ESP_05"]['ESP_Bremsdruck'] / 250.0 # FIXME: this is pressure in Bar, not sure what OP expects
|
ret.brake = pt_cp.vl["ESP_05"]['ESP_Bremsdruck'] / 250.0 # FIXME: this is pressure in Bar, not sure what OP expects
|
||||||
ret.brakePressed = bool(pt_cp.vl["ESP_05"]['ESP_Fahrer_bremst'])
|
ret.brakePressed = bool(pt_cp.vl["ESP_05"]['ESP_Fahrer_bremst'])
|
||||||
ret.brakeLights = bool(pt_cp.vl["ESP_05"]['ESP_Status_Bremsdruck'])
|
ret.brakeLights = bool(pt_cp.vl["ESP_05"]['ESP_Status_Bremsdruck'])
|
||||||
|
|
||||||
|
@ -107,7 +107,7 @@ class CarState(CarStateBase):
|
||||||
self.steeringFault = not pt_cp.vl["EPS_01"]["HCA_Ready"]
|
self.steeringFault = not pt_cp.vl["EPS_01"]["HCA_Ready"]
|
||||||
|
|
||||||
# Additional safety checks performed in CarInterface.
|
# Additional safety checks performed in CarInterface.
|
||||||
self.parkingBrakeSet = bool(pt_cp.vl["Kombi_01"]['KBI_Handbremse']) # FIXME: need to include an EPB check as well
|
self.parkingBrakeSet = bool(pt_cp.vl["Kombi_01"]['KBI_Handbremse']) # FIXME: need to include an EPB check as well
|
||||||
ret.espDisabled = pt_cp.vl["ESP_21"]['ESP_Tastung_passiv'] != 0
|
ret.espDisabled = pt_cp.vl["ESP_21"]['ESP_Tastung_passiv'] != 0
|
||||||
|
|
||||||
return ret
|
return ret
|
||||||
|
@ -159,7 +159,7 @@ class CarState(CarStateBase):
|
||||||
("GRA_Tip_Hoch", "GRA_ACC_01", 0), # ACC button, increase or accel
|
("GRA_Tip_Hoch", "GRA_ACC_01", 0), # ACC button, increase or accel
|
||||||
("GRA_Tip_Runter", "GRA_ACC_01", 0), # ACC button, decrease or decel
|
("GRA_Tip_Runter", "GRA_ACC_01", 0), # ACC button, decrease or decel
|
||||||
("GRA_Tip_Wiederaufnahme", "GRA_ACC_01", 0), # ACC button, resume
|
("GRA_Tip_Wiederaufnahme", "GRA_ACC_01", 0), # ACC button, resume
|
||||||
("GRA_Verstellung_Zeitluecke", "GRA_ACC_01", 0), # ACC button, time gap adj
|
("GRA_Verstellung_Zeitluecke", "GRA_ACC_01", 0), # ACC button, time gap adj
|
||||||
("GRA_Typ_Hauptschalter", "GRA_ACC_01", 0), # ACC main button type
|
("GRA_Typ_Hauptschalter", "GRA_ACC_01", 0), # ACC main button type
|
||||||
("GRA_Tip_Stufe_2", "GRA_ACC_01", 0), # unknown related to stalk type
|
("GRA_Tip_Stufe_2", "GRA_ACC_01", 0), # unknown related to stalk type
|
||||||
("GRA_ButtonTypeInfo", "GRA_ACC_01", 0), # unknown related to stalk type
|
("GRA_ButtonTypeInfo", "GRA_ACC_01", 0), # unknown related to stalk type
|
||||||
|
|
|
@ -25,7 +25,7 @@ def create_mqb_hud_control(packer, bus, hca_enabled, steering_pressed, hud_alert
|
||||||
rightlanehud = 2 if rightLaneVisible else 1
|
rightlanehud = 2 if rightLaneVisible else 1
|
||||||
|
|
||||||
values = {
|
values = {
|
||||||
"LDW_Unknown": 2, # FIXME: possible speed or attention relationship
|
"LDW_Unknown": 2, # FIXME: possible speed or attention relationship
|
||||||
"Kombi_Lamp_Orange": 1 if hca_enabled and steering_pressed else 0,
|
"Kombi_Lamp_Orange": 1 if hca_enabled and steering_pressed else 0,
|
||||||
"Kombi_Lamp_Green": 1 if hca_enabled and not steering_pressed else 0,
|
"Kombi_Lamp_Green": 1 if hca_enabled and not steering_pressed else 0,
|
||||||
"Left_Lane_Status": leftlanehud,
|
"Left_Lane_Status": leftlanehud,
|
||||||
|
|
|
@ -23,31 +23,31 @@ _DISTRACTED_PROMPT_TIME_TILL_TERMINAL = 6.
|
||||||
|
|
||||||
_FACE_THRESHOLD = 0.4
|
_FACE_THRESHOLD = 0.4
|
||||||
_EYE_THRESHOLD = 0.6
|
_EYE_THRESHOLD = 0.6
|
||||||
_BLINK_THRESHOLD = 0.5 # 0.225
|
_BLINK_THRESHOLD = 0.5 # 0.225
|
||||||
_BLINK_THRESHOLD_SLACK = 0.65
|
_BLINK_THRESHOLD_SLACK = 0.65
|
||||||
_BLINK_THRESHOLD_STRICT = 0.5
|
_BLINK_THRESHOLD_STRICT = 0.5
|
||||||
_PITCH_WEIGHT = 1.35 # 1.5 # pitch matters a lot more
|
_PITCH_WEIGHT = 1.35 # 1.5 # pitch matters a lot more
|
||||||
_POSESTD_THRESHOLD = 0.14
|
_POSESTD_THRESHOLD = 0.14
|
||||||
_METRIC_THRESHOLD = 0.4
|
_METRIC_THRESHOLD = 0.4
|
||||||
_METRIC_THRESHOLD_SLACK = 0.55
|
_METRIC_THRESHOLD_SLACK = 0.55
|
||||||
_METRIC_THRESHOLD_STRICT = 0.4
|
_METRIC_THRESHOLD_STRICT = 0.4
|
||||||
_PITCH_POS_ALLOWANCE = 0.12 # rad, to not be too sensitive on positive pitch
|
_PITCH_POS_ALLOWANCE = 0.12 # rad, to not be too sensitive on positive pitch
|
||||||
_PITCH_NATURAL_OFFSET = 0.02 # people don't seem to look straight when they drive relaxed, rather a bit up
|
_PITCH_NATURAL_OFFSET = 0.02 # people don't seem to look straight when they drive relaxed, rather a bit up
|
||||||
_YAW_NATURAL_OFFSET = 0.08 # people don't seem to look straight when they drive relaxed, rather a bit to the right (center of car)
|
_YAW_NATURAL_OFFSET = 0.08 # people don't seem to look straight when they drive relaxed, rather a bit to the right (center of car)
|
||||||
|
|
||||||
_HI_STD_TIMEOUT = 5
|
_HI_STD_TIMEOUT = 5
|
||||||
_HI_STD_FALLBACK_TIME = 10 # fall back to wheel touch if model is uncertain for a long time
|
_HI_STD_FALLBACK_TIME = 10 # fall back to wheel touch if model is uncertain for a long time
|
||||||
_DISTRACTED_FILTER_TS = 0.25 # 0.6Hz
|
_DISTRACTED_FILTER_TS = 0.25 # 0.6Hz
|
||||||
|
|
||||||
_POSE_CALIB_MIN_SPEED = 13 # 30 mph
|
_POSE_CALIB_MIN_SPEED = 13 # 30 mph
|
||||||
_POSE_OFFSET_MIN_COUNT = 600 # valid data counts before calibration completes, 1 seg is 600 counts
|
_POSE_OFFSET_MIN_COUNT = 600 # valid data counts before calibration completes, 1 seg is 600 counts
|
||||||
_POSE_OFFSET_MAX_COUNT = 3600 # stop deweighting new data after 6 min, aka "short term memory"
|
_POSE_OFFSET_MAX_COUNT = 3600 # stop deweighting new data after 6 min, aka "short term memory"
|
||||||
|
|
||||||
_RECOVERY_FACTOR_MAX = 5. # relative to minus step change
|
_RECOVERY_FACTOR_MAX = 5. # relative to minus step change
|
||||||
_RECOVERY_FACTOR_MIN = 1.25 # relative to minus step change
|
_RECOVERY_FACTOR_MIN = 1.25 # relative to minus step change
|
||||||
|
|
||||||
MAX_TERMINAL_ALERTS = 3 # not allowed to engage after 3 terminal alerts
|
MAX_TERMINAL_ALERTS = 3 # not allowed to engage after 3 terminal alerts
|
||||||
MAX_TERMINAL_DURATION = 300 # 30s
|
MAX_TERMINAL_DURATION = 300 # 30s
|
||||||
|
|
||||||
# model output refers to center of cropped image, so need to apply the x displacement offset
|
# model output refers to center of cropped image, so need to apply the x displacement offset
|
||||||
RESIZED_FOCAL = 320.0
|
RESIZED_FOCAL = 320.0
|
||||||
|
@ -76,7 +76,7 @@ def face_orientation_from_net(angles_desc, pos_desc, rpy_calib, is_rhd):
|
||||||
|
|
||||||
# no calib for roll
|
# no calib for roll
|
||||||
pitch -= rpy_calib[1]
|
pitch -= rpy_calib[1]
|
||||||
yaw -= rpy_calib[2] * (1 - 2 * int(is_rhd)) # lhd -> -=, rhd -> +=
|
yaw -= rpy_calib[2] * (1 - 2 * int(is_rhd)) # lhd -> -=, rhd -> +=
|
||||||
return roll, pitch, yaw
|
return roll, pitch, yaw
|
||||||
|
|
||||||
class DriverPose():
|
class DriverPose():
|
||||||
|
@ -128,7 +128,7 @@ class DriverStatus():
|
||||||
self.step_change = DT_DMON / _DISTRACTED_TIME
|
self.step_change = DT_DMON / _DISTRACTED_TIME
|
||||||
else:
|
else:
|
||||||
self.step_change = 0.
|
self.step_change = 0.
|
||||||
return # no exploit after orange alert
|
return # no exploit after orange alert
|
||||||
elif self.awareness <= 0.:
|
elif self.awareness <= 0.:
|
||||||
return
|
return
|
||||||
|
|
||||||
|
|
|
@ -12,8 +12,8 @@ def apply_deadzone(error, deadzone):
|
||||||
|
|
||||||
class PIController():
|
class PIController():
|
||||||
def __init__(self, k_p, k_i, k_f=1., pos_limit=None, neg_limit=None, rate=100, sat_limit=0.8, convert=None):
|
def __init__(self, k_p, k_i, k_f=1., pos_limit=None, neg_limit=None, rate=100, sat_limit=0.8, convert=None):
|
||||||
self._k_p = k_p # proportional gain
|
self._k_p = k_p # proportional gain
|
||||||
self._k_i = k_i # integral gain
|
self._k_i = k_i # integral gain
|
||||||
self.k_f = k_f # feedforward gain
|
self.k_f = k_f # feedforward gain
|
||||||
|
|
||||||
self.pos_limit = pos_limit
|
self.pos_limit = pos_limit
|
||||||
|
|
|
@ -147,7 +147,7 @@ class Planner():
|
||||||
a_y_max = 2.975 - v_ego * 0.0375 # ~1.85 @ 75mph, ~2.6 @ 25mph
|
a_y_max = 2.975 - v_ego * 0.0375 # ~1.85 @ 75mph, ~2.6 @ 25mph
|
||||||
v_curvature = np.sqrt(a_y_max / np.clip(np.abs(curv), 1e-4, None))
|
v_curvature = np.sqrt(a_y_max / np.clip(np.abs(curv), 1e-4, None))
|
||||||
model_speed = np.min(v_curvature)
|
model_speed = np.min(v_curvature)
|
||||||
model_speed = max(20.0 * CV.MPH_TO_MS, model_speed) # Don't slow down below 20mph
|
model_speed = max(20.0 * CV.MPH_TO_MS, model_speed) # Don't slow down below 20mph
|
||||||
else:
|
else:
|
||||||
model_speed = MAX_SPEED
|
model_speed = MAX_SPEED
|
||||||
|
|
||||||
|
|
|
@ -67,7 +67,7 @@ def speed_smoother(vEgo, aEgo, vT, aMax, aMin, jMax, jMin, ts):
|
||||||
if aPeak > aMax:
|
if aPeak > aMax:
|
||||||
aPeak = aMax
|
aPeak = aMax
|
||||||
t1 = (aPeak - aEgo) / jMax
|
t1 = (aPeak - aEgo) / jMax
|
||||||
if aPeak <= 0: # there is no solution, so stop after t1
|
if aPeak <= 0: # there is no solution, so stop after t1
|
||||||
t2 = t1 + ts + 1e-9
|
t2 = t1 + ts + 1e-9
|
||||||
t3 = t2
|
t3 = t2
|
||||||
else:
|
else:
|
||||||
|
|
|
@ -27,9 +27,9 @@ class TestAlerts(unittest.TestCase):
|
||||||
bold_font_path = os.path.join(font_path, "opensans_semibold.ttf")
|
bold_font_path = os.path.join(font_path, "opensans_semibold.ttf")
|
||||||
semibold_font_path = os.path.join(font_path, "opensans_semibold.ttf")
|
semibold_font_path = os.path.join(font_path, "opensans_semibold.ttf")
|
||||||
|
|
||||||
max_text_width = 1920 - 300 # full screen width is useable, minus sidebar
|
max_text_width = 1920 - 300 # full screen width is useable, minus sidebar
|
||||||
# TODO: get exact scale factor. found this empirically, works well enough
|
# TODO: get exact scale factor. found this empirically, works well enough
|
||||||
font_scale_factor = 1.85 # factor to scale from nanovg units to PIL
|
font_scale_factor = 1.85 # factor to scale from nanovg units to PIL
|
||||||
|
|
||||||
draw = ImageDraw.Draw(Image.new('RGB', (0, 0)))
|
draw = ImageDraw.Draw(Image.new('RGB', (0, 0)))
|
||||||
|
|
||||||
|
|
|
@ -11,7 +11,7 @@ from selfdrive.controls.lib.driver_monitor import DriverStatus, MAX_TERMINAL_ALE
|
||||||
|
|
||||||
EventName = car.CarEvent.EventName
|
EventName = car.CarEvent.EventName
|
||||||
|
|
||||||
_TEST_TIMESPAN = 120 # seconds
|
_TEST_TIMESPAN = 120 # seconds
|
||||||
_DISTRACTED_SECONDS_TO_ORANGE = _DISTRACTED_TIME - _DISTRACTED_PROMPT_TIME_TILL_TERMINAL + 1
|
_DISTRACTED_SECONDS_TO_ORANGE = _DISTRACTED_TIME - _DISTRACTED_PROMPT_TIME_TILL_TERMINAL + 1
|
||||||
_DISTRACTED_SECONDS_TO_RED = _DISTRACTED_TIME + 1
|
_DISTRACTED_SECONDS_TO_RED = _DISTRACTED_TIME + 1
|
||||||
_INVISIBLE_SECONDS_TO_ORANGE = _AWARENESS_TIME - _AWARENESS_PROMPT_TIME_TILL_TERMINAL + 1
|
_INVISIBLE_SECONDS_TO_ORANGE = _AWARENESS_TIME - _AWARENESS_PROMPT_TIME_TILL_TERMINAL + 1
|
||||||
|
@ -121,7 +121,7 @@ class TestMonitoring(unittest.TestCase):
|
||||||
# driver dodges, and then touches wheel to no avail, disengages and reengages
|
# driver dodges, and then touches wheel to no avail, disengages and reengages
|
||||||
# - orange/red alert should remain after disappearance, and only disengaging clears red
|
# - orange/red alert should remain after disappearance, and only disengaging clears red
|
||||||
def test_biggest_comma_fan(self):
|
def test_biggest_comma_fan(self):
|
||||||
_invisible_time = 2 # seconds
|
_invisible_time = 2 # seconds
|
||||||
ds_vector = always_distracted[:]
|
ds_vector = always_distracted[:]
|
||||||
interaction_vector = always_false[:]
|
interaction_vector = always_false[:]
|
||||||
op_vector = always_true[:]
|
op_vector = always_true[:]
|
||||||
|
@ -138,7 +138,7 @@ class TestMonitoring(unittest.TestCase):
|
||||||
# 5. op engaged, invisible driver, down to orange, driver touches wheel; then down to orange again, driver appears
|
# 5. op engaged, invisible driver, down to orange, driver touches wheel; then down to orange again, driver appears
|
||||||
# - both actions should clear the alert, but momentary appearence should not
|
# - both actions should clear the alert, but momentary appearence should not
|
||||||
def test_sometimes_transparent_commuter(self):
|
def test_sometimes_transparent_commuter(self):
|
||||||
_visible_time = np.random.choice([1, 10]) # seconds
|
_visible_time = np.random.choice([1, 10]) # seconds
|
||||||
# print _visible_time
|
# print _visible_time
|
||||||
ds_vector = always_no_face[:]*2
|
ds_vector = always_no_face[:]*2
|
||||||
interaction_vector = always_false[:]*2
|
interaction_vector = always_false[:]*2
|
||||||
|
@ -160,7 +160,7 @@ class TestMonitoring(unittest.TestCase):
|
||||||
# 6. op engaged, invisible driver, down to red, driver appears and then touches wheel, then disengages/reengages
|
# 6. op engaged, invisible driver, down to red, driver appears and then touches wheel, then disengages/reengages
|
||||||
# - only disengage will clear the alert
|
# - only disengage will clear the alert
|
||||||
def test_last_second_responder(self):
|
def test_last_second_responder(self):
|
||||||
_visible_time = 2 # seconds
|
_visible_time = 2 # seconds
|
||||||
ds_vector = always_no_face[:]
|
ds_vector = always_no_face[:]
|
||||||
interaction_vector = always_false[:]
|
interaction_vector = always_false[:]
|
||||||
op_vector = always_true[:]
|
op_vector = always_true[:]
|
||||||
|
@ -184,7 +184,7 @@ class TestMonitoring(unittest.TestCase):
|
||||||
# 8. op engaged, car stops at traffic light, down to orange, no action, then car starts moving
|
# 8. op engaged, car stops at traffic light, down to orange, no action, then car starts moving
|
||||||
# - should only reach green when stopped, but continues counting down on launch
|
# - should only reach green when stopped, but continues counting down on launch
|
||||||
def test_long_traffic_light_victim(self):
|
def test_long_traffic_light_victim(self):
|
||||||
_redlight_time = 60 # seconds
|
_redlight_time = 60 # seconds
|
||||||
standstill_vector = always_true[:]
|
standstill_vector = always_true[:]
|
||||||
standstill_vector[int(_redlight_time/DT_DMON):] = [False] * int((_TEST_TIMESPAN-_redlight_time)/DT_DMON)
|
standstill_vector[int(_redlight_time/DT_DMON):] = [False] * int((_TEST_TIMESPAN-_redlight_time)/DT_DMON)
|
||||||
events_output = run_DState_seq(always_distracted, always_false, always_true, standstill_vector)[0]
|
events_output = run_DState_seq(always_distracted, always_false, always_true, standstill_vector)[0]
|
||||||
|
@ -217,7 +217,7 @@ class TestMonitoring(unittest.TestCase):
|
||||||
self.assertEqual(events_output[int((2.5*(_DISTRACTED_TIME-_DISTRACTED_PRE_TIME_TILL_TERMINAL))/DT_DMON)].names[1], EventName.preDriverDistracted)
|
self.assertEqual(events_output[int((2.5*(_DISTRACTED_TIME-_DISTRACTED_PRE_TIME_TILL_TERMINAL))/DT_DMON)].names[1], EventName.preDriverDistracted)
|
||||||
self.assertEqual(events_output[int((2.5*(_DISTRACTED_TIME-_DISTRACTED_PROMPT_TIME_TILL_TERMINAL))/DT_DMON)].names[1], EventName.promptDriverDistracted)
|
self.assertEqual(events_output[int((2.5*(_DISTRACTED_TIME-_DISTRACTED_PROMPT_TIME_TILL_TERMINAL))/DT_DMON)].names[1], EventName.promptDriverDistracted)
|
||||||
self.assertEqual(events_output[int((_DISTRACTED_TIME+1)/DT_DMON)].names[1], EventName.promptDriverDistracted)
|
self.assertEqual(events_output[int((_DISTRACTED_TIME+1)/DT_DMON)].names[1], EventName.promptDriverDistracted)
|
||||||
self.assertEqual(events_output[int((_DISTRACTED_TIME*2.5)/DT_DMON)].names[1], EventName.promptDriverDistracted) # set_timer blocked
|
self.assertEqual(events_output[int((_DISTRACTED_TIME*2.5)/DT_DMON)].names[1], EventName.promptDriverDistracted) # set_timer blocked
|
||||||
|
|
||||||
if __name__ == "__main__":
|
if __name__ == "__main__":
|
||||||
print('MAX_TERMINAL_ALERTS', MAX_TERMINAL_ALERTS)
|
print('MAX_TERMINAL_ALERTS', MAX_TERMINAL_ALERTS)
|
||||||
|
|
|
@ -1,6 +1,6 @@
|
||||||
#!/usr/bin/env python3
|
#!/usr/bin/env python3
|
||||||
import numpy as np
|
import numpy as np
|
||||||
import control # pylint: disable=import-error
|
import control # pylint: disable=import-error
|
||||||
|
|
||||||
dt = 0.01
|
dt = 0.01
|
||||||
A = np.array([[ 0. , 1. ], [-0.78823806, 1.78060701]])
|
A = np.array([[ 0. , 1. ], [-0.78823806, 1.78060701]])
|
||||||
|
|
|
@ -17,7 +17,7 @@ if __name__ == '__main__':
|
||||||
print("disabling charging")
|
print("disabling charging")
|
||||||
os.system('echo "0" > /sys/class/power_supply/battery/charging_enabled')
|
os.system('echo "0" > /sys/class/power_supply/battery/charging_enabled')
|
||||||
|
|
||||||
voltage_average = (0., 0) # average, count
|
voltage_average = (0., 0) # average, count
|
||||||
current_average = (0., 0)
|
current_average = (0., 0)
|
||||||
power_average = (0., 0)
|
power_average = (0., 0)
|
||||||
capacity_average = (0., 0)
|
capacity_average = (0., 0)
|
||||||
|
|
|
@ -1,8 +1,8 @@
|
||||||
import math
|
import math
|
||||||
|
|
||||||
class Filter:
|
class Filter:
|
||||||
MIN_SPEED = 7 # m/s (~15.5mph)
|
MIN_SPEED = 7 # m/s (~15.5mph)
|
||||||
MAX_YAW_RATE = math.radians(3) # per second
|
MAX_YAW_RATE = math.radians(3) # per second
|
||||||
|
|
||||||
class Calibration:
|
class Calibration:
|
||||||
UNCALIBRATED = 0
|
UNCALIBRATED = 0
|
||||||
|
|
|
@ -6,7 +6,7 @@ import unittest
|
||||||
from selfdrive.car.honda.interface import CarInterface
|
from selfdrive.car.honda.interface import CarInterface
|
||||||
from selfdrive.car.honda.values import CAR
|
from selfdrive.car.honda.values import CAR
|
||||||
from selfdrive.controls.lib.vehicle_model import VehicleModel
|
from selfdrive.controls.lib.vehicle_model import VehicleModel
|
||||||
from selfdrive.locationd.liblocationd_py import liblocationd # pylint: disable=no-name-in-module, import-error
|
from selfdrive.locationd.liblocationd_py import liblocationd # pylint: disable=no-name-in-module, import-error
|
||||||
|
|
||||||
|
|
||||||
class TestParamsLearner(unittest.TestCase):
|
class TestParamsLearner(unittest.TestCase):
|
||||||
|
|
|
@ -19,10 +19,10 @@ debug = os.getenv("DEBUG") is not None # debug prints
|
||||||
print_dB = os.getenv("PRINT_DB") is not None # print antenna dB
|
print_dB = os.getenv("PRINT_DB") is not None # print antenna dB
|
||||||
|
|
||||||
timeout = 1
|
timeout = 1
|
||||||
dyn_model = 4 # auto model
|
dyn_model = 4 # auto model
|
||||||
baudrate = 460800
|
baudrate = 460800
|
||||||
ports = ["/dev/ttyACM0", "/dev/ttyACM1"]
|
ports = ["/dev/ttyACM0", "/dev/ttyACM1"]
|
||||||
rate = 100 # send new data every 100ms
|
rate = 100 # send new data every 100ms
|
||||||
|
|
||||||
# which SV IDs we have seen and when we got iono
|
# which SV IDs we have seen and when we got iono
|
||||||
svid_seen = {}
|
svid_seen = {}
|
||||||
|
@ -32,17 +32,17 @@ iono_seen = 0
|
||||||
def configure_ublox(dev):
|
def configure_ublox(dev):
|
||||||
# configure ports and solution parameters and rate
|
# configure ports and solution parameters and rate
|
||||||
# TODO: configure constellations and channels to allow for 10Hz and high precision
|
# TODO: configure constellations and channels to allow for 10Hz and high precision
|
||||||
dev.configure_port(port=ublox.PORT_USB, inMask=1, outMask=1) # enable only UBX on USB
|
dev.configure_port(port=ublox.PORT_USB, inMask=1, outMask=1) # enable only UBX on USB
|
||||||
dev.configure_port(port=0, inMask=0, outMask=0) # disable DDC
|
dev.configure_port(port=0, inMask=0, outMask=0) # disable DDC
|
||||||
|
|
||||||
if panda:
|
if panda:
|
||||||
payload = struct.pack('<BBHIIHHHBB', 1, 0, 0, 2240, baudrate, 1, 1, 0, 0, 0)
|
payload = struct.pack('<BBHIIHHHBB', 1, 0, 0, 2240, baudrate, 1, 1, 0, 0, 0)
|
||||||
dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_PRT, payload) # enable UART
|
dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_PRT, payload) # enable UART
|
||||||
else:
|
else:
|
||||||
payload = struct.pack('<BBHIIHHHBB', 1, 0, 0, 2240, baudrate, 0, 0, 0, 0, 0)
|
payload = struct.pack('<BBHIIHHHBB', 1, 0, 0, 2240, baudrate, 0, 0, 0, 0, 0)
|
||||||
dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_PRT, payload) # disable UART
|
dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_PRT, payload) # disable UART
|
||||||
|
|
||||||
dev.configure_port(port=4, inMask=0, outMask=0) # disable SPI
|
dev.configure_port(port=4, inMask=0, outMask=0) # disable SPI
|
||||||
dev.configure_poll_port()
|
dev.configure_poll_port()
|
||||||
dev.configure_poll_port(ublox.PORT_SERIAL1)
|
dev.configure_poll_port(ublox.PORT_SERIAL1)
|
||||||
dev.configure_poll_port(ublox.PORT_SERIAL2)
|
dev.configure_poll_port(ublox.PORT_SERIAL2)
|
||||||
|
@ -128,7 +128,7 @@ def gen_ephemeris(ephem_data):
|
||||||
|
|
||||||
|
|
||||||
def gen_solution(msg):
|
def gen_solution(msg):
|
||||||
msg_data = msg.unpack()[0] # Solutions do not have any data in repeated blocks
|
msg_data = msg.unpack()[0] # Solutions do not have any data in repeated blocks
|
||||||
timestamp = int(((datetime.datetime(msg_data['year'],
|
timestamp = int(((datetime.datetime(msg_data['year'],
|
||||||
msg_data['month'],
|
msg_data['month'],
|
||||||
msg_data['day'],
|
msg_data['day'],
|
||||||
|
@ -204,9 +204,9 @@ def gen_raw(msg):
|
||||||
'glonassFrequencyIndex': m['freqId'],
|
'glonassFrequencyIndex': m['freqId'],
|
||||||
'locktime': m['locktime'],
|
'locktime': m['locktime'],
|
||||||
'cno': m['cno'],
|
'cno': m['cno'],
|
||||||
'pseudorangeStdev': 0.01*(2**(m['prStdev'] & 15)), # weird scaling, might be wrong
|
'pseudorangeStdev': 0.01*(2**(m['prStdev'] & 15)), # weird scaling, might be wrong
|
||||||
'carrierPhaseStdev': 0.004*(m['cpStdev'] & 15),
|
'carrierPhaseStdev': 0.004*(m['cpStdev'] & 15),
|
||||||
'dopplerStdev': 0.002*(2**(m['doStdev'] & 15)), # weird scaling, might be wrong
|
'dopplerStdev': 0.002*(2**(m['doStdev'] & 15)), # weird scaling, might be wrong
|
||||||
'trackingStatus': trackingStatus})
|
'trackingStatus': trackingStatus})
|
||||||
if print_dB:
|
if print_dB:
|
||||||
cnos = {}
|
cnos = {}
|
||||||
|
|
|
@ -134,7 +134,7 @@ class Uploader():
|
||||||
is_uploaded = getxattr(fn, UPLOAD_ATTR_NAME)
|
is_uploaded = getxattr(fn, UPLOAD_ATTR_NAME)
|
||||||
except OSError:
|
except OSError:
|
||||||
cloudlog.event("uploader_getxattr_failed", exc=self.last_exc, key=key, fn=fn)
|
cloudlog.event("uploader_getxattr_failed", exc=self.last_exc, key=key, fn=fn)
|
||||||
is_uploaded = True # deleter could have deleted
|
is_uploaded = True # deleter could have deleted
|
||||||
if is_uploaded:
|
if is_uploaded:
|
||||||
continue
|
continue
|
||||||
|
|
||||||
|
|
|
@ -36,7 +36,7 @@ if ANDROID:
|
||||||
def unblock_stdout():
|
def unblock_stdout():
|
||||||
# get a non-blocking stdout
|
# get a non-blocking stdout
|
||||||
child_pid, child_pty = os.forkpty()
|
child_pid, child_pty = os.forkpty()
|
||||||
if child_pid != 0: # parent
|
if child_pid != 0: # parent
|
||||||
|
|
||||||
# child is in its own process group, manually pass kill signals
|
# child is in its own process group, manually pass kill signals
|
||||||
signal.signal(signal.SIGINT, lambda signum, frame: os.kill(child_pid, signal.SIGINT))
|
signal.signal(signal.SIGINT, lambda signum, frame: os.kill(child_pid, signal.SIGINT))
|
||||||
|
|
|
@ -5,10 +5,10 @@ from __future__ import print_function
|
||||||
import tensorflow as tf # pylint: disable=import-error
|
import tensorflow as tf # pylint: disable=import-error
|
||||||
import os
|
import os
|
||||||
import sys
|
import sys
|
||||||
import tensorflow.keras as keras # pylint: disable=import-error
|
import tensorflow.keras as keras # pylint: disable=import-error
|
||||||
import numpy as np
|
import numpy as np
|
||||||
from tensorflow.keras.models import Model # pylint: disable=import-error
|
from tensorflow.keras.models import Model # pylint: disable=import-error
|
||||||
from tensorflow.keras.models import load_model # pylint: disable=import-error
|
from tensorflow.keras.models import load_model # pylint: disable=import-error
|
||||||
|
|
||||||
def read(sz):
|
def read(sz):
|
||||||
dd = []
|
dd = []
|
||||||
|
|
|
@ -64,7 +64,7 @@ class VisionTest():
|
||||||
raise ValueError("Bad model name: {}".format(model))
|
raise ValueError("Bad model name: {}".format(model))
|
||||||
|
|
||||||
prevdir = os.getcwd()
|
prevdir = os.getcwd()
|
||||||
os.chdir(_visiond_dir) # tmp hack to find kernels
|
os.chdir(_visiond_dir) # tmp hack to find kernels
|
||||||
os.environ['BASEDIR'] = BASEDIR
|
os.environ['BASEDIR'] = BASEDIR
|
||||||
self._visiontest_c = self.clib.visiontest_create(
|
self._visiontest_c = self.clib.visiontest_create(
|
||||||
temporal_model, disable_model, self._input_size[0], self._input_size[1],
|
temporal_model, disable_model, self._input_size[0], self._input_size[1],
|
||||||
|
|
|
@ -60,9 +60,9 @@ def car_plant(pos, speed, grade, gas, brake):
|
||||||
#*** longitudinal model ***
|
#*** longitudinal model ***
|
||||||
# find speed where peak torque meets peak power
|
# find speed where peak torque meets peak power
|
||||||
force_brake = brake * force_brake_peak * brake_to_peak_linear_slope
|
force_brake = brake * force_brake_peak * brake_to_peak_linear_slope
|
||||||
if speed < speed_base: # torque control
|
if speed < speed_base: # torque control
|
||||||
force_gas = gas * force_peak * gas_to_peak_linear_slope
|
force_gas = gas * force_peak * gas_to_peak_linear_slope
|
||||||
else: # power control
|
else: # power control
|
||||||
force_gas = gas * power_peak / speed * gas_to_peak_linear_slope
|
force_gas = gas * power_peak / speed * gas_to_peak_linear_slope
|
||||||
|
|
||||||
force_grade = - grade * mass # positive grade means uphill
|
force_grade = - grade * mass # positive grade means uphill
|
||||||
|
|
|
@ -21,7 +21,7 @@ segments = [
|
||||||
("HYUNDAI", "5b7c365c50084530|2020-04-15--16-13-24--3"), # HYUNDAI.SONATA
|
("HYUNDAI", "5b7c365c50084530|2020-04-15--16-13-24--3"), # HYUNDAI.SONATA
|
||||||
#("CHRYSLER", "b6e1317e1bfbefa6|2020-03-04--13-11-40"), # CHRYSLER.JEEP_CHEROKEE
|
#("CHRYSLER", "b6e1317e1bfbefa6|2020-03-04--13-11-40"), # CHRYSLER.JEEP_CHEROKEE
|
||||||
("SUBARU", "7873afaf022d36e2|2019-07-03--18-46-44--0"), # SUBARU.IMPREZA
|
("SUBARU", "7873afaf022d36e2|2019-07-03--18-46-44--0"), # SUBARU.IMPREZA
|
||||||
("VOLKSWAGEN", "76b83eb0245de90e|2020-03-05--19-16-05--3"), # VW.GOLF
|
("VOLKSWAGEN", "76b83eb0245de90e|2020-03-05--19-16-05--3"), # VW.GOLF
|
||||||
("NISSAN", "fbbfa6af821552b9|2020-03-03--08-09-43--0"), # NISSAN.XTRAIL
|
("NISSAN", "fbbfa6af821552b9|2020-03-03--08-09-43--0"), # NISSAN.XTRAIL
|
||||||
|
|
||||||
# Enable when port is tested and dascamOnly is no longer set
|
# Enable when port is tested and dascamOnly is no longer set
|
||||||
|
|
|
@ -343,7 +343,7 @@ routes = {
|
||||||
'enableDsu': False,
|
'enableDsu': False,
|
||||||
},
|
},
|
||||||
"1dd19ceed0ee2b48|2018-12-22--17-36-49": {
|
"1dd19ceed0ee2b48|2018-12-22--17-36-49": {
|
||||||
'carFingerprint': TOYOTA.LEXUS_IS, # 300 hybrid
|
'carFingerprint': TOYOTA.LEXUS_IS, # 300 hybrid
|
||||||
'enableCamera': True,
|
'enableCamera': True,
|
||||||
'enableDsu': False,
|
'enableDsu': False,
|
||||||
},
|
},
|
||||||
|
|
|
@ -6,7 +6,7 @@ from azure.storage.blob import BlockBlobService
|
||||||
from selfdrive.test.test_car_models import routes as test_car_models_routes
|
from selfdrive.test.test_car_models import routes as test_car_models_routes
|
||||||
from selfdrive.test.process_replay.test_processes import segments as replay_segments
|
from selfdrive.test.process_replay.test_processes import segments as replay_segments
|
||||||
from xx.chffr.lib import azureutil # pylint: disable=import-error
|
from xx.chffr.lib import azureutil # pylint: disable=import-error
|
||||||
from xx.chffr.lib.storage import _DATA_ACCOUNT_PRODUCTION, _DATA_ACCOUNT_CI, _DATA_BUCKET_PRODUCTION # pylint: disable=import-error
|
from xx.chffr.lib.storage import _DATA_ACCOUNT_PRODUCTION, _DATA_ACCOUNT_CI, _DATA_BUCKET_PRODUCTION # pylint: disable=import-error
|
||||||
|
|
||||||
SOURCES = [
|
SOURCES = [
|
||||||
(_DATA_ACCOUNT_PRODUCTION, _DATA_BUCKET_PRODUCTION),
|
(_DATA_ACCOUNT_PRODUCTION, _DATA_BUCKET_PRODUCTION),
|
||||||
|
|
|
@ -53,9 +53,9 @@ textPrint = TextPrint()
|
||||||
# -------- Main Program Loop -----------
|
# -------- Main Program Loop -----------
|
||||||
while not done:
|
while not done:
|
||||||
# EVENT PROCESSING STEP
|
# EVENT PROCESSING STEP
|
||||||
for event in pygame.event.get(): # User did something
|
for event in pygame.event.get(): # User did something
|
||||||
if event.type == pygame.QUIT: # If user clicked close
|
if event.type == pygame.QUIT: # If user clicked close
|
||||||
done = True # Flag that we are done so we exit this loop
|
done = True # Flag that we are done so we exit this loop
|
||||||
|
|
||||||
# Possible joystick actions: JOYAXISMOTION JOYBALLMOTION JOYBUTTONDOWN JOYBUTTONUP JOYHATMOTION
|
# Possible joystick actions: JOYAXISMOTION JOYBALLMOTION JOYBUTTONDOWN JOYBUTTONUP JOYHATMOTION
|
||||||
if event.type == pygame.JOYBUTTONDOWN:
|
if event.type == pygame.JOYBUTTONDOWN:
|
||||||
|
|
|
@ -30,8 +30,8 @@ def joystick_thread():
|
||||||
# -------- Main Program Loop -----------
|
# -------- Main Program Loop -----------
|
||||||
while True:
|
while True:
|
||||||
# EVENT PROCESSING STEP
|
# EVENT PROCESSING STEP
|
||||||
for event in pygame.event.get(): # User did something
|
for event in pygame.event.get(): # User did something
|
||||||
if event.type == pygame.QUIT: # If user clicked close
|
if event.type == pygame.QUIT: # If user clicked close
|
||||||
pass
|
pass
|
||||||
# Available joystick events: JOYAXISMOTION JOYBALLMOTION JOYBUTTONDOWN JOYBUTTONUP JOYHATMOTION
|
# Available joystick events: JOYAXISMOTION JOYBALLMOTION JOYBUTTONDOWN JOYBUTTONUP JOYHATMOTION
|
||||||
if event.type == pygame.JOYBUTTONDOWN:
|
if event.type == pygame.JOYBUTTONDOWN:
|
||||||
|
|
|
@ -332,7 +332,7 @@ class VideoStreamDecompressor:
|
||||||
self.pix_fmt = pix_fmt
|
self.pix_fmt = pix_fmt
|
||||||
|
|
||||||
if pix_fmt == "yuv420p":
|
if pix_fmt == "yuv420p":
|
||||||
self.out_size = w*h*3//2 # yuv420p
|
self.out_size = w*h*3//2 # yuv420p
|
||||||
elif pix_fmt in ("rgb24", "yuv444p"):
|
elif pix_fmt in ("rgb24", "yuv444p"):
|
||||||
self.out_size = w*h*3
|
self.out_size = w*h*3
|
||||||
else:
|
else:
|
||||||
|
|
|
@ -72,7 +72,7 @@ if __name__ == "__main__":
|
||||||
|
|
||||||
if kb.kbhit():
|
if kb.kbhit():
|
||||||
c = kb.getch()
|
c = kb.getch()
|
||||||
if ord(c) == 27: # ESC
|
if ord(c) == 27: # ESC
|
||||||
break
|
break
|
||||||
print(c)
|
print(c)
|
||||||
|
|
||||||
|
|
|
@ -68,7 +68,7 @@ def ui_thread(addr, frame_address):
|
||||||
else:
|
else:
|
||||||
# actually RGB
|
# actually RGB
|
||||||
img = np.frombuffer(yuv_img, dtype=np.uint8).reshape((_FULL_FRAME_SIZE[1], _FULL_FRAME_SIZE[0], 3))
|
img = np.frombuffer(yuv_img, dtype=np.uint8).reshape((_FULL_FRAME_SIZE[1], _FULL_FRAME_SIZE[0], 3))
|
||||||
img = img[:, :, ::-1] # Convert BGR to RGB
|
img = img[:, :, ::-1] # Convert BGR to RGB
|
||||||
|
|
||||||
height, width = img.shape[:2]
|
height, width = img.shape[:2]
|
||||||
img_resized = cv2.resize(
|
img_resized = cv2.resize(
|
||||||
|
|
|
@ -31,7 +31,7 @@ if __name__ == "__main__":
|
||||||
sm.update(timeout=1)
|
sm.update(timeout=1)
|
||||||
rgb_img_raw = fpkt.frame.image
|
rgb_img_raw = fpkt.frame.image
|
||||||
imgff = np.frombuffer(rgb_img_raw, dtype=np.uint8).reshape((FULL_FRAME_SIZE[1], FULL_FRAME_SIZE[0], 3))
|
imgff = np.frombuffer(rgb_img_raw, dtype=np.uint8).reshape((FULL_FRAME_SIZE[1], FULL_FRAME_SIZE[0], 3))
|
||||||
imgff = imgff[:, :, ::-1] # Convert BGR to RGB
|
imgff = imgff[:, :, ::-1] # Convert BGR to RGB
|
||||||
|
|
||||||
if sm.updated['liveCalibration']:
|
if sm.updated['liveCalibration']:
|
||||||
intrinsic_matrix = eon_intrinsics
|
intrinsic_matrix = eon_intrinsics
|
||||||
|
|
|
@ -134,7 +134,7 @@ def ui_thread(addr, frame_address):
|
||||||
|
|
||||||
if rgb_img_raw and len(rgb_img_raw) == FULL_FRAME_SIZE[0] * FULL_FRAME_SIZE[1] * 3:
|
if rgb_img_raw and len(rgb_img_raw) == FULL_FRAME_SIZE[0] * FULL_FRAME_SIZE[1] * 3:
|
||||||
imgff = np.frombuffer(rgb_img_raw, dtype=np.uint8).reshape((FULL_FRAME_SIZE[1], FULL_FRAME_SIZE[0], 3))
|
imgff = np.frombuffer(rgb_img_raw, dtype=np.uint8).reshape((FULL_FRAME_SIZE[1], FULL_FRAME_SIZE[0], 3))
|
||||||
imgff = imgff[:, :, ::-1] # Convert BGR to RGB
|
imgff = imgff[:, :, ::-1] # Convert BGR to RGB
|
||||||
cv2.warpAffine(imgff, np.dot(img_transform, _BB_TO_FULL_FRAME)[:2],
|
cv2.warpAffine(imgff, np.dot(img_transform, _BB_TO_FULL_FRAME)[:2],
|
||||||
(img.shape[1], img.shape[0]), dst=img, flags=cv2.WARP_INVERSE_MAP)
|
(img.shape[1], img.shape[0]), dst=img, flags=cv2.WARP_INVERSE_MAP)
|
||||||
|
|
||||||
|
|
|
@ -110,7 +110,7 @@ class UnloggerWorker(object):
|
||||||
print("FRAME(%d) LAG -- %.2f ms" % (frame_id, fr_time*1000.0))
|
print("FRAME(%d) LAG -- %.2f ms" % (frame_id, fr_time*1000.0))
|
||||||
|
|
||||||
if img is not None:
|
if img is not None:
|
||||||
img = img[:, :, ::-1] # Convert RGB to BGR, which is what the camera outputs
|
img = img[:, :, ::-1] # Convert RGB to BGR, which is what the camera outputs
|
||||||
img = img.flatten()
|
img = img.flatten()
|
||||||
smsg.frame.image = img.tobytes()
|
smsg.frame.image = img.tobytes()
|
||||||
|
|
||||||
|
@ -259,7 +259,7 @@ def unlogger_thread(command_address, forward_commands_address, data_address, run
|
||||||
msg_time_offset = msg_time_seconds - msg_start_time
|
msg_time_offset = msg_time_seconds - msg_start_time
|
||||||
real_time_offset = realtime.sec_since_boot() - real_start_time
|
real_time_offset = realtime.sec_since_boot() - real_start_time
|
||||||
lag = msg_time_offset - real_time_offset
|
lag = msg_time_offset - real_time_offset
|
||||||
if lag > 0 and lag < 30: # a large jump is OK, likely due to an out of order segment
|
if lag > 0 and lag < 30: # a large jump is OK, likely due to an out of order segment
|
||||||
if lag > 1:
|
if lag > 1:
|
||||||
print("sleeping for", lag)
|
print("sleeping for", lag)
|
||||||
time.sleep(lag)
|
time.sleep(lag)
|
||||||
|
@ -312,17 +312,17 @@ def keyboard_controller_thread(q, route_start_time):
|
||||||
kb = KBHit()
|
kb = KBHit()
|
||||||
while 1:
|
while 1:
|
||||||
c = kb.getch()
|
c = kb.getch()
|
||||||
if c == 'm': # Move forward by 1m
|
if c == 'm': # Move forward by 1m
|
||||||
q.send_pyobj(SeekRelativeTime(60))
|
q.send_pyobj(SeekRelativeTime(60))
|
||||||
elif c == 'M': # Move backward by 1m
|
elif c == 'M': # Move backward by 1m
|
||||||
q.send_pyobj(SeekRelativeTime(-60))
|
q.send_pyobj(SeekRelativeTime(-60))
|
||||||
elif c == 's': # Move forward by 10s
|
elif c == 's': # Move forward by 10s
|
||||||
q.send_pyobj(SeekRelativeTime(10))
|
q.send_pyobj(SeekRelativeTime(10))
|
||||||
elif c == 'S': # Move backward by 10s
|
elif c == 'S': # Move backward by 10s
|
||||||
q.send_pyobj(SeekRelativeTime(-10))
|
q.send_pyobj(SeekRelativeTime(-10))
|
||||||
elif c == 'G': # Move backward by 10s
|
elif c == 'G': # Move backward by 10s
|
||||||
q.send_pyobj(SeekAbsoluteTime(0.))
|
q.send_pyobj(SeekAbsoluteTime(0.))
|
||||||
elif c == "\x20": # Space bar.
|
elif c == "\x20": # Space bar.
|
||||||
q.send_pyobj(TogglePause())
|
q.send_pyobj(TogglePause())
|
||||||
elif c == "\n":
|
elif c == "\n":
|
||||||
try:
|
try:
|
||||||
|
|
|
@ -169,7 +169,7 @@ def go(q):
|
||||||
m = message.split('_')
|
m = message.split('_')
|
||||||
if m[0] == "steer":
|
if m[0] == "steer":
|
||||||
steer_angle_out = float(m[1])
|
steer_angle_out = float(m[1])
|
||||||
fake_wheel.set_angle(steer_angle_out) # touching the wheel overrides fake wheel angle
|
fake_wheel.set_angle(steer_angle_out) # touching the wheel overrides fake wheel angle
|
||||||
# print(" === steering overriden === ")
|
# print(" === steering overriden === ")
|
||||||
if m[0] == "throttle":
|
if m[0] == "throttle":
|
||||||
throttle_out = float(m[1]) / 100.
|
throttle_out = float(m[1]) / 100.
|
||||||
|
@ -200,7 +200,7 @@ def go(q):
|
||||||
speed = math.sqrt(vel.x**2 + vel.y**2 + vel.z**2) * 3.6
|
speed = math.sqrt(vel.x**2 + vel.y**2 + vel.z**2) * 3.6
|
||||||
can_function(pm, speed, fake_wheel.angle, rk.frame, cruise_button=cruise_button, is_engaged=is_openpilot_engaged)
|
can_function(pm, speed, fake_wheel.angle, rk.frame, cruise_button=cruise_button, is_engaged=is_openpilot_engaged)
|
||||||
|
|
||||||
if rk.frame % 1 == 0: # 20Hz?
|
if rk.frame % 1 == 0: # 20Hz?
|
||||||
throttle_op, brake_op, steer_torque_op = sendcan_function(sendcan)
|
throttle_op, brake_op, steer_torque_op = sendcan_function(sendcan)
|
||||||
# print(" === torq, ",steer_torque_op, " ===")
|
# print(" === torq, ",steer_torque_op, " ===")
|
||||||
if is_openpilot_engaged:
|
if is_openpilot_engaged:
|
||||||
|
|
|
@ -1,7 +1,7 @@
|
||||||
class FakeSteeringWheel():
|
class FakeSteeringWheel():
|
||||||
def __init__(self, dt=0.01):
|
def __init__(self, dt=0.01):
|
||||||
# physical params
|
# physical params
|
||||||
self.DAC = 4. / 0.625 # convert torque value from can to Nm
|
self.DAC = 4. / 0.625 # convert torque value from can to Nm
|
||||||
self.k = 0.035
|
self.k = 0.035
|
||||||
self.centering_k = 4.1 * 0.9
|
self.centering_k = 4.1 * 0.9
|
||||||
self.b = 0.1 * 0.8
|
self.b = 0.1 * 0.8
|
||||||
|
@ -9,7 +9,7 @@ class FakeSteeringWheel():
|
||||||
self.dt = dt
|
self.dt = dt
|
||||||
# ...
|
# ...
|
||||||
|
|
||||||
self.angle = 0. # start centered
|
self.angle = 0. # start centered
|
||||||
# self.omega = 0.
|
# self.omega = 0.
|
||||||
|
|
||||||
def response(self, torque, vego=0):
|
def response(self, torque, vego=0):
|
||||||
|
@ -19,7 +19,7 @@ class FakeSteeringWheel():
|
||||||
# self.omega += self.dt * (exerted_torque + centering_torque + damping_torque) / self.I
|
# self.omega += self.dt * (exerted_torque + centering_torque + damping_torque) / self.I
|
||||||
# self.omega = np.clip(self.omega, -1.1, 1.1)
|
# self.omega = np.clip(self.omega, -1.1, 1.1)
|
||||||
# self.angle += self.dt * self.omega
|
# self.angle += self.dt * self.omega
|
||||||
self.angle += self.dt * self.k * exerted_torque # aristotle
|
self.angle += self.dt * self.k * exerted_torque # aristotle
|
||||||
|
|
||||||
# print(" ========== ")
|
# print(" ========== ")
|
||||||
# print("angle,", self.angle)
|
# print("angle,", self.angle)
|
||||||
|
|
|
@ -99,22 +99,22 @@ def wheel_poll_thread(q):
|
||||||
# Get the device name.
|
# Get the device name.
|
||||||
#buf = bytearray(63)
|
#buf = bytearray(63)
|
||||||
buf = array.array('B', [0] * 64)
|
buf = array.array('B', [0] * 64)
|
||||||
ioctl(jsdev, 0x80006a13 + (0x10000 * len(buf)), buf) # JSIOCGNAME(len)
|
ioctl(jsdev, 0x80006a13 + (0x10000 * len(buf)), buf) # JSIOCGNAME(len)
|
||||||
js_name = buf.tobytes().rstrip(b'\x00').decode('utf-8')
|
js_name = buf.tobytes().rstrip(b'\x00').decode('utf-8')
|
||||||
print('Device name: %s' % js_name)
|
print('Device name: %s' % js_name)
|
||||||
|
|
||||||
# Get number of axes and buttons.
|
# Get number of axes and buttons.
|
||||||
buf = array.array('B', [0])
|
buf = array.array('B', [0])
|
||||||
ioctl(jsdev, 0x80016a11, buf) # JSIOCGAXES
|
ioctl(jsdev, 0x80016a11, buf) # JSIOCGAXES
|
||||||
num_axes = buf[0]
|
num_axes = buf[0]
|
||||||
|
|
||||||
buf = array.array('B', [0])
|
buf = array.array('B', [0])
|
||||||
ioctl(jsdev, 0x80016a12, buf) # JSIOCGBUTTONS
|
ioctl(jsdev, 0x80016a12, buf) # JSIOCGBUTTONS
|
||||||
num_buttons = buf[0]
|
num_buttons = buf[0]
|
||||||
|
|
||||||
# Get the axis map.
|
# Get the axis map.
|
||||||
buf = array.array('B', [0] * 0x40)
|
buf = array.array('B', [0] * 0x40)
|
||||||
ioctl(jsdev, 0x80406a32, buf) # JSIOCGAXMAP
|
ioctl(jsdev, 0x80406a32, buf) # JSIOCGAXMAP
|
||||||
|
|
||||||
for axis in buf[:num_axes]:
|
for axis in buf[:num_axes]:
|
||||||
axis_name = axis_names.get(axis, 'unknown(0x%02x)' % axis)
|
axis_name = axis_names.get(axis, 'unknown(0x%02x)' % axis)
|
||||||
|
@ -123,7 +123,7 @@ def wheel_poll_thread(q):
|
||||||
|
|
||||||
# Get the button map.
|
# Get the button map.
|
||||||
buf = array.array('H', [0] * 200)
|
buf = array.array('H', [0] * 200)
|
||||||
ioctl(jsdev, 0x80406a34, buf) # JSIOCGBTNMAP
|
ioctl(jsdev, 0x80406a34, buf) # JSIOCGBTNMAP
|
||||||
|
|
||||||
for btn in buf[:num_buttons]:
|
for btn in buf[:num_buttons]:
|
||||||
btn_name = button_names.get(btn, 'unknown(0x%03x)' % btn)
|
btn_name = button_names.get(btn, 'unknown(0x%03x)' % btn)
|
||||||
|
@ -145,42 +145,42 @@ def wheel_poll_thread(q):
|
||||||
evbuf = jsdev.read(8)
|
evbuf = jsdev.read(8)
|
||||||
time, value, mtype, number = struct.unpack('IhBB', evbuf)
|
time, value, mtype, number = struct.unpack('IhBB', evbuf)
|
||||||
# print(mtype, number, value)
|
# print(mtype, number, value)
|
||||||
if mtype & 0x02: # wheel & paddles
|
if mtype & 0x02: # wheel & paddles
|
||||||
axis = axis_map[number]
|
axis = axis_map[number]
|
||||||
|
|
||||||
if axis == "z": # gas
|
if axis == "z": # gas
|
||||||
fvalue = value / 32767.0
|
fvalue = value / 32767.0
|
||||||
axis_states[axis] = fvalue
|
axis_states[axis] = fvalue
|
||||||
normalized = (1 - fvalue) * 50
|
normalized = (1 - fvalue) * 50
|
||||||
q.put(str("throttle_%f" % normalized))
|
q.put(str("throttle_%f" % normalized))
|
||||||
|
|
||||||
if axis == "rz": # brake
|
if axis == "rz": # brake
|
||||||
fvalue = value / 32767.0
|
fvalue = value / 32767.0
|
||||||
axis_states[axis] = fvalue
|
axis_states[axis] = fvalue
|
||||||
normalized = (1 - fvalue) * 50
|
normalized = (1 - fvalue) * 50
|
||||||
q.put(str("brake_%f" % normalized))
|
q.put(str("brake_%f" % normalized))
|
||||||
|
|
||||||
if axis == "x": # steer angle
|
if axis == "x": # steer angle
|
||||||
fvalue = value / 32767.0
|
fvalue = value / 32767.0
|
||||||
axis_states[axis] = fvalue
|
axis_states[axis] = fvalue
|
||||||
normalized = fvalue
|
normalized = fvalue
|
||||||
q.put(str("steer_%f" % normalized))
|
q.put(str("steer_%f" % normalized))
|
||||||
|
|
||||||
if mtype & 0x01: # buttons
|
if mtype & 0x01: # buttons
|
||||||
if number in [0, 19]: # X
|
if number in [0, 19]: # X
|
||||||
if value == 1: # press down
|
if value == 1: # press down
|
||||||
q.put(str("cruise_down"))
|
q.put(str("cruise_down"))
|
||||||
|
|
||||||
if number in [3, 18]: # triangle
|
if number in [3, 18]: # triangle
|
||||||
if value == 1: # press down
|
if value == 1: # press down
|
||||||
q.put(str("cruise_up"))
|
q.put(str("cruise_up"))
|
||||||
|
|
||||||
if number in [1, 6]: # square
|
if number in [1, 6]: # square
|
||||||
if value == 1: # press down
|
if value == 1: # press down
|
||||||
q.put(str("cruise_cancel"))
|
q.put(str("cruise_cancel"))
|
||||||
|
|
||||||
if number in [10, 21]: # R3
|
if number in [10, 21]: # R3
|
||||||
if value == 1: # press down
|
if value == 1: # press down
|
||||||
q.put(str("reverse_switch"))
|
q.put(str("reverse_switch"))
|
||||||
|
|
||||||
if __name__ == '__main__':
|
if __name__ == '__main__':
|
||||||
|
|
|
@ -2,8 +2,8 @@
|
||||||
import numpy as np
|
import numpy as np
|
||||||
|
|
||||||
# copied from common.transformations/camera.py
|
# copied from common.transformations/camera.py
|
||||||
eon_dcam_focal_length = 860.0 # pixels
|
eon_dcam_focal_length = 860.0 # pixels
|
||||||
webcam_focal_length = 908.0 # pixels
|
webcam_focal_length = 908.0 # pixels
|
||||||
|
|
||||||
eon_dcam_intrinsics = np.array([
|
eon_dcam_intrinsics = np.array([
|
||||||
[eon_dcam_focal_length, 0, 1152/2.],
|
[eon_dcam_focal_length, 0, 1152/2.],
|
||||||
|
|
|
@ -2,10 +2,10 @@
|
||||||
import numpy as np
|
import numpy as np
|
||||||
|
|
||||||
# copied from common.transformations/camera.py
|
# copied from common.transformations/camera.py
|
||||||
eon_focal_length = 910.0 # pixels
|
eon_focal_length = 910.0 # pixels
|
||||||
eon_dcam_focal_length = 860.0 # pixels
|
eon_dcam_focal_length = 860.0 # pixels
|
||||||
|
|
||||||
webcam_focal_length = -908.0/1.5 # pixels
|
webcam_focal_length = -908.0/1.5 # pixels
|
||||||
|
|
||||||
eon_intrinsics = np.array([
|
eon_intrinsics = np.array([
|
||||||
[eon_focal_length, 0., 1164/2.],
|
[eon_focal_length, 0., 1164/2.],
|
||||||
|
|
Loading…
Reference in New Issue