3a926ddfbe
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> Co-Authored-By: Jason Wen <haibin.wen3@gmail.com>
118 lines
3.6 KiB
Python
118 lines
3.6 KiB
Python
import json
|
|
|
|
from openpilot.common.conversions import Conversions as CV
|
|
from openpilot.common.params import Params
|
|
|
|
|
|
params = Params()
|
|
params_memory = Params("/dev/shm/params")
|
|
|
|
class SpeedLimitController:
|
|
car_speed_limit: float = 0 # m/s
|
|
map_speed_limit: float = 0 # m/s
|
|
nav_speed_limit: float = 0 # m/s
|
|
prv_speed_limit: float = 0 # m/s
|
|
|
|
def __init__(self) -> None:
|
|
self.update_frogpilot_params()
|
|
|
|
def update_speed_limits(self):
|
|
self.car_speed_limit = json.loads(params_memory.get("CarSpeedLimit") or '0')
|
|
self.map_speed_limit = json.loads(params_memory.get("MapSpeedLimit") or '0')
|
|
self.nav_speed_limit = json.loads(params_memory.get("NavSpeedLimit") or '0')
|
|
|
|
if params_memory.get_bool("FrogPilotTogglesUpdated"):
|
|
self.update_frogpilot_params()
|
|
|
|
@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:
|
|
limits = [self.car_speed_limit, self.map_speed_limit, self.nav_speed_limit]
|
|
filtered_limits = [limit for limit in limits if limit >= 1]
|
|
|
|
if self.highest and filtered_limits:
|
|
return max(filtered_limits)
|
|
elif self.lowest and filtered_limits:
|
|
return min(filtered_limits)
|
|
|
|
speed_limits = {
|
|
"Dashboard": self.car_speed_limit,
|
|
"Navigation": self.nav_speed_limit,
|
|
"Offline Maps": self.map_speed_limit,
|
|
}
|
|
|
|
priorities = [
|
|
self.speed_limit_priority1,
|
|
self.speed_limit_priority2,
|
|
self.speed_limit_priority3,
|
|
]
|
|
|
|
for priority in priorities:
|
|
speed_limit_value = speed_limits.get(priority, 0)
|
|
if speed_limit_value >= 1:
|
|
return speed_limit_value
|
|
|
|
if self.use_previous_limit:
|
|
return self.prv_speed_limit
|
|
|
|
return 0
|
|
|
|
def update_previous_limit(self, speed_limit: float):
|
|
if self.prv_speed_limit != speed_limit:
|
|
params.put_float("PreviousSpeedLimit", speed_limit)
|
|
self.prv_speed_limit = speed_limit
|
|
|
|
@property
|
|
def desired_speed_limit(self) -> float:
|
|
if self.speed_limit != 0:
|
|
self.update_previous_limit(self.speed_limit)
|
|
return self.speed_limit + self.offset
|
|
else:
|
|
return 0
|
|
|
|
@property
|
|
def experimental_mode(self) -> bool:
|
|
return self.speed_limit == 0 and self.use_experimental_mode
|
|
|
|
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_priority1 = params.get("SLCPriority1", encoding='utf-8')
|
|
self.speed_limit_priority2 = params.get("SLCPriority2", encoding='utf-8')
|
|
self.speed_limit_priority3 = params.get("SLCPriority3", encoding='utf-8')
|
|
|
|
self.highest = self.speed_limit_priority1 == "Highest"
|
|
self.lowest = self.speed_limit_priority1 == "Lowest"
|
|
|
|
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_float("PreviousSpeedLimit")
|
|
|
|
SpeedLimitController = SpeedLimitController()
|