joystick controls working
Kevin Roscom
parent
fdc2434de6
commit
74d47446a9
2
opendbc
2
opendbc
|
|
@ -1 +1 @@
|
|||
Subproject commit 12a00517b86d97f5dd3d6b4b90c64038f1f567fe
|
||||
Subproject commit 525ba532df7357066fd73c139dc4e2f297877322
|
||||
|
|
@ -37,9 +37,6 @@ Panda::Panda(){
|
|||
|
||||
hw_type = get_hw_type();
|
||||
|
||||
assert((hw_type != cereal::PandaState::PandaType::WHITE_PANDA) &&
|
||||
(hw_type != cereal::PandaState::PandaType::GREY_PANDA));
|
||||
|
||||
has_rtc = (hw_type == cereal::PandaState::PandaType::UNO) ||
|
||||
(hw_type == cereal::PandaState::PandaType::DOS);
|
||||
|
||||
|
|
|
|||
|
|
@ -149,6 +149,7 @@ def fingerprint(logcan, sendcan):
|
|||
|
||||
# bail if no cars left or we've been waiting for more than 2s
|
||||
failed = (all(len(cc) == 0 for cc in candidate_cars.values()) and frame > frame_fingerprint) or frame > 200
|
||||
car_fingerprint = "ALBATROSS"
|
||||
succeeded = car_fingerprint is not None
|
||||
done = failed or succeeded
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,129 @@
|
|||
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.ocelot.ocelotcan import create_steer_command, create_gas_command, create_brake_cmd
|
||||
from selfdrive.car.ocelot.values import CAR, SteerLimitParams
|
||||
from opendbc.can.packer import CANPacker
|
||||
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
|
||||
# Accel limits
|
||||
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_MIN = -3.0 # 3 m/s2
|
||||
ACCEL_SCALE = max(ACCEL_MAX, -ACCEL_MIN)
|
||||
|
||||
def accel_hysteresis(accel, accel_steady, enabled):
|
||||
|
||||
# for small accel oscillations within ACCEL_HYST_GAP, don't change the accel command
|
||||
if not enabled:
|
||||
# send 0 when disabled, otherwise acc faults
|
||||
accel_steady = 0.
|
||||
elif accel > accel_steady + ACCEL_HYST_GAP:
|
||||
accel_steady = accel - ACCEL_HYST_GAP
|
||||
elif accel < accel_steady - ACCEL_HYST_GAP:
|
||||
accel_steady = accel + ACCEL_HYST_GAP
|
||||
accel = accel_steady
|
||||
|
||||
return accel, accel_steady
|
||||
|
||||
|
||||
class CarController():
|
||||
def __init__(self, dbc_name, CP, VM):
|
||||
self.last_steer = 0
|
||||
self.accel_steady = 0.
|
||||
self.alert_active = False
|
||||
self.last_standstill = False
|
||||
self.standstill_req = False
|
||||
|
||||
self.last_fault_frame = -200
|
||||
self.steer_rate_limited = False
|
||||
|
||||
self.last_brake = 0.
|
||||
|
||||
self.packer = CANPacker(dbc_name)
|
||||
|
||||
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
|
||||
left_line, right_line, lead, left_lane_depart, right_lane_depart):
|
||||
|
||||
# *** compute control surfaces ***
|
||||
|
||||
# gas and brake
|
||||
|
||||
apply_gas = clip(actuators.gas, 0., 1.)
|
||||
|
||||
if CS.CP.enableGasInterceptor:
|
||||
# send only negative accel if interceptor is detected. otherwise, send the regular value
|
||||
# +0.06 offset to reduce ABS pump usage when OP is engaged
|
||||
apply_accel = 0.06 - actuators.brake
|
||||
else:
|
||||
apply_accel = actuators.gas - actuators.brake
|
||||
|
||||
apply_accel, self.accel_steady = accel_hysteresis(apply_accel, self.accel_steady, enabled)
|
||||
apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX)
|
||||
|
||||
# steer torque
|
||||
new_steer = int(round(actuators.steer * SteerLimitParams.STEER_MAX))
|
||||
apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, SteerLimitParams)
|
||||
self.steer_rate_limited = new_steer != apply_steer
|
||||
|
||||
# # only cut torque when steer state is a known fault
|
||||
# if CS.steer_state in [9, 25]:
|
||||
# self.last_fault_frame = frame
|
||||
|
||||
# Cut steering for 2s after fault
|
||||
if not enabled: #or (frame - self.last_fault_frame < 200):
|
||||
apply_steer = 0
|
||||
apply_steer_req = 0
|
||||
else:
|
||||
apply_steer_req = 1
|
||||
|
||||
|
||||
# on entering standstill, send standstill request
|
||||
if CS.out.standstill and not self.last_standstill:
|
||||
self.standstill_req = True
|
||||
|
||||
self.last_steer = apply_steer
|
||||
self.last_accel = apply_accel
|
||||
self.last_standstill = CS.out.standstill
|
||||
|
||||
can_sends = []
|
||||
|
||||
#*** control msgs ***
|
||||
#print("steer {0} {1} {2} {3}".format(apply_steer, min_lim, max_lim, CS.steer_torque_motor)
|
||||
|
||||
# toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
|
||||
# sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
|
||||
# on consecutive messages
|
||||
|
||||
can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, frame))
|
||||
|
||||
if (frame % 2 == 0):
|
||||
# send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
|
||||
# This prevents unexpected pedal range rescaling
|
||||
can_sends.append(create_gas_command(self.packer, apply_gas, frame//2))
|
||||
|
||||
can_sends.append(create_brake_cmd(self.packer, enabled, actuators.brake, frame//2))
|
||||
|
||||
# ui mesg is at 100Hz but we send asap if:
|
||||
# - there is something to display
|
||||
# - there is something to stop displaying
|
||||
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 ***
|
||||
|
||||
# for (addr, ecu, cars, bus, fr_step, vl) in STATIC_MSGS:
|
||||
# if frame % fr_step == 0 and ecu in self.fake_ecus and CS.CP.carFingerprint in cars:
|
||||
# can_sends.append(make_can_msg(addr, vl, bus))
|
||||
|
||||
return can_sends
|
||||
|
|
@ -0,0 +1,128 @@
|
|||
from cereal import car
|
||||
from common.numpy_fast import mean
|
||||
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
|
||||
|
||||
|
||||
class CarState(CarStateBase):
|
||||
def __init__(self, CP):
|
||||
super().__init__(CP)
|
||||
print(DBC[CP.carFingerprint]['chassis'])
|
||||
can_define = CANDefine(DBC[CP.carFingerprint]['chassis'])
|
||||
self.shifter_values = can_define.dv["GEAR_PACKET"]['GEAR']
|
||||
self.setSpeed = 0
|
||||
self.enabled = False
|
||||
self.oldEnabled = 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))
|
||||
|
||||
#Ibooster data
|
||||
ret.brakePressed = False #cp.vl["BRAKE_STATUS"]['IBOOSTER_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 = 0
|
||||
ret.gasPressed = False
|
||||
|
||||
#calculate speed from wheel speeds
|
||||
ret.vEgoRaw = mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr])
|
||||
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
|
||||
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']
|
||||
|
||||
|
||||
#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
|
||||
ret.steerWarning = False #cp.vl["STEERING_STATUS"]['STEERING_OK'] != 0
|
||||
|
||||
ret.cruiseState.available = True
|
||||
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.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
|
||||
|
||||
#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.vl["HIM_CTRLS"]['SPEEDDN_BTN']:
|
||||
# ret.cruiseState.speed = self.setSpeed - 5*CV.MPH_TO_MS
|
||||
|
||||
ret.cruiseState.enabled = self.enabled
|
||||
|
||||
|
||||
return ret
|
||||
|
||||
|
||||
|
||||
@staticmethod
|
||||
def get_can_parser(CP):
|
||||
|
||||
signals = [
|
||||
# sig_name, sig_address, default
|
||||
("BRAKE_OK", "ACTUATOR_BRAKE_STATUS", 0),
|
||||
("STEERING_OK", "ACTUATOR_STEERING_STATUS", 0),
|
||||
]
|
||||
|
||||
checks = [
|
||||
("ACTUATOR_BRAKE_STATUS", 80),
|
||||
]
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
|
||||
|
||||
@staticmethod
|
||||
def get_body_can_parser(CP):
|
||||
|
||||
signals = [
|
||||
]
|
||||
|
||||
# use steering message to check if panda is connected to frc
|
||||
checks = [
|
||||
]
|
||||
|
||||
# if CP.carFingerprint == CAR.SMART_ROADSTER_COUPE:
|
||||
# signals.append(("RIGHT_DOOR", "BODYCONTROL",0))
|
||||
# signals.append(("LEFT_DOOR", "BODYCONTROL",0))
|
||||
# signals.append(("LEFT_SIGNAL", "BODYCONTROL",0))
|
||||
# signals.append(("RIGHT_SIGNAL", "BODYCONTROL",0))
|
||||
# signals.append(("ESP_STATUS", "ABS",0))
|
||||
# signals.append(("WHEELSPEED_FL", "SMARTROADSTERWHEELSPEEDS",0))
|
||||
# signals.append(("WHEELSPEED_FR", "SMARTROADSTERWHEELSPEEDS",0))
|
||||
# signals.append(("WHEELSPEED_RL", "SMARTROADSTERWHEELSPEEDS",0))
|
||||
# signals.append(("WHEELSPEED_RR", "SMARTROADSTERWHEELSPEEDS",0))
|
||||
# signals.append(("BRAKEPEDAL", "ABS",0))
|
||||
# signals.append(("GEARPOSITION","GEARBOX", 0))
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]['chassis'], signals, checks, 1)
|
||||
|
|
@ -0,0 +1,121 @@
|
|||
#!/usr/bin/env python3
|
||||
from cereal import car
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.car.ocelot.values import CAR, FINGERPRINTS
|
||||
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint
|
||||
from selfdrive.swaglog import cloudlog
|
||||
from selfdrive.car.interfaces import CarInterfaceBase
|
||||
|
||||
EventName = car.CarEvent.EventName
|
||||
|
||||
class CarInterface(CarInterfaceBase):
|
||||
@staticmethod
|
||||
def compute_gb(accel, speed):
|
||||
return float(accel) / 3.0
|
||||
|
||||
@staticmethod
|
||||
def myround(x, base=5):
|
||||
return base * round(x/base)
|
||||
|
||||
@staticmethod
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # pylint: disable=dangerous-default-value
|
||||
ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
|
||||
|
||||
ret.carName = "ocelot"
|
||||
ret.safetyModel = car.CarParams.SafetyModel.allOutput
|
||||
|
||||
ret.steerActuatorDelay = 0.12 # Default delay, Prius has larger delay
|
||||
ret.steerLimitTimer = 0.4
|
||||
|
||||
|
||||
ret.lateralTuning.init('pid')
|
||||
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
|
||||
stop_and_go = True
|
||||
ret.safetyParam = 100
|
||||
ret.wheelbase = 2.36
|
||||
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.steerRateCost = 1.
|
||||
ret.centerToFront = ret.wheelbase * 0.44
|
||||
|
||||
# TODO: get actual value, for now starting with reasonable value for
|
||||
# civic and scaling by mass and wheelbase
|
||||
ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase)
|
||||
|
||||
# TODO: start from empirically derived lateral slip stiffness for the civic and scale by
|
||||
# mass and CG position, so all cars will have approximately similar dyn behaviors
|
||||
ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront,
|
||||
tire_stiffness_factor=tire_stiffness_factor)
|
||||
|
||||
ret.enableGasInterceptor = 0x201 in fingerprint[0]
|
||||
|
||||
ret.openpilotLongitudinalControl = True
|
||||
cloudlog.warning("ECU Gas Interceptor: %r", ret.enableGasInterceptor)
|
||||
|
||||
# min speed to enable ACC. if car can do stop and go, then set enabling speed
|
||||
# to a negative value, so it won't matter.
|
||||
ret.minEnableSpeed = -1.
|
||||
|
||||
ret.longitudinalTuning.deadzoneBP = [0., 9.]
|
||||
ret.longitudinalTuning.deadzoneV = [0., .15]
|
||||
ret.longitudinalTuning.kpBP = [0., 5., 35.]
|
||||
ret.longitudinalTuning.kiBP = [0., 35.]
|
||||
|
||||
if ret.enableGasInterceptor:
|
||||
ret.gasMaxBP = [0., 9., 35]
|
||||
ret.gasMaxV = [0.2, 0.5, 0.7]
|
||||
ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5]
|
||||
ret.longitudinalTuning.kiV = [0.18, 0.12]
|
||||
else:
|
||||
ret.gasMaxBP = [0.]
|
||||
ret.gasMaxV = [0.5]
|
||||
ret.longitudinalTuning.kpV = [3.6, 2.4, 1.5]
|
||||
ret.longitudinalTuning.kiV = [0.54, 0.36]
|
||||
|
||||
return ret
|
||||
|
||||
# returns a car.CarState
|
||||
def update(self, c, can_strings):
|
||||
# ******************* do can recv *******************
|
||||
self.cp.update_strings(can_strings)
|
||||
self.cp_body.update_strings(can_strings)
|
||||
|
||||
ret = self.CS.update(self.cp, self.cp_body)
|
||||
|
||||
ret.canValid = self.cp.can_valid and self.cp_body.can_valid
|
||||
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
|
||||
|
||||
# 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)
|
||||
|
||||
ret.events = events.to_msg()
|
||||
|
||||
self.CS.out = ret.as_reader()
|
||||
return self.CS.out
|
||||
|
||||
# pass in a car.CarControl
|
||||
# to be called @ 100hz
|
||||
def apply(self, c):
|
||||
|
||||
can_sends = self.CC.update(c.enabled, self.CS, self.frame,
|
||||
c.actuators, c.cruiseControl.cancel,
|
||||
c.hudControl.visualAlert, c.hudControl.leftLaneVisible,
|
||||
c.hudControl.rightLaneVisible, c.hudControl.leadVisible,
|
||||
c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart)
|
||||
|
||||
self.frame += 1
|
||||
return can_sends
|
||||
|
|
@ -0,0 +1,33 @@
|
|||
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("ACTUATOR_STEERING_COMMAND", 0, values)
|
||||
|
||||
def create_gas_command(packer, gas_amount, idx):
|
||||
# Common gas pedal msg generator
|
||||
enable = gas_amount > 0.001
|
||||
|
||||
values = {
|
||||
"ENABLE": enable,
|
||||
"COUNTER": idx & 0xF,
|
||||
}
|
||||
|
||||
if enable:
|
||||
values["GAS_COMMAND"] = gas_amount * 255.
|
||||
values["GAS_COMMAND2"] = gas_amount * 255.
|
||||
|
||||
return packer.make_can_msg("PEDAL_GAS_COMMAND", 0, values)
|
||||
|
||||
def create_brake_cmd(packer, enabled, brake, raw_cnt):
|
||||
values = {
|
||||
"BRAKE_POSITION_COMMAND" : 0,
|
||||
"BRAKE_RELATIVE_COMMAND": brake * 252,
|
||||
"BRAKE_MODE": enabled,
|
||||
"COUNTER" : raw_cnt,
|
||||
}
|
||||
return packer.make_can_msg("OCELOT_BRAKE_COMMAND", 0, values)
|
||||
|
|
@ -0,0 +1,62 @@
|
|||
#!/usr/bin/env python3
|
||||
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()
|
||||
|
||||
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
|
||||
|
|
@ -0,0 +1,36 @@
|
|||
# flake8: noqa
|
||||
|
||||
from selfdrive.car import dbc_dict
|
||||
from cereal import car
|
||||
Ecu = car.CarParams.Ecu
|
||||
|
||||
# Steer torque limits
|
||||
class SteerLimitParams:
|
||||
STEER_MAX = 1500
|
||||
STEER_DELTA_UP = 10 # 1.5s time to peak torque
|
||||
STEER_DELTA_DOWN = 25 # always lower than 45 otherwise the Rav4 faults (Prius seems ok with 50)
|
||||
STEER_ERROR_MAX = 350 # max delta between torque cmd and torque motor
|
||||
|
||||
class CAR:
|
||||
SMART_ROADSTER_COUPE = "SMART ROADSTER COUPE 2003-2006"
|
||||
ALBATROSS = "ALBATROSS"
|
||||
|
||||
|
||||
FINGERPRINTS = {
|
||||
CAR.SMART_ROADSTER_COUPE: [{
|
||||
36: 8, 37: 8, 170: 8, 180: 1, 186: 4, 253:8, 254:8, 255: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 512: 6, 513:6, 547: 8, 548: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 2, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 4, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1196: 8, 1222: 8, 1224:8, 1227: 8, 1235: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1596: 8, 1597: 8, 1600: 8, 1664: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2016: 8, 2017: 8, 2018: 8, 2019: 8, 2020: 8, 2021: 8, 2022: 8, 2023: 8, 2024: 8
|
||||
},
|
||||
{
|
||||
36: 8, 37: 8, 170: 8, 180: 4, 186: 4, 253:8, 254:8, 255: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 512: 6, 513:6, 547: 8, 548: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 2, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 4, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1196: 8, 1222: 8, 1224:8, 1227: 8, 1235: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1596: 8, 1597: 8, 1600: 8, 1664: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2016: 8, 2017: 8, 2018: 8, 2019: 8, 2020: 8, 2021: 8, 2022: 8, 2023: 8, 2024: 8
|
||||
}],
|
||||
CAR.ALBATROSS: [{
|
||||
36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 253:8, 254:8, 255: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 512: 6, 513:6, 547: 8, 548: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 2, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 4, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1196: 8, 1222: 8, 1224:8, 1227: 8, 1235: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1596: 8, 1597: 8, 1600: 8, 1664: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2016: 8, 2017: 8, 2018: 8, 2019: 8, 2020: 8, 2021: 8, 2022: 8, 2023: 8, 2024: 8
|
||||
}],
|
||||
}
|
||||
|
||||
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'),
|
||||
}
|
||||
|
|
@ -7,7 +7,7 @@ import cereal.messaging as messaging
|
|||
from selfdrive.car.car_helpers import get_car, get_one_can
|
||||
from selfdrive.boardd.boardd import can_list_to_can_capnp
|
||||
|
||||
PandaType = log.HealthData.PandaType
|
||||
PandaType = log.PandaState.PandaType
|
||||
|
||||
|
||||
def steer_thread():
|
||||
|
|
|
|||
Loading…
Reference in New Issue