brianhansonxyz/clearpilot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

wip

Browse Source
This commit is contained in:
Your Name
2024-04-27 13:44:34 -05:00
parent ea1aad5ed1
commit 2fbe9dbea1
25 changed files with 1501 additions and 532 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
selfdrive/controls/lib/alertmanager.py
View File

@@ -3,7 +3,6 @@ import os
import json
from collections import defaultdict
from dataclasses import dataclass
from typing import List, Dict, Optional
from openpilot.common.basedir import BASEDIR
from openpilot.common.params import Params
@@ -14,7 +13,7 @@ with open(os.path.join(BASEDIR, "selfdrive/controls/lib/alerts_offroad.json")) a
OFFROAD_ALERTS = json.load(f)
def set_offroad_alert(alert: str, show_alert: bool, extra_text: Optional[str] = None) -> None:
def set_offroad_alert(alert: str, show_alert: bool, extra_text: str = None) -> None:
if show_alert:
a = copy.copy(OFFROAD_ALERTS[alert])
a['extra'] = extra_text or ''
@@ -25,7 +24,7 @@ def set_offroad_alert(alert: str, show_alert: bool, extra_text: Optional[str] =
@dataclass
class AlertEntry:
alert: Optional[Alert] = None
alert: Alert | None = None
start_frame: int = -1
end_frame: int = -1
@@ -34,9 +33,9 @@ class AlertEntry:
class AlertManager:
def __init__(self):
self.alerts: Dict[str, AlertEntry] = defaultdict(AlertEntry)
self.alerts: dict[str, AlertEntry] = defaultdict(AlertEntry)
def add_many(self, frame: int, alerts: List[Alert]) -> None:
def add_many(self, frame: int, alerts: list[Alert]) -> None:
for alert in alerts:
entry = self.alerts[alert.alert_type]
entry.alert = alert
@@ -45,7 +44,7 @@ class AlertManager:
min_end_frame = entry.start_frame + alert.duration
entry.end_frame = max(frame + 1, min_end_frame)
def process_alerts(self, frame: int, clear_event_types: set) -> Optional[Alert]:
def process_alerts(self, frame: int, clear_event_types: set) -> Alert | None:
current_alert = AlertEntry()
for v in self.alerts.values():
if not v.alert:

20
selfdrive/controls/lib/desire_helper.py
View File

@@ -37,7 +37,6 @@ TURN_DESIRES = {
TurnDirection.turnRight: log.Desire.turnRight,
}
class DesireHelper:
def __init__(self):
self.lane_change_state = LaneChangeState.off
@@ -70,7 +69,6 @@ class DesireHelper:
lane_available = True
else:
desired_lane = frogpilotPlan.laneWidthLeft if carstate.leftBlinker else frogpilotPlan.laneWidthRight
# Check if the lane width exceeds the threshold
lane_available = desired_lane >= self.lane_detection_width
if not lateral_active or self.lane_change_timer > LANE_CHANGE_TIME_MAX:
@@ -82,7 +80,6 @@ class DesireHelper:
# Set the "turn_completed" flag to prevent lane changes after completing a turn
self.turn_completed = True
else:
# TurnDirection.turnLeft / turnRight
self.turn_direction = TurnDirection.none
# LaneChangeState.off
@@ -104,19 +101,17 @@ class DesireHelper:
blindspot_detected = ((carstate.leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or
(carstate.rightBlindspot and self.lane_change_direction == LaneChangeDirection.right))
# Conduct a nudgeless lane change if all the conditions are met
self.lane_change_wait_timer += DT_MDL
if self.nudgeless and lane_available and not self.lane_change_completed and self.lane_change_wait_timer >= self.lane_change_delay:
self.lane_change_wait_timer = 0
torque_applied = True
self.lane_change_wait_timer = 0
if not one_blinker or below_lane_change_speed:
self.lane_change_state = LaneChangeState.off
self.lane_change_direction = LaneChangeDirection.none
elif torque_applied and not blindspot_detected:
# Set the "lane_change_completed" flag to prevent any more lane changes if the toggle is on
self.lane_change_completed = self.one_lane_change
self.lane_change_state = LaneChangeState.laneChangeStarting
self.lane_change_completed = True if self.one_lane_change else False
# LaneChangeState.laneChangeStarting
elif self.lane_change_state == LaneChangeState.laneChangeStarting:
@@ -144,10 +139,8 @@ class DesireHelper:
else:
self.lane_change_timer += DT_MDL
self.prev_one_blinker = one_blinker
# Reset the flags
self.lane_change_completed &= one_blinker
self.prev_one_blinker = one_blinker
self.turn_completed &= one_blinker
if self.turn_direction != TurnDirection.none:
@@ -173,10 +166,11 @@ class DesireHelper:
def update_frogpilot_params(self):
is_metric = self.params.get_bool("IsMetric")
lateral_tune = self.params.get_bool("LateralTune")
self.turn_desires = lateral_tune and self.params.get_bool("TurnDesires")
self.nudgeless = self.params.get_bool("NudgelessLaneChange")
self.lane_change_delay = self.params.get_int("LaneChangeTime") if self.nudgeless else 0
self.lane_detection = self.nudgeless and self.params.get_bool("LaneDetection")
self.lane_detection = self.nudgeless and self.params.get_int("LaneDetectionWidth") != 0
self.lane_detection_width = self.params.get_int("LaneDetectionWidth") * (1 if is_metric else CV.FOOT_TO_METER) / 10 if self.lane_detection else 0
self.one_lane_change = self.nudgeless and self.params.get_bool("OneLaneChange")
self.turn_desires = self.params.get_bool("TurnDesires")

29
selfdrive/controls/lib/drive_helpers.py
View File

@@ -93,22 +93,18 @@ class VCruiseHelper:
long_press = True
break
# Reverse the long press value for reverse cruise increase
if frogpilot_variables.reverse_cruise_increase:
long_press = not long_press
if button_type is None:
return
# Confirm or deny the new speed limit value
if speed_limit_changed:
if button_type == ButtonType.accelCruise:
self.params_memory.put_bool("SLCConfirmed", True);
self.params_memory.put_bool("SLCConfirmedPressed", True);
self.params_memory.put_bool("SLCConfirmed", True)
self.params_memory.put_bool("SLCConfirmedPressed", True)
return
elif button_type == ButtonType.decelCruise:
self.params_memory.put_bool("SLCConfirmed", False);
self.params_memory.put_bool("SLCConfirmedPressed", True);
self.params_memory.put_bool("SLCConfirmed", False)
self.params_memory.put_bool("SLCConfirmedPressed", True)
return
# Don't adjust speed when pressing resume to exit standstill
@@ -120,13 +116,13 @@ class VCruiseHelper:
if not self.button_change_states[button_type]["enabled"]:
return
v_cruise_delta = v_cruise_delta * (5 if long_press else 1)
if long_press and self.v_cruise_kph % v_cruise_delta != 0: # partial interval
v_cruise_delta_interval = frogpilot_variables.custom_cruise_increase_long if long_press else frogpilot_variables.custom_cruise_increase
v_cruise_delta = v_cruise_delta * v_cruise_delta_interval
if v_cruise_delta_interval % 5 == 0 and self.v_cruise_kph % v_cruise_delta != 0: # partial interval
self.v_cruise_kph = CRUISE_NEAREST_FUNC[button_type](self.v_cruise_kph / v_cruise_delta) * v_cruise_delta
else:
self.v_cruise_kph += v_cruise_delta * CRUISE_INTERVAL_SIGN[button_type]
# Apply offset
v_cruise_offset = (frogpilot_variables.set_speed_offset * CRUISE_INTERVAL_SIGN[button_type]) if long_press else 0
if v_cruise_offset < 0:
v_cruise_offset = frogpilot_variables.set_speed_offset - v_cruise_delta
@@ -155,25 +151,20 @@ class VCruiseHelper:
if self.CP.pcmCruise:
return
if frogpilot_variables.conditional_experimental_mode:
initial = V_CRUISE_INITIAL
else:
initial = V_CRUISE_INITIAL_EXPERIMENTAL_MODE if experimental_mode else V_CRUISE_INITIAL
initial = V_CRUISE_INITIAL_EXPERIMENTAL_MODE if experimental_mode else V_CRUISE_INITIAL
# 250kph or above probably means we never had a set speed
if any(b.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for b in CS.buttonEvents) and self.v_cruise_kph_last < 250:
self.v_cruise_kph = self.v_cruise_kph_last
else:
# Initial set speed
if desired_speed_limit != 0 and frogpilot_variables.set_speed_limit:
# If there's a known speed limit and the corresponding FP toggle is set, push it to the car
self.v_cruise_kph = desired_speed_limit * CV.MS_TO_KPH
self.v_cruise_kph = int(round(desired_speed_limit * CV.MS_TO_KPH))
else:
# Use fixed initial set speed from mode etc.
self.v_cruise_kph = int(round(clip(CS.vEgo * CV.MS_TO_KPH, initial, V_CRUISE_MAX)))
self.v_cruise_cluster_kph = self.v_cruise_kph
def apply_deadzone(error, deadzone):
if error > deadzone:
error -= deadzone

45
selfdrive/controls/lib/events.py
View File

@@ -2,7 +2,7 @@
import math
import os
from enum import IntEnum
from typing import Dict, Union, Callable, List, Optional
from collections.abc import Callable
from cereal import log, car
import cereal.messaging as messaging
@@ -12,6 +12,7 @@ from openpilot.common.realtime import DT_CTRL
from openpilot.selfdrive.locationd.calibrationd import MIN_SPEED_FILTER
from openpilot.system.version import get_short_branch
params = Params()
params_memory = Params("/dev/shm/params")
AlertSize = log.ControlsState.AlertSize
@@ -51,12 +52,12 @@ EVENT_NAME = {v: k for k, v in EventName.schema.enumerants.items()}
class Events:
def __init__(self):
self.events: List[int] = []
self.static_events: List[int] = []
self.events: list[int] = []
self.static_events: list[int] = []
self.events_prev = dict.fromkeys(EVENTS.keys(), 0)
@property
def names(self) -> List[int]:
def names(self) -> list[int]:
return self.events
def __len__(self) -> int:
@@ -74,7 +75,7 @@ class Events:
def contains(self, event_type: str) -> bool:
return any(event_type in EVENTS.get(e, {}) for e in self.events)
def create_alerts(self, event_types: List[str], callback_args=None):
def create_alerts(self, event_types: list[str], callback_args=None):
if callback_args is None:
callback_args = []
@@ -135,7 +136,7 @@ class Alert:
self.creation_delay = creation_delay
self.alert_type = ""
self.event_type: Optional[str] = None
self.event_type: str | None = None
def __str__(self) -> str:
return f"{self.alert_text_1}/{self.alert_text_2} {self.priority} {self.visual_alert} {self.audible_alert}"
@@ -261,15 +262,14 @@ def no_gps_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, m
AlertStatus.normal, AlertSize.mid,
Priority.LOWER, VisualAlert.none, AudibleAlert.none, .2, creation_delay=300.)
def torque_nn_load_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
model_name = params_memory.get("NNFFModelName")
model_name = params.get("NNFFModelName", encoding='utf-8')
if model_name == "":
return Alert(
"NNFF Torque Controller not available",
"Donate logs to Twilsonco to get it added!",
"Donate logs to Twilsonco to get your car supported!",
AlertStatus.userPrompt, AlertSize.mid,
Priority.LOW, VisualAlert.none, AudibleAlert.prompt, 6.0)
Priority.LOW, VisualAlert.none, AudibleAlert.prompt, 5.0)
else:
return Alert(
"NNFF Torque Controller loaded",
@@ -379,13 +379,14 @@ def holiday_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster,
def no_lane_available_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
lane_width = sm['frogpilotPlan'].laneWidthLeft if CS.leftBlinker else sm['frogpilotPlan'].laneWidthRight
lane_width_msg = f"{lane_width:.1f} meters" if metric else f"{lane_width * CV.METER_TO_FOOT:.1f} feet"
return Alert(
"No lane available",
f"Detected lane width is only {lane_width_msg}",
AlertStatus.normal, AlertSize.mid,
Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2)
EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
# ********** events with no alerts **********
EventName.stockFcw: {},
@@ -1009,7 +1010,13 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
},
# FrogPilot Events
EventName.frogSteerSaturated: {
EventName.blockUser: {
ET.PERMANENT: NormalPermanentAlert("Dashcam mode",
"Please don't use the 'Development' branch!",
priority=Priority.HIGHEST),
},
EventName.goatSteerSaturated: {
ET.WARNING: Alert(
"Turn Exceeds Steering Limit",
"JESUS TAKE THE WHEEL!!",
@@ -1110,6 +1117,14 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
Priority.LOW, VisualAlert.none, AudibleAlert.nessie, 4.),
},
EventName.accel40: {
ET.WARNING: Alert(
"Great Scott!",
"🚗💨",
AlertStatus.frogpilot, AlertSize.mid,
Priority.LOW, VisualAlert.none, AudibleAlert.doc, 4.),
},
EventName.firefoxSteerSaturated: {
ET.WARNING: Alert(
"Turn Exceeds Steering Limit",
@@ -1137,7 +1152,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
EventName.yourFrogTriedToKillMe: {
ET.PERMANENT: Alert(
"Your frog tried to kill me...",
"😡",
"👺",
AlertStatus.frogpilot, AlertSize.mid,
Priority.MID, VisualAlert.none, AudibleAlert.angry, 5.),
},
@@ -1150,7 +1165,7 @@ if __name__ == '__main__':
from collections import defaultdict
event_names = {v: k for k, v in EventName.schema.enumerants.items()}
alerts_by_type: Dict[str, Dict[Priority, List[str]]] = defaultdict(lambda: defaultdict(list))
alerts_by_type: dict[str, dict[Priority, list[str]]] = defaultdict(lambda: defaultdict(list))
CP = car.CarParams.new_message()
CS = car.CarState.new_message()
@@ -1162,7 +1177,7 @@ if __name__ == '__main__':
alert = alert(CP, CS, sm, False, 1)
alerts_by_type[et][alert.priority].append(event_names[i])
all_alerts: Dict[str, List[tuple[Priority, List[str]]]] = {}
all_alerts: dict[str, list[tuple[Priority, list[str]]]] = {}
for et, priority_alerts in alerts_by_type.items():
all_alerts[et] = sorted(priority_alerts.items(), key=lambda x: x[0], reverse=True)

26
selfdrive/controls/lib/latcontrol_torque.py
View File

@@ -73,10 +73,10 @@ class LatControlTorque(LatControl):
self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg
# Twilsonco's Lateral Neural Network Feedforward
self.use_nn = CI.has_lateral_torque_nn
self.use_lateral_jerk = CI.use_lateral_jerk
self.use_nnff = CI.use_nnff
self.use_nnff_lite = CI.use_nnff_lite
if self.use_nn or self.use_lateral_jerk:
if self.use_nnff or self.use_nnff_lite:
# Instantaneous lateral jerk changes very rapidly, making it not useful on its own,
# however, we can "look ahead" to the future planned lateral jerk in order to guage
# whether the current desired lateral jerk will persist into the future, i.e.
@@ -96,7 +96,7 @@ class LatControlTorque(LatControl):
self.t_diffs = np.diff(ModelConstants.T_IDXS)
self.desired_lat_jerk_time = CP.steerActuatorDelay + 0.3
if self.use_nn:
if self.use_nnff:
self.pitch = FirstOrderFilter(0.0, 0.5, 0.01)
# NN model takes current v_ego, lateral_accel, lat accel/jerk error, roll, and past/future/planned data
# of lat accel and roll
@@ -137,7 +137,7 @@ class LatControlTorque(LatControl):
if self.use_steering_angle:
actual_curvature = actual_curvature_vm
curvature_deadzone = abs(VM.calc_curvature(math.radians(self.steering_angle_deadzone_deg), CS.vEgo, 0.0))
if self.use_nn or self.use_lateral_jerk:
if self.use_nnff or self.use_nnff_lite:
actual_curvature_rate = -VM.calc_curvature(math.radians(CS.steeringRateDeg), CS.vEgo, 0.0)
actual_lateral_jerk = actual_curvature_rate * CS.vEgo ** 2
else:
@@ -152,14 +152,14 @@ class LatControlTorque(LatControl):
actual_lateral_accel = actual_curvature * CS.vEgo ** 2
lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2
low_speed_factor = interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y if not self.use_nn else LOW_SPEED_Y_NN)**2
low_speed_factor = interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y if not self.use_nnff else LOW_SPEED_Y_NN)**2
setpoint = desired_lateral_accel + low_speed_factor * desired_curvature
measurement = actual_lateral_accel + low_speed_factor * actual_curvature
lateral_jerk_setpoint = 0
lateral_jerk_measurement = 0
if self.use_nn or self.use_lateral_jerk:
if self.use_nnff or self.use_nnff_lite:
# prepare "look-ahead" desired lateral jerk
lat_accel_friction_factor = self.lat_accel_friction_factor
if len(model_data.acceleration.y) == ModelConstants.IDX_N:
@@ -180,7 +180,7 @@ class LatControlTorque(LatControl):
lookahead_lateral_jerk = 0.0
model_good = model_data is not None and len(model_data.orientation.x) >= CONTROL_N
if self.use_nn and model_good:
if self.use_nnff and model_good:
# update past data
roll = params.roll
pitch = self.pitch.update(llk.calibratedOrientationNED.value[1]) if len(llk.calibratedOrientationNED.value) > 1 else 0.0
@@ -219,17 +219,17 @@ class LatControlTorque(LatControl):
# apply friction override for cars with low NN friction response
if self.nn_friction_override:
pid_log.error += self.torque_from_lateral_accel(LatControlInputs(0.0, 0.0, CS.vEgo, CS.aEgo), self.torque_params,
friction_input, lateral_accel_deadzone, friction_compensation=True, gravity_adjusted=False)
friction_input, lateral_accel_deadzone, friction_compensation=True, gravity_adjusted=False)
nn_log = nn_input + nnff_setpoint_input + nnff_measurement_input
else:
gravity_adjusted_lateral_accel = desired_lateral_accel - roll_compensation
torque_from_setpoint = self.torque_from_lateral_accel(LatControlInputs(setpoint, roll_compensation, CS.vEgo, CS.aEgo), self.torque_params,
lateral_jerk_setpoint, lateral_accel_deadzone, friction_compensation=self.use_lateral_jerk, gravity_adjusted=False)
lateral_jerk_setpoint, lateral_accel_deadzone, friction_compensation=self.use_nnff_lite, gravity_adjusted=False)
torque_from_measurement = self.torque_from_lateral_accel(LatControlInputs(measurement, roll_compensation, CS.vEgo, CS.aEgo), self.torque_params,
lateral_jerk_measurement, lateral_accel_deadzone, friction_compensation=self.use_lateral_jerk, gravity_adjusted=False)
lateral_jerk_measurement, lateral_accel_deadzone, friction_compensation=self.use_nnff_lite, gravity_adjusted=False)
pid_log.error = torque_from_setpoint - torque_from_measurement
error = desired_lateral_accel - actual_lateral_accel
if self.use_lateral_jerk:
if self.use_nnff_lite:
friction_input = lat_accel_friction_factor * error + self.lat_jerk_friction_factor * lookahead_lateral_jerk
else:
friction_input = error

