FrogPilot base
This commit is contained in:
@@ -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
|
||||
|
||||
@@ -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())}
|
||||
|
||||
@@ -19,5 +19,7 @@ class CV:
|
||||
# Mass
|
||||
LB_TO_KG = 0.453592
|
||||
|
||||
# FrogPilot variables
|
||||
|
||||
|
||||
ACCELERATION_DUE_TO_GRAVITY = 9.81 # m/s^2
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -35,6 +35,8 @@ struct ParamKeyAttributes {
|
||||
uint32_t flags;
|
||||
ParamKeyType type;
|
||||
std::optional<std::string> 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<void> future;
|
||||
SafeQueue<std::pair<std::string, std::string>> queue;
|
||||
|
||||
// FrogPilot variables
|
||||
};
|
||||
|
||||
@@ -131,4 +131,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"UptimeOffroad", {PERSISTENT, FLOAT, "0.0"}},
|
||||
{"UptimeOnroad", {PERSISTENT, FLOAT, "0.0"}},
|
||||
{"Version", {PERSISTENT, STRING}},
|
||||
|
||||
// FrogPilot variables
|
||||
};
|
||||
|
||||
@@ -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 = <string>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
|
||||
|
||||
@@ -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 {
|
||||
|
||||
@@ -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()
|
||||
@@ -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()
|
||||
Executable
BIN
Binary file not shown.
@@ -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<SubMaster, const std::initializer_list<const char *>>({
|
||||
"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);
|
||||
}
|
||||
@@ -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<SubMaster> sm;
|
||||
|
||||
FrogPilotUIScene frogpilot_scene;
|
||||
|
||||
Params params;
|
||||
|
||||
WifiManager *wifi;
|
||||
|
||||
signals:
|
||||
};
|
||||
|
||||
FrogPilotUIState *frogpilotUIState();
|
||||
@@ -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) {
|
||||
}
|
||||
@@ -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;
|
||||
};
|
||||
@@ -0,0 +1 @@
|
||||
#include "frogpilot/ui/qt/onroad/frogpilot_buttons.h"
|
||||
@@ -0,0 +1,3 @@
|
||||
#pragma once
|
||||
|
||||
#include "selfdrive/ui/qt/onroad/buttons.h"
|
||||
@@ -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);
|
||||
}
|
||||
@@ -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;
|
||||
};
|
||||
@@ -0,0 +1,94 @@
|
||||
#include "selfdrive/ui/ui.h"
|
||||
|
||||
#include "frogpilot/ui/frogpilot_ui.h"
|
||||
|
||||
void clearMovie(QSharedPointer<QMovie> &movie, QWidget *parent) {
|
||||
if (!movie) {
|
||||
return;
|
||||
}
|
||||
|
||||
QObject::disconnect(movie.data(), nullptr, parent, nullptr);
|
||||
movie->stop();
|
||||
movie.reset();
|
||||
}
|
||||
|
||||
void loadGif(const QString &gifPath, QSharedPointer<QMovie> &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<QMovie>::create(gifPath);
|
||||
movie->setCacheMode(QMovie::CacheAll);
|
||||
movie->setScaledSize(size);
|
||||
|
||||
QPointer<QWidget> 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<QMovie> &movie, const QSize &size, QWidget *parent) {
|
||||
if (!parent || basePath.isEmpty()) {
|
||||
return;
|
||||
}
|
||||
|
||||
static QHash<QString, QPixmap> 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();
|
||||
}
|
||||
@@ -0,0 +1,19 @@
|
||||
#pragma once
|
||||
|
||||
#include <cmath>
|
||||
#include <iostream>
|
||||
#include <set>
|
||||
|
||||
#include <QElapsedTimer>
|
||||
#include <QJsonObject>
|
||||
#include <QMovie>
|
||||
#include <QNetworkAccessManager>
|
||||
#include <QNetworkReply>
|
||||
#include <QNetworkRequest>
|
||||
#include <QStyle>
|
||||
|
||||
#include "selfdrive/ui/qt/util.h"
|
||||
#include "selfdrive/ui/qt/widgets/controls.h"
|
||||
|
||||
void loadGif(const QString &gifPath, QSharedPointer<QMovie> &movie, const QSize &size, QWidget *parent);
|
||||
void loadImage(const QString &basePath, QPixmap &pixmap, QSharedPointer<QMovie> &movie, const QSize &size, QWidget *parent);
|
||||
@@ -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"
|
||||
|
||||
@@ -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)"
|
||||
|
||||
@@ -28,6 +28,8 @@ class CarState(CarStateBase):
|
||||
ret.cruiseState.enabled = True
|
||||
ret.cruiseState.available = True
|
||||
|
||||
# FrogPilot variables
|
||||
|
||||
return ret
|
||||
|
||||
@staticmethod
|
||||
|
||||
@@ -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)
|
||||
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -113,6 +113,8 @@ class CarState(CarStateBase):
|
||||
*create_button_events(self.lc_button, prev_lc_button, {1: ButtonType.lkas}),
|
||||
]
|
||||
|
||||
# FrogPilot variables
|
||||
|
||||
return ret
|
||||
|
||||
@staticmethod
|
||||
|
||||
@@ -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,
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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]]] = {
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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):
|
||||
|
||||
Executable → Regular
@@ -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):
|
||||
|
||||
@@ -68,6 +68,9 @@ class HyundaiSafetyFlags(IntFlag):
|
||||
ALT_LIMITS_2 = 512
|
||||
|
||||
|
||||
# FrogPilot variables
|
||||
|
||||
|
||||
class HyundaiFlags(IntFlag):
|
||||
# Dynamic Flags
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -29,4 +29,6 @@ class CarInterface(CarInterfaceBase):
|
||||
|
||||
ret.centerToFront = ret.wheelbase * 0.41
|
||||
|
||||
# FrogPilot variables
|
||||
|
||||
return ret
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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}
|
||||
|
||||
|
||||
@@ -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:
|
||||
|
||||
@@ -8,3 +8,5 @@ class ALTERNATIVE_EXPERIENCE:
|
||||
DISABLE_STOCK_AEB = 2
|
||||
RAISE_LONGITUDINAL_LIMITS_TO_ISO_MAX = 8
|
||||
ALLOW_AEB = 16
|
||||
|
||||
# FrogPilot variables
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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) {
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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) {
|
||||
|
||||
@@ -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
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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) {
|
||||
|
||||
@@ -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
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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):
|
||||
|
||||
|
||||
@@ -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]]
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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()
|
||||
|
||||
@@ -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()
|
||||
|
||||
@@ -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()
|
||||
|
||||
@@ -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()
|
||||
|
||||
@@ -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):
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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):
|
||||
|
||||
|
||||
@@ -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()
|
||||
|
||||
@@ -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]]
|
||||
|
||||
@@ -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
|
||||
|
||||
Executable → Regular
+13
@@ -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")
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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']
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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):
|
||||
|
||||
@@ -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()
|
||||
|
||||
@@ -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()
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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()
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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:
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -43,6 +43,8 @@
|
||||
|
||||
ExitHandler do_exit;
|
||||
|
||||
// FrogPilot variables
|
||||
|
||||
bool check_all_connected(const std::vector<Panda *> &pandas) {
|
||||
for (const auto& panda : pandas) {
|
||||
if (!panda->connected()) {
|
||||
@@ -107,6 +109,8 @@ void can_send_thread(std::vector<Panda *> 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;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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({
|
||||
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user