FrogPilot base

This commit is contained in:
James
2025-12-01 12:00:00 -07:00
parent ff862a5f77
commit 1b590b15d6
144 changed files with 1297 additions and 41 deletions
+6
View File
@@ -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
+2
View File
@@ -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())}
+2
View File
@@ -19,5 +19,7 @@ class CV:
# Mass
LB_TO_KG = 0.453592
# FrogPilot variables
ACCELERATION_DUE_TO_GRAVITY = 9.81 # m/s^2
+9
View File
@@ -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
+6
View File
@@ -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
};
+2
View File
@@ -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
};
+13
View File
@@ -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
+2
View File
@@ -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 {
+26
View File
@@ -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()
+82
View File
@@ -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()
Binary file not shown.
+35
View File
@@ -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);
}
+36
View File
@@ -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);
}
+24
View File
@@ -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);
+2
View File
@@ -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"
+3
View File
@@ -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
+4
View File
@@ -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,
+2
View File
@@ -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]]] = {
+2
View File
@@ -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
+3
View File
@@ -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):
View File
@@ -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
+13
View File
@@ -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
+7 -1
View File
@@ -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
+3
View File
@@ -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:
+2
View File
@@ -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
+2
View File
@@ -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
}
}
}
+2
View File
@@ -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
}
+8
View File
@@ -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
}
}
+6
View File
@@ -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
View File
@@ -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")
+2
View File
@@ -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)
+4
View File
@@ -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']
+12
View File
@@ -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):
+4
View File
@@ -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()
+17
View File
@@ -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()
+4
View File
@@ -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
+4
View File
@@ -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()
+4
View File
@@ -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
+8
View File
@@ -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:
+3
View File
@@ -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)
+4
View File
@@ -65,6 +65,8 @@ void PandaSafety::setSafetyMode(const std::string &params_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 &params_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);
+6 -2
View File
@@ -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;
}
}
+12
View File
@@ -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