23
selfdrive/controls/lib/longcontrol.py
View File

@@ -1,5 +1,6 @@
from cereal import car
from openpilot.common.numpy_fast import clip, interp
from openpilot.common.params import Params
from openpilot.common.realtime import DT_CTRL
from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, apply_deadzone
from openpilot.selfdrive.controls.lib.pid import PIDController
@@ -60,6 +61,12 @@ class LongControl:
self.v_pid = 0.0
self.last_output_accel = 0.0
# FrogPilot variables
self.params = Params()
self.params_memory = Params("/dev/shm/params")
self.update_frogpilot_params()
def reset(self, v_pid):
"""Reset PID controller and change setpoint"""
self.pid.reset()
@@ -129,4 +136,20 @@ class LongControl:
self.last_output_accel = clip(output_accel, accel_limits[0], accel_limits[1])
if self.params_memory.get_bool("FrogPilotTogglesUpdated"):
self.update_frogpilot_params()
return self.last_output_accel
def update_frogpilot_params(self):
kiV = [self.params.get_float("kiV1"), self.params.get_float("kiV2"), self.params.get_float("kiV3"), self.params.get_float("kiV4")]
kpV = [self.params.get_float("kpV1"), self.params.get_float("kpV2"), self.params.get_float("kpV3"), self.params.get_float("kpV4")]
if self.params.get_bool("Tuning"):
self.pid = PIDController((self.CP.longitudinalTuning.kpBP, kpV),
(self.CP.longitudinalTuning.kiBP, kiV),
k_f=self.CP.longitudinalTuning.kf, rate=1 / DT_CTRL)
else:
self.pid = PIDController((self.CP.longitudinalTuning.kpBP, self.CP.longitudinalTuning.kpV),
(self.CP.longitudinalTuning.kiBP, self.CP.longitudinalTuning.kiV),
k_f=self.CP.longitudinalTuning.kf, rate=1 / DT_CTRL)

161
selfdrive/controls/lib/longitudinal_planner.py Executable file → Normal file
View File

@@ -2,35 +2,36 @@
import math
import numpy as np
from openpilot.common.numpy_fast import clip, interp
from openpilot.common.params import Params
from cereal import log
import cereal.messaging as messaging
from openpilot.common.conversions import Conversions as CV
from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.common.params import Params
from openpilot.common.simple_kalman import KF1D
from openpilot.common.realtime import DT_MDL
from openpilot.common.swaglog import cloudlog
from openpilot.selfdrive.modeld.constants import ModelConstants
from openpilot.selfdrive.car.interfaces import ACCEL_MIN, ACCEL_MAX
from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC, LEAD_ACCEL_TAU
from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, CONTROL_N, get_speed_error
from openpilot.common.swaglog import cloudlog
from openpilot.system.version import get_short_branch
from openpilot.selfdrive.frogpilot.functions.frogpilot_functions import CRUISING_SPEED
from openpilot.selfdrive.frogpilot.controls.lib.model_manager import RADARLESS_MODELS
LON_MPC_STEP = 0.2 # first step is 0.2s
A_CRUISE_MIN = -1.2
A_CRUISE_MAX_VALS = [1.6, 1.2, 0.8, 0.6]
A_CRUISE_MAX_BP = [0., 10.0, 25., 40.]
ACCEL_MAX_PLUS = [ACCEL_MAX, 4.0]
ACCEL_MAX_PLUS_BP = [0., CRUISING_SPEED]
# Lookup table for turns
_A_TOTAL_MAX_V = [1.7, 3.2]
_A_TOTAL_MAX_BP = [20., 40.]
# Kalman filter states enum
LEAD_KALMAN_SPEED, LEAD_KALMAN_ACCEL = 0, 1
def get_max_accel(v_ego):
return interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS)
@@ -51,6 +52,72 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP):
return [a_target[0], min(a_target[1], a_x_allowed)]
def lead_kf(v_lead: float, dt: float = 0.05):
# Lead Kalman Filter params, calculating K from A, C, Q, R requires the control library.
# hardcoding a lookup table to compute K for values of radar_ts between 0.01s and 0.2s
assert dt > .01 and dt < .2, "Radar time step must be between .01s and 0.2s"
A = [[1.0, dt], [0.0, 1.0]]
C = [1.0, 0.0]
#Q = np.matrix([[10., 0.0], [0.0, 100.]])
#R = 1e3
#K = np.matrix([[ 0.05705578], [ 0.03073241]])
dts = [dt * 0.01 for dt in range(1, 21)]
K0 = [0.12287673, 0.14556536, 0.16522756, 0.18281627, 0.1988689, 0.21372394,
0.22761098, 0.24069424, 0.253096, 0.26491023, 0.27621103, 0.28705801,
0.29750003, 0.30757767, 0.31732515, 0.32677158, 0.33594201, 0.34485814,
0.35353899, 0.36200124]
K1 = [0.29666309, 0.29330885, 0.29042818, 0.28787125, 0.28555364, 0.28342219,
0.28144091, 0.27958406, 0.27783249, 0.27617149, 0.27458948, 0.27307714,
0.27162685, 0.27023228, 0.26888809, 0.26758976, 0.26633338, 0.26511557,
0.26393339, 0.26278425]
K = [[interp(dt, dts, K0)], [interp(dt, dts, K1)]]
kf = KF1D([[v_lead], [0.0]], A, C, K)
return kf
class Lead:
def __init__(self):
self.dRel = 0.0
self.yRel = 0.0
self.vLead = 0.0
self.aLead = 0.0
self.vLeadK = 0.0
self.aLeadK = 0.0
self.aLeadTau = LEAD_ACCEL_TAU
self.prob = 0.0
self.status = False
self.kf: KF1D | None = None
def reset(self):
self.status = False
self.kf = None
self.aLeadTau = LEAD_ACCEL_TAU
def update(self, dRel: float, yRel: float, vLead: float, aLead: float, prob: float):
self.dRel = dRel
self.yRel = yRel
self.vLead = vLead
self.aLead = aLead
self.prob = prob
self.status = True
if self.kf is None:
self.kf = lead_kf(self.vLead)
else:
self.kf.update(self.vLead)
self.vLeadK = float(self.kf.x[LEAD_KALMAN_SPEED][0])
self.aLeadK = float(self.kf.x[LEAD_KALMAN_ACCEL][0])
# Learn if constant acceleration
if abs(self.aLeadK) < 0.5:
self.aLeadTau = LEAD_ACCEL_TAU
else:
self.aLeadTau *= 0.9
class LongitudinalPlanner:
def __init__(self, CP, init_v=0.0, init_a=0.0, dt=DT_MDL):
self.CP = CP
@@ -62,23 +129,26 @@ class LongitudinalPlanner:
self.v_desired_filter = FirstOrderFilter(init_v, 2.0, self.dt)
self.v_model_error = 0.0
self.lead_one = Lead()
self.lead_two = Lead()
self.v_desired_trajectory = np.zeros(CONTROL_N)
self.a_desired_trajectory = np.zeros(CONTROL_N)
self.j_desired_trajectory = np.zeros(CONTROL_N)
self.solverExecutionTime = 0.0
self.params = Params()
self.param_read_counter = 0
self.read_param()
self.personality = log.LongitudinalPersonality.standard
def read_param(self):
try:
self.personality = int(self.params.get('LongitudinalPersonality'))
except (ValueError, TypeError):
self.personality = log.LongitudinalPersonality.standard
# FrogPilot variables
self.params = Params()
self.params_memory = Params("/dev/shm/params")
self.radarless_model = self.params.get("Model", encoding='utf-8') in RADARLESS_MODELS
self.release = get_short_branch() == "FrogPilot"
self.update_frogpilot_params()
@staticmethod
def parse_model(model_msg, model_error):
def parse_model(model_msg, model_error, v_ego, taco_tune):
if (len(model_msg.position.x) == 33 and
len(model_msg.velocity.x) == 33 and
len(model_msg.acceleration.x) == 33):
@@ -91,12 +161,16 @@ class LongitudinalPlanner:
v = np.zeros(len(T_IDXS_MPC))
a = np.zeros(len(T_IDXS_MPC))
j = np.zeros(len(T_IDXS_MPC))
if taco_tune:
max_lat_accel = interp(v_ego, [5, 10, 20], [1.5, 2.0, 3.0])
curvatures = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.orientationRate.z) / np.clip(v, 0.3, 100.0)
max_v = np.sqrt(max_lat_accel / (np.abs(curvatures) + 1e-3)) - 2.0
v = np.minimum(max_v, v)
return x, v, a, j
def update(self, sm, frogpilot_planner, params_memory):
if self.param_read_counter % 50 == 0 or params_memory.get_bool("PersonalityChangedViaUI") or params_memory.get_bool("PersonalityChangedViaWheel"):
self.read_param()
self.param_read_counter += 1
def update(self, sm):
self.mpc.mode = 'blended' if sm['controlsState'].experimentalMode else 'acc'
v_ego = sm['carState'].vEgo
@@ -112,7 +186,7 @@ class LongitudinalPlanner:
# No change cost when user is controlling the speed, or when standstill
prev_accel_constraint = not (reset_state or sm['carState'].standstill)
accel_limits = frogpilot_planner.accel_limits
accel_limits = [sm['frogpilotPlan'].minAcceleration, sm['frogpilotPlan'].maxAcceleration]
if self.mpc.mode == 'acc':
accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngleDeg, accel_limits, self.CP)
else:
@@ -120,10 +194,8 @@ class LongitudinalPlanner:
if reset_state:
self.v_desired_filter.x = v_ego
# Slowly ramp up the acceleration to prevent jerky behaviors from a standstill
max_acceleration = min(interp(v_ego, ACCEL_MAX_PLUS_BP, ACCEL_MAX_PLUS), accel_limits[1])
# Clip aEgo to cruise limits to prevent large accelerations when becoming active
self.a_desired = clip(sm['carState'].aEgo, accel_limits[0], max_acceleration)
self.a_desired = clip(sm['carState'].aEgo, accel_limits[0], accel_limits[1])
# Prevent divergence, smooth in current v_ego
self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego))
@@ -136,13 +208,26 @@ class LongitudinalPlanner:
accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05)
accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05)
self.mpc.set_weights(prev_accel_constraint,
frogpilot_planner.custom_personalities, frogpilot_planner.aggressive_jerk, frogpilot_planner.standard_jerk, frogpilot_planner.relaxed_jerk,
personality=self.personality)
if self.radarless_model:
model_leads = list(sm['modelV2'].leadsV3)
# TODO lead state should be invalidated if its different point than the previous one
lead_states = [self.lead_one, self.lead_two]
for index in range(len(lead_states)):
if len(model_leads) > index:
model_lead = model_leads[index]
lead_states[index].update(model_lead.x[0], model_lead.y[0], model_lead.v[0], model_lead.a[0], model_lead.prob)
else:
lead_states[index].reset()
else:
self.lead_one = sm['radarState'].leadOne
self.lead_two = sm['radarState'].leadTwo
self.mpc.set_weights(sm['frogpilotPlan'].jerk, prev_accel_constraint, personality=sm['controlsState'].personality)
self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error)
self.mpc.update(sm['radarState'], frogpilot_planner.v_cruise, x, v, a, j, frogpilot_planner, personality=self.personality)
x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error, v_ego, self.taco_tune)
self.mpc.update(self.lead_one, self.lead_two, sm['frogpilotPlan'].vCruise, x, v, a, j, self.radarless_model, sm['frogpilotPlan'].tFollow,
sm['frogpilotCarControl'].trafficModeActive, personality=sm['controlsState'].personality)
self.v_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, self.mpc.v_solution)
self.a_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, self.mpc.a_solution)
@@ -160,9 +245,14 @@ class LongitudinalPlanner:
self.a_desired = float(interp(self.dt, ModelConstants.T_IDXS[:CONTROL_N], self.a_desired_trajectory))
self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.a_desired + a_prev) / 2.0
frogpilot_planner.update(sm['carState'], sm['controlsState'], sm['modelV2'], self.mpc, sm, v_cruise, v_ego)
if self.params_memory.get_bool("FrogPilotTogglesUpdated"):
self.update_frogpilot_params()
def publish(self, sm, pm, frogpilot_planner):
def update_frogpilot_params(self):
lateral_tune = self.params.get_bool("LateralTune")
self.taco_tune = lateral_tune and self.params.get_bool("TacoTune") and not self.release
def publish(self, sm, pm):
plan_send = messaging.new_message('longitudinalPlan')
plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState'])
@@ -175,13 +265,10 @@ class LongitudinalPlanner:
longitudinalPlan.accels = self.a_desired_trajectory.tolist()
longitudinalPlan.jerks = self.j_desired_trajectory.tolist()
longitudinalPlan.hasLead = sm['radarState'].leadOne.status
longitudinalPlan.hasLead = self.lead_one.status
longitudinalPlan.longitudinalPlanSource = self.mpc.source
longitudinalPlan.fcw = self.fcw
longitudinalPlan.solverExecutionTime = self.mpc.solve_time
longitudinalPlan.personality = self.personality
pm.send('longitudinalPlan', plan_send)
frogpilot_planner.publish(sm, pm, self.mpc)

