diff --git a/cereal/custom.capnp b/cereal/custom.capnp index fdb89da84c..5468e0f39d 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -172,6 +172,8 @@ struct OnroadEventSP @0xda96579883444c35 { experimentalModeSwitched @14; wrongCarModeAlertOnly @15; pedalPressedAlertOnly @16; + laneTurnLeft @17; + laneTurnRight @18; } } @@ -258,9 +260,16 @@ struct LiveMapDataSP @0xf416ec09499d9d19 { roadName @5 :Text; } -struct CustomReserved9 @0xa1680744031fdb2d { +struct ModelDataV2SP @0xa1680744031fdb2d { + laneTurnDirection @0 :TurnDirection; } + enum TurnDirection { + none @0; + turnLeft @1; + turnRight @2; + } + struct CustomReserved10 @0xcb9fd56c7057593a { } diff --git a/cereal/log.capnp b/cereal/log.capnp index 15795f8c38..474b4777cb 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -2631,7 +2631,7 @@ struct Event { backupManagerSP @113 :Custom.BackupManagerSP; carStateSP @114 :Custom.CarStateSP; liveMapDataSP @115 :Custom.LiveMapDataSP; - customReserved9 @116 :Custom.CustomReserved9; + modelDataV2SP @116 :Custom.ModelDataV2SP; customReserved10 @136 :Custom.CustomReserved10; customReserved11 @137 :Custom.CustomReserved11; customReserved12 @138 :Custom.CustomReserved12; diff --git a/cereal/services.py b/cereal/services.py index 373e865e34..b48290bc77 100755 --- a/cereal/services.py +++ b/cereal/services.py @@ -88,6 +88,7 @@ _services: dict[str, tuple] = { "carControlSP": (True, 100., 10), "carStateSP": (True, 100., 10), "liveMapDataSP": (True, 1., 1), + "modelDataV2SP": (True, 20.), # debug "uiDebug": (True, 0., 1), diff --git a/common/params_keys.h b/common/params_keys.h index fa60e27a9d..1ead1caa63 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -195,10 +195,12 @@ inline static std::unordered_map keys = { {"DynamicExperimentalControl", {PERSISTENT | BACKUP, BOOL, "0"}}, {"BlindSpot", {PERSISTENT | BACKUP, BOOL, "0"}}, - // model panel params + // sunnypilot model params {"LagdToggle", {PERSISTENT | BACKUP, BOOL, "1"}}, {"LagdToggleDelay", {PERSISTENT | BACKUP, FLOAT, "0.2"}}, {"LagdValueCache", {PERSISTENT, FLOAT, "0.2"}}, + {"LaneTurnDesire", {PERSISTENT | BACKUP, BOOL, "0"}}, + {"LaneTurnValue", {PERSISTENT | BACKUP, FLOAT, "19.0"}}, // mapd {"MapAdvisorySpeedLimit", {CLEAR_ON_ONROAD_TRANSITION, FLOAT}}, diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index 2adfa65f6d..93088182f9 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -1,7 +1,8 @@ -from cereal import log +from cereal import log, custom from openpilot.common.constants import CV from openpilot.common.realtime import DT_MDL from openpilot.sunnypilot.selfdrive.controls.lib.auto_lane_change import AutoLaneChangeController, AutoLaneChangeMode +from openpilot.sunnypilot.selfdrive.controls.lib.lane_turn_desire import LaneTurnController LaneChangeState = log.LaneChangeState LaneChangeDirection = log.LaneChangeDirection @@ -30,6 +31,12 @@ DESIRES = { }, } +TURN_DESIRES = { + custom.TurnDirection.none: log.Desire.none, + custom.TurnDirection.turnLeft: log.Desire.turnLeft, + custom.TurnDirection.turnRight: log.Desire.turnRight, +} + class DesireHelper: def __init__(self): @@ -41,13 +48,21 @@ class DesireHelper: self.prev_one_blinker = False self.desire = log.Desire.none self.alc = AutoLaneChangeController(self) + self.lane_turn_controller = LaneTurnController(self) + self.lane_turn_direction = custom.TurnDirection.none def update(self, carstate, lateral_active, lane_change_prob): self.alc.update_params() + self.lane_turn_controller.update_params() v_ego = carstate.vEgo one_blinker = carstate.leftBlinker != carstate.rightBlinker below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN + # Lane turn controller update + self.lane_turn_controller.update_lane_turn(blindspot_left=carstate.leftBlindspot, blindspot_right=carstate.rightBlindspot, + left_blinker=carstate.leftBlinker, right_blinker=carstate.rightBlinker, v_ego=v_ego) + self.lane_turn_direction = self.lane_turn_controller.get_turn_direction() + if not lateral_active or self.lane_change_timer > LANE_CHANGE_TIME_MAX or self.alc.lane_change_set_timer == AutoLaneChangeMode.OFF: self.lane_change_state = LaneChangeState.off self.lane_change_direction = LaneChangeDirection.none @@ -106,7 +121,10 @@ class DesireHelper: self.prev_one_blinker = one_blinker - self.desire = DESIRES[self.lane_change_direction][self.lane_change_state] + if self.lane_turn_direction != custom.TurnDirection.none: + self.desire = TURN_DESIRES[self.lane_turn_direction] + else: + self.desire = DESIRES[self.lane_change_direction][self.lane_change_state] # Send keep pulse once per second during LaneChangeStart.preLaneChange if self.lane_change_state in (LaneChangeState.off, LaneChangeState.laneChangeStarting): diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index caf342e88b..5f9ccf5072 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -216,7 +216,7 @@ def main(demo=False): cloudlog.warning(f"connected extra cam with buffer size: {vipc_client_extra.buffer_len} ({vipc_client_extra.width} x {vipc_client_extra.height})") # messaging - pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry"]) + pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry", "modelDataV2SP"]) sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl", "liveDelay"]) publish_state = PublishState() @@ -333,6 +333,7 @@ def main(demo=False): modelv2_send = messaging.new_message('modelV2') drivingdata_send = messaging.new_message('drivingModelData') posenet_send = messaging.new_message('cameraOdometry') + mdv2sp_send = messaging.new_message('modelDataV2SP') action = get_action_from_model(model_output, prev_action, lat_delay + DT_MDL, long_delay + DT_MDL, v_ego) prev_action = action @@ -347,6 +348,7 @@ def main(demo=False): DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob) modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction + mdv2sp_send.modelDataV2SP.laneTurnDirection = DH.lane_turn_direction drivingdata_send.drivingModelData.meta.laneChangeState = DH.lane_change_state drivingdata_send.drivingModelData.meta.laneChangeDirection = DH.lane_change_direction @@ -354,6 +356,7 @@ def main(demo=False): pm.send('modelV2', modelv2_send) pm.send('drivingModelData', drivingdata_send) pm.send('cameraOdometry', posenet_send) + pm.send('modelDataV2SP', mdv2sp_send) last_vipc_frame_id = meta_main.frame_id diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index 249621d6fc..c56c7fe65f 100755 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -86,7 +86,7 @@ class SelfdriveD(CruiseHelper): # TODO: de-couple selfdrived with card/conflate on carState without introducing controls mismatches self.car_state_sock = messaging.sub_sock('carState', timeout=20) - ignore = self.sensor_packets + self.gps_packets + ['alertDebug'] + ignore = self.sensor_packets + self.gps_packets + ['alertDebug'] + ['modelDataV2SP'] if SIMULATION: ignore += ['driverCameraState', 'managerState'] if REPLAY: @@ -95,7 +95,8 @@ class SelfdriveD(CruiseHelper): self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration', 'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'livePose', 'liveDelay', 'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters', - 'controlsState', 'carControl', 'driverAssistance', 'alertDebug', 'userBookmark', 'audioFeedback'] + \ + 'controlsState', 'carControl', 'driverAssistance', 'alertDebug', 'userBookmark', 'audioFeedback', + 'modelDataV2SP'] + \ self.camera_packets + self.sensor_packets + self.gps_packets, ignore_alive=ignore, ignore_avg_freq=ignore, ignore_valid=ignore, frequency=int(1/DT_CTRL)) @@ -300,6 +301,13 @@ class SelfdriveD(CruiseHelper): LaneChangeState.laneChangeFinishing): self.events.add(EventName.laneChange) + # Handle lane turn + lane_turn_direction = self.sm['modelDataV2SP'].laneTurnDirection + if lane_turn_direction == custom.TurnDirection.turnLeft: + self.events_sp.add(custom.OnroadEventSP.EventName.laneTurnLeft) + elif lane_turn_direction == custom.TurnDirection.turnRight: + self.events_sp.add(custom.OnroadEventSP.EventName.laneTurnRight) + for i, pandaState in enumerate(self.sm['pandaStates']): # All pandas must match the list of safetyConfigs, and if outside this list, must be silent or noOutput if i < len(self.CP.safetyConfigs): diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.cc index baba7d3a17..02a01a4b63 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.cc @@ -48,6 +48,25 @@ static const QString progressStyleError = progressStyleActive + " background-color: transparent;" "}"; +std::optional safeParamEventLoad(Params& params, const std::string& paramName) { + std::string raw = params.get(paramName); + if (raw.empty()) { + return std::nullopt; + } + + try { + AlignedBuffer alignedBuf; + auto buf = alignedBuf.align(raw.data(), raw.size()); + + capnp::FlatArrayMessageReader msg(kj::ArrayPtr(buf.begin(), buf.size())); + return msg.getRoot(); + } + catch (const kj::Exception& e) { + qInfo() << "Invalid param" << QString::fromStdString(paramName) << ":" << e.getDescription().cStr(); + return std::nullopt; + } +} + ModelsPanel::ModelsPanel(QWidget *parent) : QWidget(parent) { QVBoxLayout *main_layout = new QVBoxLayout(this); main_layout->setContentsMargins(50, 20, 50, 20); @@ -101,9 +120,32 @@ ModelsPanel::ModelsPanel(QWidget *parent) : QWidget(parent) { QString policyType = tr("Policy Model"); policyFrame = createModelDetailFrame(this, policyType, policyProgressBar); list->addItem(policyFrame); - list->addItem(horizontal_line()); + // Lane Turn Desire toggle + lane_turn_desire_toggle = new ParamControlSP("LaneTurnDesire", tr("Use Lane Turn Desires"), + "If you’re driving at 20 mph (32 km/h) or below and have your blinker on, " + "the car will plan a turn in that direction at the nearest drivable path. " + "This prevents situations (like at red lights) where the car might plan the wrong turn direction.", + "../assets/offroad/icon_shell.png"); + list->addItem(lane_turn_desire_toggle); + + // Lane Turn Value control + int max_value_mph = 20; + bool is_metric_initial = params.getBool("IsMetric"); + const float K = 1.609344f; + int per_value_change_scaled = is_metric_initial ? static_cast(std::round((1.0f / K) * 100.0f)) : 100; // 100 -> 1 mph + lane_turn_value_control = new OptionControlSP("LaneTurnValue", tr("Adjust Lane Turn Speed"), + tr("Set the maximum speed for lane turn desires. Default is 19 %1.").arg(is_metric_initial ? "km/h" : "mph"), + "", {5 * 100, max_value_mph * 100}, per_value_change_scaled, false, nullptr, true, true); + lane_turn_value_control->showDescription(); + list->addItem(lane_turn_value_control); + + // Show based on toggle + refreshLaneTurnValueControl(); + connect(lane_turn_desire_toggle, &ParamControlSP::toggleFlipped, this, &ModelsPanel::refreshLaneTurnValueControl); + connect(lane_turn_value_control, &OptionControlSP::updateLabels, this, &ModelsPanel::refreshLaneTurnValueControl); + // LiveDelay toggle lagd_toggle_control = new ParamControlSP("LagdToggle", tr("Live Learning Steer Delay"), "", "../assets/offroad/icon_shell.png"); lagd_toggle_control->showDescription(); @@ -111,17 +153,10 @@ ModelsPanel::ModelsPanel(QWidget *parent) : QWidget(parent) { // Software delay control int liveDelayMaxInt = 30; - std::string liveDelayBytes = params.get("LiveDelay"); - if (!liveDelayBytes.empty()) { - capnp::FlatArrayMessageReader msg(kj::ArrayPtr( - reinterpret_cast(liveDelayBytes.data()), - liveDelayBytes.size() / sizeof(capnp::word))); - auto event = msg.getRoot(); - if (event.hasLiveDelay()) { - auto liveDelay = event.getLiveDelay(); - float lateralDelay = liveDelay.getLateralDelay(); - liveDelayMaxInt = static_cast(lateralDelay * 100.0f) + 20; - } + if (const auto event = safeParamEventLoad(params, "LiveDelay"); event && event->hasLiveDelay()) { + auto liveDelay = event->getLiveDelay(); + float lateralDelay = liveDelay.getLateralDelay(); + liveDelayMaxInt = static_cast(lateralDelay * 100.0f) + 20; } delay_control = new OptionControlSP("LagdToggleDelay", tr("Adjust Software Delay"), tr("Adjust the software delay when Live Learning Steer Delay is toggled off." @@ -159,6 +194,19 @@ QFrame* ModelsPanel::createModelDetailFrame(QWidget *parent, QString &typeName, return frame; } +void ModelsPanel::refreshLaneTurnValueControl() { + if (!lane_turn_value_control) return; + float stored_mph = QString::fromStdString(params.get("LaneTurnValue")).toFloat(); + bool is_metric = params.getBool("IsMetric"); + QString unit = is_metric ? "km/h" : "mph"; + float display_value = stored_mph; + if (is_metric) { + display_value = stored_mph * 1.609344f; + } + lane_turn_value_control->setLabel(QString::number(static_cast(std::round(display_value))) + " " + unit); + lane_turn_value_control->setVisible(params.getBool("LaneTurnDesire")); +} + /** * @brief Updates the UI with bundle download progress information * Reads status from modelManagerSP cereal message and displays status for all models @@ -401,18 +449,11 @@ void ModelsPanel::updateLabels() { "Disable to use a fixed steering response time. Keeping this on provides the stock openpilot experience."); bool lagdEnabled = params.getBool("LagdToggle"); if (lagdEnabled) { - std::string liveDelayBytes = params.get("LiveDelay"); - if (!liveDelayBytes.empty()) { - capnp::FlatArrayMessageReader msg(kj::ArrayPtr( - reinterpret_cast(liveDelayBytes.data()), - liveDelayBytes.size() / sizeof(capnp::word))); - auto event = msg.getRoot(); - if (event.hasLiveDelay()) { - auto liveDelay = event.getLiveDelay(); - float lateralDelay = liveDelay.getLateralDelay(); - desc += QString("

%1 %2 s") + if (const auto event = safeParamEventLoad(params, "LiveDelay"); event && event->hasLiveDelay()) { + auto liveDelay = event->getLiveDelay(); + float lateralDelay = liveDelay.getLateralDelay(); + desc += QString("

%1 %2 s") .arg(tr("Live Steer Delay:")).arg(QString::number(lateralDelay, 'f', 3)); - } } } else { std::string carParamsBytes = params.get("CarParamsPersistent"); @@ -439,6 +480,9 @@ void ModelsPanel::updateLabels() { delay_control->setLabel(QString::number(value, 'f', 2) + "s"); } + // Update lane turn desire label and visibility + refreshLaneTurnValueControl(); + clearModelCacheBtn->setValue(QString::number(calculateCacheSize(), 'f', 2) + " MB"); } diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.h index 93edc4de1a..1906ebd2a0 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.h +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.h @@ -37,6 +37,7 @@ private: void updateLabels(); void handleCurrentModelLblBtnClicked(); void handleBundleDownloadProgress(); + void refreshLaneTurnValueControl(); void showResetParamsDialog(); QProgressBar* createProgressBar(QWidget *parent); QFrame* createModelDetailFrame(QWidget *parent, QString &typeName, QProgressBar *progressBar); @@ -81,5 +82,6 @@ private: Params params; ButtonControlSP *clearModelCacheBtn; ButtonControlSP *refreshAvailableModelsBtn; - + ParamControlSP *lane_turn_desire_toggle; + OptionControlSP *lane_turn_value_control; }; diff --git a/sunnypilot/modeld/modeld.py b/sunnypilot/modeld/modeld.py index b94e166bc6..3d11ed23f4 100755 --- a/sunnypilot/modeld/modeld.py +++ b/sunnypilot/modeld/modeld.py @@ -177,7 +177,7 @@ def main(demo=False): cloudlog.warning(f"connected extra cam with buffer size: {vipc_client_extra.buffer_len} ({vipc_client_extra.width} x {vipc_client_extra.height})") # messaging - pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry"]) + pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry", "modelDataV2SP"]) sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl", "liveDelay"]) publish_state = PublishState() @@ -304,6 +304,7 @@ def main(demo=False): modelv2_send = messaging.new_message('modelV2') drivingdata_send = messaging.new_message('drivingModelData') posenet_send = messaging.new_message('cameraOdometry') + mdv2sp_send = messaging.new_message('modelDataV2SP') action = model.get_action_from_model(model_output, prev_action, long_delay + DT_MDL) fill_model_msg(drivingdata_send, modelv2_send, model_output, action, publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id, frame_drop_ratio, meta_main.timestamp_eof, model_execution_time, live_calib_seen, @@ -316,6 +317,7 @@ def main(demo=False): DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob) modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction + mdv2sp_send.modelDataV2SP.laneTurnDirection = DH.lane_turn_direction drivingdata_send.drivingModelData.meta.laneChangeState = DH.lane_change_state drivingdata_send.drivingModelData.meta.laneChangeDirection = DH.lane_change_direction @@ -323,6 +325,7 @@ def main(demo=False): pm.send('modelV2', modelv2_send) pm.send('drivingModelData', drivingdata_send) pm.send('cameraOdometry', posenet_send) + pm.send('modelDataV2SP', mdv2sp_send) last_vipc_frame_id = meta_main.frame_id diff --git a/sunnypilot/modeld_v2/modeld.py b/sunnypilot/modeld_v2/modeld.py index 1a420e6f1a..bf89bc98d6 100755 --- a/sunnypilot/modeld_v2/modeld.py +++ b/sunnypilot/modeld_v2/modeld.py @@ -202,7 +202,7 @@ def main(demo=False): cloudlog.warning(f"connected extra cam with buffer size: {vipc_client_extra.buffer_len} ({vipc_client_extra.width} x {vipc_client_extra.height})") # messaging - pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry"]) + pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry", "modelDataV2SP"]) sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl", "liveDelay"]) publish_state = PublishState() @@ -322,6 +322,7 @@ def main(demo=False): modelv2_send = messaging.new_message('modelV2') drivingdata_send = messaging.new_message('drivingModelData') posenet_send = messaging.new_message('cameraOdometry') + mdv2sp_send = messaging.new_message('modelDataV2SP') action = model.get_action_from_model(model_output, prev_action, lat_delay + DT_MDL, long_delay + DT_MDL, v_ego) prev_action = action @@ -336,6 +337,7 @@ def main(demo=False): DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob) modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction + mdv2sp_send.modelDataV2SP.laneTurnDirection = DH.lane_turn_direction drivingdata_send.drivingModelData.meta.laneChangeState = DH.lane_change_state drivingdata_send.drivingModelData.meta.laneChangeDirection = DH.lane_change_direction @@ -343,6 +345,7 @@ def main(demo=False): pm.send('modelV2', modelv2_send) pm.send('drivingModelData', drivingdata_send) pm.send('cameraOdometry', posenet_send) + pm.send('modelDataV2SP', mdv2sp_send) last_vipc_frame_id = meta_main.frame_id diff --git a/sunnypilot/models/fetcher.py b/sunnypilot/models/fetcher.py index 358c65fe34..e681d150b3 100644 --- a/sunnypilot/models/fetcher.py +++ b/sunnypilot/models/fetcher.py @@ -8,6 +8,7 @@ See the LICENSE.md file in the root directory for more details. import time import requests +from requests.exceptions import (SSLError, RequestException, HTTPError) from openpilot.common.params import Params from openpilot.common.swaglog import cloudlog from sunnypilot.models.helpers import is_bundle_version_compatible @@ -122,19 +123,36 @@ class ModelFetcher: self.model_cache = ModelCache(params) self.model_parser = ModelParser() - def _fetch_and_cache_models(self) -> list[custom.ModelManagerSP.ModelBundle]: - """Fetches fresh model data from remote and updates cache""" + def _fetch_and_cache_models(self) -> list[custom.ModelManagerSP.ModelBundle] | None: + """Fetches fresh model data from remote and updates cache. + Returns None on transport errors. Raises on 404 and other fatal HTTP errors. + """ try: response = requests.get(self.MODEL_URL, timeout=10) - response.raise_for_status() - json_data = response.json() + # Explicitly handle 404 differently + if response.status_code == 404: + cloudlog.error(f"Models URL returned 404 Not Found: {self.MODEL_URL}") + raise HTTPError(f"404 Not Found: {self.MODEL_URL}", response=response) + + # Raise for any other 4xx/5xx + response.raise_for_status() + + json_data = response.json() self.model_cache.set(json_data) cloudlog.debug("Successfully updated models cache") return self.model_parser.parse_models(json_data) - except Exception: - cloudlog.exception("Error fetching models") - raise + + except ConnectionError as e: + cloudlog.warning(f"DNS/connection error while fetching models: {e}") + except SSLError as e: + cloudlog.warning(f"SSL error while fetching models: {e}") + except RequestException as e: + cloudlog.exception(f"Request transport error while fetching models: {e}") + except Exception as e: + cloudlog.exception(f"Unexpected error fetching models: {e}") + + return None def get_available_bundles(self) -> list[custom.ModelManagerSP.ModelBundle]: """Gets the list of available models, with smart cache handling""" @@ -144,12 +162,12 @@ class ModelFetcher: cloudlog.debug("Using valid cached models data") return self.model_parser.parse_models(cached_data) - try: - return self._fetch_and_cache_models() - except Exception: - if not cached_data: - cloudlog.exception("Failed to fetch fresh data and no cache available") - raise + fetched_bundles = self._fetch_and_cache_models() + if fetched_bundles is not None: + return fetched_bundles + + if not cached_data: + cloudlog.warning("Failed to fetch fresh data and no cache available") cloudlog.warning("Failed to fetch fresh data. Using expired cache as fallback") return self.model_parser.parse_models(cached_data) diff --git a/sunnypilot/selfdrive/controls/lib/lane_turn_desire.py b/sunnypilot/selfdrive/controls/lib/lane_turn_desire.py new file mode 100644 index 0000000000..00ce026abb --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/lane_turn_desire.py @@ -0,0 +1,45 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" +from cereal import custom + +from openpilot.common.constants import CV +from openpilot.common.params import Params + +LANE_CHANGE_SPEED_MIN = 20 * CV.MPH_TO_MS + + +class LaneTurnController: + def __init__(self, desire_helper): + self.DH = desire_helper + self.turn_direction = custom.TurnDirection.none + self.params = Params() + self.lane_turn_value = float(self.params.get("LaneTurnValue", return_default=True)) * CV.MPH_TO_MS + self.param_read_counter = 0 + self.enabled = self.params.get_bool("LaneTurnDesire") + + def read_params(self): + self.enabled = self.params.get_bool("LaneTurnDesire") + value = float(self.params.get("LaneTurnValue", return_default=True)) * CV.MPH_TO_MS + self.lane_turn_value = min(float(LANE_CHANGE_SPEED_MIN), value) + + def update_params(self) -> None: + if self.param_read_counter % 50 == 0: + self.read_params() + self.param_read_counter += 1 + + def update_lane_turn(self, blindspot_left: bool, blindspot_right: bool, left_blinker: bool, right_blinker: bool, v_ego: float) -> None: + if left_blinker and not right_blinker and v_ego < self.lane_turn_value and not blindspot_left: + self.turn_direction = custom.TurnDirection.turnLeft + elif right_blinker and not left_blinker and v_ego < self.lane_turn_value and not blindspot_right: + self.turn_direction = custom.TurnDirection.turnRight + else: + self.turn_direction = custom.TurnDirection.none + + def get_turn_direction(self): + if not self.enabled: + return custom.TurnDirection.none + return self.turn_direction diff --git a/sunnypilot/selfdrive/controls/lib/tests/test_lane_turn_desire.py b/sunnypilot/selfdrive/controls/lib/tests/test_lane_turn_desire.py new file mode 100644 index 0000000000..5633ed6efc --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/tests/test_lane_turn_desire.py @@ -0,0 +1,113 @@ +import pytest +from cereal import log +from openpilot.common.params import Params + +from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper +from openpilot.sunnypilot.selfdrive.controls.lib.lane_turn_desire import LaneTurnController, LANE_CHANGE_SPEED_MIN +from openpilot.sunnypilot.selfdrive.controls.lib.auto_lane_change import AutoLaneChangeMode + + +class TurnDirection: + none = 0 + turnLeft = 1 + turnRight = 2 + + +@pytest.mark.parametrize("left_blinker,right_blinker,v_ego,blindspot_left,blindspot_right,expected", [ + (True, False, 5, False, False, TurnDirection.turnLeft), + (False, True, 6, False, False, TurnDirection.turnRight), + (True, False, 9, False, False, TurnDirection.none), + (True, False, 7, True, False, TurnDirection.none), + (False, True, 6, False, True, TurnDirection.none), + (False, False, 5, False, False, TurnDirection.none), + (True, True, 5, False, False, TurnDirection.none), +]) +def test_lane_turn_desire_conditions(left_blinker, right_blinker, v_ego, blindspot_left, blindspot_right, expected): + dh = DesireHelper() + controller = LaneTurnController(dh) + controller.enabled = True + controller.lane_turn_value = LANE_CHANGE_SPEED_MIN + controller.turn_direction = TurnDirection.none + controller.update_lane_turn(blindspot_left, blindspot_right, left_blinker, right_blinker, v_ego) + assert controller.get_turn_direction() == expected + + +def test_lane_turn_desire_disabled(): + dh = DesireHelper() + controller = LaneTurnController(dh) + controller.enabled = False + controller.lane_turn_value = LANE_CHANGE_SPEED_MIN + controller.turn_direction = TurnDirection.none + controller.update_lane_turn(False, False, True, False, 7) + assert controller.get_turn_direction() == TurnDirection.none + + +def test_lane_turn_overrides_lane_change(): + dh = DesireHelper() + controller = LaneTurnController(dh) + controller.enabled = True + controller.lane_turn_value = LANE_CHANGE_SPEED_MIN + controller.turn_direction = TurnDirection.none + # left turn desire + controller.update_lane_turn(False, False, True, False, 5) + assert controller.get_turn_direction() == TurnDirection.turnLeft + # right turn desire + controller.update_lane_turn(False, False, False, True, 6) + assert controller.get_turn_direction() == TurnDirection.turnRight + # no turn + controller.update_lane_turn(False, False, False, False, 7) + assert controller.get_turn_direction() == TurnDirection.none + + +@pytest.mark.parametrize("v_ego,expected", [ + (8.93, TurnDirection.turnLeft), # just below threshold + (8.96, TurnDirection.none), # above threshold + (8.95, TurnDirection.none), # just above threshold +]) +def test_lane_turn_desire_speed_boundary(v_ego, expected): + dh = DesireHelper() + controller = LaneTurnController(dh) + controller.enabled = True + controller.lane_turn_value = LANE_CHANGE_SPEED_MIN + controller.turn_direction = TurnDirection.none + controller.update_lane_turn(False, True, True, False, v_ego) + assert controller.get_turn_direction() == expected + + +class DummyCarState: + def __init__(self, vEgo=0, leftBlinker=False, rightBlinker=False, leftBlindspot=False, rightBlindspot=False, + steeringPressed=False, steeringTorque=0, brakePressed=False): + self.vEgo = vEgo + self.leftBlinker = leftBlinker + self.rightBlinker = rightBlinker + self.leftBlindspot = leftBlindspot + self.rightBlindspot = rightBlindspot + self.steeringPressed = steeringPressed + self.steeringTorque = steeringTorque + self.brakePressed = brakePressed + +@pytest.fixture +def set_lane_turn_params(): + params = Params() + params.put("LaneTurnDesire", True) + params.put("LaneTurnValue", 20.0) + +@pytest.mark.parametrize("carstate, lateral_active, lane_change_prob, expected_desire", [ + # Lane turn desire overrides lane change desire + (DummyCarState(vEgo=5, leftBlinker=True, rightBlinker=False, leftBlindspot=False, rightBlindspot=False), True, 1.0, log.Desire.turnLeft), + (DummyCarState(vEgo=7, leftBlinker=False, rightBlinker=True, leftBlindspot=False, rightBlindspot=False), True, 1.0, log.Desire.turnRight), + # Lane change desire only (no turn desires) + (DummyCarState(vEgo=9, leftBlinker=True, rightBlinker=False, leftBlindspot=False, rightBlindspot=False, + steeringPressed=True, steeringTorque=1), True, 1.0, log.Desire.laneChangeLeft), + (DummyCarState(vEgo=9, leftBlinker=False, rightBlinker=True, leftBlindspot=False, rightBlindspot=False, + steeringPressed=True, steeringTorque=-1), True, 1.0, log.Desire.laneChangeRight), + # No desire (inactive) + (DummyCarState(vEgo=9, leftBlinker=False, rightBlinker=False), False, 1.0, log.Desire.none), + (DummyCarState(vEgo=4, leftBlinker=False, rightBlinker=False), True, 1.0, log.Desire.none), # No blinkers? no desire! +]) +def test_desire_helper_integration(carstate, lateral_active, lane_change_prob, expected_desire, set_lane_turn_params): + dh = DesireHelper() + dh.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGE + for _ in range(10): + dh.update(carstate, lateral_active, lane_change_prob) + assert dh.desire == expected_desire # The first four tests were unit tests to test the controller, where this tests the integration in desire helpers diff --git a/sunnypilot/selfdrive/selfdrived/events.py b/sunnypilot/selfdrive/selfdrived/events.py index d81345a14e..7c723397fb 100644 --- a/sunnypilot/selfdrive/selfdrived/events.py +++ b/sunnypilot/selfdrive/selfdrived/events.py @@ -132,6 +132,21 @@ EVENTS_SP: dict[int, dict[str, Alert | AlertCallbackType]] = { EventNameSP.pedalPressedAlertOnly: { ET.WARNING: NoEntryAlert("Pedal Pressed") - } + }, + EventNameSP.laneTurnLeft: { + ET.WARNING: Alert( + "Turning Left", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.none, 1.), + }, + + EventNameSP.laneTurnRight: { + ET.WARNING: Alert( + "Turning Right", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.none, 1.), + } } diff --git a/sunnypilot/sunnylink/athena/sunnylinkd.py b/sunnypilot/sunnylink/athena/sunnylinkd.py index 90eae1dfe8..363fa1defc 100755 --- a/sunnypilot/sunnylink/athena/sunnylinkd.py +++ b/sunnypilot/sunnylink/athena/sunnylinkd.py @@ -3,7 +3,9 @@ from __future__ import annotations import base64 +import errno import gzip +import json import os import ssl import threading @@ -17,11 +19,11 @@ from openpilot.common.swaglog import cloudlog from openpilot.system.athena.athenad import ws_send, jsonrpc_handler, \ recv_queue, UploadQueueCache, upload_queue, cur_upload_items, backoff, ws_manage, log_handler, start_local_proxy_shim, upload_handler from websocket import (ABNF, WebSocket, WebSocketException, WebSocketTimeoutException, - create_connection) + create_connection, WebSocketConnectionClosedException) import cereal.messaging as messaging from sunnypilot.sunnylink.api import SunnylinkApi -from sunnypilot.sunnylink.utils import sunnylink_need_register, sunnylink_ready +from sunnypilot.sunnylink.utils import sunnylink_need_register, sunnylink_ready, get_param_as_byte SUNNYLINK_ATHENA_HOST = os.getenv('SUNNYLINK_ATHENA_HOST', 'wss://ws.stg.api.sunnypilot.ai') HANDLER_THREADS = int(os.getenv('HANDLER_THREADS', "4")) @@ -107,10 +109,13 @@ def ws_recv(ws: WebSocket, end_event: threading.Event) -> None: except WebSocketTimeoutException: ns_since_last_ping = int(time.monotonic() * 1e9) - last_ping if ns_since_last_ping > SUNNYLINK_RECONNECT_TIMEOUT_S * 1e9: - cloudlog.exception("sunnylinkd.ws_recv.timeout") + cloudlog.warning("sunnylinkd.ws_recv.timeout") end_event.set() - except Exception: - cloudlog.exception("sunnylinkd.ws_recv.exception") + except Exception as e: + if isinstance(e, WebSocketConnectionClosedException): + cloudlog.warning(f"sunnylinkd.ws_recv.{type(e).__name__}") + else: + cloudlog.exception("sunnylinkd.ws_recv.exception") end_event.set() @@ -137,11 +142,15 @@ def ws_queue(end_event: threading.Event) -> None: sunnylink_api.resume_queued(timeout=29) resume_requested = True tries = 0 - except Exception: - cloudlog.exception("sunnylinkd.ws_queue.resume_queued.exception") + except Exception as e: + if isinstance(e, (ConnectionError, TimeoutError)): + cloudlog.warning(f"sunnylinkd.ws_queue.resume_queued.{type(e).__name__}") + else: + cloudlog.exception("sunnylinkd.ws_queue.resume_queued.exception") + resume_requested = False tries += 1 - time.sleep(backoff(tries)) # Wait for the backoff time before the next attempt + time.sleep(backoff(tries)) if end_event.is_set(): cloudlog.debug("end_event is set, exiting ws_queue thread") @@ -171,16 +180,22 @@ def getParamsAllKeys() -> list[str]: @dispatcher.add_method def getParams(params_keys: list[str], compression: bool = False) -> str | dict[str, str]: + params = Params() + try: - params = Params() - params_dict: dict[str, bytes] = {key: params.get(key) or b'' for key in params_keys} + param_keys_validated = [key for key in params_keys if key in getParamsAllKeys()] + params_dict: dict[str, list[dict[str, str | bool | int ]]] = {"params": [ + { + "key": key, + "value": base64.b64encode(gzip.compress(get_param_as_byte(key)) if compression else get_param_as_byte(key)).decode('utf-8'), + "type": int(params.get_type(key).value), + "is_compressed": compression + } for key in param_keys_validated + ]} - # Compress the values before encoding to base64 as output from params.get is bytes and same for compression - if compression: - params_dict = {key: gzip.compress(value) for key, value in params_dict.items()} - - # Last step is to encode the values to base64 and decode to utf-8 for JSON serialization - return {key: base64.b64encode(value).decode('utf-8') for key, value in params_dict.items()} + response = {str(param.get('key')): str(param.get('value')) for param in params_dict.get("params", [])} + response |= {"params": json.dumps(params_dict.get("params", []))} # Upcoming for settings v1 + return response except Exception as e: cloudlog.exception("sunnylinkd.getParams.exception", e) @@ -252,14 +267,19 @@ def main(exit_event: threading.Event = None): handle_long_poll(ws, exit_event) except (KeyboardInterrupt, SystemExit): break - except (ConnectionError, TimeoutError, WebSocketException): + except Exception as e: conn_retries += 1 params.remove("LastSunnylinkPingTime") - except Exception: - cloudlog.exception("sunnylinkd.main.exception") - conn_retries += 1 - params.remove("LastSunnylinkPingTime") + if isinstance(e, (ConnectionError, TimeoutError, WebSocketException)): + cloudlog.warning(f"sunnylinkd.main.{type(e).__name__}") + elif isinstance(e, OSError): + name = errno.errorcode.get(e.errno or -1, "UNKNOWN") + msg = f"sunnylinkd.main.OSError.{name} ({e.errno})" + is_expected_error = e.errno in (errno.ENETDOWN, errno.ENETRESET, errno.ENETUNREACH) + cloudlog.warning(msg) if is_expected_error else cloudlog.exception(msg) + else: + cloudlog.exception("sunnylinkd.main.exception") time.sleep(backoff(conn_retries)) diff --git a/sunnypilot/sunnylink/backups/manager.py b/sunnypilot/sunnylink/backups/manager.py index f98088a1fb..315300c73c 100644 --- a/sunnypilot/sunnylink/backups/manager.py +++ b/sunnypilot/sunnylink/backups/manager.py @@ -20,6 +20,7 @@ from openpilot.system.version import get_version from cereal import messaging, custom from sunnypilot.sunnylink.api import SunnylinkApi from sunnypilot.sunnylink.backups.utils import decrypt_compressed_data, encrypt_compress_data, SnakeCaseEncoder +from sunnypilot.sunnylink.utils import get_param_as_byte class OperationType(Enum): @@ -74,7 +75,7 @@ class BackupManagerSP: config_data = {} params_to_backup = [k.decode('utf-8') for k in self.params.all_keys(ParamKeyFlag.BACKUP)] for param in params_to_backup: - value = str(self.params.get(param)).encode('utf-8') + value = get_param_as_byte(param) if value is not None: config_data[param] = base64.b64encode(value).decode('utf-8') return config_data @@ -113,6 +114,7 @@ class BackupManagerSP: payload = json.loads(json.dumps(backup_info.to_dict(), cls=SnakeCaseEncoder)) self._update_progress(75.0, OperationType.BACKUP) + cloudlog.debug(f"Uploading backup with payload: {json.dumps(payload)}") # Upload to sunnylink result = self.api.api_get( f"backup/{self.device_id}", @@ -124,9 +126,11 @@ class BackupManagerSP: if result: self.backup_status = custom.BackupManagerSP.Status.completed self._update_progress(100.0, OperationType.BACKUP) + cloudlog.info("Backup successfully created and uploaded") else: self.backup_status = custom.BackupManagerSP.Status.failed self.last_error = "Failed to upload backup" + cloudlog.error(result) self._report_status() return bool(self.backup_status == custom.BackupManagerSP.Status.completed) @@ -264,8 +268,8 @@ class BackupManagerSP: # Check for backup command if self.params.get_bool("BackupManager_CreateBackup"): try: - await self.create_backup() - reset_progress = True + if await self.create_backup(): + reset_progress = True finally: self.params.remove("BackupManager_CreateBackup") diff --git a/sunnypilot/sunnylink/utils.py b/sunnypilot/sunnylink/utils.py index 35714eafe3..569afd26b6 100644 --- a/sunnypilot/sunnylink/utils.py +++ b/sunnypilot/sunnylink/utils.py @@ -1,5 +1,6 @@ +import json from sunnypilot.sunnylink.api import SunnylinkApi, UNREGISTERED_SUNNYLINK_DONGLE_ID -from openpilot.common.params import Params +from openpilot.common.params import Params, ParamKeyType from openpilot.system.version import is_prebuilt @@ -55,3 +56,15 @@ def get_api_token(): sunnylink_api = SunnylinkApi(sunnylink_dongle_id) token = sunnylink_api.get_token() print(f"API Token: {token}") + + +def get_param_as_byte(param_name: str) -> bytes: + params = Params() + param = params.get(param_name) + param_type = params.get_type(param_name) + + if param_type == ParamKeyType.BYTES: + return bytes(param) + elif param_type == ParamKeyType.JSON: + return json.dumps(param).encode('utf-8') + return str(param).encode('utf-8')