FrogPilot setup - Configure cereal

cereal/car.capnp

@@ -118,7 +118,26 @@ struct CarEvent @0x9b1657f34caf3ad3 {
     paramsdPermanentError @119;
 
     # FrogPilot Events
+    accel30 @120;
+    accel35 @121;
+    accel40 @122;
+    blockUser @123;
+    firefoxSteerSaturated @124;
+    goatSteerSaturated @125;
+    greenLight @126;
+    holidayActive @127;
+    laneChangeBlockedLoud @128;
+    leadDeparting @129;
+    noLaneAvailable @130;
+    openpilotCrashed @131;
+    openpilotCrashedRandomEvents @132;
     pedalInterceptorNoBrake @133;
+    speedLimitChanged @134;
+    torqueNNLoad @135;
+    turningLeft @136;
+    turningRight @137;
+    vCruise69 @138;
+    yourFrogTriedToKillMe @139;
 
     radarCanErrorDEPRECATED @15;
     communityFeatureDisallowedDEPRECATED @62;
@@ -409,6 +428,18 @@ struct CarControl {
       prompt @6;
       promptRepeat @7;
       promptDistracted @8;
+
+      # Random Events
+      angry @9;
+      doc @10;
+      fart @11;
+      firefox @12;
+      nessie @13;
+      noice @14;
+      uwu @15;
+
+      # Other
+      goat @16;
     }
   }
 

cereal/custom.capnp

@@ -8,19 +8,63 @@ $Cxx.namespace("cereal");
 # cereal, so use these if you want custom events in your fork.
 
 # you can rename the struct, but don't change the identifier
