From b0c62707ad7bec9913a2b032208a7c5cbcdc04b7 Mon Sep 17 00:00:00 2001 From: James <91348155+FrogAi@users.noreply.github.com> Date: Mon, 1 Dec 2025 12:00:00 -0700 Subject: [PATCH] FrogPilot structs --- cereal/custom.capnp | 211 +++++++++++++++++- cereal/log.capnp | 18 +- cereal/messaging/__init__.py | 17 ++ cereal/services.py | 9 + frogpilot/controls/frogpilot_card.py | 25 +++ frogpilot/controls/frogpilot_planner.py | 107 +++++++++ .../controls/lib/frogpilot_acceleration.py | 19 ++ frogpilot/controls/lib/frogpilot_events.py | 24 ++ frogpilot/controls/lib/frogpilot_following.py | 55 +++++ frogpilot/controls/lib/frogpilot_vcruise.py | 20 ++ frogpilot/frogpilot_process.py | 18 +- frogpilot/ui/frogpilot_ui.cc | 10 +- .../qt/onroad/frogpilot_annotated_camera.cc | 4 + opendbc_repo/opendbc/car/body/carstate.py | 4 +- opendbc_repo/opendbc/car/car_helpers.py | 5 +- opendbc_repo/opendbc/car/chrysler/carstate.py | 8 +- opendbc_repo/opendbc/car/ford/carstate.py | 8 +- opendbc_repo/opendbc/car/gm/carstate.py | 8 +- opendbc_repo/opendbc/car/honda/carstate.py | 8 +- opendbc_repo/opendbc/car/hyundai/carstate.py | 13 +- opendbc_repo/opendbc/car/hyundai/values.py | 1 + opendbc_repo/opendbc/car/interfaces.py | 55 ++++- opendbc_repo/opendbc/car/mazda/carstate.py | 8 +- opendbc_repo/opendbc/car/mock/carstate.py | 3 +- opendbc_repo/opendbc/car/nissan/carstate.py | 8 +- opendbc_repo/opendbc/car/psa/carstate.py | 4 +- opendbc_repo/opendbc/car/rivian/carstate.py | 8 +- opendbc_repo/opendbc/car/subaru/carstate.py | 8 +- opendbc_repo/opendbc/car/tesla/carstate.py | 8 +- opendbc_repo/opendbc/car/toyota/carstate.py | 10 +- opendbc_repo/opendbc/car/toyota/values.py | 1 + .../opendbc/car/volkswagen/carstate.py | 11 +- selfdrive/car/car_specific.py | 6 +- selfdrive/car/card.py | 33 ++- selfdrive/controls/controlsd.py | 8 +- selfdrive/controls/lib/desire_helper.py | 2 +- .../lib/longitudinal_mpc_lib/long_mpc.py | 71 +++--- .../controls/lib/longitudinal_planner.py | 7 +- selfdrive/controls/plannerd.py | 1 + selfdrive/controls/radard.py | 14 +- selfdrive/locationd/lagd.py | 1 + selfdrive/locationd/paramsd.py | 1 + selfdrive/locationd/torqued.py | 1 + selfdrive/modeld/modeld.py | 7 +- selfdrive/monitoring/dmonitoringd.py | 1 + selfdrive/pandad/panda_safety.cc | 11 + selfdrive/selfdrived/events.py | 25 ++- selfdrive/selfdrived/selfdrived.py | 52 ++++- selfdrive/selfdrived/state.py | 28 +-- selfdrive/ui/qt/onroad/alerts.cc | 11 +- selfdrive/ui/qt/onroad/alerts.h | 8 +- selfdrive/ui/qt/onroad/model.cc | 3 + selfdrive/ui/qt/sidebar.cc | 2 + selfdrive/ui/soundd.py | 7 +- system/hardware/hardwared.py | 5 + system/loggerd/uploader.py | 1 + system/manager/manager.py | 2 + 57 files changed, 872 insertions(+), 152 deletions(-) create mode 100644 frogpilot/controls/frogpilot_card.py create mode 100644 frogpilot/controls/frogpilot_planner.py create mode 100644 frogpilot/controls/lib/frogpilot_acceleration.py create mode 100644 frogpilot/controls/lib/frogpilot_events.py create mode 100644 frogpilot/controls/lib/frogpilot_following.py create mode 100644 frogpilot/controls/lib/frogpilot_vcruise.py mode change 100755 => 100644 system/manager/manager.py diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 3348e859..5a55daf6 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -1,6 +1,8 @@ using Cxx = import "./include/c++.capnp"; $Cxx.namespace("cereal"); +using Car = import "car.capnp"; + @0xb526ba661d550a59; # custom.capnp: a home for empty structs reserved for custom forks @@ -10,31 +12,222 @@ $Cxx.namespace("cereal"); # DO rename the structs # DON'T change the identifier (e.g. @0x81c2f05a394cf4af) -struct CustomReserved0 @0x81c2f05a394cf4af { +struct FrogPilotCarControl @0x81c2f05a394cf4af { + hudControl @0 :HUDControl; + + struct HUDControl { + audibleAlert @0 :AudibleAlert; + + enum AudibleAlert { + none @0; + + engage @1; + disengage @2; + refuse @3; + + warningSoft @4; + warningImmediate @5; + + prompt @6; + promptRepeat @7; + promptDistracted @8; + + # Random Events + angry @9; + continued @10; + dejaVu @11; + doc @12; + fart @13; + firefox @14; + goat @15; + hal9000 @16; + mail @17; + nessie @18; + noice @19; + startup @20; + thisIsFine @21; + uwu @22; + } + } } -struct CustomReserved1 @0xaedffd8f31e7b55d { +struct FrogPilotCarParams @0xaedffd8f31e7b55d { + alternativeExperience @0 :Int16; + canUsePedal @1 :Bool; + canUseSDSU @2 :Bool; + flags @3 :UInt32; + isHDA2 @4 :Bool; + openpilotLongitudinalControlDisabled @5 :Bool; + safetyConfigs @6 :List(SafetyConfig); + + struct SafetyConfig { + safetyParam @0 :UInt16; + } } -struct CustomReserved2 @0xf35cc4560bbf6ec2 { +struct FrogPilotCarState @0xf35cc4560bbf6ec2 { + accelPressed @0 :Bool; + alwaysOnLateralEnabled @1 :Bool; + brakeLights @2 :Bool; + dashboardSpeedLimit @3 :Float32; + decelPressed @4 :Bool; + distancePressed @5 :Bool; + distanceLongPressed @6 :Bool; + distanceVeryLongPressed @7 :Bool; + ecoGear @8 :Bool; + forceCoast @9 :Bool; + pauseLateral @10 :Bool; + pauseLongitudinal @11 :Bool; + sportGear @12 :Bool; + trafficModeEnabled @13 :Bool; } -struct CustomReserved3 @0xda96579883444c35 { +struct FrogPilotDeviceState @0xda96579883444c35 { + freeSpace @0 :Int16; + usedSpace @1 :Int16; } -struct CustomReserved4 @0x80ae746ee2596b11 { +struct FrogPilotModelDataV2 @0x80ae746ee2596b11 { + turnDirection @0 :TurnDirection; + + enum TurnDirection { + none @0; + turnLeft @1; + turnRight @2; + } } -struct CustomReserved5 @0xa5cd762cd951a455 { +struct FrogPilotOnroadEvent @0xa5cd762cd951a455 { + name @0 :EventName; + + enable @1 :Bool; + noEntry @2 :Bool; + warning @3 :Bool; + userDisable @4 :Bool; + softDisable @5 :Bool; + immediateDisable @6 :Bool; + preEnable @7 :Bool; + permanent @8 :Bool; + overrideLateral @10 :Bool; + overrideLongitudinal @9 :Bool; + + enum EventName { + blockUser @0; + customStartupAlert @1; + forcingStop @2; + goatSteerSaturated @3; + greenLight @4; + holidayActive @5; + laneChangeBlockedLoud @6; + leadDeparting @7; + noLaneAvailable @8; + nnffLoaded @9; + openpilotCrashed @10; + pedalInterceptorNoBrake @11; + speedLimitChanged @12; + trafficModeActive @13; + trafficModeInactive @14; + turningLeft @15; + turningRight @16; + + # Random Events + accel30 @17; + accel35 @18; + accel40 @19; + dejaVuCurve @20; + firefoxSteerSaturated @21; + hal9000 @22; + openpilotCrashedRandomEvent @23; + thisIsFineSteerSaturated @24; + toBeContinued @25; + vCruise69 @26; + yourFrogTriedToKillMe @27; + youveGotMail @28; + } } -struct CustomReserved6 @0xf98d843bfd7004a3 { +struct FrogPilotPlan @0xf98d843bfd7004a3 { + accelerationJerk @0 :Float32; + cscControllingSpeed @1 :Bool; + cscSpeed @2 :Float32; + cscTraining @3 :Bool; + dangerFactor @4 :Float32; + dangerJerk @5 :Float32; + desiredFollowDistance @6 :Int64; + experimentalMode @7 :Bool; + forcingStop @8 :Bool; + forcingStopLength @9 :Float32; + frogpilotEvents @10 :List(FrogPilotOnroadEvent); + frogpilotToggles @11 :Text; + increasedStoppedDistance @12 :Float32; + lateralCheck @13 :Bool; + laneWidthLeft @14 :Float32; + laneWidthRight @15 :Float32; + maxAcceleration @16 :Float32; + minAcceleration @17 :Float32; + redLight @18 :Bool; + roadCurvature @19 :Float32; + slcMapSpeedLimit @20 :Float32; + slcMapboxSpeedLimit @21 :Float32; + slcNextSpeedLimit @22 :Float32; + slcOverriddenSpeed @23 :Float32; + slcSpeedLimit @24 :Float32; + slcSpeedLimitOffset @25 :Float32; + slcSpeedLimitSource @26 :Text; + speedJerk @27 :Float32; + speedLimitChanged @28 :Bool; + tFollow @29 :Float32; + themeUpdated @30 :Bool; + unconfirmedSlcSpeedLimit @31 :Float32; + vCruise @32 :Float32; + weatherDaytime @33 :Bool; + weatherId @34 :Int16; } -struct CustomReserved7 @0xb86e6369214c01c8 { +struct FrogPilotRadarState @0xb86e6369214c01c8 { + leadLeft @0 :LeadData; + leadRight @1 :LeadData; + + struct LeadData { + dRel @0 :Float32; + yRel @1 :Float32; + vRel @2 :Float32; + aRel @3 :Float32; + vLead @4 :Float32; + dPath @5 :Float32; + vLat @6 :Float32; + vLeadK @7 :Float32; + aLeadK @8 :Float32; + fcw @9 :Bool; + status @10 :Bool; + aLeadTau @11 :Float32; + modelProb @12 :Float32; + radar @13 :Bool; + radarTrackId @14 :Int32 = -1; + } } -struct CustomReserved8 @0xf416ec09499d9d19 { +struct FrogPilotSelfdriveState @0xf416ec09499d9d19 { + alertText1 @0 :Text; + alertText2 @1 :Text; + alertStatus @2 :AlertStatus; + alertSize @3 :AlertSize; + alertType @4 :Text; + alertSound @5 :Car.CarControl.HUDControl.AudibleAlert; + + enum AlertStatus { + normal @0; + userPrompt @1; + critical @2; + frogpilot @3; + } + + enum AlertSize { + none @0; + small @1; + mid @2; + full @3; + } } struct CustomReserved9 @0xa1680744031fdb2d { diff --git a/cereal/log.capnp b/cereal/log.capnp index 3a6432c8..52df4513 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -2625,15 +2625,15 @@ struct Event { # DO change the name of the field and struct # DON'T change the ID (e.g. @107) # DON'T change which struct it points to - customReserved0 @107 :Custom.CustomReserved0; - customReserved1 @108 :Custom.CustomReserved1; - customReserved2 @109 :Custom.CustomReserved2; - customReserved3 @110 :Custom.CustomReserved3; - customReserved4 @111 :Custom.CustomReserved4; - customReserved5 @112 :Custom.CustomReserved5; - customReserved6 @113 :Custom.CustomReserved6; - customReserved7 @114 :Custom.CustomReserved7; - customReserved8 @115 :Custom.CustomReserved8; + frogpilotCarControl @107 :Custom.FrogPilotCarControl; + frogpilotCarParams @108 :Custom.FrogPilotCarParams; + frogpilotCarState @109 :Custom.FrogPilotCarState; + frogpilotDeviceState @110 :Custom.FrogPilotDeviceState; + frogpilotModelV2 @111 :Custom.FrogPilotModelDataV2; + frogpilotOnroadEvents @112 :List(Custom.FrogPilotOnroadEvent); + frogpilotPlan @113 :Custom.FrogPilotPlan; + frogpilotRadarState @114 :Custom.FrogPilotRadarState; + frogpilotSelfdriveState @115 :Custom.FrogPilotSelfdriveState; customReserved9 @116 :Custom.CustomReserved9; customReserved10 @136 :Custom.CustomReserved10; customReserved11 @137 :Custom.CustomReserved11; diff --git a/cereal/messaging/__init__.py b/cereal/messaging/__init__.py index 275bcbfb..1bc9b4d8 100644 --- a/cereal/messaging/__init__.py +++ b/cereal/messaging/__init__.py @@ -198,6 +198,8 @@ class SubMaster: self.freq_tracker[s] = FrequencyTracker(SERVICE_LIST[s].frequency, self.update_freq, s == poll) # FrogPilot variables + self.addr = addr + self.poll = poll def __getitem__(self, s: str) -> capnp.lib.capnp._DynamicStructReader: return self.data[s] @@ -251,6 +253,16 @@ class SubMaster: return self.all_alive(service_list) and self.all_freq_ok(service_list) and self.all_valid(service_list) # FrogPilot variables + def extend(self, new_services: List[str]): + return SubMaster( + self.services + new_services, + poll=self.poll, + ignore_alive=self.ignore_alive, + ignore_avg_freq=self.ignore_average_freq, + ignore_valid=self.ignore_valid, + addr=self.addr, + frequency=None if self.poll is not None else self.update_freq, + ) class PubMaster: @@ -275,3 +287,8 @@ class PubMaster: return self.sock[s].all_readers_updated() # type: ignore # FrogPilot variables + def extend(self, new_services: List[str]): + for service in new_services: + if service not in self.sock: + self.sock[service] = pub_sock(service) + return self diff --git a/cereal/services.py b/cereal/services.py index ea99e8c5..ecabf0dc 100755 --- a/cereal/services.py +++ b/cereal/services.py @@ -104,6 +104,15 @@ _services: dict[str, tuple] = { "customReservedRawData2": (True, 0.), # FrogPilot variables + "frogpilotCarControl": (True, 100., 10), + "frogpilotCarParams": (True, 0.02, 1), + "frogpilotCarState": (True, 100., 10), + "frogpilotDeviceState": (True, 2., 1), + "frogpilotModelV2": (True, 20.), + "frogpilotOnroadEvents": (True, 1., 1), + "frogpilotPlan": (True, 20., 10), + "frogpilotRadarState": (True, 20., 5), + "frogpilotSelfdriveState": (True, 100., 10), } SERVICE_LIST = {name: Service(*vals) for idx, (name, vals) in enumerate(_services.items())} diff --git a/frogpilot/controls/frogpilot_card.py b/frogpilot/controls/frogpilot_card.py new file mode 100644 index 00000000..1bfeae5c --- /dev/null +++ b/frogpilot/controls/frogpilot_card.py @@ -0,0 +1,25 @@ +#!/usr/bin/env python3 +from openpilot.common.params import Params +from openpilot.selfdrive.car.cruise import ButtonType + +class FrogPilotCard: + def __init__(self, CP, FPCP): + self.CP = CP + + self.params = Params(return_defaults=True) + self.params_memory = Params(memory=True) + + self.accel_pressed = False + self.decel_pressed = False + + def update(self, carState, frogpilotCarState, sm): + if sm.updated["frogpilotPlan"] or any(be.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for be in carState.buttonEvents): + self.accel_pressed = any(be.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for be in carState.buttonEvents) + + if sm.updated["frogpilotPlan"] or any(be.type == ButtonType.decelCruise for be in carState.buttonEvents): + self.decel_pressed = any(be.type == ButtonType.decelCruise for be in carState.buttonEvents) + + frogpilotCarState.accelPressed = self.accel_pressed + frogpilotCarState.decelPressed = self.decel_pressed + + return frogpilotCarState diff --git a/frogpilot/controls/frogpilot_planner.py b/frogpilot/controls/frogpilot_planner.py new file mode 100644 index 00000000..b198c36e --- /dev/null +++ b/frogpilot/controls/frogpilot_planner.py @@ -0,0 +1,107 @@ +#!/usr/bin/env python3 +import json + +import cereal.messaging as messaging + +from openpilot.common.constants import CV +from openpilot.common.filter_simple import FirstOrderFilter +from openpilot.common.gps import get_gps_location_service +from openpilot.common.params import Params +from openpilot.common.realtime import DT_MDL +from openpilot.selfdrive.car.cruise import V_CRUISE_MAX +from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import A_CHANGE_COST, DANGER_ZONE_COST, J_EGO_COST, STOP_DISTANCE + +from openpilot.frogpilot.common.frogpilot_variables import CRUISING_SPEED, PLANNER_TIME, THRESHOLD +from openpilot.frogpilot.controls.lib.frogpilot_acceleration import FrogPilotAcceleration +from openpilot.frogpilot.controls.lib.frogpilot_events import FrogPilotEvents +from openpilot.frogpilot.controls.lib.frogpilot_following import FrogPilotFollowing +from openpilot.frogpilot.controls.lib.frogpilot_vcruise import FrogPilotVCruise + +class FrogPilotPlanner: + def __init__(self): + self.params = Params(return_defaults=True) + self.params_memory = Params(memory=True) + + self.frogpilot_acceleration = FrogPilotAcceleration(self) + self.frogpilot_events = FrogPilotEvents(self) + self.frogpilot_following = FrogPilotFollowing(self) + self.frogpilot_vcruise = FrogPilotVCruise(self) + + self.lateral_check = False + self.model_stopped = False + self.tracking_lead = False + + self.model_length = 0 + self.v_cruise = 0 + + self.gps_position = None + + self.gps_location_service = get_gps_location_service(self.params) + + self.tracking_lead_filter = FirstOrderFilter(0, 0.5, DT_MDL) + + def update(self, now, time_validated, sm): + self.lead_one = sm["radarState"].leadOne + + long_control_active = sm["carControl"].longActive + + v_cruise = min(sm["carState"].vCruise, V_CRUISE_MAX) * CV.KPH_TO_MS + v_ego = max(sm["carState"].vEgo, 0) + + if long_control_active: + self.frogpilot_acceleration.update(v_ego, sm) + else: + self.frogpilot_acceleration.max_accel = 0 + self.frogpilot_acceleration.min_accel = 0 + + self.frogpilot_events.update(v_cruise, sm) + + self.frogpilot_following.update(long_control_active, v_ego, sm) + + gps_location = sm[self.gps_location_service] + self.gps_position = { + "latitude": gps_location.latitude, + "longitude": gps_location.longitude, + "bearing": gps_location.bearingDeg, + } + self.params_memory.put("LastGPSPosition", json.dumps(self.gps_position)) + + self.lateral_check |= sm["carState"].standstill + + self.model_length = sm["modelV2"].position.x[-1] + + self.model_stopped = self.model_length < CRUISING_SPEED * PLANNER_TIME + + if not sm["carState"].standstill: + self.tracking_lead = self.update_lead_status() + + self.v_cruise = self.frogpilot_vcruise.update(long_control_active, now, time_validated, v_cruise, v_ego, sm) + + def update_lead_status(self): + following_lead = self.lead_one.status + following_lead &= self.lead_one.dRel < self.model_length + STOP_DISTANCE + + self.tracking_lead_filter.update(following_lead) + return self.tracking_lead_filter.x >= THRESHOLD + + def publish(self, sm, pm): + frogpilot_plan_send = messaging.new_message("frogpilotPlan") + frogpilot_plan_send.valid = sm.all_checks(service_list=["carState", "controlsState", "selfdriveState", "radarState"]) + frogpilotPlan = frogpilot_plan_send.frogpilotPlan + + frogpilotPlan.accelerationJerk = float(A_CHANGE_COST * self.frogpilot_following.acceleration_jerk) + frogpilotPlan.dangerFactor = float(self.frogpilot_following.danger_factor) + frogpilotPlan.dangerJerk = float(DANGER_ZONE_COST * self.frogpilot_following.danger_jerk) + frogpilotPlan.speedJerk = float(J_EGO_COST * self.frogpilot_following.speed_jerk) + frogpilotPlan.tFollow = float(self.frogpilot_following.t_follow) + + frogpilotPlan.frogpilotEvents = self.frogpilot_events.events.to_msg() + + frogpilotPlan.lateralCheck = self.lateral_check + + frogpilotPlan.maxAcceleration = float(self.frogpilot_acceleration.max_accel) + frogpilotPlan.minAcceleration = float(self.frogpilot_acceleration.min_accel) + + frogpilotPlan.vCruise = float(self.v_cruise) + + pm.send("frogpilotPlan", frogpilot_plan_send) diff --git a/frogpilot/controls/lib/frogpilot_acceleration.py b/frogpilot/controls/lib/frogpilot_acceleration.py new file mode 100644 index 00000000..0daece94 --- /dev/null +++ b/frogpilot/controls/lib/frogpilot_acceleration.py @@ -0,0 +1,19 @@ +#!/usr/bin/env python3 +import numpy as np + +from openpilot.selfdrive.controls.lib.longitudinal_planner import ACCEL_MIN, get_max_accel + +class FrogPilotAcceleration: + def __init__(self, FrogPilotPlanner): + self.frogpilot_planner = FrogPilotPlanner + + self.max_accel = 0 + self.min_accel = 0 + + def update(self, v_ego, sm, frogpilot_toggles): + self.max_accel = get_max_accel(v_ego) + + if self.frogpilot_planner.tracking_lead: + self.min_accel = ACCEL_MIN + else: + self.min_accel = ACCEL_MIN diff --git a/frogpilot/controls/lib/frogpilot_events.py b/frogpilot/controls/lib/frogpilot_events.py new file mode 100644 index 00000000..8c9ed4e7 --- /dev/null +++ b/frogpilot/controls/lib/frogpilot_events.py @@ -0,0 +1,24 @@ +#!/usr/bin/env python3 +from openpilot.selfdrive.selfdrived.events import ET, EVENT_NAME, FROGPILOT_EVENT_NAME, EventName, FrogPilotEventName, Events + +class FrogPilotEvents: + def __init__(self, FrogPilotPlanner): + self.frogpilot_planner = FrogPilotPlanner + + self.events = Events(frogpilot=True) + + self.startup_seen = False + + self.played_events = set() + + def update(self, v_cruise, sm): + current_alert = sm["selfdriveState"].alertType + current_frogpilot_alert = sm["selfdriveState"].alertType + + alerts_empty = all(sm[state].alertText1 == "" and sm[state].alertText2 == "" for state in ["selfdriveState", "frogpilotSelfdriveState"]) + + self.events.clear() + + self.startup_seen |= sm["frogpilotSelfdriveState"].alertText1 == frogpilot_toggles.startup_alert_top and sm["frogpilotSelfdriveState"].alertText2 == frogpilot_toggles.startup_alert_bottom + + self.played_events.update(FROGPILOT_EVENT_NAME[event] for event in self.events.names) diff --git a/frogpilot/controls/lib/frogpilot_following.py b/frogpilot/controls/lib/frogpilot_following.py new file mode 100644 index 00000000..0c61c88a --- /dev/null +++ b/frogpilot/controls/lib/frogpilot_following.py @@ -0,0 +1,55 @@ +#!/usr/bin/env python3 +import numpy as np + +from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import get_jerk_factor, get_T_FOLLOW + +class FrogPilotFollowing: + def __init__(self, FrogPilotPlanner): + self.frogpilot_planner = FrogPilotPlanner + + self.following_lead = False + + self.acceleration_jerk = 0 + self.danger_jerk = 0 + self.speed_jerk = 0 + self.t_follow = 0 + + def update(self, long_control_active, v_ego, sm): + if long_control_active: + if sm["carState"].aEgo >= 0: + self.base_acceleration_jerk, self.base_danger_jerk, self.base_speed_jerk = get_jerk_factor( + frogpilot_toggles.aggressive_jerk_acceleration, frogpilot_toggles.aggressive_jerk_danger, frogpilot_toggles.aggressive_jerk_speed, + frogpilot_toggles.standard_jerk_acceleration, frogpilot_toggles.standard_jerk_danger, frogpilot_toggles.standard_jerk_speed, + frogpilot_toggles.relaxed_jerk_acceleration, frogpilot_toggles.relaxed_jerk_danger, frogpilot_toggles.relaxed_jerk_speed, + frogpilot_toggles.custom_personalities, sm["selfdriveState"].personality + ) + else: + self.base_acceleration_jerk, self.base_danger_jerk, self.base_speed_jerk = get_jerk_factor( + frogpilot_toggles.aggressive_jerk_deceleration, frogpilot_toggles.aggressive_jerk_danger, frogpilot_toggles.aggressive_jerk_speed_decrease, + frogpilot_toggles.standard_jerk_deceleration, frogpilot_toggles.standard_jerk_danger, frogpilot_toggles.standard_jerk_speed_decrease, + frogpilot_toggles.relaxed_jerk_deceleration, frogpilot_toggles.relaxed_jerk_danger, frogpilot_toggles.relaxed_jerk_speed_decrease, + frogpilot_toggles.custom_personalities, sm["selfdriveState"].personality + ) + + self.t_follow = get_T_FOLLOW( + frogpilot_toggles.aggressive_follow, + frogpilot_toggles.standard_follow, + frogpilot_toggles.relaxed_follow, + frogpilot_toggles.custom_personalities, sm["selfdriveState"].personality + ) + else: + self.base_acceleration_jerk = 0 + self.base_danger_jerk = 0 + self.base_speed_jerk = 0 + self.t_follow = 0 + + self.acceleration_jerk = self.base_acceleration_jerk + self.danger_jerk = self.base_danger_jerk + self.speed_jerk = self.base_speed_jerk + + self.following_lead = self.frogpilot_planner.tracking_lead and self.frogpilot_planner.lead_one.dRel < (self.t_follow * 2) * v_ego + + if long_control_active and self.frogpilot_planner.tracking_lead: + self.update_follow_values(self.frogpilot_planner.lead_one.dRel, v_ego, self.frogpilot_planner.lead_one.vLead) + + def update_follow_values(self, lead_distance, v_ego, v_lead): diff --git a/frogpilot/controls/lib/frogpilot_vcruise.py b/frogpilot/controls/lib/frogpilot_vcruise.py new file mode 100644 index 00000000..79687e0c --- /dev/null +++ b/frogpilot/controls/lib/frogpilot_vcruise.py @@ -0,0 +1,20 @@ +#!/usr/bin/env python3 +from openpilot.common.constants import CV + +from openpilot.frogpilot.common.frogpilot_variables import CRUISING_SPEED + +class FrogPilotVCruise: + def __init__(self, FrogPilotPlanner): + self.frogpilot_planner = FrogPilotPlanner + + def update(self, long_control_active, now, time_validated, v_cruise, v_ego, sm): + v_cruise_cluster = max(sm["carState"].vCruiseCluster * CV.KPH_TO_MS, v_cruise) + v_cruise_diff = v_cruise_cluster - v_cruise + + v_ego_cluster = max(sm["carState"].vEgoCluster, v_ego) + v_ego_diff = v_ego_cluster - v_ego + + targets = [v_cruise] + v_cruise = min([target if target >= CRUISING_SPEED else v_cruise for target in targets]) + + return v_cruise diff --git a/frogpilot/frogpilot_process.py b/frogpilot/frogpilot_process.py index 39c7d751..2d54ce0a 100644 --- a/frogpilot/frogpilot_process.py +++ b/frogpilot/frogpilot_process.py @@ -1,5 +1,6 @@ #!/usr/bin/env python3 import datetime +import json import time from cereal import messaging @@ -7,11 +8,14 @@ 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 +from openpilot.frogpilot.controls.frogpilot_planner import FrogPilotPlanner + ASSET_CHECK_RATE = (1 / DT_MDL) def check_assets(params_memory): -def transition_offroad(time_validated, sm, params): +def transition_offroad(frogpilot_planner, time_validated, sm, params): + params.put("LastGPSPosition", json.dumps(frogpilot_planner.gps_position)) def transition_onroad(): @@ -26,9 +30,11 @@ def frogpilot_thread(): config_realtime_process(5, Priority.CTRL_LOW) + pm = messaging.PubMaster(["frogpilotPlan"]) sm = messaging.SubMaster(["carControl", "carState", "controlsState", "deviceState", "driverMonitoringState", "gpsLocation", "gpsLocationExternal", "liveParameters", "managerState", "modelV2", - "onroadEvents", "pandaStates", "radarState", "selfdriveState"], + "onroadEvents", "pandaStates", "radarState", "selfdriveState", "frogpilotCarState", + "frogpilotSelfdriveState", "frogpilotModelV2", "frogpilotOnroadEvents"], poll="modelV2") params = Params(return_defaults=True) @@ -46,14 +52,20 @@ def frogpilot_thread(): started = sm["deviceState"].started if not started and started_previously: - transition_offroad(time_validated, sm, params) + transition_offroad(frogpilot_planner, time_validated, sm, params) run_update_checks = True elif started and not started_previously: + frogpilot_planner = FrogPilotPlanner() + transition_onroad() if started and sm.updated["modelV2"]: + frogpilot_planner.update(now, time_validated, sm) + frogpilot_planner.publish(sm, pm) elif not started: + frogpilot_plan_send = messaging.new_message("frogpilotPlan") + pm.send("frogpilotPlan", frogpilot_plan_send) started_previously = started diff --git a/frogpilot/ui/frogpilot_ui.cc b/frogpilot/ui/frogpilot_ui.cc index 0dde7703..f1d12eda 100644 --- a/frogpilot/ui/frogpilot_ui.cc +++ b/frogpilot/ui/frogpilot_ui.cc @@ -10,6 +10,12 @@ static void update_state(FrogPilotUIState *fs) { const cereal::DeviceState::Reader &deviceState = fpsm["deviceState"].getDeviceState(); frogpilot_scene.online = deviceState.getNetworkType() != cereal::DeviceState::NetworkType::NONE; } + if (fpsm.updated("frogpilotCarState")) { + const cereal::FrogPilotCarState::Reader &frogpilotCarState = fpsm["frogpilotCarState"].getFrogpilotCarState(); + } + if (fpsm.updated("frogpilotPlan")) { + const cereal::FrogPilotPlan::Reader &frogpilotPlan = fpsm["frogpilotPlan"].getFrogpilotPlan(); + } if (fpsm.updated("selfdriveState")) { const cereal::SelfdriveState::Reader &selfdriveState = fpsm["selfdriveState"].getSelfdriveState(); frogpilot_scene.enabled = selfdriveState.getEnabled(); @@ -18,8 +24,8 @@ static void update_state(FrogPilotUIState *fs) { FrogPilotUIState::FrogPilotUIState(QObject *parent) : QObject(parent) { sm = std::make_unique>({ - "carControl", "deviceState", - "liveDelay", + "carControl", "deviceState", "frogpilotCarState", "frogpilotDeviceState", + "frogpilotPlan", "frogpilotRadarState", "frogpilotSelfdriveState", "liveDelay", "liveParameters", "liveTorqueParameters", "liveTracks", "selfdriveState" }); diff --git a/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc b/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc index 344211a3..66716926 100644 --- a/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc +++ b/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc @@ -14,6 +14,9 @@ void FrogPilotAnnotatedCameraWidget::updateState(const UIState &s, const FrogPil const SubMaster &fpsm = *(fs.sm); const cereal::CarState::Reader &carState = sm["carState"].getCarState(); + const cereal::FrogPilotCarState::Reader &frogpilotCarState = fpsm["frogpilotCarState"].getFrogpilotCarState(); + const cereal::FrogPilotPlan::Reader &frogpilotPlan = fpsm["frogpilotPlan"].getFrogpilotPlan(); + const cereal::FrogPilotSelfdriveState::Reader &frogpilotSelfdriveState = fpsm["frogpilotSelfdriveState"].getFrogpilotSelfdriveState(); const cereal::ModelDataV2::Reader &modelV2 = sm["modelV2"].getModelV2(); const cereal::SelfdriveState::Reader &selfdriveState = sm["selfdriveState"].getSelfdriveState(); @@ -36,6 +39,7 @@ void FrogPilotAnnotatedCameraWidget::updateState(const UIState &s, const FrogPil } hideBottomIcons = selfdriveState.getAlertSize() != cereal::SelfdriveState::AlertSize::NONE; + hideBottomIcons |= frogpilotSelfdriveState.getAlertSize() != cereal::FrogPilotSelfdriveState::AlertSize::NONE; } void FrogPilotAnnotatedCameraWidget::mousePressEvent(QMouseEvent *mouseEvent) { diff --git a/opendbc_repo/opendbc/car/body/carstate.py b/opendbc_repo/opendbc/car/body/carstate.py index 8bb0adbd..92c8f682 100644 --- a/opendbc_repo/opendbc/car/body/carstate.py +++ b/opendbc_repo/opendbc/car/body/carstate.py @@ -1,3 +1,4 @@ +from cereal import custom from opendbc.can import CANParser from opendbc.car import Bus, structs from opendbc.car.interfaces import CarStateBase @@ -29,8 +30,9 @@ class CarState(CarStateBase): ret.cruiseState.available = True # FrogPilot variables + fp_ret = custom.FrogPilotCarState.new_message() - return ret + return ret, fp_ret @staticmethod def get_can_parsers(CP): diff --git a/opendbc_repo/opendbc/car/car_helpers.py b/opendbc_repo/opendbc/car/car_helpers.py index f7b587f3..34b9d31a 100644 --- a/opendbc_repo/opendbc/car/car_helpers.py +++ b/opendbc_repo/opendbc/car/car_helpers.py @@ -1,6 +1,7 @@ import os import time +from cereal import custom from opendbc.car import gen_empty_fingerprint from opendbc.car.can_definitions import CanRecvCallable, CanSendCallable from opendbc.car.carlog import carlog @@ -14,6 +15,7 @@ from opendbc.car.vin import get_vin, is_valid_vin, VIN_UNKNOWN FRAME_FINGERPRINT = 100 # 1s # FrogPilot variables +FrogPilotCarParams = custom.FrogPilotCarParams def load_interfaces(brand_names): @@ -166,8 +168,9 @@ def get_car(can_recv: CanRecvCallable, can_send: CanSendCallable, set_obd_multip CP.fuzzyFingerprint = not exact_match # FrogPilot variables + FPCP: FrogPilotCarParams = CarInterface.get_frogpilot_params(candidate, fingerprints, car_fw, CP, frogpilot_toggles) - return interfaces[CP.carFingerprint](CP) + return interfaces[CP.carFingerprint](CP, FPCP) def get_demo_car_params(): diff --git a/opendbc_repo/opendbc/car/chrysler/carstate.py b/opendbc_repo/opendbc/car/chrysler/carstate.py index 1225ad3c..1d1b87c5 100644 --- a/opendbc_repo/opendbc/car/chrysler/carstate.py +++ b/opendbc_repo/opendbc/car/chrysler/carstate.py @@ -1,3 +1,4 @@ +from cereal import custom from opendbc.can import CANDefine, CANParser from opendbc.car import Bus, create_button_events, structs from opendbc.car.chrysler.values import DBC, STEER_THRESHOLD, RAM_CARS, ChryslerFrogPilotFlags @@ -8,8 +9,8 @@ ButtonType = structs.CarState.ButtonEvent.Type class CarState(CarStateBase): - def __init__(self, CP): - super().__init__(CP) + def __init__(self, CP, FPCP): + super().__init__(CP, FPCP) self.CP = CP can_define = CANDefine(DBC[CP.carFingerprint][Bus.pt]) @@ -103,10 +104,11 @@ class CarState(CarStateBase): buttonEvents = create_button_events(self.distance_button, prev_distance_button, {1: ButtonType.gapAdjustCruise}) # FrogPilot variables + fp_ret = custom.FrogPilotCarState.new_message() ret.buttonEvents = buttonEvents - return ret + return ret, fp_ret @staticmethod def get_can_parsers(CP): diff --git a/opendbc_repo/opendbc/car/ford/carstate.py b/opendbc_repo/opendbc/car/ford/carstate.py index d3d29404..e2a9b3ec 100644 --- a/opendbc_repo/opendbc/car/ford/carstate.py +++ b/opendbc_repo/opendbc/car/ford/carstate.py @@ -1,3 +1,4 @@ +from cereal import custom from opendbc.can import CANDefine, CANParser from opendbc.car import Bus, create_button_events, structs from opendbc.car.common.conversions import Conversions as CV @@ -11,8 +12,8 @@ TransmissionType = structs.CarParams.TransmissionType class CarState(CarStateBase): - def __init__(self, CP): - super().__init__(CP) + def __init__(self, CP, FPCP): + super().__init__(CP, FPCP) can_define = CANDefine(DBC[CP.carFingerprint][Bus.pt]) if CP.transmissionType == TransmissionType.automatic: self.shifter_values = can_define.dv["PowertrainData_10"]["TrnRng_D_Rq"] @@ -114,8 +115,9 @@ class CarState(CarStateBase): ] # FrogPilot variables + fp_ret = custom.FrogPilotCarState.new_message() - return ret + return ret, fp_ret @staticmethod def get_can_parsers(CP): diff --git a/opendbc_repo/opendbc/car/gm/carstate.py b/opendbc_repo/opendbc/car/gm/carstate.py index 8972bb54..0c6293df 100644 --- a/opendbc_repo/opendbc/car/gm/carstate.py +++ b/opendbc_repo/opendbc/car/gm/carstate.py @@ -1,4 +1,5 @@ import copy +from cereal import custom from opendbc.can import CANDefine, CANParser from opendbc.car import Bus, create_button_events, structs from opendbc.car.common.conversions import Conversions as CV @@ -19,8 +20,8 @@ GearShifter = structs.CarState.GearShifter class CarState(CarStateBase): - def __init__(self, CP): - super().__init__(CP) + def __init__(self, CP, FPCP): + super().__init__(CP, FPCP) can_define = CANDefine(DBC[CP.carFingerprint][Bus.pt]) self.shifter_values = can_define.dv["ECMPRDNL2"]["PRNDL2"] self.cluster_speed_hyst_gap = CV.KPH_TO_MS / 2. @@ -183,8 +184,9 @@ class CarState(CarStateBase): self.single_pedal_mode = ret.gearShifter == GearShifter.low or pt_cp.vl["EVDriveMode"]["SinglePedalModeActive"] == 1 # FrogPilot variables + fp_ret = custom.FrogPilotCarState.new_message() - return ret + return ret, fp_ret @staticmethod def get_can_parsers(CP): diff --git a/opendbc_repo/opendbc/car/honda/carstate.py b/opendbc_repo/opendbc/car/honda/carstate.py index fe444a0c..3c342c4d 100644 --- a/opendbc_repo/opendbc/car/honda/carstate.py +++ b/opendbc_repo/opendbc/car/honda/carstate.py @@ -1,6 +1,7 @@ import numpy as np from collections import defaultdict +from cereal import custom from opendbc.can import CANDefine, CANParser from opendbc.car import Bus, create_button_events, structs from opendbc.car.common.conversions import Conversions as CV @@ -19,8 +20,8 @@ SETTINGS_BUTTONS_DICT = {CruiseSettings.DISTANCE: ButtonType.gapAdjustCruise, Cr class CarState(CarStateBase): - def __init__(self, CP): - super().__init__(CP) + def __init__(self, CP, FPCP): + super().__init__(CP, FPCP) can_define = CANDefine(DBC[CP.carFingerprint][Bus.pt]) if CP.transmissionType != TransmissionType.manual: @@ -220,8 +221,9 @@ class CarState(CarStateBase): ] # FrogPilot variables + fp_ret = custom.FrogPilotCarState.new_message() - return ret + return ret, fp_ret def get_can_parsers(self, CP): parsers = { diff --git a/opendbc_repo/opendbc/car/hyundai/carstate.py b/opendbc_repo/opendbc/car/hyundai/carstate.py index ad68850e..6b8f8908 100644 --- a/opendbc_repo/opendbc/car/hyundai/carstate.py +++ b/opendbc_repo/opendbc/car/hyundai/carstate.py @@ -2,6 +2,7 @@ from collections import deque import copy import math +from cereal import custom from opendbc.can import CANDefine, CANParser from opendbc.car import Bus, create_button_events, structs from opendbc.car.common.conversions import Conversions as CV @@ -22,8 +23,8 @@ BUTTONS_DICT = {Buttons.RES_ACCEL: ButtonType.accelCruise, Buttons.SET_DECEL: Bu class CarState(CarStateBase): - def __init__(self, CP): - super().__init__(CP) + def __init__(self, CP, FPCP): + super().__init__(CP, FPCP) can_define = CANDefine(DBC[CP.carFingerprint][Bus.pt]) self.cruise_buttons: deque = deque([Buttons.NONE] * PREV_BUTTON_SAMPLES, maxlen=PREV_BUTTON_SAMPLES) @@ -69,7 +70,7 @@ class CarState(CarStateBase): # Main button also can trigger an engagement on these cars return any(btn in ENABLE_BUTTONS for btn in self.cruise_buttons) or any(self.main_buttons) - def update(self, can_parsers) -> structs.CarState: + def update(self, can_parsers, frogpilot_toggles) -> structs.CarState: cp = can_parsers[Bus.pt] cp_cam = can_parsers[Bus.cam] @@ -203,8 +204,9 @@ class CarState(CarStateBase): ret.lowSpeedAlert = self.low_speed_alert # FrogPilot variables + fp_ret = custom.FrogPilotCarState.new_message() - return ret + return ret, fp_ret def update_canfd(self, can_parsers) -> structs.CarState: cp = can_parsers[Bus.pt] @@ -296,8 +298,9 @@ class CarState(CarStateBase): ret.blockPcmEnable = not self.recent_button_interaction() # FrogPilot variables + fp_ret = custom.FrogPilotCarState.new_message() - return ret + return ret, fp_ret def get_can_parsers_canfd(self, CP): msgs = [] diff --git a/opendbc_repo/opendbc/car/hyundai/values.py b/opendbc_repo/opendbc/car/hyundai/values.py index 178b16ad..24a7a4e1 100644 --- a/opendbc_repo/opendbc/car/hyundai/values.py +++ b/opendbc_repo/opendbc/car/hyundai/values.py @@ -69,6 +69,7 @@ class HyundaiSafetyFlags(IntFlag): # FrogPilot variables +class HyundaiFrogPilotSafetyFlags(IntFlag): class HyundaiFlags(IntFlag): diff --git a/opendbc_repo/opendbc/car/interfaces.py b/opendbc_repo/opendbc/car/interfaces.py index a4028a51..d849a94f 100644 --- a/opendbc_repo/opendbc/car/interfaces.py +++ b/opendbc_repo/opendbc/car/interfaces.py @@ -8,12 +8,20 @@ from typing import Any from collections.abc import Callable from functools import cache +from cereal import custom from opendbc.car import DT_CTRL, apply_hysteresis, gen_empty_fingerprint, scale_rot_inertia, scale_tire_stiffness, STD_CARGO_KG from opendbc.car import structs from opendbc.car.can_definitions import CanData, CanRecvCallable, CanSendCallable +from opendbc.car.chrysler.values import CAR as CHRYSLER, ChryslerFrogPilotFlags from opendbc.car.common.basedir import BASEDIR from opendbc.car.common.conversions import Conversions as CV from opendbc.car.common.simple_kalman import KF1D, get_kalman_gain +from opendbc.car.gm.values import CAR as GM +from opendbc.car.honda.values import CAR as HONDA, HONDA_BOSCH, HondaSafetyFlags +from opendbc.car.hyundai.hyundaicanfd import CanBus +from opendbc.car.hyundai.values import CAR as HYUNDAI, CANFD_CAR, HyundaiFlags, HyundaiFrogPilotSafetyFlags +from opendbc.car.mock.values import CAR as MOCK +from opendbc.car.toyota.values import CAR as TOYOTA, TSS2_CAR, UNSUPPORTED_DSU_CAR, ToyotaFrogPilotFlags, ToyotaSafetyFlags from opendbc.car.values import PLATFORMS from opendbc.can import CANParser from openpilot.common.params import Params @@ -22,6 +30,7 @@ GearShifter = structs.CarState.GearShifter ButtonType = structs.CarState.ButtonEvent.Type # FrogPilot variables +Ecu = structs.CarParams.Ecu V_CRUISE_MAX = 145 MAX_CTRL_SPEED = (V_CRUISE_MAX + 4) * CV.KPH_TO_MS @@ -99,19 +108,21 @@ class CarInterfaceBase(ABC): CarController: 'CarControllerBase' RadarInterface: 'RadarInterfaceBase' = RadarInterfaceBase - def __init__(self, CP: structs.CarParams): + def __init__(self, CP: structs.CarParams, FPCP: custom.FrogPilotCarParams): self.CP = CP self.frame = 0 self.v_ego_cluster_seen = False - self.CS: CarStateBase = self.CarState(CP) + self.CS: CarStateBase = self.CarState(CP, FPCP) self.can_parsers: dict[StrEnum, CANParser] = self.CS.get_can_parsers(CP) dbc_names = {bus: cp.dbc_name for bus, cp in self.can_parsers.items()} self.CC: CarControllerBase = self.CarController(dbc_names, CP) # FrogPilot variables + self.FPCP = FPCP + self.params_memory = Params(memory=True) def apply(self, c: structs.CarControl, now_nanos: int | None = None) -> tuple[structs.CarControl.Actuators, list[CanData]]: @@ -160,6 +171,38 @@ class CarInterfaceBase(ABC): return ret # FrogPilot variables + @classmethod + def get_frogpilot_params(cls, candidate: str, fingerprint: dict[int, dict[int, int]], car_fw: list[structs.CarParams.CarFw], CP: structs.CarParams): + fp_ret = custom.FrogPilotCarParams.new_message() + + platform = PLATFORMS[candidate] + + fp_ret.flags |= int(platform.config.flags) + fp_ret.safetyConfigs = [custom.FrogPilotCarParams.SafetyConfig.new_message(safetyParam=config.safetyParam) for config in CP.safetyConfigs] + + if platform not in MOCK: + if platform in CHRYSLER: + if candidate == CHRYSLER.RAM_HD_5TH_GEN: + if 570 not in fingerprint[0]: + fp_ret.flags |= ChryslerFrogPilotFlags.RAM_HD_ALT_BUTTONS.value + + elif platform in GM: + fp_ret.canUsePedal = True + + elif platform in HONDA: + fp_ret.canUsePedal = candidate not in HONDA_BOSCH + + elif platform in HYUNDAI: + if candidate in CANFD_CAR: + hda2 = Ecu.adas in [fw.ecu for fw in car_fw] + + fp_ret.isHDA2 = hda2 + + elif platform in TOYOTA: + fp_ret.canUsePedal = not CP.autoResumeSng + fp_ret.canUseSDSU = candidate not in UNSUPPORTED_DSU_CAR and candidate not in TSS2_CAR + + return fp_ret @staticmethod @abstractmethod @@ -246,7 +289,7 @@ class CarInterfaceBase(ABC): cp.update(can_packets) # get CarState - ret = self.CS.update(self.can_parsers) + ret, fp_ret = self.CS.update(self.can_parsers) ret.canValid = all(cp.can_valid for cp in self.can_parsers.values()) ret.canTimeout = any(cp.bus_timeout for cp in self.can_parsers.values()) @@ -271,11 +314,11 @@ class CarInterfaceBase(ABC): # FrogPilot variables - return ret + return ret, fp_ret class CarStateBase(ABC): - def __init__(self, CP: structs.CarParams): + def __init__(self, CP: structs.CarParams, FPCP: custom.FrogPilotCarParams): self.CP = CP self.car_fingerprint = CP.carFingerprint self.out = structs.CarState() @@ -300,6 +343,8 @@ class CarStateBase(ABC): self.v_ego_kf = KF1D(x0=x0, A=A, C=C[0], K=K) # FrogPilot variables + self.FPCP = FPCP + self.CC: structs.CarControl = structs.CarControl.new_message() @abstractmethod diff --git a/opendbc_repo/opendbc/car/mazda/carstate.py b/opendbc_repo/opendbc/car/mazda/carstate.py index 6b7bdd25..53287f85 100644 --- a/opendbc_repo/opendbc/car/mazda/carstate.py +++ b/opendbc_repo/opendbc/car/mazda/carstate.py @@ -1,3 +1,4 @@ +from cereal import custom from opendbc.can import CANDefine, CANParser from opendbc.car import Bus, create_button_events, structs from opendbc.car.common.conversions import Conversions as CV @@ -8,8 +9,8 @@ ButtonType = structs.CarState.ButtonEvent.Type class CarState(CarStateBase): - def __init__(self, CP): - super().__init__(CP) + def __init__(self, CP, FPCP): + super().__init__(CP, FPCP) can_define = CANDefine(DBC[CP.carFingerprint][Bus.pt]) self.shifter_values = can_define.dv["GEAR"]["GEAR"] @@ -116,8 +117,9 @@ class CarState(CarStateBase): ret.buttonEvents = create_button_events(self.distance_button, prev_distance_button, {1: ButtonType.gapAdjustCruise}) # FrogPilot variables + fp_ret = custom.FrogPilotCarState.new_message() - return ret + return ret, fp_ret @staticmethod def get_can_parsers(CP): diff --git a/opendbc_repo/opendbc/car/mock/carstate.py b/opendbc_repo/opendbc/car/mock/carstate.py index 9cbdf99e..50ba0a52 100644 --- a/opendbc_repo/opendbc/car/mock/carstate.py +++ b/opendbc_repo/opendbc/car/mock/carstate.py @@ -1,7 +1,8 @@ +from cereal import custom from opendbc.car import structs from opendbc.car.interfaces import CarStateBase class CarState(CarStateBase): def update(self, *_) -> structs.CarState: - return structs.CarState() + return structs.CarState(), custom.FrogPilotCarState() diff --git a/opendbc_repo/opendbc/car/nissan/carstate.py b/opendbc_repo/opendbc/car/nissan/carstate.py index c0533765..0890742f 100644 --- a/opendbc_repo/opendbc/car/nissan/carstate.py +++ b/opendbc_repo/opendbc/car/nissan/carstate.py @@ -1,5 +1,6 @@ import copy from collections import deque +from cereal import custom from opendbc.can import CANDefine, CANParser from opendbc.car import Bus, create_button_events, structs from opendbc.car.common.conversions import Conversions as CV @@ -12,8 +13,8 @@ TORQUE_SAMPLES = 12 class CarState(CarStateBase): - def __init__(self, CP): - super().__init__(CP) + def __init__(self, CP, FPCP): + super().__init__(CP, FPCP) can_define = CANDefine(DBC[CP.carFingerprint][Bus.pt]) self.lkas_hud_msg = {} @@ -131,10 +132,11 @@ class CarState(CarStateBase): buttonEvents = create_button_events(self.distance_button, prev_distance_button, {1: ButtonType.gapAdjustCruise}) # FrogPilot variables + fp_ret = custom.FrogPilotCarState.new_message() ret.buttonEvents = buttonEvents - return ret + return ret, fp_ret @staticmethod def get_can_parsers(CP): diff --git a/opendbc_repo/opendbc/car/psa/carstate.py b/opendbc_repo/opendbc/car/psa/carstate.py index bfc88691..20f3bc35 100644 --- a/opendbc_repo/opendbc/car/psa/carstate.py +++ b/opendbc_repo/opendbc/car/psa/carstate.py @@ -1,3 +1,4 @@ +from cereal import custom from opendbc.car import structs, Bus from opendbc.can.parser import CANParser from opendbc.car.common.conversions import Conversions as CV @@ -64,8 +65,9 @@ class CarState(CarStateBase): ret.seatbeltUnlatched = cp_cam.vl['RESTRAINTS']['DRIVER_SEATBELT'] != 2 # FrogPilot variables + fp_ret = custom.FrogPilotCarState.new_message() - return ret + return ret, fp_ret @staticmethod def get_can_parsers(CP): diff --git a/opendbc_repo/opendbc/car/rivian/carstate.py b/opendbc_repo/opendbc/car/rivian/carstate.py index 222a55eb..63e2f40b 100644 --- a/opendbc_repo/opendbc/car/rivian/carstate.py +++ b/opendbc_repo/opendbc/car/rivian/carstate.py @@ -1,4 +1,5 @@ import copy +from cereal import custom from opendbc.can import CANParser from opendbc.car import Bus, structs from opendbc.car.interfaces import CarStateBase @@ -9,8 +10,8 @@ GearShifter = structs.CarState.GearShifter class CarState(CarStateBase): - def __init__(self, CP): - super().__init__(CP) + def __init__(self, CP, FPCP): + super().__init__(CP, FPCP) self.last_speed = 30 self.acm_lka_hba_cmd = None @@ -93,8 +94,9 @@ class CarState(CarStateBase): self.vdm_adas_status = copy.copy(cp.vl["VDM_AdasSts"]) # FrogPilot variables + fp_ret = custom.FrogPilotCarState.new_message() - return ret + return ret, fp_ret @staticmethod def get_can_parsers(CP): diff --git a/opendbc_repo/opendbc/car/subaru/carstate.py b/opendbc_repo/opendbc/car/subaru/carstate.py index ddfc6f60..919436ed 100644 --- a/opendbc_repo/opendbc/car/subaru/carstate.py +++ b/opendbc_repo/opendbc/car/subaru/carstate.py @@ -1,4 +1,5 @@ import copy +from cereal import custom from opendbc.can import CANDefine, CANParser from opendbc.car import Bus, structs from opendbc.car.common.conversions import Conversions as CV @@ -8,8 +9,8 @@ from opendbc.car import CanSignalRateCalculator class CarState(CarStateBase): - def __init__(self, CP): - super().__init__(CP) + def __init__(self, CP, FPCP): + super().__init__(CP, FPCP) can_define = CANDefine(DBC[CP.carFingerprint][Bus.pt]) self.shifter_values = can_define.dv["Transmission"]["Gear"] @@ -124,8 +125,9 @@ class CarState(CarStateBase): self.es_infotainment_msg = copy.copy(cp_cam.vl["ES_Infotainment"]) # FrogPilot variables + fp_ret = custom.FrogPilotCarState.new_message() - return ret + return ret, fp_ret @staticmethod def get_can_parsers(CP): diff --git a/opendbc_repo/opendbc/car/tesla/carstate.py b/opendbc_repo/opendbc/car/tesla/carstate.py index 5f73e4c7..58d51533 100644 --- a/opendbc_repo/opendbc/car/tesla/carstate.py +++ b/opendbc_repo/opendbc/car/tesla/carstate.py @@ -1,4 +1,5 @@ import copy +from cereal import custom from opendbc.can import CANDefine, CANParser from opendbc.car import Bus, structs from opendbc.car.common.conversions import Conversions as CV @@ -9,8 +10,8 @@ ButtonType = structs.CarState.ButtonEvent.Type class CarState(CarStateBase): - def __init__(self, CP): - super().__init__(CP) + def __init__(self, CP, FPCP): + super().__init__(CP, FPCP) self.can_define = CANDefine(DBC[CP.carFingerprint][Bus.party]) self.shifter_values = self.can_define.dv["DI_systemStatus"]["DI_gear"] @@ -118,8 +119,9 @@ class CarState(CarStateBase): self.das_control = copy.copy(cp_ap_party.vl["DAS_control"]) # FrogPilot variables + fp_ret = custom.FrogPilotCarState.new_message() - return ret + return ret, fp_ret @staticmethod def get_can_parsers(CP): diff --git a/opendbc_repo/opendbc/car/toyota/carstate.py b/opendbc_repo/opendbc/car/toyota/carstate.py index 43c2da73..24546c73 100644 --- a/opendbc_repo/opendbc/car/toyota/carstate.py +++ b/opendbc_repo/opendbc/car/toyota/carstate.py @@ -1,11 +1,12 @@ import copy +from cereal import custom from opendbc.can import CANDefine, CANParser from opendbc.car import Bus, DT_CTRL, create_button_events, structs from opendbc.car.common.conversions import Conversions as CV from opendbc.car.common.filter_simple import FirstOrderFilter from opendbc.car.interfaces import CarStateBase -from opendbc.car.toyota.values import ToyotaFlags, CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, \ +from opendbc.car.toyota.values import ToyotaFlags, ToyotaFrogPilotFlags, CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, \ TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE, UNSUPPORTED_DSU_CAR, \ SECOC_CAR @@ -25,8 +26,8 @@ PERM_STEER_FAULTS = (3, 17) class CarState(CarStateBase): - def __init__(self, CP): - super().__init__(CP) + def __init__(self, CP, FPCP): + super().__init__(CP, FPCP) can_define = CANDefine(DBC[CP.carFingerprint][Bus.pt]) self.eps_torque_scale = EPS_SCALE[CP.carFingerprint] / 100. self.cluster_speed_hyst_gap = CV.KPH_TO_MS / 2. @@ -201,10 +202,11 @@ class CarState(CarStateBase): buttonEvents += create_button_events(self.distance_button, prev_distance_button, {1: ButtonType.gapAdjustCruise}) # FrogPilot variables + fp_ret = custom.FrogPilotCarState.new_message() ret.buttonEvents = buttonEvents - return ret + return ret, fp_ret @staticmethod def get_can_parsers(CP): diff --git a/opendbc_repo/opendbc/car/toyota/values.py b/opendbc_repo/opendbc/car/toyota/values.py index 9db6457a..c1b1dd03 100644 --- a/opendbc_repo/opendbc/car/toyota/values.py +++ b/opendbc_repo/opendbc/car/toyota/values.py @@ -81,6 +81,7 @@ class ToyotaFlags(IntFlag): # FrogPilot variables +class ToyotaFrogPilotFlags(IntFlag): def dbc_dict(pt, radar): diff --git a/opendbc_repo/opendbc/car/volkswagen/carstate.py b/opendbc_repo/opendbc/car/volkswagen/carstate.py index e1228903..6ceacd6e 100644 --- a/opendbc_repo/opendbc/car/volkswagen/carstate.py +++ b/opendbc_repo/opendbc/car/volkswagen/carstate.py @@ -1,3 +1,4 @@ +from cereal import custom from opendbc.can import CANParser from opendbc.car import Bus, structs from opendbc.car.interfaces import CarStateBase @@ -9,8 +10,8 @@ ButtonType = structs.CarState.ButtonEvent.Type class CarState(CarStateBase): - def __init__(self, CP): - super().__init__(CP) + def __init__(self, CP, FPCP): + super().__init__(CP, FPCP) self.frame = 0 self.eps_init_complete = False self.CCP = CarControllerParams(CP) @@ -139,8 +140,9 @@ class CarState(CarStateBase): self.frame += 1 # FrogPilot variables + fp_ret = custom.FrogPilotCarState.new_message() - return ret + return ret, fp_ret def update_pq(self, pt_cp, cam_cp, ext_cp) -> structs.CarState: ret = structs.CarState() @@ -233,8 +235,9 @@ class CarState(CarStateBase): self.frame += 1 # FrogPilot variables + fp_ret = custom.FrogPilotCarState.new_message() - return ret + return ret, fp_ret def update_mlb(self, pt_cp, cam_cp, ext_cp) -> structs.CarState: ret = structs.CarState() diff --git a/selfdrive/car/car_specific.py b/selfdrive/car/car_specific.py index 81f9778b..538e6332 100644 --- a/selfdrive/car/car_specific.py +++ b/selfdrive/car/car_specific.py @@ -1,4 +1,4 @@ -from cereal import car, log +from cereal import car, custom, log import cereal.messaging as messaging from opendbc.car import DT_CTRL, structs from opendbc.car.chrysler.values import RAM_DT @@ -18,14 +18,14 @@ class MockCarState: def __init__(self): self.sm = messaging.SubMaster(['gpsLocation', 'gpsLocationExternal']) - def update(self, CS: car.CarState): + def update(self, CS: car.CarState, FPCS: custom.FrogPilotCarState): self.sm.update(0) gps_sock = 'gpsLocationExternal' if self.sm.recv_frame['gpsLocationExternal'] > 1 else 'gpsLocation' CS.vEgo = self.sm[gps_sock].speed CS.vEgoRaw = self.sm[gps_sock].speed - return CS + return CS, FPCS BRAND_EXTRA_GEARS = { diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 772627be..301c8951 100644 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -5,7 +5,7 @@ import threading import cereal.messaging as messaging -from cereal import car, log +from cereal import car, custom, log from openpilot.common.params import Params from openpilot.common.realtime import config_realtime_process, Priority, Ratekeeper @@ -21,6 +21,8 @@ from openpilot.selfdrive.pandad import can_capnp_to_list, can_list_to_can_capnp from openpilot.selfdrive.car.cruise import VCruiseHelper from openpilot.selfdrive.car.car_specific import MockCarState +from openpilot.frogpilot.controls.frogpilot_card import FrogPilotCard + REPLAY = "REPLAY" in os.environ EventName = log.OnroadEvent.EventName @@ -64,6 +66,7 @@ class Car: CP: car.CarParams # FrogPilot variables + FPCP: custom.FrogPilotCarParams def __init__(self, CI=None, RI=None) -> None: self.can_sock = messaging.sub_sock('can', timeout=20) @@ -109,8 +112,9 @@ class Car: self.params.put_bool("FirmwareQueryDone", True) # FrogPilot variables + self.FPCP = self.CI.FPCP else: - self.CI, self.CP = CI, CI.CP + self.CI, self.CP, self.FPCP = CI, CI.CP, CI.FPCP self.RI = RI self.CP.alternativeExperience = 0 @@ -167,6 +171,14 @@ class Car: self.resume_prev_button = False # FrogPilot variables + fpcp_bytes = self.FPCP.to_bytes() + self.params.put("FrogPilotCarParams", fpcp_bytes) + self.params.put_nonblocking("FrogPilotCarParamsPersistent", fpcp_bytes) + + self.frogpilot_card = FrogPilotCard(self.CP, self.FPCP) + + self.sm = self.sm.extend(['frogpilotOnroadEvents', 'frogpilotPlan', 'frogpilotSelfdriveState', 'liveCalibration', 'selfdriveState']) + self.pm = self.pm.extend(['frogpilotCarState']) def state_update(self) -> tuple[car.CarState, structs.RadarDataT | None]: """carState update loop, driven by can""" @@ -175,9 +187,9 @@ class Car: can_list = can_capnp_to_list(can_strs) # Update carState from CAN - CS = self.CI.update(can_list) + CS, FPCS = self.CI.update(can_list) if self.CP.brand == 'mock': - CS = self.mock_carstate.update(CS) + CS, FPCS = self.mock_carstate.update(CS, FPCS) # Update radar tracks from CAN RD: structs.RadarDataT | None = self.RI.update(can_list) @@ -209,10 +221,11 @@ class Car: self.resume_prev_button = False # FrogPilot variables + FPCS = self.frogpilot_card.update(CS, FPCS, self.sm) - return CS, RD + return CS, RD, FPCS - def state_publish(self, CS: car.CarState, RD: structs.RadarDataT | None): + def state_publish(self, CS: car.CarState, RD: structs.RadarDataT | None, FPCS: custom.FrogPilotCarState): """carState and carParams publish loop""" # carParams - logged every 50 seconds (> 1 per segment) @@ -243,6 +256,10 @@ class Car: self.pm.send('liveTracks', tracks_msg) # FrogPilot variables + fpcs_send = messaging.new_message('frogpilotCarState') + fpcs_send.valid = CS.canValid + fpcs_send.frogpilotCarState = FPCS + self.pm.send('frogpilotCarState', fpcs_send) def controls_update(self, CS: car.CarState, CC: car.CarControl): """control update loop, driven by carControl""" @@ -263,9 +280,9 @@ class Car: self.CC_prev = CC def step(self): - CS, RD = self.state_update() + CS, RD, FPCS = self.state_update() - self.state_publish(CS, RD) + self.state_publish(CS, RD, FPCS) initialized = (not any(e.name == EventName.selfdriveInitializing for e in self.sm['onroadEvents']) and self.sm.seen['onroadEvents']) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 2f1e20f3..bb279d73 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -2,7 +2,7 @@ import math from numbers import Number -from cereal import car, log +from cereal import car, custom, log import cereal.messaging as messaging from openpilot.common.constants import CV from openpilot.common.params import Params @@ -32,9 +32,10 @@ class Controls: self.params = Params() cloudlog.info("controlsd is waiting for CarParams") self.CP = messaging.log_from_bytes(self.params.get("CarParams", block=True), car.CarParams) + self.FPCP = messaging.log_from_bytes(self.params.get("FrogPilotCarParams", block=True), custom.FrogPilotCarParams) cloudlog.info("controlsd got CarParams") - self.CI = interfaces[self.CP.carFingerprint](self.CP) + self.CI = interfaces[self.CP.carFingerprint](self.CP, self.FPCP) self.sm = messaging.SubMaster(['liveDelay', 'liveParameters', 'liveTorqueParameters', 'modelV2', 'selfdriveState', 'liveCalibration', 'livePose', 'longitudinalPlan', 'carState', 'carOutput', @@ -59,6 +60,7 @@ class Controls: self.LaC = LatControlTorque(self.CP, self.CI, DT_CTRL) # FrogPilot variables + self.sm = self.sm.extend(['liveDelay', 'frogpilotCarState', 'frogpilotPlan']) def update(self): self.sm.update(15) @@ -98,7 +100,7 @@ class Controls: # Check which actuators can be enabled standstill = abs(CS.vEgo) <= max(self.CP.minSteerSpeed, 0.3) or CS.standstill CC.latActive = self.sm['selfdriveState'].active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \ - (not standstill or self.CP.steerAtStandstill) + (not standstill or self.CP.steerAtStandstill) and self.sm['frogpilotPlan'].lateralCheck CC.longActive = CC.enabled and not any(e.overrideLongitudinal for e in self.sm['onroadEvents']) and self.CP.openpilotLongitudinalControl actuators = CC.actuators diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index a77ab526..afcf4cbf 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -48,7 +48,7 @@ class DesireHelper: def get_lane_change_direction(CS): return LaneChangeDirection.left if CS.leftBlinker else LaneChangeDirection.right - def update(self, carstate, lateral_active, lane_change_prob): + def update(self, carstate, lateral_active, lane_change_prob, frogpilotPlan): v_ego = carstate.vEgo one_blinker = carstate.leftBlinker != carstate.rightBlinker below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 3f9d8245..f058e00b 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -58,26 +58,49 @@ STOP_DISTANCE = 6.0 CRUISE_MIN_ACCEL = -1.2 CRUISE_MAX_ACCEL = 1.6 -def get_jerk_factor(personality=log.LongitudinalPersonality.standard): - if personality==log.LongitudinalPersonality.relaxed: - return 1.0 - elif personality==log.LongitudinalPersonality.standard: - return 1.0 - elif personality==log.LongitudinalPersonality.aggressive: - return 0.5 +def get_jerk_factor(aggressive_jerk_acceleration=0.5, aggressive_jerk_danger=0.5, aggressive_jerk_speed=0.5, + standard_jerk_acceleration=1.0, standard_jerk_danger=1.0, standard_jerk_speed=1.0, + relaxed_jerk_acceleration=1.0, relaxed_jerk_danger=1.0, relaxed_jerk_speed=1.0, + custom_personalities=False, personality=log.LongitudinalPersonality.standard): + if custom_personalities: + if personality==log.LongitudinalPersonality.relaxed: + return relaxed_jerk_acceleration, relaxed_jerk_danger, relaxed_jerk_speed + elif personality==log.LongitudinalPersonality.standard: + return standard_jerk_acceleration, standard_jerk_danger, standard_jerk_speed + elif personality==log.LongitudinalPersonality.aggressive: + return aggressive_jerk_acceleration, aggressive_jerk_danger, aggressive_jerk_speed + else: + raise NotImplementedError("Longitudinal personality not supported") else: - raise NotImplementedError("Longitudinal personality not supported") + if personality==log.LongitudinalPersonality.relaxed: + return 1.0, 1.0, 1.0 + elif personality==log.LongitudinalPersonality.standard: + return 1.0, 1.0, 1.0 + elif personality==log.LongitudinalPersonality.aggressive: + return 0.5, 0.5, 0.5 + else: + raise NotImplementedError("Longitudinal personality not supported") -def get_T_FOLLOW(personality=log.LongitudinalPersonality.standard): - if personality==log.LongitudinalPersonality.relaxed: - return 1.75 - elif personality==log.LongitudinalPersonality.standard: - return 1.45 - elif personality==log.LongitudinalPersonality.aggressive: - return 1.25 +def get_T_FOLLOW(aggressive_follow=1.25, standard_follow=1.45, relaxed_follow=1.75, custom_personalities=False, personality=log.LongitudinalPersonality.standard): + if custom_personalities: + if personality==log.LongitudinalPersonality.relaxed: + return relaxed_follow + elif personality==log.LongitudinalPersonality.standard: + return standard_follow + elif personality==log.LongitudinalPersonality.aggressive: + return aggressive_follow + else: + raise NotImplementedError("Longitudinal personality not supported") else: - raise NotImplementedError("Longitudinal personality not supported") + if personality==log.LongitudinalPersonality.relaxed: + return 1.75 + elif personality==log.LongitudinalPersonality.standard: + return 1.45 + elif personality==log.LongitudinalPersonality.aggressive: + return 1.25 + else: + raise NotImplementedError("Longitudinal personality not supported") def get_stopped_equivalence_factor(v_lead): return (v_lead**2) / (2 * COMFORT_BRAKE) @@ -274,16 +297,15 @@ class LongitudinalMpc: for i in range(N): self.solver.cost_set(i, 'Zl', Zl) - def set_weights(self, prev_accel_constraint=True, personality=log.LongitudinalPersonality.standard): - jerk_factor = get_jerk_factor(personality) + def set_weights(self, acceleration_jerk=1.0, danger_jerk=1.0, speed_jerk=1.0, prev_accel_constraint=True, personality=log.LongitudinalPersonality.standard): if self.mode == 'acc': - a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0 - cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, jerk_factor * a_change_cost, jerk_factor * J_EGO_COST] - constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST] + a_change_cost = acceleration_jerk if prev_accel_constraint else 0 + cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, a_change_cost, speed_jerk] + constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, danger_jerk] elif self.mode == 'blended': a_change_cost = 40.0 if prev_accel_constraint else 0 cost_weights = [0., 0.1, 0.2, 5.0, a_change_cost, 1.0] - constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST] + constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, danger_jerk] else: raise NotImplementedError(f'Planner mode {self.mode} not recognized in planner cost set') self.set_cost_weights(cost_weights, constraint_cost_weights) @@ -327,8 +349,7 @@ class LongitudinalMpc: lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau) return lead_xv - def update(self, radarstate, v_cruise, x, v, a, j, personality=log.LongitudinalPersonality.standard): - t_follow = get_T_FOLLOW(personality) + def update(self, radarstate, v_cruise, x, v, a, j, danger_factor, t_follow, personality=log.LongitudinalPersonality.standard): v_ego = self.x0[1] self.status = radarstate.leadOne.status or radarstate.leadTwo.status @@ -346,7 +367,7 @@ class LongitudinalMpc: # Update in ACC mode or ACC/e2e blend if self.mode == 'acc': - self.params[:,5] = LEAD_DANGER_FACTOR + self.params[:,5] = danger_factor # Fake an obstacle for cruise, this ensures smooth acceleration to set speed # when the leads are no factor. diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index ad2f25e4..76c4def8 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -101,8 +101,7 @@ class LongitudinalPlanner: accel_coast = ACCEL_MAX v_ego = sm['carState'].vEgo - v_cruise_kph = min(sm['carState'].vCruise, V_CRUISE_MAX) - v_cruise = v_cruise_kph * CV.KPH_TO_MS + v_cruise = sm['frogpilotPlan'].vCruise v_cruise_initialized = sm['carState'].vCruise != V_CRUISE_UNSET long_control_off = sm['controlsState'].longControlState == LongCtrlState.off @@ -142,9 +141,9 @@ class LongitudinalPlanner: if force_slow_decel: v_cruise = 0.0 - self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality) + self.mpc.set_weights(sm['frogpilotPlan'].accelerationJerk, sm['frogpilotPlan'].dangerJerk, sm['frogpilotPlan'].speedJerk, prev_accel_constraint, personality=sm['selfdriveState'].personality) self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired) - self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=sm['selfdriveState'].personality) + self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, sm['frogpilotPlan'].dangerFactor, sm['frogpilotPlan'].tFollow, personality=sm['selfdriveState'].personality) self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution) self.a_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution) diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 6e5d5b33..8c2cddea 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -23,6 +23,7 @@ def main(): poll='modelV2') # FrogPilot variables + sm = sm.extend(['frogpilotCarState', 'frogpilotPlan']) while True: sm.update() diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index f90fd54f..125caa35 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -5,7 +5,7 @@ from collections import deque from typing import Any import capnp -from cereal import messaging, log, car +from cereal import messaging, log, car, custom from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.params import Params from openpilot.common.realtime import DT_MDL, Priority, config_realtime_process @@ -210,6 +210,7 @@ class RadarD: self.ready = False # FrogPilot variables + self.frogpilot_radar_state = custom.FrogPilotRadarState.new_message() def update(self, sm: messaging.SubMaster, rr: car.RadarData): self.ready = sm.seen['modelV2'] @@ -265,6 +266,12 @@ class RadarD: radar_msg.radarState = self.radar_state pm.send("radarState", radar_msg) + # FrogPilot variables + frogpilot_radar_msg = messaging.new_message("frogpilotRadarState") + frogpilot_radar_msg.valid = self.radar_state_valid + frogpilot_radar_msg.frogpilotRadarState = self.frogpilot_radar_state + pm.send("frogpilotRadarState", frogpilot_radar_msg) + # fuses camera and radar data for best lead detection def main() -> None: @@ -276,12 +283,15 @@ def main() -> None: cloudlog.info("radard got CarParams") # *** setup messaging - sm = messaging.SubMaster(['modelV2', 'carState', 'liveTracks'], poll='modelV2') + sm = messaging.SubMaster(['modelV2', 'carState', 'liveTracks'], poll='modelV2', + ignore_valid=['frogpilotPlan']) pm = messaging.PubMaster(['radarState']) RD = RadarD(CP.radarDelay) # FrogPilot variables + sm = sm.extend(['frogpilotPlan']) + pm = pm.extend(['frogpilotRadarState']) while 1: sm.update() diff --git a/selfdrive/locationd/lagd.py b/selfdrive/locationd/lagd.py index 7a53bce8..93be874c 100755 --- a/selfdrive/locationd/lagd.py +++ b/selfdrive/locationd/lagd.py @@ -375,6 +375,7 @@ def main(): lag_learner.reset(lag, valid_blocks) # FrogPilot variables + sm = sm.extend(['frogpilotPlan']) while True: sm.update() diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index 41473bc5..8dcef422 100755 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -284,6 +284,7 @@ def main(): learner = VehicleParamsLearner(CP, steer_ratio, stiffness_factor, np.radians(angle_offset_deg), pInitial) # FrogPilot variables + sm = sm.extend(['frogpilotPlan']) while True: sm.update() diff --git a/selfdrive/locationd/torqued.py b/selfdrive/locationd/torqued.py index 86f44f57..ebc41657 100755 --- a/selfdrive/locationd/torqued.py +++ b/selfdrive/locationd/torqued.py @@ -252,6 +252,7 @@ def main(demo=False): estimator = TorqueEstimator(messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)) # FrogPilot variables + sm = sm.extend(['frogpilotPlan']) while True: sm.update() diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 195c51da..2c32e2c1 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -297,6 +297,8 @@ def main(demo=False): DH = DesireHelper() # FrogPilot variables + sm = sm.extend(['frogpilotPlan']) + pm = pm.extend(['frogpilotModelV2']) while True: # Keep receiving frames until we are at least 1 frame ahead of previous extra frame @@ -391,7 +393,7 @@ def main(demo=False): l_lane_change_prob = desire_state[log.Desire.laneChangeLeft] r_lane_change_prob = desire_state[log.Desire.laneChangeRight] lane_change_prob = l_lane_change_prob + r_lane_change_prob - DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob) + DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob, sm['frogpilotPlan']) modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction drivingdata_send.drivingModelData.meta.laneChangeState = DH.lane_change_state @@ -403,6 +405,9 @@ def main(demo=False): pm.send('cameraOdometry', posenet_send) # FrogPilot variables + frogpilot_modelv2_send = messaging.new_message('frogpilotModelV2') + + pm.send('frogpilotModelV2', frogpilot_modelv2_send) last_vipc_frame_id = meta_main.frame_id # FrogPilot variables diff --git a/selfdrive/monitoring/dmonitoringd.py b/selfdrive/monitoring/dmonitoringd.py index 08e30955..571934eb 100755 --- a/selfdrive/monitoring/dmonitoringd.py +++ b/selfdrive/monitoring/dmonitoringd.py @@ -16,6 +16,7 @@ def dmonitoringd_thread(): demo_mode=False # FrogPilot variables + sm = sm.extend(['frogpilotCarState']) # 20Hz <- dmonitoringmodeld while True: diff --git a/selfdrive/pandad/panda_safety.cc b/selfdrive/pandad/panda_safety.cc index 594b9426..83bff602 100644 --- a/selfdrive/pandad/panda_safety.cc +++ b/selfdrive/pandad/panda_safety.cc @@ -66,6 +66,14 @@ void PandaSafety::setSafetyMode(const std::string ¶ms_string) { uint16_t alternative_experience = car_params.getAlternativeExperience(); // FrogPilot variables + std::string frogpilot_params_string = params_.get("FrogPilotCarParams"); + + AlignedBuffer frogpilot_aligned_buf; + capnp::FlatArrayMessageReader frogpilot_cmsg(frogpilot_aligned_buf.align(frogpilot_params_string.data(), frogpilot_params_string.size())); + cereal::FrogPilotCarParams::Reader frogpilot_car_params = frogpilot_cmsg.getRoot(); + + auto frogpilot_safety_configs = frogpilot_car_params.getSafetyConfigs(); + alternative_experience |= frogpilot_car_params.getAlternativeExperience(); for (int i = 0; i < pandas_.size(); ++i) { // Default to SILENT safety model if not specified @@ -77,6 +85,9 @@ void PandaSafety::setSafetyMode(const std::string ¶ms_string) { } // FrogPilot variables + if (i < frogpilot_safety_configs.size()) { + safety_param |= frogpilot_safety_configs[i].getSafetyParam(); + } 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); diff --git a/selfdrive/selfdrived/events.py b/selfdrive/selfdrived/events.py index 74705f93..c24a485c 100755 --- a/selfdrive/selfdrived/events.py +++ b/selfdrive/selfdrived/events.py @@ -5,7 +5,7 @@ import os from enum import IntEnum from collections.abc import Callable -from cereal import log, car +from cereal import log, car, custom import cereal.messaging as messaging from openpilot.common.constants import CV from openpilot.common.git import get_short_branch @@ -23,6 +23,9 @@ AudibleAlert = car.CarControl.HUDControl.AudibleAlert EventName = log.OnroadEvent.EventName # FrogPilot variables +FrogPilotAlertStatus = custom.FrogPilotSelfdriveState.AlertStatus +FrogPilotAudibleAlert = custom.FrogPilotCarControl.HUDControl.AudibleAlert +FrogPilotEventName = custom.FrogPilotOnroadEvent.EventName # Alert priorities @@ -53,13 +56,17 @@ class ET: EVENT_NAME = {v: k for k, v in EventName.schema.enumerants.items()} # FrogPilot variables +FROGPILOT_EVENT_NAME = {v: k for k, v in FrogPilotEventName.schema.enumerants.items()} class Events: - def __init__(self): + def __init__(self, frogpilot=False): self.events: list[int] = [] self.static_events: list[int] = [] - self.event_counters = dict.fromkeys(EVENTS.keys(), 0) + self.event_counters = dict.fromkeys((FROGPILOT_EVENTS if frogpilot else EVENTS).keys(), 0) + + # FrogPilot variables + self.frogpilot = frogpilot @property def names(self) -> list[int]: @@ -78,7 +85,7 @@ class Events: self.events = self.static_events.copy() def contains(self, event_type: str) -> bool: - return any(event_type in EVENTS.get(e, {}) for e in self.events) + return any(event_type in (FROGPILOT_EVENTS if self.frogpilot else EVENTS).get(e, {}) for e in self.events) def create_alerts(self, event_types: list[str], callback_args=None): if callback_args is None: @@ -86,15 +93,15 @@ class Events: ret = [] for e in self.events: - types = EVENTS[e].keys() + types = (FROGPILOT_EVENTS if self.frogpilot else EVENTS)[e].keys() for et in event_types: if et in types: - alert = EVENTS[e][et] + alert = (FROGPILOT_EVENTS if self.frogpilot else EVENTS)[e][et] if not isinstance(alert, Alert): alert = alert(*callback_args) if DT_CTRL * (self.event_counters[e] + 1) >= alert.creation_delay: - alert.alert_type = f"{EVENT_NAME[e]}/{et}" + alert.alert_type = f"{(FROGPILOT_EVENT_NAME if self.frogpilot else EVENT_NAME)[e]}/{et}" alert.event_type = et ret.append(alert) return ret @@ -106,9 +113,9 @@ class Events: def to_msg(self): ret = [] for event_name in self.events: - event = log.OnroadEvent.new_message() + event = (custom.FrogPilotOnroadEvent if self.frogpilot else log.OnroadEvent).new_message() event.name = event_name - for event_type in EVENTS.get(event_name, {}): + for event_type in (FROGPILOT_EVENTS if self.frogpilot else EVENTS).get(event_name, {}): setattr(event, event_type, True) ret.append(event) return ret diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index a19b1ea1..b710b2b7 100644 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -5,7 +5,7 @@ import threading import cereal.messaging as messaging -from cereal import car, log +from cereal import car, custom, log from msgq.visionipc import VisionIpcClient, VisionStreamType @@ -24,6 +24,8 @@ from openpilot.selfdrive.selfdrived.alertmanager import AlertManager, set_offroa from openpilot.system.version import get_build_metadata from openpilot.system.hardware import HARDWARE +from openpilot.frogpilot.common.frogpilot_utilities import contains_event_type + REPLAY = "REPLAY" in os.environ SIMULATION = "SIMULATION" in os.environ TESTING_CLOSET = "TESTING_CLOSET" in os.environ @@ -40,6 +42,7 @@ ButtonType = car.CarState.ButtonEvent.Type SafetyModel = car.CarParams.SafetyModel # FrogPilot variables +FrogPilotEventName = custom.FrogPilotOnroadEvent.EventName IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput) @@ -142,11 +145,21 @@ class SelfdriveD: self.events.add(EventName.dashcamMode, static=True) # FrogPilot variables + self.sm = self.sm.extend(['frogpilotCarState', 'frogpilotPlan']) + self.pm = self.pm.extend(['frogpilotOnroadEvents', 'frogpilotSelfdriveState']) + + self.frogpilot_AM = AlertManager() + self.frogpilot_events = Events(frogpilot=True) + + self.frogpilot_events_prev = [] + + self.FPCP = messaging.log_from_bytes(self.params.get("FrogPilotCarParams", block=True), custom.FrogPilotCarParams) def update_events(self, CS): """Compute onroadEvents from carState""" self.events.clear() + self.frogpilot_events.clear() if self.sm['controlsState'].lateralControlState.which() == 'debugState': self.events.add(EventName.joystickDebug) @@ -279,8 +292,8 @@ class SelfdriveD: # All pandas must match the list of safetyConfigs, and if outside this list, must be silent or noOutput if i < len(self.CP.safetyConfigs): safety_mismatch = pandaState.safetyModel != self.CP.safetyConfigs[i].safetyModel or \ - pandaState.safetyParam != self.CP.safetyConfigs[i].safetyParam or \ - pandaState.alternativeExperience != self.CP.alternativeExperience + pandaState.safetyParam != self.FPCP.safetyConfigs[i].safetyParam or \ + pandaState.alternativeExperience != self.FPCP.alternativeExperience else: safety_mismatch = pandaState.safetyModel not in IGNORED_SAFETY_MODES @@ -325,7 +338,9 @@ class SelfdriveD: self.events.add(EventName.canError) # generic catch-all. ideally, a more specific event should be added above instead - has_disable_events = self.events.contains(ET.NO_ENTRY) and (self.events.contains(ET.SOFT_DISABLE) or self.events.contains(ET.IMMEDIATE_DISABLE)) + has_disable_events = contains_event_type(self.events, self.frogpilot_events, ET.NO_ENTRY) and \ + (contains_event_type(self.events, self.frogpilot_events, ET.SOFT_DISABLE) or + contains_event_type(self.events, self.frogpilot_events, ET.IMMEDIATE_DISABLE)) no_system_errors = (not has_disable_events) or (len(self.events) == num_events) if not self.sm.all_checks() and no_system_errors: if not self.sm.all_alive(): @@ -409,6 +424,7 @@ class SelfdriveD: self.events.add(EventName.personalityChanged) # FrogPilot variables + self.frogpilot_events.add_from_msg(self.sm['frogpilotPlan'].frogpilotEvents) def data_sample(self): _car_state = messaging.recv_one(self.car_state_sock) @@ -471,6 +487,11 @@ class SelfdriveD: self.AM.process_alerts(self.sm.frame, clear_event_types) # FrogPilot variables + frogpilot_alerts = self.frogpilot_events.create_alerts(self.state_machine.current_alert_types, [self.CP, CS, self.sm, self.is_metric, + self.state_machine.soft_disable_timer, pers, + self.frogpilot_toggles]) + self.frogpilot_AM.add_many(self.sm.frame, frogpilot_alerts) + self.frogpilot_AM.process_alerts(self.sm.frame, clear_event_types) def publish_selfdriveState(self, CS): # selfdriveState @@ -480,7 +501,7 @@ class SelfdriveD: ss.enabled = self.enabled ss.active = self.active ss.state = self.state_machine.state - ss.engageable = not self.events.contains(ET.NO_ENTRY) + ss.engageable = not contains_event_type(self.events, self.frogpilot_events, ET.NO_ENTRY) ss.experimentalMode = self.experimental_mode ss.personality = self.personality @@ -503,12 +524,31 @@ class SelfdriveD: self.events_prev = self.events.names.copy() # FrogPilot variables + fpss_msg = messaging.new_message('frogpilotSelfdriveState') + fpss_msg.valid = True + fpss = fpss_msg.frogpilotSelfdriveState + + fpss.alertText1 = self.frogpilot_AM.current_alert.alert_text_1 + fpss.alertText2 = self.frogpilot_AM.current_alert.alert_text_2 + fpss.alertSize = self.frogpilot_AM.current_alert.alert_size + fpss.alertStatus = self.frogpilot_AM.current_alert.alert_status + fpss.alertType = self.frogpilot_AM.current_alert.alert_type + fpss.alertSound = self.frogpilot_AM.current_alert.audible_alert + + self.pm.send('frogpilotSelfdriveState', fpss_msg) + + if (self.sm.frame % int(1. / DT_CTRL) == 0) or (self.frogpilot_events.names != self.frogpilot_events_prev): + fpce_send = messaging.new_message('frogpilotOnroadEvents', len(self.frogpilot_events)) + fpce_send.valid = True + fpce_send.frogpilotOnroadEvents = self.frogpilot_events.to_msg() + self.pm.send('frogpilotOnroadEvents', fpce_send) + self.frogpilot_events_prev = self.frogpilot_events.names.copy() def step(self): CS = self.data_sample() self.update_events(CS) if not self.CP.passive and self.initialized: - self.enabled, self.active = self.state_machine.update(self.events) + self.enabled, self.active = self.state_machine.update(self.events, self.frogpilot_events) self.update_alerts(CS) self.publish_selfdriveState(CS) diff --git a/selfdrive/selfdrived/state.py b/selfdrive/selfdrived/state.py index 073ddb56..31151700 100644 --- a/selfdrive/selfdrived/state.py +++ b/selfdrive/selfdrived/state.py @@ -2,6 +2,8 @@ from cereal import log from openpilot.selfdrive.selfdrived.events import Events, ET from openpilot.common.realtime import DT_CTRL +from openpilot.frogpilot.common.frogpilot_utilities import contains_event_type + State = log.SelfdriveState.OpenpilotState SOFT_DISABLE_TIME = 3 # seconds @@ -14,7 +16,7 @@ class StateMachine: self.state = State.disabled self.soft_disable_timer = 0 - def update(self, events: Events): + def update(self, events: Events, frogpilot_events: Events): # decrement the soft disable timer at every step, as it's reset on # entrance in SOFT_DISABLING state self.soft_disable_timer = max(0, self.soft_disable_timer - 1) @@ -24,29 +26,29 @@ class StateMachine: # ENABLED, SOFT DISABLING, PRE ENABLING, OVERRIDING if self.state != State.disabled: # user and immediate disable always have priority in a non-disabled state - if events.contains(ET.USER_DISABLE): + if contains_event_type(events, frogpilot_events, ET.USER_DISABLE): self.state = State.disabled self.current_alert_types.append(ET.USER_DISABLE) - elif events.contains(ET.IMMEDIATE_DISABLE): + elif contains_event_type(events, frogpilot_events, ET.IMMEDIATE_DISABLE): self.state = State.disabled self.current_alert_types.append(ET.IMMEDIATE_DISABLE) else: # ENABLED if self.state == State.enabled: - if events.contains(ET.SOFT_DISABLE): + if contains_event_type(events, frogpilot_events, ET.SOFT_DISABLE): self.state = State.softDisabling self.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL) self.current_alert_types.append(ET.SOFT_DISABLE) - elif events.contains(ET.OVERRIDE_LATERAL) or events.contains(ET.OVERRIDE_LONGITUDINAL): + elif contains_event_type(events, frogpilot_events, ET.OVERRIDE_LATERAL) or contains_event_type(events, frogpilot_events, ET.OVERRIDE_LONGITUDINAL): self.state = State.overriding self.current_alert_types += [ET.OVERRIDE_LATERAL, ET.OVERRIDE_LONGITUDINAL] # SOFT DISABLING elif self.state == State.softDisabling: - if not events.contains(ET.SOFT_DISABLE): + if not contains_event_type(events, frogpilot_events, ET.SOFT_DISABLE): # no more soft disabling condition, so go back to ENABLED self.state = State.enabled @@ -58,32 +60,32 @@ class StateMachine: # PRE ENABLING elif self.state == State.preEnabled: - if not events.contains(ET.PRE_ENABLE): + if not contains_event_type(events, frogpilot_events, ET.PRE_ENABLE): self.state = State.enabled else: self.current_alert_types.append(ET.PRE_ENABLE) # OVERRIDING elif self.state == State.overriding: - if events.contains(ET.SOFT_DISABLE): + if contains_event_type(events, frogpilot_events, ET.SOFT_DISABLE): self.state = State.softDisabling self.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL) self.current_alert_types.append(ET.SOFT_DISABLE) - elif not (events.contains(ET.OVERRIDE_LATERAL) or events.contains(ET.OVERRIDE_LONGITUDINAL)): + elif not (contains_event_type(events, frogpilot_events, ET.OVERRIDE_LATERAL) or contains_event_type(events, frogpilot_events, ET.OVERRIDE_LONGITUDINAL)): self.state = State.enabled else: self.current_alert_types += [ET.OVERRIDE_LATERAL, ET.OVERRIDE_LONGITUDINAL] # DISABLED elif self.state == State.disabled: - if events.contains(ET.ENABLE): - if events.contains(ET.NO_ENTRY): + if contains_event_type(events, frogpilot_events, ET.ENABLE): + if contains_event_type(events, frogpilot_events, ET.NO_ENTRY): self.current_alert_types.append(ET.NO_ENTRY) else: - if events.contains(ET.PRE_ENABLE): + if contains_event_type(events, frogpilot_events, ET.PRE_ENABLE): self.state = State.preEnabled - elif events.contains(ET.OVERRIDE_LATERAL) or events.contains(ET.OVERRIDE_LONGITUDINAL): + elif contains_event_type(events, frogpilot_events, ET.OVERRIDE_LATERAL) or contains_event_type(events, frogpilot_events, ET.OVERRIDE_LONGITUDINAL): self.state = State.overriding else: self.state = State.enabled diff --git a/selfdrive/ui/qt/onroad/alerts.cc b/selfdrive/ui/qt/onroad/alerts.cc index ee0e9008..7704d04e 100644 --- a/selfdrive/ui/qt/onroad/alerts.cc +++ b/selfdrive/ui/qt/onroad/alerts.cc @@ -6,7 +6,7 @@ #include "selfdrive/ui/qt/util.h" void OnroadAlerts::updateState(const UIState &s, const FrogPilotUIState &fs) { - Alert a = getAlert(*(s.sm), s.scene.started_frame); + Alert a = getAlert(*(s.sm), *(fs.sm), s.scene.started_frame); if (!alert.equal(a)) { alert = a; update(); @@ -23,11 +23,12 @@ void OnroadAlerts::clear() { alertHeight = 0; } -OnroadAlerts::Alert OnroadAlerts::getAlert(const SubMaster &sm, uint64_t started_frame) { +OnroadAlerts::Alert OnroadAlerts::getAlert(const SubMaster &sm, const SubMaster &fpsm, uint64_t started_frame) { const cereal::SelfdriveState::Reader &ss = sm["selfdriveState"].getSelfdriveState(); const uint64_t selfdrive_frame = sm.rcv_frame("selfdriveState"); // FrogPilot variables + const cereal::FrogPilotSelfdriveState::Reader &fpss = fpsm["frogpilotSelfdriveState"].getFrogpilotSelfdriveState(); Alert a = {}; if (selfdrive_frame >= started_frame) { // Don't get old alert. @@ -35,6 +36,10 @@ OnroadAlerts::Alert OnroadAlerts::getAlert(const SubMaster &sm, uint64_t started ss.getAlertType().cStr(), ss.getAlertSize(), ss.getAlertStatus()}; // FrogPilot variables + if (a.size == cereal::SelfdriveState::AlertSize::NONE) { + a = {fpss.getAlertText1().cStr(), fpss.getAlertText2().cStr(), + fpss.getAlertType().cStr(), static_cast(fpss.getAlertSize()), static_cast(fpss.getAlertStatus())}; + } } if (!sm.updated("selfdriveState") && (sm.frame - started_frame) > 5 * UI_FREQ) { @@ -93,7 +98,7 @@ void OnroadAlerts::paintEvent(QPaintEvent *event) { // draw background + gradient p.setPen(Qt::NoPen); p.setCompositionMode(QPainter::CompositionMode_SourceOver); - p.setBrush(QBrush(alert_colors[alert.status])); + p.setBrush(QBrush(frogpilot_alert_colors[static_cast(alert.status)])); p.drawRoundedRect(r, radius, radius); QLinearGradient g(0, r.y(), 0, r.bottom()); diff --git a/selfdrive/ui/qt/onroad/alerts.h b/selfdrive/ui/qt/onroad/alerts.h index e1837634..5ffa8301 100644 --- a/selfdrive/ui/qt/onroad/alerts.h +++ b/selfdrive/ui/qt/onroad/alerts.h @@ -35,10 +35,16 @@ protected: }; void paintEvent(QPaintEvent*) override; - OnroadAlerts::Alert getAlert(const SubMaster &sm, uint64_t started_frame); + OnroadAlerts::Alert getAlert(const SubMaster &sm, const SubMaster &fpsm, uint64_t started_frame); QColor bg; Alert alert = {}; // FrogPilot variables + const QMap frogpilot_alert_colors = { + {cereal::FrogPilotSelfdriveState::AlertStatus::NORMAL, QColor(0x15, 0x15, 0x15, 0xf1)}, + {cereal::FrogPilotSelfdriveState::AlertStatus::USER_PROMPT, QColor(0xDA, 0x6F, 0x25, 0xf1)}, + {cereal::FrogPilotSelfdriveState::AlertStatus::CRITICAL, QColor(0xC9, 0x22, 0x31, 0xf1)}, + {cereal::FrogPilotSelfdriveState::AlertStatus::FROGPILOT, QColor(0x17, 0x86, 0x44, 0xf1)}, + }; }; diff --git a/selfdrive/ui/qt/onroad/model.cc b/selfdrive/ui/qt/onroad/model.cc index 478da7a7..dc25450f 100644 --- a/selfdrive/ui/qt/onroad/model.cc +++ b/selfdrive/ui/qt/onroad/model.cc @@ -50,6 +50,7 @@ void ModelRenderer::draw(QPainter &painter, const QRect &surface_rect) { // FrogPilot variables SubMaster &fpsm = *(frogpilotUIState()->sm); + const cereal::FrogPilotRadarState::Reader &frogpilot_radar_state = fpsm["frogpilotRadarState"].getFrogpilotRadarState(); } // FrogPilot variables @@ -100,6 +101,8 @@ void ModelRenderer::update_model(const cereal::ModelDataV2::Reader &model, const // FrogPilot variables FrogPilotUIState *fs = frogpilotUIState(); SubMaster &fpsm = *(fs->sm); + + const cereal::FrogPilotPlan::Reader &frogpilotPlan = fpsm["frogpilotPlan"].getFrogpilotPlan(); } void ModelRenderer::drawLaneLines(QPainter &painter) { diff --git a/selfdrive/ui/qt/sidebar.cc b/selfdrive/ui/qt/sidebar.cc index 2d217d00..ba0b3449 100644 --- a/selfdrive/ui/qt/sidebar.cc +++ b/selfdrive/ui/qt/sidebar.cc @@ -90,6 +90,8 @@ void Sidebar::updateState(const UIState &s, const FrogPilotUIState &fs) { const SubMaster &fpsm = *(fs.sm); + const cereal::FrogPilotDeviceState::Reader &frogpilotDeviceState = fpsm["frogpilotDeviceState"].getFrogpilotDeviceState(); + auto &sm = *(s.sm); networking = networking ? networking : window()->findChild(""); diff --git a/selfdrive/ui/soundd.py b/selfdrive/ui/soundd.py index 79f2c585..dfe0fef8 100644 --- a/selfdrive/ui/soundd.py +++ b/selfdrive/ui/soundd.py @@ -4,7 +4,7 @@ import time import wave -from cereal import car, messaging +from cereal import car, custom, messaging from openpilot.common.basedir import BASEDIR from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.params import Params @@ -32,6 +32,7 @@ if HARDWARE.get_device_type() in ("tici", "tizi"): AudibleAlert = car.CarControl.HUDControl.AudibleAlert # FrogPilot variables +FrogPilotAudibleAlert = custom.FrogPilotCarControl.HUDControl.AudibleAlert sound_list: dict[int, tuple[str, int | None, float]] = { @@ -134,6 +135,9 @@ class Soundd: new_alert = sm['selfdriveState'].alertSound.raw # FrogPilot variables + new_frogpilot_alert = sm['frogpilotSelfdriveState'].alertSound.raw + if new_alert == AudibleAlert.none and new_frogpilot_alert != FrogPilotAudibleAlert.none: + new_alert = new_frogpilot_alert self.update_alert(new_alert) elif check_selfdrive_timeout_alert(sm): @@ -161,6 +165,7 @@ class Soundd: sm = messaging.SubMaster(['selfdriveState', 'soundPressure']) # FrogPilot variables + sm = sm.extend(['frogpilotSelfdriveState', 'frogpilotPlan']) with self.get_stream(sd) as stream: rk = Ratekeeper(20) diff --git a/system/hardware/hardwared.py b/system/hardware/hardwared.py index 313f9a59..51a81f4c 100755 --- a/system/hardware/hardwared.py +++ b/system/hardware/hardwared.py @@ -213,6 +213,8 @@ def hardware_thread(end_event, hw_queue) -> None: fan_controller = None # FrogPilot variables + sm = sm.extend(['frogpilotPlan']) + pm = pm.extend(['frogpilotDeviceState']) while not end_event.is_set(): sm.update(PANDA_STATES_TIMEOUT) @@ -417,6 +419,9 @@ def hardware_thread(end_event, hw_queue) -> None: pm.send("deviceState", msg) # FrogPilot variables + fpmsg = messaging.new_message('frogpilotDeviceState') + + pm.send("frogpilotDeviceState", fpmsg) # Log to statsd statlog.gauge("free_space_percent", msg.deviceState.freeSpacePercent) diff --git a/system/loggerd/uploader.py b/system/loggerd/uploader.py index 6f8fd9a5..c2c67bdc 100755 --- a/system/loggerd/uploader.py +++ b/system/loggerd/uploader.py @@ -250,6 +250,7 @@ def main(exit_event: threading.Event = None) -> None: backoff = 0.1 # FrogPilot variables + sm = sm.extend(['frogpilotPlan']) while not exit_event.is_set(): sm.update(0) diff --git a/system/manager/manager.py b/system/manager/manager.py old mode 100755 new mode 100644 index 9923bff0..dd85e6ac --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -139,6 +139,8 @@ def manager_thread() -> None: ignition_prev = False # FrogPilot variables + sm = sm.extend(['frogpilotPlan']) + params_memory = Params(memory=True) while True: