From d65b84afe114d6151dc50ab9536dd12f8518f961 Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Fri, 7 Jun 2024 02:58:29 -0700 Subject: [PATCH] FrogPilot setup - Configure cereal --- cereal/car.capnp | 37 +++++++++++++++- cereal/custom.capnp | 58 ++++++++++++++++++++++--- cereal/log.capnp | 19 +++++--- cereal/services.py | 7 +++ selfdrive/car/body/carstate.py | 5 ++- selfdrive/car/body/interface.py | 4 +- selfdrive/car/card.py | 20 ++++++--- selfdrive/car/chrysler/carstate.py | 5 ++- selfdrive/car/chrysler/interface.py | 6 +-- selfdrive/car/ford/carstate.py | 5 ++- selfdrive/car/ford/interface.py | 6 +-- selfdrive/car/gm/carstate.py | 5 ++- selfdrive/car/gm/interface.py | 6 +-- selfdrive/car/honda/carstate.py | 5 ++- selfdrive/car/honda/interface.py | 6 +-- selfdrive/car/hyundai/carstate.py | 8 ++-- selfdrive/car/hyundai/interface.py | 6 +-- selfdrive/car/interfaces.py | 4 +- selfdrive/car/mazda/carstate.py | 5 ++- selfdrive/car/mazda/interface.py | 6 +-- selfdrive/car/mock/interface.py | 5 ++- selfdrive/car/nissan/carstate.py | 5 ++- selfdrive/car/nissan/interface.py | 6 +-- selfdrive/car/subaru/carstate.py | 6 +-- selfdrive/car/subaru/interface.py | 6 +-- selfdrive/car/tesla/carstate.py | 5 ++- selfdrive/car/tesla/interface.py | 4 +- selfdrive/car/toyota/carstate.py | 5 ++- selfdrive/car/toyota/interface.py | 6 +-- selfdrive/car/volkswagen/carstate.py | 8 ++-- selfdrive/car/volkswagen/interface.py | 4 +- selfdrive/controls/controlsd.py | 26 ++++++++--- selfdrive/controls/lib/desire_helper.py | 2 +- selfdrive/controls/plannerd.py | 2 +- selfdrive/modeld/modeld.py | 4 +- selfdrive/navd/navd.py | 7 ++- selfdrive/ui/qt/onroad/onroad_home.cc | 4 ++ selfdrive/ui/qt/sidebar.cc | 3 ++ selfdrive/ui/ui.cc | 29 +++++++++++++ system/hardware/hardwared.py | 6 ++- 40 files changed, 271 insertions(+), 95 deletions(-) diff --git a/cereal/car.capnp b/cereal/car.capnp index a7d299a8e..34c021d18 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -118,7 +118,29 @@ struct CarEvent @0x9b1657f34caf3ad3 { actuatorsApiUnavailable @120; # FrogPilot Events - pedalInterceptorNoBrake @134; + accel30 @121; + accel35 @122; + accel40 @123; + blockUser @124; + dejaVuCurve @125; + firefoxSteerSaturated @126; + goatSteerSaturated @127; + greenLight @128; + holidayActive @129; + laneChangeBlockedLoud @130; + leadDeparting @131; + noLaneAvailable @132; + openpilotCrashed @133; + openpilotCrashedRandomEvents @134; + pedalInterceptorNoBrake @135; + speedLimitChanged @136; + torqueNNLoad @137; + trafficModeActive @138; + trafficModeInactive @139; + turningLeft @140; + turningRight @141; + vCruise69 @142; + yourFrogTriedToKillMe @143; radarCanErrorDEPRECATED @15; communityFeatureDisallowedDEPRECATED @62; @@ -415,6 +437,19 @@ struct CarControl { prompt @6; promptRepeat @7; promptDistracted @8; + + # Random Events + angry @9; + dejaVu @10; + doc @11; + fart @12; + firefox @13; + nessie @14; + noice @15; + uwu @16; + + # Other + goat @17; } } diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 369222add..be77be569 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -8,19 +8,67 @@ $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; } -struct CustomReserved1 @0xaedffd8f31e7b55d { +struct FrogPilotCarState @0xaedffd8f31e7b55d { + struct ButtonEvent { + enum Type { + lkas @0; + } + } + + alwaysOnLateralDisabled @0 :Bool; + brakeLights @1 :Bool; + dashboardSpeedLimit @2 :Float32; + distanceLongPressed @3 :Bool; + ecoGear @4 :Bool; + hasMenu @5 :Bool; + sportGear @6 :Bool; + trafficModeActive @7 :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 :Float32; + conditionalExperimentalActive @3 :Bool; + dangerJerk @4 :Float32; + desiredFollowDistance @5 :Int16; + greenLight @6 :Bool; + laneWidthLeft @7 :Float32; + laneWidthRight @8 :Float32; + leadDeparting @9 :Bool; + maxAcceleration @10 :Float32; + minAcceleration @11 :Float32; + roadCurvature @12 :Float32; + safeObstacleDistance @13 :Int16; + safeObstacleDistanceStock @14 :Int16; + slcOverridden @15 :Bool; + slcOverriddenSpeed @16 :Float32; + slcSpeedLimit @17 :Float32; + slcSpeedLimitOffset @18 :Float32; + speedJerk @19 :Float32; + speedJerkStock @20 :Float32; + stoppedEquivalenceFactor @21 :Int16; + takingCurveQuickly @22 :Bool; + tFollow @23 :Float32; + unconfirmedSlcSpeedLimit @24 :Float32; + vCruise @25 :Float32; + vtscControllingCurve @26 :Bool; } struct CustomReserved5 @0xa5cd762cd951a455 { diff --git a/cereal/log.capnp b/cereal/log.capnp index 99b5dddf1..ed7d3f814 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -335,6 +335,12 @@ enum LaneChangeDirection { right @2; } +enum TurnDirection { + none @0; + turnLeft @1; + turnRight @2; +} + struct CanData { address @0 :UInt32; busTime @1 :UInt16; @@ -744,6 +750,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 { @@ -794,6 +801,7 @@ struct ControlsState @0x97ff69c53601abf1 { saturated @7 :Bool; actualLateralAccel @9 :Float32; desiredLateralAccel @10 :Float32; + nnLog @11 :List(Float32); } struct LateralLQRState { @@ -958,6 +966,7 @@ struct ModelDataV2 { hardBrakePredicted @7 :Bool; laneChangeState @8 :LaneChangeState; laneChangeDirection @9 :LaneChangeDirection; + turnDirection @10 :TurnDirection; # deprecated @@ -2318,11 +2327,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 2ab28f6d5..ec4ab196c 100755 --- a/cereal/services.py +++ b/cereal/services.py @@ -71,6 +71,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 fca9bcc62..e184276a2 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 f797a7ecf..87da16a26 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,4 +36,4 @@ class CarInterface(CarInterfaceBase): ret.events[0].enable = True self.frame += 1 - return ret + return ret, fp_ret diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index d9ee020ba..9f89f39e4 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -4,7 +4,7 @@ import time import cereal.messaging as messaging -from cereal import car +from cereal import car, custom from panda import ALTERNATIVE_EXPERIENCE @@ -27,7 +27,7 @@ class Car: def __init__(self, CI=None): self.can_sock = messaging.sub_sock('can', timeout=20) self.sm = messaging.SubMaster(['pandaStates', 'carControl', 'onroadEvents']) - self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput']) + self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput', 'frogpilotCarState']) self.can_rcv_cum_timeout_counter = 0 @@ -87,7 +87,7 @@ class Car: # Update carState from CAN can_strs = messaging.drain_sock_raw(self.can_sock, wait_for_one=True) - CS = self.CI.update(self.CC_prev, can_strs) + CS, FPCS = self.CI.update(self.CC_prev, can_strs) self.sm.update(0) @@ -100,7 +100,7 @@ class Car: if can_rcv_valid and REPLAY: self.can_log_mono_time = messaging.log_from_bytes(can_strs[0]).logMonoTime - return CS + return CS, FPCS def update_events(self, CS: car.CarState) -> car.CarState: self.events.clear() @@ -115,7 +115,7 @@ class Car: CS.events = self.events.to_msg() - def state_publish(self, CS: car.CarState): + def state_publish(self, CS: car.CarState, FPCS: custom.FrogPilotCarState): """carState and carParams publish loop""" # carParams - logged every 50 seconds (> 1 per segment) @@ -139,6 +139,12 @@ class Car: cs_send.carState.cumLagMs = -self.rk.remaining * 1000. self.pm.send('carState', cs_send) + # frogpilotCarState + 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""" @@ -158,11 +164,11 @@ class Car: self.CC_prev = CC def step(self): - CS = self.state_update() + CS, FPCS = self.state_update() self.update_events(CS) - self.state_publish(CS) + self.state_publish(CS, FPCS) initialized = (not any(e.name == EventName.controlsInitializing for e in self.sm['onroadEvents']) and self.sm.seen['onroadEvents']) diff --git a/selfdrive/car/chrysler/carstate.py b/selfdrive/car/chrysler/carstate.py index 91b922c59..7a94b0e6e 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 217a1a756..1ffaa026f 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 @@ -77,7 +77,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}) @@ -94,4 +94,4 @@ class CarInterface(CarInterfaceBase): ret.events = events.to_msg() - return ret + return ret, fp_ret diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py index 78f48ec5c..bf22533b3 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 @@ -107,7 +108,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 54cf86510..8c22a6d80 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 @@ -68,7 +68,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}) @@ -78,4 +78,4 @@ class CarInterface(CarInterfaceBase): ret.events = events.to_msg() - return ret + return ret, fp_ret diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index 0ce6e5a6b..481615a40 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 e4f34cf70..2230d21ae 100644 --- 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 @@ -306,7 +306,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: @@ -347,4 +347,4 @@ class CarInterface(CarInterfaceBase): ret.events = events.to_msg() - return ret + return ret, fp_ret diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index c98d1a72d..563b94268 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 @@ -106,6 +106,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 @@ -258,7 +259,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 fdcba8bb8..96374b2c6 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 @@ -223,7 +223,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), @@ -252,4 +252,4 @@ class CarInterface(CarInterfaceBase): ret.events = events.to_msg() - return ret + return ret, fp_ret diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 92c489cf3..33a7e267b 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 @@ -165,10 +166,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 @@ -247,7 +249,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 ecedf3fd7..cb513cd75 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, \ @@ -151,7 +151,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) @@ -172,4 +172,4 @@ class CarInterface(CarInterfaceBase): ret.events = events.to_msg() - return ret + return ret, fp_ret diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index e5762ed52..9aab3d0b5 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -237,7 +237,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) @@ -260,7 +260,7 @@ class CarInterfaceBase(ABC): if self.CS is not None: self.CS.out = ret.as_reader() - return ret + return ret, fp_ret def create_common_events(self, cs_out, extra_gears=None, pcm_enable=True, allow_enable=True, diff --git a/selfdrive/car/mazda/carstate.py b/selfdrive/car/mazda/carstate.py index 83b238fb6..c055152c8 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 6992d49ff..cde0d7968 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,4 +47,4 @@ class CarInterface(CarInterfaceBase): ret.events = events.to_msg() - return ret + return ret, fp_ret diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py index 7506bab05..77f77f2be 100755 --- a/selfdrive/car/mock/interface.py +++ b/selfdrive/car/mock/interface.py @@ -1,5 +1,5 @@ #!/usr/bin/env python3 -from cereal import car +from cereal import car, custom import cereal.messaging as messaging from openpilot.selfdrive.car.interfaces import CarInterfaceBase @@ -26,7 +26,8 @@ class CarInterface(CarInterfaceBase): gps_sock = 'gpsLocationExternal' if self.sm.recv_frame['gpsLocationExternal'] > 1 else 'gpsLocation' ret = car.CarState.new_message() + fp_ret = custom.FrogPilotCarState.new_message() ret.vEgo = self.sm[gps_sock].speed ret.vEgoRaw = self.sm[gps_sock].speed - return ret + return ret, fp_ret diff --git a/selfdrive/car/nissan/carstate.py b/selfdrive/car/nissan/carstate.py index 57146b49c..67d0e664a 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 2e9a99061..86100eb3c 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,4 +41,4 @@ class CarInterface(CarInterfaceBase): ret.events = events.to_msg() - return ret + return ret, fp_ret diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py index 821ff2c15..d969616cb 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 cb0093437..f9bbb6c4c 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 @@ -99,11 +99,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 645ea4601..26e36e5e8 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 deb0e0023..192a70b01 100755 --- a/selfdrive/car/tesla/interface.py +++ b/selfdrive/car/tesla/interface.py @@ -40,8 +40,8 @@ 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 diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 8315f24ae..d474a67e8 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"]]) @@ -175,7 +176,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 98f63597e..4d98d0e49 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, \ @@ -163,7 +163,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}) @@ -192,4 +192,4 @@ class CarInterface(CarInterfaceBase): ret.events = events.to_msg() - return ret + return ret, fp_ret diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py index ec6403496..4a8603fc9 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"], @@ -153,10 +154,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"], @@ -252,7 +254,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 77e56875b..11dd1364a 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -102,7 +102,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, @@ -127,5 +127,5 @@ class CarInterface(CarInterfaceBase): ret.events = events.to_msg() - return ret + return ret, fp_ret diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 678889abb..952f05985 100755 --- 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 msgq.visionipc import VisionIpcClient, VisionStreamType @@ -78,7 +78,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"] @@ -97,7 +97,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)) @@ -648,9 +648,11 @@ class Controls: self.personality = (self.personality - 1) % 3 self.params.put_nonblocking('LongitudinalPersonality', str(self.personality)) - return CC, lac_log + FPCC = self.update_frogpilot_variables(CS) - def publish_logs(self, CS, start_time, CC, lac_log): + return CC, lac_log, FPCC + + def publish_logs(self, CS, start_time, CC, lac_log, FPCC): """Send actuators and hud commands to the car, send controlsstate and MPC logging""" # Orientation and angle rates can be useful for carcontroller @@ -791,6 +793,12 @@ class Controls: cc_send.carControl = CC self.pm.send('carControl', cc_send) + # frogpilotCarControl + fpcc_send = messaging.new_message('frogpilotCarControl') + fpcc_send.valid = CS.canValid + fpcc_send.frogpilotCarControl = FPCC + self.pm.send('frogpilotCarControl', fpcc_send) + def step(self): start_time = time.monotonic() @@ -806,10 +814,10 @@ class Controls: self.state_transition(CS) # Compute actuators (runs PID loops and lateral MPC) - CC, lac_log = self.state_control(CS) + CC, lac_log, FPCC = self.state_control(CS) # Publish data - self.publish_logs(CS, start_time, CC, lac_log) + self.publish_logs(CS, start_time, CC, lac_log, FPCC) self.CS_prev = CS @@ -840,6 +848,10 @@ class Controls: e.set() t.join() + def update_frogpilot_variables(self, CS): + FPCC = custom.FrogPilotCarControl.new_message() + + return FPCC def main(): config_realtime_process(4, Priority.CTRL_HIGH) diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index 90b685864..5bb537ea1 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 eeeeda050..b14410abd 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', 'frogpilotCarState', 'frogpilotPlan'], poll='modelV2', ignore_avg_freq=['radarState']) while True: diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 3ac80aad9..3f94ceb9e 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -150,7 +150,7 @@ def main(demo=False): # messaging pm = PubMaster(["modelV2", "cameraOdometry"]) - sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl"]) + sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl", "frogpilotPlan"]) publish_state = PublishState() params = Params() @@ -267,7 +267,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 8cfc495f2..a1c770e61 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/ui/qt/onroad/onroad_home.cc b/selfdrive/ui/qt/onroad/onroad_home.cc index 66eb1812e..0152b9083 100644 --- a/selfdrive/ui/qt/onroad/onroad_home.cc +++ b/selfdrive/ui/qt/onroad/onroad_home.cc @@ -126,4 +126,8 @@ void OnroadWindow::primeChanged(bool prime) { void OnroadWindow::paintEvent(QPaintEvent *event) { QPainter p(this); p.fillRect(rect(), QColor(bg.red(), bg.green(), bg.blue(), 255)); + + // FrogPilot variables + UIState *s = uiState(); + SubMaster &sm = *(s->sm); } diff --git a/selfdrive/ui/qt/sidebar.cc b/selfdrive/ui/qt/sidebar.cc index 75b966c9a..5ff3a8327 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 c70b7594c..4c17a7a8d 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -197,9 +197,36 @@ 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("frogpilotCarState")) { + auto frogpilotCarState = sm["frogpilotCarState"].getFrogpilotCarState(); + } + if (sm.updated("frogpilotPlan")) { + auto frogpilotPlan = sm["frogpilotPlan"].getFrogpilotPlan(); + } + if (sm.updated("liveLocationKalman")) { + auto liveLocationKalman = sm["liveLocationKalman"].getLiveLocationKalman(); + } + if (sm.updated("liveTorqueParameters")) { + auto liveTorqueParameters = 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; @@ -250,6 +277,8 @@ UIState::UIState(QObject *parent) : QObject(parent) { "modelV2", "controlsState", "liveCalibration", "radarState", "deviceState", "pandaStates", "carParams", "driverMonitoringState", "carState", "liveLocationKalman", "driverStateV2", "wideRoadCameraState", "managerState", "navInstruction", "navRoute", "uiPlan", "clocks", + "carControl", "liveTorqueParameters", + "frogpilotCarControl", "frogpilotCarState", "frogpilotDeviceState", "frogpilotPlan", }); Params params; diff --git a/system/hardware/hardwared.py b/system/hardware/hardwared.py index e3a4c8171..83885f290 100755 --- a/system/hardware/hardwared.py +++ b/system/hardware/hardwared.py @@ -163,7 +163,7 @@ def hw_state_thread(end_event, hw_queue): def hardware_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 @@ -401,6 +401,10 @@ def hardware_thread(end_event, hw_queue) -> None: msg.deviceState.thermalStatus = thermal_status pm.send("deviceState", msg) + fpmsg = messaging.new_message('frogpilotDeviceState') + + pm.send("frogpilotDeviceState", fpmsg) + # Log to statsd statlog.gauge("free_space_percent", msg.deviceState.freeSpacePercent) statlog.gauge("gpu_usage_percent", msg.deviceState.gpuUsagePercent)