mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-06-28 01:52:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

openpilot v0.3.7 release

Browse Source 
old-commit-hash: daf54ad54d
This commit is contained in:
Vehicle Researcher
2017-09-30 03:07:27 -07:00
parent 7a6d152ae0
commit 2c0cc6103a
130 changed files with 3557 additions and 1717 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.travis.yml
+2 -2
View File
@@ -8,5 +8,5 @@ install:
script:
- docker run --rm
-v "$(pwd)"/selfdrive/test/plant/out:/tmp/openpilot/selfdrive/test/plant/out
tmppilot /bin/sh -c 'cd /tmp/openpilot/selfdrive/test/plant && ./runtest.sh'
-v "$(pwd)"/selfdrive/test/tests/plant/out:/tmp/openpilot/selfdrive/test/tests/plant/out
tmppilot /bin/sh -c 'cd /tmp/openpilot/selfdrive/test/tests/plant && OPTEST=1 ./test_longitudinal.py'
Dockerfile.openpilot
+9 -3
View File
@@ -1,8 +1,7 @@
FROM ubuntu:16.04
ENV PYTHONUNBUFFERED 1
RUN apt-get update && apt-get install -y build-essential clang vim screen wget bzip2 git libglib2.0-0 python-pip capnproto libcapnp-dev libzmq5-dev libffi-dev
RUN apt-get update && apt-get install -y build-essential clang vim screen wget bzip2 git libglib2.0-0 python-pip capnproto libcapnp-dev libzmq5-dev libffi-dev libusb-1.0-0
RUN pip install numpy==1.11.2 scipy==0.18.1 matplotlib
COPY requirements_openpilot.txt /tmp/
@@ -10,4 +9,11 @@ RUN pip install -r /tmp/requirements_openpilot.txt
ENV PYTHONPATH /tmp/openpilot:$PYTHONPATH
COPY . /tmp/openpilot
COPY ./common /tmp/openpilot/common
COPY ./cereal /tmp/openpilot/cereal
COPY ./opendbc /tmp/openpilot/opendbc
COPY ./selfdrive /tmp/openpilot/selfdrive
COPY ./phonelibs /tmp/openpilot/phonelibs
COPY ./pyextra /tmp/openpilot/pyextra
RUN mkdir /tmp/openpilot/selfdrive/test/out
README.md
+9 -1
View File
@@ -22,12 +22,20 @@ Supported Cars
- Acura ILX 2016 with AcuraWatch Plus
- Due to use of the cruise control for gas, it can only be enabled above 25 mph
- Honda Civic 2016 with Honda Sensing
- Honda Civic 2016-2017 with Honda Sensing
- Due to limitations in steering firmware, steering is disabled below 12 mph
- Note that the hatchback model is not supported
- Honda CR-V Touring 2015-2016 (very alpha!)
- Can only be enabled above 25 mph
In Progress Cars
------
- Chevy Volt 2016-2018 Premier with Driver Confidence II
- All 2017 Toyota Prius, Corolla, and RAV4
Directory structure
------
RELEASES.md
+15 -3
View File
@@ -1,6 +1,18 @@
Version 0.3.6.1 (2017-08-15)
============================
* Mitigate low speed steering oscillations on some vehicles
Version 0.3.7 (2017-09-30)
==========================
* Improved lateral control using model predictive control
* Improved lane centering
* Improved GPS
* Reduced tendency of path deviation near right side exits
* Enable engagement while the accelerator pedal is pressed
* Enable engagement while the brake pedal is pressed, when stationary and with lead vehicle within 5m
* Disable engagement when park brake or brake hold are active
* Fixed sporadic longitudinal pulsing in Civic
* Cleanups to vehicle interface
Version 0.3.6.1 (2017-08-15)
============================
* Mitigate low speed steering oscillations on some vehicles
* Include board steering check for CR-V
Version 0.3.6 (2017-08-08)
cereal/Makefile
+44 -3
View File
@@ -1,4 +1,45 @@
-include build_from_src.mk
SRCS := log.capnp car.capnp
GENS := gen/cpp/car.capnp.c++ gen/cpp/log.capnp.c++
UNAME_M ?= $(shell uname -m)
# only generate C++ for docker tests
ifneq ($(OPTEST),1)
GENS += gen/c/car.capnp.c gen/c/log.capnp.c gen/c/c++.capnp.h gen/c/java.capnp.h
# Dont build java on the phone...
ifeq ($(UNAME_M),x86_64)
GENS += gen/java/Car.java gen/java/Log.java
endif
endif
.PHONY: all
all: $(GENS)
.PHONY: clean
clean:
rm -rf gen
gen/c/%.capnp.c: %.capnp
@echo "[ CAPNPC C ] $@"
mkdir -p gen/c/
capnpc '$<' -o c:gen/c/
gen/cpp/%.capnp.c++: %.capnp
@echo "[ CAPNPC C++ ] $@"
mkdir -p gen/cpp/
capnpc '$<' -o c++:gen/cpp/
gen/java/Car.java gen/java/Log.java: $(SRCS)
@echo "[ CAPNPC java ] $@"
mkdir -p gen/java/
capnpc $^ -o java:gen/java
# c-capnproto needs some empty headers
gen/c/c++.capnp.h gen/c/java.capnp.h:
mkdir -p gen/c/
touch '$@'
release:
@echo "cereal: this is a release"
cereal/build_from_src.mk
-45
View File
@@ -1,45 +0,0 @@
SRCS := log.capnp car.capnp
GENS := gen/cpp/car.capnp.c++ gen/cpp/log.capnp.c++
UNAME_M ?= $(shell uname -m)
# only generate C++ for docker tests
ifneq ($(OPTEST),1)
GENS += gen/c/car.capnp.c gen/c/log.capnp.c gen/c/c++.capnp.h gen/c/java.capnp.h
# Dont build java on the phone...
ifeq ($(UNAME_M),x86_64)
GENS += gen/java/Car.java gen/java/Log.java
endif
endif
.PHONY: all
all: $(GENS)
.PHONY: clean
clean:
rm -rf gen
gen/c/%.capnp.c: %.capnp
@echo "[ CAPNPC C ] $@"
mkdir -p gen/c/
capnpc '$<' -o c:gen/c/
gen/cpp/%.capnp.c++: %.capnp
@echo "[ CAPNPC C++ ] $@"
mkdir -p gen/cpp/
capnpc '$<' -o c++:gen/cpp/
gen/java/Car.java gen/java/Log.java: $(SRCS)
@echo "[ CAPNPC java ] $@"
mkdir -p gen/java/
capnpc $^ -o java:gen/java
# c-capnproto needs some empty headers
gen/c/c++.capnp.h gen/c/java.capnp.h:
mkdir -p gen/c/
touch '$@'
cereal/car.capnp
+129 -56
View File
@@ -7,11 +7,59 @@ $Java.outerClassname("Car");
@0x8e2af1e708af8b8d;
# ******* events causing controls state machine transition *******
struct CarEvent @0x9b1657f34caf3ad3 {
name @0 :EventName;
enable @1 :Bool;
preEnable @7 :Bool;
noEntry @2 :Bool;
warning @3 :Bool;
userDisable @4 :Bool;
softDisable @5 :Bool;
immediateDisable @6 :Bool;
enum EventName @0xbaa8c5d505f727de {
# TODO: copy from error list
commIssue @0;
steerUnavailable @1;
brakeUnavailable @2;
gasUnavailable @3;
wrongGear @4;
doorOpen @5;
seatbeltNotLatched @6;
espDisabled @7;
wrongCarMode @8;
steerTempUnavailable @9;
reverseGear @10;
buttonCancel @11;
buttonEnable @12;
pedalPressed @13;
cruiseDisabled @14;
radarCommIssue @15;
dataNeeded @16;
speedTooLow @17;
outOfSpace @18;
overheat @19;
calibrationInProgress @20;
calibrationInvalid @21;
controlsMismatch @22;
pcmEnable @23;
pcmDisable @24;
noTarget @25;
radarFault @26;
modelCommIssue @27;
brakeHold @28;
parkBrake @29;
}
}
# ******* main car state @ 100hz *******
# all speeds in m/s
struct CarState {
errors @0: List(Error);
errorsDEPRECATED @0 :List(CarEvent.EventName);
events @13 :List(CarEvent);
# car speed
vEgo @1 :Float32; # best estimate of speed
@@ -33,6 +81,9 @@ struct CarState {
# cruise state
cruiseState @10 :CruiseState;
# gear
gearShifter @14 :GearShifter;
# button presses
buttonEvents @11 :List(ButtonEvent);
@@ -48,31 +99,28 @@ struct CarState {
}
struct CruiseState {
enabled @0: Bool;
speed @1: Float32;
available @2: Bool;
enabled @0 :Bool;
speed @1 :Float32;
available @2 :Bool;
speedOffset @3 :Float32;
}
enum Error {
# TODO: copy from error list
commIssue @0;
steerUnavailable @1;
brakeUnavailable @2;
gasUnavailable @3;
wrongGear @4;
doorOpen @5;
seatbeltNotLatched @6;
espDisabled @7;
wrongCarMode @8;
steerTempUnavailable @9;
reverseGear @10;
# ...
enum GearShifter {
unknown @0;
park @1;
drive @2;
neutral @3;
reverse @4;
sport @5;
low @6;
brake @7;
}
# send on change
struct ButtonEvent {
pressed @0: Bool;
type @1: Type;
pressed @0 :Bool;
type @1 :Type;
enum Type {
unknown @0;
@@ -91,29 +139,30 @@ struct CarState {
# ******* radar state @ 20hz *******
struct RadarState {
errors @0: List(Error);
points @1: List(RadarPoint);
errors @0 :List(Error);
points @1 :List(RadarPoint);
# which packets this state came from
canMonoTimes @2: List(UInt64);
canMonoTimes @2 :List(UInt64);
enum Error {
notValid @0;
commIssue @0;
fault @1;
}
# similar to LiveTracks
# is one timestamp valid for all? I think so
struct RadarPoint {
trackId @0: UInt64; # no trackId reuse
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
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
aRel @4 :Float32; # m/s^2
yvRel @5 :Float32; # m/s
}
}
@@ -121,17 +170,24 @@ struct RadarState {
struct CarControl {
# must be true for any actuator commands to work
enabled @0: Bool;
enabled @0 :Bool;
# range from 0.0 - 1.0
gas @1: Float32;
brake @2: Float32;
gasDEPRECATED @1 :Float32;
brakeDEPRECATED @2 :Float32;
steeringTorqueDEPRECATED @3 :Float32;
# range from -1.0 - 1.0
steeringTorque @3 :Float32;
actuators @6 :Actuators;
cruiseControl @4: CruiseControl;
hudControl @5: HUDControl;
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;
}
struct CruiseControl {
cancel @0: Bool;
@@ -178,31 +234,48 @@ struct CarControl {
# ****** car param ******
struct CarParams {
carName @0: Text;
radarName @1: Text;
carFingerprint @2: Text;
carName @0 :Text;
radarName @1 :Text;
carFingerprint @2 :Text;
enableSteer @3: Bool;
enableGas @4: Bool;
enableBrake @5: Bool;
enableCruise @6: Bool;
enableSteer @3 :Bool;
enableGas @4 :Bool;
enableBrake @5 :Bool;
enableCruise @6 :Bool;
minEnableSpeed @18 :Float32;
safetyModel @19 :Int16;
steerMaxBP @20 :List(Float32);
steerMaxV @21 :List(Float32);
gasMaxBP @22 :List(Float32);
gasMaxV @23 :List(Float32);
brakeMaxBP @24 :List(Float32);
brakeMaxV @25 :List(Float32);
enum SafetyModels {
# from board
default @0;
honda @1;
toyota @2;
}
# things about the car in the manual
m @7: Float32; # [kg] running weight
l @8: Float32; # [m] wheelbase
sR @9: Float32; # [] steering ratio
aF @10: Float32; # [m] GC distance to front axle
aR @11: Float32; # [m] GC distance to rear axle
chi @12: Float32; # [] rear steering ratio wrt front steering (usually 0)
m @7 :Float32; # [kg] running weight
l @8 :Float32; # [m] wheelbase
sR @9 :Float32; # [] steering ratio
aF @10 :Float32; # [m] GC distance to front axle
aR @11 :Float32; # [m] GC distance to rear axle
chi @12 :Float32; # [] rear steering ratio wrt front steering (usually 0)
# things we can derive
j @13: Float32; # [kg*m2] body rot inertia
cF @14: Float32; # [N/rad] front tire coeff of stiff
cR @15: Float32; # [N/rad] rear tire coeff of stiff
j @13 :Float32; # [kg*m2] body rotational inertia
cF @14 :Float32; # [N/rad] front tire coeff of stiff
cR @15 :Float32; # [N/rad] rear tire coeff of stiff
# Kp and Ki for the lateral control
steerKp @16: Float32;
steerKi @17: Float32;
steerKp @16 :Float32;
steerKi @17 :Float32;
# TODO: Kp and Ki for long control, perhaps not needed?
}
cereal/log.capnp
+127 -60
View File
@@ -92,10 +92,10 @@ struct InitData {
}
struct PandaInfo {
hasPanda @0: Bool;
dongleId @1: Text;
stVersion @2: Text;
espVersion @3: Text;
hasPanda @0 :Bool;
dongleId @1 :Text;
stVersion @2 :Text;
espVersion @3 :Text;
}
}
@@ -110,9 +110,10 @@ struct FrameData {
frameType @7 :FrameType;
timestampSof @8 :UInt64;
transform @10 :List(Float32);
androidCaptureResult @9 :AndroidCaptureResult;
enum FrameType {
unknown @0;
neo @1;
@@ -196,10 +197,10 @@ struct GpsLocationData {
timestamp @7 :Int64;
source @8 :SensorSource;
# Represents NED velocity in m/s.
vNED @9 :List(Float32);
# Represents expected vertical accuracy in meters. (presumably 1 sigma?)
verticalAccuracy @10 :Float32;
@@ -239,7 +240,9 @@ struct ThermalData {
# not thermal
freeSpace @7 :Float32;
batteryPercent @8 :Int16;
batteryStatus @9: Text;
batteryStatus @9 :Text;
fanSpeed @10 :UInt16;
}
struct HealthData {
@@ -254,8 +257,8 @@ struct HealthData {
struct LiveUI {
rearViewCam @0 :Bool;
alertText1 @1 :Text;
alertText2 @2 :Text;
alertText1 @1 :Text;
alertText2 @2 :Text;
awarenessStatus @3 :Float32;
}
@@ -264,6 +267,7 @@ struct Live20Data {
mdMonoTime @6 :UInt64;
ftMonoTimeDEPRECATED @7 :UInt64;
l100MonoTime @11 :UInt64;
radarErrors @12 :List(Car.RadarState.Error);
# all deprecated
warpMatrixDEPRECATED @0 :List(Float32);
@@ -294,6 +298,7 @@ struct Live20Data {
struct LiveCalibrationData {
warpMatrix @0 :List(Float32);
warpMatrix2 @5 :List(Float32);
calStatus @1 :Int8;
calCycle @2 :Int32;
calPerc @3 :Int8;
@@ -340,14 +345,14 @@ struct Live100Data {
hudLeadDEPRECATED @14 :Int32;
cumLagMs @15 :Float32;
enabled @19: Bool;
steerOverride @20: Bool;
enabled @19 :Bool;
steerOverride @20 :Bool;
vCruise @22: Float32;
vCruise @22 :Float32;
rearViewCam @23 :Bool;
alertText1 @24 :Text;
alertText2 @25 :Text;
alertText1 @24 :Text;
alertText2 @25 :Text;
awarenessStatus @26 :Float32;
angleOffset @27 :Float32;
@@ -387,6 +392,7 @@ struct ModelData {
bigBoxHeight @3 :UInt16;
boxProjection @4 :List(Float32);
yuvCorrection @5 :List(Float32);
inputTransform @6 :List(Float32);
}
}
@@ -399,7 +405,7 @@ struct CalibrationFeatures {
}
struct EncodeIndex {
# picture from camera
# picture from camera
frameId @0 :UInt32;
type @1 :Type;
# index of encoder from start of route
@@ -438,69 +444,79 @@ struct LogRotate {
struct Plan {
mdMonoTime @9 :UInt64;
l20MonoTime @10 :UInt64;
events @13 :List(Car.CarEvent);
# lateral, 3rd order polynomial
lateralValid @0: Bool;
lateralValid @0 :Bool;
dPoly @1 :List(Float32);
laneWidth @11 :Float32;
# longitudinal
longitudinalValid @2: Bool;
longitudinalValid @2 :Bool;
vTarget @3 :Float32;
aTargetMin @4 :Float32;
aTargetMax @5 :Float32;
jerkFactor @6 :Float32;
hasLead @7 :Bool;
fcw @8 :Bool;
# gps trajectory in car frame
gpsTrajectory @12 :GpsTrajectory;
struct GpsTrajectory {
x @0 :List(Float32);
y @1 :List(Float32);
}
}
struct LiveLocationData {
status @0: UInt8;
status @0 :UInt8;
# 3D fix
lat @1: Float64;
lon @2: Float64;
alt @3: Float32; # m
# 3D fix
lat @1 :Float64;
lon @2 :Float64;
alt @3 :Float32; # m
# speed
speed @4: Float32; # m/s
speed @4 :Float32; # m/s
# NED velocity components
vNED @5: List(Float32);
vNED @5 :List(Float32);
# roll, pitch, heading (x,y,z)
roll @6: Float32; # WRT to center of earth?
pitch @7: Float32; # WRT to center of earth?
heading @8: Float32; # WRT to north?
roll @6 :Float32; # WRT to center of earth?
pitch @7 :Float32; # WRT to center of earth?
heading @8 :Float32; # WRT to north?
# what are these?
wanderAngle @9: Float32;
trackAngle @10: Float32;
wanderAngle @9 :Float32;
trackAngle @10 :Float32;
# car frame -- https://upload.wikimedia.org/wikipedia/commons/f/f5/RPY_angles_of_cars.png
# gyro, in car frame, deg/s
gyro @11: List(Float32);
gyro @11 :List(Float32);
# accel, in car frame, m/s^2
accel @12: List(Float32);
accel @12 :List(Float32);
accuracy @13: Accuracy;
accuracy @13 :Accuracy;
struct Accuracy {
pNEDError @0: List(Float32);
vNEDError @1: List(Float32);
rollError @2: Float32;
pitchError @3: Float32;
headingError @4: Float32;
ellipsoidSemiMajorError @5: Float32;
ellipsoidSemiMinorError @6: Float32;
ellipsoidOrientationError @7: Float32;
pNEDError @0 :List(Float32);
vNEDError @1 :List(Float32);
rollError @2 :Float32;
pitchError @3 :Float32;
headingError @4 :Float32;
ellipsoidSemiMajorError @5 :Float32;
ellipsoidSemiMinorError @6 :Float32;
ellipsoidOrientationError @7 :Float32;
}
}
struct EthernetPacket {
pkt @0 :Data;
ts @1: Float32;
ts @1 :Float32;
}
struct NavUpdate {
@@ -602,7 +618,7 @@ struct AndroidGnss {
hasLeapSecond @4 :Bool;
leapSecond @5 :Int32;
hasFullBiasNanos @6 :Bool;
fullBiasNanos @7 :Int64;
@@ -611,10 +627,10 @@ struct AndroidGnss {
hasBiasUncertaintyNanos @10 :Bool;
biasUncertaintyNanos @11 :Float64;
hasDriftNanosPerSecond @12 :Bool;
driftNanosPerSecond @13 :Float64;
hasDriftUncertaintyNanosPerSecond @14 :Bool;
driftUncertaintyNanosPerSecond @15 :Float64;
}
@@ -633,7 +649,7 @@ struct AndroidGnss {
accumulatedDeltaRangeState @9 :Int32;
accumulatedDeltaRangeMeters @10 :Float64;
accumulatedDeltaRangeUncertaintyMeters @11 :Float64;
hasCarrierFrequencyHz @12 :Bool;
carrierFrequencyHz @13 :Float32;
hasCarrierCycles @14 :Bool;
@@ -705,6 +721,8 @@ struct QcomGnss {
measurementReport @1 :MeasurementReport;
clockReport @2 :ClockReport;
drMeasurementReport @3 :DrMeasurementReport;
drSvPoly @4 :DrSvPolyReport;
rawLog @5 :Data;
}
enum MeasurementSource @0xd71a12b6faada7ee {
@@ -742,18 +760,18 @@ struct QcomGnss {
measurementNotUsable @12 :Bool;
sirCheckIsNeeded @13 :Bool;
probationMode @14 :Bool;
glonassMeanderBitEdgeValid @15 :Bool;
glonassTimeMarkValid @16 :Bool;
gpsRoundRobinRxDiversity @17 :Bool;
gpsRxDiversity @18 :Bool;
gpsLowBandwidthRxDiversityCombined @19 :Bool;
gpsHighBandwidthNu4 @20 :Bool;
gpsHighBandwidthNu8 @21 :Bool;
gpsHighBandwidthUniform @22 :Bool;
gpsMultipathIndicator @23 :Bool;
multipathIndicator @23 :Bool;
imdJammingIndicator @24 :Bool;
lteB13TxJammingIndicator @25 :Bool;
freshMeasurementIndicator @26 :Bool;
@@ -953,6 +971,37 @@ struct QcomGnss {
goodParity @32 :Bool;
}
}
struct DrSvPolyReport {
svId @0 :UInt16;
frequencyIndex @1 :Int8;
hasPosition @2 :Bool;
hasIono @3 :Bool;
hasTropo @4 :Bool;
hasElevation @5 :Bool;
polyFromXtra @6 :Bool;
hasSbasIono @7 :Bool;
iode @8 :UInt16;
t0 @9 :Float64;
xyz0 @10 :List(Float64);
xyzN @11 :List(Float64);
other @12 :List(Float32);
positionUncertainty @13 :Float32;
ionoDelay @14 :Float32;
ionoDot @15 :Float32;
sbasIonoDelay @16 :Float32;
sbasIonoDot @17 :Float32;
tropoDelay @18 :Float32;
elevation @19 :Float32;
elevationDot @20 :Float32;
elevationUncertainty @21 :Float32;
velocityCoeff @22 :List(Float64);
}
}
struct LidarPts {
@@ -1037,12 +1086,12 @@ struct UbloxGnss {
# num of measurements to follow
numMeas @4 :UInt8;
measurements @5 :List(Measurement);
struct ReceiverStatus {
# leap seconds have been determined
# leap seconds have been determined
leapSecValid @0 :Bool;
# Clock reset applied
clkReset @1 : Bool;
clkReset @1 :Bool;
}
struct Measurement {
@@ -1060,7 +1109,7 @@ struct UbloxGnss {
# carrier phase locktime counter in ms
locktime @7 :UInt16;
# Carrier-to-noise density ratio (signal strength) in dBHz
cno @8 : UInt8;
cno @8 :UInt8;
# pseudorange standard deviation in meters
pseudorangeStdev @9 :Float32;
# carrier phase standard deviation in cycles
@@ -1103,31 +1152,47 @@ struct UbloxGnss {
ecc @15 :Float64;
cus @16 :Float64;
a @17 :Float64; # note that this is not the root!!
toe @18 :Float64;
cic @19 :Float64;
omega0 @20 :Float64;
cis @21 :Float64;
i0 @22 :Float64;
crc @23 :Float64;
omega @24 :Float64;
omegaDot @25 :Float64;
iDot @26 :Float64;
codesL2 @27 :Float64;
gpsWeek @28 :Float64;
l2 @29 :Float64;
svAcc @30 :Float64;
svHealth @31 :Float64;
tgd @32 :Float64;
iodc @33 :Float64;
transmissionTime @34 :Float64;
fitInterval @35 :Float64;
}
}
struct Clocks {
bootTimeNanos @0 :UInt64;
monotonicNanos @1 :UInt64;
monotonicRawNanos @2 :UInt64;
wallTimeNanos @3 :UInt64;
modemUptimeMillis @4 :UInt64;
}
struct LiveMpcData {
x @0 :List(Float32);
y @1 :List(Float32);
psi @2 :List(Float32);
delta @3 :List(Float32);
}
struct Event {
# in nanoseconds?
logMonoTime @0 :UInt64;
@@ -1144,7 +1209,7 @@ struct Event {
model @9 :ModelData;
features @10 :CalibrationFeatures;
sensorEvents @11 :List(SensorEventData);
health @12 : HealthData;
health @12 :HealthData;
live20 @13 :Live20Data;
liveUIDEPRECATED @14 :LiveUI;
encodeIdx @15 :EncodeIndex;
@@ -1167,5 +1232,7 @@ struct Event {
lidarPts @32 :LidarPts;
procLog @33 :ProcLog;
ubloxGnss @34 :UbloxGnss;
clocks @35 :Clocks;
liveMpc @36 :LiveMpcData;
}
}
common/basedir.py
+4
View File
@@ -0,0 +1,4 @@
import os
BASEDIR = os.path.join(os.path.dirname(os.path.realpath(__file__)), "../")
common/crash.py
+1 -1
View File
@@ -2,7 +2,7 @@
import os
import sys
if os.getenv("NOLOG"):
if os.getenv("NOLOG") or os.getenv("NOCRASH"):
def capture_exception(*exc_info):
pass
def bind_user(**kwargs):
common/fingerprints.py
+3
View File
@@ -18,6 +18,9 @@ _FINGERPRINTS = {
57L: 3, 145L: 8, 316L: 8, 340L: 8, 342L: 6, 344L: 8, 380L: 8, 398L: 3, 399L: 6, 401L: 8, 420L: 8, 422L: 8, 426L: 8, 432L: 7, 464L: 8, 474L: 5, 476L: 4, 487L: 4, 490L: 8, 493L: 3, 507L: 1, 542L: 7, 545L: 4, 597L: 8, 660L: 8, 661L: 4, 773L: 7, 777L: 8, 800L: 8, 804L: 8, 808L: 8, 882L: 2, 884L: 7, 888L: 8, 891L: 8, 892L: 8, 923L: 2, 929L: 8, 983L: 8, 985L: 3, 1024L: 5, 1027L: 5, 1029L: 8, 1033L: 5, 1036L: 8, 1039L: 8, 1057L: 5, 1064L: 7, 1108L: 8, 1125L: 8, 1296L: 8, 1365L: 5, 1424L: 5, 1600L: 5, 1601L: 8,
# sent messages
0x194: 4, 0x1fa: 8, 0x30c: 8, 0x33d: 5,
},
"TOYOTA PRIUS 2017": {
896L: 8, 832L: 8, 898L: 8, 899L: 8, 577L: 8, 528L: 8, 529L: 8, 530L: 8, 531L: 8, 532L: 8, 533L: 8, 534L: 8, 535L: 8, 536L: 8, 537L: 8, 538L: 8, 539L: 8, 540L: 8, 541L: 8, 542L: 8, 543L: 8, 544L: 8, 545L: 8, 546L: 8, 547L: 8, 548L: 8, 549L: 8, 550L: 8, 551L: 8, 296L: 6, 553L: 6, 554L: 6, 555L: 6, 556L: 6, 557L: 6, 558L: 6, 559L: 6, 560L: 7, 561L: 8, 562L: 8, 883L: 8, 837L: 8, 833L: 8, 576L: 8, 321L: 4, 834L: 8, 835L: 8, 580L: 8, 581L: 8, 897L: 8, 584L: 8, 1136L: 4, 976L: 8, 977L: 8, 978L: 8, 291L: 7, 881L: 8, 352L: 8, 353L: 7, 867L: 8, 868L: 8, 869L: 8, 1126L: 3, 304L: 7, 880L: 8, 552L: 6, 882L: 8, 979L: 8, 884L: 8, 885L: 8, 836L: 8
}
}
common/params.py
+42 -9
View File
@@ -66,6 +66,13 @@ keys = {
# read: radard
"CarParams": TxType.CLEAR_ON_CAR_START}
def fsync_dir(path):
fd = os.open(path, os.O_RDONLY)
try:
os.fsync(fd)
finally:
os.close(fd)
class FileLock(object):
def __init__(self, path, create):
@@ -182,15 +189,32 @@ class DBWriter(DBAccessor):
self._check_entered()
try:
# data_path refers to the externally used path to the params. It is a symlink.
# old_data_path is the path currently pointed to by data_path.
# tempdir_path is a path where the new params will go, which the new data path will point to.
# new_data_path is a temporary symlink that will atomically overwrite data_path.
#
# The current situation is:
# data_path -> old_data_path
# We're going to write params data to tempdir_path
# tempdir_path -> params data
# Then point new_data_path to tempdir_path
# new_data_path -> tempdir_path
# Then atomically overwrite data_path with new_data_path
# data_path -> tempdir_path
old_data_path = None
new_data_path = None
tempdir_path = tempfile.mkdtemp(prefix=".tmp", dir=self._path)
try:
# Write back all keys.
os.chmod(tempdir_path, 0o777)
for k, v in self._vals.items():
with open(os.path.join(tempdir_path, k), "wb") as f:
f.write(v)
f.flush()
os.fsync(f.fileno())
fsync_dir(tempdir_path)
data_path = self._data_path()
try:
@@ -203,16 +227,21 @@ class DBWriter(DBAccessor):
new_data_path = "{}.link".format(tempdir_path)
os.symlink(os.path.basename(tempdir_path), new_data_path)
os.rename(new_data_path, data_path)
# TODO(mgraczyk): raise useful error when values are bad.
except:
shutil.rmtree(tempdir_path)
if new_data_path is not None:
os.remove(new_data_path)
raise
fsync_dir(self._path)
finally:
# If the rename worked, we can delete the old data. Otherwise delete the new one.
success = new_data_path is not None and os.path.exists(data_path) and (
os.readlink(data_path) == os.path.basename(tempdir_path))
# Keep holding the lock while we clean up the old data.
if old_data_path is not None:
shutil.rmtree(old_data_path)
if success:
if old_data_path is not None:
shutil.rmtree(old_data_path)
else:
shutil.rmtree(tempdir_path)
# Regardless of what happened above, there should be no link at new_data_path.
if new_data_path is not None and os.path.islink(new_data_path):
os.remove(new_data_path)
finally:
os.umask(self._prev_umask)
self._prev_umask = None
@@ -249,6 +278,10 @@ class Params(object):
def car_start(self):
self._clear_keys_with_type(TxType.CLEAR_ON_CAR_START)
def delete(self, key):
with self.env.begin(write=True) as txn:
txn.delete(key)
def get(self, key, block=False):
if key not in keys:
raise UnknownKeyName(key)
common/profiler.py
+6
View File
@@ -14,6 +14,12 @@ class Profiler(object):
self.cp.append((name, tt - self.last_time))
self.last_time = tt
def reset(self, enabled=False):
self.enabled = enabled
self.cp = []
self.start_time = sec_since_boot()
self.last_time = self.start_time
def display(self):
if not self.enabled:
return
opendbc
+1 -1
Submodule opendbc updated: 008089045e...063032ff2b
panda
+1 -1
Submodule panda updated: 4901d52104...92a1c773e7
phonelibs/qpoases/EXAMPLES/example1.cpp
+3
View File
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:018b8429d7eafb7dbc55145bf29b6ee033fab367764d5994c0d2d3876d1a771e
size 1949
phonelibs/qpoases/EXAMPLES/example1b.cpp
+3
View File
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:c5a9064ef08e632ea9f9aa31c7f4d6013756b57200c5791d8fc8bb1b5d1583be
size 1776
phonelibs/qpoases/INCLUDE/Bounds.hpp
+3
View File
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:2d77876b6d9bbb9968f95cc7908d2e7a7706b71cc1d79d44c9f269b7e5078278
size 5743
phonelibs/qpoases/INCLUDE/Constants.hpp
+3
View File
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:5b515339eee590e3af6ae78679e58338fe5e164d51550ba93d3de2b8bb0f2e09
size 3501
phonelibs/qpoases/INCLUDE/Constraints.hpp
+3
View File
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:5a394fb4f49deb8598ee72920e1b03a682c457b42765b8aac0f8a2ac43bdf118
size 5864
phonelibs/qpoases/INCLUDE/CyclingManager.hpp
+3
View File
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:325c722b58ad39b7860137ea1ecfb1bd48fc44251717d16574dcd3ad28e11089
size 3861
phonelibs/qpoases/INCLUDE/EXTRAS/SolutionAnalysis.hpp
+3
View File
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:60dc80b14e5a7fac2257648a004987d0706d4d32602e3657d2c97d24363406bb
size 3435
phonelibs/qpoases/INCLUDE/Indexlist.hpp
+3
View File
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:53cf87460a46afce93acc7d5d76011366a387a93d67401bec25350e05bb783a5
size 4928
phonelibs/qpoases/INCLUDE/MessageHandling.hpp
+3
View File
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:67c24d5d24e53580b5af982cfa36b115e88314b1306e2cea6b3c1e9ea746a369
size 20367
phonelibs/qpoases/INCLUDE/QProblem.hpp
+3
View File
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:01213e5cb12bf937d65ffd05c7a9ba70a7ab4e043753449edda98fd08f733930
size 33063
phonelibs/qpoases/INCLUDE/QProblemB.hpp
+3
View File
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:3c9f053c8eed7d0b8aa72dec81ecdb7cb59bc87dee0108398bc34b79a76c7bd3
size 27333
phonelibs/qpoases/INCLUDE/SubjectTo.hpp
+3
View File
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:fdbee52661ba442d31f697828ff283505bf48e78f7740aac59f39aa62a18cae2
size 5572
phonelibs/qpoases/INCLUDE/Types.hpp
+3
View File
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:ff13c48ec416a3c30f7582d38e4ca43b30e8867b28bcebdab3a6cced6e141258
size 4467
phonelibs/qpoases/INCLUDE/Utils.hpp
+3
View File
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:2f37d5507c5288bce904e776045b68702240d2d51896262f645084cba7e971cc
size 6713
phonelibs/qpoases/LICENSE.txt
+3
View File
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:319daefe38ebeead3df178e5937898a52d67c0c795f54f1c0bb10ac3b9cffb63
size 26940
phonelibs/qpoases/README.txt
+3
View File
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:ae75304482e680a0c6bc6f81eafcf29c3a77b407a6344181eb6f67cd3b2fbf69
size 3177
phonelibs/qpoases/SRC/Bounds.cpp
+3
View File
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:dedf2112735d4beae4682878dc0a97cc2bcda648459d917e0049f4997654ac52
size 5901
phonelibs/qpoases/SRC/Bounds.ipp
+3
View File
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:099752cb994e787b7d6c9a5dbde908eb19d4d3a4e0bec63e36c3907704732e57
size 2545
phonelibs/qpoases/SRC/Constraints.cpp
+3
View File
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:3028ce5088b5690e774267d4c1f6de8c330b3ecf0add682fc2350d87cee68030
size 6102
phonelibs/qpoases/SRC/Constraints.ipp
+3
View File
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:f13088527d6bdd946bf66581edb71d93ea7659261cd797fbd2cc2a3ed836a903
size 2641
phonelibs/qpoases/SRC/CyclingManager.cpp
+3
View File
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:67bab79e60c64e87a7977957cd55726912837629115d6246cd089dbb9aae4fc7
size 3949
phonelibs/qpoases/SRC/CyclingManager.ipp
+3
View File
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:85df62984743813f8f9e92ff299b74c790d9e174906a5966b916ebb21f068165
size 1643
phonelibs/qpoases/SRC/EXTRAS/SolutionAnalysis.cpp
+3
View File
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:1cc39dede19249ae695a72203ee643e65b29a0738316b1b2e77cc5f9d72821fe
size 12434
phonelibs/qpoases/SRC/Indexlist.cpp
+3
View File
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:2d40beb3c8639d8c9bb3d529d98a5c51d7b1a9239febda97ef136a2d27a785bf
size 6642
phonelibs/qpoases/SRC/Indexlist.ipp
+3
View File
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:9bf145bff7575704135bd3f2933a9af5bf37cace1ad56c7ed887128f2bfc459f
size 2163
phonelibs/qpoases/SRC/MessageHandling.cpp
+3
View File
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:2b2de17f7e28f36f108afc05ea257b82fb112ad7afb0aef965e857484e951f06
size 19449
phonelibs/qpoases/SRC/MessageHandling.ipp
+3
View File
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:247726b5943a872b367d5eb948078428f570e3684f9c0e541a13890729020ecb
size 3215
phonelibs/qpoases/SRC/QProblem.cpp
+3
View File
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:e50488623df439ee0539898819839afd7c064be68709c32eea19db552377b81a
size 96202
phonelibs/qpoases/SRC/QProblem.ipp
+3
View File
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:279519a38bb964e574752c75a9be6ce6e2720477183397a85cd505859329654d
size 5667
phonelibs/qpoases/SRC/QProblemB.cpp
+3
View File
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:a4dc609ba60648cf2307536b2c5088fe082caff7785d31a5686ef6b8bee6db7a
size 51089
phonelibs/qpoases/SRC/QProblemB.ipp
+3
View File
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:789884d3683b59533bc5e84ee44e616c3a48b414285266f0155846f7ee73cad5
size 7273
phonelibs/qpoases/SRC/SubjectTo.cpp
+3
View File
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:6e850a791144172ed5bfec0fca3b32abd0347885f0e739ae3aa306f9fb4889bb
size 4244
phonelibs/qpoases/SRC/SubjectTo.ipp
+3
View File
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:6872db4cea8f779994636a27c100724b2a7ca7ff1451aa2bffcc768798c9bb80
size 2847
phonelibs/qpoases/SRC/Utils.cpp
+3
View File
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:695b8e4e30ab4ec7e522446463716fbbb1be92c840117830fa1bd389d969ddfa
size 10228
phonelibs/qpoases/SRC/Utils.ipp
+3
View File
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:abdbf8757354226ba1723eaa4d8ace186471c6a193812dca3a9a06f4b30b9fce
size 1315
phonelibs/qpoases/VERSIONS.txt
+3
View File
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:f02a5ff04907df394d7b905216cfcece2a7a54cdbed25a4d892bb08f2f760d77
size 3061
pyextra
+1 -1
Submodule pyextra updated: e0738376db...4eda4dd765
run_docker_tests.sh
+3 -2
View File
@@ -2,6 +2,7 @@
set -e
docker build -t tmppilot -f Dockerfile.openpilot .
docker run --rm \
-v "$(pwd)"/selfdrive/test/plant/out:/tmp/openpilot/selfdrive/test/plant/out \
tmppilot /bin/sh -c 'cd /tmp/openpilot/selfdrive/test/plant && ./runtest.sh'
-v "$(pwd)"/selfdrive/test/tests/plant/out:/tmp/openpilot/selfdrive/test/tests/plant/out \
tmppilot /bin/sh -c 'cd /tmp/openpilot/selfdrive/test/tests/plant && OPTEST=1 ./test_longitudinal.py'
selfdrive/boardd/Makefile
+7 -2
View File
@@ -4,6 +4,7 @@ CXX = clang++
ARCH := $(shell uname -m)
OS := $(shell uname -o)
BASEDIR = ../..
PHONELIBS = ../../phonelibs
WARN_FLAGS = -Werror=implicit-function-declaration \
@@ -12,8 +13,8 @@ WARN_FLAGS = -Werror=implicit-function-declaration \
-Werror=return-type \
-Werror=format-extra-args
CFLAGS = -std=gnu11 -g -fPIC -O2 $(WARN_FLAGS)
CXXFLAGS = -std=c++11 -g -fPIC -O2 $(WARN_FLAGS)
CFLAGS = -std=gnu11 -g -fPIC -I../../ -O2 $(WARN_FLAGS)
CXXFLAGS = -std=c++11 -g -fPIC -I../../ -O2 $(WARN_FLAGS)
ZMQ_FLAGS = -I$(PHONELIBS)/zmq/aarch64/include
ZMQ_LIBS = -L$(PHONELIBS)/zmq/aarch64/lib \
@@ -37,6 +38,8 @@ ZMQ_LIBS = -L$(HOME)/one/external/zmq/lib/ \
CEREAL_LIBS = -L$(HOME)/one/external/capnp/lib/ \
-l:libcapnp.a -l:libcapnp_c.a -l:libkj.a
EXTRA_LIBS = -lusb-1.0 -lpthread
CXXFLAGS += -I/usr/include/libusb-1.0
CFLAGS += -I/usr/include/libusb-1.0
endif
.PHONY: all
@@ -46,6 +49,8 @@ include ../common/cereal.mk
OBJS = boardd.o \
../common/swaglog.o \
../common/params.o \
../common/util.o \
$(PHONELIBS)/json/src/json.o \
$(CEREAL_OBJS)
selfdrive/boardd/boardd.cc
+105 -27
View File
@@ -19,7 +19,9 @@
#include <capnp/serialize.h>
#include "cereal/gen/cpp/log.capnp.h"
#include "cereal/gen/cpp/car.capnp.h"
#include "common/params.h"
#include "common/swaglog.h"
#include "common/timing.h"
@@ -38,20 +40,55 @@ bool loopback_can = false;
#define TIMEOUT 0
#define SAFETY_NOOUTPUT 0x0000
#define SAFETY_HONDA 0x0001
#define SAFETY_ALLOUTPUT 0x1337
pthread_t safety_setter_thread_handle = -1;
void *safety_setter_thread(void *s) {
char *value;
size_t value_sz = 0;
LOGW("waiting for params to set safety model");
while (1) {
const int result = read_db_value(NULL, "CarParams", &value, &value_sz);
if (value_sz > 0) break;
usleep(100*1000);
}
LOGW("got %d bytes CarParams", value_sz);
// format for board, make copy due to alignment issues, will be freed on out of scope
auto amsg = kj::heapArray<capnp::word>((value_sz / sizeof(capnp::word)) + 1);
memcpy(amsg.begin(), value, value_sz);
capnp::FlatArrayMessageReader cmsg(amsg);
cereal::CarParams::Reader car_params = cmsg.getRoot<cereal::CarParams>();
int safety_model = car_params.getSafetyModel();
LOGW("setting safety model: %d", safety_model);
pthread_mutex_lock(&usb_lock);
// set in the mutex to avoid race
safety_setter_thread_handle = -1;
libusb_control_transfer(dev_handle, 0x40, 0xdc, safety_model, 0, NULL, 0, TIMEOUT);
pthread_mutex_unlock(&usb_lock);
return NULL;
}
// must be called before threads or with mutex
bool usb_connect() {
int err;
dev_handle = libusb_open_device_with_vid_pid(ctx, 0xbbaa, 0xddcc);
if (dev_handle == NULL) { return false; }
if (dev_handle == NULL) { goto fail; }
err = libusb_set_configuration(dev_handle, 1);
if (err != 0) { return false; }
if (err != 0) { goto fail; }
err = libusb_claim_interface(dev_handle, 0);
if (err != 0) { return false; }
if (err != 0) { goto fail; }
if (loopback_can) {
libusb_control_transfer(dev_handle, 0xc0, 0xe5, 1, 0, NULL, 0, TIMEOUT);
@@ -60,30 +97,23 @@ bool usb_connect() {
// power off ESP
libusb_control_transfer(dev_handle, 0xc0, 0xd9, 0, 0, NULL, 0, TIMEOUT);
// forward CAN1 to CAN3...soon
//libusb_control_transfer(dev_handle, 0xc0, 0xdd, 1, 2, NULL, 0, TIMEOUT);
// power on charging (may trigger a reconnection, should be okay)
#ifndef __x86_64__
libusb_control_transfer(dev_handle, 0xc0, 0xe6, 1, 0, NULL, 0, TIMEOUT);
#else
LOGW("not enabling charging on x86_64");
#endif
// set UART modes for Honda Accord
/*for (int uart = 2; uart <= 3; uart++) {
// 9600 baud
libusb_control_transfer(dev_handle, 0x40, 0xe1, uart, 9600, NULL, 0, TIMEOUT);
// even parity
libusb_control_transfer(dev_handle, 0x40, 0xe2, uart, 1, NULL, 0, TIMEOUT);
// callback 1
libusb_control_transfer(dev_handle, 0x40, 0xe3, uart, 1, NULL, 0, TIMEOUT);
// no output is the default
libusb_control_transfer(dev_handle, 0x40, 0xdc, SAFETY_NOOUTPUT, 0, NULL, 0, TIMEOUT);
if (safety_setter_thread_handle == -1) {
err = pthread_create(&safety_setter_thread_handle, NULL, safety_setter_thread, NULL);
}
// TODO: Boardd should be able to set the baud rate
int baud = 500000;
libusb_control_transfer(dev_handle, 0x40, 0xde, 0, 0,
(unsigned char *)&baud, sizeof(baud), TIMEOUT); // CAN1
libusb_control_transfer(dev_handle, 0x40, 0xde, 1, 0,
(unsigned char *)&baud, sizeof(baud), TIMEOUT); // CAN2*/
// TODO: Boardd should be able to be told which safety model to use
libusb_control_transfer(dev_handle, 0x40, 0xdc, SAFETY_HONDA, 0, NULL, 0, TIMEOUT);
return true;
fail:
return false;
}
void usb_retry_connect() {
@@ -93,7 +123,7 @@ void usb_retry_connect() {
}
void handle_usb_issue(int err, const char func[]) {
LOGE("usb error %d \"%s\" in %s", err, libusb_strerror((enum libusb_error)err), func);
LOGE_100("usb error %d \"%s\" in %s", err, libusb_strerror((enum libusb_error)err), func);
if (err == -4) {
LOGE("lost connection");
usb_retry_connect();
@@ -113,7 +143,7 @@ void can_recv(void *s) {
do {
err = libusb_bulk_transfer(dev_handle, 0x81, (uint8_t*)data, RECV_SIZE, &recv, TIMEOUT);
if (err != 0) { handle_usb_issue(err, __func__); }
if (err == -8) { LOGE("overflow got 0x%x", recv); };
if (err == -8) { LOGE_100("overflow got 0x%x", recv); };
// timeout is okay to exit, recv still happened
if (err == -7) { break; }
@@ -261,6 +291,46 @@ void can_send(void *s) {
// **** threads ****
void *thermal_thread(void *crap) {
int err;
LOGD("start thermal thread");
// thermal = 8005
void *context = zmq_ctx_new();
void *subscriber = zmq_socket(context, ZMQ_SUB);
zmq_setsockopt(subscriber, ZMQ_SUBSCRIBE, "", 0);
zmq_connect(subscriber, "tcp://127.0.0.1:8005");
// run as fast as messages come in
while (!do_exit) {
// recv from thermal
zmq_msg_t msg;
zmq_msg_init(&msg);
err = zmq_msg_recv(&msg, subscriber, 0);
assert(err >= 0);
// format for board, make copy due to alignment issues, will be freed on out of scope
// copied from send thread...
auto amsg = kj::heapArray<capnp::word>((zmq_msg_size(&msg) / sizeof(capnp::word)) + 1);
memcpy(amsg.begin(), zmq_msg_data(&msg), zmq_msg_size(&msg));
capnp::FlatArrayMessageReader cmsg(amsg);
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
uint16_t target_fan_speed = event.getThermal().getFanSpeed();
//LOGW("setting fan speed %d", target_fan_speed);
pthread_mutex_lock(&usb_lock);
libusb_control_transfer(dev_handle, 0xc0, 0xd3, target_fan_speed, 0, NULL, 0, TIMEOUT);
pthread_mutex_unlock(&usb_lock);
}
// turn the fan off when we exit
libusb_control_transfer(dev_handle, 0xc0, 0xd3, 0, 0, NULL, 0, TIMEOUT);
return NULL;
}
void *can_send_thread(void *crap) {
LOGD("start send thread");
@@ -364,8 +434,16 @@ int main() {
can_recv_thread, NULL);
assert(err == 0);
pthread_t thermal_thread_handle;
err = pthread_create(&thermal_thread_handle, NULL,
thermal_thread, NULL);
assert(err == 0);
// join threads
err = pthread_join(thermal_thread_handle, NULL);
assert(err == 0);
err = pthread_join(can_recv_thread_handle, NULL);
assert(err == 0);
selfdrive/boardd/boardd.py
+4 -1
View File
@@ -20,6 +20,8 @@ except Exception:
SAFETY_NOOUTPUT = 0
SAFETY_HONDA = 1
SAFETY_TOYOTA = 2
SAFETY_TOYOTA_NOLIMITS = 0x1336
SAFETY_ALLOUTPUT = 0x1337
# *** serialization functions ***
@@ -33,7 +35,7 @@ def can_list_to_can_capnp(can_msgs, msgtype='can'):
cc = dat.can[i]
cc.address = can_msg[0]
cc.busTime = can_msg[1]
cc.dat = can_msg[2]
cc.dat = str(can_msg[2])
cc.src = can_msg[3]
return dat
@@ -111,6 +113,7 @@ def can_init():
def boardd_mock_loop():
context = zmq.Context()
can_init()
handle.controlWrite(0x40, 0xdc, SAFETY_ALLOUTPUT, 0, b'')
logcan = messaging.sub_sock(context, service_list['can'].port)
selfdrive/can/Makefile
+3 -2
View File
@@ -1,6 +1,7 @@
CC = clang
CXX = clang++
BASEDIR = ../..
PHONELIBS := ../../phonelibs
UNAME_S := $(shell uname -s)
@@ -53,7 +54,7 @@ libdbc.so:: ../../cereal/gen/cpp/log.capnp.h
../../cereal/gen/cpp/log.capnp.h:
cd ../../cereal && make
libdbc.so:: parser.cc $(DBC_CCS)
libdbc.so:: dbc.cc parser.cc packer.cc $(DBC_CCS)
$(CXX) -fPIC -shared -o '$@' $^ \
-I. \
-I../.. \
@@ -63,7 +64,7 @@ libdbc.so:: parser.cc $(DBC_CCS)
$(CEREAL_CXXFLAGS) \
$(CEREAL_LIBS)
dbc_out/%.cc: $(OPENDBC_PATH)/%.dbc
dbc_out/%.cc: $(OPENDBC_PATH)/%.dbc process_dbc.py dbc_template.cc
PYTHONPATH=$(PYTHONPATH):$(CWD)/../../pyextra ./process_dbc.py '$<' '$@'
.PHONY: clean
selfdrive/can/parser_common.h → selfdrive/can/common.h
+11 -2
View File
@@ -1,13 +1,20 @@
#ifndef PARSER_COMMON_H
#define PARSER_COMMON_H
#ifndef SELFDRIVE_CAN_COMMON_H
#define SELFDRIVE_CAN_COMMON_H
#include <cstddef>
#include <cstdint>
#include <string>
#define ARRAYSIZE(x) (sizeof(x)/sizeof(x[0]))
struct SignalPackValue {
const char* name;
double value;
};
struct SignalParseOptions {
uint32_t address;
const char* name;
@@ -54,6 +61,8 @@ struct DBC {
const Msg *msgs;
};
const DBC* dbc_lookup(const std::string& dbc_name);
void dbc_register(const DBC* dbc);
#define dbc_init(dbc) \
selfdrive/can/dbc.cc
+26
View File
@@ -0,0 +1,26 @@
#include <string>
#include <vector>
#include "common.h"
namespace {
std::vector<const DBC*>& get_dbcs() {
static std::vector<const DBC*> vec;
return vec;
}
}
const DBC* dbc_lookup(const std::string& dbc_name) {
for (const auto& dbci : get_dbcs()) {
if (dbc_name == dbci->name) {
return dbci;
}
}
return NULL;
}
void dbc_register(const DBC* dbc) {
get_dbcs().push_back(dbc);
}
selfdrive/can/dbc_template.cc
+1 -1
View File
@@ -1,6 +1,6 @@
#include <cstdint>
#include "parser_common.h"
#include "common.h"
namespace {
selfdrive/can/libdbc_py.py
+51
View File
@@ -0,0 +1,51 @@
import os
import subprocess
from cffi import FFI
can_dir = os.path.dirname(os.path.abspath(__file__))
libdbc_fn = os.path.join(can_dir, "libdbc.so")
subprocess.check_output(["make"], cwd=can_dir)
ffi = FFI()
ffi.cdef("""
typedef struct SignalParseOptions {
uint32_t address;
const char* name;
double default_value;
} SignalParseOptions;
typedef struct MessageParseOptions {
uint32_t address;
int check_frequency;
} MessageParseOptions;
typedef struct SignalValue {
uint32_t address;
uint16_t ts;
const char* name;
double value;
} SignalValue;
void* can_init(int bus, const char* dbc_name,
size_t num_message_options, const MessageParseOptions* message_options,
size_t num_signal_options, const SignalParseOptions* signal_options);
void can_update(void* can, uint64_t sec, bool wait);
size_t can_query(void* can, uint64_t sec, bool *out_can_valid, size_t out_values_size, SignalValue* out_values);
typedef struct SignalPackValue {
const char* name;
double value;
} SignalPackValue;
void* canpack_init(const char* dbc_name);
uint64_t canpack_pack(void* inst, uint32_t address, size_t num_vals, const SignalPackValue *vals);
""")
libdbc = ffi.dlopen(libdbc_fn)
selfdrive/can/packer.cc
+81
View File
@@ -0,0 +1,81 @@
#include <cassert>
#include <string>
#include <vector>
#include <utility>
#include <algorithm>
#include <map>
#include "common.h"
#define WARN printf
namespace {
class CANPacker {
public:
CANPacker(const std::string& dbc_name) {
dbc = dbc_lookup(dbc_name);
assert(dbc);
for (int i=0; i<dbc->num_msgs; i++) {
const Msg* msg = &dbc->msgs[i];
for (int j=0; j<msg->num_sigs; j++) {
const Signal* sig = &msg->sigs[j];
signal_lookup[std::make_pair(msg->address, std::string(sig->name))] = *sig;
}
}
}
uint64_t pack(uint32_t address, const std::vector<SignalPackValue> &signals) {
uint64_t ret = 0;
for (const auto& sigval : signals) {
std::string name = std::string(sigval.name);
double value = sigval.value;
auto sig_it = signal_lookup.find(make_pair(address, name));
if (sig_it == signal_lookup.end()) {
WARN("undefined signal %s", name.c_str());
continue;
}
auto sig = sig_it->second;
int64_t ival = (int64_t)((value - sig.offset) / sig.factor);
if (ival < 0) {
WARN("signed pack unsupported right now");
continue;
}
uint64_t mask = ((1ULL << sig.b2)-1) << sig.bo;
uint64_t dat = (ival & ((1ULL << sig.b2)-1)) << sig.bo;
ret &= ~mask;
ret |= dat;
}
return ret;
}
private:
const DBC *dbc = NULL;
std::map<std::pair<uint32_t, std::string>, Signal> signal_lookup;
};
}
extern "C" {
void* canpack_init(const char* dbc_name) {
CANPacker *ret = new CANPacker(std::string(dbc_name));
return (void*)ret;
}
uint64_t canpack_pack(void* inst, uint32_t address, size_t num_vals, const SignalPackValue *vals) {
CANPacker *cp = (CANPacker*)inst;
return cp->pack(address, std::vector<SignalPackValue>(vals, vals+num_vals));
}
}
selfdrive/can/packer.py
+37
View File
@@ -0,0 +1,37 @@
import struct
from selfdrive.can.libdbc_py import libdbc, ffi
class CANPacker(object):
def __init__(self, dbc_name):
self.packer = libdbc.canpack_init(dbc_name)
self.sig_names = {}
def pack(self, addr, values):
# values: [(signal_name, signal_value)]
values_thing = []
for name, value in values:
if name not in self.sig_names:
self.sig_names[name] = ffi.new("char[]", name)
values_thing.append({
'name': self.sig_names[name],
'value': value
})
values_c = ffi.new("SignalPackValue[]", values_thing)
return libdbc.canpack_pack(self.packer, addr, len(values_thing), values_c)
def pack_bytes(self, addr, values):
return struct.pack(">Q", self.pack(addr, values))
if __name__ == "__main__":
cp = CANPacker("honda_civic_touring_2016_can")
s = cp.pack_bytes(0x30c, [
("PCM_SPEED", 123),
("PCM_GAS", 10),
])
print s.encode("hex")
selfdrive/can/parser.cc
+11 -21
View File
@@ -18,7 +18,7 @@
#include <capnp/serialize.h>
#include "cereal/gen/cpp/log.capnp.h"
#include "parser_common.h"
#include "common.h"
#define DEBUG(...)
// #define DEBUG printf
@@ -40,8 +40,6 @@ uint64_t read_u64_be(const uint8_t* v) {
| (uint64_t)v[7]);
}
std::vector<const DBC*> g_dbc;
bool honda_checksum(int address, uint64_t d, int l) {
int target = (d >> l) & 0xF;
@@ -77,9 +75,9 @@ struct MessageState {
for (int i=0; i < parse_sigs.size(); i++) {
auto& sig = parse_sigs[i];
int64_t tmp = (dat >> sig.bo) & ((1 << sig.b2)-1);
int64_t tmp = (dat >> sig.bo) & ((1ULL << sig.b2)-1);
if (sig.is_signed) {
tmp -= (tmp >> (sig.b2-1)) ? (1<<sig.b2) : 0; //signed
tmp -= (tmp >> (sig.b2-1)) ? (1ULL << sig.b2) : 0; //signed
}
DEBUG("parse %X %s -> %ld\n", address, sig.name, tmp);
@@ -123,6 +121,7 @@ struct MessageState {
};
class CANParser {
public:
CANParser(int abus, const std::string& dbc_name,
@@ -135,15 +134,10 @@ class CANParser {
zmq_setsockopt(subscriber, ZMQ_SUBSCRIBE, "", 0);
zmq_connect(subscriber, "tcp://127.0.0.1:8006");
for (auto dbci : g_dbc) {
if (dbci->name == dbc_name) {
dbc = dbci;
break;
}
}
dbc = dbc_lookup(dbc_name);
assert(dbc);
for (auto &op : options) {
for (const auto& op : options) {
MessageState state = {
.address = op.address,
// .check_frequency = op.check_frequency,
@@ -178,7 +172,7 @@ class CANParser {
}
// track requested signals for this message
for (auto &sigop : sigoptions) {
for (const auto& sigop : sigoptions) {
if (sigop.address != op.address) continue;
for (int i=0; i<msg->num_sigs; i++) {
@@ -230,8 +224,8 @@ class CANParser {
void UpdateValid(uint64_t sec) {
can_valid = true;
for (auto &kv : message_states) {
auto &state = kv.second;
for (const auto& kv : message_states) {
const auto& state = kv.second;
if (state.check_threshold > 0 && (sec - state.seen) > state.check_threshold) {
if (state.seen > 0) {
INFO("%X TIMEOUT\n", state.address);
@@ -279,8 +273,8 @@ class CANParser {
std::vector<SignalValue> query(uint64_t sec) {
std::vector<SignalValue> ret;
for (auto &kv : message_states) {
auto &state = kv.second;
for (const auto& kv : message_states) {
const auto& state = kv.second;
if (sec != 0 && state.seen != sec) continue;
for (int i=0; i<state.parse_sigs.size(); i++) {
@@ -311,10 +305,6 @@ class CANParser {
}
void dbc_register(const DBC* dbc) {
g_dbc.push_back(dbc);
}
extern "C" {
void* can_init(int bus, const char* dbc_name,
selfdrive/can/parser.py
+1 -39
View File
@@ -1,46 +1,8 @@
import os
import time
import subprocess
from collections import defaultdict
from cffi import FFI
can_dir = os.path.dirname(os.path.abspath(__file__))
libdbc_fn = os.path.join(can_dir, "libdbc.so")
subprocess.check_output(["make"], cwd=can_dir)
ffi = FFI()
ffi.cdef("""
typedef struct SignalParseOptions {
uint32_t address;
const char* name;
double default_value;
} SignalParseOptions;
typedef struct MessageParseOptions {
uint32_t address;
int check_frequency;
} MessageParseOptions;
typedef struct SignalValue {
uint32_t address;
uint16_t ts;
const char* name;
double value;
} SignalValue;
void* can_init(int bus, const char* dbc_name,
size_t num_message_options, const MessageParseOptions* message_options,
size_t num_signal_options, const SignalParseOptions* signal_options);
void can_update(void* can, uint64_t sec, bool wait);
size_t can_query(void* can, uint64_t sec, bool *out_can_valid, size_t out_values_size, SignalValue* out_values);
""")
libdbc = ffi.dlopen(libdbc_fn)
from selfdrive.can.libdbc_py import libdbc, ffi
class CANParser(object):
def __init__(self, dbc_name, signals, checks=[], bus=0):
selfdrive/car/__init__.py
+6
View File
@@ -2,6 +2,11 @@ from common.fingerprints import fingerprint
from .honda.interface import CarInterface as HondaInterface
try:
from .toyota.interface import CarInterface as ToyotaInterface
except ImportError:
ToyotaInterface = None
try:
from .simulator.interface import CarInterface as SimInterface
except ImportError:
@@ -17,6 +22,7 @@ interfaces = {
"ACURA ILX 2016 ACURAWATCH PLUS": HondaInterface,
"HONDA ACCORD 2016 TOURING": HondaInterface,
"HONDA CR-V 2016 TOURING": HondaInterface,
"TOYOTA PRIUS 2017": ToyotaInterface,
"simulator": SimInterface,
"simulator2": Sim2Interface
selfdrive/car/honda/carcontroller.py
+26 -60
View File
@@ -1,5 +1,5 @@
from collections import namedtuple
import os
import common.numpy_fast as np
from common.numpy_fast import clip, interp
from common.realtime import sec_since_boot
@@ -10,25 +10,25 @@ from selfdrive.controls.lib.drive_helpers import rate_limit
from . import hondacan
def actuator_hystereses(final_brake, braking, brake_steady, v_ego, civic):
def actuator_hystereses(brake, braking, brake_steady, v_ego, civic):
# hyst params... TODO: move these to VehicleParams
brake_hyst_on = 0.055 if civic else 0.1 # to activate brakes exceed this value
brake_hyst_off = 0.005 # to deactivate brakes below this value
brake_hyst_gap = 0.01 # don't change brake command for small ocilalitons within this value
#*** histeresys logic to avoid brake blinking. go above 0.1 to trigger
if (final_brake < brake_hyst_on and not braking) or final_brake < brake_hyst_off:
final_brake = 0.
braking = final_brake > 0.
if (brake < brake_hyst_on and not braking) or brake < brake_hyst_off:
brake = 0.
braking = brake > 0.
# for small brake oscillations within brake_hyst_gap, don't change the brake command
if final_brake == 0.:
if brake == 0.:
brake_steady = 0.
elif final_brake > brake_steady + brake_hyst_gap:
brake_steady = final_brake - brake_hyst_gap
elif final_brake < brake_steady - brake_hyst_gap:
brake_steady = final_brake + brake_hyst_gap
final_brake = brake_steady
elif brake > brake_steady + brake_hyst_gap:
brake_steady = brake - brake_hyst_gap
elif brake < brake_steady - brake_hyst_gap:
brake_steady = brake + brake_hyst_gap
brake = brake_steady
if not civic:
brake_on_offset_v = [.25, .15] # min brake command on brake activation. below this no decel is perceived
@@ -36,10 +36,10 @@ def actuator_hystereses(final_brake, braking, brake_steady, v_ego, civic):
# offset the brake command for threshold in the brake system. no brake torque perceived below it
brake_on_offset = interp(v_ego, brake_on_offset_bp, brake_on_offset_v)
brake_offset = brake_on_offset - brake_hyst_on
if final_brake > 0.0:
final_brake += brake_offset
if brake > 0.0:
brake += brake_offset
return final_brake, braking, brake_steady
return brake, braking, brake_steady
class AH:
#[alert_idx, value]
@@ -78,12 +78,9 @@ class CarController(object):
def __init__(self):
self.braking = False
self.brake_steady = 0.
self.final_brake_last = 0.
self.brake_last = 0.
# redundant safety check with the board
self.controls_allowed = False
def update(self, sendcan, enabled, CS, frame, final_gas, final_brake, final_steer, \
def update(self, sendcan, enabled, CS, frame, actuators, \
pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \
hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert, \
snd_beep, snd_chime):
@@ -94,20 +91,16 @@ class CarController(object):
return
# *** apply brake hysteresis ***
final_brake, self.braking, self.brake_steady = actuator_hystereses(final_brake, self.braking, self.brake_steady, CS.v_ego, CS.civic)
brake, self.braking, self.brake_steady = actuator_hystereses(actuators.brake, self.braking, self.brake_steady, CS.v_ego, CS.civic)
# *** no output if not enabled ***
if not enabled:
final_gas = 0.
final_brake = 0.
final_steer = 0.
if not enabled and CS.pcm_acc_status:
# send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
if CS.pcm_acc_status:
pcm_cancel_cmd = True
pcm_cancel_cmd = True
# *** rate limit after the enable check ***
final_brake = rate_limit(final_brake, self.final_brake_last, -2., 1./100)
self.final_brake_last = final_brake
brake = rate_limit(brake, self.brake_last, -2., 1./100)
self.brake_last = brake
# vehicle hud display, wait for one update from 10Hz 0x304 msg
#TODO: use enum!!
@@ -143,7 +136,8 @@ class CarController(object):
GAS_MAX = 1004
BRAKE_MAX = 1024/4
if CS.civic:
STEER_MAX = 0x1000
is_fw_modified = os.getenv("DONGLE_ID") in ['b0f5a01cf604185c']
STEER_MAX = 0x1FFF if is_fw_modified else 0x1000
elif CS.crv:
STEER_MAX = 0x300 # CR-V only uses 12-bits and requires a lower value
else:
@@ -151,50 +145,22 @@ class CarController(object):
GAS_OFFSET = 328
# steer torque is converted back to CAN reference (positive when steering right)
apply_gas = int(clip(final_gas*GAS_MAX, 0, GAS_MAX-1))
apply_brake = int(clip(final_brake*BRAKE_MAX, 0, BRAKE_MAX-1))
apply_steer = int(clip(-final_steer*STEER_MAX, -STEER_MAX, STEER_MAX))
apply_gas = int(clip(actuators.gas * GAS_MAX, 0, GAS_MAX - 1))
apply_brake = int(clip(brake * BRAKE_MAX, 0, BRAKE_MAX - 1))
apply_steer = int(clip(-actuators.steer * STEER_MAX, -STEER_MAX, STEER_MAX))
# no gas if you are hitting the brake or the user is
if apply_gas > 0 and (apply_brake != 0 or CS.brake_pressed):
print "CANCELLING GAS", apply_brake
apply_gas = 0
# no computer brake if the gas is being pressed
if CS.car_gas > 0 and apply_brake != 0:
print "CANCELLING BRAKE"
apply_brake = 0
# any other cp.vl[0x18F]['STEER_STATUS'] is common and can happen during user override. sending 0 torque to avoid EPS sending error 5
if CS.steer_not_allowed:
print "STEER ALERT, TORQUE INHIBITED"
apply_steer = 0
# *** entry into controls state ***
if (CS.prev_cruise_buttons == CruiseButtons.DECEL_SET or CS.prev_cruise_buttons == CruiseButtons.RES_ACCEL) and \
CS.cruise_buttons == 0 and not self.controls_allowed:
print "CONTROLS ARE LIVE"
self.controls_allowed = True
# *** exit from controls state on cancel, gas, or brake ***
if (CS.cruise_buttons == CruiseButtons.CANCEL or CS.brake_pressed or
CS.user_gas_pressed or (CS.pedal_gas > 0 and CS.brake_only)) and self.controls_allowed:
print "CONTROLS ARE DEAD"
self.controls_allowed = False
# *** controls fail on steer error, brake error, or invalid can ***
if CS.steer_error:
print "STEER ERROR"
self.controls_allowed = False
if CS.brake_error:
print "BRAKE ERROR"
self.controls_allowed = False
if not CS.can_valid and self.controls_allowed: # 200 ms
print "CAN INVALID"
self.controls_allowed = False
# Send CAN commands.
can_sends = []
selfdrive/car/honda/carstate.py
+103 -40
View File
@@ -1,18 +1,54 @@
import os
import time
from cereal import car
import common.numpy_fast as np
from common.realtime import sec_since_boot
import selfdrive.messaging as messaging
from selfdrive.can.parser import CANParser
from selfdrive.config import Conversions as CV
from selfdrive.car.honda.can_parser import CANParser
from selfdrive.can.parser import CANParser as CANParserC
NEW_CAN = os.getenv("OLD_CAN") is None
def parse_gear_shifter(can_gear_shifter, is_acura):
def get_can_parser(CP):
# this function generates lists for signal, messages and initial values
if can_gear_shifter == 0x1:
return "park"
elif can_gear_shifter == 0x2:
return "reverse"
if is_acura:
if can_gear_shifter == 0x3:
return "neutral"
elif can_gear_shifter == 0x4:
return "drive"
elif can_gear_shifter == 0xa:
return "sport"
else:
if can_gear_shifter == 0x4:
return "neutral"
elif can_gear_shifter == 0x8:
return "drive"
elif can_gear_shifter == 0x10:
return "sport"
elif can_gear_shifter == 0x20:
return "low"
return "unknown"
_K0 = -0.3
_K1 = -0.01879
_K2 = 0.01013
def calc_cruise_offset(offset, speed):
# euristic formula so that speed is controlled to ~ 0.3m/s below pid_speed
# constraints to solve for _K0, _K1, _K2 are:
# - speed = 0m/s, out = -0.3
# - speed = 34m/s, offset = 20, out = -0.25
# - speed = 34m/s, offset = -2.5, out = -1.8
return min(_K0 + _K1 * speed + _K2 * speed * offset, 0.)
def get_can_signals(CP):
# this function generates lists for signal, messages and initial values
if CP.carFingerprint == "HONDA CIVIC 2016 TOURING":
dbc_f = 'honda_civic_touring_2016_can.dbc'
signals = [
@@ -50,8 +86,9 @@ def get_can_parser(CP):
("CRUISE_SETTING", 0x296, 0),
("LEFT_BLINKER", 0x326, 0),
("RIGHT_BLINKER", 0x326, 0),
("COUNTER", 0x324, 0),
("ENGINE_RPM", 0x17C, 0)
("CRUISE_SPEED_OFFSET", 0x37c, 0),
("EPB_STATE", 0x1c2, 0),
("BRAKE_HOLD_ACTIVE", 0x1A4, 0),
]
checks = [
# address, frequency
@@ -65,6 +102,7 @@ def get_can_parser(CP):
(0x1d0, 50),
(0x305, 10),
(0x324, 10),
(0x37c, 10),
(0x405, 3),
]
@@ -104,8 +142,7 @@ def get_can_parser(CP):
("CRUISE_SETTING", 0x1a6, 0),
("LEFT_BLINKER", 0x294, 0),
("RIGHT_BLINKER", 0x294, 0),
("COUNTER", 0x324, 0),
("ENGINE_RPM", 0x17C, 0)
("CRUISE_SPEED_OFFSET", 0x37c, 0)
]
checks = [
(0x156, 100),
@@ -118,6 +155,7 @@ def get_can_parser(CP):
(0x1d0, 50),
(0x305, 10),
(0x324, 10),
(0x37c, 10),
(0x405, 3),
]
elif CP.carFingerprint == "HONDA ACCORD 2016 TOURING":
@@ -157,8 +195,6 @@ def get_can_parser(CP):
("CRUISE_SETTING", 0x1a6, 0),
("LEFT_BLINKER", 0x294, 0),
("RIGHT_BLINKER", 0x294, 0),
("COUNTER", 0x324, 0),
("ENGINE_RPM", 0x17C, 0)
]
checks = [
(0x156, 100),
@@ -209,8 +245,6 @@ def get_can_parser(CP):
("CRUISE_SETTING", 0x1a6, 0),
("LEFT_BLINKER", 0x294, 0),
("RIGHT_BLINKER", 0x294, 0),
("COUNTER", 0x324, 0),
("ENGINE_RPM", 0x17C, 0)
]
checks = [
(0x156, 100),
@@ -230,22 +264,23 @@ def get_can_parser(CP):
signals.append(("INTERCEPTOR_GAS", 0x201, 0))
checks.append((0x201, 50))
if NEW_CAN:
return CANParserC(os.path.splitext(dbc_f)[0], signals, checks, 0)
else:
return CANParser(dbc_f, signals, checks)
return dbc_f, signals, checks
def get_can_parser(CP):
dbc_f, signals, checks = get_can_signals(CP)
return CANParser(os.path.splitext(dbc_f)[0], signals, checks, 0)
class CarState(object):
def __init__(self, CP, logcan):
self.acura = False
self.civic = False
self.accord = False
self.crv = False
if CP.carFingerprint == "HONDA CIVIC 2016 TOURING":
self.civic = True
elif CP.carFingerprint == "ACURA ILX 2016 ACURAWATCH PLUS":
self.civic = False
self.acura = True
elif CP.carFingerprint == "HONDA ACCORD 2016 TOURING":
# fake civic
self.accord = True
elif CP.carFingerprint == "HONDA CR-V 2016 TOURING":
self.crv = True
@@ -274,10 +309,7 @@ class CarState(object):
def update(self, can_pub_main=None):
cp = self.cp
if NEW_CAN:
cp.update(int(sec_since_boot() * 1e9), False)
else:
cp.update_can(can_pub_main)
cp.update(int(sec_since_boot() * 1e9), False)
# copy can_valid
self.can_valid = cp.can_valid
@@ -294,8 +326,6 @@ class CarState(object):
self.prev_left_blinker_on = self.left_blinker_on
self.prev_right_blinker_on = self.right_blinker_on
self.rpm = cp.vl[0x17C]['ENGINE_RPM']
# ******************* parse out can *******************
self.door_all_closed = not any([cp.vl[0x405]['DOOR_OPEN_FL'], cp.vl[0x405]['DOOR_OPEN_FR'],
cp.vl[0x405]['DOOR_OPEN_RL'], cp.vl[0x405]['DOOR_OPEN_RR']])
@@ -316,9 +346,11 @@ class CarState(object):
self.brake_error = cp.vl[0x1B0]['BRAKE_ERROR_1'] or cp.vl[0x1B0]['BRAKE_ERROR_2']
self.esp_disabled = cp.vl[0x1A4]['ESP_DISABLED']
# calc best v_ego estimate, by averaging two opposite corners
self.v_wheel = (
cp.vl[0x1D0]['WHEEL_SPEED_FL'] + cp.vl[0x1D0]['WHEEL_SPEED_FR'] +
cp.vl[0x1D0]['WHEEL_SPEED_RL'] + cp.vl[0x1D0]['WHEEL_SPEED_RR']) / 4.
self.v_wheel_fl = cp.vl[0x1D0]['WHEEL_SPEED_FL']
self.v_wheel_fr = cp.vl[0x1D0]['WHEEL_SPEED_FR']
self.v_wheel_rl = cp.vl[0x1D0]['WHEEL_SPEED_RL']
self.v_wheel_rr = cp.vl[0x1D0]['WHEEL_SPEED_RR']
self.v_wheel = (self.v_wheel_fl + self.v_wheel_fr + self.v_wheel_rl + self.v_wheel_rr) / 4.
# blend in transmission speed at low speed, since it has more low speed accuracy
self.v_weight = np.interp(self.v_wheel, v_weight_bp, v_weight_v)
self.v_ego = (1. - self.v_weight) * cp.vl[0x158]['XMISSION_SPEED'] + self.v_weight * self.v_wheel
@@ -328,49 +360,59 @@ class CarState(object):
self.user_gas_pressed = self.user_gas > 0 # this works because interceptor read < 0 when pedal position is 0. Once calibrated, this will change
#print self.user_gas, self.user_gas_pressed
if self.civic:
self.gear_shifter = cp.vl[0x191]['GEAR_SHIFTER']
can_gear_shifter = cp.vl[0x191]['GEAR_SHIFTER']
self.angle_steers = cp.vl[0x14A]['STEER_ANGLE']
self.gear = 0 # TODO: civic has CVT... needs rev engineering
self.cruise_setting = cp.vl[0x296]['CRUISE_SETTING']
self.cruise_buttons = cp.vl[0x296]['CRUISE_BUTTONS']
self.main_on = cp.vl[0x326]['MAIN_ON']
self.gear_shifter_valid = self.gear_shifter in [1,8] # TODO: 1/P allowed for debug
self.blinker_on = cp.vl[0x326]['LEFT_BLINKER'] or cp.vl[0x326]['RIGHT_BLINKER']
self.left_blinker_on = cp.vl[0x326]['LEFT_BLINKER']
self.right_blinker_on = cp.vl[0x326]['RIGHT_BLINKER']
self.cruise_speed_offset = calc_cruise_offset(cp.vl[0x37c]['CRUISE_SPEED_OFFSET'], self.v_ego)
self.park_brake = cp.vl[0x1c2]['EPB_STATE'] != 0
self.brake_hold = cp.vl[0x1A4]['BRAKE_HOLD_ACTIVE']
elif self.accord:
self.gear_shifter = cp.vl[0x191]['GEAR_SHIFTER']
can_gear_shifter = cp.vl[0x191]['GEAR_SHIFTER']
self.angle_steers = cp.vl[0x156]['STEER_ANGLE']
self.gear = 0 # TODO: accord has CVT... needs rev engineering
self.cruise_setting = cp.vl[0x1A6]['CRUISE_SETTING']
self.cruise_buttons = cp.vl[0x1A6]['CRUISE_BUTTONS']
self.main_on = cp.vl[0x1A6]['MAIN_ON']
self.gear_shifter_valid = self.gear_shifter in [1,8] # TODO: 1/P allowed for debug
self.blinker_on = cp.vl[0x294]['LEFT_BLINKER'] or cp.vl[0x294]['RIGHT_BLINKER']
self.left_blinker_on = cp.vl[0x294]['LEFT_BLINKER']
self.right_blinker_on = cp.vl[0x294]['RIGHT_BLINKER']
self.cruise_speed_offset = -0.3
self.park_brake = 0 # TODO
self.brake_hold = 0 # TODO
elif self.crv:
self.gear_shifter = cp.vl[0x191]['GEAR_SHIFTER']
can_gear_shifter = cp.vl[0x191]['GEAR_SHIFTER']
self.angle_steers = cp.vl[0x156]['STEER_ANGLE']
self.gear = cp.vl[0x191]['GEAR']
self.cruise_setting = cp.vl[0x1A6]['CRUISE_SETTING']
self.cruise_buttons = cp.vl[0x1A6]['CRUISE_BUTTONS']
self.main_on = cp.vl[0x1A6]['MAIN_ON']
self.gear_shifter_valid = self.gear_shifter in [1,8] # TODO: 1/P allowed for debug
self.blinker_on = cp.vl[0x294]['LEFT_BLINKER'] or cp.vl[0x294]['RIGHT_BLINKER']
self.left_blinker_on = cp.vl[0x294]['LEFT_BLINKER']
self.right_blinker_on = cp.vl[0x294]['RIGHT_BLINKER']
else:
self.gear_shifter = cp.vl[0x1A3]['GEAR_SHIFTER']
self.cruise_speed_offset = -0.3
self.park_brake = 0 # TODO
self.brake_hold = 0 # TODO
elif self.acura:
can_gear_shifter = cp.vl[0x1A3]['GEAR_SHIFTER']
self.angle_steers = cp.vl[0x156]['STEER_ANGLE']
self.gear = cp.vl[0x1A3]['GEAR']
self.cruise_setting = cp.vl[0x1A6]['CRUISE_SETTING']
self.cruise_buttons = cp.vl[0x1A6]['CRUISE_BUTTONS']
self.main_on = cp.vl[0x1A6]['MAIN_ON']
self.gear_shifter_valid = self.gear_shifter in [1,4] # TODO: 1/P allowed for debug
self.blinker_on = cp.vl[0x294]['LEFT_BLINKER'] or cp.vl[0x294]['RIGHT_BLINKER']
self.left_blinker_on = cp.vl[0x294]['LEFT_BLINKER']
self.right_blinker_on = cp.vl[0x294]['RIGHT_BLINKER']
self.cruise_speed_offset = calc_cruise_offset(cp.vl[0x37c]['CRUISE_SPEED_OFFSET'], self.v_ego)
self.park_brake = 0 # TODO
self.brake_hold = 0
self.gear_shifter = parse_gear_shifter(can_gear_shifter, self.acura)
if self.accord:
# on the accord, this doesn't seem to include cruise control
@@ -398,4 +440,25 @@ class CarState(object):
self.pcm_acc_status = cp.vl[0x17C]['ACC_STATUS']
self.pedal_gas = cp.vl[0x17C]['PEDAL_GAS']
self.hud_lead = cp.vl[0x30C]['HUD_LEAD']
self.counter_pcm = cp.vl[0x324]['COUNTER']
# carstate standalone tester
if __name__ == '__main__':
import zmq
import time
from selfdrive.services import service_list
context = zmq.Context()
logcan = messaging.sub_sock(context, service_list['can'].port)
class CarParams(object):
def __init__(self):
self.carFingerprint = "HONDA CIVIC 2016 TOURING"
self.enableGas = 0
self.enableCruise = 0
CP = CarParams()
CS = CarState(CP, logcan)
while 1:
CS.update()
time.sleep(0.01)
selfdrive/car/honda/interface.py
+171 -60
View File
@@ -1,19 +1,65 @@
#!/usr/bin/env python
import os
import time
import common.numpy_fast as np
import numpy as np
from common.numpy_fast import clip
from common.realtime import sec_since_boot
from selfdrive.config import Conversions as CV
from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET, get_events
from .carstate import CarState
from .carcontroller import CarController, AH
from selfdrive.boardd.boardd import can_capnp_to_can_list
from cereal import car
from selfdrive.services import service_list
import selfdrive.messaging as messaging
NEW_CAN = os.getenv("OLD_CAN") is None
def get_compute_gb():
# generate a function that takes in [desired_accel, current_speed] -> [-1.0, 1.0]
# where -1.0 is max brake and 1.0 is max gas
# see debug/dump_accel_from_fiber.py to see how those parameters were generated
w0 = np.array([[ 1.22056961, -0.39625418, 0.67952657],
[ 1.03691769, 0.78210306, -0.41343188]])
b0 = np.array([ 0.01536703, -0.14335321, -0.26932889])
w2 = np.array([[-0.59124422, 0.42899439, 0.38660881],
[ 0.79973811, 0.13178682, 0.08550351],
[-0.15651935, -0.44360259, 0.76910877]])
b2 = np.array([ 0.15624429, 0.02294923, -0.0341086 ])
w4 = np.array([[-0.31521443],
[-0.38626176],
[ 0.52667892]])
b4 = np.array([-0.02922216])
def compute_output(dat, w0, b0, w2, b2, w4, b4):
m0 = np.dot(dat, w0) + b0
m0 = leakyrelu(m0, 0.1)
m2 = np.dot(m0, w2) + b2
m2 = leakyrelu(m2, 0.1)
m4 = np.dot(m2, w4) + b4
return m4
def leakyrelu(x, alpha):
return np.maximum(x, alpha * x)
def _compute_gb(accel, speed):
#linearly extrap below v1 using v1 and v2 data
v1 = 5.
v2 = 10.
dat = np.array([accel, speed])
if speed > 5.:
m4 = compute_output(dat, w0, b0, w2, b2, w4, b4)
else:
dat[1] = v1
m4v1 = compute_output(dat, w0, b0, w2, b2, w4, b4)
dat[1] = v2
m4v2 = compute_output(dat, w0, b0, w2, b2, w4, b4)
m4 = (speed - v1) * (m4v2 - m4v1) / (v2 - v1) + m4v1
return float(m4)
return _compute_gb
# Car button codes
class CruiseButtons:
@@ -22,6 +68,7 @@ class CruiseButtons:
CANCEL = 2
MAIN = 1
#car chimes: enumeration from dbc file. Chimes are for alerts and warnings
class CM:
MUTE = 0
@@ -30,6 +77,7 @@ class CM:
REPEATED = 1
CONTINUOUS = 2
#car beepss: enumeration from dbc file. Beeps are for activ and deactiv
class BP:
MUTE = 0
@@ -37,12 +85,17 @@ class BP:
TRIPLE = 2
REPEATED = 1
class CarInterface(object):
def __init__(self, CP, logcan, sendcan=None):
self.logcan = logcan
self.CP = CP
self.frame = 0
self.last_enable_pressed = 0
self.last_enable_sent = 0
self.gas_pressed_prev = False
self.brake_pressed_prev = False
self.can_invalid_count = 0
# *** init the major players ***
@@ -54,7 +107,8 @@ class CarInterface(object):
self.CC = CarController()
if self.CS.accord:
self.accord_msg = []
# self.accord_msg = []
raise NotImplementedError
@staticmethod
def get_params(candidate, fingerprint):
@@ -68,6 +122,8 @@ class CarInterface(object):
ret.radarName = "nidec"
ret.carFingerprint = candidate
ret.safetyModel = car.CarParams.SafetyModels.honda
ret.enableSteer = True
ret.enableBrake = True
@@ -75,7 +131,7 @@ class CarInterface(object):
ret.enableGas = 0x201 in fingerprint
ret.enableCruise = not ret.enableGas
# FIXME: hardcoding honda civic 2016 touring wight so it can be used to
# FIXME: hardcoding honda civic 2016 touring params so they can be used to
# scale unknown params for other cars
m_civic = 2923./2.205 + std_cargo
l_civic = 2.70
@@ -86,27 +142,32 @@ class CarInterface(object):
cR_civic = 90000
if candidate == "HONDA CIVIC 2016 TOURING":
stop_and_go = True
ret.m = m_civic
ret.l = l_civic
ret.aF = aF_civic
ret.sR = 13.0
ret.steerKp, ret.steerKi = 0.8, 0.24
# Civic at comma has modified steering FW, so different tuning for the Neo in that car
is_fw_modified = os.getenv("DONGLE_ID") in ['b0f5a01cf604185c']
ret.steerKp, ret.steerKi = [0.4, 0.12] if is_fw_modified else [0.8, 0.24]
elif candidate == "ACURA ILX 2016 ACURAWATCH PLUS":
stop_and_go = False
ret.m = 3095./2.205 + std_cargo
ret.l = 2.67
ret.aF = ret.l * 0.37
ret.sR = 15.3
# Acura at comma has modified steering FW, so different tuning for the Neo in that car
# FIXME: using dongleId isn't great, better to identify the car than the Neo
is_fw_modified = os.getenv("DONGLE_ID") == 'cb38263377b873ee'
is_fw_modified = os.getenv("DONGLE_ID") in ['cb38263377b873ee']
ret.steerKp, ret.steerKi = [0.4, 0.12] if is_fw_modified else [0.8, 0.24]
elif candidate == "HONDA ACCORD 2016 TOURING":
stop_and_go = False
ret.m = 3580./2.205 + std_cargo
ret.l = 2.74
ret.aF = ret.l * 0.38
ret.sR = 15.3
ret.steerKp, ret.steerKi = 0.8, 0.24
elif candidate == "HONDA CR-V 2016 TOURING":
stop_and_go = False
ret.m = 3572./2.205 + std_cargo
ret.l = 2.62
ret.aF = ret.l * 0.41
@@ -114,48 +175,54 @@ class CarInterface(object):
ret.steerKp, ret.steerKi = 0.8, 0.24
else:
raise ValueError("unsupported car %s" % candidate)
# min speed to enable ACC. if car can do stop and go, then set enabling speed
# to a negative value, so it won't matter. Otherwise, add 0.5 mph margin to not
# conflict with PCM acc
ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGas) else 25.5 * CV.MPH_TO_MS
ret.aR = ret.l - ret.aF
# TODO: get actual value, for now starting with reasonable value for
# civic and scaling by mass and wheelbase
ret.j = j_civic * ret.m * ret.l**2 / (m_civic * l_civic**2)
# TODO: start from empirically derived lateral slip stiffness for the civic and scale by
# mass and CG position... all cars will have approximately similar dyn behaviors
# mass and CG position, so all cars will have approximately similar dyn behaviors
ret.cF = cF_civic * ret.m / m_civic * (ret.aR / ret.l) / (aR_civic / l_civic)
ret.cR = cR_civic * ret.m / m_civic * (ret.aF / ret.l) / (aF_civic / l_civic)
# no rear steering, at least on the listed cars above
ret.chi = 0.
# no max steer limit VS speed
ret.steerMaxBP = [0.] # m/s
ret.steerMaxV = [1.] # max steer allowed
ret.gasMaxBP = [0.] # m/s
ret.gasMaxV = [0.6] if ret.enableGas else [0.] # max gas allowed
ret.brakeMaxBP = [5., 20.] # m/s
ret.brakeMaxV = [1., 0.8] # max brake allowed
return ret
compute_gb = staticmethod(get_compute_gb())
# returns a car.CarState
def update(self):
def update(self, c):
# ******************* do can recv *******************
can_pub_main = []
canMonoTimes = []
if NEW_CAN:
self.CS.update(can_pub_main)
else:
for a in messaging.drain_sock(self.logcan):
canMonoTimes.append(a.logMonoTime)
can_pub_main.extend(can_capnp_to_can_list(a.can, [0,0x80]))
if self.CS.accord:
self.accord_msg.extend(can_capnp_to_can_list(a.can, [9]))
self.accord_msg = self.accord_msg[-1:]
self.CS.update(can_pub_main)
self.CS.update(can_pub_main)
# create message
ret = car.CarState.new_message()
# speeds
ret.vEgo = self.CS.v_ego
ret.wheelSpeeds.fl = self.CS.cp.vl[0x1D0]['WHEEL_SPEED_FL']
ret.wheelSpeeds.fr = self.CS.cp.vl[0x1D0]['WHEEL_SPEED_FR']
ret.wheelSpeeds.rl = self.CS.cp.vl[0x1D0]['WHEEL_SPEED_RL']
ret.wheelSpeeds.rr = self.CS.cp.vl[0x1D0]['WHEEL_SPEED_RR']
ret.wheelSpeeds.fl = self.CS.v_wheel_fl
ret.wheelSpeeds.fr = self.CS.v_wheel_fr
ret.wheelSpeeds.rl = self.CS.v_wheel_rl
ret.wheelSpeeds.rr = self.CS.v_wheel_rr
# gas pedal
ret.gas = self.CS.car_gas / 256.0
@@ -172,28 +239,17 @@ class CarInterface(object):
# TODO: units
ret.steeringAngle = self.CS.angle_steers
if self.CS.accord:
# TODO: move this into the CAN parser
ret.steeringTorque = 0
if len(self.accord_msg) > 0:
aa = map(lambda x: ord(x)&0x7f, self.accord_msg[0][2])
if len(aa) != 5 or (-(aa[0]+aa[1]+aa[2]+aa[3]))&0x7f != aa[4]:
print "ACCORD MSG BAD LEN OR CHECKSUM!"
# TODO: throw an error here?
else:
st = ((aa[0]&0xF) << 5) + (aa[1]&0x1F)
if st >= 256:
st = -(512-st)
ret.steeringTorque = st
ret.steeringPressed = abs(ret.steeringTorque) > 20
else:
ret.steeringTorque = self.CS.cp.vl[0x18F]['STEER_TORQUE_SENSOR']
ret.steeringPressed = self.CS.steer_override
# gear shifter lever
ret.gearShifter = self.CS.gear_shifter
ret.steeringTorque = self.CS.cp.vl[0x18F]['STEER_TORQUE_SENSOR']
ret.steeringPressed = self.CS.steer_override
# cruise state
ret.cruiseState.enabled = self.CS.pcm_acc_status != 0
ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS
ret.cruiseState.available = bool(self.CS.main_on)
ret.cruiseState.speedOffset = self.CS.cruise_speed_offset
# TODO: button presses
buttonEvents = []
@@ -244,38 +300,94 @@ class CarInterface(object):
buttonEvents.append(be)
ret.buttonEvents = buttonEvents
# errors
# events
# TODO: I don't like the way capnp does enums
# These strings aren't checked at compile time
errors = []
events = []
if not self.CS.can_valid:
self.can_invalid_count += 1
if self.can_invalid_count >= 5:
errors.append('commIssue')
events.append(create_event('commIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
else:
self.can_invalid_count = 0
if self.CS.steer_error:
errors.append('steerUnavailable')
events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
elif self.CS.steer_not_allowed:
errors.append('steerTempUnavailable')
events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING]))
if self.CS.brake_error:
errors.append('brakeUnavailable')
if not self.CS.gear_shifter_valid:
errors.append('wrongGear')
events.append(create_event('brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if not ret.gearShifter == 'drive':
events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not self.CS.door_all_closed:
errors.append('doorOpen')
events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not self.CS.seatbelt:
errors.append('seatbeltNotLatched')
events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if self.CS.esp_disabled:
errors.append('espDisabled')
events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not self.CS.main_on:
errors.append('wrongCarMode')
if self.CS.gear_shifter == 2:
errors.append('reverseGear')
events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.gearShifter == 'reverse':
events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if self.CS.brake_hold:
events.append(create_event('brakeHold', [ET.NO_ENTRY, ET.USER_DISABLE]))
if self.CS.park_brake:
events.append(create_event('parkBrake', [ET.NO_ENTRY, ET.USER_DISABLE]))
ret.errors = errors
if self.CP.enableCruise and ret.vEgo < self.CP.minEnableSpeed:
events.append(create_event('speedTooLow', [ET.NO_ENTRY]))
# disable on pedals rising edge or when brake is pressed and speed isn't zero
if (ret.gasPressed and not self.gas_pressed_prev) or \
(ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)):
events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
#if (ret.brakePressed and ret.vEgo < 0.001) or ret.gasPressed:
if ret.gasPressed:
events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
# it can happen that car cruise disables while comma system is enabled: need to
# keep braking if needed or if the speed is very low
# TODO: for the Acura, cancellation below 25mph is normal. Issue a non loud alert
if self.CP.enableCruise and not ret.cruiseState.enabled and \
(c.actuators.brake <= 0. or ret.vEgo < 0.3):
events.append(create_event("cruiseDisabled", [ET.IMMEDIATE_DISABLE]))
cur_time = sec_since_boot()
enable_pressed = False
# handle button presses
for b in ret.buttonEvents:
# do enable on both accel and decel buttons
if b.type in ["accelCruise", "decelCruise"] and not b.pressed:
print "enabled pressed at", cur_time
self.last_enable_pressed = cur_time
enable_pressed = True
# do disable on button down
if b.type == "cancel" and b.pressed:
events.append(create_event('buttonCancel', [ET.USER_DISABLE]))
if self.CP.enableCruise:
# KEEP THIS EVENT LAST! send enable event if button is pressed and there are
# NO_ENTRY events, so controlsd will display alerts. Also not send enable events
# too close in time, so a no_entry will not be followed by another one.
# TODO: button press should be the only thing that triggers enble
if ((cur_time - self.last_enable_pressed) < 0.2 and
(cur_time - self.last_enable_sent) > 0.2 and
ret.cruiseState.enabled) or \
(enable_pressed and get_events(events, [ET.NO_ENTRY])):
events.append(create_event('buttonEnable', [ET.ENABLE]))
self.last_enable_sent = cur_time
elif enable_pressed:
events.append(create_event('buttonEnable', [ET.ENABLE]))
ret.events = events
ret.canMonoTimes = canMonoTimes
# update previous brake/gas pressed
self.gas_pressed_prev = ret.gasPressed
self.brake_pressed_prev = ret.brakePressed
# cast to reader so it can't be modified
#print ret
return ret.as_reader()
@@ -309,10 +421,10 @@ class CarInterface(object):
"chimeRepeated": (BP.MUTE, CM.REPEATED),
"chimeContinuous": (BP.MUTE, CM.CONTINUOUS)}[str(c.hudControl.audibleAlert)]
pcm_accel = int(np.clip(c.cruiseControl.accelOverride,0,1)*0xc6)
pcm_accel = int(clip(c.cruiseControl.accelOverride,0,1)*0xc6)
self.CC.update(self.sendcan, c.enabled, self.CS, self.frame, \
c.gas, c.brake, c.steeringTorque, \
c.actuators, \
c.cruiseControl.speedOverride, \
c.cruiseControl.override, \
c.cruiseControl.cancel, \
@@ -324,4 +436,3 @@ class CarInterface(object):
snd_chime = snd_chime)
self.frame += 1
return not (c.enabled and not self.CC.controls_allowed)
selfdrive/car/honda/can_parser.py → selfdrive/car/honda/old_can_parser.py
+5 -3
View File
@@ -36,15 +36,16 @@ class CANParser(object):
for _, addr, _ in signals:
self.vl[addr] = {}
self.ts[addr] = 0
self.ts[addr] = {}
self.ct[addr] = sec_since_boot()
self.ok[addr] = False
self.ok[addr] = True
self.cn[addr] = 0
self.cn_vl[addr] = 0
self.ck[addr] = False
for name, addr, ival in signals:
self.vl[addr][name] = ival
self.ts[addr][name] = 0
self._msgs = [s[1] for s in signals]
self._sgs = [s[0] for s in signals]
@@ -94,7 +95,6 @@ class CANParser(object):
self.ok[msg] = False
# update msg time stamps and counter value
self.ts[msg] = ts
self.ct[msg] = self.sec_since_boot_cached
self.cn[msg] = cn
self.cn_vl[msg] = min(max(self.cn_vl[msg], 0), cn_vl_max)
@@ -107,12 +107,14 @@ class CANParser(object):
for ii in idxs:
sg = self._sgs[ii]
self.vl[msg][sg] = out[sg]
self.ts[msg][sg] = ts
# for each message, check if it's too long since last time we received it
self._check_dead_msgs()
# assess overall can validity: if there is one relevant message invalid, then set can validity flag to False
self.can_valid = True
if False in self.ok.values():
#print "CAN INVALID!"
self.can_valid = False
selfdrive/common/cereal.mk
+16 -4
View File
@@ -1,14 +1,27 @@
UNAME_M ?= $(shell uname -m)
ifeq ($(UNAME_M),x86_64)
UNAME_S ?= $(shell uname -s)
CEREAL_CFLAGS = -I$(ONE)/external/capnp/include
ifeq ($(UNAME_S),Darwin)
CEREAL_CFLAGS = -I$(PHONELIBS)/capnp-c/include
CEREAL_CXXFLAGS = -I$(PHONELIBS)/capnp-cpp/mac/include
CEREAL_LIBS = $(PHONELIBS)/capnp-cpp/mac/lib/libcapnp.a \
$(PHONELIBS)/capnp-cpp/mac/lib/libkj.a \
$(PHONELIBS)/capnp-c/mac/lib/libcapnp_c.a
else ifeq ($(UNAME_M),x86_64)
CEREAL_CFLAGS = -I$(BASEDIR)/external/capnp/include
CEREAL_CXXFLAGS = $(CEREAL_CFLAGS)
ifeq ($(CEREAL_LIBS),)
CEREAL_LIBS = -L$(ONE)/external/capnp/lib \
CEREAL_LIBS = -L$(BASEDIR)/external/capnp/lib \
-l:libcapnp.a -l:libkj.a -l:libcapnp_c.a
endif
else
CEREAL_CFLAGS = -I$(PHONELIBS)/capnp-c/include
CEREAL_CXXFLAGS = -I$(PHONELIBS)/capnp-cpp/include
ifeq ($(CEREAL_LIBS),)
@@ -30,4 +43,3 @@ car.capnp.o: ../../cereal/gen/cpp/car.capnp.c++
@echo "[ CXX ] $@"
$(CXX) $(CXXFLAGS) $(CEREAL_CXXFLAGS) \
-c -o '$@' '$<'
selfdrive/common/mat.h
+20
View File
@@ -65,4 +65,24 @@ static inline vec4 matvecmul(const mat4 a, const vec4 b) {
return ret;
}
// scales the input and output space of a transformation matrix
// that assumes pixel-center origin.
static inline mat3 transform_scale_buffer(const mat3 in, float s) {
// in_pt = ( transform(out_pt/s + 0.5) - 0.5) * s
mat3 transform_out = {{
1.0f/s, 0.0f, 0.5f,
0.0f, 1.0f/s, 0.5f,
0.0f, 0.0f, 1.0f,
}};
mat3 transform_in = {{
s, 0.0f, -0.5f*s,
0.0f, s, -0.5f*s,
0.0f, 0.0f, 1.0f,
}};
return matmul3(transform_in, matmul3(in, transform_out));
}
#endif
selfdrive/common/params.c → selfdrive/common/params.cc
+23 -2
View File
@@ -11,6 +11,18 @@
#include <stdlib.h>
#include <stdio.h>
namespace {
template <typename T>
T* null_coalesce(T* a, T* b) {
return a != NULL ? a : b;
}
static const char* default_params_path = null_coalesce(
const_cast<const char*>(getenv("PARAMS_PATH")), "/data/params");
} // namespace
int write_db_value(const char* params_path, const char* key, const char* value,
size_t value_size) {
int lock_fd = -1;
@@ -18,6 +30,11 @@ int write_db_value(const char* params_path, const char* key, const char* value,
int result;
char tmp_path[1024];
char path[1024];
ssize_t bytes_written;
if (params_path == NULL) {
params_path = default_params_path;
}
// Write value to temp.
result =
@@ -27,7 +44,7 @@ int write_db_value(const char* params_path, const char* key, const char* value,
}
tmp_fd = mkstemp(tmp_path);
const ssize_t bytes_written = write(tmp_fd, value, value_size);
bytes_written = write(tmp_fd, value, value_size);
if (bytes_written != value_size) {
result = -20;
goto cleanup;
@@ -79,6 +96,10 @@ int read_db_value(const char* params_path, const char* key, char** value,
int result;
char path[1024];
if (params_path == NULL) {
params_path = default_params_path;
}
result = snprintf(path, sizeof(path), "%s/.lock", params_path);
if (result < 0) {
goto cleanup;
@@ -99,7 +120,7 @@ int read_db_value(const char* params_path, const char* key, char** value,
// Read value.
// TODO(mgraczyk): If there is a lot of contention, we can release the lock
// after opening the file, before reading.
*value = read_file(path, value_sz);
*value = static_cast<char*>(read_file(path, value_sz));
if (*value == NULL) {
result = -22;
goto cleanup;
selfdrive/common/params.h
+1 -3
View File
@@ -7,14 +7,12 @@
extern "C" {
#endif
#define PARAMS_PATH "/data/params"
int write_db_value(const char* params_path, const char* key, const char* value,
size_t value_size);
// Reads a value from the params database.
// Inputs:
// params_path: The path of the database, eg /sdcard/params.
// params_path: The path of the database, or NULL to use the default.
// key: The key to read.
// value: A pointer where a newly allocated string containing the db value will
// be written.
selfdrive/common/swaglog.h
+36
View File
@@ -1,6 +1,8 @@
#ifndef SWAGLOG_H
#define SWAGLOG_H
#include "common/timing.h"
#define CLOUDLOG_DEBUG 10
#define CLOUDLOG_INFO 20
#define CLOUDLOG_WARNING 30
@@ -24,9 +26,43 @@ void cloudlog_bind(const char* k, const char* v);
__func__, \
fmt, ## __VA_ARGS__)
#define cloudlog_rl(burst, millis, lvl, fmt, ...) \
{ \
static uint64_t __begin = 0; \
static int __printed = 0; \
static int __missed = 0; \
\
int __burst = (burst); \
int __millis = (millis); \
uint64_t __ts = nanos_since_boot(); \
\
if (!__begin) __begin = __ts; \
\
if (__begin + __millis*1000000ULL < __ts) { \
if (__missed) { \
cloudlog(CLOUDLOG_WARNING, "cloudlog: %d messages supressed", __missed); \
} \
__begin = 0; \
__printed = 0; \
__missed = 0; \
} \
\
if (__printed < __burst) { \
cloudlog(lvl, fmt, ## __VA_ARGS__); \
__printed++; \
} else { \
__missed++; \
} \
}
#define LOGD(fmt, ...) cloudlog(CLOUDLOG_DEBUG, fmt, ## __VA_ARGS__)
#define LOG(fmt, ...) cloudlog(CLOUDLOG_INFO, fmt, ## __VA_ARGS__)
#define LOGW(fmt, ...) cloudlog(CLOUDLOG_WARNING, fmt, ## __VA_ARGS__)
#define LOGE(fmt, ...) cloudlog(CLOUDLOG_ERROR, fmt, ## __VA_ARGS__)
#define LOGD_100(fmt, ...) cloudlog_rl(2, 100, CLOUDLOG_DEBUG, fmt, ## __VA_ARGS__)
#define LOG_100(fmt, ...) cloudlog_rl(2, 100, CLOUDLOG_INFO, fmt, ## __VA_ARGS__)
#define LOGW_100(fmt, ...) cloudlog_rl(2, 100, CLOUDLOG_WARNING, fmt, ## __VA_ARGS__)
#define LOGE_100(fmt, ...) cloudlog_rl(2, 100, CLOUDLOG_ERROR, fmt, ## __VA_ARGS__)
#endif
selfdrive/common/timing.h
+14 -1
View File
@@ -5,7 +5,7 @@
#include <time.h>
#ifdef __APPLE__
#define CLOCK_BOOTTIME CLOCK_REALTIME
#define CLOCK_BOOTTIME CLOCK_MONOTONIC
#endif
static inline uint64_t nanos_since_boot() {
@@ -38,4 +38,17 @@ static inline double seconds_since_epoch() {
return (double)t.tv_sec + t.tv_nsec * 1e-9;
}
// you probably should use nanos_since_boot instead
static inline uint64_t nanos_monotonic() {
struct timespec t;
clock_gettime(CLOCK_MONOTONIC, &t);
return t.tv_sec * 1000000000ULL + t.tv_nsec;
}
static inline uint64_t nanos_monotonic_raw() {
struct timespec t;
clock_gettime(CLOCK_MONOTONIC_RAW, &t);
return t.tv_sec * 1000000000ULL + t.tv_nsec;
}
#endif
selfdrive/common/util.c
+11
View File
@@ -3,6 +3,10 @@
#include <string.h>
#include <assert.h>
#ifdef __linux__
#include <sys/prctl.h>
#endif
void* read_file(const char* path, size_t* out_len) {
FILE* f = fopen(path, "r");
if (!f) {
@@ -28,3 +32,10 @@ void* read_file(const char* path, size_t* out_len) {
return buf;
}
void set_thread_name(const char* name) {
#ifdef __linux__
// pthread_setname_np is dumb (fails instead of truncates)
prctl(PR_SET_NAME, (unsigned long)name, 0, 0, 0);
#endif
}
selfdrive/common/util.h
+14
View File
@@ -1,6 +1,9 @@
#ifndef COMMON_UTIL_H
#define COMMON_UTIL_H
#ifndef __cplusplus
#define min(a,b) \
({ __typeof__ (a) _a = (a); \
__typeof__ (b) _b = (b); \
@@ -11,6 +14,8 @@
__typeof__ (b) _b = (b); \
_a > _b ? _a : _b; })
#endif
#define clamp(a,b,c) \
({ __typeof__ (a) _a = (a); \
__typeof__ (b) _b = (b); \
@@ -19,11 +24,20 @@
#define ARRAYSIZE(x) (sizeof(x)/sizeof(x[0]))
#ifdef __cplusplus
extern "C" {
#endif
// Reads a file into a newly allocated buffer.
//
// Returns NULL on failure, otherwise the NULL-terminated file contents.
// The result must be freed by the caller.
void* read_file(const char* path, size_t* out_len);
void set_thread_name(const char* name);
#ifdef __cplusplus
}
#endif
#endif
selfdrive/common/utilpp.h
+4
View File
@@ -9,6 +9,10 @@
#include <sstream>
#include <fstream>
#ifdef __x86_64
#include <linux/limits.h>
#endif
namespace util {
inline bool starts_with(std::string s, std::string prefix) {
selfdrive/common/version.h
+1 -1
View File
@@ -1 +1 @@
#define OPENPILOT_VERSION "0.3.6.1"
#define OPENPILOT_VERSION "0.3.7"
selfdrive/common/visionipc.c
+5 -4
View File
@@ -1,6 +1,7 @@
#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include <string.h>
#include <unistd.h>
#include <assert.h>
#include <errno.h>
@@ -126,7 +127,7 @@ int vipc_send(int fd, const VisionPacket *p2) {
return sendrecv_with_fds(true, fd, (void*)&p, sizeof(p), (int*)p2->fds, p2->num_fds, NULL);
}
void visionbufs_load(VisionBuf *bufs, const VisionStreamBufs *stream_bufs,
void vipc_bufs_load(VIPCBuf *bufs, const VisionStreamBufs *stream_bufs,
int num_fds, const int* fds) {
for (int i=0; i<num_fds; i++) {
if (bufs[i].addr) {
@@ -180,10 +181,10 @@ int visionstream_init(VisionStream *s, VisionStreamType type, bool tbuffer, Visi
s->bufs_info = rp.d.stream_bufs;
s->num_bufs = rp.num_fds;
s->bufs = calloc(s->num_bufs, sizeof(VisionBuf));
s->bufs = calloc(s->num_bufs, sizeof(VIPCBuf));
assert(s->bufs);
visionbufs_load(s->bufs, &rp.d.stream_bufs, s->num_bufs, rp.fds);
vipc_bufs_load(s->bufs, &rp.d.stream_bufs, s->num_bufs, rp.fds);
if (out_bufs_info) {
*out_bufs_info = s->bufs_info;
@@ -192,7 +193,7 @@ int visionstream_init(VisionStream *s, VisionStreamType type, bool tbuffer, Visi
return 0;
}
VisionBuf* visionstream_get(VisionStream *s, VisionBufExtra *out_extra) {
VIPCBuf* visionstream_get(VisionStream *s, VIPCBufExtra *out_extra) {
int err;
VisionPacket rp;
selfdrive/common/visionipc.h
+8 -8
View File
@@ -47,9 +47,9 @@ typedef struct VisionStreamBufs {
} buf_info;
} VisionStreamBufs;
typedef struct VisionBufExtra {
typedef struct VIPCBufExtra {
uint32_t frame_id; // only for yuv
} VisionBufExtra;
} VIPCBufExtra;
typedef union VisionPacketData {
struct {
@@ -60,7 +60,7 @@ typedef union VisionPacketData {
struct {
VisionStreamType type;
int idx;
VisionBufExtra extra;
VIPCBufExtra extra;
} stream_acq;
struct {
VisionStreamType type;
@@ -79,12 +79,12 @@ int vipc_connect(void);
int vipc_recv(int fd, VisionPacket *out_p);
int vipc_send(int fd, const VisionPacket *p);
typedef struct VisionBuf {
typedef struct VIPCBuf {
int fd;
size_t len;
void* addr;
} VisionBuf;
void visionbufs_load(VisionBuf *bufs, const VisionStreamBufs *stream_bufs,
} VIPCBuf;
void vipc_bufs_load(VIPCBuf *bufs, const VisionStreamBufs *stream_bufs,
int num_fds, const int* fds);
@@ -94,11 +94,11 @@ typedef struct VisionStream {
int last_idx;
int num_bufs;
VisionStreamBufs bufs_info;
VisionBuf *bufs;
VIPCBuf *bufs;
} VisionStream;
int visionstream_init(VisionStream *s, VisionStreamType type, bool tbuffer, VisionStreamBufs *out_bufs_info);
VisionBuf* visionstream_get(VisionStream *s, VisionBufExtra *out_extra);
VIPCBuf* visionstream_get(VisionStream *s, VIPCBufExtra *out_extra);
void visionstream_destroy(VisionStream *s);
#ifdef __cplusplus
selfdrive/controls/controlsd.py
+413 -369
View File
@@ -2,24 +2,24 @@
import os
import json
from copy import copy
import zmq
from cereal import car, log
from common.numpy_fast import clip
from common.fingerprints import fingerprint
from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper
from common.profiler import Profiler
from common.params import Params
import selfdrive.messaging as messaging
from selfdrive.swaglog import cloudlog
from selfdrive.config import Conversions as CV
from selfdrive.services import service_list
from selfdrive.car import get_car
from selfdrive.controls.lib.planner import Planner
from selfdrive.controls.lib.drive_helpers import learn_angle_offset
from selfdrive.controls.lib.longcontrol import LongControl
from selfdrive.controls.lib.drive_helpers import learn_angle_offset, \
get_events, \
create_event, \
EventTypes as ET
from selfdrive.controls.lib.longcontrol import LongControl, STARTING_TARGET_SPEED
from selfdrive.controls.lib.latcontrol import LatControl
from selfdrive.controls.lib.alertmanager import AlertManager
from selfdrive.controls.lib.vehicle_model import VehicleModel
@@ -34,427 +34,472 @@ AWARENESS_TIME = 360. # 6 minutes limit without user touching steering whee
AWARENESS_PRE_TIME = 20. # a first alert is issued 20s before start decelerating the car
AWARENESS_DECEL = -0.2 # car smoothly decel at .2m/s^2 when user is distracted
# class Cal
class Calibration:
UNCALIBRATED = 0
CALIBRATED = 1
INVALID = 2
# to be used
class State():
class State:
DISABLED = 0
ENABLED = 1
SOFT_DISABLE = 2
PRE_ENABLED = 2
SOFT_DISABLING = 3
class Controls(object):
def __init__(self, gctx, rate=100):
self.rate = rate
# *** log ***
context = zmq.Context()
# True when actuators are controlled
def isActive(state):
return state in [State.ENABLED, State.SOFT_DISABLING]
# pub
self.live100 = messaging.pub_sock(context, service_list['live100'].port)
self.carstate = messaging.pub_sock(context, service_list['carState'].port)
self.carcontrol = messaging.pub_sock(context, service_list['carControl'].port)
sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
# sub
self.thermal = messaging.sub_sock(context, service_list['thermal'].port)
self.health = messaging.sub_sock(context, service_list['health'].port)
logcan = messaging.sub_sock(context, service_list['can'].port)
self.cal = messaging.sub_sock(context, service_list['liveCalibration'].port)
self.CC = car.CarControl.new_message()
self.CI, self.CP = get_car(logcan, sendcan)
self.PL = Planner(self.CP)
self.AM = AlertManager()
self.LoC = LongControl()
self.LaC = LatControl()
self.VM = VehicleModel(self.CP)
# write CarParams
params = Params()
params.put("CarParams", self.CP.to_bytes())
# fake plan
self.plan_ts = 0
self.plan = log.Plan.new_message()
self.plan.lateralValid = False
self.plan.longitudinalValid = False
# controls enabled state
self.enabled = False
self.last_enable_request = 0
# learned angle offset
self.angle_offset = 0
calibration_params = params.get("CalibrationParams")
if calibration_params:
try:
calibration_params = json.loads(calibration_params)
self.angle_offset = calibration_params["angle_offset"]
except (ValueError, KeyError):
pass
# rear view camera state
self.rear_view_toggle = False
self.rear_view_allowed = (params.get("IsRearViewMirror") == "1")
self.v_cruise_kph = 255
# 0.0 - 1.0
self.awareness_status = 1.0
self.soft_disable_timer = None
self.overtemp = False
self.free_space = 1.0
self.cal_status = Calibration.UNCALIBRATED
self.cal_perc = 0
self.rk = Ratekeeper(self.rate, print_delay_threshold=2./1000)
def data_sample(self):
self.prof = Profiler()
self.cur_time = sec_since_boot()
# first read can and compute car states
self.CS = self.CI.update()
self.prof.checkpoint("CarInterface")
# True if system is engaged
def isEnabled(state):
return (isActive(state) or state == State.PRE_ENABLED)
# *** thermal checking logic ***
# thermal data, checked every second
td = messaging.recv_sock(self.thermal)
if td is not None:
# Check temperature.
self.overtemp = any(
t > 950
for t in (td.thermal.cpu0, td.thermal.cpu1, td.thermal.cpu2,
td.thermal.cpu3, td.thermal.mem, td.thermal.gpu))
# under 15% of space free
self.free_space = td.thermal.freeSpace
# read calibration status
cal = messaging.recv_sock(self.cal)
if cal is not None:
self.cal_status = cal.liveCalibration.calStatus
self.cal_perc = cal.liveCalibration.calPerc
def data_sample(CI, CC, thermal, health, cal, cal_status, cal_perc):
def state_transition(self):
pass # for now
# *** read can and compute car states ***
CS = CI.update(CC)
events = list(CS.events)
def state_control(self):
# did it request to enable?
enable_request, enable_condition = False, False
# *** thermal checking logic ***
# thermal data, checked every second
td = messaging.recv_sock(thermal)
if td is not None:
# Check temperature
overtemp = any(
t > 950
for t in (td.thermal.cpu0, td.thermal.cpu1, td.thermal.cpu2,
td.thermal.cpu3, td.thermal.mem, td.thermal.gpu))
if overtemp:
events.append(create_event('overheat', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
# reset awareness status on steering
if self.CS.steeringPressed or not self.enabled:
self.awareness_status = 1.0
elif self.enabled:
# gives the user 6 minutes
self.awareness_status -= 1.0/(self.rate * AWARENESS_TIME)
if self.awareness_status <= 0.:
self.AM.add("driverDistracted", self.enabled)
elif self.awareness_status <= AWARENESS_PRE_TIME / AWARENESS_TIME and \
self.awareness_status >= (AWARENESS_PRE_TIME - 0.1) / AWARENESS_TIME:
self.AM.add("preDriverDistracted", self.enabled)
# under 15% of space free
if td.thermal.freeSpace < 0.15:
events.append(create_event('outOfSpace', [ET.NO_ENTRY]))
# handle button presses
for b in self.CS.buttonEvents:
print b
# *** read calibration status ***
cal = messaging.recv_sock(cal)
if cal is not None:
cal_status = cal.liveCalibration.calStatus
cal_perc = cal.liveCalibration.calPerc
# button presses for rear view
if b.type == "leftBlinker" or b.type == "rightBlinker":
if b.pressed and self.rear_view_allowed:
self.rear_view_toggle = True
else:
self.rear_view_toggle = False
if b.type == "altButton1" and b.pressed:
self.rear_view_toggle = not self.rear_view_toggle
if not self.CP.enableCruise and self.enabled and not b.pressed:
if b.type == "accelCruise":
self.v_cruise_kph -= (self.v_cruise_kph % V_CRUISE_DELTA) - V_CRUISE_DELTA
elif b.type == "decelCruise":
self.v_cruise_kph -= (self.v_cruise_kph % V_CRUISE_DELTA) + V_CRUISE_DELTA
self.v_cruise_kph = clip(self.v_cruise_kph, V_CRUISE_MIN, V_CRUISE_MAX)
if not self.enabled and b.type in ["accelCruise", "decelCruise"] and not b.pressed:
enable_request = True
# do disable on button down
if b.type == "cancel" and b.pressed:
self.AM.add("disable", self.enabled)
self.prof.checkpoint("Buttons")
# *** health checking logic ***
hh = messaging.recv_sock(self.health)
if hh is not None:
# if the board isn't allowing controls but somehow we are enabled!
# TODO: this should be in state transition with a function follower logic
if not hh.health.controlsAllowed and self.enabled:
self.AM.add("controlsMismatch", self.enabled)
# disable if the pedals are pressed while engaged, this is a user disable
if self.enabled:
if self.CS.gasPressed or self.CS.brakePressed or not self.CS.cruiseState.available:
self.AM.add("disable", self.enabled)
# it can happen that car cruise disables while comma system is enabled: need to
# keep braking if needed or if the speed is very low
# TODO: for the Acura, cancellation below 25mph is normal. Issue a non loud alert
if self.CP.enableCruise and not self.CS.cruiseState.enabled and \
(self.CC.brake <= 0. or self.CS.vEgo < 0.3):
self.AM.add("cruiseDisabled", self.enabled)
if enable_request:
# check for pressed pedals
if self.CS.gasPressed or self.CS.brakePressed:
self.AM.add("pedalPressed", self.enabled)
enable_request = False
else:
print "enabled pressed at", self.cur_time
self.last_enable_request = self.cur_time
# don't engage with less than 15% free
if self.free_space < 0.15:
self.AM.add("outOfSpace", self.enabled)
enable_request = False
if self.CP.enableCruise:
enable_condition = ((self.cur_time - self.last_enable_request) < 0.2) and self.CS.cruiseState.enabled
if cal_status != Calibration.CALIBRATED:
if cal_status == Calibration.UNCALIBRATED:
events.append(create_event('calibrationInProgress', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
else:
enable_condition = enable_request
events.append(create_event('calibrationInvalid', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if self.CP.enableCruise and self.CS.cruiseState.enabled:
self.v_cruise_kph = self.CS.cruiseState.speed * CV.MS_TO_KPH
# *** health checking logic ***
hh = messaging.recv_sock(health)
if hh is not None:
controls_allowed = hh.health.controlsAllowed
if not controls_allowed:
events.append(create_event('controlsMismatch', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
self.prof.checkpoint("AdaptiveCruise")
return CS, events, cal_status, cal_perc
# *** what's the plan ***
plan_packet = self.PL.update(self.CS, self.LoC)
self.plan = plan_packet.plan
self.plan_ts = plan_packet.logMonoTime
# if user is not responsive to awareness alerts, then start a smooth deceleration
if self.awareness_status < -0.:
self.plan.aTargetMax = min(self.plan.aTargetMax, AWARENESS_DECEL)
self.plan.aTargetMin = min(self.plan.aTargetMin, self.plan.aTargetMax)
def calc_plan(CS, events, PL, LoC):
# plan runs always, independently of the state
plan_packet = PL.update(CS, LoC)
plan = plan_packet.plan
plan_ts = plan_packet.logMonoTime
if enable_request or enable_condition or self.enabled:
# add all alerts from car
for alert in self.CS.errors:
self.AM.add(alert, self.enabled)
# add events from planner
events += list(plan.events)
if not self.plan.longitudinalValid:
self.AM.add("radarCommIssue", self.enabled)
# disable if lead isn't close when system is active and brake is pressed to avoid
# unexpected vehicle accelerations
if CS.brakePressed and plan.vTarget >= STARTING_TARGET_SPEED:
events.append(create_event('noTarget', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if self.cal_status != Calibration.CALIBRATED:
if self.cal_status == Calibration.UNCALIBRATED:
self.AM.add("calibrationInProgress", self.enabled, str(self.cal_perc) + '%')
else:
self.AM.add("calibrationInvalid", self.enabled)
return plan, plan_ts
if not self.plan.lateralValid:
# If the model is not broadcasting, assume that it is because
# the user has uploaded insufficient data for calibration.
# Other cases that would trigger this are rare and unactionable by the user.
self.AM.add("dataNeeded", self.enabled)
if self.overtemp:
self.AM.add("overheat", self.enabled)
def state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, cal_perc, AM):
# compute conditional state transitions and execute actions on state transitions
enabled = isEnabled(state)
# *** angle offset learning ***
if self.rk.frame % 5 == 2 and self.plan.lateralValid:
# *** run this at 20hz again ***
self.angle_offset = learn_angle_offset(self.enabled, self.CS.vEgo, self.angle_offset,
self.PL.PP.c_poly, self.PL.PP.c_prob, self.LaC.y_des,
self.CS.steeringPressed)
# handle button presses. TODO: this should be in state_control, but a decelCruise press
# would have the effect of both enabling and changing speed is checked after the state transition
for b in CS.buttonEvents:
if not CP.enableCruise and enabled and not b.pressed:
if b.type == "accelCruise":
v_cruise_kph -= (v_cruise_kph % V_CRUISE_DELTA) - V_CRUISE_DELTA
elif b.type == "decelCruise":
v_cruise_kph -= (v_cruise_kph % V_CRUISE_DELTA) + V_CRUISE_DELTA
v_cruise_kph = clip(v_cruise_kph, V_CRUISE_MIN, V_CRUISE_MAX)
# *** gas/brake PID loop ***
final_gas, final_brake = self.LoC.update(self.enabled, self.CS.vEgo, self.v_cruise_kph,
self.plan.vTarget,
[self.plan.aTargetMin, self.plan.aTargetMax],
self.plan.jerkFactor, self.CP)
# decrease the soft disable timer at every step, as it's reset on
# entrance in SOFT_DISABLING state
soft_disable_timer = max(0, soft_disable_timer - 1)
# *** steering PID loop ***
final_steer, sat_flag = self.LaC.update(self.enabled, self.CS.vEgo, self.CS.steeringAngle,
self.CS.steeringPressed, self.plan.dPoly, self.angle_offset, self.VM)
self.prof.checkpoint("PID")
# ***** handle alerts ****
# send FCW alert if triggered by planner
if self.plan.fcw:
self.AM.add("fcw", self.enabled)
# ***** handle state transitions *****
# send a "steering required alert" if saturation count has reached the limit
if sat_flag:
self.AM.add("steerSaturated", self.enabled)
if self.enabled and self.AM.alertShouldDisable():
print "DISABLING IMMEDIATELY ON ALERT"
self.enabled = False
if self.enabled and self.AM.alertShouldSoftDisable():
if self.soft_disable_timer is None:
self.soft_disable_timer = 3 * self.rate
elif self.soft_disable_timer == 0:
print "SOFT DISABLING ON ALERT"
self.enabled = False
# DISABLED
if state == State.DISABLED:
if get_events(events, [ET.ENABLE]):
if get_events(events, [ET.NO_ENTRY, ET.SOFT_DISABLE, ET.IMMEDIATE_DISABLE]):
for e in get_events(events, [ET.SOFT_DISABLE, ET.IMMEDIATE_DISABLE]):
AM.add(e, enabled)
for e in get_events(events, [ET.NO_ENTRY]):
txt = str(cal_perc) + '%' if e == 'calibrationInProgress' else ''
AM.add(str(e) + "NoEntry", enabled, txt)
else:
self.soft_disable_timer -= 1
else:
self.soft_disable_timer = None
if get_events(events, [ET.PRE_ENABLE]):
state = State.PRE_ENABLED
else:
state = State.ENABLED
AM.add("enable", enabled)
# on activation, let's always set v_cruise from where we are, even if PCM ACC is active
v_cruise_kph = int(round(max(CS.vEgo * CV.MS_TO_KPH, V_CRUISE_ENABLE_MIN)))
if enable_condition and not self.enabled and not self.AM.alertPresent():
print "*** enabling controls"
# ENABLED
elif state == State.ENABLED:
if get_events(events, [ET.USER_DISABLE]):
state = State.DISABLED
AM.add("disable", enabled)
# beep for enabling
self.AM.add("enable", self.enabled)
elif get_events(events, [ET.IMMEDIATE_DISABLE]):
state = State.DISABLED
for e in get_events(events, [ET.IMMEDIATE_DISABLE]):
AM.add(e, enabled)
# enable both lateral and longitudinal controls
self.enabled = True
elif get_events(events, [ET.SOFT_DISABLE]):
state = State.SOFT_DISABLING
soft_disable_timer = 300 # 3s TODO: use rate
for e in get_events(events, [ET.SOFT_DISABLE]):
AM.add(e, enabled)
# on activation, let's always set v_cruise from where we are, even if PCM ACC is active
self.v_cruise_kph = int(round(max(self.CS.vEgo * CV.MS_TO_KPH, V_CRUISE_ENABLE_MIN)))
# SOFT DISABLING
elif state == State.SOFT_DISABLING:
if get_events(events, [ET.USER_DISABLE]):
state = State.DISABLED
AM.add("disable", enabled)
# 6 minutes driver you're on
self.awareness_status = 1.0
elif get_events(events, [ET.IMMEDIATE_DISABLE]):
state = State.DISABLED
for e in get_events(events, [ET.IMMEDIATE_DISABLE]):
AM.add(e, enabled)
# reset the PID loops
self.LaC.reset()
# start long control at actual speed
self.LoC.reset(v_pid = self.CS.vEgo)
elif not get_events(events, [ET.SOFT_DISABLE]):
# no more soft disabling condition, so go back to ENABLED
state = State.ENABLED
# *** push the alerts to current ***
# TODO: remove output, store them inside AM class instead
self.alert_text_1, self.alert_text_2, self.visual_alert, self.audible_alert = self.AM.process_alerts(self.cur_time)
# ***** control the car *****
self.CC.enabled = self.enabled
elif soft_disable_timer <= 0:
state = State.DISABLED
# TODO: PRE ENABLING
elif state == State.PRE_ENABLED:
if get_events(events, [ET.USER_DISABLE]):
state = State.DISABLED
AM.add("disable", enabled)
self.CC.gas = float(final_gas)
self.CC.brake = float(final_brake)
self.CC.steeringTorque = float(final_steer)
elif get_events(events, [ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE]):
state = State.DISABLED
for e in get_events(events, [ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE]):
AM.add(e, enabled)
self.CC.cruiseControl.override = True
# always cancel if we have an interceptor
self.CC.cruiseControl.cancel = bool(not self.CP.enableCruise or
(not self.enabled and self.CS.cruiseState.enabled))
elif not get_events(events, [ET.PRE_ENABLE]):
state = State.ENABLED
# brake discount removes a sharp nonlinearity
brake_discount = (1.0 - clip(final_brake*3., 0.0, 1.0))
self.CC.cruiseControl.speedOverride = float(max(0.0, ((self.LoC.v_pid - .5) * brake_discount)) if self.CP.enableCruise else 0.0)
return state, soft_disable_timer, v_cruise_kph
#CC.cruiseControl.accelOverride = float(AC.a_pcm)
# TODO: parametrize 0.714 in interface?
# accelOverride is more or less the max throttle allowed to pcm: usually set to a constant
# unless aTargetMax is very high and then we scale with it; this helpw in quicker restart
self.CC.cruiseControl.accelOverride = float(max(0.714, self.plan.aTargetMax/A_ACC_MAX))
self.CC.hudControl.setSpeed = float(self.v_cruise_kph * CV.KPH_TO_MS)
self.CC.hudControl.speedVisible = self.enabled
self.CC.hudControl.lanesVisible = self.enabled
self.CC.hudControl.leadVisible = self.plan.hasLead
self.CC.hudControl.visualAlert = self.visual_alert
self.CC.hudControl.audibleAlert = self.audible_alert
def state_control(plan, CS, CP, state, events, v_cruise_kph, AM, rk, awareness_status,
PL, LaC, LoC, VM, angle_offset, rear_view_allowed, rear_view_toggle):
# Given the state, this function returns the actuators
# TODO: remove it from here and put it in state_transition
# this alert will apply next controls cycle
if not self.CI.apply(self.CC):
self.AM.add("controlsFailed", self.enabled)
# reset actuators to zero
actuators = car.CarControl.Actuators.new_message()
def data_send(self):
# broadcast carControl first
cc_send = messaging.new_message()
cc_send.init('carControl')
cc_send.carControl = copy(self.CC)
self.carcontrol.send(cc_send.to_bytes())
enabled = isEnabled(state)
active = isActive(state)
self.prof.checkpoint("CarControl")
for b in CS.buttonEvents:
# any button event resets awarenesss_status
awareness_status = 1.
# broadcast carState
cs_send = messaging.new_message()
cs_send.init('carState')
cs_send.carState = copy(self.CS)
self.carstate.send(cs_send.to_bytes())
# ***** publish state to logger *****
# button presses for rear view
if b.type == "leftBlinker" or b.type == "rightBlinker":
if b.pressed and rear_view_allowed:
rear_view_toggle = True
else:
rear_view_toggle = False
# publish controls state at 100Hz
if b.type == "altButton1" and b.pressed:
rear_view_toggle = not rear_view_toggle
# send FCW alert if triggered by planner
if plan.fcw:
AM.add("fcw", enabled)
# ***** state specific actions *****
# DISABLED
if state in [State.PRE_ENABLED, State.DISABLED]:
awareness_status = 1.
LaC.reset()
LoC.reset(v_pid=CS.vEgo)
# ENABLED or SOFT_DISABLING
elif state in [State.ENABLED, State.SOFT_DISABLING]:
if CS.steeringPressed:
# reset awareness status on steering
awareness_status = 1.0
# 6 minutes driver you're on
awareness_status -= 0.01/(AWARENESS_TIME)
if awareness_status <= 0.:
AM.add("driverDistracted", enabled)
elif awareness_status <= AWARENESS_PRE_TIME / AWARENESS_TIME and \
awareness_status >= (AWARENESS_PRE_TIME - 0.1) / AWARENESS_TIME:
AM.add("preDriverDistracted", enabled)
# parse warnings from car specific interface
for e in get_events(events, [ET.WARNING]):
AM.add(e, enabled)
# if user is not responsive to awareness alerts, then start a smooth deceleration
if awareness_status < -0.:
plan.aTargetMax = min(plan.aTargetMax, AWARENESS_DECEL)
plan.aTargetMin = min(plan.aTargetMin, plan.aTargetMax)
# *** angle offset learning ***
if rk.frame % 5 == 2 and plan.lateralValid:
# *** run this at 20hz again ***
angle_offset = learn_angle_offset(active, CS.vEgo, angle_offset,
PL.PP.c_poly, PL.PP.c_prob, LaC.y_des,
CS.steeringPressed)
# *** gas/brake PID loop ***
actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.brakePressed,
v_cruise_kph, plan.vTarget,
[plan.aTargetMin, plan.aTargetMax],
plan.jerkFactor, CP)
# *** steering PID loop ***
actuators.steer = LaC.update(active, CS.vEgo, CS.steeringAngle,
CS.steeringPressed, plan.dPoly, angle_offset, VM, PL)
# send a "steering required alert" if saturation count has reached the limit
if LaC.sat_flag:
AM.add("steerSaturated", enabled)
if CP.enableCruise and CS.cruiseState.enabled:
v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
# *** process alerts ***
AM.process_alerts(sec_since_boot())
return actuators, v_cruise_kph, awareness_status, angle_offset, rear_view_toggle
def data_send(plan, plan_ts, CS, CI, CP, state, events, actuators, v_cruise_kph, rk, carstate,
carcontrol, live100, livempc, AM, rear_view_allowed, rear_view_toggle, awareness_status,
LaC, LoC, angle_offset):
# ***** control the car *****
CC = car.CarControl.new_message()
CC.enabled = isEnabled(state)
CC.actuators = actuators
CC.cruiseControl.override = True
# always cancel if we have an interceptor
CC.cruiseControl.cancel = not CP.enableCruise or (not isEnabled(state) and CS.cruiseState.enabled)
# brake discount removes a sharp nonlinearity
brake_discount = (1.0 - clip(actuators.brake*3., 0.0, 1.0))
CC.cruiseControl.speedOverride = float(max(0.0, (LoC.v_pid + CS.cruiseState.speedOffset) * brake_discount) if CP.enableCruise else 0.0)
# TODO: parametrize 0.714 in interface?
# accelOverride is more or less the max throttle allowed to pcm: usually set to a constant
# unless aTargetMax is very high and then we scale with it; this helpw in quicker restart
CC.cruiseControl.accelOverride = float(max(0.714, plan.aTargetMax/A_ACC_MAX))
CC.hudControl.setSpeed = float(v_cruise_kph * CV.KPH_TO_MS)
CC.hudControl.speedVisible = isEnabled(state)
CC.hudControl.lanesVisible = isEnabled(state)
CC.hudControl.leadVisible = plan.hasLead
CC.hudControl.visualAlert = AM.visual_alert
CC.hudControl.audibleAlert = AM.audible_alert
# send car controls over can
CI.apply(CC)
# ***** publish state to logger *****
# publish controls state at 100Hz
dat = messaging.new_message()
dat.init('live100')
# show rear view camera on phone if in reverse gear or when button is pressed
dat.live100.rearViewCam = ('reverseGear' in [e.name for e in events] and rear_view_allowed) or rear_view_toggle
dat.live100.alertText1 = AM.alert_text_1
dat.live100.alertText2 = AM.alert_text_2
dat.live100.awarenessStatus = max(awareness_status, 0.0) if isEnabled(state) else 0.0
# what packets were used to process
dat.live100.canMonoTimes = list(CS.canMonoTimes)
dat.live100.planMonoTime = plan_ts
# if controls is enabled
dat.live100.enabled = isEnabled(state)
# car state
dat.live100.vEgo = CS.vEgo
dat.live100.angleSteers = CS.steeringAngle
dat.live100.steerOverride = CS.steeringPressed
# longitudinal control state
dat.live100.vPid = float(LoC.v_pid)
dat.live100.vCruise = float(v_cruise_kph)
dat.live100.upAccelCmd = float(LoC.pid.p)
dat.live100.uiAccelCmd = float(LoC.pid.i)
# lateral control state
dat.live100.yDes = float(LaC.y_des)
dat.live100.angleSteersDes = float(LaC.angle_steers_des)
dat.live100.upSteer = float(LaC.pid.p)
dat.live100.uiSteer = float(LaC.pid.i)
# processed radar state, should add a_pcm?
dat.live100.vTargetLead = float(plan.vTarget)
dat.live100.aTargetMin = float(plan.aTargetMin)
dat.live100.aTargetMax = float(plan.aTargetMax)
dat.live100.jerkFactor = float(plan.jerkFactor)
# log learned angle offset
dat.live100.angleOffset = float(angle_offset)
# lag
dat.live100.cumLagMs = -rk.remaining*1000.
live100.send(dat.to_bytes())
# broadcast carState
cs_send = messaging.new_message()
cs_send.init('carState')
# TODO: override CS.events with all the cumulated events
cs_send.carState = copy(CS)
carstate.send(cs_send.to_bytes())
# broadcast carControl
cc_send = messaging.new_message()
cc_send.init('carControl')
cc_send.carControl = copy(CC)
carcontrol.send(cc_send.to_bytes())
#print [i.name for i in events]
# publish mpc state at 20Hz
if hasattr(LaC, 'mpc_updated') and LaC.mpc_updated:
dat = messaging.new_message()
dat.init('live100')
dat.init('liveMpc')
dat.liveMpc.x = list(LaC.mpc_solution[0].x)
dat.liveMpc.y = list(LaC.mpc_solution[0].y)
dat.liveMpc.psi = list(LaC.mpc_solution[0].psi)
dat.liveMpc.delta = list(LaC.mpc_solution[0].delta)
livempc.send(dat.to_bytes())
# show rear view camera on phone if in reverse gear or when button is pressed
dat.live100.rearViewCam = ('reverseGear' in self.CS.errors and self.rear_view_allowed) or self.rear_view_toggle
dat.live100.alertText1 = self.alert_text_1
dat.live100.alertText2 = self.alert_text_2
dat.live100.awarenessStatus = max(self.awareness_status, 0.0) if self.enabled else 0.0
# what packets were used to process
dat.live100.canMonoTimes = list(self.CS.canMonoTimes)
dat.live100.planMonoTime = self.plan_ts
# if controls is enabled
dat.live100.enabled = self.enabled
# car state
dat.live100.vEgo = self.CS.vEgo
dat.live100.angleSteers = self.CS.steeringAngle
dat.live100.steerOverride = self.CS.steeringPressed
# longitudinal control state
dat.live100.vPid = float(self.LoC.v_pid)
dat.live100.vCruise = float(self.v_cruise_kph)
dat.live100.upAccelCmd = float(self.LoC.Up_accel_cmd)
dat.live100.uiAccelCmd = float(self.LoC.Ui_accel_cmd)
# lateral control state
dat.live100.yDes = float(self.LaC.y_des)
dat.live100.angleSteersDes = float(self.LaC.angle_steers_des)
dat.live100.upSteer = float(self.LaC.Up_steer)
dat.live100.uiSteer = float(self.LaC.Ui_steer)
# processed radar state, should add a_pcm?
dat.live100.vTargetLead = float(self.plan.vTarget)
dat.live100.aTargetMin = float(self.plan.aTargetMin)
dat.live100.aTargetMax = float(self.plan.aTargetMax)
dat.live100.jerkFactor = float(self.plan.jerkFactor)
# log learned angle offset
dat.live100.angleOffset = float(self.angle_offset)
# lag
dat.live100.cumLagMs = -self.rk.remaining*1000.
self.live100.send(dat.to_bytes())
self.prof.checkpoint("Live100")
def wait(self):
# *** run loop at fixed rate ***
if self.rk.keep_time():
self.prof.display()
return CC
def controlsd_thread(gctx, rate=100):
# start the loop
set_realtime_priority(2)
CTRLS = Controls(gctx, rate)
context = zmq.Context()
# pub
live100 = messaging.pub_sock(context, service_list['live100'].port)
carstate = messaging.pub_sock(context, service_list['carState'].port)
carcontrol = messaging.pub_sock(context, service_list['carControl'].port)
sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
livempc = messaging.pub_sock(context, service_list['liveMpc'].port)
# sub
thermal = messaging.sub_sock(context, service_list['thermal'].port)
health = messaging.sub_sock(context, service_list['health'].port)
cal = messaging.sub_sock(context, service_list['liveCalibration'].port)
logcan = messaging.sub_sock(context, service_list['can'].port)
CC = car.CarControl.new_message()
CI, CP = get_car(logcan, sendcan)
PL = Planner(CP)
LoC = LongControl(CI.compute_gb)
VM = VehicleModel(CP)
LaC = LatControl(VM)
AM = AlertManager()
# write CarParams
params = Params()
params.put("CarParams", CP.to_bytes())
state = State.DISABLED
soft_disable_timer = 0
v_cruise_kph = 255
cal_perc = 0
cal_status = Calibration.UNCALIBRATED
rear_view_toggle = False
rear_view_allowed = params.get("IsRearViewMirror") == "1"
# 0.0 - 1.0
awareness_status = 0.
rk = Ratekeeper(rate, print_delay_threshold=2./1000)
# learned angle offset
angle_offset = 0.
calibration_params = params.get("CalibrationParams")
if calibration_params:
try:
calibration_params = json.loads(calibration_params)
angle_offset = calibration_params["angle_offset"]
except (ValueError, KeyError):
pass
prof = Profiler()
while 1:
CTRLS.data_sample()
CTRLS.state_transition()
CTRLS.state_control()
CTRLS.data_send()
CTRLS.wait()
prof.reset() # avoid memory leak
# sample data and compute car events
CS, events, cal_status, cal_perc = data_sample(CI, CC, thermal, health, cal, cal_status, cal_perc)
prof.checkpoint("Sample")
# define plan
plan, plan_ts = calc_plan(CS, events, PL, LoC)
prof.checkpoint("Plan")
# update control state
state, soft_disable_timer, v_cruise_kph = state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, cal_perc, AM)
prof.checkpoint("State transition")
# compute actuators
actuators, v_cruise_kph, awareness_status, angle_offset, rear_view_toggle = state_control(plan, CS, CP, state, events, v_cruise_kph,
AM, rk, awareness_status, PL, LaC, LoC, VM, angle_offset,
rear_view_allowed, rear_view_toggle)
prof.checkpoint("State Control")
# publish data
CC = data_send(plan, plan_ts, CS, CI, CP, state, events, actuators, v_cruise_kph,
rk, carstate, carcontrol, live100, livempc, AM, rear_view_allowed,
rear_view_toggle, awareness_status, LaC, LoC, angle_offset)
prof.checkpoint("Sent")
# *** run loop at fixed rate ***
if rk.keep_time():
prof.display()
def main(gctx=None):
@@ -462,4 +507,3 @@ def main(gctx=None):
if __name__ == "__main__":
main()
selfdrive/controls/lib/adaptivecruise.py
+2 -10
View File
@@ -109,7 +109,7 @@ def calc_critical_decel(d_lead, v_rel, d_offset, v_offset):
# maximum acceleration adjustment
_A_CORR_BY_SPEED_V = [0.4, 0.4, 0]
# speeds
_A_CORR_BY_SPEED_BP = [0., 5., 20.]
_A_CORR_BY_SPEED_BP = [0., 2., 10.]
# max acceleration allowed in acc, which happens in restart
A_ACC_MAX = max(_A_CORR_BY_SPEED_V) + max(_A_CRUISE_MAX_V)
@@ -283,20 +283,12 @@ def compute_speed_with_leads(v_ego, angle_steers, v_pid, l1, l2, CP):
class AdaptiveCruise(object):
def __init__(self):
self.last_cal = 0.
self.l1, self.l2 = None, None
self.dead = True
def update(self, cur_time, v_ego, angle_steers, v_pid, CP, l20):
def update(self, v_ego, angle_steers, v_pid, CP, l20):
if l20 is not None:
self.l1 = l20.live20.leadOne
self.l2 = l20.live20.leadTwo
# TODO: no longer has anything to do with calibration
self.last_cal = cur_time
self.dead = False
elif cur_time - self.last_cal > 0.5:
self.dead = True
self.v_target_lead, self.a_target, self.a_pcm, self.jerk_factor = \
compute_speed_with_leads(v_ego, angle_steers, v_pid, self.l1, self.l2, CP)
self.has_lead = self.v_target_lead != MAX_SPEED_POSSIBLE
selfdrive/controls/lib/alertmanager.py
+319 -73
View File
@@ -2,22 +2,31 @@ from cereal import car
from selfdrive.swaglog import cloudlog
import copy
class ET:
ENABLE = 0
NO_ENTRY = 1
WARNING = 2
USER_DISABLE = 3
SOFT_DISABLE = 4
IMMEDIATE_DISABLE = 5
class alert(object):
def __init__(self, alert_text_1, alert_text_2, alert_type, visual_alert, audible_alert, duration_sound, duration_hud_alert, duration_text):
# Priority
class PT:
HIGH = 3
MID = 2
LOW = 1
class Alert(object):
def __init__(self,
alert_text_1,
alert_text_2,
alert_priority,
visual_alert,
audible_alert,
duration_sound,
duration_hud_alert,
duration_text):
self.alert_text_1 = alert_text_1
self.alert_text_2 = alert_text_2
self.alert_type = alert_type
self.alert_priority = alert_priority
self.visual_alert = visual_alert if visual_alert is not None else "none"
self.audible_alert = audible_alert if audible_alert is not None else "none"
self.duration_sound = duration_sound
self.duration_hud_alert = duration_hud_alert
self.duration_text = duration_text
@@ -28,45 +37,297 @@ class alert(object):
tst.hudControl.audibleAlert = self.audible_alert
def __str__(self):
return self.alert_text_1 + "/" + self.alert_text_2 + " " + str(self.alert_type) + " " + str(self.visual_alert) + " " + str(self.audible_alert)
return self.alert_text_1 + "/" + self.alert_text_2 + " " + str(self.alert_priority) + " " + str(
self.visual_alert) + " " + str(self.audible_alert)
def __gt__(self, alert2):
return self.alert_type > alert2.alert_type
return self.alert_priority > alert2.alert_priority
class AlertManager(object):
alerts = {
"enable": alert("", "", ET.ENABLE, None, "beepSingle", .2, 0., 0.),
"disable": alert("", "", ET.USER_DISABLE, None, "beepSingle", .2, 0., 0.),
"pedalPressed": alert("Comma Unavailable", "Pedal Pressed", ET.NO_ENTRY, "brakePressed", "chimeDouble", .4, 2., 3.),
"preDriverDistracted": alert("Take Control ", "User Distracted", ET.WARNING, "steerRequired", "chimeDouble", .4, 2., 3.),
"driverDistracted": alert("Take Control to Regain Speed", "User Distracted", ET.WARNING, "steerRequired", "chimeRepeated", .5, .5, .5),
"steerSaturated": alert("Take Control", "Turn Exceeds Limit", ET.WARNING, "steerRequired", "chimeSingle", 1., 2., 3.),
"overheat": alert("Take Control Immediately", "System Overheated", ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"controlsMismatch": alert("Take Control Immediately", "Controls Mismatch", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"radarCommIssue": alert("Take Control Immediately", "Radar Error: Restart the Car",ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"calibrationInvalid": alert("Take Control Immediately", "Calibration Invalid: Reposition Neo and Recalibrate", ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"calibrationInProgress": alert("Take Control Immediately", "Calibration in Progress: ", ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"modelCommIssue": alert("Take Control Immediately", "Model Error: Restart the Car",ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"controlsFailed": alert("Take Control Immediately", "Controls Failed", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"fcw": alert("", "", ET.WARNING, None, None, .1, .1, .1),
# car errors
"commIssue": alert("Take Control Immediately","CAN Error: Restart the Car", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"steerUnavailable": alert("Take Control Immediately","Steer Error: Restart the Car", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"steerTempUnavailable": alert("Take Control", "Steer Temporarily Unavailable", ET.WARNING, "steerRequired", "chimeDouble", .4, 2., 3.),
"brakeUnavailable": alert("Take Control Immediately","Brake Error: Restart the Car", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"gasUnavailable": alert("Take Control Immediately","Gas Error: Restart the Car", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"wrongGear": alert("Take Control Immediately","Gear not D", ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"reverseGear": alert("Take Control Immediately","Car in Reverse", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"doorOpen": alert("Take Control Immediately","Door Open", ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"seatbeltNotLatched": alert("Take Control Immediately","Seatbelt Unlatched", ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"espDisabled": alert("Take Control Immediately","ESP Off", ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"cruiseDisabled": alert("Take Control Immediately","Cruise Is Off", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"wrongCarMode": alert("Comma Unavailable","Main Switch Off", ET.NO_ENTRY, None, "chimeDouble", .4, 0., 3.),
"outOfSpace": alert("Comma Unavailable","Out of Space", ET.NO_ENTRY, None, "chimeDouble", .4, 0., 3.),
"dataNeeded": alert("Comma Unavailable","Data needed for calibration. Upload drive, try again", ET.NO_ENTRY, None, "chimeDouble", .4, 0., 3.),
"ethicalDilemma": alert("Take Control Immediately","Ethical Dilemma Detected", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"startup": alert("Always Keep Hands on Wheel","Be Ready to Take Over Any Time", ET.NO_ENTRY, None, None, 0., 0., 15.),
# Miscellaneous alerts
"enable": Alert(
"",
"",
PT.MID, None, "beepSingle", .2, 0., 0.),
"disable": Alert(
"",
"",
PT.MID, None, "beepSingle", .2, 0., 0.),
"fcw": Alert(
"",
"",
PT.LOW, None, None, .1, .1, .1),
"steerSaturated": Alert(
"Take Control",
"Turn Exceeds Limit",
PT.LOW, "steerRequired", "chimeSingle", 1., 2., 3.),
"steerTempUnavailable": Alert(
"Take Control",
"Steer Temporarily Unavailable",
PT.LOW, "steerRequired", "chimeDouble", .4, 2., 3.),
"preDriverDistracted": Alert(
"Take Control ",
"User Distracted",
PT.LOW, "steerRequired", "chimeDouble", .4, 2., 3.),
"driverDistracted": Alert(
"Take Control to Regain Speed",
"User Distracted",
PT.LOW, "steerRequired", "chimeRepeated", .5, .5, .5),
"startup": Alert(
"Always Keep Hands on Wheel",
"Be Ready to Take Over Any Time",
PT.LOW, None, None, 0., 0., 15.),
"ethicalDilemma": Alert(
"Take Control Immediately",
"Ethical Dilemma Detected",
PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
"steerTempUnavailableNoEntry": Alert(
"Comma Unavailable",
"Steer Temporary Unavailable",
PT.LOW, None, "chimeDouble", .4, 0., 3.),
# Non-entry only alerts
"wrongCarModeNoEntry": Alert(
"Comma Unavailable",
"Main Switch Off",
PT.LOW, None, "chimeDouble", .4, 0., 3.),
"dataNeededNoEntry": Alert(
"Comma Unavailable",
"Data needed for calibration. Upload drive, try again",
PT.LOW, None, "chimeDouble", .4, 0., 3.),
"outOfSpaceNoEntry": Alert(
"Comma Unavailable",
"Out of Space",
PT.LOW, None, "chimeDouble", .4, 0., 3.),
"pedalPressedNoEntry": Alert(
"Comma Unavailable",
"Pedal Pressed",
PT.LOW, "brakePressed", "chimeDouble", .4, 2., 3.),
"speedTooLowNoEntry": Alert(
"Comma Unavailable",
"Speed Too Low",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"brakeHoldNoEntry": Alert(
"Comma Unavailable",
"Brake Hold Active",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"parkBrakeNoEntry": Alert(
"Comma Unavailable",
"Park Brake Engaged",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
# Cancellation alerts causing disabling
"overheat": Alert(
"Take Control Immediately",
"System Overheated",
PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
"wrongGear": Alert(
"Take Control Immediately",
"Gear not D",
PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
"calibrationInvalid": Alert(
"Take Control Immediately",
"Calibration Invalid: Reposition Neo and Recalibrate",
PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
"calibrationInProgress": Alert(
"Take Control Immediately",
"Calibration in Progress",
PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
"doorOpen": Alert(
"Take Control Immediately",
"Door Open",
PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
"seatbeltNotLatched": Alert(
"Take Control Immediately",
"Seatbelt Unlatched",
PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
"espDisabled": Alert(
"Take Control Immediately",
"ESP Off",
PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
"radarCommIssue": Alert(
"Take Control Immediately",
"Radar Error: Restart the Car",
PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
"radarFault": Alert(
"Take Control Immediately",
"Radar Error: Restart the Car",
PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
"modelCommIssue": Alert(
"Take Control Immediately",
"Model Error: Restart the Car",
PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
"controlsFailed": Alert(
"Take Control Immediately",
"Controls Failed",
PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
"controlsMismatch": Alert(
"Take Control Immediately",
"Controls Mismatch",
PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
"commIssue": Alert(
"Take Control Immediately",
"CAN Error: Restart the Car",
PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
"steerUnavailable": Alert(
"Take Control Immediately",
"Steer Error: Restart the Car",
PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
"brakeUnavailable": Alert(
"Take Control Immediately",
"Brake Error: Restart the Car",
PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
"gasUnavailable": Alert(
"Take Control Immediately",
"Gas Error: Restart the Car",
PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
"reverseGear": Alert(
"Take Control Immediately",
"Reverse Gear",
PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
"cruiseDisabled": Alert(
"Take Control Immediately",
"Cruise Is Off",
PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
# not loud cancellations (user is in control)
"noTarget": Alert(
"Comma Canceled",
"No Close Lead",
PT.HIGH, None, "chimeDouble", .4, 2., 3.),
# Cancellation alerts causing non-entry
"overheatNoEntry": Alert(
"Comma Unavailable",
"System Overheated",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"wrongGearNoEntry": Alert(
"Comma Unavailable",
"Gear not D",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"calibrationInvalidNoEntry": Alert(
"Comma Unavailable",
"Calibration Invalid: Reposition Neo and Recalibrate",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"calibrationInProgressNoEntry": Alert(
"Comma Unavailable",
"Calibration in Progress: ",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"doorOpenNoEntry": Alert(
"Comma Unavailable",
"Door Open",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"seatbeltNotLatchedNoEntry": Alert(
"Comma Unavailable",
"Seatbelt Unlatched",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"espDisabledNoEntry": Alert(
"Comma Unavailable",
"ESP Off",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"radarCommIssueNoEntry": Alert(
"Comma Unavailable",
"Radar Error: Restart the Car",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"radarFaultNoEntry": Alert(
"Comma Unavailable",
"Radar Error: Restart the Car",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"modelCommIssueNoEntry": Alert(
"Comma Unavailable",
"Model Error: Restart the Car",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"controlsFailedNoEntry": Alert(
"Comma Unavailable",
"Controls Failed",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"controlsMismatchNoEntry": Alert(
"Comma Unavailable",
"Controls Mismatch",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"commIssueNoEntry": Alert(
"Comma Unavailable",
"CAN Error: Restart the Car",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"steerUnavailableNoEntry": Alert(
"Comma Unavailable",
"Steer Error: Restart the Car",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"brakeUnavailableNoEntry": Alert(
"Comma Unavailable",
"Brake Error: Restart the Car",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"gasUnavailableNoEntry": Alert(
"Comma Unavailable",
"Gas Error: Restart the Car",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"reverseGearNoEntry": Alert(
"Comma Unavailable",
"Reverse Gear",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"cruiseDisabledNoEntry": Alert(
"Comma Unavailable",
"Cruise is Off",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"noTargetNoEntry": Alert(
"Comma Unavailable",
"No Close Lead",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
}
def __init__(self):
self.activealerts = []
self.current_alert = None
@@ -75,31 +336,16 @@ class AlertManager(object):
def alertPresent(self):
return len(self.activealerts) > 0
def alertShouldSoftDisable(self):
return len(self.activealerts) > 0 and any(a.alert_type == ET.SOFT_DISABLE for a in self.activealerts)
def alertShouldDisable(self):
return len(self.activealerts) > 0 and any(a.alert_type in [ET.IMMEDIATE_DISABLE, ET.USER_DISABLE] for a in self.activealerts)
def add(self, alert_type, enabled=True, extra_text=''):
alert_type = str(alert_type)
this_alert = copy.copy(self.alerts[alert_type])
this_alert.alert_text_2 += extra_text
# downgrade the alert if we aren't enabled, except if it's FCW, which remains the same
# TODO: remove this 'if' by adding more alerts
if not enabled and this_alert.alert_type in [ET.WARNING, ET.SOFT_DISABLE, ET.IMMEDIATE_DISABLE] \
and this_alert != self.alerts['fcw']:
this_alert = alert("Comma Unavailable" if this_alert.alert_text_1 != "" else "", this_alert.alert_text_2, ET.NO_ENTRY, None, "chimeDouble", .4, 0., 3.)
# ignore no entries if we are enabled
if enabled and this_alert.alert_type in [ET.ENABLE, ET.NO_ENTRY]:
return
# if new alert is higher priority, log it
if self.current_alert is None or this_alert > self.current_alert:
cloudlog.event('alert_add',
alert_type=alert_type,
enabled=enabled)
alert_type=alert_type,
enabled=enabled)
self.activealerts.append(this_alert)
self.activealerts.sort()
@@ -109,29 +355,29 @@ class AlertManager(object):
self.alert_start_time = cur_time
self.current_alert = self.activealerts[0]
print self.current_alert
alert_text_1 = ""
alert_text_2 = ""
visual_alert = "none"
audible_alert = "none"
# start with assuming no alerts
self.alert_text_1 = ""
self.alert_text_2 = ""
self.visual_alert = "none"
self.audible_alert = "none"
if self.current_alert is not None:
# ewwwww
if self.alert_start_time + self.current_alert.duration_sound > cur_time:
audible_alert = self.current_alert.audible_alert
self.audible_alert = self.current_alert.audible_alert
if self.alert_start_time + self.current_alert.duration_hud_alert > cur_time:
visual_alert = self.current_alert.visual_alert
self.visual_alert = self.current_alert.visual_alert
if self.alert_start_time + self.current_alert.duration_text > cur_time:
alert_text_1 = self.current_alert.alert_text_1
alert_text_2 = self.current_alert.alert_text_2
self.alert_text_1 = self.current_alert.alert_text_1
self.alert_text_2 = self.current_alert.alert_text_2
# disable current alert
if self.alert_start_time + max(self.current_alert.duration_sound, self.current_alert.duration_hud_alert, self.current_alert.duration_text) < cur_time:
if self.alert_start_time + max(self.current_alert.duration_sound, self.current_alert.duration_hud_alert,
self.current_alert.duration_text) < cur_time:
self.current_alert = None
# reset
self.activealerts = []
return alert_text_1, alert_text_2, visual_alert, audible_alert
selfdrive/controls/lib/drive_helpers.py
+30
View File
@@ -1,8 +1,38 @@
from common.numpy_fast import clip
from cereal import car
class EventTypes:
ENABLE = 'enable'
PRE_ENABLE = 'preEnable'
NO_ENTRY = 'noEntry'
WARNING = 'warning'
USER_DISABLE = 'userDisable'
SOFT_DISABLE = 'softDisable'
IMMEDIATE_DISABLE = 'immediateDisable'
def create_event(name, types):
event = car.CarEvent.new_message()
event.name = name
for t in types:
setattr(event, t, True)
return event
def get_events(events, types):
out = []
for e in events:
for t in types:
if getattr(e, t):
out.append(e.name)
return out
def rate_limit(new_value, last_value, dw_step, up_step):
return clip(new_value, last_value + dw_step, last_value + up_step)
def learn_angle_offset(lateral_control, v_ego, angle_offset, c_poly, c_prob, y_des, steer_override):
# simple integral controller that learns how much steering offset to put to have the car going straight
# while being in the middle of the lane
selfdrive/controls/lib/latcontrol.py
+60 -113
View File
@@ -1,133 +1,80 @@
import math
import numpy as np
from selfdrive.controls.lib.pid import PIController
from selfdrive.controls.lib.lateral_mpc import libmpc_py
from common.numpy_fast import clip, interp
from selfdrive.config import Conversions as CV
_K_CURV_V = [1., 0.6]
_K_CURV_BP = [0., 0.002]
def calc_d_lookahead(v_ego, d_poly):
#*** this function computes how far too look for lateral control
# howfar we look ahead is function of speed and how much curvy is the path
offset_lookahead = 1.
k_lookahead = 7.
# integrate abs value of second derivative of poly to get a measure of path curvature
pts_len = 50. # m
if len(d_poly)>0:
pts = np.polyval([6*d_poly[0], 2*d_poly[1]], np.arange(0, pts_len))
else:
pts = 0.
curv = np.sum(np.abs(pts))/pts_len
# 100ms is a rule of thumb estimation of lag from image processing to actuator command
ACTUATORS_DELAY = 0.1
k_curv = interp(curv, _K_CURV_BP, _K_CURV_V)
# sqrt on speed is needed to keep, for a given curvature, the y_des
# proportional to speed. Indeed, y_des is prop to d_lookahead^2
# 36m at 25m/s
d_lookahead = offset_lookahead + math.sqrt(max(v_ego, 0)) * k_lookahead * k_curv
return d_lookahead
def calc_states_after_delay(states, v_ego, steer_angle, curvature_factor, steer_ratio):
states[0].x = v_ego * ACTUATORS_DELAY
states[0].psi = v_ego * curvature_factor * math.radians(steer_angle) / steer_ratio * ACTUATORS_DELAY
return states
def calc_lookahead_offset(v_ego, angle_steers, d_lookahead, VM, angle_offset):
#*** this function returns the lateral offset given the steering angle, speed and the lookahead distance
sa = (angle_steers - angle_offset) * CV.DEG_TO_RAD
curvature = VM.calc_curvature(sa, v_ego)
# clip is to avoid arcsin NaNs due to too sharp turns
y_actual = d_lookahead * np.tan(np.arcsin(np.clip(d_lookahead * curvature, -0.999, 0.999))/2.)
return y_actual, curvature
def calc_desired_steer_angle(v_ego, y_des, d_lookahead, VM, angle_offset):
# inverse of the above function
curvature = np.sin(np.arctan(y_des / d_lookahead) * 2.) / d_lookahead
steer_des = VM.get_steer_from_curvature(curvature, v_ego) * CV.RAD_TO_DEG + angle_offset
return steer_des, curvature
def get_steer_max(CP, v_ego):
return interp(v_ego, CP.steerMaxBP, CP.steerMaxV)
def pid_lateral_control(v_ego, sa_actual, sa_des, Ui_steer, steer_max,
steer_override, sat_count, enabled, Kp, Ki, rate):
sat_count_rate = 1./rate
sat_count_limit = 0.8 # after 0.8s of continuous saturation, an alert will be sent
error_steer = sa_des - sa_actual
Ui_unwind_speed = 0.3/rate #.3 per second
Up_steer = error_steer*Kp
Ui_steer_new = Ui_steer + error_steer*Ki * 1./rate
output_steer_new = Ui_steer_new + Up_steer
# Anti-wind up for integrator: do not integrate if we are against the steer limits
if (
(error_steer >= 0. and (output_steer_new < steer_max or Ui_steer < 0)) or
(error_steer <= 0. and
(output_steer_new > -steer_max or Ui_steer > 0))) and not steer_override:
#update integrator
Ui_steer = Ui_steer_new
# unwind integrator if driver is maneuvering the steering wheel
elif steer_override:
Ui_steer -= Ui_unwind_speed * np.sign(Ui_steer)
# still, intergral term should not be bigger then limits
Ui_steer = clip(Ui_steer, -steer_max, steer_max)
output_steer = Up_steer + Ui_steer
# don't run steer control if at very low speed
if v_ego < 0.3 or not enabled:
output_steer = 0.
Ui_steer = 0.
# useful to know if control is against the limit
lateral_control_sat = False
if abs(output_steer) > steer_max:
lateral_control_sat = True
output_steer = clip(output_steer, -steer_max, steer_max)
# if lateral control is saturated for a certain period of time, send an alert for taking control of the car
# wind
if lateral_control_sat and not steer_override and v_ego > 10 and abs(error_steer) > 0.1:
sat_count += sat_count_rate
# unwind
else:
sat_count -= sat_count_rate
sat_flag = False
if sat_count >= sat_count_limit:
sat_flag = True
sat_count = clip(sat_count, 0, 1)
return output_steer, Up_steer, Ui_steer, lateral_control_sat, sat_count, sat_flag
class LatControl(object):
def __init__(self):
self.Up_steer = 0.
self.sat_count = 0
self.y_des = 0.0
self.lateral_control_sat = False
self.Ui_steer = 0.
self.reset()
def __init__(self, VM):
self.pid = PIController(VM.CP.steerKp, VM.CP.steerKi, pos_limit=1.0)
self.setup_mpc()
self.y_des = -1 # Legacy
def setup_mpc(self):
self.libmpc = libmpc_py.libmpc
self.libmpc.init()
self.mpc_solution = libmpc_py.ffi.new("log_t *")
self.cur_state = libmpc_py.ffi.new("state_t *")
self.mpc_updated = False
self.cur_state[0].x = 0.0
self.cur_state[0].y = 0.0
self.cur_state[0].psi = 0.0
self.cur_state[0].delta = 0.0
self.last_mpc_ts = 0.0
self.angle_steers_des = 0
def reset(self):
self.Ui_steer = 0.
self.pid.reset()
def update(self, enabled, v_ego, angle_steers, steer_override, d_poly, angle_offset, VM):
rate = 100
def update(self, active, v_ego, angle_steers, steer_override, d_poly, angle_offset, VM, PL):
self.mpc_updated = False
if self.last_mpc_ts + 0.001 < PL.last_md_ts:
self.last_mpc_ts = PL.last_md_ts
steer_max = 1.0
curvature_factor = VM.curvature_factor(v_ego)
# how far we look ahead is function of speed and desired path
d_lookahead = calc_d_lookahead(v_ego, d_poly)
l_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.l_poly))
r_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.r_poly))
p_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.p_poly))
# desired lookahead offset
self.y_des = np.polyval(d_poly, d_lookahead)
# account for actuation delay
self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers, curvature_factor, VM.CP.sR)
# calculate actual offset at the lookahead point
self.angle_steers_des, _ = calc_desired_steer_angle(v_ego, self.y_des,
d_lookahead, VM, angle_offset)
self.libmpc.run_mpc(self.cur_state, self.mpc_solution,
l_poly, r_poly, p_poly,
PL.PP.l_prob, PL.PP.r_prob, PL.PP.p_prob, curvature_factor, v_ego, PL.PP.lane_width)
output_steer, self.Up_steer, self.Ui_steer, self.lateral_control_sat, self.sat_count, sat_flag = pid_lateral_control(
v_ego, angle_steers, self.angle_steers_des, self.Ui_steer, steer_max,
steer_override, self.sat_count, enabled, VM.CP.steerKp, VM.CP.steerKi, rate)
delta_desired = self.mpc_solution[0].delta[1]
self.cur_state[0].delta = delta_desired
final_steer = clip(output_steer, -steer_max, steer_max)
return final_steer, sat_flag
self.angle_steers_des = math.degrees(delta_desired * VM.CP.sR) + angle_offset
self.mpc_updated = True
if v_ego < 0.3 or not active:
output_steer = 0.0
self.pid.reset()
else:
steer_max = get_steer_max(VM.CP, v_ego)
self.pid.pos_limit = steer_max
self.pid.neg_limit = -steer_max
output_steer = self.pid.update(self.angle_steers_des, angle_steers, check_saturation=(v_ego > 10), override=steer_override)
self.sat_flag = self.pid.saturated
return output_steer
selfdrive/controls/lib/latcontrol_helpers.py
+89
View File
@@ -0,0 +1,89 @@
import numpy as np
import math
from common.numpy_fast import interp
_K_CURV_V = [1., 0.6]
_K_CURV_BP = [0., 0.002]
# lane width http://safety.fhwa.dot.gov/geometric/pubs/mitigationstrategies/chapter3/3_lanewidth.cfm
_LANE_WIDTH_V = [3., 3.8]
# break points of speed
_LANE_WIDTH_BP = [0., 31.]
def calc_d_lookahead(v_ego, d_poly):
# this function computes how far too look for lateral control
# howfar we look ahead is function of speed and how much curvy is the path
offset_lookahead = 1.
k_lookahead = 7.
# integrate abs value of second derivative of poly to get a measure of path curvature
pts_len = 50. # m
if len(d_poly) > 0:
pts = np.polyval([6 * d_poly[0], 2 * d_poly[1]], np.arange(0, pts_len))
else:
pts = 0.
curv = np.sum(np.abs(pts)) / pts_len
k_curv = interp(curv, _K_CURV_BP, _K_CURV_V)
# sqrt on speed is needed to keep, for a given curvature, the y_des
# proportional to speed. Indeed, y_des is prop to d_lookahead^2
# 36m at 25m/s
d_lookahead = offset_lookahead + math.sqrt(max(v_ego, 0)) * k_lookahead * k_curv
return d_lookahead
def calc_lookahead_offset(v_ego, angle_steers, d_lookahead, VM, angle_offset):
# this function returns the lateral offset given the steering angle, speed and the lookahead distance
sa = math.radians(angle_steers - angle_offset)
curvature = VM.calc_curvature(sa, v_ego)
# clip is to avoid arcsin NaNs due to too sharp turns
y_actual = d_lookahead * np.tan(np.arcsin(np.clip(d_lookahead * curvature, -0.999, 0.999)) / 2.)
return y_actual, curvature
def calc_desired_steer_angle(v_ego, y_des, d_lookahead, VM, angle_offset):
# inverse of the above function
curvature = np.sin(np.arctan(y_des / d_lookahead) * 2.) / d_lookahead
steer_des = math.degrees(VM.get_steer_from_curvature(curvature, v_ego)) + angle_offset
return steer_des, curvature
def compute_path_pinv():
deg = 3
x = np.arange(50.0)
X = np.vstack(tuple(x**n for n in range(deg, -1, -1))).T
pinv = np.linalg.pinv(X)
return pinv
def model_polyfit(points, path_pinv):
return np.dot(path_pinv, map(float, points))
def calc_desired_path(l_poly,
r_poly,
p_poly,
l_prob,
r_prob,
p_prob,
speed,
lane_width=None):
# this function computes the poly for the center of the lane, averaging left and right polys
if lane_width is None:
lane_width = interp(speed, _LANE_WIDTH_BP, _LANE_WIDTH_V)
# lanes in US are ~3.6m wide
half_lane_poly = np.array([0., 0., 0., lane_width / 2.])
if l_prob + r_prob > 0.01:
c_poly = ((l_poly - half_lane_poly) * l_prob +
(r_poly + half_lane_poly) * r_prob) / (l_prob + r_prob)
c_prob = l_prob + r_prob - l_prob * r_prob
else:
c_poly = np.zeros(4)
c_prob = 0.
p_weight = 1. # predicted path weight relatively to the center of the lane
d_poly = list((c_poly * c_prob + p_poly * p_prob * p_weight) / (c_prob + p_prob * p_weight))
return d_poly, c_poly, c_prob
selfdrive/controls/lib/lateral_mpc/Makefile
+75
View File
@@ -0,0 +1,75 @@
CC = clang
CXX = clang++
PHONELIBS = ../../../../phonelibs
UNAME_M := $(shell uname -m)
CFLAGS = -O3 -fPIC -I.
CXXFLAGS = -O3 -fPIC -I.
QPOASES_FLAGS = -I$(PHONELIBS)/qpoases -I$(PHONELIBS)/qpoases/INCLUDE -I$(PHONELIBS)/qpoases/SRC
ACADO_FLAGS = -I$(PHONELIBS)/acado/include -I$(PHONELIBS)/acado/include/acado
ifeq ($(UNAME_M),aarch64)
ACADO_LIBS := -L $(PHONELIBS)/acado/aarch64/lib -l:libacado_toolkit.a -l:libacado_casadi.a -l:libacado_csparse.a
else
ACADO_LIBS := -L $(PHONELIBS)/acado/x64/lib -l:libacado_toolkit.a -l:libacado_casadi.a -l:libacado_csparse.a
endif
OBJS = \
$(PHONELIBS)/qpoases/SRC/Bounds.o \
$(PHONELIBS)/qpoases/SRC/Constraints.o \
$(PHONELIBS)/qpoases/SRC/CyclingManager.o \
$(PHONELIBS)/qpoases/SRC/Indexlist.o \
$(PHONELIBS)/qpoases/SRC/MessageHandling.o \
$(PHONELIBS)/qpoases/SRC/QProblem.o \
$(PHONELIBS)/qpoases/SRC/QProblemB.o \
$(PHONELIBS)/qpoases/SRC/SubjectTo.o \
$(PHONELIBS)/qpoases/SRC/Utils.o \
$(PHONELIBS)/qpoases/SRC/EXTRAS/SolutionAnalysis.o \
mpc_export/acado_qpoases_interface.o \
mpc_export/acado_integrator.o \
mpc_export/acado_solver.o \
mpc_export/acado_auxiliary_functions.o \
mpc.o
DEPS := $(OBJS:.o=.d)
.PHONY: all
all: libcommampc.so
libcommampc.so: $(OBJS)
$(CXX) -shared -o '$@' $^ -lm
%.o: %.cpp
@echo "[ CXX ] $@"
$(CXX) $(CXXFLAGS) -MMD \
-I mpc_export/ \
$(QPOASES_FLAGS) \
-c -o '$@' '$<'
%.o: %.c
@echo "[ CC ] $@"
$(CC) $(CFLAGS) -MMD \
-I mpc_export/ \
$(QPOASES_FLAGS) \
-c -o '$@' '$<'
generator: generator.cpp
$(CXX) -Wall -std=c++11 \
generator.cpp \
-o generator \
$(ACADO_FLAGS) \
$(ACADO_LIBS)
.PHONY: generate
generate: generator
./generator
.PHONY: clean
clean:
rm -f libcommampc.so generator $(OBJS) $(DEPS)
-include $(DEPS)
selfdrive/controls/lib/lateral_mpc/__init__.py
View File
selfdrive/controls/lib/lateral_mpc/generator.cpp
+137
View File
@@ -0,0 +1,137 @@
#include <acado_code_generation.hpp>
#define PI 3.1415926536
#define deg2rad(d) (d/180.0*PI)
const int controlHorizon = 50;
const double samplingTime = 0.05; // 20 Hz
using namespace std;
int main( )
{
USING_NAMESPACE_ACADO
DifferentialEquation f;
DifferentialState xx; // x position
DifferentialState yy; // y position
DifferentialState psi; // vehicle heading
DifferentialState delta;
OnlineData curvature_factor;
OnlineData v_ref; // m/s
OnlineData l_poly_r0, l_poly_r1, l_poly_r2, l_poly_r3;
OnlineData r_poly_r0, r_poly_r1, r_poly_r2, r_poly_r3;
OnlineData p_poly_r0, p_poly_r1, p_poly_r2, p_poly_r3;
OnlineData l_prob, r_prob, p_prob;
OnlineData lane_width;
Control t;
// Equations of motion
f << dot(xx) == v_ref * cos(psi);
f << dot(yy) == v_ref * sin(psi);
f << dot(psi) == v_ref * delta * curvature_factor;
f << dot(delta) == t;
auto lr_prob = l_prob + r_prob - l_prob * r_prob;
auto poly_l = l_poly_r0*(xx*xx*xx) + l_poly_r1*(xx*xx) + l_poly_r2*xx + l_poly_r3;
auto poly_r = r_poly_r0*(xx*xx*xx) + r_poly_r1*(xx*xx) + r_poly_r2*xx + r_poly_r3;
auto poly_p = p_poly_r0*(xx*xx*xx) + p_poly_r1*(xx*xx) + p_poly_r2*xx + p_poly_r3;
auto angle_l = atan(3*l_poly_r0*xx*xx + 2*l_poly_r1*xx + l_poly_r2);
auto angle_r = atan(3*r_poly_r0*xx*xx + 2*r_poly_r1*xx + r_poly_r2);
auto angle_p = atan(3*p_poly_r0*xx*xx + 2*p_poly_r1*xx + p_poly_r2);
auto c_left_lane = exp(-(poly_l - yy));
auto c_right_lane = exp(poly_r - yy);
auto r_phantom = poly_l - lane_width/2.0;
auto l_phantom = poly_r + lane_width/2.0;
auto path = lr_prob * (l_prob * r_phantom + r_prob * l_phantom) / (l_prob + r_prob + 0.0001)
+ (1-lr_prob) * poly_p;
auto angle = lr_prob * (l_prob * angle_l + r_prob * angle_r) / (l_prob + r_prob + 0.0001)
+ (1-lr_prob) * angle_p;
// Running cost
Function h;
// Distance errors
h << path - yy;
h << l_prob * c_left_lane;
h << r_prob * c_right_lane;
// Heading error
h << (v_ref + 1.0 ) * (angle - psi);
// Angular rate error
h << (v_ref + 1.0 ) * t;
DMatrix Q(5,5);
Q(0,0) = 1.0;
Q(1,1) = 1.0;
Q(2,2) = 1.0;
Q(3,3) = 1.0;
Q(4,4) = 1.0;
// Terminal cost
Function hN;
// Distance errors
hN << path - yy;
hN << l_prob * c_left_lane;
hN << r_prob * c_right_lane;
// Heading errors
hN << (2.0 * v_ref + 1.0 ) * (angle - psi);
DMatrix QN(4,4);
QN(0,0) = 1.0;
QN(1,1) = 1.0;
QN(2,2) = 1.0;
QN(3,3) = 1.0;
// Setup Optimal Control Problem
const double tStart = 0.0;
const double tEnd = samplingTime * controlHorizon;
OCP ocp( tStart, tEnd, controlHorizon );
ocp.subjectTo(f);
ocp.minimizeLSQ(Q, h);
ocp.minimizeLSQEndTerm(QN, hN);
ocp.subjectTo( deg2rad(-90) <= psi <= deg2rad(90));
ocp.subjectTo( deg2rad(-25) <= delta <= deg2rad(25));
ocp.subjectTo( -0.1 <= t <= 0.1);
ocp.setNOD(18);
OCPexport mpc(ocp);
mpc.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON );
mpc.set( DISCRETIZATION_TYPE, MULTIPLE_SHOOTING );
mpc.set( INTEGRATOR_TYPE, INT_RK4 );
mpc.set( NUM_INTEGRATOR_STEPS, 250 );
mpc.set( SPARSE_QP_SOLUTION, CONDENSING );
mpc.set( QP_SOLVER, QP_QPOASES );
mpc.set( HOTSTART_QP, YES );
mpc.set( GENERATE_TEST_FILE, NO);
mpc.set( GENERATE_MAKE_FILE, NO );
mpc.set( GENERATE_MATLAB_INTERFACE, NO );
mpc.set( GENERATE_SIMULINK_INTERFACE, NO );
if (mpc.exportCode( "mpc_export" ) != SUCCESSFUL_RETURN)
exit( EXIT_FAILURE );
mpc.printDimensionsQP( );
return EXIT_SUCCESS;
}
selfdrive/controls/lib/lateral_mpc/libmpc_py.py
+30
View File
@@ -0,0 +1,30 @@
import os
import subprocess
from cffi import FFI
mpc_dir = os.path.dirname(os.path.abspath(__file__))
libmpc_fn = os.path.join(mpc_dir, "libcommampc.so")
subprocess.check_output(["make", "-j4"], cwd=mpc_dir)
ffi = FFI()
ffi.cdef("""
typedef struct {
double x, y, psi, delta, t;
} state_t;
typedef struct {
double x[50];
double y[50];
double psi[50];
double delta[50];
} log_t;
void init();
void run_mpc(state_t * x0, log_t * solution,
double l_poly[4], double r_poly[4], double p_poly[4],
double l_prob, double r_prob, double p_prob, double curvature_factor, double v_ref, double lane_width);
""")
libmpc = ffi.dlopen(libmpc_fn)
selfdrive/controls/lib/lateral_mpc/mpc.c
+100
View File
@@ -0,0 +1,100 @@
#include "acado_common.h"
#include "acado_auxiliary_functions.h"
#include <stdio.h>
#define NX ACADO_NX /* Number of differential state variables. */
#define NXA ACADO_NXA /* Number of algebraic variables. */
#define NU ACADO_NU /* Number of control inputs. */
#define NOD ACADO_NOD /* Number of online data values. */
#define NY ACADO_NY /* Number of measurements/references on nodes 0..N - 1. */
#define NYN ACADO_NYN /* Number of measurements/references on node N. */
#define N ACADO_N /* Number of intervals in the horizon. */
ACADOvariables acadoVariables;
ACADOworkspace acadoWorkspace;
typedef struct {
double x, y, psi, delta, t;
} state_t;
typedef struct {
double x[N];
double y[N];
double psi[N];
double delta[N];
} log_t;
void init(){
acado_initializeSolver();
int i;
/* Initialize the states and controls. */
for (i = 0; i < NX * (N + 1); ++i) acadoVariables.x[ i ] = 0.0;
for (i = 0; i < NU * N; ++i) acadoVariables.u[ i ] = 0.1;
/* Initialize the measurements/reference. */
for (i = 0; i < NY * N; ++i) acadoVariables.y[ i ] = 0.0;
for (i = 0; i < NYN; ++i) acadoVariables.yN[ i ] = 0.0;
/* MPC: initialize the current state feedback. */
for (i = 0; i < NX; ++i) acadoVariables.x0[ i ] = 0.0;
}
void run_mpc(state_t * x0, log_t * solution,
double l_poly[4], double r_poly[4], double p_poly[4],
double l_prob, double r_prob, double p_prob, double curvature_factor, double v_ref, double lane_width){
int i;
for (i = 0; i <= NOD * N; i+= NOD){
acadoVariables.od[i] = curvature_factor;
acadoVariables.od[i+1] = v_ref;
acadoVariables.od[i+2] = l_poly[0];
acadoVariables.od[i+3] = l_poly[1];
acadoVariables.od[i+4] = l_poly[2];
acadoVariables.od[i+5] = l_poly[3];
acadoVariables.od[i+6] = r_poly[0];
acadoVariables.od[i+7] = r_poly[1];
acadoVariables.od[i+8] = r_poly[2];
acadoVariables.od[i+9] = r_poly[3];
acadoVariables.od[i+10] = p_poly[0];
acadoVariables.od[i+11] = p_poly[1];
acadoVariables.od[i+12] = p_poly[2];
acadoVariables.od[i+13] = p_poly[3];
acadoVariables.od[i+14] = l_prob;
acadoVariables.od[i+15] = r_prob;
acadoVariables.od[i+16] = p_prob;
acadoVariables.od[i+17] = lane_width;
}
acadoVariables.x0[0] = x0->x;
acadoVariables.x0[1] = x0->y;
acadoVariables.x0[2] = x0->psi;
acadoVariables.x0[3] = x0->delta;
acado_preparationStep();
acado_feedbackStep( );
acado_shiftStates(2, 0, 0);
acado_shiftControls( 0 );
for (i = 0; i <= N; i++){
solution->x[i] = acadoVariables.x[i*NX];
solution->y[i] = acadoVariables.x[i*NX+1];
solution->psi[i] = acadoVariables.x[i*NX+2];
solution->delta[i] = acadoVariables.x[i*NX+3];
}
return;
}
selfdrive/controls/lib/lateral_mpc/mpc_export/acado_auxiliary_functions.c
+3
View File
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:36c26a2590e54135f7f03b8c784b434d2bd5ef0d42e7e2a9022c2bb56d0e2357
size 4906
selfdrive/controls/lib/lateral_mpc/mpc_export/acado_auxiliary_functions.h
+3
View File
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:606244b9b31362cc30c324793191d9bd34a098d5ebf526612749f437a75a4270
size 3428
selfdrive/controls/lib/lateral_mpc/mpc_export/acado_common.h
+3
View File
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:c78a95b2550fdfdc0182add44508c59ec7af9d1c58d776dcd32ee85ea52a771e
size 8680
selfdrive/controls/lib/lateral_mpc/mpc_export/acado_integrator.c
+3
View File
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:963720039f3655eb3926e5bb3bbde21d92c824245055588ebaee223223accc02
size 18490
selfdrive/controls/lib/lateral_mpc/mpc_export/acado_qpoases_interface.cpp
+3
View File
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:10ff52f5d8dbe27def800fe490cdee82a7c054183d2fb1888752609ea00bbea1
size 1949
selfdrive/controls/lib/lateral_mpc/mpc_export/acado_qpoases_interface.hpp
+3
View File
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:55ef4b230fd392bb43a49b2bd4fbf6e478b6f7b6144f65df97fe3a952d6b0b49
size 1822
selfdrive/controls/lib/lateral_mpc/mpc_export/acado_solver.c
+3
View File
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:dd891792d5bc3c780941a0e3d8c37829d6f4ceef554a4704017f6024e29fba20
size 194688

Some files were not shown because too many files have changed in this diff Show More