joystick controls working
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();
|
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) ||
|
has_rtc = (hw_type == cereal::PandaState::PandaType::UNO) ||
|
||||||
(hw_type == cereal::PandaState::PandaType::DOS);
|
(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
|
# 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
|
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
|
succeeded = car_fingerprint is not None
|
||||||
done = failed or succeeded
|
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.car.car_helpers import get_car, get_one_can
|
||||||
from selfdrive.boardd.boardd import can_list_to_can_capnp
|
from selfdrive.boardd.boardd import can_list_to_can_capnp
|
||||||
|
|
||||||
PandaType = log.HealthData.PandaType
|
PandaType = log.PandaState.PandaType
|
||||||
|
|
||||||
|
|
||||||
def steer_thread():
|
def steer_thread():
|
||||||
|
|
Loading…
Reference in New Issue