-struct CustomReserved0 @0x81c2f05a394cf4af {
+struct FrogPilotCarControl @0x81c2f05a394cf4af {
+  alwaysOnLateral @0 :Bool;
+  speedLimitChanged @1 :Bool;
+  trafficModeActive @2 :Bool;
 }
 
-struct CustomReserved1 @0xaedffd8f31e7b55d {
+struct FrogPilotCarState @0xaedffd8f31e7b55d {
+  struct ButtonEvent {
+    enum Type {
+      lkas @0;
+    }
+  }
+
+  brakeLights @0 :Bool;
+  dashboardSpeedLimit @1 :Float32;
+  distanceLongPressed @2 :Bool;
+  ecoGear @3 :Bool;
+  hasCamera @4 :Bool;
+  sportGear @5 :Bool;
 }
 
-struct CustomReserved2 @0xf35cc4560bbf6ec2 {
+struct FrogPilotDeviceState @0xf35cc4560bbf6ec2 {
+  freeSpace @0 :Int16;
+  usedSpace @1 :Int16;
 }
 
-struct CustomReserved3 @0xda96579883444c35 {
+struct FrogPilotNavigation @0xda96579883444c35 {
+  approachingIntersection @0 :Bool;
+  approachingTurn @1 :Bool;
+  navigationSpeedLimit @2 :Float32;
 }
 
-struct CustomReserved4 @0x80ae746ee2596b11 {
+struct FrogPilotPlan @0x80ae746ee2596b11 {
+  accelerationJerk @0 :Float32;
+  accelerationJerkStock @1 :Float32;
+  adjustedCruise @2 :Float64;
+  conditionalExperimental @3 :Bool;
+  desiredFollowDistance @4 :Int16;
+  laneWidthLeft @5 :Float32;
+  laneWidthRight @6 :Float32;
+  leadDeparting @7 :Bool;
+  maxAcceleration @8 :Float32;
+  minAcceleration @9 :Float32;
+  redLight @10 :Bool;
+  safeObstacleDistance @11 :Int16;
+  safeObstacleDistanceStock @12 :Int16;
+  slcOverridden @13 :Bool;
+  slcOverriddenSpeed @14 :Float64;
+  slcSpeedLimit @15 :Float64;
+  slcSpeedLimitOffset @16 :Float32;
+  speedJerk @17 :Float32;
+  speedJerkStock @18 :Float32;
+  stoppedEquivalenceFactor @19 :Int16;
+  tFollow @20 :Float32;
+  unconfirmedSlcSpeedLimit @21 :Float64;
+  vCruise @22 :Float32;
+  vtscControllingCurve @23 :Bool;
 }
 
 struct CustomReserved5 @0xa5cd762cd951a455 {

cereal/log.capnp

@@ -327,6 +327,12 @@ enum LaneChangeDirection {
   right @2;
 }
 
+enum TurnDirection {
+  none @0;
+  turnLeft @1;
+  turnRight @2;
+}
+
 struct CanData {
   address @0 :UInt32;
   busTime @1 :UInt16;
@@ -738,6 +744,7 @@ struct ControlsState @0x97ff69c53601abf1 {
     normal @0;       # low priority alert for user's convenience
     userPrompt @1;   # mid priority alert that might require user intervention
     critical @2;     # high priority alert that needs immediate user intervention
+    frogpilot @3;    # FrogPilot startup alert
   }
 
   enum AlertSize {
@@ -788,6 +795,7 @@ struct ControlsState @0x97ff69c53601abf1 {
     saturated @7 :Bool;
     actualLateralAccel @9 :Float32;
     desiredLateralAccel @10 :Float32;
+    nnLog @11 :List(Float32);
    }
 
   struct LateralLQRState {
@@ -950,6 +958,7 @@ struct ModelDataV2 {
     hardBrakePredicted @7 :Bool;
     laneChangeState @8 :LaneChangeState;
     laneChangeDirection @9 :LaneChangeDirection;
+    turnDirection @10 :TurnDirection;
 
 
     # deprecated
@@ -2330,11 +2339,11 @@ struct Event {
     customReservedRawData2 @126 :Data;
 
     # *********** Custom: reserved for forks ***********
-    customReserved0 @107 :Custom.CustomReserved0;
+    frogpilotCarControl @107 :Custom.FrogPilotCarControl;
-    customReserved1 @108 :Custom.CustomReserved1;
+    frogpilotCarState @108 :Custom.FrogPilotCarState;
-    customReserved2 @109 :Custom.CustomReserved2;
+    frogpilotDeviceState @109 :Custom.FrogPilotDeviceState;
-    customReserved3 @110 :Custom.CustomReserved3;
+    frogpilotNavigation @110 :Custom.FrogPilotNavigation;
-    customReserved4 @111 :Custom.CustomReserved4;
+    frogpilotPlan @111 :Custom.FrogPilotPlan;
     customReserved5 @112 :Custom.CustomReserved5;
     customReserved6 @113 :Custom.CustomReserved6;
     customReserved7 @114 :Custom.CustomReserved7;

cereal/services.py

@@ -82,6 +82,13 @@ services: dict[str, tuple] = {
   "userFlag": (True, 0., 1),
   "microphone": (True, 10., 10),
 
+  # FrogPilot
+  "frogpilotCarControl": (True, 100., 10),
+  "frogpilotCarState": (True, 100., 10),
+  "frogpilotDeviceState": (True, 2., 1),
+  "frogpilotNavigation": (True, 1., 10),
+  "frogpilotPlan": (True, 20., 5),
+
   # debug
   "uiDebug": (True, 0., 1),
   "testJoystick": (True, 0.),

selfdrive/car/body/carstate.py

@@ -1,4 +1,4 @@
-from cereal import car
+from cereal import car, custom
 from opendbc.can.parser import CANParser
 from openpilot.selfdrive.car.interfaces import CarStateBase
 from openpilot.selfdrive.car.body.values import DBC
@@ -8,6 +8,7 @@ STARTUP_TICKS = 100
 class CarState(CarStateBase):
   def update(self, cp):
     ret = car.CarState.new_message()
+    fp_ret = custom.FrogPilotCarState.new_message()
 
     ret.wheelSpeeds.fl = cp.vl['MOTORS_DATA']['SPEED_L']
     ret.wheelSpeeds.fr = cp.vl['MOTORS_DATA']['SPEED_R']
@@ -28,7 +29,7 @@ class CarState(CarStateBase):
     ret.cruiseState.enabled = True
     ret.cruiseState.available = True
 
-    return ret
+    return ret, fp_ret
 
   @staticmethod
   def get_can_parser(CP):

selfdrive/car/body/interface.py

@@ -26,7 +26,7 @@ class CarInterface(CarInterfaceBase):
     return ret
 
   def _update(self, c):
-    ret = self.CS.update(self.cp)
+    ret, fp_ret = self.CS.update(self.cp)
 
     # wait for everything to init first
     if self.frame > int(5. / DT_CTRL):
@@ -36,7 +36,7 @@ class CarInterface(CarInterfaceBase):
       ret.events[0].enable = True
     self.frame += 1
 
-    return ret
+    return ret, fp_ret
 
   def apply(self, c, now_nanos):
     return self.CC.update(c, self.CS, now_nanos)

selfdrive/car/card.py

@@ -26,7 +26,7 @@ class CarD:
   def __init__(self, CI=None):
     self.can_sock = messaging.sub_sock('can', timeout=20)
     self.sm = messaging.SubMaster(['pandaStates'])
-    self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput'])
+    self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput', 'frogpilotCarState'])
 
     self.can_rcv_timeout_counter = 0      # conseuctive timeout count
     self.can_rcv_cum_timeout_counter = 0  # cumulative timeout count
@@ -85,7 +85,7 @@ class CarD:
 
     # Update carState from CAN
     can_strs = messaging.drain_sock_raw(self.can_sock, wait_for_one=True)
-    self.CS = self.CI.update(self.CC_prev, can_strs)
+    self.CS, self.FPCS = self.CI.update(self.CC_prev, can_strs)
 
     self.sm.update(0)
 
@@ -130,6 +130,12 @@ class CarD:
       co_send.carOutput.actuatorsOutput = self.last_actuators
     self.pm.send('carOutput', co_send)
 
+    # FrogPilot carState
+    fp_cs_send = messaging.new_message('frogpilotCarState')
+    fp_cs_send.valid = self.CS.canValid
+    fp_cs_send.frogpilotCarState = self.FPCS
+    self.pm.send('frogpilotCarState', fp_cs_send)
+
   def controls_update(self, CC: car.CarControl):
     """control update loop, driven by carControl"""
 
@@ -139,4 +145,3 @@ class CarD:
     self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=self.CS.canValid))
 
     self.CC_prev = CC
-

selfdrive/car/chrysler/carstate.py

@@ -1,4 +1,4 @@
-from cereal import car
+from cereal import car, custom
 from openpilot.common.conversions import Conversions as CV
 from opendbc.can.parser import CANParser
 from opendbc.can.can_define import CANDefine
@@ -27,6 +27,7 @@ class CarState(CarStateBase):
   def update(self, cp, cp_cam):
 
     ret = car.CarState.new_message()
+    fp_ret = custom.FrogPilotCarState.new_message()
 
     self.prev_distance_button = self.distance_button
     self.distance_button = cp.vl["CRUISE_BUTTONS"]["ACC_Distance_Dec"]
@@ -101,7 +102,7 @@ class CarState(CarStateBase):
     self.lkas_car_model = cp_cam.vl["DAS_6"]["CAR_MODEL"]
     self.button_counter = cp.vl["CRUISE_BUTTONS"]["COUNTER"]
 
-    return ret
+    return ret, fp_ret
 
   @staticmethod
   def get_cruise_messages():

selfdrive/car/chrysler/interface.py

@@ -1,5 +1,5 @@
 #!/usr/bin/env python3
-from cereal import car
+from cereal import car, custom
 from panda import Panda
 from openpilot.selfdrive.car import create_button_events, get_safety_config
 from openpilot.selfdrive.car.chrysler.values import CAR, RAM_HD, RAM_DT, RAM_CARS, ChryslerFlags
@@ -76,7 +76,7 @@ class CarInterface(CarInterfaceBase):
     return ret
 
   def _update(self, c):
-    ret = self.CS.update(self.cp, self.cp_cam)
+    ret, fp_ret = self.CS.update(self.cp, self.cp_cam)
 
     ret.buttonEvents = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise})
 
@@ -93,7 +93,7 @@ class CarInterface(CarInterfaceBase):
 
     ret.events = events.to_msg()
 
-    return ret
+    return ret, fp_ret
 
   def apply(self, c, now_nanos):
     return self.CC.update(c, self.CS, now_nanos)

selfdrive/car/ford/carstate.py

@@ -1,4 +1,4 @@
-from cereal import car
+from cereal import car, custom
 from opendbc.can.can_define import CANDefine
 from opendbc.can.parser import CANParser
 from openpilot.common.conversions import Conversions as CV
@@ -24,6 +24,7 @@ class CarState(CarStateBase):
 
   def update(self, cp, cp_cam):
     ret = car.CarState.new_message()
+    fp_ret = custom.FrogPilotCarState.new_message()
 
     # Occasionally on startup, the ABS module recalibrates the steering pinion offset, so we need to block engagement
     # The vehicle usually recovers out of this state within a minute of normal driving
@@ -106,7 +107,7 @@ class CarState(CarStateBase):
     self.acc_tja_status_stock_values = cp_cam.vl["ACCDATA_3"]
     self.lkas_status_stock_values = cp_cam.vl["IPMA_Data"]
 
-    return ret
+    return ret, fp_ret
 
   @staticmethod
   def get_can_parser(CP):

selfdrive/car/ford/interface.py

@@ -1,4 +1,4 @@
-from cereal import car
+from cereal import car, custom
 from panda import Panda
 from openpilot.common.conversions import Conversions as CV
 from openpilot.selfdrive.car import create_button_events, get_safety_config
@@ -60,7 +60,7 @@ class CarInterface(CarInterfaceBase):
     return ret
 
   def _update(self, c):
-    ret = self.CS.update(self.cp, self.cp_cam)
+    ret, fp_ret = self.CS.update(self.cp, self.cp_cam)
 
     ret.buttonEvents = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise})
 
@@ -70,7 +70,7 @@ class CarInterface(CarInterfaceBase):
 
     ret.events = events.to_msg()
 
-    return ret
+    return ret, fp_ret
 
   def apply(self, c, now_nanos):
     return self.CC.update(c, self.CS, now_nanos)

selfdrive/car/gm/carstate.py

@@ -1,5 +1,5 @@
 import copy
-from cereal import car
+from cereal import car, custom
 from openpilot.common.conversions import Conversions as CV
 from openpilot.common.numpy_fast import mean
 from opendbc.can.can_define import CANDefine
@@ -35,6 +35,7 @@ class CarState(CarStateBase):
 
   def update(self, pt_cp, cam_cp, loopback_cp):
     ret = car.CarState.new_message()
+    fp_ret = custom.FrogPilotCarState.new_message()
 
     self.prev_cruise_buttons = self.cruise_buttons
     self.prev_distance_button = self.distance_button
@@ -168,7 +169,7 @@ class CarState(CarStateBase):
         ret.leftBlindspot = cam_cp.vl["BCMBlindSpotMonitor"]["LeftBSM"] == 1
         ret.rightBlindspot = cam_cp.vl["BCMBlindSpotMonitor"]["RightBSM"] == 1
 
-    return ret
+    return ret, fp_ret
 
   @staticmethod
   def get_cam_can_parser(CP):

selfdrive/car/gm/interface.py

@@ -1,6 +1,6 @@
 #!/usr/bin/env python3
 import os
-from cereal import car
+from cereal import car, custom
 from math import fabs, exp
 from panda import Panda
 
@@ -294,7 +294,7 @@ class CarInterface(CarInterfaceBase):
 
   # returns a car.CarState
   def _update(self, c):
-    ret = self.CS.update(self.cp, self.cp_cam, self.cp_loopback)
+    ret, fp_ret = self.CS.update(self.cp, self.cp_cam, self.cp_loopback)
 
     # Don't add event if transitioning from INIT, unless it's to an actual button
     if self.CS.cruise_buttons != CruiseButtons.UNPRESS or self.CS.prev_cruise_buttons != CruiseButtons.INIT:
@@ -335,7 +335,7 @@ class CarInterface(CarInterfaceBase):
 
     ret.events = events.to_msg()
 
-    return ret
+    return ret, fp_ret
 
   def apply(self, c, now_nanos):
     return self.CC.update(c, self.CS, now_nanos)

selfdrive/car/honda/carstate.py

@@ -1,6 +1,6 @@
 from collections import defaultdict
 
-from cereal import car
+from cereal import car, custom
 from openpilot.common.conversions import Conversions as CV
 from openpilot.common.numpy_fast import interp
 from opendbc.can.can_define import CANDefine
@@ -110,6 +110,7 @@ class CarState(CarStateBase):
 
   def update(self, cp, cp_cam, cp_body):
     ret = car.CarState.new_message()
+    fp_ret = custom.FrogPilotCarState.new_message()
 
     # car params
     v_weight_v = [0., 1.]  # don't trust smooth speed at low values to avoid premature zero snapping
@@ -267,7 +268,7 @@ class CarState(CarStateBase):
       ret.leftBlindspot = cp_body.vl["BSM_STATUS_LEFT"]["BSM_ALERT"] == 1
       ret.rightBlindspot = cp_body.vl["BSM_STATUS_RIGHT"]["BSM_ALERT"] == 1
 
-    return ret
+    return ret, fp_ret
 
   def get_can_parser(self, CP):
     messages = get_can_messages(CP, self.gearbox_msg)

selfdrive/car/honda/interface.py

@@ -1,5 +1,5 @@
 #!/usr/bin/env python3
-from cereal import car
+from cereal import car, custom
 from panda import Panda
 from openpilot.common.conversions import Conversions as CV
 from openpilot.common.numpy_fast import interp
@@ -233,7 +233,7 @@ class CarInterface(CarInterfaceBase):
 
   # returns a car.CarState
   def _update(self, c):
-    ret = self.CS.update(self.cp, self.cp_cam, self.cp_body)
+    ret, fp_ret = self.CS.update(self.cp, self.cp_cam, self.cp_body)
 
     ret.buttonEvents = [
       *create_button_events(self.CS.cruise_buttons, self.CS.prev_cruise_buttons, BUTTONS_DICT),
@@ -262,7 +262,7 @@ class CarInterface(CarInterfaceBase):
 
     ret.events = events.to_msg()
 
-    return ret
+    return ret, fp_ret
 
   # pass in a car.CarControl
   # to be called @ 100hz

selfdrive/car/hyundai/carstate.py

@@ -2,7 +2,7 @@ from collections import deque
 import copy
 import math
 
-from cereal import car
+from cereal import car, custom
 from openpilot.common.conversions import Conversions as CV
 from opendbc.can.parser import CANParser
 from opendbc.can.can_define import CANDefine
@@ -57,6 +57,7 @@ class CarState(CarStateBase):
       return self.update_canfd(cp, cp_cam)
 
     ret = car.CarState.new_message()
+    fp_ret = custom.FrogPilotCarState.new_message()
     cp_cruise = cp_cam if self.CP.carFingerprint in CAMERA_SCC_CAR else cp
     self.is_metric = cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"] == 0
     speed_conv = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS
@@ -164,10 +165,11 @@ class CarState(CarStateBase):
     self.cruise_buttons.extend(cp.vl_all["CLU11"]["CF_Clu_CruiseSwState"])
     self.main_buttons.extend(cp.vl_all["CLU11"]["CF_Clu_CruiseSwMain"])
 
-    return ret
+    return ret, fp_ret
 
   def update_canfd(self, cp, cp_cam):
     ret = car.CarState.new_message()
+    fp_ret = custom.FrogPilotCarState.new_message()
 
     self.is_metric = cp.vl["CRUISE_BUTTONS_ALT"]["DISTANCE_UNIT"] != 1
     speed_factor = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS
@@ -246,7 +248,7 @@ class CarState(CarStateBase):
       self.hda2_lfa_block_msg = copy.copy(cp_cam.vl["CAM_0x362"] if self.CP.flags & HyundaiFlags.CANFD_HDA2_ALT_STEERING
                                           else cp_cam.vl["CAM_0x2a4"])
 
-    return ret
+    return ret, fp_ret
 
   def get_can_parser(self, CP):
     if CP.carFingerprint in CANFD_CAR:

selfdrive/car/hyundai/interface.py

@@ -1,4 +1,4 @@
-from cereal import car
+from cereal import car, custom
 from panda import Panda
 from openpilot.selfdrive.car.hyundai.hyundaicanfd import CanBus
 from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, CAR, DBC, CANFD_CAR, CAMERA_SCC_CAR, CANFD_RADAR_SCC_CAR, \
@@ -153,7 +153,7 @@ class CarInterface(CarInterfaceBase):
       disable_ecu(logcan, sendcan, bus=CanBus(CP).ECAN, addr=0x7B1, com_cont_req=b'\x28\x83\x01')
 
   def _update(self, c):
-    ret = self.CS.update(self.cp, self.cp_cam)
+    ret, fp_ret = self.CS.update(self.cp, self.cp_cam)
 
     if self.CS.CP.openpilotLongitudinalControl:
       ret.buttonEvents = create_button_events(self.CS.cruise_buttons[-1], self.CS.prev_cruise_buttons, BUTTONS_DICT)
@@ -174,7 +174,7 @@ class CarInterface(CarInterfaceBase):
 
     ret.events = events.to_msg()
 
-    return ret
+    return ret, fp_ret
 
   def apply(self, c, now_nanos):
     return self.CC.update(c, self.CS, now_nanos)

selfdrive/car/interfaces.py

@@ -224,7 +224,7 @@ class CarInterfaceBase(ABC):
         cp.update_strings(can_strings)
 
     # get CarState
-    ret = self._update(c)
+    ret, fp_ret = self._update(c)
 
     ret.canValid = all(cp.can_valid for cp in self.can_parsers if cp is not None)
     ret.canTimeout = any(cp.bus_timeout for cp in self.can_parsers if cp is not None)
@@ -245,10 +245,11 @@ class CarInterfaceBase(ABC):
 
     # copy back for next iteration
     reader = ret.as_reader()
+    frogpilot_reader = fp_ret.as_reader()
     if self.CS is not None:
       self.CS.out = reader
 
-    return reader
+    return reader, frogpilot_reader
 
   @abstractmethod
   def apply(self, c: car.CarControl, now_nanos: int) -> tuple[car.CarControl.Actuators, list[bytes]]:

selfdrive/car/mazda/carstate.py

@@ -1,4 +1,4 @@
-from cereal import car
+from cereal import car, custom
 from openpilot.common.conversions import Conversions as CV
 from opendbc.can.can_define import CANDefine
 from opendbc.can.parser import CANParser
@@ -24,6 +24,7 @@ class CarState(CarStateBase):
   def update(self, cp, cp_cam):
 
     ret = car.CarState.new_message()
+    fp_ret = custom.FrogPilotCarState.new_message()
 
     self.prev_distance_button = self.distance_button
     self.distance_button = cp.vl["CRZ_BTNS"]["DISTANCE_LESS"]
@@ -110,7 +111,7 @@ class CarState(CarStateBase):
     self.cam_laneinfo = cp_cam.vl["CAM_LANEINFO"]
     ret.steerFaultPermanent = cp_cam.vl["CAM_LKAS"]["ERR_BIT_1"] == 1
 
-    return ret
+    return ret, fp_ret
 
   @staticmethod
   def get_can_parser(CP):

selfdrive/car/mazda/interface.py

@@ -1,5 +1,5 @@
 #!/usr/bin/env python3
-from cereal import car
+from cereal import car, custom
 from openpilot.common.conversions import Conversions as CV
 from openpilot.selfdrive.car.mazda.values import CAR, LKAS_LIMITS
 from openpilot.selfdrive.car import create_button_events, get_safety_config
@@ -32,7 +32,7 @@ class CarInterface(CarInterfaceBase):
 
   # returns a car.CarState
   def _update(self, c):
-    ret = self.CS.update(self.cp, self.cp_cam)
+    ret, fp_ret = self.CS.update(self.cp, self.cp_cam)
 
      # TODO: add button types for inc and dec
     ret.buttonEvents = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise})
@@ -47,7 +47,7 @@ class CarInterface(CarInterfaceBase):
 
     ret.events = events.to_msg()
 
-    return ret
+    return ret, fp_ret
 
   def apply(self, c, now_nanos):
     return self.CC.update(c, self.CS, now_nanos)

selfdrive/car/nissan/carstate.py

@@ -1,6 +1,6 @@
 import copy
 from collections import deque
-from cereal import car
+from cereal import car, custom
 from opendbc.can.can_define import CANDefine
 from openpilot.selfdrive.car.interfaces import CarStateBase
 from openpilot.common.conversions import Conversions as CV
@@ -25,6 +25,7 @@ class CarState(CarStateBase):
 
   def update(self, cp, cp_adas, cp_cam):
     ret = car.CarState.new_message()
+    fp_ret = custom.FrogPilotCarState.new_message()
 
     self.prev_distance_button = self.distance_button
     self.distance_button = cp.vl["CRUISE_THROTTLE"]["FOLLOW_DISTANCE_BUTTON"]
@@ -121,7 +122,7 @@ class CarState(CarStateBase):
       self.lkas_hud_msg = copy.copy(cp_adas.vl["PROPILOT_HUD"])
       self.lkas_hud_info_msg = copy.copy(cp_adas.vl["PROPILOT_HUD_INFO_MSG"])
 
-    return ret
+    return ret, fp_ret
 
   @staticmethod
   def get_can_parser(CP):

selfdrive/car/nissan/interface.py

@@ -1,4 +1,4 @@
-from cereal import car
+from cereal import car, custom
 from panda import Panda
 from openpilot.selfdrive.car import create_button_events, get_safety_config
 from openpilot.selfdrive.car.interfaces import CarInterfaceBase
@@ -30,7 +30,7 @@ class CarInterface(CarInterfaceBase):
 
   # returns a car.CarState
   def _update(self, c):
-    ret = self.CS.update(self.cp, self.cp_adas, self.cp_cam)
+    ret, fp_ret = self.CS.update(self.cp, self.cp_adas, self.cp_cam)
 
     ret.buttonEvents = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise})
 
@@ -41,7 +41,7 @@ class CarInterface(CarInterfaceBase):
 
     ret.events = events.to_msg()
 
-    return ret
+    return ret, fp_ret
 
   def apply(self, c, now_nanos):
     return self.CC.update(c, self.CS, now_nanos)

selfdrive/car/subaru/carstate.py

@@ -1,5 +1,5 @@
 import copy
-from cereal import car
+from cereal import car, custom
 from opendbc.can.can_define import CANDefine
 from openpilot.common.conversions import Conversions as CV
 from openpilot.selfdrive.car.interfaces import CarStateBase
@@ -18,6 +18,7 @@ class CarState(CarStateBase):
 
   def update(self, cp, cp_cam, cp_body):
     ret = car.CarState.new_message()
+    fp_ret = custom.FrogPilotCarState.new_message()
 
     throttle_msg = cp.vl["Throttle"] if not (self.CP.flags & SubaruFlags.HYBRID) else cp_body.vl["Throttle_Hybrid"]
     ret.gas = throttle_msg["Throttle_Pedal"] / 255.
@@ -125,7 +126,7 @@ class CarState(CarStateBase):
     if self.CP.flags & SubaruFlags.SEND_INFOTAINMENT:
       self.es_infotainment_msg = copy.copy(cp_cam.vl["ES_Infotainment"])
 
-    return ret
+    return ret, fp_ret
 
   @staticmethod
   def get_common_global_body_messages(CP):
@@ -226,4 +227,3 @@ class CarState(CarStateBase):
       ]
 
     return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus.alt)
-

selfdrive/car/subaru/interface.py

@@ -1,4 +1,4 @@
-from cereal import car
+from cereal import car, custom
 from panda import Panda
 from openpilot.selfdrive.car import get_safety_config
 from openpilot.selfdrive.car.disable_ecu import disable_ecu
@@ -104,11 +104,11 @@ class CarInterface(CarInterfaceBase):
   # returns a car.CarState
   def _update(self, c):
 
-    ret = self.CS.update(self.cp, self.cp_cam, self.cp_body)
+    ret, fp_ret = self.CS.update(self.cp, self.cp_cam, self.cp_body)
 
     ret.events = self.create_common_events(ret).to_msg()
 
-    return ret
+    return ret, fp_ret
 
   @staticmethod
   def init(CP, logcan, sendcan):

selfdrive/car/tesla/carstate.py

@@ -1,6 +1,6 @@
 import copy
 from collections import deque
-from cereal import car
+from cereal import car, custom
 from openpilot.common.conversions import Conversions as CV
 from openpilot.selfdrive.car.tesla.values import CAR, DBC, CANBUS, GEAR_MAP, DOORS, BUTTONS
 from openpilot.selfdrive.car.interfaces import CarStateBase
@@ -22,6 +22,7 @@ class CarState(CarStateBase):
 
   def update(self, cp, cp_cam):
     ret = car.CarState.new_message()
+    fp_ret = custom.FrogPilotCarState.new_message()
 
     # Vehicle speed
     ret.vEgoRaw = cp.vl["ESP_B"]["ESP_vehicleSpeed"] * CV.KPH_TO_MS
@@ -102,7 +103,7 @@ class CarState(CarStateBase):
     self.acc_state = cp_cam.vl["DAS_control"]["DAS_accState"]
     self.das_control_counters.extend(cp_cam.vl_all["DAS_control"]["DAS_controlCounter"])
 
-    return ret
+    return ret, fp_ret
 
   @staticmethod
   def get_can_parser(CP):

selfdrive/car/tesla/interface.py

@@ -45,11 +45,11 @@ class CarInterface(CarInterfaceBase):
     return ret
 
   def _update(self, c):
-    ret = self.CS.update(self.cp, self.cp_cam)
+    ret, fp_ret = self.CS.update(self.cp, self.cp_cam)
 
     ret.events = self.create_common_events(ret).to_msg()
 
-    return ret
+    return ret, fp_ret
 
   def apply(self, c, now_nanos):
     return self.CC.update(c, self.CS, now_nanos)

selfdrive/car/toyota/carstate.py

@@ -1,6 +1,6 @@
 import copy
 
-from cereal import car
+from cereal import car, custom
 from openpilot.common.conversions import Conversions as CV
 from openpilot.common.numpy_fast import mean
 from openpilot.common.filter_simple import FirstOrderFilter
@@ -51,6 +51,7 @@ class CarState(CarStateBase):
 
   def update(self, cp, cp_cam):
     ret = car.CarState.new_message()
+    fp_ret = custom.FrogPilotCarState.new_message()
 
     ret.doorOpen = any([cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_FL"], cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_FR"],
                         cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_RL"], cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_RR"]])
