80 lines
2.6 KiB
Python
80 lines
2.6 KiB
Python
from opendbc.car.toyota.values import ToyotaFlags
|
|
from opendbc.can.parser import CANParser
|
|
|
|
ANGLE_DIFF_THRESHOLD = 4.0
|
|
THRESHOLD_COUNT = 10
|
|
|
|
class ZSS:
|
|
|
|
def __init__(self, flags: int):
|
|
self.enabled = flags & ToyotaFlags.ZSS.value
|
|
self._alka_enabled = flags & ToyotaFlags.ALKA.value
|
|
|
|
self._offset_compute_once = True
|
|
self._alka_active_prev = False
|
|
self._cruise_active_prev = False
|
|
self._offset = 0.
|
|
self._threshold_count = 0
|
|
self._zss_value = 0.
|
|
self._print_allow = True
|
|
|
|
def set_values(self, cp: CANParser):
|
|
self._zss_value = cp.vl["SECONDARY_STEER_ANGLE"]["ZORRO_STEER"]
|
|
|
|
def get_enabled(self):
|
|
return self.enabled
|
|
|
|
def get_steering_angle_deg(self, main_on, cruise_active, stock_steering_angle_deg):
|
|
# off, fall back to stock
|
|
if not self.enabled:
|
|
return stock_steering_angle_deg
|
|
|
|
# when lka control is off, use stock
|
|
alka_active = self._is_alka_active(main_on)
|
|
if not cruise_active and not alka_active:
|
|
return stock_steering_angle_deg
|
|
|
|
# lka just activated
|
|
if not self._offset_compute_once and ((alka_active and not self._alka_active_prev) or (cruise_active and not self._cruise_active_prev)):
|
|
self._threshold_count = 0
|
|
self._offset_compute_once = True
|
|
self._print_allow = True
|
|
|
|
self._alka_active_prev = alka_active
|
|
self._cruise_active_prev = cruise_active
|
|
|
|
# compute offset when required
|
|
if self._offset_compute_once:
|
|
self._offset_compute_once = not self._compute_offset(stock_steering_angle_deg)
|
|
|
|
# error checking
|
|
if self._threshold_count >= THRESHOLD_COUNT:
|
|
if self._print_allow:
|
|
print("ZSS: Too many large diff, fallback to stock.")
|
|
self._print_allow = False
|
|
return stock_steering_angle_deg
|
|
|
|
if self._offset_compute_once:
|
|
print("ZSS: Compute offset required, fallback to stock.")
|
|
return stock_steering_angle_deg
|
|
|
|
zss_steering_angle_deg = self._zss_value - self._offset
|
|
angle_diff = abs(stock_steering_angle_deg - zss_steering_angle_deg)
|
|
if angle_diff > ANGLE_DIFF_THRESHOLD:
|
|
print(f"ZSS: Diff too large ({angle_diff}), fallback to stock. ")
|
|
if self._is_alka_active(main_on) or cruise_active:
|
|
self._threshold_count += 1
|
|
return stock_steering_angle_deg
|
|
|
|
return zss_steering_angle_deg
|
|
|
|
def _is_alka_active(self, main_on):
|
|
return self._alka_enabled and main_on != 0
|
|
|
|
def _compute_offset(self, steering_angle_deg):
|
|
if abs(steering_angle_deg) > 1e-3 and abs(self._zss_value) > 1e-3:
|
|
self._offset = self._zss_value - steering_angle_deg
|
|
print(f"ZSS: offset computed: {self._offset}")
|
|
return True
|
|
return False
|