From 8d693e1d41c84153f15f01136aca7a03fac50cd6 Mon Sep 17 00:00:00 2001 From: rav4kumar Date: Sat, 3 Aug 2024 21:49:10 -0700 Subject: [PATCH] dec update --- cereal/custom.capnp | 9 ++- .../controls/lib/longitudinal_planner.py | 11 ++- .../dynamic_experimental_controller.py | 81 +++++++++++-------- .../sunnypilot/qt/onroad/annotated_camera.cc | 12 ++- .../sunnypilot/qt/onroad/annotated_camera.h | 2 +- 5 files changed, 71 insertions(+), 44 deletions(-) diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 8825cf6f07..0c32edcf1d 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -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; diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index c2e3af1cd9..04df561c14 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -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) diff --git a/selfdrive/controls/lib/sunnypilot/dynamic_experimental_controller.py b/selfdrive/controls/lib/sunnypilot/dynamic_experimental_controller.py index ca118d666c..01b3f5441f 100644 --- a/selfdrive/controls/lib/sunnypilot/dynamic_experimental_controller.py +++ b/selfdrive/controls/lib/sunnypilot/dynamic_experimental_controller.py @@ -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 diff --git a/selfdrive/ui/sunnypilot/qt/onroad/annotated_camera.cc b/selfdrive/ui/sunnypilot/qt/onroad/annotated_camera.cc index 61247bea14..142400b8c3 100644 --- a/selfdrive/ui/sunnypilot/qt/onroad/annotated_camera.cc +++ b/selfdrive/ui/sunnypilot/qt/onroad/annotated_camera.cc @@ -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(scene.track_vertices.length() / 2, acceleration.size()); diff --git a/selfdrive/ui/sunnypilot/qt/onroad/annotated_camera.h b/selfdrive/ui/sunnypilot/qt/onroad/annotated_camera.h index 92bf5056b3..afcdfd860b 100644 --- a/selfdrive/ui/sunnypilot/qt/onroad/annotated_camera.h +++ b/selfdrive/ui/sunnypilot/qt/onroad/annotated_camera.h @@ -192,7 +192,7 @@ private: cereal::LongitudinalPlanSP::SpeedLimitControlState slcState; int longitudinalPersonality; int dynamicLaneProfile; - QString mpcMode; + QString mpcSource; int speed_limit_frame; bool slcShowSign = true;