@@ -179,7 +180,7 @@ class CarState(CarStateBase):
       else:
         self.distance_button = cp.vl["SDSU"]["FD_BUTTON"]
 
-    return ret
+    return ret, fp_ret
 
   @staticmethod
   def get_can_parser(CP):

selfdrive/car/toyota/interface.py

@@ -1,4 +1,4 @@
-from cereal import car
+from cereal import car, custom
 from panda import Panda
 from panda.python import uds
 from openpilot.selfdrive.car.toyota.values import Ecu, CAR, DBC, ToyotaFlags, CarControllerParams, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, \
@@ -174,7 +174,7 @@ class CarInterface(CarInterfaceBase):
 
   # returns a car.CarState
   def _update(self, c):
-    ret = self.CS.update(self.cp, self.cp_cam)
+    ret, fp_ret = self.CS.update(self.cp, self.cp_cam)
 
     if self.CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR) or (self.CP.flags & ToyotaFlags.SMART_DSU and not self.CP.flags & ToyotaFlags.RADAR_CAN_FILTER):
       ret.buttonEvents = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise})
@@ -203,7 +203,7 @@ class CarInterface(CarInterfaceBase):
 
     ret.events = events.to_msg()
 
-    return ret
+    return ret, fp_ret
 
   # pass in a car.CarControl
   # to be called @ 100hz

