Shane Smiskol
7 months ago
commit
bcf8494694
210 changed files with 6023 additions and 5670 deletions
@ -1,715 +0,0 @@ |
||||
using Cxx = import "./include/c++.capnp"; |
||||
$Cxx.namespace("cereal"); |
||||
|
||||
@0x8e2af1e708af8b8d; |
||||
|
||||
# ******* events causing controls state machine transition ******* |
||||
|
||||
# FIXME: OnroadEvent shouldn't be in car.capnp, but can't immediately |
||||
# move due to being referenced by structs in this file |
||||
struct OnroadEvent @0x9b1657f34caf3ad3 { |
||||
name @0 :EventName; |
||||
|
||||
# event types |
||||
enable @1 :Bool; |
||||
noEntry @2 :Bool; |
||||
warning @3 :Bool; # alerts presented only when enabled or soft disabling |
||||
userDisable @4 :Bool; |
||||
softDisable @5 :Bool; |
||||
immediateDisable @6 :Bool; |
||||
preEnable @7 :Bool; |
||||
permanent @8 :Bool; # alerts presented regardless of openpilot state |
||||
overrideLateral @10 :Bool; |
||||
overrideLongitudinal @9 :Bool; |
||||
|
||||
enum EventName @0xbaa8c5d505f727de { |
||||
canError @0; |
||||
steerUnavailable @1; |
||||
wrongGear @4; |
||||
doorOpen @5; |
||||
seatbeltNotLatched @6; |
||||
espDisabled @7; |
||||
wrongCarMode @8; |
||||
steerTempUnavailable @9; |
||||
reverseGear @10; |
||||
buttonCancel @11; |
||||
buttonEnable @12; |
||||
pedalPressed @13; # exits active state |
||||
preEnableStandstill @73; # added during pre-enable state with brake |
||||
gasPressedOverride @108; # added when user is pressing gas with no disengage on gas |
||||
steerOverride @114; |
||||
cruiseDisabled @14; |
||||
speedTooLow @17; |
||||
outOfSpace @18; |
||||
overheat @19; |
||||
calibrationIncomplete @20; |
||||
calibrationInvalid @21; |
||||
calibrationRecalibrating @117; |
||||
controlsMismatch @22; |
||||
pcmEnable @23; |
||||
pcmDisable @24; |
||||
radarFault @26; |
||||
brakeHold @28; |
||||
parkBrake @29; |
||||
manualRestart @30; |
||||
lowSpeedLockout @31; |
||||
joystickDebug @34; |
||||
steerTempUnavailableSilent @35; |
||||
resumeRequired @36; |
||||
preDriverDistracted @37; |
||||
promptDriverDistracted @38; |
||||
driverDistracted @39; |
||||
preDriverUnresponsive @43; |
||||
promptDriverUnresponsive @44; |
||||
driverUnresponsive @45; |
||||
belowSteerSpeed @46; |
||||
lowBattery @48; |
||||
accFaulted @51; |
||||
sensorDataInvalid @52; |
||||
commIssue @53; |
||||
commIssueAvgFreq @109; |
||||
tooDistracted @54; |
||||
posenetInvalid @55; |
||||
soundsUnavailable @56; |
||||
preLaneChangeLeft @57; |
||||
preLaneChangeRight @58; |
||||
laneChange @59; |
||||
lowMemory @63; |
||||
stockAeb @64; |
||||
ldw @65; |
||||
carUnrecognized @66; |
||||
invalidLkasSetting @69; |
||||
speedTooHigh @70; |
||||
laneChangeBlocked @71; |
||||
relayMalfunction @72; |
||||
stockFcw @74; |
||||
startup @75; |
||||
startupNoCar @76; |
||||
startupNoControl @77; |
||||
startupMaster @78; |
||||
startupNoFw @104; |
||||
fcw @79; |
||||
steerSaturated @80; |
||||
belowEngageSpeed @84; |
||||
noGps @85; |
||||
wrongCruiseMode @87; |
||||
modeldLagging @89; |
||||
deviceFalling @90; |
||||
fanMalfunction @91; |
||||
cameraMalfunction @92; |
||||
cameraFrameRate @110; |
||||
processNotRunning @95; |
||||
dashcamMode @96; |
||||
controlsInitializing @98; |
||||
usbError @99; |
||||
roadCameraError @100; |
||||
driverCameraError @101; |
||||
wideRoadCameraError @102; |
||||
highCpuUsage @105; |
||||
cruiseMismatch @106; |
||||
lkasDisabled @107; |
||||
canBusMissing @111; |
||||
controlsdLagging @112; |
||||
resumeBlocked @113; |
||||
steerTimeLimit @115; |
||||
vehicleSensorsInvalid @116; |
||||
locationdTemporaryError @103; |
||||
locationdPermanentError @118; |
||||
paramsdTemporaryError @50; |
||||
paramsdPermanentError @119; |
||||
actuatorsApiUnavailable @120; |
||||
espActive @121; |
||||
personalityChanged @122; |
||||
|
||||
radarCanErrorDEPRECATED @15; |
||||
communityFeatureDisallowedDEPRECATED @62; |
||||
radarCommIssueDEPRECATED @67; |
||||
driverMonitorLowAccDEPRECATED @68; |
||||
gasUnavailableDEPRECATED @3; |
||||
dataNeededDEPRECATED @16; |
||||
modelCommIssueDEPRECATED @27; |
||||
ipasOverrideDEPRECATED @33; |
||||
geofenceDEPRECATED @40; |
||||
driverMonitorOnDEPRECATED @41; |
||||
driverMonitorOffDEPRECATED @42; |
||||
calibrationProgressDEPRECATED @47; |
||||
invalidGiraffeHondaDEPRECATED @49; |
||||
invalidGiraffeToyotaDEPRECATED @60; |
||||
internetConnectivityNeededDEPRECATED @61; |
||||
whitePandaUnsupportedDEPRECATED @81; |
||||
commIssueWarningDEPRECATED @83; |
||||
focusRecoverActiveDEPRECATED @86; |
||||
neosUpdateRequiredDEPRECATED @88; |
||||
modelLagWarningDEPRECATED @93; |
||||
startupOneplusDEPRECATED @82; |
||||
startupFuzzyFingerprintDEPRECATED @97; |
||||
noTargetDEPRECATED @25; |
||||
brakeUnavailableDEPRECATED @2; |
||||
plannerErrorDEPRECATED @32; |
||||
gpsMalfunctionDEPRECATED @94; |
||||
} |
||||
} |
||||
|
||||
# ******* main car state @ 100hz ******* |
||||
# all speeds in m/s |
||||
|
||||
struct CarState { |
||||
events @13 :List(OnroadEvent); |
||||
|
||||
# CAN health |
||||
canValid @26 :Bool; # invalid counter/checksums |
||||
canTimeout @40 :Bool; # CAN bus dropped out |
||||
canErrorCounter @48 :UInt32; |
||||
|
||||
# car speed |
||||
vEgo @1 :Float32; # best estimate of speed |
||||
aEgo @16 :Float32; # best estimate of aCAN cceleration |
||||
vEgoRaw @17 :Float32; # unfiltered speed from wheel speed sensors |
||||
vEgoCluster @44 :Float32; # best estimate of speed shown on car's instrument cluster, used for UI |
||||
|
||||
vCruise @53 :Float32; # actual set speed |
||||
vCruiseCluster @54 :Float32; # set speed to display in the UI |
||||
|
||||
yawRate @22 :Float32; # best estimate of yaw rate |
||||
standstill @18 :Bool; |
||||
wheelSpeeds @2 :WheelSpeeds; |
||||
|
||||
# gas pedal, 0.0-1.0 |
||||
gas @3 :Float32; # this is user pedal only |
||||
gasPressed @4 :Bool; # this is user pedal only |
||||
|
||||
engineRpm @46 :Float32; |
||||
|
||||
# brake pedal, 0.0-1.0 |
||||
brake @5 :Float32; # this is user pedal only |
||||
brakePressed @6 :Bool; # this is user pedal only |
||||
regenBraking @45 :Bool; # this is user pedal only |
||||
parkingBrake @39 :Bool; |
||||
brakeHoldActive @38 :Bool; |
||||
|
||||
# steering wheel |
||||
steeringAngleDeg @7 :Float32; |
||||
steeringAngleOffsetDeg @37 :Float32; # Offset betweens sensors in case there multiple |
||||
steeringRateDeg @15 :Float32; |
||||
steeringTorque @8 :Float32; # TODO: standardize units |
||||
steeringTorqueEps @27 :Float32; # TODO: standardize units |
||||
steeringPressed @9 :Bool; # if the user is using the steering wheel |
||||
steerFaultTemporary @35 :Bool; # temporary EPS fault |
||||
steerFaultPermanent @36 :Bool; # permanent EPS fault |
||||
stockAeb @30 :Bool; |
||||
stockFcw @31 :Bool; |
||||
espDisabled @32 :Bool; |
||||
accFaulted @42 :Bool; |
||||
carFaultedNonCritical @47 :Bool; # some ECU is faulted, but car remains controllable |
||||
espActive @51 :Bool; |
||||
vehicleSensorsInvalid @52 :Bool; # invalid steering angle readings, etc. |
||||
|
||||
# cruise state |
||||
cruiseState @10 :CruiseState; |
||||
|
||||
# gear |
||||
gearShifter @14 :GearShifter; |
||||
|
||||
# button presses |
||||
buttonEvents @11 :List(ButtonEvent); |
||||
leftBlinker @20 :Bool; |
||||
rightBlinker @21 :Bool; |
||||
genericToggle @23 :Bool; |
||||
|
||||
# lock info |
||||
doorOpen @24 :Bool; |
||||
seatbeltUnlatched @25 :Bool; |
||||
|
||||
# clutch (manual transmission only) |
||||
clutchPressed @28 :Bool; |
||||
|
||||
# blindspot sensors |
||||
leftBlindspot @33 :Bool; # Is there something blocking the left lane change |
||||
rightBlindspot @34 :Bool; # Is there something blocking the right lane change |
||||
|
||||
fuelGauge @41 :Float32; # battery or fuel tank level from 0.0 to 1.0 |
||||
charging @43 :Bool; |
||||
|
||||
# process meta |
||||
cumLagMs @50 :Float32; |
||||
|
||||
struct WheelSpeeds { |
||||
# optional wheel speeds |
||||
fl @0 :Float32; |
||||
fr @1 :Float32; |
||||
rl @2 :Float32; |
||||
rr @3 :Float32; |
||||
} |
||||
|
||||
struct CruiseState { |
||||
enabled @0 :Bool; |
||||
speed @1 :Float32; |
||||
speedCluster @6 :Float32; # Set speed as shown on instrument cluster |
||||
available @2 :Bool; |
||||
speedOffset @3 :Float32; |
||||
standstill @4 :Bool; |
||||
nonAdaptive @5 :Bool; |
||||
} |
||||
|
||||
enum GearShifter { |
||||
unknown @0; |
||||
park @1; |
||||
drive @2; |
||||
neutral @3; |
||||
reverse @4; |
||||
sport @5; |
||||
low @6; |
||||
brake @7; |
||||
eco @8; |
||||
manumatic @9; |
||||
} |
||||
|
||||
# send on change |
||||
struct ButtonEvent { |
||||
pressed @0 :Bool; |
||||
type @1 :Type; |
||||
|
||||
enum Type { |
||||
unknown @0; |
||||
leftBlinker @1; |
||||
rightBlinker @2; |
||||
accelCruise @3; |
||||
decelCruise @4; |
||||
cancel @5; |
||||
altButton1 @6; |
||||
altButton2 @7; |
||||
altButton3 @8; |
||||
setCruise @9; |
||||
resumeCruise @10; |
||||
gapAdjustCruise @11; |
||||
} |
||||
} |
||||
|
||||
# deprecated |
||||
errorsDEPRECATED @0 :List(OnroadEvent.EventName); |
||||
brakeLightsDEPRECATED @19 :Bool; |
||||
steeringRateLimitedDEPRECATED @29 :Bool; |
||||
canMonoTimesDEPRECATED @12: List(UInt64); |
||||
canRcvTimeoutDEPRECATED @49 :Bool; |
||||
} |
||||
|
||||
# ******* radar state @ 20hz ******* |
||||
|
||||
struct RadarData @0x888ad6581cf0aacb { |
||||
errors @0 :List(Error); |
||||
points @1 :List(RadarPoint); |
||||
|
||||
enum Error { |
||||
canError @0; |
||||
fault @1; |
||||
wrongConfig @2; |
||||
} |
||||
|
||||
# similar to LiveTracks |
||||
# is one timestamp valid for all? I think so |
||||
struct RadarPoint { |
||||
trackId @0 :UInt64; # no trackId reuse |
||||
|
||||
# these 3 are the minimum required |
||||
dRel @1 :Float32; # m from the front bumper of the car |
||||
yRel @2 :Float32; # m |
||||
vRel @3 :Float32; # m/s |
||||
|
||||
# these are optional and valid if they are not NaN |
||||
aRel @4 :Float32; # m/s^2 |
||||
yvRel @5 :Float32; # m/s |
||||
|
||||
# some radars flag measurements VS estimates |
||||
measured @6 :Bool; |
||||
} |
||||
|
||||
# deprecated |
||||
canMonoTimesDEPRECATED @2 :List(UInt64); |
||||
} |
||||
|
||||
# ******* car controls @ 100hz ******* |
||||
|
||||
struct CarControl { |
||||
# must be true for any actuator commands to work |
||||
enabled @0 :Bool; |
||||
latActive @11: Bool; |
||||
longActive @12: Bool; |
||||
|
||||
# Actuator commands as computed by controlsd |
||||
actuators @6 :Actuators; |
||||
|
||||
# moved to CarOutput |
||||
actuatorsOutputDEPRECATED @10 :Actuators; |
||||
|
||||
leftBlinker @15: Bool; |
||||
rightBlinker @16: Bool; |
||||
|
||||
orientationNED @13 :List(Float32); |
||||
angularVelocity @14 :List(Float32); |
||||
|
||||
cruiseControl @4 :CruiseControl; |
||||
hudControl @5 :HUDControl; |
||||
|
||||
struct Actuators { |
||||
# range from 0.0 - 1.0 |
||||
gas @0: Float32; |
||||
brake @1: Float32; |
||||
# range from -1.0 - 1.0 |
||||
steer @2: Float32; |
||||
# value sent over can to the car |
||||
steerOutputCan @8: Float32; |
||||
steeringAngleDeg @3: Float32; |
||||
|
||||
curvature @7: Float32; |
||||
|
||||
speed @6: Float32; # m/s |
||||
accel @4: Float32; # m/s^2 |
||||
longControlState @5: LongControlState; |
||||
|
||||
enum LongControlState @0xe40f3a917d908282{ |
||||
off @0; |
||||
pid @1; |
||||
stopping @2; |
||||
starting @3; |
||||
} |
||||
} |
||||
|
||||
struct CruiseControl { |
||||
cancel @0: Bool; |
||||
resume @1: Bool; |
||||
override @4: Bool; |
||||
speedOverrideDEPRECATED @2: Float32; |
||||
accelOverrideDEPRECATED @3: Float32; |
||||
} |
||||
|
||||
struct HUDControl { |
||||
speedVisible @0: Bool; |
||||
setSpeed @1: Float32; |
||||
lanesVisible @2: Bool; |
||||
leadVisible @3: Bool; |
||||
visualAlert @4: VisualAlert; |
||||
audibleAlert @5: AudibleAlert; |
||||
rightLaneVisible @6: Bool; |
||||
leftLaneVisible @7: Bool; |
||||
rightLaneDepart @8: Bool; |
||||
leftLaneDepart @9: Bool; |
||||
leadDistanceBars @10: Int8; # 1-3: 1 is closest, 3 is farthest. some ports may utilize 2-4 bars instead |
||||
|
||||
enum VisualAlert { |
||||
# these are the choices from the Honda |
||||
# map as good as you can for your car |
||||
none @0; |
||||
fcw @1; |
||||
steerRequired @2; |
||||
brakePressed @3; |
||||
wrongGear @4; |
||||
seatbeltUnbuckled @5; |
||||
speedTooHigh @6; |
||||
ldw @7; |
||||
} |
||||
|
||||
enum AudibleAlert { |
||||
none @0; |
||||
|
||||
engage @1; |
||||
disengage @2; |
||||
refuse @3; |
||||
|
||||
warningSoft @4; |
||||
warningImmediate @5; |
||||
|
||||
prompt @6; |
||||
promptRepeat @7; |
||||
promptDistracted @8; |
||||
} |
||||
} |
||||
|
||||
gasDEPRECATED @1 :Float32; |
||||
brakeDEPRECATED @2 :Float32; |
||||
steeringTorqueDEPRECATED @3 :Float32; |
||||
activeDEPRECATED @7 :Bool; |
||||
rollDEPRECATED @8 :Float32; |
||||
pitchDEPRECATED @9 :Float32; |
||||
} |
||||
|
||||
struct CarOutput { |
||||
# Any car specific rate limits or quirks applied by |
||||
# the CarController are reflected in actuatorsOutput |
||||
# and matches what is sent to the car |
||||
actuatorsOutput @0 :CarControl.Actuators; |
||||
} |
||||
|
||||
# ****** car param ****** |
||||
|
||||
struct CarParams { |
||||
carName @0 :Text; |
||||
carFingerprint @1 :Text; |
||||
fuzzyFingerprint @55 :Bool; |
||||
|
||||
notCar @66 :Bool; # flag for non-car robotics platforms |
||||
|
||||
pcmCruise @3 :Bool; # is openpilot's state tied to the PCM's cruise state? |
||||
enableDsu @5 :Bool; # driving support unit |
||||
enableBsm @56 :Bool; # blind spot monitoring |
||||
flags @64 :UInt32; # flags for car specific quirks |
||||
experimentalLongitudinalAvailable @71 :Bool; |
||||
|
||||
minEnableSpeed @7 :Float32; |
||||
minSteerSpeed @8 :Float32; |
||||
safetyConfigs @62 :List(SafetyConfig); |
||||
alternativeExperience @65 :Int16; # panda flag for features like no disengage on gas |
||||
|
||||
# Car docs fields |
||||
maxLateralAccel @68 :Float32; |
||||
autoResumeSng @69 :Bool; # describes whether car can resume from a stop automatically |
||||
|
||||
# things about the car in the manual |
||||
mass @17 :Float32; # [kg] curb weight: all fluids no cargo |
||||
wheelbase @18 :Float32; # [m] distance from rear axle to front axle |
||||
centerToFront @19 :Float32; # [m] distance from center of mass to front axle |
||||
steerRatio @20 :Float32; # [] ratio of steering wheel angle to front wheel angle |
||||
steerRatioRear @21 :Float32; # [] ratio of steering wheel angle to rear wheel angle (usually 0) |
||||
|
||||
# things we can derive |
||||
rotationalInertia @22 :Float32; # [kg*m2] body rotational inertia |
||||
tireStiffnessFactor @72 :Float32; # scaling factor used in calculating tireStiffness[Front,Rear] |
||||
tireStiffnessFront @23 :Float32; # [N/rad] front tire coeff of stiff |
||||
tireStiffnessRear @24 :Float32; # [N/rad] rear tire coeff of stiff |
||||
|
||||
longitudinalTuning @25 :LongitudinalPIDTuning; |
||||
lateralParams @48 :LateralParams; |
||||
lateralTuning :union { |
||||
pid @26 :LateralPIDTuning; |
||||
indiDEPRECATED @27 :LateralINDITuning; |
||||
lqrDEPRECATED @40 :LateralLQRTuning; |
||||
torque @67 :LateralTorqueTuning; |
||||
} |
||||
|
||||
steerLimitAlert @28 :Bool; |
||||
steerLimitTimer @47 :Float32; # time before steerLimitAlert is issued |
||||
|
||||
vEgoStopping @29 :Float32; # Speed at which the car goes into stopping state |
||||
vEgoStarting @59 :Float32; # Speed at which the car goes into starting state |
||||
stoppingControl @31 :Bool; # Does the car allow full control even at lows speeds when stopping |
||||
steerControlType @34 :SteerControlType; |
||||
radarUnavailable @35 :Bool; # True when radar objects aren't visible on CAN or aren't parsed out |
||||
stopAccel @60 :Float32; # Required acceleration to keep vehicle stationary |
||||
stoppingDecelRate @52 :Float32; # m/s^2/s while trying to stop |
||||
startAccel @32 :Float32; # Required acceleration to get car moving |
||||
startingState @70 :Bool; # Does this car make use of special starting state |
||||
|
||||
steerActuatorDelay @36 :Float32; # Steering wheel actuator delay in seconds |
||||
longitudinalActuatorDelay @58 :Float32; # Gas/Brake actuator delay in seconds |
||||
openpilotLongitudinalControl @37 :Bool; # is openpilot doing the longitudinal control? |
||||
carVin @38 :Text; # VIN number queried during fingerprinting |
||||
dashcamOnly @41: Bool; |
||||
passive @73: Bool; # is openpilot in control? |
||||
transmissionType @43 :TransmissionType; |
||||
carFw @44 :List(CarFw); |
||||
|
||||
radarTimeStep @45: Float32 = 0.05; # time delta between radar updates, 20Hz is very standard |
||||
fingerprintSource @49: FingerprintSource; |
||||
networkLocation @50 :NetworkLocation; # Where Panda/C2 is integrated into the car's CAN network |
||||
|
||||
wheelSpeedFactor @63 :Float32; # Multiplier on wheels speeds to computer actual speeds |
||||
|
||||
struct SafetyConfig { |
||||
safetyModel @0 :SafetyModel; |
||||
safetyParam @3 :UInt16; |
||||
safetyParamDEPRECATED @1 :Int16; |
||||
safetyParam2DEPRECATED @2 :UInt32; |
||||
} |
||||
|
||||
struct LateralParams { |
||||
torqueBP @0 :List(Int32); |
||||
torqueV @1 :List(Int32); |
||||
} |
||||
|
||||
struct LateralPIDTuning { |
||||
kpBP @0 :List(Float32); |
||||
kpV @1 :List(Float32); |
||||
kiBP @2 :List(Float32); |
||||
kiV @3 :List(Float32); |
||||
kf @4 :Float32; |
||||
} |
||||
|
||||
struct LateralTorqueTuning { |
||||
useSteeringAngle @0 :Bool; |
||||
kp @1 :Float32; |
||||
ki @2 :Float32; |
||||
friction @3 :Float32; |
||||
kf @4 :Float32; |
||||
steeringAngleDeadzoneDeg @5 :Float32; |
||||
latAccelFactor @6 :Float32; |
||||
latAccelOffset @7 :Float32; |
||||
} |
||||
|
||||
struct LongitudinalPIDTuning { |
||||
kpBP @0 :List(Float32); |
||||
kpV @1 :List(Float32); |
||||
kiBP @2 :List(Float32); |
||||
kiV @3 :List(Float32); |
||||
kf @6 :Float32; |
||||
deadzoneBPDEPRECATED @4 :List(Float32); |
||||
deadzoneVDEPRECATED @5 :List(Float32); |
||||
} |
||||
|
||||
struct LateralINDITuning { |
||||
outerLoopGainBP @4 :List(Float32); |
||||
outerLoopGainV @5 :List(Float32); |
||||
innerLoopGainBP @6 :List(Float32); |
||||
innerLoopGainV @7 :List(Float32); |
||||
timeConstantBP @8 :List(Float32); |
||||
timeConstantV @9 :List(Float32); |
||||
actuatorEffectivenessBP @10 :List(Float32); |
||||
actuatorEffectivenessV @11 :List(Float32); |
||||
|
||||
outerLoopGainDEPRECATED @0 :Float32; |
||||
innerLoopGainDEPRECATED @1 :Float32; |
||||
timeConstantDEPRECATED @2 :Float32; |
||||
actuatorEffectivenessDEPRECATED @3 :Float32; |
||||
} |
||||
|
||||
struct LateralLQRTuning { |
||||
scale @0 :Float32; |
||||
ki @1 :Float32; |
||||
dcGain @2 :Float32; |
||||
|
||||
# State space system |
||||
a @3 :List(Float32); |
||||
b @4 :List(Float32); |
||||
c @5 :List(Float32); |
||||
|
||||
k @6 :List(Float32); # LQR gain |
||||
l @7 :List(Float32); # Kalman gain |
||||
} |
||||
|
||||
enum SafetyModel { |
||||
silent @0; |
||||
hondaNidec @1; |
||||
toyota @2; |
||||
elm327 @3; |
||||
gm @4; |
||||
hondaBoschGiraffe @5; |
||||
ford @6; |
||||
cadillac @7; |
||||
hyundai @8; |
||||
chrysler @9; |
||||
tesla @10; |
||||
subaru @11; |
||||
gmPassive @12; |
||||
mazda @13; |
||||
nissan @14; |
||||
volkswagen @15; |
||||
toyotaIpas @16; |
||||
allOutput @17; |
||||
gmAscm @18; |
||||
noOutput @19; # like silent but without silent CAN TXs |
||||
hondaBosch @20; |
||||
volkswagenPq @21; |
||||
subaruPreglobal @22; # pre-Global platform |
||||
hyundaiLegacy @23; |
||||
hyundaiCommunity @24; |
||||
volkswagenMlb @25; |
||||
hongqi @26; |
||||
body @27; |
||||
hyundaiCanfd @28; |
||||
volkswagenMqbEvo @29; |
||||
chryslerCusw @30; |
||||
psa @31; |
||||
} |
||||
|
||||
enum SteerControlType { |
||||
torque @0; |
||||
angle @1; |
||||
|
||||
curvatureDEPRECATED @2; |
||||
} |
||||
|
||||
enum TransmissionType { |
||||
unknown @0; |
||||
automatic @1; # Traditional auto, including DSG |
||||
manual @2; # True "stick shift" only |
||||
direct @3; # Electric vehicle or other direct drive |
||||
cvt @4; |
||||
} |
||||
|
||||
struct CarFw { |
||||
ecu @0 :Ecu; |
||||
fwVersion @1 :Data; |
||||
address @2 :UInt32; |
||||
subAddress @3 :UInt8; |
||||
responseAddress @4 :UInt32; |
||||
request @5 :List(Data); |
||||
brand @6 :Text; |
||||
bus @7 :UInt8; |
||||
logging @8 :Bool; |
||||
obdMultiplexing @9 :Bool; |
||||
} |
||||
|
||||
enum Ecu { |
||||
eps @0; |
||||
abs @1; |
||||
fwdRadar @2; |
||||
fwdCamera @3; |
||||
engine @4; |
||||
unknown @5; |
||||
transmission @8; # Transmission Control Module |
||||
hybrid @18; # hybrid control unit, e.g. Chrysler's HCP, Honda's IMA Control Unit, Toyota's hybrid control computer |
||||
srs @9; # airbag |
||||
gateway @10; # can gateway |
||||
hud @11; # heads up display |
||||
combinationMeter @12; # instrument cluster |
||||
electricBrakeBooster @15; |
||||
shiftByWire @16; |
||||
adas @19; |
||||
cornerRadar @21; |
||||
hvac @20; |
||||
parkingAdas @7; # parking assist system ECU, e.g. Toyota's IPAS, Hyundai's RSPA, etc. |
||||
epb @22; # electronic parking brake |
||||
telematics @23; |
||||
body @24; # body control module |
||||
|
||||
# Toyota only |
||||
dsu @6; |
||||
|
||||
# Honda only |
||||
vsa @13; # Vehicle Stability Assist |
||||
programmedFuelInjection @14; |
||||
|
||||
debug @17; |
||||
} |
||||
|
||||
enum FingerprintSource { |
||||
can @0; |
||||
fw @1; |
||||
fixed @2; |
||||
} |
||||
|
||||
enum NetworkLocation { |
||||
fwdCamera @0; # Standard/default integration at LKAS camera |
||||
gateway @1; # Integration at vehicle's CAN gateway |
||||
} |
||||
|
||||
enableGasInterceptorDEPRECATED @2 :Bool; |
||||
enableCameraDEPRECATED @4 :Bool; |
||||
enableApgsDEPRECATED @6 :Bool; |
||||
steerRateCostDEPRECATED @33 :Float32; |
||||
isPandaBlackDEPRECATED @39 :Bool; |
||||
hasStockCameraDEPRECATED @57 :Bool; |
||||
safetyParamDEPRECATED @10 :Int16; |
||||
safetyModelDEPRECATED @9 :SafetyModel; |
||||
safetyModelPassiveDEPRECATED @42 :SafetyModel = silent; |
||||
minSpeedCanDEPRECATED @51 :Float32; |
||||
communityFeatureDEPRECATED @46: Bool; |
||||
startingAccelRateDEPRECATED @53 :Float32; |
||||
steerMaxBPDEPRECATED @11 :List(Float32); |
||||
steerMaxVDEPRECATED @12 :List(Float32); |
||||
gasMaxBPDEPRECATED @13 :List(Float32); |
||||
gasMaxVDEPRECATED @14 :List(Float32); |
||||
brakeMaxBPDEPRECATED @15 :List(Float32); |
||||
brakeMaxVDEPRECATED @16 :List(Float32); |
||||
directAccelControlDEPRECATED @30 :Bool; |
||||
maxSteeringAngleDegDEPRECATED @54 :Float32; |
||||
longitudinalActuatorDelayLowerBoundDEPRECATEDDEPRECATED @61 :Float32; |
||||
} |
||||
@ -0,0 +1 @@ |
||||
../opendbc_repo/opendbc/car/car.capnp |
||||
@ -1,22 +1,39 @@ |
||||
# What is a car port? |
||||
|
||||
A car port enables openpilot support on a particular car. Each car model openpilot supports needs to be individually ported. All car ports live in `openpilot/selfdrive/car/car_specific.py` and `opendbc_repo/opendbc/car`. |
||||
A car port enables openpilot support on a particular car. Each car model openpilot supports needs to be individually ported. The complexity of a car port varies depending on many factors including: |
||||
|
||||
The complexity of a car port varies depending on many factors including: |
||||
* existing openpilot support for similar cars |
||||
* architecture and APIs available in the car |
||||
|
||||
|
||||
# Structure of a car port |
||||
|
||||
Virtually all car-specific code is contained in two other repositories: [opendbc](https://github.com/commaai/opendbc) and [panda](https://github.com/commaai/panda). |
||||
|
||||
## opendbc |
||||
|
||||
Each car brand is supported by a standard interface structure in `opendbc/car/[brand]`: |
||||
|
||||
* `interface.py`: Interface for the car, defines the CarInterface class |
||||
* `carstate.py`: Reads CAN from car and builds openpilot CarState message |
||||
* `carcontroller.py`: Builds CAN messages to send to car |
||||
* `carstate.py`: Reads CAN messages from the car and builds openpilot CarState messages |
||||
* `carcontroller.py`: Control logic for executing openpilot CarControl actions on the car |
||||
* `[brand]can.py`: Composes CAN messages for carcontroller to send |
||||
* `values.py`: Limits for actuation, general constants for cars, and supported car documentation |
||||
* `radar_interface.py`: Interface for parsing radar points from the car |
||||
* `radar_interface.py`: Interface for parsing radar points from the car, if applicable |
||||
|
||||
## panda |
||||
|
||||
* `board/safety/safety_[brand].h`: Brand-specific safety logic |
||||
* `tests/safety/test_[brand].py`: Brand-specific safety CI tests |
||||
|
||||
## openpilot |
||||
|
||||
For historical reasons, openpilot still contains a small amount of car-specific logic. This will eventually be migrated to opendbc or otherwise removed. |
||||
|
||||
* `selfdrive/car/car_specific.py`: Brand-specific event logic |
||||
|
||||
# Overiew |
||||
# Overview |
||||
|
||||
[Jason Young](https://github.com/jyoung8607) gave a talk at COMMA_CON with an overview of the car porting process. The talk is available on YouTube: |
||||
|
||||
https://youtu.be/KcfzEHB6ms4?si=5szh1PX6TksOCKmM |
||||
https://www.youtube.com/watch?v=XxPS5TpTUnI |
||||
|
||||
@ -1 +1 @@ |
||||
Subproject commit ff198e9af1cea1ab93d24b01beeb04ab3f2de742 |
||||
Subproject commit 739117d561b265de5caf843c3f117490a9de57e6 |
||||
@ -1 +1 @@ |
||||
Subproject commit aac60b8a7938d4d32c0c02608058022e4d876156 |
||||
Subproject commit 08c95bf47ba6d0c5d11a7616e42e10b8dbde19eb |
||||
@ -1,3 +0,0 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:a3cd3b91673eded1282e2082be0efa8e54ed477b5feb3580e521d08078e18ed1 |
||||
size 42640 |
||||
@ -1,19 +0,0 @@ |
||||
## Car port structure |
||||
|
||||
### interface.py |
||||
Generic interface to send and receive messages from CAN (controlsd uses this to communicate with car) |
||||
|
||||
### fingerprints.py |
||||
Fingerprints for matching to a specific car |
||||
|
||||
### carcontroller.py |
||||
Builds CAN messages to send to car |
||||
|
||||
##### carstate.py |
||||
Reads CAN from car and builds openpilot CarState message |
||||
|
||||
##### values.py |
||||
Limits for actuation, general constants for cars, and supported car documentation |
||||
|
||||
##### radar_interface.py |
||||
Interface for parsing radar points from the car |
||||
@ -1,72 +0,0 @@ |
||||
import capnp |
||||
from typing import Any |
||||
|
||||
from cereal import car |
||||
from opendbc.car import structs |
||||
|
||||
_FIELDS = '__dataclass_fields__' # copy of dataclasses._FIELDS |
||||
|
||||
|
||||
def is_dataclass(obj): |
||||
"""Similar to dataclasses.is_dataclass without instance type check checking""" |
||||
return hasattr(obj, _FIELDS) |
||||
|
||||
|
||||
def _asdictref_inner(obj) -> dict[str, Any] | Any: |
||||
if is_dataclass(obj): |
||||
ret = {} |
||||
for field in getattr(obj, _FIELDS): # similar to dataclasses.fields() |
||||
ret[field] = _asdictref_inner(getattr(obj, field)) |
||||
return ret |
||||
elif isinstance(obj, (tuple, list)): |
||||
return type(obj)(_asdictref_inner(v) for v in obj) |
||||
else: |
||||
return obj |
||||
|
||||
|
||||
def asdictref(obj) -> dict[str, Any]: |
||||
""" |
||||
Similar to dataclasses.asdict without recursive type checking and copy.deepcopy |
||||
Note that the resulting dict will contain references to the original struct as a result |
||||
""" |
||||
if not is_dataclass(obj): |
||||
raise TypeError("asdictref() should be called on dataclass instances") |
||||
|
||||
return _asdictref_inner(obj) |
||||
|
||||
|
||||
def convert_to_capnp(struct: structs.CarParams | structs.CarState | structs.CarControl.Actuators) -> capnp.lib.capnp._DynamicStructBuilder: |
||||
struct_dict = asdictref(struct) |
||||
|
||||
if isinstance(struct, structs.CarParams): |
||||
del struct_dict['lateralTuning'] |
||||
struct_capnp = car.CarParams.new_message(**struct_dict) |
||||
|
||||
# this is the only union, special handling |
||||
which = struct.lateralTuning.which() |
||||
struct_capnp.lateralTuning.init(which) |
||||
lateralTuning_dict = asdictref(getattr(struct.lateralTuning, which)) |
||||
setattr(struct_capnp.lateralTuning, which, lateralTuning_dict) |
||||
elif isinstance(struct, structs.CarState): |
||||
struct_capnp = car.CarState.new_message(**struct_dict) |
||||
elif isinstance(struct, structs.CarControl.Actuators): |
||||
struct_capnp = car.CarControl.Actuators.new_message(**struct_dict) |
||||
else: |
||||
raise ValueError(f"Unsupported struct type: {type(struct)}") |
||||
|
||||
return struct_capnp |
||||
|
||||
|
||||
def convert_carControl(struct: capnp.lib.capnp._DynamicStructReader) -> structs.CarControl: |
||||
# TODO: recursively handle any car struct as needed |
||||
def remove_deprecated(s: dict) -> dict: |
||||
return {k: v for k, v in s.items() if not k.endswith('DEPRECATED')} |
||||
|
||||
struct_dict = struct.to_dict() |
||||
struct_dataclass = structs.CarControl(**remove_deprecated({k: v for k, v in struct_dict.items() if not isinstance(k, dict)})) |
||||
|
||||
struct_dataclass.actuators = structs.CarControl.Actuators(**remove_deprecated(struct_dict.get('actuators', {}))) |
||||
struct_dataclass.cruiseControl = structs.CarControl.CruiseControl(**remove_deprecated(struct_dict.get('cruiseControl', {}))) |
||||
struct_dataclass.hudControl = structs.CarControl.HUDControl(**remove_deprecated(struct_dict.get('hudControl', {}))) |
||||
|
||||
return struct_dataclass |
||||
@ -0,0 +1,41 @@ |
||||
from cereal import log |
||||
from openpilot.common.realtime import DT_CTRL |
||||
from openpilot.common.conversions import Conversions as CV |
||||
|
||||
|
||||
CAMERA_OFFSET = 0.04 |
||||
LDW_MIN_SPEED = 31 * CV.MPH_TO_MS |
||||
LANE_DEPARTURE_THRESHOLD = 0.1 |
||||
|
||||
class LaneDepartureWarning: |
||||
def __init__(self): |
||||
self.left = False |
||||
self.right = False |
||||
self.last_blinker_frame = 0 |
||||
|
||||
def update(self, frame, modelV2, CS, CC): |
||||
if CS.leftBlinker or CS.rightBlinker: |
||||
self.last_blinker_frame = frame |
||||
|
||||
recent_blinker = (frame - self.last_blinker_frame) * DT_CTRL < 5.0 # 5s blinker cooldown |
||||
ldw_allowed = CS.vEgo > LDW_MIN_SPEED and not recent_blinker and not CC.latActive |
||||
|
||||
desire_prediction = modelV2.meta.desirePrediction |
||||
if len(desire_prediction) and ldw_allowed: |
||||
right_lane_visible = modelV2.laneLineProbs[2] > 0.5 |
||||
left_lane_visible = modelV2.laneLineProbs[1] > 0.5 |
||||
l_lane_change_prob = desire_prediction[log.Desire.laneChangeLeft] |
||||
r_lane_change_prob = desire_prediction[log.Desire.laneChangeRight] |
||||
|
||||
lane_lines = modelV2.laneLines |
||||
l_lane_close = left_lane_visible and (lane_lines[1].y[0] > -(1.08 + CAMERA_OFFSET)) |
||||
r_lane_close = right_lane_visible and (lane_lines[2].y[0] < (1.08 - CAMERA_OFFSET)) |
||||
|
||||
self.left = bool(l_lane_change_prob > LANE_DEPARTURE_THRESHOLD and l_lane_close) |
||||
self.right = bool(r_lane_change_prob > LANE_DEPARTURE_THRESHOLD and r_lane_close) |
||||
else: |
||||
self.left, self.right = False, False |
||||
|
||||
@property |
||||
def warning(self) -> bool: |
||||
return bool(self.left or self.right) |
||||
@ -1,121 +0,0 @@ |
||||
import os |
||||
from parameterized import parameterized |
||||
|
||||
from cereal import log, car |
||||
import cereal.messaging as messaging |
||||
from opendbc.car.fingerprints import _FINGERPRINTS |
||||
from opendbc.car.toyota.values import CAR as TOYOTA |
||||
from opendbc.car.mazda.values import CAR as MAZDA |
||||
from openpilot.common.params import Params |
||||
from openpilot.selfdrive.pandad.pandad_api_impl import can_list_to_can_capnp |
||||
from openpilot.selfdrive.controls.lib.events import EVENT_NAME |
||||
from openpilot.system.manager.process_config import managed_processes |
||||
|
||||
EventName = car.OnroadEvent.EventName |
||||
Ecu = car.CarParams.Ecu |
||||
|
||||
COROLLA_FW_VERSIONS = [ |
||||
(Ecu.engine, 0x7e0, None, b'\x0230ZC2000\x00\x00\x00\x00\x00\x00\x00\x0050212000\x00\x00\x00\x00\x00\x00\x00\x00'), |
||||
(Ecu.abs, 0x7b0, None, b'F152602190\x00\x00\x00\x00\x00\x00'), |
||||
(Ecu.eps, 0x7a1, None, b'8965B02181\x00\x00\x00\x00\x00\x00'), |
||||
(Ecu.fwdRadar, 0x750, 0xf, b'8821F4702100\x00\x00\x00\x00'), |
||||
(Ecu.fwdCamera, 0x750, 0x6d, b'8646F0201101\x00\x00\x00\x00'), |
||||
(Ecu.dsu, 0x791, None, b'881510201100\x00\x00\x00\x00'), |
||||
] |
||||
COROLLA_FW_VERSIONS_FUZZY = COROLLA_FW_VERSIONS[:-1] + [(Ecu.dsu, 0x791, None, b'xxxxxx')] |
||||
COROLLA_FW_VERSIONS_NO_DSU = COROLLA_FW_VERSIONS[:-1] |
||||
|
||||
CX5_FW_VERSIONS = [ |
||||
(Ecu.engine, 0x7e0, None, b'PYNF-188K2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'), |
||||
(Ecu.abs, 0x760, None, b'K123-437K2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'), |
||||
(Ecu.eps, 0x730, None, b'KJ01-3210X-G-00\x00\x00\x00\x00\x00\x00\x00\x00\x00'), |
||||
(Ecu.fwdRadar, 0x764, None, b'K123-67XK2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'), |
||||
(Ecu.fwdCamera, 0x706, None, b'B61L-67XK2-T\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'), |
||||
(Ecu.transmission, 0x7e1, None, b'PYNC-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'), |
||||
] |
||||
|
||||
|
||||
@parameterized.expand([ |
||||
# TODO: test EventName.startup for release branches |
||||
|
||||
# officially supported car |
||||
(EventName.startupMaster, TOYOTA.TOYOTA_COROLLA, COROLLA_FW_VERSIONS, "toyota"), |
||||
(EventName.startupMaster, TOYOTA.TOYOTA_COROLLA, COROLLA_FW_VERSIONS, "toyota"), |
||||
|
||||
# dashcamOnly car |
||||
(EventName.startupNoControl, MAZDA.MAZDA_CX5, CX5_FW_VERSIONS, "mazda"), |
||||
(EventName.startupNoControl, MAZDA.MAZDA_CX5, CX5_FW_VERSIONS, "mazda"), |
||||
|
||||
# unrecognized car with no fw |
||||
(EventName.startupNoFw, None, None, ""), |
||||
(EventName.startupNoFw, None, None, ""), |
||||
|
||||
# unrecognized car |
||||
(EventName.startupNoCar, None, COROLLA_FW_VERSIONS[:1], "toyota"), |
||||
(EventName.startupNoCar, None, COROLLA_FW_VERSIONS[:1], "toyota"), |
||||
|
||||
# fuzzy match |
||||
(EventName.startupMaster, TOYOTA.TOYOTA_COROLLA, COROLLA_FW_VERSIONS_FUZZY, "toyota"), |
||||
(EventName.startupMaster, TOYOTA.TOYOTA_COROLLA, COROLLA_FW_VERSIONS_FUZZY, "toyota"), |
||||
]) |
||||
def test_startup_alert(expected_event, car_model, fw_versions, brand): |
||||
controls_sock = messaging.sub_sock("selfdriveState") |
||||
pm = messaging.PubMaster(['can', 'pandaStates']) |
||||
|
||||
params = Params() |
||||
params.put_bool("OpenpilotEnabledToggle", True) |
||||
|
||||
# Build capnn version of FW array |
||||
if fw_versions is not None: |
||||
car_fw = [] |
||||
cp = car.CarParams.new_message() |
||||
for ecu, addr, subaddress, version in fw_versions: |
||||
f = car.CarParams.CarFw.new_message() |
||||
f.ecu = ecu |
||||
f.address = addr |
||||
f.fwVersion = version |
||||
f.brand = brand |
||||
|
||||
if subaddress is not None: |
||||
f.subAddress = subaddress |
||||
|
||||
car_fw.append(f) |
||||
cp.carVin = "1" * 17 |
||||
cp.carFw = car_fw |
||||
params.put("CarParamsCache", cp.to_bytes()) |
||||
else: |
||||
os.environ['SKIP_FW_QUERY'] = '1' |
||||
|
||||
managed_processes['controlsd'].start() |
||||
managed_processes['card'].start() |
||||
|
||||
assert pm.wait_for_readers_to_update('can', 5) |
||||
pm.send('can', can_list_to_can_capnp([[0, b"", 0]])) |
||||
|
||||
assert pm.wait_for_readers_to_update('pandaStates', 5) |
||||
msg = messaging.new_message('pandaStates', 1) |
||||
msg.pandaStates[0].pandaType = log.PandaState.PandaType.uno |
||||
pm.send('pandaStates', msg) |
||||
|
||||
# fingerprint |
||||
if (car_model is None) or (fw_versions is not None): |
||||
finger = {addr: 1 for addr in range(1, 100)} |
||||
else: |
||||
finger = _FINGERPRINTS[car_model][0] |
||||
|
||||
msgs = [[addr, b'\x00'*length, 0] for addr, length in finger.items()] |
||||
for _ in range(1000): |
||||
# card waits for pandad to echo back that it has changed the multiplexing mode |
||||
if not params.get_bool("ObdMultiplexingChanged"): |
||||
params.put_bool("ObdMultiplexingChanged", True) |
||||
|
||||
pm.send('can', can_list_to_can_capnp(msgs)) |
||||
assert pm.wait_for_readers_to_update('can', 5, dt=0.001), f"step: {_}" |
||||
|
||||
ctrls = messaging.drain_sock(controls_sock) |
||||
if len(ctrls): |
||||
event_name = ctrls[0].selfdriveState.alertType.split("/")[0] |
||||
assert EVENT_NAME[expected_event] == event_name, f"expected {EVENT_NAME[expected_event]} for '{car_model}', got {event_name}" |
||||
break |
||||
else: |
||||
raise Exception(f"failed to fingerprint {car_model}") |
||||
@ -1,102 +0,0 @@ |
||||
from cereal import car, log |
||||
from opendbc.car.car_helpers import interfaces |
||||
from opendbc.car.mock.values import CAR as MOCK |
||||
from openpilot.common.realtime import DT_CTRL |
||||
from openpilot.selfdrive.controls.controlsd import Controls, SOFT_DISABLE_TIME |
||||
from openpilot.selfdrive.controls.lib.events import Events, ET, Alert, Priority, AlertSize, AlertStatus, VisualAlert, \ |
||||
AudibleAlert, EVENTS |
||||
|
||||
State = log.SelfdriveState.OpenpilotState |
||||
|
||||
# The event types that maintain the current state |
||||
MAINTAIN_STATES = {State.enabled: (None,), State.disabled: (None,), State.softDisabling: (ET.SOFT_DISABLE,), |
||||
State.preEnabled: (ET.PRE_ENABLE,), State.overriding: (ET.OVERRIDE_LATERAL, ET.OVERRIDE_LONGITUDINAL)} |
||||
ALL_STATES = tuple(State.schema.enumerants.values()) |
||||
# The event types checked in DISABLED section of state machine |
||||
ENABLE_EVENT_TYPES = (ET.ENABLE, ET.PRE_ENABLE, ET.OVERRIDE_LATERAL, ET.OVERRIDE_LONGITUDINAL) |
||||
|
||||
|
||||
def make_event(event_types): |
||||
event = {} |
||||
for ev in event_types: |
||||
event[ev] = Alert("", "", AlertStatus.normal, AlertSize.small, Priority.LOW, |
||||
VisualAlert.none, AudibleAlert.none, 1.) |
||||
EVENTS[0] = event |
||||
return 0 |
||||
|
||||
|
||||
class TestStateMachine: |
||||
|
||||
def setup_method(self): |
||||
CarInterface, CarController, CarState, RadarInterface = interfaces[MOCK.MOCK] |
||||
CP = CarInterface.get_non_essential_params(MOCK.MOCK) |
||||
CI = CarInterface(CP, CarController, CarState) |
||||
|
||||
self.controlsd = Controls(CI=CI) |
||||
self.controlsd.events = Events() |
||||
self.controlsd.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL) |
||||
self.CS = car.CarState() |
||||
|
||||
def test_immediate_disable(self): |
||||
for state in ALL_STATES: |
||||
for et in MAINTAIN_STATES[state]: |
||||
self.controlsd.events.add(make_event([et, ET.IMMEDIATE_DISABLE])) |
||||
self.controlsd.state = state |
||||
self.controlsd.state_transition(self.CS) |
||||
assert State.disabled == self.controlsd.state |
||||
self.controlsd.events.clear() |
||||
|
||||
def test_user_disable(self): |
||||
for state in ALL_STATES: |
||||
for et in MAINTAIN_STATES[state]: |
||||
self.controlsd.events.add(make_event([et, ET.USER_DISABLE])) |
||||
self.controlsd.state = state |
||||
self.controlsd.state_transition(self.CS) |
||||
assert State.disabled == self.controlsd.state |
||||
self.controlsd.events.clear() |
||||
|
||||
def test_soft_disable(self): |
||||
for state in ALL_STATES: |
||||
if state == State.preEnabled: # preEnabled considers NO_ENTRY instead |
||||
continue |
||||
for et in MAINTAIN_STATES[state]: |
||||
self.controlsd.events.add(make_event([et, ET.SOFT_DISABLE])) |
||||
self.controlsd.state = state |
||||
self.controlsd.state_transition(self.CS) |
||||
assert self.controlsd.state == State.disabled if state == State.disabled else State.softDisabling |
||||
self.controlsd.events.clear() |
||||
|
||||
def test_soft_disable_timer(self): |
||||
self.controlsd.state = State.enabled |
||||
self.controlsd.events.add(make_event([ET.SOFT_DISABLE])) |
||||
self.controlsd.state_transition(self.CS) |
||||
for _ in range(int(SOFT_DISABLE_TIME / DT_CTRL)): |
||||
assert self.controlsd.state == State.softDisabling |
||||
self.controlsd.state_transition(self.CS) |
||||
|
||||
assert self.controlsd.state == State.disabled |
||||
|
||||
def test_no_entry(self): |
||||
# Make sure noEntry keeps us disabled |
||||
for et in ENABLE_EVENT_TYPES: |
||||
self.controlsd.events.add(make_event([ET.NO_ENTRY, et])) |
||||
self.controlsd.state_transition(self.CS) |
||||
assert self.controlsd.state == State.disabled |
||||
self.controlsd.events.clear() |
||||
|
||||
def test_no_entry_pre_enable(self): |
||||
# preEnabled with noEntry event |
||||
self.controlsd.state = State.preEnabled |
||||
self.controlsd.events.add(make_event([ET.NO_ENTRY, ET.PRE_ENABLE])) |
||||
self.controlsd.state_transition(self.CS) |
||||
assert self.controlsd.state == State.preEnabled |
||||
|
||||
def test_maintain_states(self): |
||||
# Given current state's event type, we should maintain state |
||||
for state in ALL_STATES: |
||||
for et in MAINTAIN_STATES[state]: |
||||
self.controlsd.state = state |
||||
self.controlsd.events.add(make_event([et])) |
||||
self.controlsd.state_transition(self.CS) |
||||
assert self.controlsd.state == state |
||||
self.controlsd.events.clear() |
||||
@ -0,0 +1,10 @@ |
||||
#!/usr/bin/env bash |
||||
|
||||
DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)" |
||||
cd "$DIR/../../" |
||||
|
||||
if [ -f "$DIR/libthneed.so" ]; then |
||||
export LD_PRELOAD="$DIR/libthneed.so" |
||||
fi |
||||
|
||||
exec "$DIR/dmonitoringmodeld.py" "$@" |
||||
@ -1,2 +1,2 @@ |
||||
5ec97a39-0095-4cea-adfa-6d72b1966cc1 |
||||
26cac7a9757a27c783a365403040a1bd27ccdaea |
||||
fa69be01-b430-4504-9d72-7dcb058eb6dd |
||||
d9fb22d1c4fa3ca3d201dbc8edf1d0f0918e53e6 |
||||
|
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:3dd3982940d823c4fbb0429b733a0b78b0688d7d67aa76ff7b754a3e2f3d8683 |
||||
size 16132780 |
||||
oid sha256:50efe6451a3fb3fa04b6bb0e846544533329bd46ecefe9e657e91214dee2aaeb |
||||
size 7196502 |
||||
|
||||
@ -1,3 +0,0 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:7c26f13816b143f5bb29ac2980f8557bd5687a75729e4d895313fb9a5a1f0f46 |
||||
size 4488449 |
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:39786068cae1ed8c0dc34ef80c281dfcc67ed18a50e06b90765c49bcfdbf7db4 |
||||
size 51453312 |
||||
oid sha256:2431f40b8ca9926629e461e06316f9bbba984c821ebbc11e6449ca0c96c42d95 |
||||
size 50309976 |
||||
|
||||
@ -0,0 +1,502 @@ |
||||
#!/usr/bin/env python3 |
||||
import os |
||||
import time |
||||
import threading |
||||
|
||||
import cereal.messaging as messaging |
||||
|
||||
from cereal import car, log |
||||
from msgq.visionipc import VisionIpcClient, VisionStreamType |
||||
from panda import ALTERNATIVE_EXPERIENCE |
||||
|
||||
|
||||
from openpilot.common.params import Params |
||||
from openpilot.common.realtime import config_realtime_process, Priority, Ratekeeper, DT_CTRL |
||||
from openpilot.common.swaglog import cloudlog |
||||
from openpilot.common.gps import get_gps_location_service |
||||
|
||||
from openpilot.selfdrive.car.car_specific import CarSpecificEvents |
||||
from openpilot.selfdrive.selfdrived.events import Events, ET |
||||
from openpilot.selfdrive.selfdrived.state import StateMachine |
||||
from openpilot.selfdrive.selfdrived.alertmanager import AlertManager, set_offroad_alert |
||||
from openpilot.selfdrive.controls.lib.latcontrol import MIN_LATERAL_CONTROL_SPEED |
||||
|
||||
from openpilot.system.hardware import HARDWARE |
||||
from openpilot.system.version import get_build_metadata |
||||
|
||||
REPLAY = "REPLAY" in os.environ |
||||
SIMULATION = "SIMULATION" in os.environ |
||||
TESTING_CLOSET = "TESTING_CLOSET" in os.environ |
||||
IGNORE_PROCESSES = {"loggerd", "encoderd", "statsd"} |
||||
LONGITUDINAL_PERSONALITY_MAP = {v: k for k, v in log.LongitudinalPersonality.schema.enumerants.items()} |
||||
|
||||
ThermalStatus = log.DeviceState.ThermalStatus |
||||
State = log.SelfdriveState.OpenpilotState |
||||
PandaType = log.PandaState.PandaType |
||||
LaneChangeState = log.LaneChangeState |
||||
LaneChangeDirection = log.LaneChangeDirection |
||||
EventName = log.OnroadEvent.EventName |
||||
ButtonType = car.CarState.ButtonEvent.Type |
||||
SafetyModel = car.CarParams.SafetyModel |
||||
|
||||
IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput) |
||||
|
||||
|
||||
class SelfdriveD: |
||||
def __init__(self, CP=None): |
||||
self.params = Params() |
||||
|
||||
# Ensure the current branch is cached, otherwise the first cycle lags |
||||
build_metadata = get_build_metadata() |
||||
|
||||
if CP is None: |
||||
cloudlog.info("selfdrived is waiting for CarParams") |
||||
self.CP = messaging.log_from_bytes(self.params.get("CarParams", block=True), car.CarParams) |
||||
cloudlog.info("selfdrived got CarParams") |
||||
else: |
||||
self.CP = CP |
||||
|
||||
self.car_events = CarSpecificEvents(self.CP) |
||||
self.disengage_on_accelerator = not (self.CP.alternativeExperience & ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS) |
||||
|
||||
# Setup sockets |
||||
self.pm = messaging.PubMaster(['selfdriveState', 'onroadEvents']) |
||||
|
||||
self.gps_location_service = get_gps_location_service(self.params) |
||||
self.gps_packets = [self.gps_location_service] |
||||
self.sensor_packets = ["accelerometer", "gyroscope"] |
||||
self.camera_packets = ["roadCameraState", "driverCameraState", "wideRoadCameraState"] |
||||
|
||||
# TODO: de-couple selfdrived with card/conflate on carState without introducing controls mismatches |
||||
self.car_state_sock = messaging.sub_sock('carState', timeout=20) |
||||
|
||||
ignore = self.sensor_packets + self.gps_packets + ['alertDebug'] |
||||
if SIMULATION: |
||||
ignore += ['driverCameraState', 'managerState'] |
||||
if REPLAY: |
||||
# no vipc in replay will make them ignored anyways |
||||
ignore += ['roadCameraState', 'wideRoadCameraState'] |
||||
self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration', |
||||
'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'livePose', |
||||
'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters', |
||||
'controlsState', 'carControl', 'driverAssistance', 'alertDebug'] + \ |
||||
self.camera_packets + self.sensor_packets + self.gps_packets, |
||||
ignore_alive=ignore, ignore_avg_freq=ignore+['radarState',], |
||||
ignore_valid=ignore, frequency=int(1/DT_CTRL)) |
||||
|
||||
# read params |
||||
self.is_metric = self.params.get_bool("IsMetric") |
||||
self.is_ldw_enabled = self.params.get_bool("IsLdwEnabled") |
||||
|
||||
# detect sound card presence and ensure successful init |
||||
sounds_available = HARDWARE.get_sound_card_online() |
||||
|
||||
car_recognized = self.CP.carName != 'mock' |
||||
|
||||
# cleanup old params |
||||
if not self.CP.experimentalLongitudinalAvailable: |
||||
self.params.remove("ExperimentalLongitudinalEnabled") |
||||
if not self.CP.openpilotLongitudinalControl: |
||||
self.params.remove("ExperimentalMode") |
||||
|
||||
self.CS_prev = car.CarState.new_message() |
||||
self.AM = AlertManager() |
||||
self.events = Events() |
||||
|
||||
self.initialized = False |
||||
self.enabled = False |
||||
self.active = False |
||||
self.mismatch_counter = 0 |
||||
self.cruise_mismatch_counter = 0 |
||||
self.last_steering_pressed_frame = 0 |
||||
self.distance_traveled = 0 |
||||
self.last_functional_fan_frame = 0 |
||||
self.events_prev = [] |
||||
self.logged_comm_issue = None |
||||
self.not_running_prev = None |
||||
self.experimental_mode = False |
||||
self.personality = self.read_personality_param() |
||||
self.recalibrating_seen = False |
||||
self.state_machine = StateMachine() |
||||
self.rk = Ratekeeper(100, print_delay_threshold=None) |
||||
|
||||
# Determine startup event |
||||
self.startup_event = EventName.startup if build_metadata.openpilot.comma_remote and build_metadata.tested_channel else EventName.startupMaster |
||||
if not car_recognized: |
||||
self.startup_event = EventName.startupNoCar |
||||
elif car_recognized and self.CP.passive: |
||||
self.startup_event = EventName.startupNoControl |
||||
elif self.CP.secOcRequired and not self.CP.secOcKeyAvailable: |
||||
self.startup_event = EventName.startupNoSecOcKey |
||||
|
||||
if not sounds_available: |
||||
self.events.add(EventName.soundsUnavailable, static=True) |
||||
if not car_recognized: |
||||
self.events.add(EventName.carUnrecognized, static=True) |
||||
set_offroad_alert("Offroad_CarUnrecognized", True) |
||||
elif self.CP.passive: |
||||
self.events.add(EventName.dashcamMode, static=True) |
||||
|
||||
def update_events(self, CS): |
||||
"""Compute onroadEvents from carState""" |
||||
|
||||
self.events.clear() |
||||
|
||||
if self.sm['controlsState'].lateralControlState.which() == 'debugState': |
||||
self.events.add(EventName.joystickDebug) |
||||
self.startup_event = None |
||||
|
||||
if self.sm.recv_frame['alertDebug'] > 0: |
||||
self.events.add(EventName.longitudinalManeuver) |
||||
self.startup_event = None |
||||
|
||||
# Add startup event |
||||
if self.startup_event is not None: |
||||
self.events.add(self.startup_event) |
||||
self.startup_event = None |
||||
|
||||
# Don't add any more events if not initialized |
||||
if not self.initialized: |
||||
self.events.add(EventName.selfdriveInitializing) |
||||
return |
||||
|
||||
# no more events while in dashcam mode |
||||
if self.CP.passive: |
||||
return |
||||
|
||||
# Block resume if cruise never previously enabled |
||||
resume_pressed = any(be.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for be in CS.buttonEvents) |
||||
if not self.CP.pcmCruise and CS.vCruise > 250 and resume_pressed: |
||||
self.events.add(EventName.resumeBlocked) |
||||
|
||||
if not self.CP.notCar: |
||||
self.events.add_from_msg(self.sm['driverMonitoringState'].events) |
||||
|
||||
# Add car events, ignore if CAN isn't valid |
||||
if CS.canValid: |
||||
car_events = self.car_events.update(CS, self.CS_prev, self.sm['carControl']).to_msg() |
||||
self.events.add_from_msg(car_events) |
||||
|
||||
if self.CP.notCar: |
||||
# wait for everything to init first |
||||
if self.sm.frame > int(5. / DT_CTRL) and self.initialized: |
||||
# body always wants to enable |
||||
self.events.add(EventName.pcmEnable) |
||||
|
||||
# Disable on rising edge of accelerator or brake. Also disable on brake when speed > 0 |
||||
if (CS.gasPressed and not self.CS_prev.gasPressed and self.disengage_on_accelerator) or \ |
||||
(CS.brakePressed and (not self.CS_prev.brakePressed or not CS.standstill)) or \ |
||||
(CS.regenBraking and (not self.CS_prev.regenBraking or not CS.standstill)): |
||||
self.events.add(EventName.pedalPressed) |
||||
|
||||
# Create events for temperature, disk space, and memory |
||||
if self.sm['deviceState'].thermalStatus >= ThermalStatus.red: |
||||
self.events.add(EventName.overheat) |
||||
if self.sm['deviceState'].freeSpacePercent < 7 and not SIMULATION: |
||||
self.events.add(EventName.outOfSpace) |
||||
if self.sm['deviceState'].memoryUsagePercent > 90 and not SIMULATION: |
||||
self.events.add(EventName.lowMemory) |
||||
|
||||
# Alert if fan isn't spinning for 5 seconds |
||||
if self.sm['peripheralState'].pandaType != log.PandaState.PandaType.unknown: |
||||
if self.sm['peripheralState'].fanSpeedRpm < 500 and self.sm['deviceState'].fanSpeedPercentDesired > 50: |
||||
# allow enough time for the fan controller in the panda to recover from stalls |
||||
if (self.sm.frame - self.last_functional_fan_frame) * DT_CTRL > 15.0: |
||||
self.events.add(EventName.fanMalfunction) |
||||
else: |
||||
self.last_functional_fan_frame = self.sm.frame |
||||
|
||||
# Handle calibration status |
||||
cal_status = self.sm['liveCalibration'].calStatus |
||||
if cal_status != log.LiveCalibrationData.Status.calibrated: |
||||
if cal_status == log.LiveCalibrationData.Status.uncalibrated: |
||||
self.events.add(EventName.calibrationIncomplete) |
||||
elif cal_status == log.LiveCalibrationData.Status.recalibrating: |
||||
if not self.recalibrating_seen: |
||||
set_offroad_alert("Offroad_Recalibration", True) |
||||
self.recalibrating_seen = True |
||||
self.events.add(EventName.calibrationRecalibrating) |
||||
else: |
||||
self.events.add(EventName.calibrationInvalid) |
||||
|
||||
# Lane departure warning |
||||
if self.is_ldw_enabled and self.sm.valid['driverAssistance']: |
||||
if self.sm['driverAssistance'].leftLaneDeparture or self.sm['driverAssistance'].rightLaneDeparture: |
||||
self.events.add(EventName.ldw) |
||||
|
||||
# Handle lane change |
||||
if self.sm['modelV2'].meta.laneChangeState == LaneChangeState.preLaneChange: |
||||
direction = self.sm['modelV2'].meta.laneChangeDirection |
||||
if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \ |
||||
(CS.rightBlindspot and direction == LaneChangeDirection.right): |
||||
self.events.add(EventName.laneChangeBlocked) |
||||
else: |
||||
if direction == LaneChangeDirection.left: |
||||
self.events.add(EventName.preLaneChangeLeft) |
||||
else: |
||||
self.events.add(EventName.preLaneChangeRight) |
||||
elif self.sm['modelV2'].meta.laneChangeState in (LaneChangeState.laneChangeStarting, |
||||
LaneChangeState.laneChangeFinishing): |
||||
self.events.add(EventName.laneChange) |
||||
|
||||
for i, pandaState in enumerate(self.sm['pandaStates']): |
||||
# All pandas must match the list of safetyConfigs, and if outside this list, must be silent or noOutput |
||||
if i < len(self.CP.safetyConfigs): |
||||
safety_mismatch = pandaState.safetyModel != self.CP.safetyConfigs[i].safetyModel or \ |
||||
pandaState.safetyParam != self.CP.safetyConfigs[i].safetyParam or \ |
||||
pandaState.alternativeExperience != self.CP.alternativeExperience |
||||
else: |
||||
safety_mismatch = pandaState.safetyModel not in IGNORED_SAFETY_MODES |
||||
|
||||
# safety mismatch allows some time for pandad to set the safety mode and publish it back from panda |
||||
if (safety_mismatch and self.sm.frame*DT_CTRL > 10.) or pandaState.safetyRxChecksInvalid or self.mismatch_counter >= 200: |
||||
self.events.add(EventName.controlsMismatch) |
||||
|
||||
if log.PandaState.FaultType.relayMalfunction in pandaState.faults: |
||||
self.events.add(EventName.relayMalfunction) |
||||
|
||||
# Handle HW and system malfunctions |
||||
# Order is very intentional here. Be careful when modifying this. |
||||
# All events here should at least have NO_ENTRY and SOFT_DISABLE. |
||||
num_events = len(self.events) |
||||
|
||||
not_running = {p.name for p in self.sm['managerState'].processes if not p.running and p.shouldBeRunning} |
||||
if self.sm.recv_frame['managerState'] and (not_running - IGNORE_PROCESSES): |
||||
self.events.add(EventName.processNotRunning) |
||||
if not_running != self.not_running_prev: |
||||
cloudlog.event("process_not_running", not_running=not_running, error=True) |
||||
self.not_running_prev = not_running |
||||
else: |
||||
if not SIMULATION and not self.rk.lagging: |
||||
if not self.sm.all_alive(self.camera_packets): |
||||
self.events.add(EventName.cameraMalfunction) |
||||
elif not self.sm.all_freq_ok(self.camera_packets): |
||||
self.events.add(EventName.cameraFrameRate) |
||||
if not REPLAY and self.rk.lagging: |
||||
self.events.add(EventName.selfdrivedLagging) |
||||
if len(self.sm['radarState'].radarErrors) or ((not self.rk.lagging or REPLAY) and not self.sm.all_checks(['radarState'])): |
||||
self.events.add(EventName.radarFault) |
||||
if not self.sm.valid['pandaStates']: |
||||
self.events.add(EventName.usbError) |
||||
if CS.canTimeout: |
||||
self.events.add(EventName.canBusMissing) |
||||
elif not CS.canValid: |
||||
self.events.add(EventName.canError) |
||||
|
||||
# generic catch-all. ideally, a more specific event should be added above instead |
||||
has_disable_events = self.events.contains(ET.NO_ENTRY) and (self.events.contains(ET.SOFT_DISABLE) or self.events.contains(ET.IMMEDIATE_DISABLE)) |
||||
no_system_errors = (not has_disable_events) or (len(self.events) == num_events) |
||||
if not self.sm.all_checks() and no_system_errors: |
||||
if not self.sm.all_alive(): |
||||
self.events.add(EventName.commIssue) |
||||
elif not self.sm.all_freq_ok(): |
||||
self.events.add(EventName.commIssueAvgFreq) |
||||
else: |
||||
self.events.add(EventName.commIssue) |
||||
|
||||
logs = { |
||||
'invalid': [s for s, valid in self.sm.valid.items() if not valid], |
||||
'not_alive': [s for s, alive in self.sm.alive.items() if not alive], |
||||
'not_freq_ok': [s for s, freq_ok in self.sm.freq_ok.items() if not freq_ok], |
||||
} |
||||
if logs != self.logged_comm_issue: |
||||
cloudlog.event("commIssue", error=True, **logs) |
||||
self.logged_comm_issue = logs |
||||
else: |
||||
self.logged_comm_issue = None |
||||
|
||||
if not self.CP.notCar: |
||||
if not self.sm['livePose'].posenetOK: |
||||
self.events.add(EventName.posenetInvalid) |
||||
if not self.sm['livePose'].inputsOK: |
||||
self.events.add(EventName.locationdTemporaryError) |
||||
if not self.sm['liveParameters'].valid and not TESTING_CLOSET and (not SIMULATION or REPLAY): |
||||
self.events.add(EventName.paramsdTemporaryError) |
||||
|
||||
# conservative HW alert. if the data or frequency are off, locationd will throw an error |
||||
if any((self.sm.frame - self.sm.recv_frame[s])*DT_CTRL > 10. for s in self.sensor_packets): |
||||
self.events.add(EventName.sensorDataInvalid) |
||||
|
||||
if not REPLAY: |
||||
# Check for mismatch between openpilot and car's PCM |
||||
cruise_mismatch = CS.cruiseState.enabled and (not self.enabled or not self.CP.pcmCruise) |
||||
self.cruise_mismatch_counter = self.cruise_mismatch_counter + 1 if cruise_mismatch else 0 |
||||
if self.cruise_mismatch_counter > int(6. / DT_CTRL): |
||||
self.events.add(EventName.cruiseMismatch) |
||||
|
||||
# Send a "steering required alert" if saturation count has reached the limit |
||||
if CS.steeringPressed: |
||||
self.last_steering_pressed_frame = self.sm.frame |
||||
recent_steer_pressed = (self.sm.frame - self.last_steering_pressed_frame)*DT_CTRL < 2.0 |
||||
controlstate = self.sm['controlsState'] |
||||
lac = getattr(controlstate.lateralControlState, controlstate.lateralControlState.which()) |
||||
if lac.active and not recent_steer_pressed and not self.CP.notCar: |
||||
clipped_speed = max(CS.vEgo, MIN_LATERAL_CONTROL_SPEED) |
||||
actual_lateral_accel = controlstate.curvature * (clipped_speed**2) |
||||
desired_lateral_accel = controlstate.desiredCurvature * (clipped_speed**2) |
||||
undershooting = abs(desired_lateral_accel) / abs(1e-3 + actual_lateral_accel) > 1.2 |
||||
turning = abs(desired_lateral_accel) > 1.0 |
||||
good_speed = CS.vEgo > 5 |
||||
if undershooting and turning and good_speed and lac.saturated: |
||||
self.events.add(EventName.steerSaturated) |
||||
|
||||
# Check for FCW |
||||
stock_long_is_braking = self.enabled and not self.CP.openpilotLongitudinalControl and CS.aEgo < -1.25 |
||||
model_fcw = self.sm['modelV2'].meta.hardBrakePredicted and not CS.brakePressed and not stock_long_is_braking |
||||
planner_fcw = self.sm['longitudinalPlan'].fcw and self.enabled |
||||
if (planner_fcw or model_fcw) and not self.CP.notCar: |
||||
self.events.add(EventName.fcw) |
||||
|
||||
# TODO: fix simulator |
||||
if not SIMULATION or REPLAY: |
||||
# Not show in first 1.5 km to allow for driving out of garage. This event shows after 5 minutes |
||||
gps_ok = self.sm.recv_frame[self.gps_location_service] > 0 and (self.sm.frame - self.sm.recv_frame[self.gps_location_service]) * DT_CTRL < 2.0 |
||||
if not gps_ok and self.sm['livePose'].inputsOK and (self.distance_traveled > 1500): |
||||
self.events.add(EventName.noGps) |
||||
if gps_ok: |
||||
self.distance_traveled = 0 |
||||
self.distance_traveled += abs(CS.vEgo) * DT_CTRL |
||||
|
||||
if self.sm['modelV2'].frameDropPerc > 20: |
||||
self.events.add(EventName.modeldLagging) |
||||
|
||||
# decrement personality on distance button press |
||||
if self.CP.openpilotLongitudinalControl: |
||||
if any(not be.pressed and be.type == ButtonType.gapAdjustCruise for be in CS.buttonEvents): |
||||
self.personality = (self.personality - 1) % 3 |
||||
self.params.put_nonblocking('LongitudinalPersonality', str(self.personality)) |
||||
self.events.add(EventName.personalityChanged) |
||||
|
||||
def data_sample(self): |
||||
car_state = messaging.recv_one(self.car_state_sock) |
||||
CS = car_state.carState if car_state else self.CS_prev |
||||
|
||||
self.sm.update(0) |
||||
|
||||
if not self.initialized: |
||||
all_valid = CS.canValid and self.sm.all_checks() |
||||
timed_out = self.sm.frame * DT_CTRL > 6. |
||||
if all_valid or timed_out or (SIMULATION and not REPLAY): |
||||
available_streams = VisionIpcClient.available_streams("camerad", block=False) |
||||
if VisionStreamType.VISION_STREAM_ROAD not in available_streams: |
||||
self.sm.ignore_alive.append('roadCameraState') |
||||
self.sm.ignore_valid.append('roadCameraState') |
||||
if VisionStreamType.VISION_STREAM_WIDE_ROAD not in available_streams: |
||||
self.sm.ignore_alive.append('wideRoadCameraState') |
||||
self.sm.ignore_valid.append('wideRoadCameraState') |
||||
|
||||
if REPLAY and any(ps.controlsAllowed for ps in self.sm['pandaStates']): |
||||
self.state_machine.state = State.enabled |
||||
|
||||
self.initialized = True |
||||
cloudlog.event( |
||||
"selfdrived.initialized", |
||||
dt=self.sm.frame*DT_CTRL, |
||||
timeout=timed_out, |
||||
canValid=CS.canValid, |
||||
invalid=[s for s, valid in self.sm.valid.items() if not valid], |
||||
not_alive=[s for s, alive in self.sm.alive.items() if not alive], |
||||
not_freq_ok=[s for s, freq_ok in self.sm.freq_ok.items() if not freq_ok], |
||||
error=True, |
||||
) |
||||
|
||||
# When the panda and selfdrived do not agree on controls_allowed |
||||
# we want to disengage openpilot. However the status from the panda goes through |
||||
# another socket other than the CAN messages and one can arrive earlier than the other. |
||||
# Therefore we allow a mismatch for two samples, then we trigger the disengagement. |
||||
if not self.enabled: |
||||
self.mismatch_counter = 0 |
||||
|
||||
# All pandas not in silent mode must have controlsAllowed when openpilot is enabled |
||||
if self.enabled and any(not ps.controlsAllowed for ps in self.sm['pandaStates'] |
||||
if ps.safetyModel not in IGNORED_SAFETY_MODES): |
||||
self.mismatch_counter += 1 |
||||
|
||||
return CS |
||||
|
||||
def update_alerts(self, CS): |
||||
clear_event_types = set() |
||||
if ET.WARNING not in self.state_machine.current_alert_types: |
||||
clear_event_types.add(ET.WARNING) |
||||
if self.enabled: |
||||
clear_event_types.add(ET.NO_ENTRY) |
||||
|
||||
pers = LONGITUDINAL_PERSONALITY_MAP[self.personality] |
||||
alerts = self.events.create_alerts(self.state_machine.current_alert_types, [self.CP, CS, self.sm, self.is_metric, |
||||
self.state_machine.soft_disable_timer, pers]) |
||||
self.AM.add_many(self.sm.frame, alerts) |
||||
self.AM.process_alerts(self.sm.frame, clear_event_types) |
||||
|
||||
def publish_selfdriveState(self, CS): |
||||
# selfdriveState |
||||
ss_msg = messaging.new_message('selfdriveState') |
||||
ss_msg.valid = True |
||||
ss = ss_msg.selfdriveState |
||||
ss.enabled = self.enabled |
||||
ss.active = self.active |
||||
ss.state = self.state_machine.state |
||||
ss.engageable = not self.events.contains(ET.NO_ENTRY) |
||||
ss.experimentalMode = self.experimental_mode |
||||
ss.personality = self.personality |
||||
|
||||
ss.alertText1 = self.AM.current_alert.alert_text_1 |
||||
ss.alertText2 = self.AM.current_alert.alert_text_2 |
||||
ss.alertSize = self.AM.current_alert.alert_size |
||||
ss.alertStatus = self.AM.current_alert.alert_status |
||||
ss.alertType = self.AM.current_alert.alert_type |
||||
ss.alertSound = self.AM.current_alert.audible_alert |
||||
|
||||
self.pm.send('selfdriveState', ss_msg) |
||||
|
||||
# onroadEvents - logged every second or on change |
||||
if (self.sm.frame % int(1. / DT_CTRL) == 0) or (self.events.names != self.events_prev): |
||||
ce_send = messaging.new_message('onroadEvents', len(self.events)) |
||||
ce_send.valid = True |
||||
ce_send.onroadEvents = self.events.to_msg() |
||||
self.pm.send('onroadEvents', ce_send) |
||||
self.events_prev = self.events.names.copy() |
||||
|
||||
def step(self): |
||||
CS = self.data_sample() |
||||
self.update_events(CS) |
||||
if not self.CP.passive and self.initialized: |
||||
self.enabled, self.active = self.state_machine.update(self.events) |
||||
self.update_alerts(CS) |
||||
|
||||
self.publish_selfdriveState(CS) |
||||
|
||||
self.CS_prev = CS |
||||
|
||||
def read_personality_param(self): |
||||
try: |
||||
return int(self.params.get('LongitudinalPersonality')) |
||||
except (ValueError, TypeError): |
||||
return log.LongitudinalPersonality.standard |
||||
|
||||
def params_thread(self, evt): |
||||
while not evt.is_set(): |
||||
self.is_metric = self.params.get_bool("IsMetric") |
||||
self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl |
||||
self.personality = self.read_personality_param() |
||||
time.sleep(0.1) |
||||
|
||||
def run(self): |
||||
e = threading.Event() |
||||
t = threading.Thread(target=self.params_thread, args=(e, )) |
||||
try: |
||||
t.start() |
||||
while True: |
||||
self.step() |
||||
self.rk.monitor_time() |
||||
finally: |
||||
e.set() |
||||
t.join() |
||||
|
||||
|
||||
def main(): |
||||
config_realtime_process(4, Priority.CTRL_HIGH) |
||||
s = SelfdriveD() |
||||
s.run() |
||||
|
||||
if __name__ == "__main__": |
||||
main() |
||||
@ -0,0 +1,98 @@ |
||||
from cereal import log |
||||
from openpilot.selfdrive.selfdrived.events import Events, ET |
||||
from openpilot.common.realtime import DT_CTRL |
||||
|
||||
State = log.SelfdriveState.OpenpilotState |
||||
|
||||
SOFT_DISABLE_TIME = 3 # seconds |
||||
ACTIVE_STATES = (State.enabled, State.softDisabling, State.overriding) |
||||
ENABLED_STATES = (State.preEnabled, *ACTIVE_STATES) |
||||
|
||||
class StateMachine: |
||||
def __init__(self): |
||||
self.current_alert_types = [ET.PERMANENT] |
||||
self.state = State.disabled |
||||
self.soft_disable_timer = 0 |
||||
|
||||
def update(self, events: Events): |
||||
# decrement the soft disable timer at every step, as it's reset on |
||||
# entrance in SOFT_DISABLING state |
||||
self.soft_disable_timer = max(0, self.soft_disable_timer - 1) |
||||
|
||||
self.current_alert_types = [ET.PERMANENT] |
||||
|
||||
# ENABLED, SOFT DISABLING, PRE ENABLING, OVERRIDING |
||||
if self.state != State.disabled: |
||||
# user and immediate disable always have priority in a non-disabled state |
||||
if events.contains(ET.USER_DISABLE): |
||||
self.state = State.disabled |
||||
self.current_alert_types.append(ET.USER_DISABLE) |
||||
|
||||
elif events.contains(ET.IMMEDIATE_DISABLE): |
||||
self.state = State.disabled |
||||
self.current_alert_types.append(ET.IMMEDIATE_DISABLE) |
||||
|
||||
else: |
||||
# ENABLED |
||||
if self.state == State.enabled: |
||||
if events.contains(ET.SOFT_DISABLE): |
||||
self.state = State.softDisabling |
||||
self.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL) |
||||
self.current_alert_types.append(ET.SOFT_DISABLE) |
||||
|
||||
elif events.contains(ET.OVERRIDE_LATERAL) or events.contains(ET.OVERRIDE_LONGITUDINAL): |
||||
self.state = State.overriding |
||||
self.current_alert_types += [ET.OVERRIDE_LATERAL, ET.OVERRIDE_LONGITUDINAL] |
||||
|
||||
# SOFT DISABLING |
||||
elif self.state == State.softDisabling: |
||||
if not events.contains(ET.SOFT_DISABLE): |
||||
# no more soft disabling condition, so go back to ENABLED |
||||
self.state = State.enabled |
||||
|
||||
elif self.soft_disable_timer > 0: |
||||
self.current_alert_types.append(ET.SOFT_DISABLE) |
||||
|
||||
elif self.soft_disable_timer <= 0: |
||||
self.state = State.disabled |
||||
|
||||
# PRE ENABLING |
||||
elif self.state == State.preEnabled: |
||||
if not events.contains(ET.PRE_ENABLE): |
||||
self.state = State.enabled |
||||
else: |
||||
self.current_alert_types.append(ET.PRE_ENABLE) |
||||
|
||||
# OVERRIDING |
||||
elif self.state == State.overriding: |
||||
if events.contains(ET.SOFT_DISABLE): |
||||
self.state = State.softDisabling |
||||
self.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL) |
||||
self.current_alert_types.append(ET.SOFT_DISABLE) |
||||
elif not (events.contains(ET.OVERRIDE_LATERAL) or events.contains(ET.OVERRIDE_LONGITUDINAL)): |
||||
self.state = State.enabled |
||||
else: |
||||
self.current_alert_types += [ET.OVERRIDE_LATERAL, ET.OVERRIDE_LONGITUDINAL] |
||||
|
||||
# DISABLED |
||||
elif self.state == State.disabled: |
||||
if events.contains(ET.ENABLE): |
||||
if events.contains(ET.NO_ENTRY): |
||||
self.current_alert_types.append(ET.NO_ENTRY) |
||||
|
||||
else: |
||||
if events.contains(ET.PRE_ENABLE): |
||||
self.state = State.preEnabled |
||||
elif events.contains(ET.OVERRIDE_LATERAL) or events.contains(ET.OVERRIDE_LONGITUDINAL): |
||||
self.state = State.overriding |
||||
else: |
||||
self.state = State.enabled |
||||
self.current_alert_types.append(ET.ENABLE) |
||||
|
||||
# Check if openpilot is engaged and actuators are enabled |
||||
enabled = self.state in ENABLED_STATES |
||||
active = self.state in ACTIVE_STATES |
||||
if active: |
||||
self.current_alert_types.append(ET.WARNING) |
||||
return enabled, active |
||||
|
||||
@ -0,0 +1,92 @@ |
||||
from cereal import log |
||||
from openpilot.common.realtime import DT_CTRL |
||||
from openpilot.selfdrive.selfdrived.state import StateMachine, SOFT_DISABLE_TIME |
||||
from openpilot.selfdrive.selfdrived.events import Events, ET, EVENTS, NormalPermanentAlert |
||||
|
||||
State = log.SelfdriveState.OpenpilotState |
||||
|
||||
# The event types that maintain the current state |
||||
MAINTAIN_STATES = {State.enabled: (None,), State.disabled: (None,), State.softDisabling: (ET.SOFT_DISABLE,), |
||||
State.preEnabled: (ET.PRE_ENABLE,), State.overriding: (ET.OVERRIDE_LATERAL, ET.OVERRIDE_LONGITUDINAL)} |
||||
ALL_STATES = tuple(State.schema.enumerants.values()) |
||||
# The event types checked in DISABLED section of state machine |
||||
ENABLE_EVENT_TYPES = (ET.ENABLE, ET.PRE_ENABLE, ET.OVERRIDE_LATERAL, ET.OVERRIDE_LONGITUDINAL) |
||||
|
||||
|
||||
def make_event(event_types): |
||||
event = {} |
||||
for ev in event_types: |
||||
event[ev] = NormalPermanentAlert("alert") |
||||
EVENTS[0] = event |
||||
return 0 |
||||
|
||||
|
||||
class TestStateMachine: |
||||
def setup_method(self): |
||||
self.events = Events() |
||||
self.state_machine = StateMachine() |
||||
self.state_machine.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL) |
||||
|
||||
def test_immediate_disable(self): |
||||
for state in ALL_STATES: |
||||
for et in MAINTAIN_STATES[state]: |
||||
self.events.add(make_event([et, ET.IMMEDIATE_DISABLE])) |
||||
self.state_machine.state = state |
||||
self.state_machine.update(self.events) |
||||
assert State.disabled == self.state_machine.state |
||||
self.events.clear() |
||||
|
||||
def test_user_disable(self): |
||||
for state in ALL_STATES: |
||||
for et in MAINTAIN_STATES[state]: |
||||
self.events.add(make_event([et, ET.USER_DISABLE])) |
||||
self.state_machine.state = state |
||||
self.state_machine.update(self.events) |
||||
assert State.disabled == self.state_machine.state |
||||
self.events.clear() |
||||
|
||||
def test_soft_disable(self): |
||||
for state in ALL_STATES: |
||||
if state == State.preEnabled: # preEnabled considers NO_ENTRY instead |
||||
continue |
||||
for et in MAINTAIN_STATES[state]: |
||||
self.events.add(make_event([et, ET.SOFT_DISABLE])) |
||||
self.state_machine.state = state |
||||
self.state_machine.update(self.events) |
||||
assert self.state_machine.state == State.disabled if state == State.disabled else State.softDisabling |
||||
self.events.clear() |
||||
|
||||
def test_soft_disable_timer(self): |
||||
self.state_machine.state = State.enabled |
||||
self.events.add(make_event([ET.SOFT_DISABLE])) |
||||
self.state_machine.update(self.events) |
||||
for _ in range(int(SOFT_DISABLE_TIME / DT_CTRL)): |
||||
assert self.state_machine.state == State.softDisabling |
||||
self.state_machine.update(self.events) |
||||
|
||||
assert self.state_machine.state == State.disabled |
||||
|
||||
def test_no_entry(self): |
||||
# Make sure noEntry keeps us disabled |
||||
for et in ENABLE_EVENT_TYPES: |
||||
self.events.add(make_event([ET.NO_ENTRY, et])) |
||||
self.state_machine.update(self.events) |
||||
assert self.state_machine.state == State.disabled |
||||
self.events.clear() |
||||
|
||||
def test_no_entry_pre_enable(self): |
||||
# preEnabled with noEntry event |
||||
self.state_machine.state = State.preEnabled |
||||
self.events.add(make_event([ET.NO_ENTRY, ET.PRE_ENABLE])) |
||||
self.state_machine.update(self.events) |
||||
assert self.state_machine.state == State.preEnabled |
||||
|
||||
def test_maintain_states(self): |
||||
# Given current state's event type, we should maintain state |
||||
for state in ALL_STATES: |
||||
for et in MAINTAIN_STATES[state]: |
||||
self.state_machine.state = state |
||||
self.events.add(make_event([et])) |
||||
self.state_machine.update(self.events) |
||||
assert self.state_machine.state == state |
||||
self.events.clear() |
||||
@ -1 +1 @@ |
||||
8726813f978d6baf519055f3105350cd071741f3 |
||||
c4d60dfe4b677f9230eebb47614501ea8d0b99a3 |
||||
|
||||
@ -1 +1 @@ |
||||
c2022c388da6eb2e26bb23ad6009be9d5314c0be |
||||
bfbdd4706abcf5757790526d99d0000644017b1e |
||||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in new issue