zhudongjer/carrot
1
1
Fork 0
Code Issues Pull Requests Actions 2 Packages Projects Releases Wiki Activity

FrogPilot setup - Configure cereal

Browse Source
This commit is contained in:
FrogAi 2024-05-29 03:31:09 -07:00
parent f1a2a6474c
commit 7965a1eaa7
39 changed files with 242 additions and 85 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
cereal/car.capnp
View File

@ -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;
}
}

54
cereal/custom.capnp
View File

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

19
cereal/log.capnp
View File

@ -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;

7
cereal/services.py
View File

@ -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.),

5
selfdrive/car/body/carstate.py
View File

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

4
selfdrive/car/body/interface.py
View File

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

11
selfdrive/car/card.py
View File

@ -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

5
selfdrive/car/chrysler/carstate.py
View File

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

6
selfdrive/car/chrysler/interface.py
View File

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

5
selfdrive/car/ford/carstate.py
View File

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

6
selfdrive/car/ford/interface.py
View File

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

5
selfdrive/car/gm/carstate.py
View File

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

6
selfdrive/car/gm/interface.py
View File

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

5
selfdrive/car/honda/carstate.py
View File

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

6
selfdrive/car/honda/interface.py
View File

@ -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

8
selfdrive/car/hyundai/carstate.py
View File

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

6
selfdrive/car/hyundai/interface.py
View File

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

5
selfdrive/car/interfaces.py
View File

@ -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]]:

5
selfdrive/car/mazda/carstate.py
View File

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

6
selfdrive/car/mazda/interface.py
View File

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

5
selfdrive/car/nissan/carstate.py
View File

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

6
selfdrive/car/nissan/interface.py
View File

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

6
selfdrive/car/subaru/carstate.py
View File

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

6
selfdrive/car/subaru/interface.py
View File

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

5
selfdrive/car/tesla/carstate.py
View File

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

4
selfdrive/car/tesla/interface.py
View File

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

5
selfdrive/car/toyota/carstate.py
View File

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

6
selfdrive/car/toyota/interface.py
View File

@ -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

8
selfdrive/car/volkswagen/carstate.py
View File

@ -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

4
selfdrive/car/volkswagen/interface.py
View File

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

13
selfdrive/controls/controlsd.py
View File

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

2
selfdrive/controls/lib/desire_helper.py
View File

@ -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

2
selfdrive/controls/plannerd.py
View File

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

4
selfdrive/modeld/modeld.py
View File

@ -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

7
selfdrive/navd/navd.py
View File

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

6
selfdrive/thermald/thermald.py
View File

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

3
selfdrive/ui/qt/onroad.cc
View File

@ -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));
}

3
selfdrive/ui/qt/sidebar.cc
View File

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

27
selfdrive/ui/ui.cc
View File

@ -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;