selfdrive/car/volkswagen/carstate.py

@@ -1,5 +1,5 @@
 import numpy as np
-from cereal import car
+from cereal import car, custom
 from openpilot.common.conversions import Conversions as CV
 from openpilot.selfdrive.car.interfaces import CarStateBase
 from opendbc.can.parser import CANParser
@@ -37,6 +37,7 @@ class CarState(CarStateBase):
       return self.update_pq(pt_cp, cam_cp, ext_cp, trans_type)
 
     ret = car.CarState.new_message()
+    fp_ret = custom.FrogPilotCarState.new_message()
     # Update vehicle speed and acceleration from ABS wheel speeds.
     ret.wheelSpeeds = self.get_wheel_speeds(
       pt_cp.vl["ESP_19"]["ESP_VL_Radgeschw_02"],
@@ -150,10 +151,11 @@ class CarState(CarStateBase):
     self.upscale_lead_car_signal = bool(pt_cp.vl["Kombi_03"]["KBI_Variante"])
 
     self.frame += 1
-    return ret
+    return ret, fp_ret
 
   def update_pq(self, pt_cp, cam_cp, ext_cp, trans_type):
     ret = car.CarState.new_message()
+    fp_ret = custom.FrogPilotCarState.new_message()
     # Update vehicle speed and acceleration from ABS wheel speeds.
     ret.wheelSpeeds = self.get_wheel_speeds(
       pt_cp.vl["Bremse_3"]["Radgeschw__VL_4_1"],
@@ -249,7 +251,7 @@ class CarState(CarStateBase):
     ret.espDisabled = bool(pt_cp.vl["Bremse_1"]["ESP_Passiv_getastet"])
 
     self.frame += 1
-    return ret
+    return ret, fp_ret
 
   def update_hca_state(self, hca_status):
     # Treat INITIALIZING and FAULT as temporary for worst likely EPS recovery time, for cars without factory Lane Assist

selfdrive/car/volkswagen/interface.py

@@ -106,7 +106,7 @@ class CarInterface(CarInterfaceBase):
 
   # returns a car.CarState
   def _update(self, c):
-    ret = self.CS.update(self.cp, self.cp_cam, self.cp_ext, self.CP.transmissionType)
+    ret, fp_ret = self.CS.update(self.cp, self.cp_cam, self.cp_ext, self.CP.transmissionType)
 
     events = self.create_common_events(ret, extra_gears=[GearShifter.eco, GearShifter.sport, GearShifter.manumatic],
                                        pcm_enable=not self.CS.CP.openpilotLongitudinalControl,
@@ -131,7 +131,7 @@ class CarInterface(CarInterfaceBase):
 
     ret.events = events.to_msg()
 
-    return ret
+    return ret, fp_ret
 
   def apply(self, c, now_nanos):
     new_actuators, can_sends, self.eps_timer_soft_disable_alert = self.CC.update(c, self.CS, self.ext_bus, now_nanos)

selfdrive/controls/controlsd.py

@@ -7,7 +7,7 @@ from typing import SupportsFloat
 
 import cereal.messaging as messaging
 
-from cereal import car, log
+from cereal import car, custom, log
 from cereal.visionipc import VisionIpcClient, VisionStreamType
 
 
@@ -76,7 +76,7 @@ class Controls:
     self.branch = get_short_branch()
 
     # Setup sockets
-    self.pm = messaging.PubMaster(['controlsState', 'carControl', 'onroadEvents'])
+    self.pm = messaging.PubMaster(['controlsState', 'carControl', 'onroadEvents', 'frogpilotCarControl'])
 
     self.sensor_packets = ["accelerometer", "gyroscope"]
     self.camera_packets = ["roadCameraState", "driverCameraState", "wideRoadCameraState"]
@@ -89,7 +89,7 @@ class Controls:
     self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration',
                                    'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'liveLocationKalman',
                                    'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters',
-                                   'testJoystick'] + self.camera_packets + self.sensor_packets,
+                                   'testJoystick', 'frogpilotCarState', 'frogpilotPlan'] + self.camera_packets + self.sensor_packets,
                                   ignore_alive=ignore, ignore_avg_freq=ignore+['radarState', 'testJoystick'], ignore_valid=['testJoystick', ],
                                   frequency=int(1/DT_CTRL))
 
@@ -113,6 +113,7 @@ class Controls:
 
     self.CC = car.CarControl.new_message()
     self.CS_prev = car.CarState.new_message()
+    self.FPCC = custom.FrogPilotCarControl.new_message()
     self.AM = AlertManager()
     self.events = Events()
 
@@ -811,6 +812,12 @@ class Controls:
     # copy CarControl to pass to CarInterface on the next iteration
     self.CC = CC
 
+    # frogpilotCarControl
+    fpcc_send = messaging.new_message('frogpilotCarControl')
+    fpcc_send.valid = CS.canValid
+    fpcc_send.frogpilotCarControl = self.FPCC
+    self.pm.send('frogpilotCarControl', fpcc_send)
+
   def step(self):
     start_time = time.monotonic()
 

selfdrive/controls/lib/desire_helper.py

@@ -40,7 +40,7 @@ class DesireHelper:
     self.prev_one_blinker = False
     self.desire = log.Desire.none
 
-  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/plannerd.py

@@ -28,7 +28,7 @@ def plannerd_thread():
 
   longitudinal_planner = LongitudinalPlanner(CP)
   pm = messaging.PubMaster(['longitudinalPlan', 'uiPlan'])
-  sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2'],
+  sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2', 'frogpilotCarControl', 'frogpilotPlan'],
                            poll='modelV2', ignore_avg_freq=['radarState'])
 
   while True:

