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

support custom personality

Browse Source
This commit is contained in:
rav4kumar
2025-03-31 06:15:21 -07:00
parent 63ba5e864f
commit 7aff82909a
6 changed files with 32 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
cereal/custom.capnp
+10
View File
@@ -10,6 +10,13 @@ $Cxx.namespace("cereal");
# DO rename the structs
# DON'T change the identifier (e.g. @0x81c2f05a394cf4af)
enum LongitudinalPersonalitySP {
aggressive @0;
standard @1;
relaxed @2;
overtake @3;
}
struct ModularAssistiveDrivingSystem {
state @0 :ModularAssistiveDrivingSystemState;
enabled @1 :Bool;
@@ -27,6 +34,7 @@ struct ModularAssistiveDrivingSystem {
struct SelfdriveStateSP @0x81c2f05a394cf4af {
mads @0 :ModularAssistiveDrivingSystem;
personality @1 :LongitudinalPersonalitySP;
}
struct ModelManagerSP @0xaedffd8f31e7b55d {
@@ -89,6 +97,8 @@ struct ModelManagerSP @0xaedffd8f31e7b55d {
struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
dec @0 :DynamicExperimentalControl;
personalityDEPRECATED @2 :LongitudinalPersonalitySP;
struct DynamicExperimentalControl {
state @0 :DynamicExperimentalControlState;
enabled @1 :Bool;
selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
+11 -11
View File
@@ -2,7 +2,7 @@
import os
import time
import numpy as np
from cereal import log
from cereal import log, custom
from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
from openpilot.common.realtime import DT_MDL
from openpilot.common.swaglog import cloudlog
@@ -58,23 +58,23 @@ STOP_DISTANCE = 6.0
CRUISE_MIN_ACCEL = -1.2
CRUISE_MAX_ACCEL = 1.6
def get_jerk_factor(personality=log.LongitudinalPersonality.standard):
if personality==log.LongitudinalPersonality.relaxed:
def get_jerk_factor(personality=custom.LongitudinalPersonalitySP.standard):
if personality==custom.LongitudinalPersonalitySP.relaxed:
return 1.0
elif personality==log.LongitudinalPersonality.standard:
elif personality==custom.LongitudinalPersonalitySP.standard:
return 1.0
elif personality==log.LongitudinalPersonality.aggressive:
elif personality==custom.LongitudinalPersonalitySP.aggressive:
return 0.5
else:
raise NotImplementedError("Longitudinal personality not supported")
def get_T_FOLLOW(personality=log.LongitudinalPersonality.standard):
if personality==log.LongitudinalPersonality.relaxed:
def get_T_FOLLOW(personality=custom.LongitudinalPersonalitySP.standard):
if personality==custom.LongitudinalPersonalitySP.relaxed:
return 1.75
elif personality==log.LongitudinalPersonality.standard:
elif personality==custom.LongitudinalPersonalitySP.standard:
return 1.45
elif personality==log.LongitudinalPersonality.aggressive:
elif personality==custom.LongitudinalPersonalitySP.aggressive:
return 1.25
else:
raise NotImplementedError("Longitudinal personality not supported")
@@ -274,7 +274,7 @@ class LongitudinalMpc:
for i in range(N):
self.solver.cost_set(i, 'Zl', Zl)
def set_weights(self, prev_accel_constraint=True, personality=log.LongitudinalPersonality.standard):
def set_weights(self, prev_accel_constraint=True, personality=custom.LongitudinalPersonalitySP.standard):
jerk_factor = get_jerk_factor(personality)
if self.mode == 'acc':
a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0
@@ -327,7 +327,7 @@ class LongitudinalMpc:
lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau)
return lead_xv
def update(self, radarstate, v_cruise, x, v, a, j, personality=log.LongitudinalPersonality.standard):
def update(self, radarstate, v_cruise, x, v, a, j, personality=custom.LongitudinalPersonalitySP.standard):
t_follow = get_T_FOLLOW(personality)
v_ego = self.x0[1]
self.status = radarstate.leadOne.status or radarstate.leadTwo.status
selfdrive/controls/lib/longitudinal_planner.py
+6 -4
View File
@@ -1,7 +1,7 @@
#!/usr/bin/env python3
import math
import numpy as np
from cereal import custom
import cereal.messaging as messaging
from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
from openpilot.common.conversions import Conversions as CV
@@ -163,9 +163,11 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
if force_slow_decel:
v_cruise = 0.0
self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality)
self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveStateSP'].personality)
self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=sm['selfdriveState'].personality)
self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=sm['selfdriveStateSP'].personality)
self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution)
self.a_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution)
@@ -193,7 +195,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
def publish(self, sm, pm):
plan_send = messaging.new_message('longitudinalPlan')
plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState', 'selfdriveState'])
plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState', 'selfdriveState', 'selfdriveStateSP'])
longitudinalPlan = plan_send.longitudinalPlan
longitudinalPlan.modelMonoTime = sm.logMonoTime['modelV2']
selfdrive/controls/plannerd.py
+1 -1
View File
@@ -19,7 +19,7 @@ def main():
ldw = LaneDepartureWarning()
longitudinal_planner = LongitudinalPlanner(CP)
pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance', 'longitudinalPlanSP'])
sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'liveParameters', 'radarState', 'modelV2', 'selfdriveState'],
sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'liveParameters', 'radarState', 'modelV2', 'selfdriveState', 'selfdriveStateSP'],
poll='modelV2')
while True:
selfdrive/selfdrived/events.py
+1 -1
View File
@@ -2,7 +2,7 @@
import math
import os
from cereal import log, car
from cereal import log, car, custom
import cereal.messaging as messaging
from openpilot.common.conversions import Conversions as CV
from openpilot.common.git import get_short_branch
selfdrive/selfdrived/selfdrived.py
+3 -3
View File
@@ -32,7 +32,7 @@ REPLAY = "REPLAY" in os.environ
SIMULATION = "SIMULATION" in os.environ
TESTING_CLOSET = "TESTING_CLOSET" in os.environ
IGNORE_PROCESSES = {"loggerd", "encoderd", "statsd"}
LONGITUDINAL_PERSONALITY_MAP = {v: k for k, v in log.LongitudinalPersonality.schema.enumerants.items()}
LONGITUDINAL_PERSONALITY_MAP = {v: k for k, v in custom.LongitudinalPersonalitySP.schema.enumerants.items()}
ThermalStatus = log.DeviceState.ThermalStatus
State = log.SelfdriveState.OpenpilotState
@@ -475,7 +475,6 @@ class SelfdriveD(CruiseHelper):
ss.state = self.state_machine.state
ss.engageable = not self.events.contains(ET.NO_ENTRY)
ss.experimentalMode = self.experimental_mode
ss.personality = self.personality
ss.alertText1 = self.AM.current_alert.alert_text_1
ss.alertText2 = self.AM.current_alert.alert_text_2
@@ -504,6 +503,7 @@ class SelfdriveD(CruiseHelper):
mads.enabled = self.mads.enabled
mads.active = self.mads.active
mads.available = self.mads.enabled_toggle
ss_sp.personality = self.personality
self.pm.send('selfdriveStateSP', ss_sp_msg)
@@ -532,7 +532,7 @@ class SelfdriveD(CruiseHelper):
try:
return int(self.params.get('LongitudinalPersonality'))
except (ValueError, TypeError):
return log.LongitudinalPersonality.standard
return custom.LongitudinalPersonality.standard
def params_thread(self, evt):
while not evt.is_set():