mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-08-23 08:53:44 +08:00
Code Issues Packages Projects Releases Wiki Activity

GAC: Optimizations (#49)

Browse Source 
* lower calls

* reduce blocking!

* do this in another PR
This commit is contained in:
Jason Wen
2023-03-11 03:33:58 -05:00
committed by GitHub
parent a48a9f00bd
commit e2900841da
3 changed files with 21 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/car/interfaces.py
+7 -14
View File
@@ -1,5 +1,4 @@
import yaml
import operator
import os
import time
from abc import abstractmethod, ABC
@@ -10,7 +9,7 @@ from common.basedir import BASEDIR
from common.conversions import Conversions as CV
from common.kalman.simple_kalman import KF1D
from common.numpy_fast import clip, interp
from common.params import Params
from common.params import Params, put_nonblocking
from common.realtime import DT_CTRL
from selfdrive.car import apply_hysteresis, gen_empty_fingerprint, scale_rot_inertia, scale_tire_stiffness
from selfdrive.controls.lib.desire_helper import LANE_CHANGE_SPEED_MIN
@@ -103,7 +102,6 @@ class CarInterfaceBase(ABC):
self.experimental_mode_hold = False
self.experimental_mode = self.param_s.get_bool("ExperimentalMode")
self._frame = 0
self.op_lookup = {"+": operator.add, "-": operator.sub}
self.gac = self.param_s.get_bool("GapAdjustCruise")
self.gac_mode = round(float(self.param_s.get("GapAdjustCruiseMode", encoding="utf8")))
self.prev_gac_button = False
@@ -441,21 +439,16 @@ class CarInterfaceBase(ABC):
self.experimental_mode_hold = False
def get_sp_gac_state(self, gac_tr, gac_min, gac_max, inc_dec):
op = self.op_lookup.get(inc_dec)
gac_tr = op(gac_tr, 1)
if inc_dec == "+":
gac_tr = gac_min if gac_tr > gac_max else gac_tr
gac_tr = min(gac_tr + 1, gac_max)
else:
gac_tr = gac_max if gac_tr < gac_min else gac_tr
gac_tr = max(gac_tr - 1, gac_min)
return int(gac_tr)
def get_sp_distance(self, gac_tr, gac_max, gac_dict=None):
if gac_dict is None:
gac_dict = GAC_DICT
for key, value in gac_dict.items():
if gac_tr == value:
return key
return gac_max
return next((key for key, value in gac_dict.items() if value == gac_tr), gac_max)
def toggle_gac(self, cs_out, CS, gac_button, gac_min, gac_max, gac_default, inc_dec):
if (not (self.CP.openpilotLongitudinalControl or self.gac)) or (self.experimental_mode and self.CP.openpilotLongitudinalControl):
@@ -464,17 +457,17 @@ class CarInterfaceBase(ABC):
return
if self.gac_min != gac_min:
self.gac_min = gac_min
self.param_s.put("GapAdjustCruiseMin", str(self.gac_min))
put_nonblocking("GapAdjustCruiseMin", str(self.gac_min))
if self.gac_max != gac_max:
self.gac_max = gac_max
self.param_s.put("GapAdjustCruiseMax", str(self.gac_max))
put_nonblocking("GapAdjustCruiseMax", str(self.gac_max))
if self.gac_mode in (0, 2):
if gac_button:
self.gac_button_counter += 1
elif self.prev_gac_button and not gac_button and self.gac_button_counter < 50:
self.gac_button_counter = 0
CS.gac_tr = self.get_sp_gac_state(CS.gac_tr, gac_min, gac_max, inc_dec)
self.param_s.put("GapAdjustCruiseTr", str(CS.gac_tr))
put_nonblocking("GapAdjustCruiseTr", str(CS.gac_tr))
else:
self.gac_button_counter = 0
self.prev_gac_button = gac_button
selfdrive/car/toyota/carstate.py
-1
View File
@@ -46,7 +46,6 @@ class CarState(CarStateBase):
self.gac_send = False
self.gac_send_counter = 0
self.follow_distance = 0
self.follow_distance_converted = 0
def update(self, cp, cp_cam):
ret = car.CarState.new_message()
selfdrive/car/toyota/interface.py
+14 -10
View File
@@ -1,7 +1,7 @@
#!/usr/bin/env python3
from cereal import car
from common.conversions import Conversions as CV
from common.params import Params
from common.params import Params, put_nonblocking
from panda import Panda
from selfdrive.car.toyota.values import Ecu, CAR, ToyotaFlags, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, MIN_ACC_SPEED, EPS_SCALE, EV_HYBRID_CAR, UNSUPPORTED_DSU_CAR, CarControllerParams, NO_STOP_TIMER_CAR
from selfdrive.car import STD_CARGO_KG, create_button_event, scale_tire_stiffness, get_safety_config, create_mads_event
@@ -273,23 +273,27 @@ class CarInterface(CarInterfaceBase):
else:
if self.gac_min != 1:
self.gac_min = 1
self.param_s.put("GapAdjustCruiseMin", str(self.gac_min))
put_nonblocking("GapAdjustCruiseMin", str(self.gac_min))
if self.gac_max != 3:
self.gac_max = 3
self.param_s.put("GapAdjustCruiseMax", str(self.gac_max))
put_nonblocking("GapAdjustCruiseMax", str(self.gac_max))
gap_dist_button = bool(self.CS.gap_dist_button)
if self.gac_mode in (0, 2):
if bool(self.CS.gap_dist_button):
if gap_dist_button:
self.gac_button_counter += 1
elif self.prev_gac_button and not bool(self.CS.gap_dist_button) and self.gac_button_counter < 50:
elif self.prev_gac_button and not gap_dist_button and self.gac_button_counter < 50:
self.gac_button_counter = 0
self.CS.follow_distance_converted = self.get_sp_gac_state(self.CS.follow_distance, self.gac_min, self.gac_max, "+")
self.CS.gac_tr = self.get_sp_distance(self.CS.follow_distance_converted, self.gac_max, gac_dict=GAC_DICT)
self.param_s.put("GapAdjustCruiseTr", str(self.CS.gac_tr))
follow_distance_converted = self.get_sp_gac_state(self.CS.follow_distance, self.gac_min, self.gac_max, "+")
gac_tr = self.get_sp_distance(follow_distance_converted, self.gac_max, gac_dict=GAC_DICT)
if gac_tr != self.CS.gac_tr:
put_nonblocking("GapAdjustCruiseTr", str(gac_tr))
self.CS.gac_tr = gac_tr
else:
self.gac_button_counter = 0
self.prev_gac_button = bool(self.CS.gap_dist_button)
self.prev_gac_button = gap_dist_button
ret.gapAdjustCruiseTr = self.CS.gac_tr
if self.CS.gac_send_counter < 10 and (self.get_sp_distance(ret.gapAdjustCruiseTr, self.gac_max, gac_dict=GAC_DICT) != self.CS.follow_distance):
gap_distance = self.get_sp_distance(ret.gapAdjustCruiseTr, self.gac_max, gac_dict=GAC_DICT)
if self.CS.gac_send_counter < 10 and gap_distance != self.CS.follow_distance:
self.CS.gac_send_counter += 1
self.CS.gac_send = 1
else: