diff --git a/cereal/car.capnp b/cereal/car.capnp index 1e2502a..f90e71d 100644 --- a/cereal/car.capnp +++ b/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; } } diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 369222a..7074104 100644 --- a/cereal/custom.capnp +++ b/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 { diff --git a/cereal/log.capnp b/cereal/log.capnp index 42313c7..f765ff3 100644 --- a/cereal/log.capnp +++ b/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; - customReserved1 @108 :Custom.CustomReserved1; - customReserved2 @109 :Custom.CustomReserved2; - customReserved3 @110 :Custom.CustomReserved3; - customReserved4 @111 :Custom.CustomReserved4; + frogpilotCarControl @107 :Custom.FrogPilotCarControl; + frogpilotCarState @108 :Custom.FrogPilotCarState; + frogpilotDeviceState @109 :Custom.FrogPilotDeviceState; + frogpilotNavigation @110 :Custom.FrogPilotNavigation; + frogpilotPlan @111 :Custom.FrogPilotPlan; customReserved5 @112 :Custom.CustomReserved5; customReserved6 @113 :Custom.CustomReserved6; customReserved7 @114 :Custom.CustomReserved7; diff --git a/cereal/services.py b/cereal/services.py index 77ced2b..3e0aaee 100755 --- a/cereal/services.py +++ b/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.), diff --git a/selfdrive/car/body/carstate.py b/selfdrive/car/body/carstate.py index fca9bcc..e184276 100644 --- a/selfdrive/car/body/carstate.py +++ b/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): diff --git a/selfdrive/car/body/interface.py b/selfdrive/car/body/interface.py index 4d72d2f..62f9aac 100644 --- a/selfdrive/car/body/interface.py +++ b/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) diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 8233550..1063ebf 100755 --- a/selfdrive/car/card.py +++ b/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 - diff --git a/selfdrive/car/chrysler/carstate.py b/selfdrive/car/chrysler/carstate.py index 91b922c..7a94b0e 100644 --- a/selfdrive/car/chrysler/carstate.py +++ b/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(): diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index 198bf63..de1c4d6 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/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) diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py index b3ee6a4..c0f0f15 100644 --- a/selfdrive/car/ford/carstate.py +++ b/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): diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index ed8b010..3c52d2d 100644 --- a/selfdrive/car/ford/interface.py +++ b/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) diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index d976f63..f2124d0 100644 --- a/selfdrive/car/gm/carstate.py +++ b/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): diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index d697724..42df06d 100755 --- a/selfdrive/car/gm/interface.py +++ b/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) diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index d429da3..d5ecaec 100644 --- a/selfdrive/car/honda/carstate.py +++ b/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) diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index d316626..eb8c5fb 100755 --- a/selfdrive/car/honda/interface.py +++ b/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 diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 64a9fdf..b3e519f 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/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: diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 69b5132..c25a8ce 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/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) diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 97c9e84..cf5cd32 100644 --- a/selfdrive/car/interfaces.py +++ b/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]]: diff --git a/selfdrive/car/mazda/carstate.py b/selfdrive/car/mazda/carstate.py index 83b238f..c055152 100644 --- a/selfdrive/car/mazda/carstate.py +++ b/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): diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py index 12d156f..29ac400 100755 --- a/selfdrive/car/mazda/interface.py +++ b/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) diff --git a/selfdrive/car/nissan/carstate.py b/selfdrive/car/nissan/carstate.py index 694d6c3..2f85fa7 100644 --- a/selfdrive/car/nissan/carstate.py +++ b/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): diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py index 3e82b51..ad4a75d 100644 --- a/selfdrive/car/nissan/interface.py +++ b/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) diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py index 821ff2c..d969616 100644 --- a/selfdrive/car/subaru/carstate.py +++ b/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) - diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index 30e186b..bb93abf 100644 --- a/selfdrive/car/subaru/interface.py +++ b/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): diff --git a/selfdrive/car/tesla/carstate.py b/selfdrive/car/tesla/carstate.py index bee652f..9a6990a 100644 --- a/selfdrive/car/tesla/carstate.py +++ b/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): diff --git a/selfdrive/car/tesla/interface.py b/selfdrive/car/tesla/interface.py index f989886..09c46fb 100755 --- a/selfdrive/car/tesla/interface.py +++ b/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) diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 5d99467..3c55223 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/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): diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 7ad9fee..60f790e 100644 --- a/selfdrive/car/toyota/interface.py +++ b/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 diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py index 9107d41..d21aa57 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/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 diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index 43a8bcd..c3a4054 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/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) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 0eda445..caa9ef3 100644 --- a/selfdrive/controls/controlsd.py +++ b/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() diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index 90b6858..5bb537e 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/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 diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index eeeeda0..71dc5fe 100755 --- a/selfdrive/controls/plannerd.py +++ b/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: diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index c3b3918..fb1b8f1 100755 --- a/selfdrive/modeld/modeld.py +++ b/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 diff --git a/selfdrive/navd/navd.py b/selfdrive/navd/navd.py index 8cfc495..a1c770e 100755 --- a/selfdrive/navd/navd.py +++ b/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) diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py index bfc50c4..9a744f0 100755 --- a/selfdrive/thermald/thermald.py +++ b/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())) diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 01750ea..8db559f 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/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)); } diff --git a/selfdrive/ui/qt/sidebar.cc b/selfdrive/ui/qt/sidebar.cc index e952940..a63c518 100644 --- a/selfdrive/ui/qt/sidebar.cc +++ b/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) { diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 9afd22f..b0a4bbf 100644 --- a/selfdrive/ui/ui.cc +++ b/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>({ "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;