selfdrive/modeld/modeld.py

@@ -154,7 +154,7 @@ def main(demo=False):
 
   # messaging
   pm = PubMaster(["modelV2", "cameraOdometry"])
-  sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "navModel", "navInstruction", "carControl"])
+  sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "navModel", "navInstruction", "carControl", "frogpilotPlan"])
 
   publish_state = PublishState()
   params = Params()
@@ -299,7 +299,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
 

selfdrive/navd/navd.py

@@ -304,6 +304,11 @@ class RouteEngine:
           self.params.remove("NavDestination")
           self.clear_route()
 
+    frogpilot_plan_send = messaging.new_message('frogpilotNavigation')
+    frogpilotNavigation = frogpilot_plan_send.frogpilotNavigation
+
+    self.pm.send('frogpilotNavigation', frogpilot_plan_send)
+
   def send_route(self):
     coords = []
 
@@ -354,7 +359,7 @@ class RouteEngine:
 
 
 def main():
-  pm = messaging.PubMaster(['navInstruction', 'navRoute'])
+  pm = messaging.PubMaster(['navInstruction', 'navRoute', 'frogpilotNavigation'])
   sm = messaging.SubMaster(['liveLocationKalman', 'managerState'])
 
   rk = Ratekeeper(1.0)

selfdrive/thermald/thermald.py

