diff --git a/cereal/messaging/__init__.py b/cereal/messaging/__init__.py index 0ad846f0..275bcbfb 100644 --- a/cereal/messaging/__init__.py +++ b/cereal/messaging/__init__.py @@ -197,6 +197,8 @@ class SubMaster: self.data[s] = getattr(data.as_reader(), s) self.freq_tracker[s] = FrequencyTracker(SERVICE_LIST[s].frequency, self.update_freq, s == poll) + # FrogPilot variables + def __getitem__(self, s: str) -> capnp.lib.capnp._DynamicStructReader: return self.data[s] @@ -248,6 +250,8 @@ class SubMaster: def all_checks(self, service_list: Optional[List[str]] = None) -> bool: return self.all_alive(service_list) and self.all_freq_ok(service_list) and self.all_valid(service_list) + # FrogPilot variables + class PubMaster: def __init__(self, services: List[str]): @@ -269,3 +273,5 @@ class PubMaster: def all_readers_updated(self, s: str) -> bool: return self.sock[s].all_readers_updated() # type: ignore + + # FrogPilot variables diff --git a/cereal/services.py b/cereal/services.py index e7350ace..ea99e8c5 100755 --- a/cereal/services.py +++ b/cereal/services.py @@ -102,6 +102,8 @@ _services: dict[str, tuple] = { "customReservedRawData0": (True, 0.), "customReservedRawData1": (True, 0.), "customReservedRawData2": (True, 0.), + + # FrogPilot variables } SERVICE_LIST = {name: Service(*vals) for idx, (name, vals) in enumerate(_services.items())} diff --git a/common/constants.py b/common/constants.py index 7ca425c4..1653076b 100644 --- a/common/constants.py +++ b/common/constants.py @@ -19,5 +19,7 @@ class CV: # Mass LB_TO_KG = 0.453592 + # FrogPilot variables + ACCELERATION_DUE_TO_GRAVITY = 9.81 # m/s^2 diff --git a/common/params.cc b/common/params.cc index 6af00fe9..7ce03158 100644 --- a/common/params.cc +++ b/common/params.cc @@ -94,6 +94,8 @@ private: Params::Params(const std::string &path) { params_prefix = "/" + util::getenv("OPENPILOT_PREFIX", "d"); params_path = ensure_params_path(params_prefix, path); + + // FrogPilot variables } Params::~Params() { @@ -169,6 +171,9 @@ int Params::put(const char* key, const char* value, size_t value_size) { int Params::remove(const std::string &key) { FileLock file_lock(params_path + "/.lock"); int result = unlink(getParamPath(key).c_str()); + + // FrogPilot variables + if (result != 0) { return result; } @@ -215,6 +220,8 @@ void Params::clearAll(ParamKeyFlag key_flag) { auto it = keys.find(de->d_name); if (it == keys.end() || (it->second.flags & key_flag)) { unlink(getParamPath(de->d_name).c_str()); + + // FrogPilot variables } } } @@ -240,3 +247,5 @@ void Params::asyncWriteThread() { put(p.first, p.second); } } + +// FrogPilot variables diff --git a/common/params.h b/common/params.h index 8169063a..d6b13030 100644 --- a/common/params.h +++ b/common/params.h @@ -35,6 +35,8 @@ struct ParamKeyAttributes { uint32_t flags; ParamKeyType type; std::optional default_value = std::nullopt; + + // FrogPilot variables }; class Params { @@ -78,6 +80,8 @@ public: putNonBlocking(key, val ? "1" : "0"); } + // FrogPilot variables + private: void asyncWriteThread(); @@ -87,4 +91,6 @@ private: // for nonblocking write std::future future; SafeQueue> queue; + + // FrogPilot variables }; diff --git a/common/params_keys.h b/common/params_keys.h index b71f6298..e7979f51 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -131,4 +131,6 @@ inline static std::unordered_map keys = { {"UptimeOffroad", {PERSISTENT, FLOAT, "0.0"}}, {"UptimeOnroad", {PERSISTENT, FLOAT, "0.0"}}, {"Version", {PERSISTENT, STRING}}, + + // FrogPilot variables }; diff --git a/common/params_pyx.pyx b/common/params_pyx.pyx index 93c550f2..5e562b2b 100644 --- a/common/params_pyx.pyx +++ b/common/params_pyx.pyx @@ -20,6 +20,8 @@ cdef extern from "common/params.h": CLEAR_ON_IGNITION_ON ALL + # FrogPilot variables + cpdef enum ParamKeyType: STRING BOOL @@ -45,6 +47,8 @@ cdef extern from "common/params.h": void clearAll(ParamKeyFlag) vector[string] allKeys() + # FrogPilot variables + PYTHON_2_CPP = { (str, STRING): lambda v: v, (builtins.bool, BOOL): lambda v: "1" if v else "0", @@ -75,12 +79,19 @@ cdef class Params: cdef c_Params* p cdef str d + # FrogPilot variables + def __cinit__(self, d=""): cdef string path = d.encode() + + # FrogPilot variables + with nogil: self.p = new c_Params(path) self.d = d + # FrogPilot variables + def __reduce__(self): return (type(self), (self.d,)) @@ -194,3 +205,5 @@ cdef class Params: cdef string k = self.check_key(key) cdef ParamKeyType t = self.p.getKeyType(k) return self._cpp2python(t, value, None, key) + + # FrogPilot variables diff --git a/common/util.h b/common/util.h index f46db4d9..7bccadb8 100644 --- a/common/util.h +++ b/common/util.h @@ -37,6 +37,8 @@ const double MS_TO_MPH = MS_TO_KPH * KM_TO_MILE; const double METER_TO_MILE = KM_TO_MILE / 1000.0; const double METER_TO_FOOT = 3.28084; +// FrogPilot variables + #define ALIGNED_SIZE(x, align) (((x) + (align)-1) & ~((align)-1)) namespace util { diff --git a/frogpilot/common/frogpilot_functions.py b/frogpilot/common/frogpilot_functions.py new file mode 100644 index 00000000..e5fc603f --- /dev/null +++ b/frogpilot/common/frogpilot_functions.py @@ -0,0 +1,26 @@ +#!/usr/bin/env python3 +import threading +import time + +from openpilot.common.time_helpers import system_time_valid +from openpilot.system.hardware import HARDWARE + + +def frogpilot_boot_functions(): + def boot_thread(): + while not system_time_valid(): + print("Waiting for system time to become valid...") + time.sleep(1) + + threading.Thread(target=boot_thread, daemon=True).start() + + +def install_frogpilot(): + paths = [ + ] + for path in paths: + path.mkdir(parents=True, exist_ok=True) + + +def uninstall_frogpilot(): + HARDWARE.uninstall() diff --git a/frogpilot/frogpilot_process.py b/frogpilot/frogpilot_process.py new file mode 100644 index 00000000..240d2ef9 --- /dev/null +++ b/frogpilot/frogpilot_process.py @@ -0,0 +1,82 @@ +#!/usr/bin/env python3 +import datetime +import time + +from cereal import messaging +from openpilot.common.params import Params +from openpilot.common.realtime import DT_MDL, Priority, Ratekeeper, config_realtime_process +from openpilot.common.time_helpers import system_time_valid + +ASSET_CHECK_RATE = (1 / DT_MDL) + +def check_assets(): + +def transition_offroad(time_validated, sm, params): + +def transition_onroad(): + +def update_checks(now, params, boot_run=False): + while not (is_url_pingable("https://github.com") or is_url_pingable("https://gitlab.com")): + time.sleep(60) + + time.sleep(1) + +def frogpilot_thread(): + rate_keeper = Ratekeeper(1 / DT_MDL, None) + + config_realtime_process(5, Priority.CTRL_LOW) + + sm = messaging.SubMaster(["carControl", "carState", "controlsState", "deviceState", "driverMonitoringState", + "gpsLocation", "gpsLocationExternal", "liveParameters", "managerState", "modelV2", + "onroadEvents", "pandaStates", "radarState", "selfdriveState"], + poll="modelV2") + + params = Params() + + run_update_checks = False + started_previously = False + time_validated = False + + while True: + sm.update() + + now = datetime.datetime.now(datetime.timezone.utc) + + started = sm["deviceState"].started + + if not started and started_previously: + transition_offroad(time_validated, sm, params) + + run_update_checks = True + elif started and not started_previously: + transition_onroad() + + if started and sm.updated["modelV2"]: + elif not started: + + started_previously = started + + if rate_keeper.frame % ASSET_CHECK_RATE == 0: + check_assets() + + run_update_checks |= now.second == 0 and (now.minute % 60 == 0) + run_update_checks &= time_validated + + if run_update_checks: + thread_manager.run_with_lock(update_checks, (now, params)) + + run_update_checks = False + elif not time_validated: + time_validated = system_time_valid() + if not time_validated: + continue + + thread_manager.run_with_lock(update_checks, (now, params, True)) + + rate_keeper.keep_time() + +def main(): + frogpilot_thread() + +if __name__ == "__main__": + main() diff --git a/frogpilot/system/environment_variables b/frogpilot/system/environment_variables new file mode 100755 index 00000000..088ed20a Binary files /dev/null and b/frogpilot/system/environment_variables differ diff --git a/frogpilot/ui/frogpilot_ui.cc b/frogpilot/ui/frogpilot_ui.cc new file mode 100644 index 00000000..4a075eb2 --- /dev/null +++ b/frogpilot/ui/frogpilot_ui.cc @@ -0,0 +1,35 @@ +#include "frogpilot/ui/frogpilot_ui.h" + +static void update_state(FrogPilotUIState *fs) { + FrogPilotUIScene &frogpilot_scene = fs->frogpilot_scene; + + SubMaster &fpsm = *(fs->sm); + fpsm.update(0); + + if (fpsm.updated("deviceState")) { + const cereal::DeviceState::Reader &deviceState = fpsm["deviceState"].getDeviceState(); + } + if (fpsm.updated("selfdriveState")) { + const cereal::SelfdriveState::Reader &selfdriveState = fpsm["selfdriveState"].getSelfdriveState(); + frogpilot_scene.enabled = selfdriveState.getEnabled(); + } +} + +FrogPilotUIState::FrogPilotUIState(QObject *parent) : QObject(parent) { + sm = std::make_unique>({ + "carControl", "deviceState", + "liveDelay", + "liveParameters", "liveTorqueParameters", "liveTracks", "selfdriveState" + }); + + wifi = new WifiManager(this); +} + +FrogPilotUIState *frogpilotUIState() { + static FrogPilotUIState frogpilot_ui_state; + return &frogpilot_ui_state; +} + +void FrogPilotUIState::update() { + update_state(this); +} diff --git a/frogpilot/ui/frogpilot_ui.h b/frogpilot/ui/frogpilot_ui.h new file mode 100644 index 00000000..a784f33d --- /dev/null +++ b/frogpilot/ui/frogpilot_ui.h @@ -0,0 +1,36 @@ +#pragma once + +#include "cereal/messaging/messaging.h" +#include "selfdrive/ui/qt/network/wifi_manager.h" + +#include "frogpilot/ui/qt/widgets/frogpilot_controls.h" + +struct FrogPilotUIScene { + bool enabled; + bool parked; + bool reverse; + bool standstill; + + int started_timer; +}; + +class FrogPilotUIState : public QObject { + Q_OBJECT + +public: + explicit FrogPilotUIState(QObject *parent = nullptr); + + void update(); + + std::unique_ptr sm; + + FrogPilotUIScene frogpilot_scene; + + Params params; + + WifiManager *wifi; + +signals: +}; + +FrogPilotUIState *frogpilotUIState(); diff --git a/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc b/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc new file mode 100644 index 00000000..344211a3 --- /dev/null +++ b/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc @@ -0,0 +1,46 @@ +#include "frogpilot/ui/qt/onroad/frogpilot_annotated_camera.h" + +FrogPilotAnnotatedCameraWidget::FrogPilotAnnotatedCameraWidget(QWidget *parent) : QWidget(parent) { + QSize iconSize(img_size / 4, img_size / 4); +} + +void FrogPilotAnnotatedCameraWidget::showEvent(QShowEvent *event) { +} + +void FrogPilotAnnotatedCameraWidget::updateState(const UIState &s, const FrogPilotUIState &fs) { + const UIScene &scene = s.scene; + + const SubMaster &sm = *(s.sm); + const SubMaster &fpsm = *(fs.sm); + + const cereal::CarState::Reader &carState = sm["carState"].getCarState(); + const cereal::ModelDataV2::Reader &modelV2 = sm["modelV2"].getModelV2(); + const cereal::SelfdriveState::Reader &selfdriveState = sm["selfdriveState"].getSelfdriveState(); + + if (scene.is_metric || frogpilot_toggles.value("use_si_metrics").toBool()) { + leadDistanceUnit = tr(" meters"); + leadSpeedUnit = frogpilot_toggles.value("use_si_metrics").toBool() ? tr(" m/s") : tr(" km/h"); + speedUnit = scene.is_metric ? tr("km/h") : tr("mph"); + + distanceConversion = 1.0f; + speedConversion = scene.is_metric ? MS_TO_KPH : MS_TO_MPH; + speedConversionMetrics = frogpilot_toggles.value("use_si_metrics").toBool() ? 1.0f : MS_TO_KPH; + } else { + leadDistanceUnit = tr(" feet"); + leadSpeedUnit = tr(" mph"); + speedUnit = tr("mph"); + + distanceConversion = METER_TO_FOOT; + speedConversion = MS_TO_MPH; + speedConversionMetrics = MS_TO_MPH; + } + + hideBottomIcons = selfdriveState.getAlertSize() != cereal::SelfdriveState::AlertSize::NONE; +} + +void FrogPilotAnnotatedCameraWidget::mousePressEvent(QMouseEvent *mouseEvent) { + mouseEvent->ignore(); +} + +void FrogPilotAnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &p, UIState &s) { +} diff --git a/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.h b/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.h new file mode 100644 index 00000000..527b02cc --- /dev/null +++ b/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.h @@ -0,0 +1,52 @@ +#pragma once + +#include "selfdrive/ui/qt/onroad/buttons.h" +#include "selfdrive/ui/qt/widgets/cameraview.h" + +const int widget_size = img_size + (UI_BORDER_SIZE / 2); + +class FrogPilotAnnotatedCameraWidget : public QWidget { + Q_OBJECT + +public: + explicit FrogPilotAnnotatedCameraWidget(QWidget *parent = 0); + + void mousePressEvent(QMouseEvent *mouseEvent) override; + void paintFrogPilotWidgets(QPainter &p, UIState &s); + void updateState(const UIState &s, const FrogPilotUIState &fs); + + bool hideBottomIcons; + bool isCruiseSet; + bool rightHandDM; + + float speed; + + FrogPilotUIScene frogpilot_scene; + + QColor blueColor(int alpha = 255) { return QColor(0, 0, 255, alpha); } + QColor purpleColor(int alpha = 255) { return QColor(128, 0, 128, alpha); } + QColor whiteColor(int alpha = 255) { return QColor(255, 255, 255, alpha); } + + QPoint dmIconPosition; + QPoint experimentalButtonPosition; + + QRect setSpeedRect; + + QSize defaultSize; + +protected: + void showEvent(QShowEvent *event) override; + +private: + float distanceConversion; + float setSpeed; + float speedConversion; + float speedConversionMetrics; + + QColor blackColor(int alpha = 255) { return QColor(0, 0, 0, alpha); } + QColor redColor(int alpha = 255) { return QColor(201, 34, 49, alpha); } + + QString leadDistanceUnit; + QString leadSpeedUnit; + QString speedUnit; +}; diff --git a/frogpilot/ui/qt/onroad/frogpilot_buttons.cc b/frogpilot/ui/qt/onroad/frogpilot_buttons.cc new file mode 100644 index 00000000..dcf07008 --- /dev/null +++ b/frogpilot/ui/qt/onroad/frogpilot_buttons.cc @@ -0,0 +1 @@ +#include "frogpilot/ui/qt/onroad/frogpilot_buttons.h" diff --git a/frogpilot/ui/qt/onroad/frogpilot_buttons.h b/frogpilot/ui/qt/onroad/frogpilot_buttons.h new file mode 100644 index 00000000..d427996f --- /dev/null +++ b/frogpilot/ui/qt/onroad/frogpilot_buttons.h @@ -0,0 +1,3 @@ +#pragma once + +#include "selfdrive/ui/qt/onroad/buttons.h" diff --git a/frogpilot/ui/qt/onroad/frogpilot_onroad.cc b/frogpilot/ui/qt/onroad/frogpilot_onroad.cc new file mode 100644 index 00000000..e9e0ea85 --- /dev/null +++ b/frogpilot/ui/qt/onroad/frogpilot_onroad.cc @@ -0,0 +1,26 @@ +#include "frogpilot/ui/qt/onroad/frogpilot_onroad.h" + +FrogPilotOnroadWindow::FrogPilotOnroadWindow(QWidget *parent) : QWidget(parent) { +} + +void FrogPilotOnroadWindow::resizeEvent(QResizeEvent *event) { + rect = QWidget::rect(); + marginRegion = QRegion(rect) - QRegion(rect.marginsRemoved(QMargins(UI_BORDER_SIZE, UI_BORDER_SIZE, UI_BORDER_SIZE, UI_BORDER_SIZE))); +} + +void FrogPilotOnroadWindow::updateState(const UIState &s, const FrogPilotUIState &fs) { + const SubMaster &sm = *(s.sm); + const SubMaster &fpsm = *(fs.sm); + + const cereal::CarState::Reader &carState = sm["carState"].getCarState(); + const cereal::CarControl::Reader &carControl = fpsm["carControl"].getCarControl(); + + update(); +} + +void FrogPilotOnroadWindow::paintEvent(QPaintEvent *event) { + QPainter p(this); + + p.setClipRegion(marginRegion); + p.setRenderHints(QPainter::Antialiasing | QPainter::TextAntialiasing); +} diff --git a/frogpilot/ui/qt/onroad/frogpilot_onroad.h b/frogpilot/ui/qt/onroad/frogpilot_onroad.h new file mode 100644 index 00000000..49bb02c4 --- /dev/null +++ b/frogpilot/ui/qt/onroad/frogpilot_onroad.h @@ -0,0 +1,24 @@ +#pragma once + +#include "selfdrive/ui/qt/onroad/annotated_camera.h" + +class FrogPilotOnroadWindow : public QWidget { + Q_OBJECT + +public: + FrogPilotOnroadWindow(QWidget* parent = 0); + + void updateState(const UIState &s, const FrogPilotUIState &fs); + + FrogPilotUIScene frogpilot_scene; + + QColor bg; + +private: + void paintEvent(QPaintEvent *event); + void resizeEvent(QResizeEvent *event); + + QRect rect; + + QRegion marginRegion; +}; diff --git a/frogpilot/ui/qt/widgets/frogpilot_controls.cc b/frogpilot/ui/qt/widgets/frogpilot_controls.cc new file mode 100644 index 00000000..e35184bf --- /dev/null +++ b/frogpilot/ui/qt/widgets/frogpilot_controls.cc @@ -0,0 +1,94 @@ +#include "selfdrive/ui/ui.h" + +#include "frogpilot/ui/frogpilot_ui.h" + +void clearMovie(QSharedPointer &movie, QWidget *parent) { + if (!movie) { + return; + } + + QObject::disconnect(movie.data(), nullptr, parent, nullptr); + movie->stop(); + movie.reset(); +} + +void loadGif(const QString &gifPath, QSharedPointer &movie, const QSize &size, QWidget *parent) { + if (!parent || gifPath.isEmpty()) { + return; + } + + if (movie && movie->fileName() == gifPath && movie->state() == QMovie::Running) { + if (movie->scaledSize() != size) { + movie->setScaledSize(size); + } + return; + } + + if (!QFileInfo::exists(gifPath)) { + clearMovie(movie, parent); + return; + } + + clearMovie(movie, parent); + + movie = QSharedPointer::create(gifPath); + movie->setCacheMode(QMovie::CacheAll); + movie->setScaledSize(size); + + QPointer safeParent(parent); + QObject::connect(movie.data(), &QMovie::frameChanged, parent, [safeParent]() { + if (safeParent && safeParent->isVisible()) { + safeParent->update(); + } + }, Qt::UniqueConnection); + + movie->start(); +} + +void loadImage(const QString &basePath, QPixmap &pixmap, QSharedPointer &movie, const QSize &size, QWidget *parent) { + if (!parent || basePath.isEmpty()) { + return; + } + + static QHash pixmapCache; + QString cacheKey = basePath + QString("_%1x%2").arg(size.width()).arg(size.height()); + + QString gifPath = basePath + ".gif"; + if (QFileInfo::exists(gifPath)) { + loadGif(gifPath, movie, size, parent); + if (!pixmap.isNull()) { + pixmap = QPixmap(); + } + return; + } + + clearMovie(movie, parent); + + if (pixmapCache.contains(cacheKey)) { + QPixmap &cached = pixmapCache[cacheKey]; + if (pixmap.cacheKey() != cached.cacheKey()) { + pixmap = cached; + parent->update(); + } + return; + } + + QString pngPath = basePath + ".png"; + if (!QFileInfo::exists(pngPath)) { + if (!pixmap.isNull()) { + pixmap = QPixmap(); + parent->update(); + } + return; + } + + QPixmap loadedPixmap(pngPath); + if (!loadedPixmap.isNull()) { + pixmap = loadedPixmap.scaled(size, Qt::KeepAspectRatio, Qt::SmoothTransformation); + pixmapCache.insert(cacheKey, pixmap); + } else { + pixmap = QPixmap(); + } + + parent->update(); +} diff --git a/frogpilot/ui/qt/widgets/frogpilot_controls.h b/frogpilot/ui/qt/widgets/frogpilot_controls.h new file mode 100644 index 00000000..f930059c --- /dev/null +++ b/frogpilot/ui/qt/widgets/frogpilot_controls.h @@ -0,0 +1,19 @@ +#pragma once + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "selfdrive/ui/qt/util.h" +#include "selfdrive/ui/qt/widgets/controls.h" + +void loadGif(const QString &gifPath, QSharedPointer &movie, const QSize &size, QWidget *parent); +void loadImage(const QString &basePath, QPixmap &pixmap, QSharedPointer &movie, const QSize &size, QWidget *parent); diff --git a/launch_chffrplus.sh b/launch_chffrplus.sh index 13545b89..2f605625 100755 --- a/launch_chffrplus.sh +++ b/launch_chffrplus.sh @@ -16,6 +16,8 @@ function agnos_init { sudo chgrp gpu /dev/adsprpc-smd /dev/ion /dev/kgsl-3d0 sudo chmod 660 /dev/adsprpc-smd /dev/ion /dev/kgsl-3d0 + # FrogPilot variables + # Check if AGNOS update is required if [ $(< /VERSION) != "$AGNOS_VERSION" ]; then AGNOS_PY="$DIR/system/hardware/tici/agnos.py" diff --git a/launch_env.sh b/launch_env.sh index e4a6b669..a35bd2b0 100755 --- a/launch_env.sh +++ b/launch_env.sh @@ -20,3 +20,6 @@ if [ -z "$AGNOS_VERSION" ]; then fi export STAGING_ROOT="/data/safe_staging" + +# FrogPilot variables +eval "$(/data/openpilot/frogpilot/system/environment_variables)" diff --git a/opendbc_repo/opendbc/car/body/carstate.py b/opendbc_repo/opendbc/car/body/carstate.py index b346a3df..8bb0adbd 100644 --- a/opendbc_repo/opendbc/car/body/carstate.py +++ b/opendbc_repo/opendbc/car/body/carstate.py @@ -28,6 +28,8 @@ class CarState(CarStateBase): ret.cruiseState.enabled = True ret.cruiseState.available = True + # FrogPilot variables + return ret @staticmethod diff --git a/opendbc_repo/opendbc/car/car_helpers.py b/opendbc_repo/opendbc/car/car_helpers.py index dfda0155..f7b587f3 100644 --- a/opendbc_repo/opendbc/car/car_helpers.py +++ b/opendbc_repo/opendbc/car/car_helpers.py @@ -13,6 +13,8 @@ from opendbc.car.vin import get_vin, is_valid_vin, VIN_UNKNOWN FRAME_FINGERPRINT = 100 # 1s +# FrogPilot variables + def load_interfaces(brand_names): ret = {} @@ -163,6 +165,8 @@ def get_car(can_recv: CanRecvCallable, can_send: CanSendCallable, set_obd_multip CP.fingerprintSource = source CP.fuzzyFingerprint = not exact_match + # FrogPilot variables + return interfaces[CP.carFingerprint](CP) diff --git a/opendbc_repo/opendbc/car/chrysler/carstate.py b/opendbc_repo/opendbc/car/chrysler/carstate.py index 7c8fd505..1225ad3c 100644 --- a/opendbc_repo/opendbc/car/chrysler/carstate.py +++ b/opendbc_repo/opendbc/car/chrysler/carstate.py @@ -27,6 +27,8 @@ class CarState(CarStateBase): # RealFast variables self.button_message = "CRUISE_BUTTONS_ALT" if FPCP.flags & ChryslerFrogPilotFlags.RAM_HD_ALT_BUTTONS else "CRUISE_BUTTONS" + # FrogPilot variables + def update(self, can_parsers) -> structs.CarState: cp = can_parsers[Bus.pt] cp_cam = can_parsers[Bus.cam] @@ -98,7 +100,11 @@ class CarState(CarStateBase): self.lkas_car_model = cp_cam.vl["DAS_6"]["CAR_MODEL"] self.button_counter = cp.vl[self.button_message]["COUNTER"] - ret.buttonEvents = create_button_events(self.distance_button, prev_distance_button, {1: ButtonType.gapAdjustCruise}) + buttonEvents = create_button_events(self.distance_button, prev_distance_button, {1: ButtonType.gapAdjustCruise}) + + # FrogPilot variables + + ret.buttonEvents = buttonEvents return ret diff --git a/opendbc_repo/opendbc/car/ford/carstate.py b/opendbc_repo/opendbc/car/ford/carstate.py index d0d45f70..d3d29404 100644 --- a/opendbc_repo/opendbc/car/ford/carstate.py +++ b/opendbc_repo/opendbc/car/ford/carstate.py @@ -113,6 +113,8 @@ class CarState(CarStateBase): *create_button_events(self.lc_button, prev_lc_button, {1: ButtonType.lkas}), ] + # FrogPilot variables + return ret @staticmethod diff --git a/opendbc_repo/opendbc/car/gm/carcontroller.py b/opendbc_repo/opendbc/car/gm/carcontroller.py index 44f1afb5..fb2d48c0 100644 --- a/opendbc_repo/opendbc/car/gm/carcontroller.py +++ b/opendbc_repo/opendbc/car/gm/carcontroller.py @@ -141,6 +141,8 @@ class CarController(CarControllerBase): at_full_stop = at_full_stop and stopping friction_brake_bus = CanBus.POWERTRAIN + # FrogPilot variables + # GasRegenCmdActive needs to be 1 to avoid cruise faults. It describes the ACC state, not actuation can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, self.apply_gas, idx, CC.enabled, at_full_stop)) can_sends.append(gmcan.create_friction_brake_command(self.packer_ch, friction_brake_bus, self.apply_brake, diff --git a/opendbc_repo/opendbc/car/gm/carstate.py b/opendbc_repo/opendbc/car/gm/carstate.py index d8196005..8972bb54 100644 --- a/opendbc_repo/opendbc/car/gm/carstate.py +++ b/opendbc_repo/opendbc/car/gm/carstate.py @@ -182,6 +182,8 @@ class CarState(CarStateBase): if self.CP.transmissionType == TransmissionType.direct: self.single_pedal_mode = ret.gearShifter == GearShifter.low or pt_cp.vl["EVDriveMode"]["SinglePedalModeActive"] == 1 + # FrogPilot variables + return ret @staticmethod diff --git a/opendbc_repo/opendbc/car/gm/fingerprints.py b/opendbc_repo/opendbc/car/gm/fingerprints.py index 7a879a16..f0fc9248 100644 --- a/opendbc_repo/opendbc/car/gm/fingerprints.py +++ b/opendbc_repo/opendbc/car/gm/fingerprints.py @@ -175,6 +175,7 @@ FINGERPRINTS = { { 190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 208: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 313: 8, 320: 3, 322: 7, 328: 1, 352: 5, 353: 3, 381: 6, 384: 4, 386: 8, 388: 8, 393: 7, 398: 8, 407: 7, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 454: 8, 455: 7, 462: 4, 463: 3, 479: 3, 481: 7, 485: 8, 487: 8, 489: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 503: 1, 508: 8, 510: 8, 532: 6, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 567: 5, 647: 3, 707: 8, 717: 5, 723: 2, 753: 5, 761: 7, 800: 6, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 872: 1, 961: 8, 967: 4, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1011: 6, 1013: 3, 1017: 8, 1019: 2, 1020: 8, 1022: 1, 1105: 6, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1233: 8, 1243: 3, 1249: 8, 1257: 6, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1280: 4, 1300: 8, 1322: 6, 1328: 4, 1417: 8, 1904: 7, 1906: 7, 1907: 7, 1912: 7, 1913: 7, 1914: 7, 1919: 7, 1920: 7 }], + # FrogPilot variables } FW_VERSIONS: dict[str, dict[tuple, list[bytes]]] = { diff --git a/opendbc_repo/opendbc/car/gm/interface.py b/opendbc_repo/opendbc/car/gm/interface.py index 7c57c825..54b87e87 100755 --- a/opendbc_repo/opendbc/car/gm/interface.py +++ b/opendbc_repo/opendbc/car/gm/interface.py @@ -276,4 +276,6 @@ class CarInterface(CarInterfaceBase): if candidate in CC_ONLY_CAR: ret.safetyConfigs[0].safetyParam |= GMSafetyFlags.FLAG_GM_NO_ACC.value + # FrogPilot variables + return ret diff --git a/opendbc_repo/opendbc/car/gm/values.py b/opendbc_repo/opendbc/car/gm/values.py index ca72359c..ef2474ba 100644 --- a/opendbc_repo/opendbc/car/gm/values.py +++ b/opendbc_repo/opendbc/car/gm/values.py @@ -245,6 +245,7 @@ class CAR(Platforms): [GMCarDocs("Chevrolet Trailblazer 2021-22 (NO ACC)")], CHEVROLET_TRAILBLAZER.specs, ) + # FrogPilot variables class CruiseButtons: @@ -350,3 +351,5 @@ CAMERA_ACC_CAR.update(CC_ONLY_CAR) EV_CAR.update(CAR.CHEVROLET_VOLT, CAR.CHEVROLET_VOLT_2019, CAR.CHEVROLET_BOLT_EUV) DBC = CAR.create_dbc_map() + +# FrogPilot variables diff --git a/opendbc_repo/opendbc/car/honda/carstate.py b/opendbc_repo/opendbc/car/honda/carstate.py index 33449196..fe444a0c 100644 --- a/opendbc_repo/opendbc/car/honda/carstate.py +++ b/opendbc_repo/opendbc/car/honda/carstate.py @@ -219,6 +219,8 @@ class CarState(CarStateBase): *create_button_events(self.cruise_setting, prev_cruise_setting, SETTINGS_BUTTONS_DICT), ] + # FrogPilot variables + return ret def get_can_parsers(self, CP): diff --git a/opendbc_repo/opendbc/car/honda/interface.py b/opendbc_repo/opendbc/car/honda/interface.py old mode 100755 new mode 100644 diff --git a/opendbc_repo/opendbc/car/hyundai/carstate.py b/opendbc_repo/opendbc/car/hyundai/carstate.py index 60978970..ad68850e 100644 --- a/opendbc_repo/opendbc/car/hyundai/carstate.py +++ b/opendbc_repo/opendbc/car/hyundai/carstate.py @@ -202,6 +202,8 @@ class CarState(CarStateBase): self.low_speed_alert = False ret.lowSpeedAlert = self.low_speed_alert + # FrogPilot variables + return ret def update_canfd(self, can_parsers) -> structs.CarState: @@ -293,6 +295,8 @@ class CarState(CarStateBase): ret.blockPcmEnable = not self.recent_button_interaction() + # FrogPilot variables + return ret def get_can_parsers_canfd(self, CP): diff --git a/opendbc_repo/opendbc/car/hyundai/values.py b/opendbc_repo/opendbc/car/hyundai/values.py index e239298a..178b16ad 100644 --- a/opendbc_repo/opendbc/car/hyundai/values.py +++ b/opendbc_repo/opendbc/car/hyundai/values.py @@ -68,6 +68,9 @@ class HyundaiSafetyFlags(IntFlag): ALT_LIMITS_2 = 512 +# FrogPilot variables + + class HyundaiFlags(IntFlag): # Dynamic Flags diff --git a/opendbc_repo/opendbc/car/interfaces.py b/opendbc_repo/opendbc/car/interfaces.py index 8328062f..268de6cc 100644 --- a/opendbc_repo/opendbc/car/interfaces.py +++ b/opendbc_repo/opendbc/car/interfaces.py @@ -20,6 +20,8 @@ from opendbc.can import CANParser GearShifter = structs.CarState.GearShifter ButtonType = structs.CarState.ButtonEvent.Type +# FrogPilot variables + V_CRUISE_MAX = 145 MAX_CTRL_SPEED = (V_CRUISE_MAX + 4) * CV.KPH_TO_MS ACCEL_MAX = 2.0 @@ -108,6 +110,8 @@ class CarInterfaceBase(ABC): dbc_names = {bus: cp.dbc_name for bus, cp in self.can_parsers.items()} self.CC: CarControllerBase = self.CarController(dbc_names, CP) + # FrogPilot variables + def apply(self, c: structs.CarControl, now_nanos: int | None = None) -> tuple[structs.CarControl.Actuators, list[CanData]]: if now_nanos is None: now_nanos = int(time.monotonic() * 1e9) @@ -149,8 +153,12 @@ class CarInterfaceBase(ABC): ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, ret.tireStiffnessFactor) + # FrogPilot variables + return ret + # FrogPilot variables + @staticmethod @abstractmethod def _get_params(ret: structs.CarParams, candidate, fingerprint: dict[int, dict[int, int]], @@ -259,6 +267,8 @@ class CarInterfaceBase(ABC): # save for next iteration self.CS.out = ret + # FrogPilot variables + return ret @@ -287,6 +297,9 @@ class CarStateBase(ABC): K = get_kalman_gain(DT_CTRL, np.array(A), np.array(C), np.array(Q), R) self.v_ego_kf = KF1D(x0=x0, A=A, C=C[0], K=K) + # FrogPilot variables + self.CC: structs.CarControl = structs.CarControl.new_message() + @abstractmethod def update(self, can_parsers) -> structs.CarState: pass diff --git a/opendbc_repo/opendbc/car/mazda/carstate.py b/opendbc_repo/opendbc/car/mazda/carstate.py index 074a13c2..6b7bdd25 100644 --- a/opendbc_repo/opendbc/car/mazda/carstate.py +++ b/opendbc_repo/opendbc/car/mazda/carstate.py @@ -115,6 +115,8 @@ class CarState(CarStateBase): # TODO: add button types for inc and dec ret.buttonEvents = create_button_events(self.distance_button, prev_distance_button, {1: ButtonType.gapAdjustCruise}) + # FrogPilot variables + return ret @staticmethod diff --git a/opendbc_repo/opendbc/car/mazda/interface.py b/opendbc_repo/opendbc/car/mazda/interface.py index 814846e8..e59497ea 100755 --- a/opendbc_repo/opendbc/car/mazda/interface.py +++ b/opendbc_repo/opendbc/car/mazda/interface.py @@ -29,4 +29,6 @@ class CarInterface(CarInterfaceBase): ret.centerToFront = ret.wheelbase * 0.41 + # FrogPilot variables + return ret diff --git a/opendbc_repo/opendbc/car/nissan/carstate.py b/opendbc_repo/opendbc/car/nissan/carstate.py index 8f38590a..c0533765 100644 --- a/opendbc_repo/opendbc/car/nissan/carstate.py +++ b/opendbc_repo/opendbc/car/nissan/carstate.py @@ -24,6 +24,8 @@ class CarState(CarStateBase): self.distance_button = 0 + # FrogPilot variables + def update(self, can_parsers) -> structs.CarState: cp = can_parsers[Bus.pt] cp_cam = can_parsers[Bus.cam] @@ -126,7 +128,11 @@ class CarState(CarStateBase): self.lkas_hud_msg = copy.copy(cp_adas.vl["PROPILOT_HUD"]) self.lkas_hud_info_msg = copy.copy(cp_adas.vl["PROPILOT_HUD_INFO_MSG"]) - ret.buttonEvents = create_button_events(self.distance_button, prev_distance_button, {1: ButtonType.gapAdjustCruise}) + buttonEvents = create_button_events(self.distance_button, prev_distance_button, {1: ButtonType.gapAdjustCruise}) + + # FrogPilot variables + + ret.buttonEvents = buttonEvents return ret diff --git a/opendbc_repo/opendbc/car/psa/carstate.py b/opendbc_repo/opendbc/car/psa/carstate.py index 81335ef5..bfc88691 100644 --- a/opendbc_repo/opendbc/car/psa/carstate.py +++ b/opendbc_repo/opendbc/car/psa/carstate.py @@ -62,6 +62,9 @@ class CarState(CarStateBase): # lock info ret.doorOpen = any((cp_cam.vl['Dat_BSI']['DRIVER_DOOR'], cp_cam.vl['Dat_BSI']['PASSENGER_DOOR'])) ret.seatbeltUnlatched = cp_cam.vl['RESTRAINTS']['DRIVER_SEATBELT'] != 2 + + # FrogPilot variables + return ret @staticmethod diff --git a/opendbc_repo/opendbc/car/rivian/carstate.py b/opendbc_repo/opendbc/car/rivian/carstate.py index 001d705e..222a55eb 100644 --- a/opendbc_repo/opendbc/car/rivian/carstate.py +++ b/opendbc_repo/opendbc/car/rivian/carstate.py @@ -92,6 +92,8 @@ class CarState(CarStateBase): self.sccm_wheel_touch = copy.copy(cp.vl["SCCM_WheelTouch"]) self.vdm_adas_status = copy.copy(cp.vl["VDM_AdasSts"]) + # FrogPilot variables + return ret @staticmethod diff --git a/opendbc_repo/opendbc/car/subaru/carcontroller.py b/opendbc_repo/opendbc/car/subaru/carcontroller.py index 3b9abf59..29cdd982 100644 --- a/opendbc_repo/opendbc/car/subaru/carcontroller.py +++ b/opendbc_repo/opendbc/car/subaru/carcontroller.py @@ -11,6 +11,8 @@ from opendbc.car.subaru.values import DBC, GLOBAL_ES_ADDR, CanBus, CarController MAX_STEER_RATE = 25 # deg/s MAX_STEER_RATE_FRAMES = 7 # tx control frames needed before torque can be cut +# FrogPilot variables + class CarController(CarControllerBase): def __init__(self, dbc_names, CP): @@ -23,6 +25,8 @@ class CarController(CarControllerBase): self.p = CarControllerParams(CP) self.packer = CANPacker(DBC[CP.carFingerprint][Bus.pt]) + # FrogPilot variables + def update(self, CC, CS, now_nanos): actuators = CC.actuators hud_control = CC.hudControl @@ -57,6 +61,8 @@ class CarController(CarControllerBase): self.apply_torque_last = apply_torque + # FrogPilot variables + # *** longitudinal *** if CC.longActive: @@ -93,6 +99,7 @@ class CarController(CarControllerBase): can_sends.append(subarucan.create_preglobal_es_distance(self.packer, cruise_button, CS.es_distance_msg)) + # FrogPilot variables else: if self.frame % 10 == 0: can_sends.append(subarucan.create_es_dashstatus(self.packer, self.frame // 10, CS.es_dashstatus_msg, CC.enabled, @@ -105,6 +112,8 @@ class CarController(CarControllerBase): if self.CP.flags & SubaruFlags.SEND_INFOTAINMENT: can_sends.append(subarucan.create_es_infotainment(self.packer, self.frame // 10, CS.es_infotainment_msg, hud_control.visualAlert)) + # FrogPilot variables + if self.CP.openpilotLongitudinalControl: if self.frame % 5 == 0: can_sends.append(subarucan.create_es_status(self.packer, self.frame // 5, CS.es_status_msg, @@ -142,3 +151,5 @@ class CarController(CarControllerBase): self.frame += 1 return new_actuators, can_sends + + # FrogPilot variables diff --git a/opendbc_repo/opendbc/car/subaru/carstate.py b/opendbc_repo/opendbc/car/subaru/carstate.py index 76bc4ff8..ddfc6f60 100644 --- a/opendbc_repo/opendbc/car/subaru/carstate.py +++ b/opendbc_repo/opendbc/car/subaru/carstate.py @@ -123,6 +123,8 @@ class CarState(CarStateBase): if self.CP.flags & SubaruFlags.SEND_INFOTAINMENT: self.es_infotainment_msg = copy.copy(cp_cam.vl["ES_Infotainment"]) + # FrogPilot variables + return ret @staticmethod diff --git a/opendbc_repo/opendbc/car/subaru/subarucan.py b/opendbc_repo/opendbc/car/subaru/subarucan.py index 64cab421..a8a5458b 100644 --- a/opendbc_repo/opendbc/car/subaru/subarucan.py +++ b/opendbc_repo/opendbc/car/subaru/subarucan.py @@ -334,3 +334,6 @@ def subaru_checksum(address: int, sig, d: bytearray) -> int: for i in range(1, len(d)): s += d[i] return s & 0xFF + + +# FrogPilot variables diff --git a/opendbc_repo/opendbc/car/subaru/values.py b/opendbc_repo/opendbc/car/subaru/values.py index a702d394..54d43da8 100644 --- a/opendbc_repo/opendbc/car/subaru/values.py +++ b/opendbc_repo/opendbc/car/subaru/values.py @@ -26,6 +26,7 @@ class CarControllerParams: elif CP.carFingerprint == CAR.SUBARU_IMPREZA_2020: self.STEER_DELTA_UP = 35 self.STEER_MAX = 1439 + # FrogPilot variables else: self.STEER_MAX = 2047 diff --git a/opendbc_repo/opendbc/car/tesla/carstate.py b/opendbc_repo/opendbc/car/tesla/carstate.py index fa16116b..5f73e4c7 100644 --- a/opendbc_repo/opendbc/car/tesla/carstate.py +++ b/opendbc_repo/opendbc/car/tesla/carstate.py @@ -117,6 +117,8 @@ class CarState(CarStateBase): # Messages needed by carcontroller self.das_control = copy.copy(cp_ap_party.vl["DAS_control"]) + # FrogPilot variables + return ret @staticmethod diff --git a/opendbc_repo/opendbc/car/torque_data/params.toml b/opendbc_repo/opendbc/car/torque_data/params.toml index fde2ca22..d2cfd4cc 100644 --- a/opendbc_repo/opendbc/car/torque_data/params.toml +++ b/opendbc_repo/opendbc/car/torque_data/params.toml @@ -80,3 +80,5 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"] "VOLKSWAGEN_JETTA_MK7" = [1.2271623034089392, 1.216955117387, 0.19437384688370712] "VOLKSWAGEN_PASSAT_MK8" = [1.3432120736752917, 1.7087275587362314, 0.19444383787326647] "VOLKSWAGEN_TIGUAN_MK2" = [0.9711965500094828, 1.0001565939459098, 0.1465626137072916] + +# FrogPilot variables diff --git a/opendbc_repo/opendbc/car/torque_data/substitute.toml b/opendbc_repo/opendbc/car/torque_data/substitute.toml index 238dd7c4..e308be89 100644 --- a/opendbc_repo/opendbc/car/torque_data/substitute.toml +++ b/opendbc_repo/opendbc/car/torque_data/substitute.toml @@ -95,3 +95,5 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"] "CHEVROLET_EQUINOX_CC" = "CHEVROLET_EQUINOX" "CHEVROLET_SUBURBAN_CC" = "CHEVROLET_SILVERADO" "CHEVROLET_TRAILBLAZER_CC" = "CHEVROLET_TRAILBLAZER" + +# FrogPilot variables diff --git a/opendbc_repo/opendbc/car/toyota/carcontroller.py b/opendbc_repo/opendbc/car/toyota/carcontroller.py index 627a2e1b..65315fc3 100644 --- a/opendbc_repo/opendbc/car/toyota/carcontroller.py +++ b/opendbc_repo/opendbc/car/toyota/carcontroller.py @@ -34,6 +34,8 @@ MAX_STEER_RATE_FRAMES = 18 # tx control frames needed before torque can be cut # EPS allows user torque above threshold for 50 frames before permanently faulting MAX_USER_TORQUE = 500 +# FrogPilot variables + def get_long_tune(CP, params): if CP.carFingerprint in TSS2_CAR: @@ -78,6 +80,8 @@ class CarController(CarControllerBase): self.secoc_acc_message_counter = 0 self.secoc_prev_reset_counter = 0 + # FrogPilot variables + def update(self, CC, CS, now_nanos): actuators = CC.actuators stopping = actuators.longControlState == LongCtrlState.stopping @@ -319,4 +323,7 @@ class CarController(CarControllerBase): new_actuators.accel = self.accel self.frame += 1 + + # FrogPilot variables + return new_actuators, can_sends diff --git a/opendbc_repo/opendbc/car/toyota/carstate.py b/opendbc_repo/opendbc/car/toyota/carstate.py index 3fb22740..43c2da73 100644 --- a/opendbc_repo/opendbc/car/toyota/carstate.py +++ b/opendbc_repo/opendbc/car/toyota/carstate.py @@ -53,6 +53,8 @@ class CarState(CarStateBase): self.gvc = 0.0 self.secoc_synchronization = None + # FrogPilot variables + def update(self, can_parsers) -> structs.CarState: cp = can_parsers[Bus.pt] cp_cam = can_parsers[Bus.cam] @@ -198,7 +200,10 @@ class CarState(CarStateBase): buttonEvents += create_button_events(self.distance_button, prev_distance_button, {1: ButtonType.gapAdjustCruise}) + # FrogPilot variables + ret.buttonEvents = buttonEvents + return ret @staticmethod diff --git a/opendbc_repo/opendbc/car/toyota/values.py b/opendbc_repo/opendbc/car/toyota/values.py index 0b85d386..9db6457a 100644 --- a/opendbc_repo/opendbc/car/toyota/values.py +++ b/opendbc_repo/opendbc/car/toyota/values.py @@ -80,6 +80,9 @@ class ToyotaFlags(IntFlag): SNG_WITHOUT_DSU_DEPRECATED = 512 +# FrogPilot variables + + def dbc_dict(pt, radar): return {Bus.pt: pt, Bus.radar: radar} diff --git a/opendbc_repo/opendbc/car/volkswagen/carstate.py b/opendbc_repo/opendbc/car/volkswagen/carstate.py index a43a1665..e1228903 100644 --- a/opendbc_repo/opendbc/car/volkswagen/carstate.py +++ b/opendbc_repo/opendbc/car/volkswagen/carstate.py @@ -137,6 +137,9 @@ class CarState(CarStateBase): ret.lowSpeedAlert = self.update_low_speed_alert(ret.vEgo) self.frame += 1 + + # FrogPilot variables + return ret def update_pq(self, pt_cp, cam_cp, ext_cp) -> structs.CarState: @@ -228,6 +231,9 @@ class CarState(CarStateBase): ret.lowSpeedAlert = self.update_low_speed_alert(ret.vEgo) self.frame += 1 + + # FrogPilot variables + return ret def update_mlb(self, pt_cp, cam_cp, ext_cp) -> structs.CarState: diff --git a/opendbc_repo/opendbc/safety/__init__.py b/opendbc_repo/opendbc/safety/__init__.py index 5d4c2464..91063855 100644 --- a/opendbc_repo/opendbc/safety/__init__.py +++ b/opendbc_repo/opendbc/safety/__init__.py @@ -8,3 +8,5 @@ class ALTERNATIVE_EXPERIENCE: DISABLE_STOCK_AEB = 2 RAISE_LONGITUDINAL_LIMITS_TO_ISO_MAX = 8 ALLOW_AEB = 16 + + # FrogPilot variables diff --git a/opendbc_repo/opendbc/safety/declarations.h b/opendbc_repo/opendbc/safety/declarations.h index b83b35fc..18b451b8 100644 --- a/opendbc_repo/opendbc/safety/declarations.h +++ b/opendbc_repo/opendbc/safety/declarations.h @@ -268,6 +268,8 @@ extern bool acc_main_on; // referred to as "ACC off" in ISO 15622:2018 extern int cruise_button_prev; extern bool safety_rx_checks_invalid; +// FrogPilot variables + // for safety modes with torque steering control extern int desired_torque_last; // last desired steer torque extern int rt_torque_last; // last desired torque for real time check @@ -309,6 +311,8 @@ extern bool enable_gas_interceptor; // This flag allows AEB to be commanded from openpilot. #define ALT_EXP_ALLOW_AEB 16 +// FrogPilot variables + extern int alternative_experience; // time since safety mode has been changed diff --git a/opendbc_repo/opendbc/safety/modes/chrysler.h b/opendbc_repo/opendbc/safety/modes/chrysler.h index 70e2a3de..30d9096a 100644 --- a/opendbc_repo/opendbc/safety/modes/chrysler.h +++ b/opendbc_repo/opendbc/safety/modes/chrysler.h @@ -78,6 +78,8 @@ static void chrysler_rx_hook(const CANPacket_t *msg) { if ((msg->bus == das_3_bus) && (msg->addr == chrysler_addrs->DAS_3)) { bool cruise_engaged = GET_BIT(msg, 21U); pcm_cruise_check(cruise_engaged); + + // FrogPilot variables } // TODO: use the same message for both diff --git a/opendbc_repo/opendbc/safety/modes/ford.h b/opendbc_repo/opendbc/safety/modes/ford.h index 4b35cf00..447cc7d1 100644 --- a/opendbc_repo/opendbc/safety/modes/ford.h +++ b/opendbc_repo/opendbc/safety/modes/ford.h @@ -158,6 +158,8 @@ static void ford_rx_hook(const CANPacket_t *msg) { unsigned int cruise_state = msg->data[1] & 0x07U; bool cruise_engaged = (cruise_state == 4U) || (cruise_state == 5U); pcm_cruise_check(cruise_engaged); + + // FrogPilot variables } } } diff --git a/opendbc_repo/opendbc/safety/modes/gm.h b/opendbc_repo/opendbc/safety/modes/gm.h index a4004e42..73806c9d 100644 --- a/opendbc_repo/opendbc/safety/modes/gm.h +++ b/opendbc_repo/opendbc/safety/modes/gm.h @@ -123,6 +123,8 @@ static void gm_rx_hook(const CANPacket_t *msg) { gas_pressed = gas_interceptor > GM_GAS_INTERCEPTOR_THRESHOLD; } } + + // FrogPilot variables } static bool gm_tx_hook(const CANPacket_t *msg) { diff --git a/opendbc_repo/opendbc/safety/modes/hyundai.h b/opendbc_repo/opendbc/safety/modes/hyundai.h index b867f185..70903e7a 100644 --- a/opendbc_repo/opendbc/safety/modes/hyundai.h +++ b/opendbc_repo/opendbc/safety/modes/hyundai.h @@ -52,6 +52,8 @@ const LongitudinalLimits HYUNDAI_LONG_LIMITS = { #define HYUNDAI_FCEV_GAS_ADDR_CHECK \ {.msg = {{0x91, 0, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \ +// FrogPilot variables + static const CanMsg HYUNDAI_TX_MSGS[] = { HYUNDAI_COMMON_TX_MSGS(0) }; @@ -173,6 +175,8 @@ static void hyundai_rx_hook(const CANPacket_t *msg) { if (msg->addr == 0x394U) { brake_pressed = ((msg->data[5] >> 5U) & 0x3U) == 0x2U; } + + // FrogPilot variables } } @@ -279,6 +283,8 @@ static safety_config hyundai_init(uint16_t param) { HYUNDAI_FCEV_GAS_ADDR_CHECK }; + // FrogPilot variables + if (hyundai_fcev_gas_signal) { SET_RX_CHECKS(hyundai_fcev_long_rx_checks, ret); } else { @@ -296,6 +302,8 @@ static safety_config hyundai_init(uint16_t param) { HYUNDAI_SCC12_ADDR_CHECK(2) }; + // FrogPilot variables + ret = BUILD_SAFETY_CFG(hyundai_cam_scc_rx_checks, HYUNDAI_CAMERA_SCC_TX_MSGS); } else { static RxCheck hyundai_rx_checks[] = { @@ -309,6 +317,8 @@ static safety_config hyundai_init(uint16_t param) { HYUNDAI_FCEV_GAS_ADDR_CHECK }; + // FrogPilot variables + SET_TX_MSGS(HYUNDAI_TX_MSGS, ret); if (hyundai_fcev_gas_signal) { SET_RX_CHECKS(hyundai_fcev_rx_checks, ret); diff --git a/opendbc_repo/opendbc/safety/modes/hyundai_canfd.h b/opendbc_repo/opendbc/safety/modes/hyundai_canfd.h index 7c345bf4..fca50984 100644 --- a/opendbc_repo/opendbc/safety/modes/hyundai_canfd.h +++ b/opendbc_repo/opendbc/safety/modes/hyundai_canfd.h @@ -88,9 +88,13 @@ static void hyundai_canfd_rx_hook(const CANPacket_t *msg) { if (msg->addr == 0x1cfU) { cruise_button = msg->data[2] & 0x7U; main_button = GET_BIT(msg, 19U); + + // FrogPilot variables } else { cruise_button = (msg->data[4] >> 4) & 0x7U; main_button = GET_BIT(msg, 34U); + + // FrogPilot variables } hyundai_common_cruise_buttons_check(cruise_button, main_button); } diff --git a/opendbc_repo/opendbc/safety/modes/hyundai_common.h b/opendbc_repo/opendbc/safety/modes/hyundai_common.h index 3cca057d..6a11f852 100644 --- a/opendbc_repo/opendbc/safety/modes/hyundai_common.h +++ b/opendbc_repo/opendbc/safety/modes/hyundai_common.h @@ -42,6 +42,8 @@ bool hyundai_fcev_gas_signal = false; extern bool hyundai_alt_limits_2; bool hyundai_alt_limits_2 = false; +// FrogPilot variables + static uint8_t hyundai_last_button_interaction; // button messages since the user pressed an enable button void hyundai_common_init(uint16_t param) { @@ -53,6 +55,8 @@ void hyundai_common_init(uint16_t param) { const uint16_t HYUNDAI_PARAM_FCEV_GAS = 256; const uint16_t HYUNDAI_PARAM_ALT_LIMITS_2 = 512; + // FrogPilot variables + hyundai_ev_gas_signal = GET_FLAG(param, HYUNDAI_PARAM_EV_GAS); hyundai_hybrid_gas_signal = !hyundai_ev_gas_signal && GET_FLAG(param, HYUNDAI_PARAM_HYBRID_GAS); hyundai_camera_scc = GET_FLAG(param, HYUNDAI_PARAM_CAMERA_SCC); @@ -61,6 +65,8 @@ void hyundai_common_init(uint16_t param) { hyundai_fcev_gas_signal = GET_FLAG(param, HYUNDAI_PARAM_FCEV_GAS); hyundai_alt_limits_2 = GET_FLAG(param, HYUNDAI_PARAM_ALT_LIMITS_2); + // FrogPilot variables + hyundai_last_button_interaction = HYUNDAI_PREV_BUTTON_SAMPLES; #ifdef ALLOW_DEBUG @@ -110,6 +116,8 @@ void hyundai_common_cruise_buttons_check(const int cruise_button, const bool mai cruise_button_prev = cruise_button; } + + // FrogPilot variables } #ifdef CANFD @@ -138,3 +146,5 @@ uint32_t hyundai_common_canfd_compute_checksum(const CANPacket_t *msg) { return crc; } #endif + +// FrogPilot variables diff --git a/opendbc_repo/opendbc/safety/modes/mazda.h b/opendbc_repo/opendbc/safety/modes/mazda.h index f95cfaf8..b4624c93 100644 --- a/opendbc_repo/opendbc/safety/modes/mazda.h +++ b/opendbc_repo/opendbc/safety/modes/mazda.h @@ -34,6 +34,8 @@ static void mazda_rx_hook(const CANPacket_t *msg) { if (msg->addr == MAZDA_CRZ_CTRL) { bool cruise_engaged = msg->data[0] & 0x8U; pcm_cruise_check(cruise_engaged); + + // FrogPilot variables } if (msg->addr == MAZDA_ENGINE_DATA) { diff --git a/opendbc_repo/opendbc/safety/modes/nissan.h b/opendbc_repo/opendbc/safety/modes/nissan.h index 9289935f..f41034c4 100644 --- a/opendbc_repo/opendbc/safety/modes/nissan.h +++ b/opendbc_repo/opendbc/safety/modes/nissan.h @@ -50,6 +50,8 @@ static void nissan_rx_hook(const CANPacket_t *msg) { bool cruise_engaged = (msg->data[0] >> 3) & 1U; pcm_cruise_check(cruise_engaged); } + + // FrogPilot variables } diff --git a/opendbc_repo/opendbc/safety/modes/psa.h b/opendbc_repo/opendbc/safety/modes/psa.h index 6bbd7b9c..971922e6 100644 --- a/opendbc_repo/opendbc/safety/modes/psa.h +++ b/opendbc_repo/opendbc/safety/modes/psa.h @@ -10,6 +10,8 @@ #define PSA_DAT_BSI 1042U // RX from BSI, brake #define PSA_LANE_KEEP_ASSIST 1010U // TX from OP, EPS +// FrogPilot variables + // CAN bus #define PSA_MAIN_BUS 0U #define PSA_ADAS_BUS 1U @@ -21,6 +23,7 @@ static uint8_t psa_get_counter(const CANPacket_t *msg) { cnt = (msg->data[3] >> 4) & 0xFU; } else if (msg->addr == PSA_HS2_DYN_ABR_38D) { cnt = (msg->data[5] >> 4) & 0xFU; + // FrogPilot variables } else { } return cnt; @@ -32,6 +35,7 @@ static uint32_t psa_get_checksum(const CANPacket_t *msg) { chksum = msg->data[5] & 0xFU; } else if (msg->addr == PSA_HS2_DYN_ABR_38D) { chksum = msg->data[5] & 0xFU; + // FrogPilot variables } else { } return chksum; @@ -59,6 +63,7 @@ static uint32_t psa_compute_checksum(const CANPacket_t *msg) { chk = _psa_compute_checksum(msg, 0x4, 5); } else if (msg->addr == PSA_HS2_DYN_ABR_38D) { chk = _psa_compute_checksum(msg, 0x7, 5); + // FrogPilot variables } else { } return chk; @@ -84,6 +89,7 @@ static void psa_rx_hook(const CANPacket_t *msg) { if (msg->addr == PSA_HS2_DAT_MDD_CMD_452) { pcm_cruise_check((msg->data[2U] >> 7U) & 1U); // RVV_ACC_ACTIVATION_REQ } + // FrogPilot variables } @@ -136,6 +142,8 @@ static safety_config psa_init(uint16_t param) { {.msg = {{PSA_STEERING, PSA_MAIN_BUS, 7, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, // driver torque {.msg = {{PSA_DYN_CMM, PSA_MAIN_BUS, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, // gas pedal {.msg = {{PSA_DAT_BSI, PSA_CAM_BUS, 8, 20U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, // brake + + // FrogPilot variables }; return BUILD_SAFETY_CFG(psa_rx_checks, PSA_TX_MSGS); diff --git a/opendbc_repo/opendbc/safety/modes/rivian.h b/opendbc_repo/opendbc/safety/modes/rivian.h index 35645b7c..879034fc 100644 --- a/opendbc_repo/opendbc/safety/modes/rivian.h +++ b/opendbc_repo/opendbc/safety/modes/rivian.h @@ -97,6 +97,8 @@ static void rivian_rx_hook(const CANPacket_t *msg) { if (msg->addr == 0x100U) { const int feature_status = msg->data[2] >> 5U; pcm_cruise_check(feature_status == 1); + + // FrogPilot variables } } } diff --git a/opendbc_repo/opendbc/safety/modes/subaru.h b/opendbc_repo/opendbc/safety/modes/subaru.h index 9b01d39d..38167047 100644 --- a/opendbc_repo/opendbc/safety/modes/subaru.h +++ b/opendbc_repo/opendbc/safety/modes/subaru.h @@ -109,6 +109,8 @@ static void subaru_rx_hook(const CANPacket_t *msg) { if ((msg->addr == MSG_SUBARU_CruiseControl) && (msg->bus == alt_main_bus)) { bool cruise_engaged = (msg->data[5] >> 1) & 1U; pcm_cruise_check(cruise_engaged); + + // FrogPilot variables } // update vehicle moving with any non-zero wheel speed diff --git a/opendbc_repo/opendbc/safety/modes/subaru_preglobal.h b/opendbc_repo/opendbc/safety/modes/subaru_preglobal.h index 4ddcdc10..1d025410 100644 --- a/opendbc_repo/opendbc/safety/modes/subaru_preglobal.h +++ b/opendbc_repo/opendbc/safety/modes/subaru_preglobal.h @@ -33,6 +33,8 @@ static void subaru_preglobal_rx_hook(const CANPacket_t *msg) { if (msg->addr == MSG_SUBARU_PG_CruiseControl) { bool cruise_engaged = (msg->data[6] >> 1) & 1U; pcm_cruise_check(cruise_engaged); + + // FrogPilot variables } // update vehicle moving with any non-zero wheel speed diff --git a/opendbc_repo/opendbc/safety/modes/tesla.h b/opendbc_repo/opendbc/safety/modes/tesla.h index b8f41f04..1a258600 100644 --- a/opendbc_repo/opendbc/safety/modes/tesla.h +++ b/opendbc_repo/opendbc/safety/modes/tesla.h @@ -159,6 +159,8 @@ static void tesla_rx_hook(const CANPacket_t *msg) { cruise_engaged = cruise_engaged && !tesla_autopark; pcm_cruise_check(cruise_engaged); + + // FrogPilot variables } if (msg->addr == 0x155U) { diff --git a/opendbc_repo/opendbc/safety/modes/toyota.h b/opendbc_repo/opendbc/safety/modes/toyota.h index c2a46627..0e58d8db 100644 --- a/opendbc_repo/opendbc/safety/modes/toyota.h +++ b/opendbc_repo/opendbc/safety/modes/toyota.h @@ -39,6 +39,7 @@ #define TOYOTA_COMMON_RX_CHECKS(lta) \ {.msg = {{ 0xaa, 0, 8, 83U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \ {.msg = {{0x260, 0, 8, 50U, .ignore_counter = true, .ignore_quality_flag=!(lta)}, { 0 }, { 0 }}}, \ + /* FrogPilot Variables */ \ #define TOYOTA_RX_CHECKS(lta) \ TOYOTA_COMMON_RX_CHECKS(lta) \ @@ -159,6 +160,8 @@ static void toyota_rx_hook(const CANPacket_t *msg) { UPDATE_VEHICLE_SPEED(speed / 4.0 * 0.01 * KPH_TO_MS); } + + // FrogPilot variables } } diff --git a/opendbc_repo/opendbc/safety/safety.h b/opendbc_repo/opendbc/safety/safety.h index 6211ab2e..2c0683e3 100644 --- a/opendbc_repo/opendbc/safety/safety.h +++ b/opendbc_repo/opendbc/safety/safety.h @@ -60,6 +60,8 @@ bool acc_main_on = false; // referred to as "ACC off" in ISO 15622:2018 int cruise_button_prev = 0; bool safety_rx_checks_invalid = false; +// FrogPilot variables + // for safety modes with torque steering control int desired_torque_last = 0; // last desired steer torque int rt_torque_last = 0; // last desired torque for real time check @@ -366,6 +368,8 @@ static void generic_rx_checks(void) { controls_allowed = false; } steering_disengage_prev = steering_disengage; + + // FrogPilot variables } static void stock_ecu_check(bool stock_ecu_detected) { @@ -489,6 +493,8 @@ int set_safety_hooks(uint16_t mode, uint16_t param) { // OPGM variables enable_gas_interceptor = false; + // FrogPilot variables + return set_status; } diff --git a/opendbc_repo/opendbc/safety/tests/common.py b/opendbc_repo/opendbc/safety/tests/common.py index 20f7cf7a..5440b455 100644 --- a/opendbc_repo/opendbc/safety/tests/common.py +++ b/opendbc_repo/opendbc/safety/tests/common.py @@ -301,6 +301,8 @@ class TorqueSteeringSafetyTestBase(SafetyTestBase, abc.ABC): for _ in range(10): self.assertFalse(self._tx(self._torque_cmd_msg(self.MAX_TORQUE, 1))) + # FrogPilot variables + class SteerRequestCutSafetyTest(TorqueSteeringSafetyTestBase, abc.ABC): @@ -806,6 +808,8 @@ class AngleSteeringSafetyTest(VehicleSpeedSafetyTest): for _ in range(5): self.assertTrue(self._tx(self._angle_cmd_msg(0, True, increment_timer=False))) + # FrogPilot variables + class SafetyTest(SafetyTestBase): TX_MSGS: list[list[int]] | None = None diff --git a/opendbc_repo/opendbc/safety/tests/hyundai_common.py b/opendbc_repo/opendbc/safety/tests/hyundai_common.py index 5d79155a..df7dd8c7 100644 --- a/opendbc_repo/opendbc/safety/tests/hyundai_common.py +++ b/opendbc_repo/opendbc/safety/tests/hyundai_common.py @@ -71,6 +71,8 @@ class HyundaiButtonBase: self.assertEqual(controls_allowed, self.safety.get_controls_allowed()) self._rx(self._button_msg(Buttons.NONE)) + # FrogPilot variables + class HyundaiLongitudinalBase(common.LongitudinalAccelSafetyTest): diff --git a/opendbc_repo/opendbc/safety/tests/test_chrysler.py b/opendbc_repo/opendbc/safety/tests/test_chrysler.py index daa585ab..8a3f1c32 100755 --- a/opendbc_repo/opendbc/safety/tests/test_chrysler.py +++ b/opendbc_repo/opendbc/safety/tests/test_chrysler.py @@ -71,6 +71,8 @@ class TestChryslerSafety(common.CarSafetyTest, common.MotorTorqueSteeringSafetyT self.assertFalse(self._tx(self._button_msg(cancel=True, resume=True))) self.assertFalse(self._tx(self._button_msg(cancel=False, resume=False))) + # FrogPilot variables + class TestChryslerRamDTSafety(TestChryslerSafety): TX_MSGS = [[0xB1, 2], [0xA6, 0], [0xFA, 0]] diff --git a/opendbc_repo/opendbc/safety/tests/test_ford.py b/opendbc_repo/opendbc/safety/tests/test_ford.py index f8cc80e0..88c90a1f 100755 --- a/opendbc_repo/opendbc/safety/tests/test_ford.py +++ b/opendbc_repo/opendbc/safety/tests/test_ford.py @@ -378,6 +378,8 @@ class TestFordSafetyBase(common.CarSafetyTest): for bus in (0, 2): self.assertEqual(enabled, self._tx(self._acc_button_msg(Buttons.CANCEL, bus))) + # FrogPilot variables + class TestFordCANFDStockSafety(TestFordSafetyBase): STEER_MESSAGE = MSG_LateralMotionControl2 diff --git a/opendbc_repo/opendbc/safety/tests/test_gm.py b/opendbc_repo/opendbc/safety/tests/test_gm.py index 007ec734..e3955191 100755 --- a/opendbc_repo/opendbc/safety/tests/test_gm.py +++ b/opendbc_repo/opendbc/safety/tests/test_gm.py @@ -132,6 +132,8 @@ class TestGmSafetyBase(common.CarSafetyTest, common.DriverTorqueSteeringSafetyTe values = {"ACCButtons": buttons} return self.packer.make_can_msg_safety("ASCMSteeringButton", self.BUTTONS_BUS, values) + # FrogPilot variables + class TestGmEVSafetyBase(TestGmSafetyBase): EXTRA_SAFETY_PARAM = GMSafetyFlags.EV diff --git a/opendbc_repo/opendbc/safety/tests/test_honda.py b/opendbc_repo/opendbc/safety/tests/test_honda.py index 44220095..38714741 100755 --- a/opendbc_repo/opendbc/safety/tests/test_honda.py +++ b/opendbc_repo/opendbc/safety/tests/test_honda.py @@ -237,6 +237,8 @@ class HondaBase(common.CarSafetyTest): self.assertTrue(self._tx(self._send_steer_msg(0x0000))) self.assertFalse(self._tx(self._send_steer_msg(0x1000))) + # FrogPilot variables + # ********************* Honda Nidec ********************** @@ -598,5 +600,6 @@ class TestHondaBoschCANFDAltBrakeSafety(HondaPcmEnableBase, TestHondaBoschCANFDS self.safety.init_tests() + if __name__ == "__main__": unittest.main() diff --git a/opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py b/opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py index dd992f33..f638f2b0 100755 --- a/opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py +++ b/opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py @@ -81,6 +81,8 @@ class TestHyundaiCanfdBase(HyundaiButtonBase, common.CarSafetyTest, common.Drive } return self.packer.make_can_msg_safety("CRUISE_BUTTONS", bus, values) + # FrogPilot variables + class TestHyundaiCanfdLFASteeringBase(TestHyundaiCanfdBase): @@ -150,6 +152,8 @@ class TestHyundaiCanfdLFASteeringAltButtonsBase(TestHyundaiCanfdLFASteeringBase) self.assertFalse(self._tx(self._acc_cancel_msg(True, accel=1))) self.assertFalse(self._tx(self._acc_cancel_msg(False))) + # FrogPilot variables + @parameterized_class(ALL_GAS_EV_HYBRID_COMBOS) class TestHyundaiCanfdLFASteeringAltButtons(TestHyundaiCanfdLFASteeringAltButtonsBase): @@ -284,5 +288,6 @@ class TestHyundaiCanfdLFASteeringLongAltButtons(TestHyundaiCanfdLFASteeringLongB pass + if __name__ == "__main__": unittest.main() diff --git a/opendbc_repo/opendbc/safety/tests/test_mazda.py b/opendbc_repo/opendbc/safety/tests/test_mazda.py index 607edcbf..5d228826 100755 --- a/opendbc_repo/opendbc/safety/tests/test_mazda.py +++ b/opendbc_repo/opendbc/safety/tests/test_mazda.py @@ -80,6 +80,8 @@ class TestMazdaSafety(common.CarSafetyTest, common.DriverTorqueSteeringSafetyTes self.assertTrue(self._tx(self._button_msg(cancel=True))) self.assertTrue(self._tx(self._button_msg(resume=True))) + # FrogPilot variables + if __name__ == "__main__": unittest.main() diff --git a/opendbc_repo/opendbc/safety/tests/test_nissan.py b/opendbc_repo/opendbc/safety/tests/test_nissan.py index 8f034cd7..7090695b 100755 --- a/opendbc_repo/opendbc/safety/tests/test_nissan.py +++ b/opendbc_repo/opendbc/safety/tests/test_nissan.py @@ -79,6 +79,8 @@ class TestNissanSafety(common.CarSafetyTest, common.AngleSteeringSafetyTest): tx = self._tx(self._acc_button_cmd(**args)) self.assertEqual(tx, should_tx) + # FrogPilot variables + class TestNissanSafetyAltEpsBus(TestNissanSafety): """Altima uses different buses""" @@ -113,6 +115,8 @@ class TestNissanLeafSafety(TestNissanSafety): def test_acc_buttons(self): pass + # FrogPilot variables + if __name__ == "__main__": unittest.main() diff --git a/opendbc_repo/opendbc/safety/tests/test_subaru.py b/opendbc_repo/opendbc/safety/tests/test_subaru.py index a0ad42d8..c7c3c8d6 100755 --- a/opendbc_repo/opendbc/safety/tests/test_subaru.py +++ b/opendbc_repo/opendbc/safety/tests/test_subaru.py @@ -111,6 +111,8 @@ class TestSubaruSafetyBase(common.CarSafetyTest): values = {"Cruise_Activated": enable} return self.packer.make_can_msg_safety("CruiseControl", self.ALT_MAIN_BUS, values) + # FrogPilot variables + class TestSubaruStockLongitudinalSafetyBase(TestSubaruSafetyBase): def _cancel_msg(self, cancel, cruise_throttle=0): diff --git a/opendbc_repo/opendbc/safety/tests/test_subaru_preglobal.py b/opendbc_repo/opendbc/safety/tests/test_subaru_preglobal.py index fb96de24..5d470187 100755 --- a/opendbc_repo/opendbc/safety/tests/test_subaru_preglobal.py +++ b/opendbc_repo/opendbc/safety/tests/test_subaru_preglobal.py @@ -59,6 +59,8 @@ class TestSubaruPreglobalSafety(common.CarSafetyTest, common.DriverTorqueSteerin values = {"Cruise_Activated": enable} return self.packer.make_can_msg_safety("CruiseControl", 0, values) + # FrogPilot variables + class TestSubaruPreglobalReversedDriverTorqueSafety(TestSubaruPreglobalSafety): FLAGS = SubaruSafetyFlags.PREGLOBAL_REVERSED_DRIVER_TORQUE diff --git a/opendbc_repo/opendbc/safety/tests/test_tesla.py b/opendbc_repo/opendbc/safety/tests/test_tesla.py index 52390f81..07df31d8 100755 --- a/opendbc_repo/opendbc/safety/tests/test_tesla.py +++ b/opendbc_repo/opendbc/safety/tests/test_tesla.py @@ -353,6 +353,8 @@ class TestTeslaSafetyBase(common.CarSafetyTest, common.AngleSteeringSafetyTest, # Recover self.assertTrue(self._tx(self._angle_cmd_msg(0, True))) + # FrogPilot variables + class TestTeslaStockSafety(TestTeslaSafetyBase): diff --git a/opendbc_repo/opendbc/safety/tests/test_toyota.py b/opendbc_repo/opendbc/safety/tests/test_toyota.py index dc94ddbe..3bdff15c 100755 --- a/opendbc_repo/opendbc/safety/tests/test_toyota.py +++ b/opendbc_repo/opendbc/safety/tests/test_toyota.py @@ -122,6 +122,8 @@ class TestToyotaSafetyBase(common.CarSafetyTest, common.LongitudinalAccelSafetyT self.assertFalse(self._rx(msg)) self.assertFalse(self.safety.get_controls_allowed()) + # FrogPilot variables + class TestToyotaSafetyTorque(TestToyotaSafetyBase, common.MotorTorqueSteeringSafetyTest, common.SteerRequestCutSafetyTest): @@ -394,5 +396,6 @@ class TestToyotaSecOcSafety(TestToyotaSecOcSafetyBase): self.assertEqual(should_tx, self._tx(self._accel_msg_343(accel, cancel_req=1))) + if __name__ == "__main__": unittest.main() diff --git a/opendbc_repo/opendbc/safety/tests/test_volkswagen_mqb.py b/opendbc_repo/opendbc/safety/tests/test_volkswagen_mqb.py index 47f77a8d..5418f913 100755 --- a/opendbc_repo/opendbc/safety/tests/test_volkswagen_mqb.py +++ b/opendbc_repo/opendbc/safety/tests/test_volkswagen_mqb.py @@ -126,6 +126,8 @@ class TestVolkswagenMqbSafetyBase(common.CarSafetyTest, common.DriverTorqueSteer self.assertEqual(0, self.safety.get_torque_driver_max()) self.assertEqual(0, self.safety.get_torque_driver_min()) + # FrogPilot variables + class TestVolkswagenMqbStockSafety(TestVolkswagenMqbSafetyBase): TX_MSGS = [[MSG_HCA_01, 0], [MSG_LDW_02, 0], [MSG_LH_EPS_03, 2], [MSG_GRA_ACC_01, 0], [MSG_GRA_ACC_01, 2]] diff --git a/opendbc_repo/opendbc/safety/tests/test_volkswagen_pq.py b/opendbc_repo/opendbc/safety/tests/test_volkswagen_pq.py index 72f0bfa6..bb099573 100755 --- a/opendbc_repo/opendbc/safety/tests/test_volkswagen_pq.py +++ b/opendbc_repo/opendbc/safety/tests/test_volkswagen_pq.py @@ -108,6 +108,8 @@ class TestVolkswagenPqSafetyBase(common.CarSafetyTest, common.DriverTorqueSteeri self.assertEqual(0, self.safety.get_torque_driver_max()) self.assertEqual(0, self.safety.get_torque_driver_min()) + # FrogPilot variables + class TestVolkswagenPqStockSafety(TestVolkswagenPqSafetyBase): # Transmit of GRA_Neu is allowed on bus 0 and 2 to keep compatibility with gateway and camera integration diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py old mode 100755 new mode 100644 index 42cdc1ff..772627be --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -63,6 +63,8 @@ class Car: RI: RadarInterfaceBase CP: car.CarParams + # FrogPilot variables + def __init__(self, CI=None, RI=None) -> None: self.can_sock = messaging.sub_sock('can', timeout=20) self.sm = messaging.SubMaster(['pandaStates', 'carControl', 'onroadEvents']) @@ -105,6 +107,8 @@ class Car: # continue onto next fingerprinting step in pandad self.params.put_bool("FirmwareQueryDone", True) + + # FrogPilot variables else: self.CI, self.CP = CI, CI.CP self.RI = RI @@ -162,6 +166,8 @@ class Car: # OPGM variables self.resume_prev_button = False + # FrogPilot variables + def state_update(self) -> tuple[car.CarState, structs.RadarDataT | None]: """carState update loop, driven by can""" @@ -202,6 +208,8 @@ class Car: elif any(be.type in (ButtonType.decelCruise, ButtonType.setCruise) for be in CS.buttonEvents): self.resume_prev_button = False + # FrogPilot variables + return CS, RD def state_publish(self, CS: car.CarState, RD: structs.RadarDataT | None): @@ -234,6 +242,8 @@ class Car: tracks_msg.liveTracks = RD self.pm.send('liveTracks', tracks_msg) + # FrogPilot variables + def controls_update(self, CS: car.CarState, CC: car.CarControl): """control update loop, driven by carControl""" @@ -265,6 +275,9 @@ class Car: self.initialized_prev = initialized self.CS_prev = CS + # FrogPilot variables + self.CI.CS.CC = self.sm['carControl'] + def params_thread(self, evt): while not evt.is_set(): self.is_metric = self.params.get_bool("IsMetric") diff --git a/selfdrive/car/cruise.py b/selfdrive/car/cruise.py index 644337b9..187e13b8 100644 --- a/selfdrive/car/cruise.py +++ b/selfdrive/car/cruise.py @@ -109,6 +109,8 @@ class VCruiseHelper: else: self.v_cruise_kph += v_cruise_delta * CRUISE_INTERVAL_SIGN[button_type] + # FrogPilot variables + # If set is pressed while overriding, clip cruise speed to minimum of vEgo if CS.gasPressed and button_type in (ButtonType.decelCruise, ButtonType.setCruise): self.v_cruise_kph = max(self.v_cruise_kph, CS.vEgo * CV.MS_TO_KPH) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 1599c7c6..2f1e20f3 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -58,6 +58,8 @@ class Controls: elif self.CP.lateralTuning.which() == 'torque': self.LaC = LatControlTorque(self.CP, self.CI, DT_CTRL) + # FrogPilot variables + def update(self): self.sm.update(15) if self.sm.updated["liveCalibration"]: @@ -66,6 +68,8 @@ class Controls: device_pose = Pose.from_live_pose(self.sm['livePose']) self.calibrated_pose = self.pose_calibrator.build_calibrated_pose(device_pose) + # FrogPilot variables + def state_control(self): CS = self.sm['carState'] diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index ee4567f1..a77ab526 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -29,6 +29,8 @@ DESIRES = { }, } +# FrogPilot variables + class DesireHelper: def __init__(self): @@ -40,6 +42,8 @@ class DesireHelper: self.prev_one_blinker = False self.desire = log.Desire.none + # FrogPilot variables + @staticmethod def get_lane_change_direction(CS): return LaneChangeDirection.left if CS.leftBlinker else LaneChangeDirection.right @@ -72,12 +76,18 @@ class DesireHelper: blindspot_detected = ((carstate.leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or (carstate.rightBlindspot and self.lane_change_direction == LaneChangeDirection.right)) + # FrogPilot variables + if not one_blinker or below_lane_change_speed: self.lane_change_state = LaneChangeState.off self.lane_change_direction = LaneChangeDirection.none elif torque_applied and not blindspot_detected: self.lane_change_state = LaneChangeState.laneChangeStarting + # FrogPilot variables + + # FrogPilot variables + # LaneChangeState.laneChangeStarting elif self.lane_change_state == LaneChangeState.laneChangeStarting: # fade out over .5s @@ -117,3 +127,5 @@ class DesireHelper: self.keep_pulse_timer = 0.0 elif self.desire in (log.Desire.keepLeft, log.Desire.keepRight): self.desire = log.Desire.none + + # FrogPilot variables diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 34fc85f8..ad2f25e4 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -87,6 +87,9 @@ class LongitudinalPlanner: throttle_prob = model_msg.meta.disengagePredictions.gasPressProbs[1] else: throttle_prob = 1.0 + + # FrogPilot variables + return x, v, a, j, throttle_prob def update(self, sm): diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index bec7eede..6e5d5b33 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -22,6 +22,8 @@ def main(): sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'liveParameters', 'radarState', 'modelV2', 'selfdriveState'], poll='modelV2') + # FrogPilot variables + while True: sm.update() if sm.updated['modelV2']: @@ -35,6 +37,8 @@ def main(): msg.driverAssistance.rightLaneDeparture = ldw.right pm.send('driverAssistance', msg) + # FrogPilot variables + if __name__ == "__main__": main() diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 98fce1cb..f90fd54f 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -58,6 +58,8 @@ class Track: self.K_K = kalman_params.K self.kf = KF1D([[v_lead], [0.0]], self.K_A, self.K_C, self.K_K) + # FrogPilot variables + def update(self, d_rel: float, y_rel: float, v_rel: float, v_lead: float, measured: float): # relative values, copy self.dRel = d_rel # LONG_DIST @@ -109,6 +111,8 @@ class Track: ret = f"x: {self.dRel:4.1f} y: {self.yRel:4.1f} v: {self.vRel:4.1f} a: {self.aLeadK:4.1f}" return ret + # FrogPilot variables + def laplacian_pdf(x: float, mu: float, b: float): b = max(b, 1e-4) @@ -116,6 +120,8 @@ def laplacian_pdf(x: float, mu: float, b: float): def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, tracks: dict[int, Track]): + # FrogPilot variables + offset_vision_dist = lead.x[0] - RADAR_TO_CAMERA def prob(c): @@ -179,9 +185,14 @@ def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capn if (not lead_dict['status']) or (closest_track.dRel < lead_dict['dRel']): lead_dict = closest_track.get_RadarState() + # FrogPilot variables + return lead_dict +# FrogPilot variables + + class RadarD: def __init__(self, delay: float = 0.0): self.current_time = 0.0 @@ -198,6 +209,8 @@ class RadarD: self.ready = False + # FrogPilot variables + def update(self, sm: messaging.SubMaster, rr: car.RadarData): self.ready = sm.seen['modelV2'] self.current_time = 1e-9*max(sm.logMonoTime.values()) @@ -242,6 +255,8 @@ class RadarD: self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, low_speed_override=True) self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, low_speed_override=False) + # FrogPilot variables + def publish(self, pm: messaging.PubMaster): assert self.radar_state is not None @@ -266,6 +281,8 @@ def main() -> None: RD = RadarD(CP.radarDelay) + # FrogPilot variables + while 1: sm.update() diff --git a/selfdrive/locationd/lagd.py b/selfdrive/locationd/lagd.py index d7834f7f..7a53bce8 100755 --- a/selfdrive/locationd/lagd.py +++ b/selfdrive/locationd/lagd.py @@ -374,6 +374,8 @@ def main(): lag, valid_blocks = initial_lag_params lag_learner.reset(lag, valid_blocks) + # FrogPilot variables + while True: sm.update() if sm.all_checks(): @@ -392,3 +394,5 @@ def main(): if sm.frame % 1200 == 0: # cache every 60 seconds params.put_nonblocking("LiveDelay", lag_msg_dat) + + # FrogPilot variables diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index a5a7228e..41473bc5 100755 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -283,6 +283,8 @@ def main(): steer_ratio, stiffness_factor, angle_offset_deg, pInitial = retrieve_initial_vehicle_params(params, CP, REPLAY, DEBUG) learner = VehicleParamsLearner(CP, steer_ratio, stiffness_factor, np.radians(angle_offset_deg), pInitial) + # FrogPilot variables + while True: sm.update() if sm.all_checks(): @@ -300,6 +302,8 @@ def main(): pm.send('liveParameters', msg_dat) + # FrogPilot variables + if __name__ == "__main__": main() diff --git a/selfdrive/locationd/torqued.py b/selfdrive/locationd/torqued.py index 3f9b846e..86f44f57 100755 --- a/selfdrive/locationd/torqued.py +++ b/selfdrive/locationd/torqued.py @@ -251,6 +251,8 @@ def main(demo=False): params = Params() estimator = TorqueEstimator(messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)) + # FrogPilot variables + while True: sm.update() if sm.all_checks(): @@ -268,6 +270,8 @@ def main(demo=False): msg = estimator.get_msg(valid=sm.all_checks(), with_points=True) params.put_nonblocking("LiveTorqueParameters", msg.to_bytes()) + # FrogPilot variables + if __name__ == "__main__": import argparse diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 006eeef6..195c51da 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -225,6 +225,8 @@ class ModelState: def main(demo=False): cloudlog.warning("modeld init") + # FrogPilot variables + if not USBGPU: # USB GPU currently saturates a core so can't do this yet, # also need to move the aux USB interrupts for good timings @@ -294,6 +296,8 @@ def main(demo=False): DH = DesireHelper() + # FrogPilot variables + while True: # Keep receiving frames until we are at least 1 frame ahead of previous extra frame while meta_main.timestamp_sof < meta_extra.timestamp_sof + 25000000: @@ -397,8 +401,12 @@ def main(demo=False): pm.send('modelV2', modelv2_send) pm.send('drivingModelData', drivingdata_send) pm.send('cameraOdometry', posenet_send) + + # FrogPilot variables last_vipc_frame_id = meta_main.frame_id + # FrogPilot variables + if __name__ == "__main__": try: diff --git a/selfdrive/monitoring/dmonitoringd.py b/selfdrive/monitoring/dmonitoringd.py index 1ac2c2dc..08e30955 100755 --- a/selfdrive/monitoring/dmonitoringd.py +++ b/selfdrive/monitoring/dmonitoringd.py @@ -15,6 +15,8 @@ def dmonitoringd_thread(): DM = DriverMonitoring(rhd_saved=params.get_bool("IsRhdDetected"), always_on=params.get_bool("AlwaysOnDM")) demo_mode=False + # FrogPilot variables + # 20Hz <- dmonitoringmodeld while True: sm.update() @@ -27,6 +29,7 @@ def dmonitoringd_thread(): DM.run_step(sm, demo=demo_mode) elif valid: DM.run_step(sm, demo=demo_mode) + # FrogPilot variables # publish dat = DM.get_state_packet(valid=valid) diff --git a/selfdrive/pandad/panda_safety.cc b/selfdrive/pandad/panda_safety.cc index b0895034..594b9426 100644 --- a/selfdrive/pandad/panda_safety.cc +++ b/selfdrive/pandad/panda_safety.cc @@ -65,6 +65,8 @@ void PandaSafety::setSafetyMode(const std::string ¶ms_string) { auto safety_configs = car_params.getSafetyConfigs(); uint16_t alternative_experience = car_params.getAlternativeExperience(); + // FrogPilot variables + for (int i = 0; i < pandas_.size(); ++i) { // Default to SILENT safety model if not specified cereal::CarParams::SafetyModel safety_model = cereal::CarParams::SafetyModel::SILENT; @@ -74,6 +76,8 @@ void PandaSafety::setSafetyMode(const std::string ¶ms_string) { safety_param = safety_configs[i].getSafetyParam(); } + // FrogPilot variables + LOGW("Panda %d: setting safety model: %d, param: %d, alternative experience: %d", i, (int)safety_model, safety_param, alternative_experience); pandas_[i]->set_alternative_experience(alternative_experience); pandas_[i]->set_safety_model(safety_model, safety_param); diff --git a/selfdrive/pandad/pandad.cc b/selfdrive/pandad/pandad.cc index 2fd4a4de..fe5a1b6d 100644 --- a/selfdrive/pandad/pandad.cc +++ b/selfdrive/pandad/pandad.cc @@ -43,6 +43,8 @@ ExitHandler do_exit; +// FrogPilot variables + bool check_all_connected(const std::vector &pandas) { for (const auto& panda : pandas) { if (!panda->connected()) { @@ -107,6 +109,8 @@ void can_send_thread(std::vector pandas, bool fake_send) { } else { LOGE("sendcan too old to send: %" PRIu64 ", %" PRIu64, nanos_since_boot(), event.getLogMonoTime()); } + + // FrogPilot variables } } @@ -421,9 +425,9 @@ void process_peripheral_state(Panda *panda, PubMaster *pm, bool no_fan_control) } if (ir_pwr != prev_ir_pwr || sm.frame % 100 == 0) { - int16_t ir_panda = util::map_val(ir_pwr, 0, 100, 0, MAX_IR_PANDA_VAL); + int16_t ir_panda = util::map_val(ir_pwr, 0, 100, 0, MAX_IR_PANDA_VAL); panda->set_ir_pwr(ir_panda); - Hardware::set_ir_power(ir_pwr); + Hardware::set_ir_power(ir_pwr); prev_ir_pwr = ir_pwr; } } diff --git a/selfdrive/selfdrived/events.py b/selfdrive/selfdrived/events.py index 1acfa296..e9b91fd8 100755 --- a/selfdrive/selfdrived/events.py +++ b/selfdrive/selfdrived/events.py @@ -9,6 +9,7 @@ from cereal import log, car import cereal.messaging as messaging from openpilot.common.constants import CV from openpilot.common.git import get_short_branch +from openpilot.common.params import Params from openpilot.common.realtime import DT_CTRL from openpilot.selfdrive.locationd.calibrationd import MIN_SPEED_FILTER from openpilot.system.micd import SAMPLE_RATE, SAMPLE_BUFFER @@ -21,6 +22,8 @@ VisualAlert = car.CarControl.HUDControl.VisualAlert AudibleAlert = car.CarControl.HUDControl.AudibleAlert EventName = log.OnroadEvent.EventName +# FrogPilot variables + # Alert priorities class Priority(IntEnum): @@ -49,6 +52,8 @@ class ET: # get event name from enum EVENT_NAME = {v: k for k, v in EventName.schema.enumerants.items()} +# FrogPilot variables + class Events: def __init__(self): @@ -389,6 +394,9 @@ def invalid_lkas_setting_alert(CP: car.CarParams, CS: car.CarState, sm: messagin return NormalPermanentAlert("Invalid LKAS setting", text) +# FrogPilot variables + + EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { # ********** events with no alerts ********** @@ -1020,6 +1028,10 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { }, } +# FrogPilot variables +FROGPILOT_EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { +} + if HARDWARE.get_device_type() == 'mici': EVENTS.update({ diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index 997c7e37..2221c77b 100755 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -39,6 +39,8 @@ EventName = log.OnroadEvent.EventName ButtonType = car.CarState.ButtonEvent.Type SafetyModel = car.CarParams.SafetyModel +# FrogPilot variables + IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput) @@ -139,6 +141,8 @@ class SelfdriveD: elif self.CP.passive: self.events.add(EventName.dashcamMode, static=True) + # FrogPilot variables + def update_events(self, CS): """Compute onroadEvents from carState""" @@ -401,6 +405,8 @@ class SelfdriveD: self.params.put_nonblocking('LongitudinalPersonality', self.personality) self.events.add(EventName.personalityChanged) + # FrogPilot variables + def data_sample(self): _car_state = messaging.recv_one(self.car_state_sock) CS = _car_state.carState if _car_state else self.CS_prev @@ -461,6 +467,8 @@ class SelfdriveD: self.AM.add_many(self.sm.frame, alerts) self.AM.process_alerts(self.sm.frame, clear_event_types) + # FrogPilot variables + def publish_selfdriveState(self, CS): # selfdriveState ss_msg = messaging.new_message('selfdriveState') @@ -491,6 +499,8 @@ class SelfdriveD: self.pm.send('onroadEvents', ce_send) self.events_prev = self.events.names.copy() + # FrogPilot variables + def step(self): CS = self.data_sample() self.update_events(CS) @@ -502,6 +512,8 @@ class SelfdriveD: self.CS_prev = CS + # FrogPilot variables + def params_thread(self, evt): while not evt.is_set(): self.is_metric = self.params.get_bool("IsMetric") diff --git a/selfdrive/ui/SConscript b/selfdrive/ui/SConscript index ba9fa8b7..d89d991a 100644 --- a/selfdrive/ui/SConscript +++ b/selfdrive/ui/SConscript @@ -4,6 +4,8 @@ Import('env', 'qt_env', 'arch', 'common', 'messaging', 'visionipc', 'transformat base_libs = [common, messaging, visionipc, transformations, 'm', 'OpenCL', 'ssl', 'crypto', 'pthread'] + qt_env["LIBS"] +# FrogPilot variables + if arch == 'larch64': base_libs.append('EGL') @@ -21,6 +23,10 @@ widgets_src = ["qt/widgets/input.cc", "qt/widgets/wifi.cc", "qt/prime_state.cc", "qt/widgets/scrollview.cc", "qt/widgets/cameraview.cc", "#third_party/qrcode/QrCode.cc", "qt/request_repeater.cc", "qt/qt_window.cc", "qt/network/networking.cc", "qt/network/wifi_manager.cc"] +# FrogPilot variables +frogpilot_widgets_src = ["../../frogpilot/ui/qt/widgets/frogpilot_controls.cc"] +widgets_src += frogpilot_widgets_src + widgets = qt_env.Library("qt_widgets", widgets_src, LIBS=base_libs) Export('widgets') qt_libs = [widgets, qt_util] + base_libs @@ -32,6 +38,11 @@ qt_src = ["main.cc", "ui.cc", "qt/sidebar.cc", "qt/body.cc", "qt/onroad/onroad_home.cc", "qt/onroad/annotated_camera.cc", "qt/onroad/model.cc", "qt/onroad/buttons.cc", "qt/onroad/alerts.cc", "qt/onroad/driver_monitoring.cc", "qt/onroad/hud.cc"] +# FrogPilot variables +frogpilot_src = ["../../frogpilot/ui/frogpilot_ui.cc", "../../frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc", + "../../frogpilot/ui/qt/onroad/frogpilot_buttons.cc", "../../frogpilot/ui/qt/onroad/frogpilot_onroad.cc"] +qt_src += frogpilot_src + # build translation files with open(File("translations/languages.json").abspath) as f: languages = json.loads(f.read()) diff --git a/selfdrive/ui/qt/home.cc b/selfdrive/ui/qt/home.cc index 8b1fdf47..7d7f9147 100644 --- a/selfdrive/ui/qt/home.cc +++ b/selfdrive/ui/qt/home.cc @@ -42,13 +42,15 @@ HomeWindow::HomeWindow(QWidget* parent) : QWidget(parent) { QObject::connect(uiState(), &UIState::uiUpdate, this, &HomeWindow::updateState); QObject::connect(uiState(), &UIState::offroadTransition, this, &HomeWindow::offroadTransition); QObject::connect(uiState(), &UIState::offroadTransition, sidebar, &Sidebar::offroadTransition); + + // FrogPilot variables } void HomeWindow::showSidebar(bool show) { sidebar->setVisible(show); } -void HomeWindow::updateState(const UIState &s) { +void HomeWindow::updateState(const UIState &s, const FrogPilotUIState &fs) { const SubMaster &sm = *(s.sm); // switch to the generic robot UI @@ -56,13 +58,22 @@ void HomeWindow::updateState(const UIState &s) { body->setEnabled(true); slayout->setCurrentWidget(body); } + + // FrogPilot variables + const FrogPilotUIScene &frogpilot_scene = fs.frogpilot_scene; } void HomeWindow::offroadTransition(bool offroad) { + // FrogPilot variables + FrogPilotUIState &fs = *frogpilotUIState(); + FrogPilotUIScene &frogpilot_scene = fs.frogpilot_scene; + body->setEnabled(false); sidebar->setVisible(offroad); if (offroad) { slayout->setCurrentWidget(home); + + // FrogPilot variables } else { slayout->setCurrentWidget(onroad); } @@ -76,12 +87,16 @@ void HomeWindow::showDriverView(bool show) { slayout->setCurrentWidget(home); } sidebar->setVisible(show == false); + + // FrogPilot variables } void HomeWindow::mousePressEvent(QMouseEvent* e) { // Handle sidebar collapsing if ((onroad->isVisible() || body->isVisible()) && (!sidebar->isVisible() || e->x() > sidebar->width())) { sidebar->setVisible(!sidebar->isVisible()); + + // FrogPilot variables } } @@ -109,6 +124,8 @@ OffroadHome::OffroadHome(QWidget* parent) : QFrame(parent) { header_layout->setContentsMargins(0, 0, 0, 0); header_layout->setSpacing(16); + // FrogPilot variables + update_notif = new QPushButton(tr("UPDATE")); update_notif->setVisible(false); update_notif->setStyleSheet("background-color: #364DEF;"); @@ -240,4 +257,8 @@ void OffroadHome::refresh() { if (alerts) { alert_notif->setText(QString::number(alerts) + (alerts > 1 ? tr(" ALERTS") : tr(" ALERT"))); } + + // FrogPilot variables + FrogPilotUIState &fs = *frogpilotUIState(); + FrogPilotUIScene &frogpilot_scene = fs.frogpilot_scene; } diff --git a/selfdrive/ui/qt/home.h b/selfdrive/ui/qt/home.h index a19b70ac..4e5068d8 100644 --- a/selfdrive/ui/qt/home.h +++ b/selfdrive/ui/qt/home.h @@ -39,6 +39,8 @@ private: OffroadAlert* alerts_widget; QPushButton* alert_notif; QPushButton* update_notif; + + // FrogPilot variables }; class HomeWindow : public QWidget { @@ -68,6 +70,8 @@ private: DriverViewWindow *driver_view; QStackedLayout *slayout; + // FrogPilot variables + private slots: - void updateState(const UIState &s); + void updateState(const UIState &s, const FrogPilotUIState &fs); }; diff --git a/selfdrive/ui/qt/network/wifi_manager.h b/selfdrive/ui/qt/network/wifi_manager.h index cab932a3..76baec3b 100644 --- a/selfdrive/ui/qt/network/wifi_manager.h +++ b/selfdrive/ui/qt/network/wifi_manager.h @@ -69,6 +69,8 @@ public: void changeTetheringPassword(const QString &newPassword); QString getTetheringPassword(); + // FrogPilot variables + private: QString adapter; // Path to network manager wifi-device QTimer timer; diff --git a/selfdrive/ui/qt/offroad/developer_panel.cc b/selfdrive/ui/qt/offroad/developer_panel.cc index a095228d..9370d7cf 100644 --- a/selfdrive/ui/qt/offroad/developer_panel.cc +++ b/selfdrive/ui/qt/offroad/developer_panel.cc @@ -45,6 +45,8 @@ DeveloperPanel::DeveloperPanel(SettingsWindow *parent) : ListWidget(parent) { // Toggles should be not available to change in onroad state QObject::connect(uiState(), &UIState::offroadTransition, this, &DeveloperPanel::updateToggles); + + // FrogPilot variables } void DeveloperPanel::updateToggles(bool _offroad) { @@ -59,6 +61,8 @@ void DeveloperPanel::updateToggles(bool _offroad) { if (btn != experimentalLongitudinalToggle) { btn->setEnabled(_offroad); } + + // FrogPilot variables } // longManeuverToggle and experimentalLongitudinalToggle should not be toggleable if the car does not have longitudinal control @@ -81,6 +85,8 @@ void DeveloperPanel::updateToggles(bool _offroad) { experimentalLongitudinalToggle->setVisible(CP.getAlphaLongitudinalAvailable() && !is_release); longManeuverToggle->setEnabled(hasLongitudinalControl(CP) && _offroad); + + // FrogPilot variables } else { longManeuverToggle->setEnabled(false); experimentalLongitudinalToggle->setVisible(false); @@ -88,8 +94,12 @@ void DeveloperPanel::updateToggles(bool _offroad) { experimentalLongitudinalToggle->refresh(); offroad = _offroad; + + // FrogPilot variables } void DeveloperPanel::showEvent(QShowEvent *event) { updateToggles(offroad); + + // FrogPilot variables } diff --git a/selfdrive/ui/qt/offroad/developer_panel.h b/selfdrive/ui/qt/offroad/developer_panel.h index c73421b1..e707cffd 100644 --- a/selfdrive/ui/qt/offroad/developer_panel.h +++ b/selfdrive/ui/qt/offroad/developer_panel.h @@ -8,6 +8,8 @@ public: explicit DeveloperPanel(SettingsWindow *parent); void showEvent(QShowEvent *event) override; +// FrogPilot variables + private: Params params; ParamControl* adbToggle; @@ -17,6 +19,8 @@ private: bool is_release; bool offroad = false; + // FrogPilot variables + private slots: void updateToggles(bool _offroad); }; diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index 96734d69..e676456f 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -119,6 +119,8 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) { // Toggles with confirmation dialogs toggles["ExperimentalMode"]->setActiveIcon("../assets/icons/experimental.svg"); toggles["ExperimentalMode"]->setConfirmation(true, true); + + // FrogPilot variables } void TogglesPanel::updateState(const UIState &s) { @@ -201,6 +203,10 @@ void TogglesPanel::updateToggles() { } else { experimental_mode_toggle->setDescription(e2e_description); } + + // FrogPilot variables + FrogPilotUIState &fs = *frogpilotUIState(); + FrogPilotUIScene &frogpilot_scene = fs.frogpilot_scene; } DevicePanel::DevicePanel(SettingsWindow *parent) : ListWidget(parent) { @@ -406,6 +412,10 @@ void SettingsWindow::showEvent(QShowEvent *event) { setCurrentPanel(0); } +// FrogPilot variables +void SettingsWindow::hideEvent(QHideEvent *event) { +} + void SettingsWindow::setCurrentPanel(int index, const QString ¶m) { if (!param.isEmpty()) { // Check if param ends with "Panel" to determine if it's a panel name @@ -454,7 +464,10 @@ SettingsWindow::SettingsWindow(QWidget *parent) : QFrame(parent) { close_btn->setFixedSize(200, 200); sidebar_layout->addSpacing(45); sidebar_layout->addWidget(close_btn, 0, Qt::AlignCenter); - QObject::connect(close_btn, &QPushButton::clicked, this, &SettingsWindow::closeSettings); + QObject::connect(close_btn, &QPushButton::clicked, [this]() { + // FrogPilot variables + closeSettings(); + }); // setup panels DevicePanel *device = new DevicePanel(this); @@ -468,6 +481,8 @@ SettingsWindow::SettingsWindow(QWidget *parent) : QFrame(parent) { auto networking = new Networking(this); QObject::connect(uiState()->prime_state, &PrimeState::changed, networking, &Networking::setPrimeType); + // FrogPilot variables + QList> panels = { {tr("Device"), device}, {tr("Network"), networking}, @@ -508,6 +523,8 @@ SettingsWindow::SettingsWindow(QWidget *parent) : QFrame(parent) { panel_widget->addWidget(panel_frame); QObject::connect(btn, &QPushButton::clicked, [=, w = panel_frame]() { + // FrogPilot variables + btn->setChecked(true); panel_widget->setCurrentWidget(w); }); diff --git a/selfdrive/ui/qt/offroad/settings.h b/selfdrive/ui/qt/offroad/settings.h index d52cf16b..c5b7a5aa 100644 --- a/selfdrive/ui/qt/offroad/settings.h +++ b/selfdrive/ui/qt/offroad/settings.h @@ -25,6 +25,9 @@ public: protected: void showEvent(QShowEvent *event) override; + // FrogPilot variables + void hideEvent(QHideEvent *event) override; + signals: void closeSettings(); void reviewTrainingGuide(); @@ -32,11 +35,16 @@ signals: void expandToggleDescription(const QString ¶m); void scrollToToggle(const QString ¶m); + // FrogPilot variables + private: QPushButton *sidebar_alert_widget; QWidget *sidebar_widget; QButtonGroup *nav_btns; QStackedWidget *panel_widget; + + // FrogPilot variables + Params params; }; class DevicePanel : public ListWidget { @@ -65,6 +73,9 @@ public: explicit TogglesPanel(SettingsWindow *parent); void showEvent(QShowEvent *event) override; +signals: + // FrogPilot variables + public slots: void expandToggleDescription(const QString ¶m); void scrollToToggle(const QString ¶m); diff --git a/selfdrive/ui/qt/offroad/software_settings.cc b/selfdrive/ui/qt/offroad/software_settings.cc index 9bc3fad3..346948f9 100644 --- a/selfdrive/ui/qt/offroad/software_settings.cc +++ b/selfdrive/ui/qt/offroad/software_settings.cc @@ -68,6 +68,8 @@ SoftwarePanel::SoftwarePanel(QWidget* parent) : ListWidget(parent) { params.put("UpdaterTargetBranch", selection.toStdString()); targetBranchBtn->setValue(QString::fromStdString(params.get("UpdaterTargetBranch"))); checkForUpdates(); + + // FrogPilot variables } }); if (!params.getBool("IsTestedBranch")) { @@ -101,9 +103,17 @@ void SoftwarePanel::showEvent(QShowEvent *event) { installBtn->setEnabled(true); updateLabels(); + + // FrogPilot variables + FrogPilotUIState &fs = *frogpilotUIState(); + FrogPilotUIScene &frogpilot_scene = fs.frogpilot_scene; } void SoftwarePanel::updateLabels() { + // FrogPilot variables + FrogPilotUIState &fs = *frogpilotUIState(); + FrogPilotUIScene &frogpilot_scene = fs.frogpilot_scene; + // add these back in case the files got removed fs_watch->addParam("LastUpdateTime"); fs_watch->addParam("UpdateFailedCount"); @@ -111,6 +121,7 @@ void SoftwarePanel::updateLabels() { fs_watch->addParam("UpdateAvailable"); if (!isVisible()) { + // FrogPilot variables return; } @@ -124,6 +135,8 @@ void SoftwarePanel::updateLabels() { if (updater_state != "idle") { downloadBtn->setEnabled(false); downloadBtn->setValue(updater_state); + + // FrogPilot variables } else { if (failed) { downloadBtn->setText(tr("CHECK")); @@ -141,6 +154,8 @@ void SoftwarePanel::updateLabels() { downloadBtn->setValue(tr("up to date, last checked %1").arg(lastUpdate)); } downloadBtn->setEnabled(true); + + // FrogPilot variables } targetBranchBtn->setValue(QString::fromStdString(params.get("UpdaterTargetBranch"))); diff --git a/selfdrive/ui/qt/onroad/alerts.cc b/selfdrive/ui/qt/onroad/alerts.cc index 0236e20f..ee0e9008 100644 --- a/selfdrive/ui/qt/onroad/alerts.cc +++ b/selfdrive/ui/qt/onroad/alerts.cc @@ -5,27 +5,36 @@ #include "selfdrive/ui/qt/util.h" -void OnroadAlerts::updateState(const UIState &s) { +void OnroadAlerts::updateState(const UIState &s, const FrogPilotUIState &fs) { Alert a = getAlert(*(s.sm), s.scene.started_frame); if (!alert.equal(a)) { alert = a; update(); } + + // FrogPilot variables } void OnroadAlerts::clear() { alert = {}; update(); + + // FrogPilot variables + alertHeight = 0; } OnroadAlerts::Alert OnroadAlerts::getAlert(const SubMaster &sm, uint64_t started_frame) { const cereal::SelfdriveState::Reader &ss = sm["selfdriveState"].getSelfdriveState(); const uint64_t selfdrive_frame = sm.rcv_frame("selfdriveState"); + // FrogPilot variables + Alert a = {}; if (selfdrive_frame >= started_frame) { // Don't get old alert. a = {ss.getAlertText1().cStr(), ss.getAlertText2().cStr(), ss.getAlertType().cStr(), ss.getAlertSize(), ss.getAlertStatus()}; + + // FrogPilot variables } if (!sm.updated("selfdriveState") && (sm.frame - started_frame) > 5 * UI_FREQ) { @@ -56,6 +65,8 @@ OnroadAlerts::Alert OnroadAlerts::getAlert(const SubMaster &sm, uint64_t started void OnroadAlerts::paintEvent(QPaintEvent *event) { if (alert.size == cereal::SelfdriveState::AlertSize::NONE) { + // FrogPilot variables + alertHeight = 0; return; } static std::map alert_heights = { @@ -63,7 +74,9 @@ void OnroadAlerts::paintEvent(QPaintEvent *event) { {cereal::SelfdriveState::AlertSize::MID, 420}, {cereal::SelfdriveState::AlertSize::FULL, height()}, }; - int h = alert_heights[alert.size]; + // FrogPilot variables + alertHeight = alert_heights[alert.size]; + int h = alertHeight; int margin = 40; int radius = 30; @@ -71,6 +84,8 @@ void OnroadAlerts::paintEvent(QPaintEvent *event) { margin = 0; radius = 0; } + // FrogPilot variables + alertHeight -= margin; QRect r = QRect(0 + margin, height() - h + margin, width() - margin*2, h - margin*2); QPainter p(this); diff --git a/selfdrive/ui/qt/onroad/alerts.h b/selfdrive/ui/qt/onroad/alerts.h index de38d8ff..e1837634 100644 --- a/selfdrive/ui/qt/onroad/alerts.h +++ b/selfdrive/ui/qt/onroad/alerts.h @@ -9,9 +9,12 @@ class OnroadAlerts : public QWidget { public: OnroadAlerts(QWidget *parent = 0) : QWidget(parent) {} - void updateState(const UIState &s); + void updateState(const UIState &s, const FrogPilotUIState &fs); void clear(); + // FrogPilot variables + int alertHeight; + protected: struct Alert { QString text1; @@ -36,4 +39,6 @@ protected: QColor bg; Alert alert = {}; + + // FrogPilot variables }; diff --git a/selfdrive/ui/qt/onroad/annotated_camera.cc b/selfdrive/ui/qt/onroad/annotated_camera.cc index f504ad69..d179d740 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.cc +++ b/selfdrive/ui/qt/onroad/annotated_camera.cc @@ -19,12 +19,21 @@ AnnotatedCameraWidget::AnnotatedCameraWidget(VisionStreamType type, QWidget *par experimental_btn = new ExperimentalButton(this); main_layout->addWidget(experimental_btn, 0, Qt::AlignTop | Qt::AlignRight); + + // FrogPilot variables } -void AnnotatedCameraWidget::updateState(const UIState &s) { +void AnnotatedCameraWidget::updateState(const UIState &s, const FrogPilotUIState &fs) { // update engageability/experimental mode button - experimental_btn->updateState(s); + experimental_btn->updateState(s, fs); dmon.updateState(s); + + // FrogPilot variables + const SubMaster &sm = *(s.sm); + + const cereal::CarState::Reader &carState = sm["carState"].getCarState(); + + frogpilot_nvg->experimentalButtonPosition = QPoint(experimental_btn->x(), experimental_btn->y()); } void AnnotatedCameraWidget::initializeGL() { @@ -129,11 +138,22 @@ void AnnotatedCameraWidget::paintGL() { painter.setRenderHint(QPainter::Antialiasing); painter.setPen(Qt::NoPen); + // FrogPilot variables + dmon.frogpilot_nvg = frogpilot_nvg; + hud.frogpilot_nvg = frogpilot_nvg; + model.frogpilot_nvg = frogpilot_nvg; + + experimental_btn->frogpilot_scene = frogpilot_scene; + model.frogpilot_scene = frogpilot_scene; + model.draw(painter, rect()); dmon.draw(painter, rect()); hud.updateState(*s); hud.draw(painter, rect()); + // FrogPilot variables + frogpilot_nvg->paintFrogPilotWidgets(painter, *s); + double cur_draw_t = millis_since_boot(); double dt = cur_draw_t - prev_draw_t; double fps = fps_filter.update(1. / dt * 1000); diff --git a/selfdrive/ui/qt/onroad/annotated_camera.h b/selfdrive/ui/qt/onroad/annotated_camera.h index d205579f..8953f4cc 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.h +++ b/selfdrive/ui/qt/onroad/annotated_camera.h @@ -8,12 +8,19 @@ #include "selfdrive/ui/qt/onroad/model.h" #include "selfdrive/ui/qt/widgets/cameraview.h" +#include "frogpilot/ui/qt/onroad/frogpilot_buttons.h" + class AnnotatedCameraWidget : public CameraWidget { Q_OBJECT public: explicit AnnotatedCameraWidget(VisionStreamType type, QWidget* parent = 0); - void updateState(const UIState &s); + void updateState(const UIState &s, const FrogPilotUIState &fs); + + // FrogPilot variables + FrogPilotAnnotatedCameraWidget *frogpilot_nvg; + + FrogPilotUIScene frogpilot_scene; private: QVBoxLayout *main_layout; @@ -26,6 +33,8 @@ private: int skip_frame_count = 0; bool wide_cam_requested = false; + // FrogPilot variables + protected: void paintGL() override; void initializeGL() override; diff --git a/selfdrive/ui/qt/onroad/buttons.cc b/selfdrive/ui/qt/onroad/buttons.cc index 32e58c9d..45901634 100644 --- a/selfdrive/ui/qt/onroad/buttons.cc +++ b/selfdrive/ui/qt/onroad/buttons.cc @@ -22,17 +22,20 @@ ExperimentalButton::ExperimentalButton(QWidget *parent) : experimental_mode(fals engage_img = loadPixmap("../assets/icons/chffr_wheel.png", {img_size, img_size}); experimental_img = loadPixmap("../assets/icons/experimental.svg", {img_size, img_size}); QObject::connect(this, &QPushButton::clicked, this, &ExperimentalButton::changeMode); + + // FrogPilot variables } void ExperimentalButton::changeMode() { const auto cp = (*uiState()->sm)["carParams"].getCarParams(); bool can_change = hasLongitudinalControl(cp) && params.getBool("ExperimentalModeConfirmed"); if (can_change) { + // FrogPilot variables params.putBool("ExperimentalMode", !experimental_mode); } } -void ExperimentalButton::updateState(const UIState &s) { +void ExperimentalButton::updateState(const UIState &s, const FrogPilotUIState &fs) { const auto cs = (*s.sm)["selfdriveState"].getSelfdriveState(); bool eng = cs.getEngageable() || cs.getEnabled(); if ((cs.getExperimentalMode() != experimental_mode) || (eng != engageable)) { @@ -40,6 +43,9 @@ void ExperimentalButton::updateState(const UIState &s) { experimental_mode = cs.getExperimentalMode(); update(); } + + // FrogPilot variables + const cereal::CarState::Reader &carState = (*s.sm)["carState"].getCarState(); } void ExperimentalButton::paintEvent(QPaintEvent *event) { @@ -47,3 +53,7 @@ void ExperimentalButton::paintEvent(QPaintEvent *event) { QPixmap img = experimental_mode ? experimental_img : engage_img; drawIcon(p, QPoint(btn_size / 2, btn_size / 2), img, QColor(0, 0, 0, 166), (isDown() || !engageable) ? 0.6 : 1.0); } + +// FrogPilot variables +void ExperimentalButton::showEvent(QShowEvent *event) { +} diff --git a/selfdrive/ui/qt/onroad/buttons.h b/selfdrive/ui/qt/onroad/buttons.h index 9c91bc3c..ec31c937 100644 --- a/selfdrive/ui/qt/onroad/buttons.h +++ b/selfdrive/ui/qt/onroad/buttons.h @@ -12,7 +12,10 @@ class ExperimentalButton : public QPushButton { public: explicit ExperimentalButton(QWidget *parent = 0); - void updateState(const UIState &s); + void updateState(const UIState &s, const FrogPilotUIState &fs); + + // FrogPilot variables + FrogPilotUIScene frogpilot_scene; private: void paintEvent(QPaintEvent *event) override; @@ -23,6 +26,9 @@ private: QPixmap experimental_img; bool experimental_mode; bool engageable; + + // FrogPilot variables + void showEvent(QShowEvent *event) override; }; void drawIcon(QPainter &p, const QPoint ¢er, const QPixmap &img, const QBrush &bg, float opacity); diff --git a/selfdrive/ui/qt/onroad/driver_monitoring.cc b/selfdrive/ui/qt/onroad/driver_monitoring.cc index 49f2c950..9cc4c66e 100644 --- a/selfdrive/ui/qt/onroad/driver_monitoring.cc +++ b/selfdrive/ui/qt/onroad/driver_monitoring.cc @@ -73,6 +73,8 @@ void DriverMonitorRenderer::draw(QPainter &painter, const QRect &surface_rect) { float y = surface_rect.height() - offset; float opacity = is_active ? 0.65f : 0.2f; + // FrogPilot variables + drawIcon(painter, QPoint(x, y), dm_img, QColor(0, 0, 0, 70), opacity); QPointF keypoints[std::size(DEFAULT_FACE_KPTS_3D)]; @@ -104,4 +106,11 @@ void DriverMonitorRenderer::draw(QPainter &painter, const QRect &surface_rect) { painter.drawArc(QRectF(x - arc_l / 2, std::min(y + delta_y, y), arc_l, std::abs(delta_y)), (driver_pose_sins[0] > 0 ? 0 : 180) * 16, 180 * 16); painter.restore(); + + // FrogPilot variables + if (frogpilot_nvg) { + frogpilot_nvg->dmIconPosition.setX(x); + frogpilot_nvg->dmIconPosition.setY(y); + frogpilot_nvg->rightHandDM = is_rhd; + } } diff --git a/selfdrive/ui/qt/onroad/driver_monitoring.h b/selfdrive/ui/qt/onroad/driver_monitoring.h index 47db151c..6a27fabc 100644 --- a/selfdrive/ui/qt/onroad/driver_monitoring.h +++ b/selfdrive/ui/qt/onroad/driver_monitoring.h @@ -4,12 +4,17 @@ #include #include "selfdrive/ui/ui.h" +#include "frogpilot/ui/qt/onroad/frogpilot_annotated_camera.h" + class DriverMonitorRenderer { public: DriverMonitorRenderer(); void updateState(const UIState &s); void draw(QPainter &painter, const QRect &surface_rect); + // FrogPilot variables + FrogPilotAnnotatedCameraWidget *frogpilot_nvg; + private: float driver_pose_vals[3] = {}; float driver_pose_diff[3] = {}; diff --git a/selfdrive/ui/qt/onroad/hud.cc b/selfdrive/ui/qt/onroad/hud.cc index 4cfa3d0e..0ac34ef9 100644 --- a/selfdrive/ui/qt/onroad/hud.cc +++ b/selfdrive/ui/qt/onroad/hud.cc @@ -60,6 +60,9 @@ void HudRenderer::drawSetSpeed(QPainter &p, const QRect &surface_rect) { // Draw outer box + border to contain set speed const QSize default_size = {172, 204}; QSize set_speed_size = is_metric ? QSize(200, 204) : default_size; + + // FrogPilot variables + QRect set_speed_rect(QPoint(60 + (default_size.width() - set_speed_size.width()) / 2, 45), set_speed_size); // Draw set speed box @@ -91,6 +94,12 @@ void HudRenderer::drawSetSpeed(QPainter &p, const QRect &surface_rect) { p.setFont(InterFont(90, QFont::Bold)); p.setPen(set_speed_color); p.drawText(set_speed_rect.adjusted(0, 77, 0, 0), Qt::AlignTop | Qt::AlignHCenter, setSpeedStr); + + // FrogPilot variables + frogpilot_nvg->defaultSize = default_size; + frogpilot_nvg->isCruiseSet = is_cruise_set; + frogpilot_nvg->setSpeedRect = set_speed_rect; + frogpilot_nvg->speed = speed; } void HudRenderer::drawCurrentSpeed(QPainter &p, const QRect &surface_rect) { diff --git a/selfdrive/ui/qt/onroad/hud.h b/selfdrive/ui/qt/onroad/hud.h index b2ac379d..4457f24f 100644 --- a/selfdrive/ui/qt/onroad/hud.h +++ b/selfdrive/ui/qt/onroad/hud.h @@ -3,6 +3,8 @@ #include #include "selfdrive/ui/ui.h" +#include "frogpilot/ui/qt/onroad/frogpilot_annotated_camera.h" + class HudRenderer : public QObject { Q_OBJECT @@ -11,6 +13,9 @@ public: void updateState(const UIState &s); void draw(QPainter &p, const QRect &surface_rect); + // FrogPilot variables + FrogPilotAnnotatedCameraWidget *frogpilot_nvg; + private: void drawSetSpeed(QPainter &p, const QRect &surface_rect); void drawCurrentSpeed(QPainter &p, const QRect &surface_rect); diff --git a/selfdrive/ui/qt/onroad/model.cc b/selfdrive/ui/qt/onroad/model.cc index 52902abd..478da7a7 100644 --- a/selfdrive/ui/qt/onroad/model.cc +++ b/selfdrive/ui/qt/onroad/model.cc @@ -42,12 +42,18 @@ void ModelRenderer::draw(QPainter &painter, const QRect &surface_rect) { const auto &lead_two = radar_state.getLeadTwo(); if (lead_one.getStatus()) { drawLead(painter, lead_one, lead_vertices[0], surface_rect); - } + } else { + // FrogPilot variables if (lead_two.getStatus() && (std::abs(lead_one.getDRel() - lead_two.getDRel()) > 3.0)) { drawLead(painter, lead_two, lead_vertices[1], surface_rect); } + + // FrogPilot variables + SubMaster &fpsm = *(frogpilotUIState()->sm); } + // FrogPilot variables + painter.restore(); } @@ -88,7 +94,12 @@ void ModelRenderer::update_model(const cereal::ModelDataV2::Reader &model, const max_distance = std::clamp((float)(lead_d - fmin(lead_d * 0.35, 10.)), 0.0f, max_distance); } max_idx = get_path_length_idx(model_position, max_distance); + // FrogPilot variables mapLineToPolygon(model_position, 0.9, path_offset_z, &track_vertices, max_idx, false); + + // FrogPilot variables + FrogPilotUIState *fs = frogpilotUIState(); + SubMaster &fpsm = *(fs->sm); } void ModelRenderer::drawLaneLines(QPainter &painter) { @@ -140,6 +151,8 @@ void ModelRenderer::drawPath(QPainter &painter, const cereal::ModelDataV2::Reade painter.setBrush(bg); painter.drawPolygon(track_vertices); + + // FrogPilot variables } void ModelRenderer::updatePathGradient(QLinearGradient &bg) { @@ -217,6 +230,8 @@ void ModelRenderer::drawLead(QPainter &painter, const cereal::RadarState::LeadDa QPointF chevron[] = {{x + (sz * 1.25), y + sz}, {x, y}, {x - (sz * 1.25), y + sz}}; painter.setBrush(QColor(201, 34, 49, fillAlpha)); painter.drawPolygon(chevron, std::size(chevron)); + + // FrogPilot variables } // Projects a point in car to space to the corresponding point in full frame image space. @@ -248,3 +263,5 @@ void ModelRenderer::mapLineToPolygon(const cereal::XYZTData::Reader &line, float } } } + +// FrogPilot variables diff --git a/selfdrive/ui/qt/onroad/model.h b/selfdrive/ui/qt/onroad/model.h index 79547e4b..ddad6a13 100644 --- a/selfdrive/ui/qt/onroad/model.h +++ b/selfdrive/ui/qt/onroad/model.h @@ -5,12 +5,19 @@ #include "selfdrive/ui/ui.h" +#include "frogpilot/ui/qt/onroad/frogpilot_annotated_camera.h" + class ModelRenderer { public: ModelRenderer() {} void setTransform(const Eigen::Matrix3f &transform) { car_space_transform = transform; } void draw(QPainter &painter, const QRect &surface_rect); + // FrogPilot variables + FrogPilotAnnotatedCameraWidget *frogpilot_nvg; + + FrogPilotUIScene frogpilot_scene; + private: bool mapToScreen(float in_x, float in_y, float in_z, QPointF *out); void mapLineToPolygon(const cereal::XYZTData::Reader &line, float y_off, float z_off, @@ -36,4 +43,6 @@ private: QPointF lead_vertices[2] = {}; Eigen::Matrix3f car_space_transform = Eigen::Matrix3f::Zero(); QRectF clip_region; + + // FrogPilot variables }; diff --git a/selfdrive/ui/qt/onroad/onroad_home.cc b/selfdrive/ui/qt/onroad/onroad_home.cc index 080f9bd5..e9bcfb02 100644 --- a/selfdrive/ui/qt/onroad/onroad_home.cc +++ b/selfdrive/ui/qt/onroad/onroad_home.cc @@ -37,15 +37,27 @@ OnroadWindow::OnroadWindow(QWidget *parent) : QWidget(parent) { setAttribute(Qt::WA_OpaquePaintEvent); QObject::connect(uiState(), &UIState::uiUpdate, this, &OnroadWindow::updateState); QObject::connect(uiState(), &UIState::offroadTransition, this, &OnroadWindow::offroadTransition); + + // FrogPilot variables + frogpilot_nvg = new FrogPilotAnnotatedCameraWidget(this); + frogpilot_onroad = new FrogPilotOnroadWindow(this); + frogpilot_onroad->setAttribute(Qt::WA_TransparentForMouseEvents, true); + + stacked_layout->addWidget(frogpilot_nvg); + stacked_layout->addWidget(frogpilot_onroad); + + frogpilot_onroad->raise(); + + nvg->frogpilot_nvg = frogpilot_nvg; } -void OnroadWindow::updateState(const UIState &s) { +void OnroadWindow::updateState(const UIState &s, const FrogPilotUIState &fs) { if (!s.scene.started) { return; } - alerts->updateState(s); - nvg->updateState(s); + alerts->updateState(s, fs); + nvg->updateState(s, fs); QColor bgColor = bg_colors[s.status]; if (bg != bgColor) { @@ -53,6 +65,24 @@ void OnroadWindow::updateState(const UIState &s) { bg = bgColor; update(); } + + // FrogPilot variables + const FrogPilotUIScene &frogpilot_scene = fs.frogpilot_scene; + + frogpilot_nvg->alertHeight = alerts->alertHeight; + + frogpilot_onroad->bg = bg; + + nvg->frogpilot_nvg = frogpilot_nvg; + + nvg->frogpilot_scene = frogpilot_scene; + frogpilot_nvg->frogpilot_scene = frogpilot_scene; + frogpilot_onroad->frogpilot_scene = frogpilot_scene; + + frogpilot_onroad->setGeometry(rect()); + + frogpilot_nvg->updateState(s, fs); + frogpilot_onroad->updateState(s, fs); } void OnroadWindow::offroadTransition(bool offroad) { @@ -63,3 +93,15 @@ void OnroadWindow::paintEvent(QPaintEvent *event) { QPainter p(this); p.fillRect(rect(), QColor(bg.red(), bg.green(), bg.blue(), 255)); } + +// FrogPilot variables +void OnroadWindow::mousePressEvent(QMouseEvent* mouseEvent) { + frogpilot_nvg->mousePressEvent(mouseEvent); + + if (mouseEvent->isAccepted()) { + return; + } + + // propagation event to parent(HomeWindow) + QWidget::mousePressEvent(mouseEvent); +} diff --git a/selfdrive/ui/qt/onroad/onroad_home.h b/selfdrive/ui/qt/onroad/onroad_home.h index c321d2d4..484ceaad 100644 --- a/selfdrive/ui/qt/onroad/onroad_home.h +++ b/selfdrive/ui/qt/onroad/onroad_home.h @@ -3,6 +3,8 @@ #include "selfdrive/ui/qt/onroad/alerts.h" #include "selfdrive/ui/qt/onroad/annotated_camera.h" +#include "frogpilot/ui/qt/onroad/frogpilot_onroad.h" + class OnroadWindow : public QWidget { Q_OBJECT @@ -16,7 +18,13 @@ private: QColor bg = bg_colors[STATUS_DISENGAGED]; QHBoxLayout* split; + // FrogPilot variables + void mousePressEvent(QMouseEvent* mouseEvent); + + FrogPilotAnnotatedCameraWidget *frogpilot_nvg; + FrogPilotOnroadWindow *frogpilot_onroad; + private slots: void offroadTransition(bool offroad); - void updateState(const UIState &s); + void updateState(const UIState &s, const FrogPilotUIState &fs); }; diff --git a/selfdrive/ui/qt/sidebar.cc b/selfdrive/ui/qt/sidebar.cc index e9c6a2f7..2d217d00 100644 --- a/selfdrive/ui/qt/sidebar.cc +++ b/selfdrive/ui/qt/sidebar.cc @@ -40,9 +40,15 @@ Sidebar::Sidebar(QWidget *parent) : QFrame(parent), onroad(false), flag_pressed( QObject::connect(uiState(), &UIState::uiUpdate, this, &Sidebar::updateState); pm = std::make_unique(std::vector{"bookmarkButton"}); + + // FrogPilot variables } void Sidebar::mousePressEvent(QMouseEvent *event) { + // FrogPilot variables + FrogPilotUIState *fs = frogpilotUIState(); + FrogPilotUIScene &frogpilot_scene = fs->frogpilot_scene; + if (onroad && home_btn.contains(event->pos())) { flag_pressed = true; update(); @@ -76,9 +82,14 @@ void Sidebar::offroadTransition(bool offroad) { update(); } -void Sidebar::updateState(const UIState &s) { +void Sidebar::updateState(const UIState &s, const FrogPilotUIState &fs) { if (!isVisible()) return; + // FrogPilot variables + const FrogPilotUIScene &frogpilot_scene = fs.frogpilot_scene; + + const SubMaster &fpsm = *(fs.sm); + auto &sm = *(s.sm); networking = networking ? networking : window()->findChild(""); @@ -115,6 +126,8 @@ void Sidebar::updateState(const UIState &s) { setProperty("pandaStatus", QVariant::fromValue(pandaStatus)); setProperty("recordingAudio", s.scene.recording_audio); + + // FrogPilot variables } void Sidebar::paintEvent(QPaintEvent *event) { @@ -139,6 +152,10 @@ void Sidebar::paintEvent(QPaintEvent *event) { } p.setOpacity(1.0); + // FrogPilot variables + FrogPilotUIState *fs = frogpilotUIState(); + FrogPilotUIScene &frogpilot_scene = fs->frogpilot_scene; + // network int x = 58; const QColor gray(0x54, 0x54, 0x54); @@ -163,3 +180,7 @@ void Sidebar::paintEvent(QPaintEvent *event) { drawMetric(p, panda_status.first, panda_status.second, 496); drawMetric(p, connect_status.first, connect_status.second, 654); } + +// FrogPilot variables +void Sidebar::showEvent(QShowEvent *event) { +} diff --git a/selfdrive/ui/qt/sidebar.h b/selfdrive/ui/qt/sidebar.h index 6a2b7b69..24611786 100644 --- a/selfdrive/ui/qt/sidebar.h +++ b/selfdrive/ui/qt/sidebar.h @@ -20,6 +20,8 @@ class Sidebar : public QFrame { Q_PROPERTY(int netStrength MEMBER net_strength NOTIFY valueChanged); Q_PROPERTY(bool recordingAudio MEMBER recording_audio NOTIFY valueChanged); + // FrogPilot properties + public: explicit Sidebar(QWidget* parent = 0); @@ -29,7 +31,7 @@ signals: public slots: void offroadTransition(bool offroad); - void updateState(const UIState &s); + void updateState(const UIState &s, const FrogPilotUIState &fs); protected: void paintEvent(QPaintEvent *event) override; @@ -60,7 +62,14 @@ protected: QString net_type; int net_strength = 0; + // FrogPilot variables + private: std::unique_ptr pm; Networking *networking = nullptr; + + // FrogPilot variables + void showEvent(QShowEvent *event); + + Params params; }; diff --git a/selfdrive/ui/qt/widgets/controls.h b/selfdrive/ui/qt/widgets/controls.h index 3342de53..4dc094a2 100644 --- a/selfdrive/ui/qt/widgets/controls.h +++ b/selfdrive/ui/qt/widgets/controls.h @@ -67,6 +67,8 @@ public slots: signals: void showDescriptionEvent(); + // FrogPilot variables + protected: AbstractControl(const QString &title, const QString &desc = "", const QString &icon = "", QWidget *parent = nullptr); void hideEvent(QHideEvent *e) override; @@ -132,6 +134,8 @@ public: toggle.update(); } + // FrogPilot variables + signals: void toggleFlipped(bool state); diff --git a/selfdrive/ui/qt/widgets/input.cc b/selfdrive/ui/qt/widgets/input.cc index 0cbf1493..cec7c418 100644 --- a/selfdrive/ui/qt/widgets/input.cc +++ b/selfdrive/ui/qt/widgets/input.cc @@ -141,6 +141,8 @@ InputDialog::InputDialog(const QString &title, QWidget *parent, const QString &s QObject::connect(k, &Keyboard::emitEnter, this, &InputDialog::handleEnter); QObject::connect(k, &Keyboard::emitBackspace, this, [=]() { line->backspace(); + + // FrogPilot variables }); QObject::connect(k, &Keyboard::emitKey, this, [=](const QString &key) { line->insert(key.left(1)); @@ -154,6 +156,9 @@ QString InputDialog::getText(const QString &prompt, QWidget *parent, const QStri InputDialog d(prompt, parent, subtitle, secret); d.line->setText(defaultText); d.setMinLength(minLength); + + // FrogPilot variables + const int ret = d.exec(); return ret ? d.text() : QString(); } @@ -186,6 +191,8 @@ void InputDialog::setMinLength(int length) { minLength = length; } +// FrogPilot variables + // ConfirmationDialog ConfirmationDialog::ConfirmationDialog(const QString &prompt_text, const QString &confirm_text, const QString &cancel_text, diff --git a/selfdrive/ui/qt/widgets/input.h b/selfdrive/ui/qt/widgets/input.h index 089e54e4..2cbbe21a 100644 --- a/selfdrive/ui/qt/widgets/input.h +++ b/selfdrive/ui/qt/widgets/input.h @@ -33,6 +33,8 @@ public: void setMinLength(int length); void show(); + // FrogPilot variables + private: int minLength; QLineEdit *line; @@ -42,6 +44,8 @@ private: QVBoxLayout *main_layout; QPushButton *eye_btn; + // FrogPilot variables + private slots: void handleEnter(); diff --git a/selfdrive/ui/qt/widgets/ssh_keys.cc b/selfdrive/ui/qt/widgets/ssh_keys.cc index 26743952..83213d59 100644 --- a/selfdrive/ui/qt/widgets/ssh_keys.cc +++ b/selfdrive/ui/qt/widgets/ssh_keys.cc @@ -19,6 +19,7 @@ SshControl::SshControl() : } else { params.remove("GithubUsername"); params.remove("GithubSshKeys"); + refresh(); } }); diff --git a/selfdrive/ui/qt/window.cc b/selfdrive/ui/qt/window.cc index 6b579fcc..fd416b5b 100644 --- a/selfdrive/ui/qt/window.cc +++ b/selfdrive/ui/qt/window.cc @@ -79,6 +79,10 @@ void MainWindow::closeSettings() { } bool MainWindow::eventFilter(QObject *obj, QEvent *event) { + // FrogPilot variables + FrogPilotUIState &fs = *frogpilotUIState(); + FrogPilotUIScene &frogpilot_scene = fs.frogpilot_scene; + bool ignore = false; switch (event->type()) { case QEvent::TouchBegin: diff --git a/selfdrive/ui/qt/window.h b/selfdrive/ui/qt/window.h index 05b61e1f..87e907f8 100644 --- a/selfdrive/ui/qt/window.h +++ b/selfdrive/ui/qt/window.h @@ -22,4 +22,6 @@ private: HomeWindow *homeWindow; SettingsWindow *settingsWindow; OnboardingWindow *onboardingWindow; + + // FrogPilot variables }; diff --git a/selfdrive/ui/soundd.py b/selfdrive/ui/soundd.py index 704a6a59..c5797333 100644 --- a/selfdrive/ui/soundd.py +++ b/selfdrive/ui/soundd.py @@ -30,6 +30,8 @@ if HARDWARE.get_device_type() in ("tici", "tizi"): AudibleAlert = car.CarControl.HUDControl.AudibleAlert +# FrogPilot variables + sound_list: dict[int, tuple[str, int | None, float]] = { # AudibleAlert, file name, play count (none for infinite) @@ -43,6 +45,8 @@ sound_list: dict[int, tuple[str, int | None, float]] = { AudibleAlert.warningSoft: ("warning_soft.wav", None, MAX_VOLUME), AudibleAlert.warningImmediate: ("warning_immediate.wav", None, MAX_VOLUME), + + # FrogPilot variables } if HARDWARE.get_device_type() in ("tici", "tizi"): sound_list.update({ @@ -72,6 +76,9 @@ class Soundd: self.spl_filter_weighted = FirstOrderFilter(0, 2.5, FILTER_DT, initialized=False) + # FrogPilot variables + self.update_frogpilot_sounds() + def load_sounds(self): self.loaded_sounds: dict[int, np.ndarray] = {} @@ -122,6 +129,9 @@ class Soundd: def get_audible_alert(self, sm): if sm.updated['selfdriveState']: new_alert = sm['selfdriveState'].alertSound.raw + + # FrogPilot variables + self.update_alert(new_alert) elif check_selfdrive_timeout_alert(sm): self.update_alert(AudibleAlert.warningImmediate) @@ -147,6 +157,8 @@ class Soundd: sm = messaging.SubMaster(['selfdriveState', 'soundPressure']) + # FrogPilot variables + with self.get_stream(sd) as stream: rk = Ratekeeper(20) @@ -164,6 +176,10 @@ class Soundd: assert stream.active + # FrogPilot variables + + def update_frogpilot_sounds(self): + def main(): s = Soundd() diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 4f8bd7dd..4109a3b6 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -18,7 +18,7 @@ static void update_sockets(UIState *s) { s->sm->update(0); } -static void update_state(UIState *s) { +static void update_state(UIState *s, FrogPilotUIState *fs) { SubMaster &sm = *(s->sm); UIScene &scene = s->scene; @@ -63,6 +63,20 @@ static void update_state(UIState *s) { auto params = Params(); scene.recording_audio = params.getBool("RecordAudio") && scene.started; + + // FrogPilot variables + FrogPilotUIScene &frogpilot_scene = fs->frogpilot_scene; + + if (sm.updated("carState")) { + const cereal::CarState::Reader &carState = sm["carState"].getCarState(); + frogpilot_scene.parked = carState.getGearShifter() == cereal::CarState::GearShifter::PARK; + frogpilot_scene.reverse = carState.getGearShifter() == cereal::CarState::GearShifter::REVERSE; + frogpilot_scene.standstill = carState.getStandstill() && !frogpilot_scene.reverse; + } + + if (scene.started) { + frogpilot_scene.started_timer += 1; + } } void ui_update_params(UIState *s) { @@ -70,15 +84,23 @@ void ui_update_params(UIState *s) { s->scene.is_metric = params.getBool("IsMetric"); } -void UIState::updateStatus() { +void UIState::updateStatus(FrogPilotUIState *fs) { + // FrogPilot variables + FrogPilotUIScene &frogpilot_scene = fs->frogpilot_scene; + if (scene.started && sm->updated("selfdriveState")) { auto ss = (*sm)["selfdriveState"].getSelfdriveState(); auto state = ss.getState(); + + // FrogPilot variables + if (state == cereal::SelfdriveState::OpenpilotState::PRE_ENABLED || state == cereal::SelfdriveState::OpenpilotState::OVERRIDING) { status = STATUS_OVERRIDE; } else { status = ss.getEnabled() ? STATUS_ENGAGED : STATUS_DISENGAGED; } + + // FrogPilot variables } if (engaged() != engaged_prev) { @@ -94,6 +116,8 @@ void UIState::updateStatus() { } started_prev = scene.started; emit offroadTransition(!scene.started); + + // FrogPilot variables } } @@ -114,13 +138,19 @@ UIState::UIState(QObject *parent) : QObject(parent) { void UIState::update() { update_sockets(this); - update_state(this); - updateStatus(); + update_state(this, frogpilotUIState()); + updateStatus(frogpilotUIState()); if (sm->frame % UI_FREQ == 0) { watchdog_kick(nanos_since_boot()); } - emit uiUpdate(*this); + emit uiUpdate(*this, *frogpilotUIState()); + + // FrogPilot variables + FrogPilotUIState *fs = frogpilotUIState(); + FrogPilotUIScene &frogpilot_scene = fs->frogpilot_scene; + + fs->update(); } Device::Device(QObject *parent) : brightness_filter(BACKLIGHT_OFFROAD, BACKLIGHT_TS, BACKLIGHT_DT), QObject(parent) { @@ -130,9 +160,9 @@ Device::Device(QObject *parent) : brightness_filter(BACKLIGHT_OFFROAD, BACKLIGHT QObject::connect(uiState(), &UIState::uiUpdate, this, &Device::update); } -void Device::update(const UIState &s) { - updateBrightness(s); - updateWakefulness(s); +void Device::update(const UIState &s, const FrogPilotUIState &fs) { + updateBrightness(s, fs); + updateWakefulness(s, fs); } void Device::setAwake(bool on) { @@ -147,11 +177,16 @@ void Device::setAwake(bool on) { void Device::resetInteractiveTimeout(int timeout) { if (timeout == -1) { timeout = (ignition_on ? 10 : 30); + } else { + // FrogPilot variables } interactive_timeout = timeout * UI_FREQ; } -void Device::updateBrightness(const UIState &s) { +void Device::updateBrightness(const UIState &s, const FrogPilotUIState &fs) { + // FrogPilot variables + const FrogPilotUIScene &frogpilot_scene = fs.frogpilot_scene; + float clipped_brightness = offroad_brightness; if (s.scene.started && s.scene.light_sensor >= 0) { clipped_brightness = s.scene.light_sensor; @@ -180,7 +215,10 @@ void Device::updateBrightness(const UIState &s) { } } -void Device::updateWakefulness(const UIState &s) { +void Device::updateWakefulness(const UIState &s, const FrogPilotUIState &fs) { + // FrogPilot variables + const FrogPilotUIScene &frogpilot_scene = fs.frogpilot_scene; + bool ignition_just_turned_off = !s.scene.ignition && ignition_on; ignition_on = s.scene.ignition; diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index b3c482aa..b992d19a 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -15,6 +15,8 @@ #include "system/hardware/hw.h" #include "selfdrive/ui/qt/prime_state.h" +#include "frogpilot/ui/frogpilot_ui.h" + const int UI_BORDER_SIZE = 30; const int UI_HEADER_HEIGHT = 420; @@ -42,12 +44,18 @@ typedef enum UIStatus { STATUS_DISENGAGED, STATUS_OVERRIDE, STATUS_ENGAGED, + + // FrogPilot variables + STATUS_EXPERIMENTAL_MODE_ENABLED, } UIStatus; const QColor bg_colors [] = { [STATUS_DISENGAGED] = QColor(0x17, 0x33, 0x49, 0xc8), [STATUS_OVERRIDE] = QColor(0x91, 0x9b, 0x95, 0xf1), [STATUS_ENGAGED] = QColor(0x17, 0x86, 0x44, 0xf1), + + // FrogPilot variables + [STATUS_EXPERIMENTAL_MODE_ENABLED] = QColor(0xda, 0x6f, 0x25, 0xf1), }; typedef struct UIScene { @@ -67,7 +75,7 @@ class UIState : public QObject { public: UIState(QObject* parent = 0); - void updateStatus(); + void updateStatus(FrogPilotUIState *fs); inline bool engaged() const { return scene.started && (*sm)["selfdriveState"].getSelfdriveState().getEnabled(); } @@ -79,7 +87,7 @@ public: PrimeState *prime_state; signals: - void uiUpdate(const UIState &s); + void uiUpdate(const UIState &s, const FrogPilotUIState &fs); void offroadTransition(bool offroad); void engagedChanged(bool engaged); @@ -115,8 +123,8 @@ private: FirstOrderFilter brightness_filter; QFuture brightness_future; - void updateBrightness(const UIState &s); - void updateWakefulness(const UIState &s); + void updateBrightness(const UIState &s, const FrogPilotUIState &fs); + void updateWakefulness(const UIState &s, const FrogPilotUIState &fs); void setAwake(bool on); signals: @@ -125,7 +133,7 @@ signals: public slots: void resetInteractiveTimeout(int timeout = -1); - void update(const UIState &s); + void update(const UIState &s, const FrogPilotUIState &fs); }; Device *device(); diff --git a/system/hardware/hardwared.py b/system/hardware/hardwared.py index 8ddc4da2..313f9a59 100755 --- a/system/hardware/hardwared.py +++ b/system/hardware/hardwared.py @@ -212,6 +212,8 @@ def hardware_thread(end_event, hw_queue) -> None: fan_controller = None + # FrogPilot variables + while not end_event.is_set(): sm.update(PANDA_STATES_TIMEOUT) @@ -292,6 +294,8 @@ def hardware_thread(end_event, hw_queue) -> None: if fan_controller is not None: msg.deviceState.fanSpeedPercentDesired = fan_controller.update(all_comp_temp, onroad_conditions["ignition"]) + # FrogPilot variables + is_offroad_for_5_min = (started_ts is None) and ((not started_seen) or (off_ts is None) or (time.monotonic() - off_ts > 60 * 5)) if is_offroad_for_5_min and offroad_comp_temp > OFFROAD_DANGER_TEMP: # if device is offroad and already hot without the extra onroad load, @@ -341,6 +345,8 @@ def hardware_thread(end_event, hw_queue) -> None: if started_ts is None: should_start = should_start and all(startup_conditions.values()) + # FrogPilot variables + if should_start != should_start_prev or (count == 0): params.put_bool("IsEngaged", False) engaged_prev = False @@ -410,6 +416,8 @@ def hardware_thread(end_event, hw_queue) -> None: msg.deviceState.thermalStatus = thermal_status pm.send("deviceState", msg) + # FrogPilot variables + # Log to statsd statlog.gauge("free_space_percent", msg.deviceState.freeSpacePercent) statlog.gauge("gpu_usage_percent", msg.deviceState.gpuUsagePercent) @@ -463,6 +471,8 @@ def hardware_thread(end_event, hw_queue) -> None: count += 1 should_start_prev = should_start + # FrogPilot variables + def main(): hw_queue = queue.Queue(maxsize=1) diff --git a/system/hardware/hw.h b/system/hardware/hw.h index d2083a59..f837a4fa 100644 --- a/system/hardware/hw.h +++ b/system/hardware/hw.h @@ -26,7 +26,14 @@ namespace Path { if (const char *env = getenv("LOG_ROOT")) { return env; } - return Hardware::PC() ? Path::comma_home() + "/media/0/realdata" : "/data/media/0/realdata"; + + if (Hardware::PC()) { + return Path::comma_home() + "/media/0/realdata"; + } + + // FrogPilot variables + + return "/data/media/0/realdata"; } inline std::string params() { diff --git a/system/loggerd/config.py b/system/loggerd/config.py index e1c47c76..ece38d0c 100644 --- a/system/loggerd/config.py +++ b/system/loggerd/config.py @@ -27,3 +27,6 @@ def get_available_bytes(default: int) -> int: available_bytes = default return available_bytes + + +# FrogPilot variables diff --git a/system/loggerd/uploader.py b/system/loggerd/uploader.py index 5b6234e1..6f8fd9a5 100755 --- a/system/loggerd/uploader.py +++ b/system/loggerd/uploader.py @@ -248,6 +248,9 @@ def main(exit_event: threading.Event = None) -> None: uploader = Uploader(dongle_id, Paths.log_root()) backoff = 0.1 + + # FrogPilot variables + while not exit_event.is_set(): sm.update(0) offroad = params.get_bool("IsOffroad") @@ -268,6 +271,8 @@ def main(exit_event: threading.Event = None) -> None: if allow_sleep: time.sleep(backoff + random.uniform(0, backoff)) + # FrogPilot variables + if __name__ == "__main__": main() diff --git a/system/manager/manager.py b/system/manager/manager.py index 1d3c0cc7..5e701887 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -21,6 +21,8 @@ from openpilot.common.swaglog import cloudlog, add_file_handler from openpilot.system.version import get_build_metadata, terms_version, training_version from openpilot.system.hardware.hw import Paths +from openpilot.frogpilot.common.frogpilot_functions import frogpilot_boot_functions, install_frogpilot, uninstall_frogpilot + def manager_init() -> None: save_bootlog() @@ -38,6 +40,8 @@ def manager_init() -> None: if params.get_bool("RecordFrontLock"): params.put_bool("RecordFront", True) + # FrogPilot variables + # set unset params to their default value for k in params.all_keys(): default_value = params.get_default_value(k) @@ -93,6 +97,10 @@ def manager_init() -> None: for p in managed_processes.values(): p.prepare() + # FrogPilot variables + install_frogpilot() + frogpilot_boot_functions() + def manager_cleanup() -> None: # send signals to kill all procs @@ -129,6 +137,8 @@ def manager_thread() -> None: started_prev = False ignition_prev = False + # FrogPilot variables + while True: sm.update(1000) @@ -136,9 +146,13 @@ def manager_thread() -> None: if started and not started_prev: params.clear_all(ParamKeyFlag.CLEAR_ON_ONROAD_TRANSITION) + + # FrogPilot variables elif not started and started_prev: params.clear_all(ParamKeyFlag.CLEAR_ON_OFFROAD_TRANSITION) + # FrogPilot variables + ignition = any(ps.ignitionLine or ps.ignitionCan for ps in sm['pandaStates'] if ps.pandaType != log.PandaState.PandaType.unknown) if ignition and not ignition_prev: params.clear_all(ParamKeyFlag.CLEAR_ON_IGNITION_ON) @@ -181,6 +195,8 @@ def manager_thread() -> None: if shutdown: break + # FrogPilot variables + def main() -> None: manager_init() @@ -201,7 +217,7 @@ def main() -> None: params = Params() if params.get_bool("DoUninstall"): cloudlog.warning("uninstalling") - HARDWARE.uninstall() + uninstall_frogpilot() elif params.get_bool("DoReboot"): cloudlog.warning("reboot") HARDWARE.reboot() diff --git a/system/manager/process_config.py b/system/manager/process_config.py index 5bb9ee25..54efc45b 100644 --- a/system/manager/process_config.py +++ b/system/manager/process_config.py @@ -4,7 +4,7 @@ import platform from cereal import car from openpilot.common.params import Params -from openpilot.system.hardware import PC, TICI +from openpilot.system.hardware import HARDWARE, PC, TICI from openpilot.system.manager.process import PythonProcess, NativeProcess, DaemonProcess WEBCAM = os.getenv("USE_WEBCAM") is not None @@ -61,6 +61,8 @@ def or_(*fns): def and_(*fns): return lambda *args: operator.and_(*(fn(*args) for fn in fns)) +# FrogPilot variables + procs = [ DaemonProcess("manage_athenad", "system.athena.manage_athenad", "AthenadPid"), @@ -80,7 +82,6 @@ procs = [ PythonProcess("dmonitoringmodeld", "selfdrive.modeld.dmonitoringmodeld", driverview, enabled=(WEBCAM or not PC)), PythonProcess("sensord", "system.sensord.sensord", only_onroad, enabled=not PC), - NativeProcess("ui", "selfdrive/ui", ["./ui"], always_run, watchdog_max_dt=(5 if not PC else None)), PythonProcess("soundd", "selfdrive.ui.soundd", driverview), PythonProcess("locationd", "selfdrive.locationd.locationd", only_onroad), NativeProcess("_pandad", "selfdrive/pandad", ["./pandad"], always_run, enabled=False), @@ -115,4 +116,13 @@ procs = [ PythonProcess("joystick", "tools.joystick.joystick_control", and_(joystick, iscar)), ] +# FrogPilot variables +if HARDWARE.get_device_type() == "mici": + procs.append(PythonProcess("ui", "selfdrive.ui.ui", always_run)) +elif TICI: + procs.append(NativeProcess("ui", "selfdrive/ui", ["./ui"], always_run, watchdog_max_dt=5)), +procs += [ + PythonProcess("frogpilot_process", "frogpilot.frogpilot_process", always_run), +] + managed_processes = {p.name: p for p in procs} diff --git a/system/timed.py b/system/timed.py index b7131b04..69851738 100755 --- a/system/timed.py +++ b/system/timed.py @@ -24,6 +24,9 @@ def set_time(new_time): cloudlog.exception("timed.failed_setting_time") +# FrogPilot variables + + def main() -> NoReturn: """ timed has two responsibilities: @@ -38,6 +41,9 @@ def main() -> NoReturn: pm = messaging.PubMaster(['clocks']) sm = messaging.SubMaster([gps_location_service]) + + # FrogPilot variables + while True: sm.update(1000) @@ -56,6 +62,9 @@ def main() -> NoReturn: continue set_time(gps_time) + + # FrogPilot variables + time.sleep(10) if __name__ == "__main__": diff --git a/system/ui/spinner.py b/system/ui/spinner.py index 2a48b388..2d5e7037 100755 --- a/system/ui/spinner.py +++ b/system/ui/spinner.py @@ -27,6 +27,8 @@ FONT_SIZE = 96 LINE_HEIGHT = 104 DARKGRAY = (55, 55, 55, 255) +# FrogPilot variables + def clamp(value, min_value, max_value): return max(min(value, max_value), min_value) diff --git a/system/updated/updated.py b/system/updated/updated.py index a4a1f8f3..9b4b4198 100755 --- a/system/updated/updated.py +++ b/system/updated/updated.py @@ -410,6 +410,7 @@ class Updater: finalize_update() cloudlog.info("finalize success!") + # FrogPilot variables def main() -> None: params = Params() @@ -449,9 +450,14 @@ def main() -> None: # Run the update loop first_run = True + + # FrogPilot variables + while True: wait_helper.ready_event.clear() + # FrogPilot variables + # Attempt an update exception = None try: