From b8340bb259920a9eb6a3dfe031ec5f66370bce0f Mon Sep 17 00:00:00 2001 From: jc01rho Date: Sun, 19 Nov 2023 18:08:48 +0900 Subject: [PATCH] nda speed recovery process --- selfdrive/controls/lib/cruise_helper.py | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/selfdrive/controls/lib/cruise_helper.py b/selfdrive/controls/lib/cruise_helper.py index 4d2f891498329a..8abf8b8abffebc 100644 --- a/selfdrive/controls/lib/cruise_helper.py +++ b/selfdrive/controls/lib/cruise_helper.py @@ -664,19 +664,20 @@ def update_v_cruise_apilot(self, v_cruise_kph, buttonEvents, enabled, metric, co self.frame = controls.sm.frame self.update_params(self.frame) + self.naviSpeed, self.roadSpeed = self.update_speed_nda(CS, controls) if self.naviSpeed > 0 : self.naviSpeedPrev = self.naviSpeed self.naviSpeedRecoveryProcessOngoing = True - if self.naviSpeedRecoveryProcessOngoing and self.naviSpeed == 0: - if self.frame % 50 == 0: + elif self.naviSpeedRecoveryProcessOngoing and self.naviSpeed <= 0: + if self.frame % 40 == 0: self.naviSpeedRecoveryProcessCounter += 1 - speed_diff = self.v_cruise_kph_apply - self.naviSpeedPrev - self.naviSpeed = round(self.naviSpeedPrev + speed_diff * (self.naviSpeedRecoveryProcessCounter/10.0)) + speed_diff = self.v_cruise_kph - self.naviSpeedPrev + self.naviSpeed = ceil(self.naviSpeedPrev + speed_diff * (self.naviSpeedRecoveryProcessCounter/15.0)) - if self.naviSpeedRecoveryProcessCounter > 10 : + if self.naviSpeedRecoveryProcessCounter > 15 : self.naviSpeedRecoveryProcessOngoing = False self.naviSpeedRecoveryProcessCounter = 0 self.naviSpeedPrev = 0