22bfc8d9b7
Added toggle to control the cruise set speed according to speed limit supplied by OSM, NOO, or the vehicle itself. Co-Authored-By: Jacob Pfeifer <jacob@pfeifer.dev> Co-Authored-By: Efini <19368997+efini@users.noreply.github.com> Co-Authored-By: Kumar <36933347+rav4kumar@users.noreply.github.com>
120 lines
3.7 KiB
Python
120 lines
3.7 KiB
Python
from openpilot.common.conversions import Conversions as CV
|
|
from openpilot.common.params import Params
|
|
import json
|
|
|
|
params = Params()
|
|
params_memory = Params("/dev/shm/params")
|
|
|
|
class SpeedLimitController:
|
|
car_speed_limit: float = 0 # m/s
|
|
lst_speed_limit: float = 0 # m/s
|
|
map_speed_limit: float = 0 # m/s
|
|
nav_speed_limit: float = 0 # m/s
|
|
|
|
def __init__(self) -> None:
|
|
self.update_frogpilot_params()
|
|
self.write_car_state()
|
|
self.write_map_state()
|
|
self.write_nav_state()
|
|
|
|
def update_current_max_velocity(self, max_v: float, load_state: bool = True, write_state: bool = True) -> None:
|
|
if load_state:
|
|
self.load_state()
|
|
|
|
@property
|
|
def offset(self) -> float:
|
|
if self.speed_limit < 14:
|
|
return self.offset1
|
|
elif self.speed_limit < 24:
|
|
return self.offset2
|
|
elif self.speed_limit < 29:
|
|
return self.offset3
|
|
else:
|
|
return self.offset4
|
|
|
|
@property
|
|
def speed_limit(self) -> float:
|
|
params_memory.put_bool("SLCExperimentalMode", False)
|
|
|
|
limits = [self.car_speed_limit, self.map_speed_limit, self.nav_speed_limit]
|
|
filtered_limits = [limit for limit in limits if limit > 0]
|
|
|
|
if self.highest and filtered_limits:
|
|
return max(filtered_limits)
|
|
if self.lowest and filtered_limits:
|
|
return min(filtered_limits)
|
|
|
|
priority_orders = [
|
|
["nav", "car", "map"],
|
|
["nav", "map", "car"],
|
|
["nav", "map"],
|
|
["nav", "car"],
|
|
["nav"],
|
|
["map", "car", "nav"],
|
|
["map", "nav", "car"],
|
|
["map", "nav"],
|
|
["map", "car"],
|
|
["map"],
|
|
["car", "nav", "map"],
|
|
["car", "map", "nav"],
|
|
["car", "map"],
|
|
["car", "nav"],
|
|
["car"]
|
|
]
|
|
|
|
if self.speed_limit_priority < len(priority_orders):
|
|
for source in priority_orders[self.speed_limit_priority]:
|
|
limit = getattr(self, f"{source}_speed_limit", 0)
|
|
if limit > 0:
|
|
self.prv_speed_limit = limit
|
|
return limit
|
|
|
|
if self.use_experimental_mode:
|
|
params_memory.put_bool("SLCExperimentalMode", True)
|
|
return 0
|
|
elif self.use_previous_limit:
|
|
if self.lst_speed_limit != self.prv_speed_limit:
|
|
params.put_int("PreviousSpeedLimit", self.prv_speed_limit * 100)
|
|
self.lst_speed_limit = self.prv_speed_limit
|
|
return self.prv_speed_limit
|
|
|
|
return 0
|
|
|
|
@property
|
|
def desired_speed_limit(self):
|
|
return self.speed_limit + self.offset if self.speed_limit else 0
|
|
|
|
def load_state(self):
|
|
self.car_speed_limit = json.loads(params_memory.get("CarSpeedLimit"))
|
|
self.map_speed_limit = json.loads(params_memory.get("MapSpeedLimit"))
|
|
self.nav_speed_limit = json.loads(params_memory.get("NavSpeedLimit"))
|
|
|
|
def write_car_state(self):
|
|
params_memory.put("CarSpeedLimit", json.dumps(self.car_speed_limit))
|
|
|
|
def write_map_state(self):
|
|
params_memory.put("MapSpeedLimit", json.dumps(self.map_speed_limit))
|
|
|
|
def write_nav_state(self):
|
|
params_memory.put("NavSpeedLimit", json.dumps(self.nav_speed_limit))
|
|
|
|
def update_frogpilot_params(self):
|
|
conversion = CV.KPH_TO_MS if params.get_bool("IsMetric") else CV.MPH_TO_MS
|
|
|
|
self.offset1 = params.get_int("Offset1") * conversion
|
|
self.offset2 = params.get_int("Offset2") * conversion
|
|
self.offset3 = params.get_int("Offset3") * conversion
|
|
self.offset4 = params.get_int("Offset4") * conversion
|
|
|
|
self.speed_limit_priority = params.get_int("SLCPriority")
|
|
self.highest = self.speed_limit_priority == 15
|
|
self.lowest = self.speed_limit_priority == 16
|
|
|
|
slc_fallback = params.get_int("SLCFallback")
|
|
self.use_experimental_mode = slc_fallback == 1
|
|
self.use_previous_limit = slc_fallback == 2
|
|
|
|
self.prv_speed_limit = params.get_int("PreviousSpeedLimit") / 100
|
|
|
|
SpeedLimitController = SpeedLimitController()
|