openpilot is an open source driver assistance system. openpilot performs the functions of Automated Lane Centering and Adaptive Cruise Control for over 200 supported car makes and models.
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

86 lines
3.3 KiB

from opendbc.can.packer import CANPacker
from openpilot.common.realtime import DT_CTRL
from openpilot.selfdrive.car import apply_meas_steer_torque_limits
from openpilot.selfdrive.car.chrysler import chryslercan
from openpilot.selfdrive.car.chrysler.values import RAM_CARS, CarControllerParams, ChryslerFlags
from openpilot.selfdrive.car.interfaces import CarControllerBase
5 years ago
class CarController(CarControllerBase):
def __init__(self, dbc_name, CP, VM):
self.CP = CP
5 years ago
self.apply_steer_last = 0
self.frame = 0
5 years ago
self.hud_count = 0
self.last_lkas_falling_edge = 0
self.lkas_control_bit_prev = False
self.last_button_frame = 0
5 years ago
self.packer = CANPacker(dbc_name)
self.params = CarControllerParams(CP)
5 years ago
def update(self, CC, CS, now_nanos):
5 years ago
can_sends = []
lkas_active = CC.latActive and self.lkas_control_bit_prev
# cruise buttons
if (self.frame - self.last_button_frame)*DT_CTRL > 0.05:
das_bus = 2 if self.CP.carFingerprint in RAM_CARS else 0
# ACC cancellation
if CC.cruiseControl.cancel:
self.last_button_frame = self.frame
can_sends.append(chryslercan.create_cruise_buttons(self.packer, CS.button_counter + 1, das_bus, cancel=True))
# ACC resume from standstill
elif CC.cruiseControl.resume:
self.last_button_frame = self.frame
can_sends.append(chryslercan.create_cruise_buttons(self.packer, CS.button_counter + 1, das_bus, resume=True))
5 years ago
# HUD alerts
if self.frame % 25 == 0:
if CS.lkas_car_model != -1:
can_sends.append(chryslercan.create_lkas_hud(self.packer, self.CP, lkas_active, CC.hudControl.visualAlert,
self.hud_count, CS.lkas_car_model, CS.auto_high_beam))
5 years ago
self.hud_count += 1
# steering
if self.frame % self.params.STEER_STEP == 0:
# TODO: can we make this more sane? why is it different for all the cars?
lkas_control_bit = self.lkas_control_bit_prev
if CS.out.vEgo > self.CP.minSteerSpeed:
lkas_control_bit = True
elif self.CP.flags & ChryslerFlags.HIGHER_MIN_STEERING_SPEED:
if CS.out.vEgo < (self.CP.minSteerSpeed - 3.0):
lkas_control_bit = False
elif self.CP.carFingerprint in RAM_CARS:
if CS.out.vEgo < (self.CP.minSteerSpeed - 0.5):
lkas_control_bit = False
# EPS faults if LKAS re-enables too quickly
lkas_control_bit = lkas_control_bit and (self.frame - self.last_lkas_falling_edge > 200)
if not lkas_control_bit and self.lkas_control_bit_prev:
self.last_lkas_falling_edge = self.frame
self.lkas_control_bit_prev = lkas_control_bit
# steer torque
new_steer = int(round(CC.actuators.steer * self.params.STEER_MAX))
apply_steer = apply_meas_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorqueEps, self.params)
if not lkas_active or not lkas_control_bit:
apply_steer = 0
self.apply_steer_last = apply_steer
can_sends.append(chryslercan.create_lkas_command(self.packer, self.CP, int(apply_steer), lkas_control_bit))
5 years ago
self.frame += 1
5 years ago
card: process that abstracts car interface and CAN (#32380) * format card * standalone process * no class member CS, there's no point also can be confusing; what else could be using this? * rename CoS * Update selfdrive/controls/controlsd.py * never works first time :D * canRcvTimeout is bool * hack * add cpu * see what testing closet comes up with * first * some clean up * support passable CI, fix test models * fix startup alert * process replay changes * test_fuzzy * gate carOutput valid on carControl valid * we should publish after we update carOutput * controlsd was using actuatorsOutput from 2 frames ago for torque, not the most up to date * check all checks for carControl in case controlsd dies * log more timestamps * more generic latency logger; needs some clean up latency_logger.py was difficult to understand and modify * card polls on can and carControl to get latest carControl possible * temp try to send earlier * add log * remove latencylogger * no mpld3! * old loop * detect first event * normal send * revert "card polls on can and carControl to get latest carControl possible" how it was is best * sheesh! update should be first * first timestamp * temp comment ( timestamp is slow :( ) * more final ordering, and make polling on/off test repeatable * Received can * new plot timestamps * clean up * no poll * add controllers (draft) * Revert "add controllers (draft)" This reverts commit e2c3f01b2fadcff74347bac90c8a5cc1ef4e27b3. * fix that * conventions * just use CS * consider controlsd state machine in card: not fully done * hmm it's just becoming controlsd * rm debugging * Revert "hmm it's just becoming controlsd" This reverts commit 534a357ee95bec4ed070667186af55d59421bbc7. * Revert "just use CS" This reverts commit 9fa7406f30c86200f20457f7b9ff95e731201bf9. * add vCruise * migrate car state * Revert "migrate car state" This reverts commit 4ae86ca163c6920070f410f608f7644ab632850b. * Revert "add vCruise" This reverts commit af247a8da41c3626ada4231b98042da1a1ae4633. * simple state machine in card (doesn't work as is) * Revert "simple state machine in card (doesn't work as is)" This reverts commit b4af8a9b0a2e17fdfc89d344c64678ef51305c24. * poll carState without conflate * bump * remove state transition * fix * update refs * ignore cumLagMs and don't ignore valid * fix controls mismatch; controlsd used to set alt exp * controlsd_config_callback not needed for card * revert ref temp * update refs * no poll * not builder! * test fix * need to migrate initialized * CC will be a reader * more as_reader! * fix None * init after publish like before - no real difference * controlsd clean up * remove redundant check and check passive for init * stash * flip * migrate missing carOutput for controlsd * Update ref_commit * bump cereal * comment * no class params * no class * Revert "no class" This reverts commit 5499b83c2dcb5462070626f8523e3aec6f4c209d. * add todo * regen and update refs * fix * update refs * and fix that * should be controlsstate * remove controlsState migration CoS.initialized isn't needed yet * fix * flip! * bump * fix that * update refs * fix * if canValid goes false, controlsd would still send * bump * rm diff * need to be very careful with initializing * update refs
11 months ago
new_actuators = CC.actuators.as_builder()
new_actuators.steer = self.apply_steer_last / self.params.STEER_MAX
new_actuators.steerOutputCan = self.apply_steer_last
return new_actuators, can_sends