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? |
# 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 |
* existing openpilot support for similar cars |
||||||
* architecture and APIs available in the car |
* architecture and APIs available in the car |
||||||
|
|
||||||
|
|
||||||
# Structure of a car port |
# 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 |
* `interface.py`: Interface for the car, defines the CarInterface class |
||||||
* `carstate.py`: Reads CAN from car and builds openpilot CarState message |
* `carstate.py`: Reads CAN messages from the car and builds openpilot CarState messages |
||||||
* `carcontroller.py`: Builds CAN messages to send to car |
* `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 |
* `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: |
[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 |
fa69be01-b430-4504-9d72-7dcb058eb6dd |
||||||
26cac7a9757a27c783a365403040a1bd27ccdaea |
d9fb22d1c4fa3ca3d201dbc8edf1d0f0918e53e6 |
||||||
|
|||||||
@ -1,3 +1,3 @@ |
|||||||
version https://git-lfs.github.com/spec/v1 |
version https://git-lfs.github.com/spec/v1 |
||||||
oid sha256:3dd3982940d823c4fbb0429b733a0b78b0688d7d67aa76ff7b754a3e2f3d8683 |
oid sha256:50efe6451a3fb3fa04b6bb0e846544533329bd46ecefe9e657e91214dee2aaeb |
||||||
size 16132780 |
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 |
version https://git-lfs.github.com/spec/v1 |
||||||
oid sha256:39786068cae1ed8c0dc34ef80c281dfcc67ed18a50e06b90765c49bcfdbf7db4 |
oid sha256:2431f40b8ca9926629e461e06316f9bbba984c821ebbc11e6449ca0c96c42d95 |
||||||
size 51453312 |
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