mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-09-03 06:33:50 +08:00
Code Issues Packages Projects Releases Wiki Activity

FrogPilot 0.9.7

Browse Source
This commit is contained in:
FrogAi
2024-06-27 10:22:08 -07:00
parent da00915ac8
commit b705b02e70
682 changed files with 181798 additions and 1348 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/controlsd.py
Executable → Regular
+149 -29
View File
@@ -7,7 +7,7 @@ from typing import SupportsFloat
import cereal.messaging as messaging
from cereal import car, log
from cereal import car, custom, log
from msgq.visionipc import VisionIpcClient, VisionStreamType
@@ -15,7 +15,7 @@ from openpilot.common.conversions import Conversions as CV
from openpilot.common.git import get_short_branch
from openpilot.common.numpy_fast import clip
from openpilot.common.params import Params
from openpilot.common.realtime import config_realtime_process, Priority, Ratekeeper, DT_CTRL
from openpilot.common.realtime import config_realtime_process, Priority, Ratekeeper, DT_CTRL, DT_MDL
from openpilot.common.swaglog import cloudlog
from openpilot.selfdrive.car.car_helpers import get_car_interface, get_startup_event
@@ -31,6 +31,9 @@ from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel
from openpilot.system.hardware import HARDWARE
from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_acceleration import get_max_allowed_accel
from openpilot.selfdrive.frogpilot.frogpilot_variables import NON_DRIVING_GEARS, get_frogpilot_toggles, params_memory
SOFT_DISABLE_TIME = 3 # seconds
LDW_MIN_SPEED = 31 * CV.MPH_TO_MS
LANE_DEPARTURE_THRESHOLD = 0.1
@@ -49,6 +52,7 @@ LaneChangeState = log.LaneChangeState
LaneChangeDirection = log.LaneChangeDirection
EventName = car.CarEvent.EventName
ButtonType = car.CarState.ButtonEvent.Type
FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type
SafetyModel = car.CarParams.SafetyModel
IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput)
@@ -78,7 +82,7 @@ class Controls:
self.branch = get_short_branch()
# Setup sockets
self.pm = messaging.PubMaster(['controlsState', 'carControl', 'onroadEvents'])
self.pm = messaging.PubMaster(['controlsState', 'carControl', 'onroadEvents', 'frogpilotCarControl'])
self.sensor_packets = ["accelerometer", "gyroscope"]
self.camera_packets = ["roadCameraState", "driverCameraState", "wideRoadCameraState"]
@@ -94,10 +98,12 @@ class Controls:
if REPLAY:
# no vipc in replay will make them ignored anyways
ignore += ['roadCameraState', 'wideRoadCameraState']
if get_frogpilot_toggles().radarless_model:
ignore += ['radarState']
self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration',
'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'liveLocationKalman',
'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters',
'testJoystick'] + self.camera_packets + self.sensor_packets,
'testJoystick', 'frogpilotCarState', 'frogpilotPlan'] + self.camera_packets + self.sensor_packets,
ignore_alive=ignore, ignore_avg_freq=ignore+['radarState', 'testJoystick'], ignore_valid=['testJoystick', ],
frequency=int(1/DT_CTRL))
@@ -157,7 +163,10 @@ class Controls:
self.can_log_mono_time = 0
self.startup_event = get_startup_event(car_recognized, not self.CP.passive, len(self.CP.carFw) > 0)
if car_recognized and not self.CP.passive and self.CP.secOcRequired and not self.CP.secOcKeyAvailable:
self.startup_event = EventName.startupNoSecOcKey
else:
self.startup_event = get_startup_event(car_recognized, not self.CP.passive, len(self.CP.carFw) > 0)
if not sounds_available:
self.events.add(EventName.soundsUnavailable, static=True)
@@ -173,6 +182,24 @@ class Controls:
# controlsd is driven by carState, expected at 100Hz
self.rk = Ratekeeper(100, print_delay_threshold=None)
# FrogPilot variables
self.frogpilot_toggles = get_frogpilot_toggles()
self.accel_pressed = False
self.always_on_lateral_active = False
self.always_on_lateral_active_previously = False
self.decel_pressed = False
self.fcw_event_triggered = False
self.no_entry_alert_triggered = False
self.onroad_distance_pressed = False
self.steer_saturated_event_triggered = False
self.planner_curves = self.frogpilot_toggles.planner_curvature_model
self.radarless_model = self.frogpilot_toggles.radarless_model
self.use_old_long = self.frogpilot_toggles.old_long_api
self.display_timer = 0
def set_initial_state(self):
if REPLAY:
controls_state = self.params.get("ReplayControlsState")
@@ -260,12 +287,21 @@ class Controls:
direction = self.sm['modelV2'].meta.laneChangeDirection
if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \
(CS.rightBlindspot and direction == LaneChangeDirection.right):
self.events.add(EventName.laneChangeBlocked)
if self.frogpilot_toggles.loud_blindspot_alert:
self.events.add(EventName.laneChangeBlockedLoud)
else:
self.events.add(EventName.laneChangeBlocked)
else:
if direction == LaneChangeDirection.left:
self.events.add(EventName.preLaneChangeLeft)
if self.sm['frogpilotPlan'].laneWidthLeft >= self.frogpilot_toggles.lane_detection_width:
self.events.add(EventName.preLaneChangeLeft)
else:
self.events.add(EventName.noLaneAvailable)
else:
self.events.add(EventName.preLaneChangeRight)
if self.sm['frogpilotPlan'].laneWidthRight >= self.frogpilot_toggles.lane_detection_width:
self.events.add(EventName.preLaneChangeRight)
else:
self.events.add(EventName.noLaneAvailable)
elif self.sm['modelV2'].meta.laneChangeState in (LaneChangeState.laneChangeStarting,
LaneChangeState.laneChangeFinishing):
self.events.add(EventName.laneChange)
@@ -305,8 +341,9 @@ class Controls:
self.events.add(EventName.cameraFrameRate)
if not REPLAY and self.rk.lagging:
self.events.add(EventName.controlsdLagging)
if len(self.sm['radarState'].radarErrors) or ((not self.rk.lagging or REPLAY) and not self.sm.all_checks(['radarState'])):
self.events.add(EventName.radarFault)
if not self.radarless_model:
if len(self.sm['radarState'].radarErrors) or ((not self.rk.lagging or REPLAY) and not self.sm.all_checks(['radarState'])):
self.events.add(EventName.radarFault)
if not self.sm.valid['pandaStates']:
self.events.add(EventName.usbError)
if CS.canTimeout:
@@ -363,6 +400,8 @@ class Controls:
planner_fcw = self.sm['longitudinalPlan'].fcw and self.enabled
if (planner_fcw or model_fcw) and not (self.CP.notCar and self.joystick_mode):
self.events.add(EventName.fcw)
if self.frogpilot_toggles.random_events:
self.fcw_event_triggered = True
for m in messaging.drain_sock(self.log_sock, wait_for_one=False):
try:
@@ -387,6 +426,12 @@ class Controls:
if self.sm['modelV2'].frameDropPerc > 20:
self.events.add(EventName.modeldLagging)
# Add FrogPilot events
self.events.add_from_msg(self.sm['frogpilotPlan'].frogpilotEvents)
if self.frogpilot_toggles.block_user:
self.events.add(EventName.blockUser, static=True)
def data_sample(self):
"""Receive data from sockets"""
@@ -436,7 +481,7 @@ class Controls:
def state_transition(self, CS):
"""Compute conditional state transitions and execute actions on state transitions"""
self.v_cruise_helper.update_v_cruise(CS, self.enabled, self.is_metric)
self.v_cruise_helper.update_v_cruise(CS, self.enabled, self.is_metric, self.sm['frogpilotPlan'].speedLimitChanged, self.frogpilot_toggles)
# decrement the soft disable timer at every step, as it's reset on
# entrance in SOFT_DISABLING state
@@ -502,6 +547,8 @@ class Controls:
if self.events.contains(ET.ENABLE):
if self.events.contains(ET.NO_ENTRY):
self.current_alert_types.append(ET.NO_ENTRY)
if self.frogpilot_toggles.random_events:
self.no_entry_alert_triggered = True
else:
if self.events.contains(ET.PRE_ENABLE):
@@ -511,12 +558,12 @@ class Controls:
else:
self.state = State.enabled
self.current_alert_types.append(ET.ENABLE)
self.v_cruise_helper.initialize_v_cruise(CS, self.experimental_mode)
self.v_cruise_helper.initialize_v_cruise(CS, self.experimental_mode, self.sm['frogpilotPlan'].unconfirmedSlcSpeedLimit, self.frogpilot_toggles)
# Check if openpilot is engaged and actuators are enabled
self.enabled = self.state in ENABLED_STATES
self.active = self.state in ACTIVE_STATES
if self.active:
if self.active or self.always_on_lateral_active:
self.current_alert_types.append(ET.WARNING)
def state_control(self, CS):
@@ -531,7 +578,7 @@ class Controls:
# Update Torque Params
if self.CP.lateralTuning.which() == 'torque':
torque_params = self.sm['liveTorqueParameters']
if self.sm.all_checks(['liveTorqueParameters']) and torque_params.useParams:
if self.sm.all_checks(['liveTorqueParameters']) and (torque_params.useParams or self.frogpilot_toggles.force_auto_tune):
self.LaC.update_live_torque_params(torque_params.latAccelFactorFiltered, torque_params.latAccelOffsetFiltered,
torque_params.frictionCoefficientFiltered)
@@ -543,8 +590,8 @@ class Controls:
# Check which actuators can be enabled
standstill = CS.vEgo <= max(self.CP.minSteerSpeed, MIN_LATERAL_CONTROL_SPEED) or CS.standstill
CC.latActive = self.active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \
(not standstill or self.joystick_mode)
CC.latActive = (self.active or self.always_on_lateral_active) and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \
(not standstill or self.joystick_mode) and self.sm['frogpilotPlan'].lateralCheck
CC.longActive = self.enabled and not self.events.contains(ET.OVERRIDE_LONGITUDINAL) and self.CP.openpilotLongitudinalControl
actuators = CC.actuators
@@ -563,22 +610,33 @@ class Controls:
if not CC.latActive:
self.LaC.reset()
if not CC.longActive:
self.LoC.reset()
if self.use_old_long:
self.LoC.reset_old_long(v_pid=CS.vEgo)
else:
self.LoC.reset()
if not self.joystick_mode:
# accel PID loop
pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, self.v_cruise_helper.v_cruise_kph * CV.KPH_TO_MS)
actuators.accel = self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits)
if self.frogpilot_toggles.sport_plus:
pid_accel_limits = (pid_accel_limits[0], get_max_allowed_accel(CS.vEgo))
if self.use_old_long:
t_since_plan = (self.sm.frame - self.sm.recv_frame['longitudinalPlan']) * DT_CTRL
actuators.accel = self.LoC.update_old_long(CC.longActive, CS, long_plan, pid_accel_limits, t_since_plan, self.frogpilot_toggles)
else:
actuators.accel = self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop or self.sm['frogpilotPlan'].forcingStopLength < 1, pid_accel_limits, self.frogpilot_toggles)
if len(long_plan.speeds):
actuators.speed = long_plan.speeds[-1]
# Steering PID loop and lateral MPC
self.desired_curvature = clip_curvature(CS.vEgo, self.desired_curvature, model_v2.action.desiredCurvature)
self.desired_curvature = clip_curvature(CS.vEgo, self.desired_curvature, model_v2.action.desiredCurvature, self.planner_curves)
actuators.curvature = self.desired_curvature
actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp,
self.steer_limited, self.desired_curvature,
self.sm['liveLocationKalman'])
self.sm['liveLocationKalman'],
model_data=self.sm['modelV2'], frogpilot_toggles=self.frogpilot_toggles)
else:
lac_log = log.ControlsState.LateralDebugState.new_message()
if self.sm.recv_frame['testJoystick'] > 0:
@@ -614,7 +672,11 @@ class Controls:
good_speed = CS.vEgo > 5
max_torque = abs(self.sm['carOutput'].actuatorsOutput.steer) > 0.99
if undershooting and turning and good_speed and max_torque:
lac_log.active and self.events.add(EventName.steerSaturated)
lac_log.active and self.events.add(EventName.goatSteerSaturated if self.frogpilot_toggles.goat_scream_alert else EventName.steerSaturated)
if self.frogpilot_toggles.random_events:
self.steer_saturated_event_triggered = True
else:
self.steer_saturated_event_triggered = False
elif lac_log.saturated:
# TODO probably should not use dpath_points but curvature
dpath_points = model_v2.position.y
@@ -644,13 +706,64 @@ class Controls:
# decrement personality on distance button press
if self.CP.openpilotLongitudinalControl:
if any(not be.pressed and be.type == ButtonType.gapAdjustCruise for be in CS.buttonEvents):
self.personality = (self.personality - 1) % 3
self.params.put_nonblocking('LongitudinalPersonality', str(self.personality))
if any(not be.pressed and be.type == ButtonType.gapAdjustCruise for be in CS.buttonEvents) or self.onroad_distance_pressed:
menu_open = self.display_timer > 0 or not self.sm['frogpilotCarState'].hasMenu
if not (self.sm['frogpilotCarState'].distanceLongPressed or params_memory.get_bool("OnroadDistanceButtonPressed")) and menu_open:
self.personality = (self.personality - 1) % 3
self.params.put_nonblocking('LongitudinalPersonality', str(self.personality))
self.display_timer = 350
self.onroad_distance_pressed = params_memory.get_bool("OnroadDistanceButtonPressed")
return CC, lac_log
self.display_timer -= 1
def publish_logs(self, CS, start_time, CC, lac_log):
FPCC = self.update_frogpilot_variables(CS)
return CC, lac_log, FPCC
def update_frogpilot_variables(self, CS):
self.always_on_lateral_active |= self.frogpilot_toggles.always_on_lateral_main or CS.cruiseState.enabled
self.always_on_lateral_active &= self.frogpilot_toggles.always_on_lateral_set and CS.cruiseState.available
self.always_on_lateral_active &= CS.gearShifter not in NON_DRIVING_GEARS
self.always_on_lateral_active &= self.sm['frogpilotPlan'].lateralCheck
self.always_on_lateral_active &= self.sm['liveCalibration'].calPerc >= 1
self.always_on_lateral_active &= not (self.frogpilot_toggles.always_on_lateral_lkas and self.sm['frogpilotCarState'].alwaysOnLateralDisabled)
self.always_on_lateral_active &= not (CS.brakePressed and CS.vEgo < self.frogpilot_toggles.always_on_lateral_pause_speed) or CS.standstill
self.always_on_lateral_active = bool(self.always_on_lateral_active)
if self.frogpilot_toggles.conditional_experimental_mode or self.frogpilot_toggles.slc_fallback_experimental_mode:
self.experimental_mode = self.sm['frogpilotPlan'].experimentalMode
if any(be.pressed and be.type == FrogPilotButtonType.lkas for be in CS.buttonEvents):
if self.frogpilot_toggles.experimental_mode_via_lkas and self.enabled:
if self.frogpilot_toggles.conditional_experimental_mode:
conditional_status = params_memory.get_int("CEStatus")
override_value = 0 if conditional_status in {1, 2, 3, 4, 5, 6} else 3 if conditional_status >= 7 else 4
params_memory.put_int("CEStatus", override_value)
else:
self.experimental_mode = not self.experimental_mode
self.params.put_bool_nonblocking("ExperimentalMode", self.experimental_mode)
if self.sm.updated['frogpilotPlan'] or any(be.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for be in CS.buttonEvents):
self.accel_pressed = any(be.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for be in CS.buttonEvents)
if self.sm.updated['frogpilotPlan'] or any(be.type == ButtonType.decelCruise for be in CS.buttonEvents):
self.decel_pressed = any(be.type == ButtonType.decelCruise for be in CS.buttonEvents)
FPCC = custom.FrogPilotCarControl.new_message()
FPCC.alwaysOnLateralActive = self.always_on_lateral_active
FPCC.accelPressed = self.accel_pressed
FPCC.decelPressed = self.decel_pressed
FPCC.fcwEventTriggered = self.fcw_event_triggered
FPCC.noEntryEventTriggered = self.no_entry_alert_triggered
FPCC.steerSaturatedEventTriggered = self.steer_saturated_event_triggered
# Update FrogPilot parameters
if self.sm['frogpilotPlan'].togglesUpdated:
self.frogpilot_toggles = get_frogpilot_toggles()
return FPCC
def publish_logs(self, CS, start_time, CC, lac_log, FPCC):
"""Send actuators and hud commands to the car, send controlsstate and MPC logging"""
# Orientation and angle rates can be useful for carcontroller
@@ -709,7 +822,7 @@ class Controls:
if self.enabled:
clear_event_types.add(ET.NO_ENTRY)
alerts = self.events.create_alerts(self.current_alert_types, [self.CP, CS, self.sm, self.is_metric, self.soft_disable_timer])
alerts = self.events.create_alerts(self.current_alert_types, [self.CP, CS, self.sm, self.is_metric, self.soft_disable_timer, self.frogpilot_toggles])
self.AM.add_many(self.sm.frame, alerts)
current_alert = self.AM.process_alerts(self.sm.frame, clear_event_types)
if current_alert:
@@ -754,6 +867,7 @@ class Controls:
controlsState.state = self.state
controlsState.engageable = not self.events.contains(ET.NO_ENTRY)
controlsState.longControlState = self.LoC.long_control_state
controlsState.vPid = float(self.LoC.v_pid)
controlsState.vCruise = float(self.v_cruise_helper.v_cruise_kph)
controlsState.vCruiseCluster = float(self.v_cruise_helper.v_cruise_cluster_kph)
controlsState.upAccelCmd = float(self.LoC.pid.p)
@@ -791,6 +905,12 @@ class Controls:
cc_send.carControl = CC
self.pm.send('carControl', cc_send)
# frogpilotCarControl
fpcc_send = messaging.new_message('frogpilotCarControl')
fpcc_send.valid = CS.canValid
fpcc_send.frogpilotCarControl = FPCC
self.pm.send('frogpilotCarControl', fpcc_send)
def step(self):
start_time = time.monotonic()
@@ -806,10 +926,10 @@ class Controls:
self.state_transition(CS)
# Compute actuators (runs PID loops and lateral MPC)
CC, lac_log = self.state_control(CS)
CC, lac_log, FPCC = self.state_control(CS)
# Publish data
self.publish_logs(CS, start_time, CC, lac_log)
self.publish_logs(CS, start_time, CC, lac_log, FPCC)
self.CS_prev = CS