mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-08-05 17:45:40 +08:00
Code Issues Packages Projects Releases Wiki Activity

dec update

Browse Source
This commit is contained in:
rav4kumar
2024-08-03 21:49:10 -07:00
parent cab9ccf0d8
commit 8d693e1d41
5 changed files with 71 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files
cereal/custom.capnp
+8 -1
View File
@@ -34,6 +34,11 @@ enum ModelGeneration {
five @5;
}
enum MpcSource {
acc @0;
blended @1;
}
struct ControlsStateSP @0x81c2f05a394cf4af {
lateralState @0 :Text;
personality @8 :LongitudinalPersonalitySP;
@@ -90,8 +95,10 @@ struct LongitudinalPlanSP @0xaedffd8f31e7b55d {
desiredTF @13 :Float32;
notSpeedLimit @14 :Int16;
e2eX @15 :List(Float32);
e2eBlended @18 :Text;
e2eBlendedDEPRECATED @18 :Text;
e2eStatus @22 :Bool;
mpcSource @23 :MpcSource;
dynamicExperimentalControl @24 :Bool;
distToTurn @7 :Float32;
turnSpeed @8 :Float32;
selfdrive/controls/lib/longitudinal_planner.py
+8 -3
View File
@@ -3,7 +3,7 @@ import math
import numpy as np
from openpilot.common.numpy_fast import clip, interp
from openpilot.common.params import Params
from cereal import car
from cereal import car, custom
import cereal.messaging as messaging
from openpilot.common.conversions import Conversions as CV
@@ -35,6 +35,7 @@ _A_TOTAL_MAX_V = [1.7, 3.2]
_A_TOTAL_MAX_BP = [20., 40.]
MpcSource = custom.MpcSource
EventName = car.CarEvent.EventName
@@ -129,10 +130,13 @@ class LongitudinalPlanner:
self.read_param()
self.param_read_counter += 1
if self.dynamic_experimental_controller.is_enabled() and sm['controlsState'].experimentalMode:
self.mpc.mode = self.dynamic_experimental_controller.get_mpc_mode(self.CP.radarUnavailable, sm['carState'], sm['radarState'].leadOne, sm['modelV2'], sm['controlsState'], sm['navInstruction'].maneuverDistance)
self.dynamic_experimental_controller.set_mpc_fcw_crash_cnt(self.mpc.crash_cnt)
self.dynamic_experimental_controller.update(self.CP.radarUnavailable, sm['carState'], sm['radarState'].leadOne, sm['modelV2'], sm['controlsState'], sm['navInstruction'].maneuverDistance)
self.mpc.mode = self.dynamic_experimental_controller.get_mpc_mode()
else:
self.mpc.mode = 'blended' if sm['controlsState'].experimentalMode else 'acc'
v_ego = sm['carState'].vEgo
v_cruise_kph = min(sm['controlsState'].vCruise, V_CRUISE_MAX)
v_cruise = v_cruise_kph * CV.KPH_TO_MS
@@ -262,7 +266,8 @@ class LongitudinalPlanner:
longitudinalPlanSP.turnSpeedControlState = self.turn_speed_controller.state
longitudinalPlanSP.turnSpeed = float(self.turn_speed_controller.v_target)
longitudinalPlanSP.e2eBlended = self.mpc.mode
longitudinalPlanSP.mpcSource = MpcSource.blended if self.mpc.mode == 'blended' else MpcSource.acc
longitudinalPlanSP.dynamicExperimentalControl = self.dynamic_experimental_controller.is_enabled()
pm.send('longitudinalPlanSP', plan_sp_send)
selfdrive/controls/lib/sunnypilot/dynamic_experimental_controller.py
+46 -35
View File
@@ -21,33 +21,38 @@
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
# THE SOFTWARE.
#
# Version = 2024-1-29
# Version = 2024-7-11
from common.numpy_fast import interp
from openpilot.selfdrive.controls.lib.lateral_planner import TRAJECTORY_SIZE
LEAD_WINDOW_SIZE = 5
LEAD_PROB = 0.6
# d-e2e, from modeldata.h
TRAJECTORY_SIZE = 33
LEAD_WINDOW_SIZE = 3
LEAD_PROB = 0.5
SLOW_DOWN_WINDOW_SIZE = 5
SLOW_DOWN_PROB = 0.6
SLOW_DOWN_PROB = 0.5
SLOW_DOWN_BP = [0., 10., 20., 30., 40., 50., 55., 60.]
SLOW_DOWN_DIST = [20, 30., 50., 70., 80., 90., 105., 120.]
SLOWNESS_WINDOW_SIZE = 20
SLOWNESS_PROB = 0.6
SLOWNESS_WINDOW_SIZE = 10
SLOWNESS_PROB = 0.5
SLOWNESS_CRUISE_OFFSET = 1.05
DANGEROUS_TTC_WINDOW_SIZE = 5
DANGEROUS_TTC = 2.0
DANGEROUS_TTC_WINDOW_SIZE = 3
DANGEROUS_TTC = 2.3
HIGHWAY_CRUISE_KPH = 75
HIGHWAY_CRUISE_KPH = 70
STOP_AND_GO_FRAME = 60
SET_MODE_TIMEOUT = 10
MPC_FCW_WINDOW_SIZE = 5
MPC_FCW_PROB = 0.6
MPC_FCW_WINDOW_SIZE = 10
MPC_FCW_PROB = 0.5
V_ACC_MIN = 9.72
class SNG_State:
@@ -83,6 +88,7 @@ class DynamicExperimentalController:
self._is_enabled = False
self._mode = 'acc'
self._mode_prev = 'acc'
self._mode_changed = False
self._frame = 0
self._lead_gmac = GenericMovingAverageCalculator(window_size=LEAD_WINDOW_SIZE)
@@ -128,18 +134,18 @@ class DynamicExperimentalController:
# fcw detection
self._mpc_fcw_gmac.add_data(self._mpc_fcw_crash_cnt > 0)
self._has_mpc_fcw = self._mpc_fcw_gmac.get_moving_average() >= MPC_FCW_PROB
self._has_mpc_fcw = self._mpc_fcw_gmac.get_moving_average() > MPC_FCW_PROB
# nav enable detection
self._has_nav_instruction = md.navEnabledDEPRECATED and maneuver_distance / max(car_state.vEgo, 1) < 13
# lead detection
self._lead_gmac.add_data(lead_one.status)
self._has_lead_filtered = self._lead_gmac.get_moving_average() >= LEAD_PROB
self._has_lead_filtered = self._lead_gmac.get_moving_average() > LEAD_PROB
# slow down detection
self._slow_down_gmac.add_data(len(md.orientation.x) == len(md.position.x) == TRAJECTORY_SIZE and md.position.x[TRAJECTORY_SIZE - 1] < interp(self._v_ego_kph, SLOW_DOWN_BP, SLOW_DOWN_DIST))
self._has_slow_down = self._slow_down_gmac.get_moving_average() >= SLOW_DOWN_PROB
self._has_slow_down = self._slow_down_gmac.get_moving_average() > SLOW_DOWN_PROB
# blinker detection
self._has_blinkers = car_state.leftBlinker or car_state.rightBlinker
@@ -161,7 +167,7 @@ class DynamicExperimentalController:
# slowness detection
if not self._has_standstill:
self._slowness_gmac.add_data(self._v_ego_kph <= (self._v_cruise_kph*SLOWNESS_CRUISE_OFFSET))
self._has_slowness = self._slowness_gmac.get_moving_average() >= SLOWNESS_PROB
self._has_slowness = self._slowness_gmac.get_moving_average() > SLOWNESS_PROB
# dangerous TTC detection
if not self._has_lead_filtered and self._has_lead_filtered_prev:
@@ -178,7 +184,7 @@ class DynamicExperimentalController:
self._has_lead_filtered_prev = self._has_lead_filtered
self._frame += 1
def _blended_priority_mode(self):
def _radarless_mode(self):
# when mpc fcw crash prob is high
# use blended to slow down quickly
if self._has_mpc_fcw:
@@ -190,21 +196,21 @@ class DynamicExperimentalController:
self._set_mode('blended')
return
# when blinker is on and speed is driving below highway cruise speed: blended
# when blinker is on and speed is driving below V_ACC_MIN: blended
# we dont want it to switch mode at higher speed, blended may trigger hard brake
if self._has_blinkers and self._v_ego_kph < HIGHWAY_CRUISE_KPH:
if self._has_blinkers and self._v_ego_kph < V_ACC_MIN:
self._set_mode('blended')
return
# when at highway cruise and SNG: blended
# ensuring blended mode is used because acc is bad at catching SNG lead car
# especially those who accel very fast and then brake very hard.
if self._sng_state == SNG_State.going and self._v_cruise_kph >= HIGHWAY_CRUISE_KPH:
self._set_mode('blended')
return
#if self._sng_state == SNG_State.going and self._v_cruise_kph >= V_ACC_MIN:
# self._set_mode('blended')
# return
# when standstill: blended
# in case of lead car suddenly move away under traffic light, acc mode wont brake at traffic light.
# in case of lead car suddenly move away under traffic light, acc mode won't brake at traffic light.
if self._has_standstill:
self._set_mode('blended')
return
@@ -226,9 +232,9 @@ class DynamicExperimentalController:
self._set_mode('acc')
return
self._set_mode('blended')
self._set_mode('acc')
def _acc_priority_mode(self):
def _radar_mode(self):
# when mpc fcw crash prob is high
# use blended to slow down quickly
if self._has_mpc_fcw:
@@ -236,18 +242,18 @@ class DynamicExperimentalController:
return
# If there is a filtered lead, the vehicle is not in standstill, and the lead vehicle's yRel meets the condition,
#if self._has_lead_filtered and not self._has_standstill:
# self._set_mode('acc')
# return
if self._has_lead_filtered and not self._has_standstill:
self._set_mode('acc')
return
# when blinker is on and speed is driving below highway cruise speed: blended
# when blinker is on and speed is driving below V_ACC_MIN: blended
# we dont want it to switch mode at higher speed, blended may trigger hard brake
if self._has_blinkers and self._v_ego_kph < HIGHWAY_CRUISE_KPH:
if self._has_blinkers and self._v_ego_kph < V_ACC_MIN:
self._set_mode('blended')
return
# when standstill: blended
# in case of lead car suddenly move away under traffic light, acc mode wont brake at traffic light.
# in case of lead car suddenly move away under traffic light, acc mode won't brake at traffic light.
if self._has_standstill:
self._set_mode('blended')
return
@@ -270,17 +276,22 @@ class DynamicExperimentalController:
self._set_mode('acc')
def get_mpc_mode(self, radar_unavailable, car_state, lead_one, md, controls_state, maneuver_distance):
def update(self, radar_unavailable, car_state, lead_one, md, controls_state, maneuver_distance):
if self._is_enabled:
self._update(car_state, lead_one, md, controls_state, maneuver_distance)
if radar_unavailable:
self._blended_priority_mode()
self._radarless_mode()
else:
self._acc_priority_mode()
self._radar_mode()
self._mode_changed = self._mode != self._mode_prev
self._mode_prev = self._mode
def get_mpc_mode(self):
return self._mode
def has_changed(self):
return self._mode_changed
def set_enabled(self, enabled):
self._is_enabled = enabled
selfdrive/ui/sunnypilot/qt/onroad/annotated_camera.cc
+8 -4
View File
@@ -320,8 +320,8 @@ void AnnotatedCameraWidgetSP::updateState(const UIStateSP &s) {
// TODO: Add toggle variables to cereal, and parse from cereal
longitudinalPersonality = s.scene.longitudinal_personality;
dynamicLaneProfile = s.scene.dynamic_lane_profile;
mpcMode = QString::fromStdString(lp_sp.getE2eBlended());
mpcMode = (mpcMode == "blended") ? mpcMode.replace(0, 1, mpcMode[0].toUpper()) : mpcMode.toUpper();
const auto mpc_source = lp_sp.getMpcSource();
mpcSource = mpc_source == cereal::MpcSource::BLENDED ? QString(tr("blended")) : QString(tr("acc"));
static int reverse_delay = 0;
bool reverse_allowed = false;
@@ -1061,7 +1061,7 @@ void AnnotatedCameraWidgetSP::drawFeatureStatusText(QPainter &p, int x, int y) {
p.setBrush(dec_color);
p.drawEllipse(dec_btn);
QString dec_status_text;
dec_status_text.sprintf("DEC: %s\n", dynamicExperimentalControlToggle ? (experimentalMode ? QString(mpcMode).toStdString().c_str() : QString("Inactive").toStdString().c_str()) : "OFF");
dec_status_text.sprintf("DEC: %s\n", dynamicExperimentalControlToggle ? (experimentalMode ? QString(mpcSource).toStdString().c_str() : QString("Inactive").toStdString().c_str()) : "OFF");
p.setPen(QPen(shadow_color, 2));
p.drawText(x + drop_shadow_size, y + drop_shadow_size, dec_status_text);
p.setPen(QPen(text_color, 2));
@@ -1177,6 +1177,10 @@ void AnnotatedCameraWidgetSP::drawLaneLines(QPainter &painter, const UIStateSP *
// paint path
QLinearGradient bg(0, height(), 0, 0);
const auto long_plan_sp = sm["longitudinalPlanSP"].getLongitudinalPlanSP();
bool exp_mode_path = (long_plan_sp.getDynamicExperimentalControl() && long_plan_sp.getMpcSource() == cereal::MpcSource::BLENDED) ||
(!long_plan_sp.getDynamicExperimentalControl() && sm["controlsState"].getControlsState().getExperimentalMode());
if (madsEnabled || car_state.getCruiseState().getEnabled()) {
if (steerOverride && latActive) {
bg.setColorAt(0.0, QColor::fromHslF(20 / 360., 0.94, 0.51, 0.17));
@@ -1185,7 +1189,7 @@ void AnnotatedCameraWidgetSP::drawLaneLines(QPainter &painter, const UIStateSP *
} else if (!(latActive || car_state.getCruiseState().getEnabled())) {
bg.setColorAt(0, whiteColor());
bg.setColorAt(1, whiteColor(0));
} else if (sm["controlsState"].getControlsState().getExperimentalMode()) {
} else if (exp_mode_path) {
// The first half of track_vertices are the points for the right side of the path
const auto &acceleration = sm["modelV2"].getModelV2().getAcceleration().getX();
const int max_len = std::min<int>(scene.track_vertices.length() / 2, acceleration.size());
selfdrive/ui/sunnypilot/qt/onroad/annotated_camera.h
+1 -1
View File
@@ -192,7 +192,7 @@ private:
cereal::LongitudinalPlanSP::SpeedLimitControlState slcState;
int longitudinalPersonality;
int dynamicLaneProfile;
QString mpcMode;
QString mpcSource;
int speed_limit_frame;
bool slcShowSign = true;