0
selfdrive/controls/lib/tests/__init__.py Normal file
View File

45
selfdrive/controls/lib/tests/test_alertmanager.py Normal file
View File

@@ -0,0 +1,45 @@
#!/usr/bin/env python3
import random
import unittest
from openpilot.selfdrive.controls.lib.events import Alert, EVENTS
from openpilot.selfdrive.controls.lib.alertmanager import AlertManager
class TestAlertManager(unittest.TestCase):
def test_duration(self):
"""
Enforce that an alert lasts for max(alert duration, duration the alert is added)
"""
for duration in range(1, 100):
alert = None
while not isinstance(alert, Alert):
event = random.choice([e for e in EVENTS.values() if len(e)])
alert = random.choice(list(event.values()))
alert.duration = duration
# check two cases:
# - alert is added to AM for <= the alert's duration
# - alert is added to AM for > alert's duration
for greater in (True, False):
if greater:
add_duration = duration + random.randint(1, 10)
else:
add_duration = random.randint(1, duration)
show_duration = max(duration, add_duration)
AM = AlertManager()
for frame in range(duration+10):
if frame < add_duration:
AM.add_many(frame, [alert, ])
current_alert = AM.process_alerts(frame, {})
shown = current_alert is not None
should_show = frame <= show_duration
self.assertEqual(shown, should_show, msg=f"{frame=} {add_duration=} {duration=}")
if __name__ == "__main__":
unittest.main()

