mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-09-08 17:13:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

FrogPilot setup - Configure cereal

Browse Source
This commit is contained in:
FrogAi
2024-06-07 02:58:29 -07:00
parent 7bcab97eee
commit d65b84afe1
40 changed files with 271 additions and 95 deletions
Download Patch File Download Diff File Expand all files Collapse all files
cereal/car.capnp
+36 -1
View File
@@ -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;
}
}
cereal/custom.capnp
+53 -5
View File
@@ -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 {
cereal/log.capnp
+14 -5
View File
@@ -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;
cereal/services.py
+7
View File
@@ -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.),
selfdrive/car/body/carstate.py
+3 -2
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):
selfdrive/car/body/interface.py
+2 -2
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,4 +36,4 @@ class CarInterface(CarInterfaceBase):
ret.events[0].enable = True
self.frame += 1
return ret
return ret, fp_ret
selfdrive/car/card.py
+13 -7
View File
@@ -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'])
selfdrive/car/chrysler/carstate.py
+3 -2
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():
selfdrive/car/chrysler/interface.py
+3 -3
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
@@ -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
selfdrive/car/ford/carstate.py
+3 -2
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
@@ -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):
selfdrive/car/ford/interface.py
+3 -3
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
@@ -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
selfdrive/car/gm/carstate.py
+3 -2
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):
selfdrive/car/gm/interface.py
+3 -3
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
@@ -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
selfdrive/car/honda/carstate.py
+3 -2
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
@@ -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)
selfdrive/car/honda/interface.py
+3 -3
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
@@ -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
selfdrive/car/hyundai/carstate.py
+5 -3
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
@@ -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:
selfdrive/car/hyundai/interface.py
+3 -3
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, \
@@ -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
selfdrive/car/interfaces.py
+2 -2
View File
@@ -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,
selfdrive/car/mazda/carstate.py
+3 -2
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):
selfdrive/car/mazda/interface.py
+3 -3
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,4 +47,4 @@ class CarInterface(CarInterfaceBase):
ret.events = events.to_msg()
return ret
return ret, fp_ret
selfdrive/car/mock/interface.py
+3 -2
View File
@@ -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
selfdrive/car/nissan/carstate.py
+3 -2
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):
selfdrive/car/nissan/interface.py
+3 -3
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,4 +41,4 @@ class CarInterface(CarInterfaceBase):
ret.events = events.to_msg()
return ret
return ret, fp_ret
selfdrive/car/subaru/carstate.py
+3 -3
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)
selfdrive/car/subaru/interface.py
+3 -3
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
@@ -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):
selfdrive/car/tesla/carstate.py
+3 -2
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):
selfdrive/car/tesla/interface.py
+2 -2
View File
@@ -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
selfdrive/car/toyota/carstate.py
+3 -2
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"]])
@@ -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):
selfdrive/car/toyota/interface.py
+3 -3
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, \
@@ -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
selfdrive/car/volkswagen/carstate.py
+5 -3
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"],
@@ -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
selfdrive/car/volkswagen/interface.py
+2 -2
View File
@@ -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
selfdrive/controls/controlsd.py
+19 -7
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 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)
selfdrive/controls/lib/desire_helper.py
+1 -1
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
selfdrive/controls/plannerd.py
+1 -1
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', 'frogpilotCarState', 'frogpilotPlan'],
poll='modelV2', ignore_avg_freq=['radarState'])
while True:
selfdrive/modeld/modeld.py
+2 -2
View File
@@ -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
selfdrive/navd/navd.py
+6 -1
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)
selfdrive/ui/qt/onroad/onroad_home.cc
+4
View File
@@ -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);
}
selfdrive/ui/qt/sidebar.cc
+3
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) {
selfdrive/ui/ui.cc
+29
View File
@@ -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;
system/hardware/hardwared.py
+5 -1
View File
@@ -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)