@@ -163,7 +163,7 @@ def hw_state_thread(end_event, hw_queue):
 
 
 def thermald_thread(end_event, hw_queue) -> None:
-  pm = messaging.PubMaster(['deviceState'])
+  pm = messaging.PubMaster(['deviceState', 'frogpilotDeviceState'])
   sm = messaging.SubMaster(["peripheralState", "gpsLocationExternal", "controlsState", "pandaStates"], poll="pandaStates")
 
   count = 0
@@ -242,6 +242,10 @@ def thermald_thread(end_event, hw_queue) -> None:
     except queue.Empty:
       pass
 
+    fpmsg = messaging.new_message('frogpilotDeviceState')
+
+    pm.send("frogpilotDeviceState", fpmsg)
+
     msg.deviceState.freeSpacePercent = get_available_percent(default=100.0)
     msg.deviceState.memoryUsagePercent = int(round(psutil.virtual_memory().percent))
     msg.deviceState.gpuUsagePercent = int(round(HARDWARE.get_gpu_usage_percent()))

selfdrive/ui/qt/onroad.cc

@@ -140,6 +140,9 @@ void OnroadWindow::primeChanged(bool prime) {
 }
 
 void OnroadWindow::paintEvent(QPaintEvent *event) {
+  UIState *s = uiState();
+  SubMaster &sm = *(s->sm);
+
   QPainter p(this);
   p.fillRect(rect(), QColor(bg.red(), bg.green(), bg.blue(), 255));
 }

