From 74d47446a9b80b1657ff6600b68224daf57ee38c Mon Sep 17 00:00:00 2001 From: Kevin Roscom Date: Mon, 21 Feb 2022 14:05:24 -0700 Subject: [PATCH] joystick controls working --- opendbc | 2 +- selfdrive/boardd/panda.cc | 3 - selfdrive/car/car_helpers.py | 1 + selfdrive/car/ocelot/carcontroller.py | 129 ++++++++++++++++++++++++ selfdrive/car/ocelot/carstate.py | 128 +++++++++++++++++++++++ selfdrive/car/ocelot/interface.py | 121 ++++++++++++++++++++++ selfdrive/car/ocelot/ocelotcan.py | 33 ++++++ selfdrive/car/ocelot/radar_interface.py | 62 ++++++++++++ selfdrive/car/ocelot/values.py | 36 +++++++ tools/carcontrols/debug_controls.py | 2 +- 10 files changed, 512 insertions(+), 5 deletions(-) create mode 100644 selfdrive/car/ocelot/carcontroller.py create mode 100644 selfdrive/car/ocelot/carstate.py create mode 100644 selfdrive/car/ocelot/interface.py create mode 100644 selfdrive/car/ocelot/ocelotcan.py create mode 100644 selfdrive/car/ocelot/radar_interface.py create mode 100644 selfdrive/car/ocelot/values.py diff --git a/opendbc b/opendbc index 12a00517..525ba532 160000 --- a/opendbc +++ b/opendbc @@ -1 +1 @@ -Subproject commit 12a00517b86d97f5dd3d6b4b90c64038f1f567fe +Subproject commit 525ba532df7357066fd73c139dc4e2f297877322 diff --git a/selfdrive/boardd/panda.cc b/selfdrive/boardd/panda.cc index 1b563b53..9e35513e 100644 --- a/selfdrive/boardd/panda.cc +++ b/selfdrive/boardd/panda.cc @@ -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); diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index 9f3ebfa0..991595ef 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -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 diff --git a/selfdrive/car/ocelot/carcontroller.py b/selfdrive/car/ocelot/carcontroller.py new file mode 100644 index 00000000..e297bc19 --- /dev/null +++ b/selfdrive/car/ocelot/carcontroller.py @@ -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 diff --git a/selfdrive/car/ocelot/carstate.py b/selfdrive/car/ocelot/carstate.py new file mode 100644 index 00000000..4bed0e74 --- /dev/null +++ b/selfdrive/car/ocelot/carstate.py @@ -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) diff --git a/selfdrive/car/ocelot/interface.py b/selfdrive/car/ocelot/interface.py new file mode 100644 index 00000000..69395dcf --- /dev/null +++ b/selfdrive/car/ocelot/interface.py @@ -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 diff --git a/selfdrive/car/ocelot/ocelotcan.py b/selfdrive/car/ocelot/ocelotcan.py new file mode 100644 index 00000000..4f4946a3 --- /dev/null +++ b/selfdrive/car/ocelot/ocelotcan.py @@ -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) \ No newline at end of file diff --git a/selfdrive/car/ocelot/radar_interface.py b/selfdrive/car/ocelot/radar_interface.py new file mode 100644 index 00000000..3130e418 --- /dev/null +++ b/selfdrive/car/ocelot/radar_interface.py @@ -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 diff --git a/selfdrive/car/ocelot/values.py b/selfdrive/car/ocelot/values.py new file mode 100644 index 00000000..e197fab0 --- /dev/null +++ b/selfdrive/car/ocelot/values.py @@ -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'), +} diff --git a/tools/carcontrols/debug_controls.py b/tools/carcontrols/debug_controls.py index 9264b2d7..0c093372 100755 --- a/tools/carcontrols/debug_controls.py +++ b/tools/carcontrols/debug_controls.py @@ -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():