forked from IQ.Lvbs/IQ.Pilot
63 lines
2.6 KiB
Python
63 lines
2.6 KiB
Python
import numpy as np
|
|
from iqdbc.can import CANPacker
|
|
from iqdbc.car import Bus
|
|
from iqdbc.car.lateral import apply_driver_steer_torque_limits
|
|
from iqdbc.car.interfaces import CarControllerBase
|
|
from iqdbc.car.rivian.riviancan import create_lka_steering, create_longitudinal, create_wheel_touch, create_adas_status
|
|
from iqdbc.car.rivian.values import CarControllerParams
|
|
|
|
from iqdbc.lvbs.car.rivian.aol import AolCarController
|
|
|
|
|
|
class CarController(CarControllerBase, AolCarController):
|
|
def __init__(self, dbc_names, CP, CP_IQ):
|
|
CarControllerBase.__init__(self, dbc_names, CP, CP_IQ)
|
|
AolCarController.__init__(self)
|
|
self.apply_torque_last = 0
|
|
self.packer = CANPacker(dbc_names[Bus.pt])
|
|
|
|
self.cancel_frames = 0
|
|
|
|
def update(self, CC, CC_IQ, CS, now_nanos):
|
|
AolCarController.update(self, CC, CC_IQ, CS)
|
|
actuators = CC.actuators
|
|
can_sends = []
|
|
|
|
apply_torque = 0
|
|
steer_max = round(float(np.interp(CS.out.vEgoRaw, CarControllerParams.STEER_MAX_LOOKUP[0],
|
|
CarControllerParams.STEER_MAX_LOOKUP[1])))
|
|
if self.aol.lat_active:
|
|
new_torque = int(round(CC.actuators.torque * steer_max))
|
|
apply_torque = apply_driver_steer_torque_limits(new_torque, self.apply_torque_last,
|
|
CS.out.steeringTorque, CarControllerParams, steer_max)
|
|
|
|
# send steering command
|
|
self.apply_torque_last = apply_torque
|
|
can_sends.append(create_lka_steering(self.packer, self.frame, CS.acm_lka_hba_cmd, apply_torque, CC.enabled, CC.latActive, self.aol))
|
|
|
|
if self.frame % 5 == 0:
|
|
can_sends.append(create_wheel_touch(self.packer, CS.sccm_wheel_touch, CC.enabled))
|
|
|
|
# Longitudinal control
|
|
if self.CP.openpilotLongitudinalControl:
|
|
accel = float(np.clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX))
|
|
can_sends.append(create_longitudinal(self.packer, self.frame, accel, CC.enabled))
|
|
else:
|
|
interface_status = None
|
|
if CC.cruiseControl.cancel:
|
|
# if there is a noEntry, we need to send a status of "available" before the ACM will accept "unavailable"
|
|
# send "available" right away as the VDM itself takes a few frames to acknowledge
|
|
interface_status = 1 if self.cancel_frames < 5 else 0
|
|
self.cancel_frames += 1
|
|
else:
|
|
self.cancel_frames = 0
|
|
|
|
can_sends.append(create_adas_status(self.packer, CS.vdm_adas_status, interface_status))
|
|
|
|
new_actuators = actuators.as_builder()
|
|
new_actuators.torque = apply_torque / steer_max
|
|
new_actuators.torqueOutputCan = apply_torque
|
|
|
|
self.frame += 1
|
|
return new_actuators, can_sends
|