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

mvp

Browse Source
This commit is contained in:
royjr
2026-11-03 17:39:26 -05:00
parent b57b184c73
commit 4087a4f369
7 changed files with 6 additions and 99 deletions
Download Patch File Download Diff File Expand all files Collapse all files
opendbc_repo
+1 -1
Submodule opendbc_repo updated: 8f8b4114c7...5e53e1d8a8
openpilot/cereal/custom.capnp
-38
View File
@@ -389,44 +389,6 @@ struct CarControlSP @0xa5cd762cd951a455 {
leadOne @2 :LeadData;
leadTwo @3 :LeadData;
intelligentCruiseButtonManagement @4 :IntelligentCruiseButtonManagement;
smartCruiseControl @5 :SmartCruiseControl;
speedLimit @6 :SpeedLimit;
laneChange @7 :LaneChange;
driverMonitoring @8 :DriverMonitoring;
leadFollowing @9 :Bool;
struct SmartCruiseControl {
visionState @0 :LongitudinalPlanSP.SmartCruiseControl.VisionState;
mapState @1 :LongitudinalPlanSP.SmartCruiseControl.MapState;
}
struct SpeedLimit {
assistState @0 :LongitudinalPlanSP.SpeedLimit.AssistState;
speedLimitFinalLast @1 :Float32;
}
struct DriverMonitoring {
alertLevel @0 :UInt8;
visionPolicy @1 :Bool;
}
struct LaneChange {
state @0 :State;
direction @1 :Direction;
enum State {
off @0;
preLaneChange @1;
laneChangeStarting @2;
laneChangeFinishing @3;
}
enum Direction {
none @0;
left @1;
right @2;
}
}
struct Param {
key @0 :Text;
openpilot/selfdrive/car/helpers.py
-4
View File
@@ -63,9 +63,5 @@ def convert_carControlSP(struct: capnp.lib.capnp._DynamicStructReader) -> struct
struct_dataclass.intelligentCruiseButtonManagement = structs.IntelligentCruiseButtonManagement(
**remove_deprecated(struct_dict.get('intelligentCruiseButtonManagement', {}))
)
struct_dataclass.smartCruiseControl = structs.CarControlSP.SmartCruiseControl(**struct_dict.get('smartCruiseControl', {}))
struct_dataclass.speedLimit = structs.CarControlSP.SpeedLimit(**struct_dict.get('speedLimit', {}))
struct_dataclass.laneChange = structs.CarControlSP.LaneChange(**struct_dict.get('laneChange', {}))
struct_dataclass.driverMonitoring = structs.CarControlSP.DriverMonitoring(**struct_dict.get('driverMonitoring', {}))
return struct_dataclass
openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
+2 -15
View File
@@ -57,14 +57,6 @@ COMFORT_BRAKE = 2.5
STOP_DISTANCE = 6.0
MIN_X_LEAD_FACTOR = 0.5
# aggressive follows close to stock Hyundai SCC closest setting: 2.4 m stopped (stock 2.4), 16.3 m @ 31 mph (stock 14.8), 22 m @ 40 (20.2), 27.5 m @ 48 (25.2)
# the MPC settles ~0.8 m short of its target at standstill, so a 3.2 m target stops at ~2.4 m
# the short stop gap fades out by 10 m/s and moving gaps are ~0.1 s over stock: exact stock gaps crash in sim when the lead brakes 2 m/s^2 to a stop
AGGRESSIVE_STOP_DISTANCE_BP = [0., 3., 10.]
AGGRESSIVE_STOP_DISTANCE_V = [3.2, 3.2, STOP_DISTANCE]
AGGRESSIVE_T_FOLLOW_BP = [0., 10., 14., 16., 18., 20., 21.3, 30.]
AGGRESSIVE_T_FOLLOW_V = [0.74, 0.74, 0.74, 0.81, 0.90, 0.945, 1.0, 1.1]
def get_jerk_factor(personality=log.LongitudinalPersonality.standard):
if personality==log.LongitudinalPersonality.relaxed:
return 1.0
@@ -317,11 +309,6 @@ class LongitudinalMpc:
def update(self, radarstate, personality=log.LongitudinalPersonality.standard):
t_follow = get_T_FOLLOW(personality)
stop_distance_offset = 0.
if personality == log.LongitudinalPersonality.aggressive:
t_follow = float(np.interp(self.x0[1], AGGRESSIVE_T_FOLLOW_BP, AGGRESSIVE_T_FOLLOW_V))
# STOP_DISTANCE is compiled into the solver, so move the lead forward instead
stop_distance_offset = STOP_DISTANCE - float(np.interp(self.x0[1], AGGRESSIVE_STOP_DISTANCE_BP, AGGRESSIVE_STOP_DISTANCE_V))
lead_xv_0 = self.process_lead(radarstate.leadOne)
lead_xv_1 = self.process_lead(radarstate.leadTwo)
@@ -329,8 +316,8 @@ class LongitudinalMpc:
# To estimate a safe distance from a moving lead, we calculate how much stopping
# distance that lead needs as a minimum. We can add that to the current distance
# and then treat that as a stopped car/obstacle at this new distance.
lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1]) + stop_distance_offset
lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1]) + stop_distance_offset
lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1])
lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1])
x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle])
self.source = MPC_SOURCES[np.argmin(x_obstacles[0])]
openpilot/selfdrive/controls/lib/longitudinal_planner.py
-6
View File
@@ -2,7 +2,6 @@
import numpy as np
import openpilot.cereal.messaging as messaging
from openpilot.cereal import log
from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
from openpilot.common.constants import CV
from openpilot.common.filter_simple import FirstOrderFilter
@@ -21,7 +20,6 @@ from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import Lon
J_CRUISE_VALS = [1.6, 1.2, 0.8, 0.6]
J_CRUISE_BP = [0., 10.0, 25., 40.]
A_CRUISE_MIN = -0.5
AGGRESSIVE_LEAD_ONLY_DIST = 40. # m
CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]
@@ -116,10 +114,6 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
candidates = [(output_a_target_mpc, self.mpc.source, output_should_stop_mpc),
(self.a_cruise, LongitudinalPlanSource.cruise, cruise_should_stop),
(output_a_target_e2e, LongitudinalPlanSource.e2e, output_should_stop_e2e)]
# aggressive: with a close radar lead, follow it like stock SCC instead of the model's larger stop gap
lead = sm['radarState'].leadOne
if sm['selfdriveState'].personality == log.LongitudinalPersonality.aggressive and lead.radar and lead.dRel < AGGRESSIVE_LEAD_ONLY_DIST:
candidates.pop()
output_a_target, self.mpc.source, _ = min(candidates, key=lambda c: c[0])
self.output_should_stop = any(should_stop for _, _, should_stop in candidates)
openpilot/selfdrive/controls/tests/test_following_distance.py
+2 -9
View File
@@ -4,9 +4,7 @@ from openpilot.common.parameterized import parameterized_class
from openpilot.cereal import log
import numpy as np
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import get_safe_obstacle_distance, get_stopped_equivalence_factor, get_T_FOLLOW, \
STOP_DISTANCE, AGGRESSIVE_STOP_DISTANCE_BP, AGGRESSIVE_STOP_DISTANCE_V, AGGRESSIVE_T_FOLLOW_BP, AGGRESSIVE_T_FOLLOW_V
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import get_safe_obstacle_distance, get_stopped_equivalence_factor, get_T_FOLLOW
from openpilot.selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver
@@ -40,12 +38,7 @@ class TestFollowingDistance(OpenpilotTestCase):
def test_following_distance(self):
v_lead = float(self.speed)
simulation_steady_state = run_following_distance_simulation(v_lead, personality=self.personality)
if self.personality == log.LongitudinalPersonality.aggressive:
t_follow = float(np.interp(v_lead, AGGRESSIVE_T_FOLLOW_BP, AGGRESSIVE_T_FOLLOW_V))
stop_distance = float(np.interp(v_lead, AGGRESSIVE_STOP_DISTANCE_BP, AGGRESSIVE_STOP_DISTANCE_V))
correct_steady_state = desired_follow_distance(v_lead, v_lead, t_follow) - (STOP_DISTANCE - stop_distance)
else:
correct_steady_state = desired_follow_distance(v_lead, v_lead, get_T_FOLLOW(self.personality))
correct_steady_state = desired_follow_distance(v_lead, v_lead, get_T_FOLLOW(self.personality))
abs_err_margin = 0.5 if v_lead > 0.0 else 1.15
self.assertAlmostEqual(simulation_steady_state, correct_steady_state,
delta=0.2 * correct_steady_state + abs_err_margin)
openpilot/sunnypilot/selfdrive/controls/controlsd_ext.py
+1 -26
View File
@@ -31,7 +31,7 @@ class ControlsExt(ModelStateBase):
self.CP_SP = messaging.log_from_bytes(params.get("CarParamsSP", block=True), custom.CarParamsSP)
cloudlog.info("controlsd_ext got CarParamsSP")
self.sm_services_ext = ['radarState', 'selfdriveStateSP', 'longitudinalPlanSP']
self.sm_services_ext = ['radarState', 'selfdriveStateSP']
self.pm_services_ext = ['carControlSP']
def initialize_lateral_control(self, lac, CI, dt):
@@ -104,31 +104,6 @@ class ControlsExt(ModelStateBase):
CC_SP.intelligentCruiseButtonManagement.sendButton = icbm_src.sendButton
CC_SP.intelligentCruiseButtonManagement.vTarget = icbm_src.vTarget
# Smart Cruise Control state
scc_src = sm['longitudinalPlanSP'].smartCruiseControl
CC_SP.smartCruiseControl.visionState = scc_src.vision.state
CC_SP.smartCruiseControl.mapState = scc_src.map.state
# Speed Limit Assist state
sl_src = sm['longitudinalPlanSP'].speedLimit
CC_SP.speedLimit.assistState = sl_src.assist.state
CC_SP.speedLimit.speedLimitFinalLast = sl_src.resolver.speedLimitFinalLast
# Lane change state
meta = sm['modelV2'].meta
CC_SP.laneChange.state = str(meta.laneChangeState)
CC_SP.laneChange.direction = str(meta.laneChangeDirection)
# Driver monitoring alert
dm = sm['driverMonitoringState']
CC_SP.driverMonitoring.alertLevel = dm.alertLevel.raw
CC_SP.driverMonitoring.visionPolicy = dm.activePolicy == 'vision'
# Planner following a lead, the model path (e2e) wins often while following so count it when there is a lead
long_plan = sm['longitudinalPlan']
CC_SP.leadFollowing = str(long_plan.longitudinalPlanSource) in ('lead0', 'lead1', 'lead2') or \
(str(long_plan.longitudinalPlanSource) == 'e2e' and long_plan.hasLead)
return CC_SP
@staticmethod