selfdrive/ui/qt/sidebar.cc

@@ -79,6 +79,9 @@ void Sidebar::updateState(const UIState &s) {
   int strength = (int)deviceState.getNetworkStrength();
   setProperty("netStrength", strength > 0 ? strength + 1 : 0);
 
+  // FrogPilot properties
+  auto frogpilotDeviceState = sm["frogpilotDeviceState"].getFrogpilotDeviceState();
+
   ItemStatus connectStatus;
   auto last_ping = deviceState.getLastAthenaPingTime();
   if (last_ping == 0) {

selfdrive/ui/ui.cc

@@ -198,9 +198,33 @@ static void update_state(UIState *s) {
   } else if ((s->sm->frame - s->sm->rcv_frame("pandaStates")) > 5*UI_FREQ) {
     scene.pandaType = cereal::PandaState::PandaType::UNKNOWN;
   }
+  if (sm.updated("carControl")) {
+    auto carControl = sm["carControl"].getCarControl();
+  }
   if (sm.updated("carParams")) {
     scene.longitudinal_control = sm["carParams"].getCarParams().getOpenpilotLongitudinalControl();
   }
+  if (sm.updated("carState")) {
+    auto carState = sm["carState"].getCarState();
+  }
+  if (sm.updated("controlsState")) {
+    auto controlsState = sm["controlsState"].getControlsState();
+  }
+  if (sm.updated("deviceState")) {
+    auto deviceState = sm["deviceState"].getDeviceState();
+  }
+  if (sm.updated("frogpilotCarControl")) {
+    auto frogpilotCarControl = sm["frogpilotCarControl"].getFrogpilotCarControl();
+  }
+  if (sm.updated("frogpilotPlan")) {
+    auto frogpilotPlan = sm["frogpilotPlan"].getFrogpilotPlan();
+  }
+  if (sm.updated("liveLocationKalman")) {
+    auto liveLocationKalman = sm["liveLocationKalman"].getLiveLocationKalman();
+  }
+  if (sm.updated("liveTorqueParameters")) {
+    auto torque_params = sm["liveTorqueParameters"].getLiveTorqueParameters();
+  }
   if (sm.updated("wideRoadCameraState")) {
     auto cam_state = sm["wideRoadCameraState"].getWideRoadCameraState();
     float scale = (cam_state.getSensor() == cereal::FrameData::ImageSensor::AR0231) ? 6.0f : 1.0f;
@@ -248,7 +272,8 @@ UIState::UIState(QObject *parent) : QObject(parent) {
   sm = std::make_unique<SubMaster, const std::initializer_list<const char *>>({
     "modelV2", "controlsState", "liveCalibration", "radarState", "deviceState",
     "pandaStates", "carParams", "driverMonitoringState", "carState", "liveLocationKalman", "driverStateV2",
-    "wideRoadCameraState", "managerState", "navInstruction", "navRoute", "uiPlan",
+    "wideRoadCameraState", "managerState", "navInstruction", "navRoute", "uiPlan", "carControl", "liveTorqueParameters",
+    "frogpilotCarControl", "frogpilotDeviceState", "frogpilotPlan",
   });
 
   Params params;