43
selfdrive/controls/lib/tests/test_latcontrol.py Normal file
View File

@@ -0,0 +1,43 @@
#!/usr/bin/env python3
import unittest
from parameterized import parameterized
from cereal import car, log
from openpilot.selfdrive.car.car_helpers import interfaces
from openpilot.selfdrive.car.honda.values import CAR as HONDA
from openpilot.selfdrive.car.toyota.values import CAR as TOYOTA
from openpilot.selfdrive.car.nissan.values import CAR as NISSAN
from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle
from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel
from openpilot.common.mock.generators import generate_liveLocationKalman
class TestLatControl(unittest.TestCase):
@parameterized.expand([(HONDA.CIVIC, LatControlPID), (TOYOTA.RAV4, LatControlTorque), (NISSAN.LEAF, LatControlAngle)])
def test_saturation(self, car_name, controller):
CarInterface, CarController, CarState = interfaces[car_name]
CP = CarInterface.get_non_essential_params(car_name)
CI = CarInterface(CP, CarController, CarState)
VM = VehicleModel(CP)
controller = controller(CP, CI)
CS = car.CarState.new_message()
CS.vEgo = 30
CS.steeringPressed = False
params = log.LiveParametersData.new_message()
llk = generate_liveLocationKalman()
for _ in range(1000):
_, _, lac_log = controller.update(True, CS, VM, params, False, 1, llk)
self.assertTrue(lac_log.saturated)
if __name__ == "__main__":
unittest.main()

70
selfdrive/controls/lib/tests/test_vehicle_model.py Normal file
View File

@@ -0,0 +1,70 @@
#!/usr/bin/env python3
import math
import unittest
import numpy as np
from control import StateSpace
from openpilot.selfdrive.car.honda.interface import CarInterface
from openpilot.selfdrive.car.honda.values import CAR
from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel, dyn_ss_sol, create_dyn_state_matrices
class TestVehicleModel(unittest.TestCase):
def setUp(self):
CP = CarInterface.get_non_essential_params(CAR.CIVIC)
self.VM = VehicleModel(CP)
def test_round_trip_yaw_rate(self):
# TODO: fix VM to work at zero speed
for u in np.linspace(1, 30, num=10):
for roll in np.linspace(math.radians(-20), math.radians(20), num=11):
for sa in np.linspace(math.radians(-20), math.radians(20), num=11):
yr = self.VM.yaw_rate(sa, u, roll)
new_sa = self.VM.get_steer_from_yaw_rate(yr, u, roll)
self.assertAlmostEqual(sa, new_sa)
def test_dyn_ss_sol_against_yaw_rate(self):
"""Verify that the yaw_rate helper function matches the results
from the state space model."""
for roll in np.linspace(math.radians(-20), math.radians(20), num=11):
for u in np.linspace(1, 30, num=10):
for sa in np.linspace(math.radians(-20), math.radians(20), num=11):
# Compute yaw rate based on state space model
_, yr1 = dyn_ss_sol(sa, u, roll, self.VM)
# Compute yaw rate using direct computations
yr2 = self.VM.yaw_rate(sa, u, roll)
self.assertAlmostEqual(float(yr1[0]), yr2)
def test_syn_ss_sol_simulate(self):
"""Verifies that dyn_ss_sol matches a simulation"""
for roll in np.linspace(math.radians(-20), math.radians(20), num=11):
for u in np.linspace(1, 30, num=10):
A, B = create_dyn_state_matrices(u, self.VM)
# Convert to discrete time system
ss = StateSpace(A, B, np.eye(2), np.zeros((2, 2)))
ss = ss.sample(0.01)
for sa in np.linspace(math.radians(-20), math.radians(20), num=11):
inp = np.array([[sa], [roll]])
# Simulate for 1 second
x1 = np.zeros((2, 1))
for _ in range(100):
x1 = ss.A @ x1 + ss.B @ inp
# Compute steady state solution directly
x2 = dyn_ss_sol(sa, u, roll, self.VM)
np.testing.assert_almost_equal(x1, x2, decimal=3)
if __name__ == "__main__":
unittest.main()

3
selfdrive/controls/lib/vehicle_model.py
View File

@@ -12,7 +12,6 @@ x_dot = A*x + B*u
A depends on longitudinal speed, u [m/s], and vehicle parameters CP
"""
from typing import Tuple
import numpy as np
from numpy.linalg import solve
@@ -169,7 +168,7 @@ def kin_ss_sol(sa: float, u: float, VM: VehicleModel) -> np.ndarray:
return K * sa
def create_dyn_state_matrices(u: float, VM: VehicleModel) -> Tuple[np.ndarray, np.ndarray]:
def create_dyn_state_matrices(u: float, VM: VehicleModel) -> tuple[np.ndarray, np.ndarray]:
"""Returns the A and B matrix for the dynamics system
Args: