FrogPilot structs

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 {

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;

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

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())}

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

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)

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

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)

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):

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

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
 

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<SubMaster, const std::initializer_list<const char *>>({
-    "carControl", "deviceState",
-    "liveDelay",
+    "carControl", "deviceState", "frogpilotCarState", "frogpilotDeviceState",
+    "frogpilotPlan", "frogpilotRadarState", "frogpilotSelfdriveState", "liveDelay",
     "liveParameters", "liveTorqueParameters", "liveTracks", "selfdriveState"
   });
 

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) {

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):

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():

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):

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):

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):

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 = {

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 = []

opendbc_repo/opendbc/car/hyundai/values.py

@@ -69,6 +69,7 @@ class HyundaiSafetyFlags(IntFlag):
 
 
 # FrogPilot variables
+class HyundaiFrogPilotSafetyFlags(IntFlag):
 
 
 class HyundaiFlags(IntFlag):

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

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):

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()

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):

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):

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):

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):

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):

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):

opendbc_repo/opendbc/car/toyota/values.py

@@ -81,6 +81,7 @@ class ToyotaFlags(IntFlag):
 
 
 # FrogPilot variables
+class ToyotaFrogPilotFlags(IntFlag):
 
 
 def dbc_dict(pt, radar):

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()

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 = {

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'])

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

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

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.

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)

selfdrive/controls/plannerd.py

@@ -23,6 +23,7 @@ def main():
                            poll='modelV2')
 
   # FrogPilot variables
+  sm = sm.extend(['frogpilotCarState', 'frogpilotPlan'])
 
   while True:
     sm.update()

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()

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()

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()

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()

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

selfdrive/monitoring/dmonitoringd.py

@@ -16,6 +16,7 @@ def dmonitoringd_thread():
   demo_mode=False
 
   # FrogPilot variables
+  sm = sm.extend(['frogpilotCarState'])
 
   # 20Hz <- dmonitoringmodeld
   while True:

selfdrive/pandad/panda_safety.cc

@@ -66,6 +66,14 @@ void PandaSafety::setSafetyMode(const std::string &params_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<cereal::FrogPilotCarParams>();
+
+  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 &params_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);

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

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)

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

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<cereal::SelfdriveState::AlertSize>(fpss.getAlertSize()), static_cast<cereal::SelfdriveState::AlertStatus>(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<cereal::FrogPilotSelfdriveState::AlertStatus>(alert.status)]));
   p.drawRoundedRect(r, radius, radius);
 
   QLinearGradient g(0, r.y(), 0, r.bottom());

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<cereal::FrogPilotSelfdriveState::AlertStatus, QColor> 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)},
+  };
 };

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) {

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<Networking *>("");

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)

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)

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)

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: