diff --git a/.travis.yml b/.travis.yml index 9c668d75d9..74411b5de8 100644 --- a/.travis.yml +++ b/.travis.yml @@ -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' diff --git a/Dockerfile.openpilot b/Dockerfile.openpilot index 3b3638f9ca..4efafd40df 100644 --- a/Dockerfile.openpilot +++ b/Dockerfile.openpilot @@ -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 \ No newline at end of file diff --git a/README.md b/README.md index 32513579c4..c70574e84a 100644 --- a/README.md +++ b/README.md @@ -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 ------ diff --git a/RELEASES.md b/RELEASES.md index bb2a2b85a4..bf4baad56c 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -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) diff --git a/cereal/Makefile b/cereal/Makefile index ead08bf5be..3a580592b1 100644 --- a/cereal/Makefile +++ b/cereal/Makefile @@ -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" diff --git a/cereal/build_from_src.mk b/cereal/build_from_src.mk deleted file mode 100644 index 3a580592b1..0000000000 --- a/cereal/build_from_src.mk +++ /dev/null @@ -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 '$@' - diff --git a/cereal/car.capnp b/cereal/car.capnp index c52a5d12af..6b5a019c7b 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -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; - - enableSteer @3: Bool; - enableGas @4: Bool; - enableBrake @5: Bool; - enableCruise @6: Bool; + carName @0 :Text; + radarName @1 :Text; + carFingerprint @2 :Text; + + 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? } diff --git a/cereal/log.capnp b/cereal/log.capnp index 183ca904d8..1496ff1356 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -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; } } diff --git a/common/basedir.py b/common/basedir.py new file mode 100644 index 0000000000..991f6aaed9 --- /dev/null +++ b/common/basedir.py @@ -0,0 +1,4 @@ +import os +BASEDIR = os.path.join(os.path.dirname(os.path.realpath(__file__)), "../") + + diff --git a/common/crash.py b/common/crash.py index 06fe9a9132..bc5be003d7 100644 --- a/common/crash.py +++ b/common/crash.py @@ -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): diff --git a/common/fingerprints.py b/common/fingerprints.py index 298424940b..34c84b7449 100644 --- a/common/fingerprints.py +++ b/common/fingerprints.py @@ -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 } } diff --git a/common/params.py b/common/params.py index 8e8a11edc2..ca3ee99923 100755 --- a/common/params.py +++ b/common/params.py @@ -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: + 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)) + + 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) - raise - - # Keep holding the lock while we clean up the old data. - if old_data_path is not None: - shutil.rmtree(old_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) diff --git a/common/profiler.py b/common/profiler.py index b35a52d16f..26fbeb382b 100644 --- a/common/profiler.py +++ b/common/profiler.py @@ -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 diff --git a/opendbc b/opendbc index 008089045e..063032ff2b 160000 --- a/opendbc +++ b/opendbc @@ -1 +1 @@ -Subproject commit 008089045e371a9eebbe79d978ff22ae3e70bdba +Subproject commit 063032ff2b9b878c2cc10301504bad9db54f655f diff --git a/panda b/panda index 4901d52104..92a1c773e7 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 4901d52104e369f2444b5d56846199fdfc3fd695 +Subproject commit 92a1c773e763297b7478b5bca44e25fb8cd8bdf2 diff --git a/phonelibs/qpoases/EXAMPLES/example1.cpp b/phonelibs/qpoases/EXAMPLES/example1.cpp new file mode 100644 index 0000000000..bb66893ddf --- /dev/null +++ b/phonelibs/qpoases/EXAMPLES/example1.cpp @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:018b8429d7eafb7dbc55145bf29b6ee033fab367764d5994c0d2d3876d1a771e +size 1949 diff --git a/phonelibs/qpoases/EXAMPLES/example1b.cpp b/phonelibs/qpoases/EXAMPLES/example1b.cpp new file mode 100644 index 0000000000..e3d41f84d9 --- /dev/null +++ b/phonelibs/qpoases/EXAMPLES/example1b.cpp @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c5a9064ef08e632ea9f9aa31c7f4d6013756b57200c5791d8fc8bb1b5d1583be +size 1776 diff --git a/phonelibs/qpoases/INCLUDE/Bounds.hpp b/phonelibs/qpoases/INCLUDE/Bounds.hpp new file mode 100644 index 0000000000..a953a28f3e --- /dev/null +++ b/phonelibs/qpoases/INCLUDE/Bounds.hpp @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:2d77876b6d9bbb9968f95cc7908d2e7a7706b71cc1d79d44c9f269b7e5078278 +size 5743 diff --git a/phonelibs/qpoases/INCLUDE/Constants.hpp b/phonelibs/qpoases/INCLUDE/Constants.hpp new file mode 100644 index 0000000000..69ace2b105 --- /dev/null +++ b/phonelibs/qpoases/INCLUDE/Constants.hpp @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:5b515339eee590e3af6ae78679e58338fe5e164d51550ba93d3de2b8bb0f2e09 +size 3501 diff --git a/phonelibs/qpoases/INCLUDE/Constraints.hpp b/phonelibs/qpoases/INCLUDE/Constraints.hpp new file mode 100644 index 0000000000..bd0d20085f --- /dev/null +++ b/phonelibs/qpoases/INCLUDE/Constraints.hpp @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:5a394fb4f49deb8598ee72920e1b03a682c457b42765b8aac0f8a2ac43bdf118 +size 5864 diff --git a/phonelibs/qpoases/INCLUDE/CyclingManager.hpp b/phonelibs/qpoases/INCLUDE/CyclingManager.hpp new file mode 100644 index 0000000000..11ededfcba --- /dev/null +++ b/phonelibs/qpoases/INCLUDE/CyclingManager.hpp @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:325c722b58ad39b7860137ea1ecfb1bd48fc44251717d16574dcd3ad28e11089 +size 3861 diff --git a/phonelibs/qpoases/INCLUDE/EXTRAS/SolutionAnalysis.hpp b/phonelibs/qpoases/INCLUDE/EXTRAS/SolutionAnalysis.hpp new file mode 100644 index 0000000000..733058d329 --- /dev/null +++ b/phonelibs/qpoases/INCLUDE/EXTRAS/SolutionAnalysis.hpp @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:60dc80b14e5a7fac2257648a004987d0706d4d32602e3657d2c97d24363406bb +size 3435 diff --git a/phonelibs/qpoases/INCLUDE/Indexlist.hpp b/phonelibs/qpoases/INCLUDE/Indexlist.hpp new file mode 100644 index 0000000000..a96909efcf --- /dev/null +++ b/phonelibs/qpoases/INCLUDE/Indexlist.hpp @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:53cf87460a46afce93acc7d5d76011366a387a93d67401bec25350e05bb783a5 +size 4928 diff --git a/phonelibs/qpoases/INCLUDE/MessageHandling.hpp b/phonelibs/qpoases/INCLUDE/MessageHandling.hpp new file mode 100644 index 0000000000..a36e884324 --- /dev/null +++ b/phonelibs/qpoases/INCLUDE/MessageHandling.hpp @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:67c24d5d24e53580b5af982cfa36b115e88314b1306e2cea6b3c1e9ea746a369 +size 20367 diff --git a/phonelibs/qpoases/INCLUDE/QProblem.hpp b/phonelibs/qpoases/INCLUDE/QProblem.hpp new file mode 100644 index 0000000000..3f1447a4b4 --- /dev/null +++ b/phonelibs/qpoases/INCLUDE/QProblem.hpp @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:01213e5cb12bf937d65ffd05c7a9ba70a7ab4e043753449edda98fd08f733930 +size 33063 diff --git a/phonelibs/qpoases/INCLUDE/QProblemB.hpp b/phonelibs/qpoases/INCLUDE/QProblemB.hpp new file mode 100644 index 0000000000..db88646991 --- /dev/null +++ b/phonelibs/qpoases/INCLUDE/QProblemB.hpp @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3c9f053c8eed7d0b8aa72dec81ecdb7cb59bc87dee0108398bc34b79a76c7bd3 +size 27333 diff --git a/phonelibs/qpoases/INCLUDE/SubjectTo.hpp b/phonelibs/qpoases/INCLUDE/SubjectTo.hpp new file mode 100644 index 0000000000..7cb0ba2cfa --- /dev/null +++ b/phonelibs/qpoases/INCLUDE/SubjectTo.hpp @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:fdbee52661ba442d31f697828ff283505bf48e78f7740aac59f39aa62a18cae2 +size 5572 diff --git a/phonelibs/qpoases/INCLUDE/Types.hpp b/phonelibs/qpoases/INCLUDE/Types.hpp new file mode 100644 index 0000000000..352cd88bd3 --- /dev/null +++ b/phonelibs/qpoases/INCLUDE/Types.hpp @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ff13c48ec416a3c30f7582d38e4ca43b30e8867b28bcebdab3a6cced6e141258 +size 4467 diff --git a/phonelibs/qpoases/INCLUDE/Utils.hpp b/phonelibs/qpoases/INCLUDE/Utils.hpp new file mode 100644 index 0000000000..9edab60790 --- /dev/null +++ b/phonelibs/qpoases/INCLUDE/Utils.hpp @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:2f37d5507c5288bce904e776045b68702240d2d51896262f645084cba7e971cc +size 6713 diff --git a/phonelibs/qpoases/LICENSE.txt b/phonelibs/qpoases/LICENSE.txt new file mode 100644 index 0000000000..493ebad4e8 --- /dev/null +++ b/phonelibs/qpoases/LICENSE.txt @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:319daefe38ebeead3df178e5937898a52d67c0c795f54f1c0bb10ac3b9cffb63 +size 26940 diff --git a/phonelibs/qpoases/README.txt b/phonelibs/qpoases/README.txt new file mode 100644 index 0000000000..6082f5f537 --- /dev/null +++ b/phonelibs/qpoases/README.txt @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ae75304482e680a0c6bc6f81eafcf29c3a77b407a6344181eb6f67cd3b2fbf69 +size 3177 diff --git a/phonelibs/qpoases/SRC/Bounds.cpp b/phonelibs/qpoases/SRC/Bounds.cpp new file mode 100644 index 0000000000..e31bf121f9 --- /dev/null +++ b/phonelibs/qpoases/SRC/Bounds.cpp @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:dedf2112735d4beae4682878dc0a97cc2bcda648459d917e0049f4997654ac52 +size 5901 diff --git a/phonelibs/qpoases/SRC/Bounds.ipp b/phonelibs/qpoases/SRC/Bounds.ipp new file mode 100644 index 0000000000..1bf3c6da26 --- /dev/null +++ b/phonelibs/qpoases/SRC/Bounds.ipp @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:099752cb994e787b7d6c9a5dbde908eb19d4d3a4e0bec63e36c3907704732e57 +size 2545 diff --git a/phonelibs/qpoases/SRC/Constraints.cpp b/phonelibs/qpoases/SRC/Constraints.cpp new file mode 100644 index 0000000000..1e347d9978 --- /dev/null +++ b/phonelibs/qpoases/SRC/Constraints.cpp @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3028ce5088b5690e774267d4c1f6de8c330b3ecf0add682fc2350d87cee68030 +size 6102 diff --git a/phonelibs/qpoases/SRC/Constraints.ipp b/phonelibs/qpoases/SRC/Constraints.ipp new file mode 100644 index 0000000000..fa520d3d7a --- /dev/null +++ b/phonelibs/qpoases/SRC/Constraints.ipp @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:f13088527d6bdd946bf66581edb71d93ea7659261cd797fbd2cc2a3ed836a903 +size 2641 diff --git a/phonelibs/qpoases/SRC/CyclingManager.cpp b/phonelibs/qpoases/SRC/CyclingManager.cpp new file mode 100644 index 0000000000..514bea92be --- /dev/null +++ b/phonelibs/qpoases/SRC/CyclingManager.cpp @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:67bab79e60c64e87a7977957cd55726912837629115d6246cd089dbb9aae4fc7 +size 3949 diff --git a/phonelibs/qpoases/SRC/CyclingManager.ipp b/phonelibs/qpoases/SRC/CyclingManager.ipp new file mode 100644 index 0000000000..08ad04d1a8 --- /dev/null +++ b/phonelibs/qpoases/SRC/CyclingManager.ipp @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:85df62984743813f8f9e92ff299b74c790d9e174906a5966b916ebb21f068165 +size 1643 diff --git a/phonelibs/qpoases/SRC/EXTRAS/SolutionAnalysis.cpp b/phonelibs/qpoases/SRC/EXTRAS/SolutionAnalysis.cpp new file mode 100644 index 0000000000..9a273a9e4a --- /dev/null +++ b/phonelibs/qpoases/SRC/EXTRAS/SolutionAnalysis.cpp @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:1cc39dede19249ae695a72203ee643e65b29a0738316b1b2e77cc5f9d72821fe +size 12434 diff --git a/phonelibs/qpoases/SRC/Indexlist.cpp b/phonelibs/qpoases/SRC/Indexlist.cpp new file mode 100644 index 0000000000..2b5d1a40ae --- /dev/null +++ b/phonelibs/qpoases/SRC/Indexlist.cpp @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:2d40beb3c8639d8c9bb3d529d98a5c51d7b1a9239febda97ef136a2d27a785bf +size 6642 diff --git a/phonelibs/qpoases/SRC/Indexlist.ipp b/phonelibs/qpoases/SRC/Indexlist.ipp new file mode 100644 index 0000000000..0e11cba5a2 --- /dev/null +++ b/phonelibs/qpoases/SRC/Indexlist.ipp @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:9bf145bff7575704135bd3f2933a9af5bf37cace1ad56c7ed887128f2bfc459f +size 2163 diff --git a/phonelibs/qpoases/SRC/MessageHandling.cpp b/phonelibs/qpoases/SRC/MessageHandling.cpp new file mode 100644 index 0000000000..b20664f97c --- /dev/null +++ b/phonelibs/qpoases/SRC/MessageHandling.cpp @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:2b2de17f7e28f36f108afc05ea257b82fb112ad7afb0aef965e857484e951f06 +size 19449 diff --git a/phonelibs/qpoases/SRC/MessageHandling.ipp b/phonelibs/qpoases/SRC/MessageHandling.ipp new file mode 100644 index 0000000000..aaf8e0c177 --- /dev/null +++ b/phonelibs/qpoases/SRC/MessageHandling.ipp @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:247726b5943a872b367d5eb948078428f570e3684f9c0e541a13890729020ecb +size 3215 diff --git a/phonelibs/qpoases/SRC/QProblem.cpp b/phonelibs/qpoases/SRC/QProblem.cpp new file mode 100644 index 0000000000..db18a07db4 --- /dev/null +++ b/phonelibs/qpoases/SRC/QProblem.cpp @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e50488623df439ee0539898819839afd7c064be68709c32eea19db552377b81a +size 96202 diff --git a/phonelibs/qpoases/SRC/QProblem.ipp b/phonelibs/qpoases/SRC/QProblem.ipp new file mode 100644 index 0000000000..a1d44fe780 --- /dev/null +++ b/phonelibs/qpoases/SRC/QProblem.ipp @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:279519a38bb964e574752c75a9be6ce6e2720477183397a85cd505859329654d +size 5667 diff --git a/phonelibs/qpoases/SRC/QProblemB.cpp b/phonelibs/qpoases/SRC/QProblemB.cpp new file mode 100644 index 0000000000..12ff414aa0 --- /dev/null +++ b/phonelibs/qpoases/SRC/QProblemB.cpp @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a4dc609ba60648cf2307536b2c5088fe082caff7785d31a5686ef6b8bee6db7a +size 51089 diff --git a/phonelibs/qpoases/SRC/QProblemB.ipp b/phonelibs/qpoases/SRC/QProblemB.ipp new file mode 100644 index 0000000000..e422066eae --- /dev/null +++ b/phonelibs/qpoases/SRC/QProblemB.ipp @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:789884d3683b59533bc5e84ee44e616c3a48b414285266f0155846f7ee73cad5 +size 7273 diff --git a/phonelibs/qpoases/SRC/SubjectTo.cpp b/phonelibs/qpoases/SRC/SubjectTo.cpp new file mode 100644 index 0000000000..da1a53b4aa --- /dev/null +++ b/phonelibs/qpoases/SRC/SubjectTo.cpp @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:6e850a791144172ed5bfec0fca3b32abd0347885f0e739ae3aa306f9fb4889bb +size 4244 diff --git a/phonelibs/qpoases/SRC/SubjectTo.ipp b/phonelibs/qpoases/SRC/SubjectTo.ipp new file mode 100644 index 0000000000..afc54d992f --- /dev/null +++ b/phonelibs/qpoases/SRC/SubjectTo.ipp @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:6872db4cea8f779994636a27c100724b2a7ca7ff1451aa2bffcc768798c9bb80 +size 2847 diff --git a/phonelibs/qpoases/SRC/Utils.cpp b/phonelibs/qpoases/SRC/Utils.cpp new file mode 100644 index 0000000000..ab0b563abb --- /dev/null +++ b/phonelibs/qpoases/SRC/Utils.cpp @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:695b8e4e30ab4ec7e522446463716fbbb1be92c840117830fa1bd389d969ddfa +size 10228 diff --git a/phonelibs/qpoases/SRC/Utils.ipp b/phonelibs/qpoases/SRC/Utils.ipp new file mode 100644 index 0000000000..62033914af --- /dev/null +++ b/phonelibs/qpoases/SRC/Utils.ipp @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:abdbf8757354226ba1723eaa4d8ace186471c6a193812dca3a9a06f4b30b9fce +size 1315 diff --git a/phonelibs/qpoases/VERSIONS.txt b/phonelibs/qpoases/VERSIONS.txt new file mode 100644 index 0000000000..7402fe3365 --- /dev/null +++ b/phonelibs/qpoases/VERSIONS.txt @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:f02a5ff04907df394d7b905216cfcece2a7a54cdbed25a4d892bb08f2f760d77 +size 3061 diff --git a/pyextra b/pyextra index e0738376db..4eda4dd765 160000 --- a/pyextra +++ b/pyextra @@ -1 +1 @@ -Subproject commit e0738376db27d208603d7e63dd465e003ca06325 +Subproject commit 4eda4dd765c2bc719da9064774de6b2c14c322d1 diff --git a/run_docker_tests.sh b/run_docker_tests.sh index 96b9e4ab78..a153aac73b 100755 --- a/run_docker_tests.sh +++ b/run_docker_tests.sh @@ -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' diff --git a/selfdrive/boardd/Makefile b/selfdrive/boardd/Makefile index d2d3c04e0b..cb00d6ac1a 100644 --- a/selfdrive/boardd/Makefile +++ b/selfdrive/boardd/Makefile @@ -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) diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index a2390ac02d..c7e3d01603 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -19,7 +19,9 @@ #include #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((value_sz / sizeof(capnp::word)) + 1); + memcpy(amsg.begin(), value, value_sz); + + capnp::FlatArrayMessageReader cmsg(amsg); + cereal::CarParams::Reader car_params = cmsg.getRoot(); + + 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); - - // 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); - } + // 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 - // 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*/ + // no output is the default + libusb_control_transfer(dev_handle, 0x40, 0xdc, SAFETY_NOOUTPUT, 0, NULL, 0, TIMEOUT); - // 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); + if (safety_setter_thread_handle == -1) { + err = pthread_create(&safety_setter_thread_handle, NULL, safety_setter_thread, NULL); + } 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((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(); + + 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); diff --git a/selfdrive/boardd/boardd.py b/selfdrive/boardd/boardd.py index 8bda67923f..06abe8b3a5 100755 --- a/selfdrive/boardd/boardd.py +++ b/selfdrive/boardd/boardd.py @@ -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) diff --git a/selfdrive/can/Makefile b/selfdrive/can/Makefile index 9c2ebbd732..5a91e70291 100644 --- a/selfdrive/can/Makefile +++ b/selfdrive/can/Makefile @@ -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 diff --git a/selfdrive/can/parser_common.h b/selfdrive/can/common.h similarity index 81% rename from selfdrive/can/parser_common.h rename to selfdrive/can/common.h index e3b944f83e..3790c1358a 100644 --- a/selfdrive/can/parser_common.h +++ b/selfdrive/can/common.h @@ -1,13 +1,20 @@ -#ifndef PARSER_COMMON_H -#define PARSER_COMMON_H +#ifndef SELFDRIVE_CAN_COMMON_H +#define SELFDRIVE_CAN_COMMON_H #include #include +#include #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) \ diff --git a/selfdrive/can/dbc.cc b/selfdrive/can/dbc.cc new file mode 100644 index 0000000000..c8295d1e0c --- /dev/null +++ b/selfdrive/can/dbc.cc @@ -0,0 +1,26 @@ +#include +#include + +#include "common.h" + +namespace { + +std::vector& get_dbcs() { + static std::vector 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); +} diff --git a/selfdrive/can/dbc_template.cc b/selfdrive/can/dbc_template.cc index 3676c823ec..1d85b1e2f3 100644 --- a/selfdrive/can/dbc_template.cc +++ b/selfdrive/can/dbc_template.cc @@ -1,6 +1,6 @@ #include -#include "parser_common.h" +#include "common.h" namespace { diff --git a/selfdrive/can/libdbc_py.py b/selfdrive/can/libdbc_py.py new file mode 100644 index 0000000000..0afeee40ef --- /dev/null +++ b/selfdrive/can/libdbc_py.py @@ -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) diff --git a/selfdrive/can/packer.cc b/selfdrive/can/packer.cc new file mode 100644 index 0000000000..3eddc6aeef --- /dev/null +++ b/selfdrive/can/packer.cc @@ -0,0 +1,81 @@ +#include + +#include +#include +#include +#include +#include + +#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; inum_msgs; i++) { + const Msg* msg = &dbc->msgs[i]; + for (int j=0; jnum_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 &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, 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(vals, vals+num_vals)); +} + +} + diff --git a/selfdrive/can/packer.py b/selfdrive/can/packer.py new file mode 100644 index 0000000000..ff52e165e2 --- /dev/null +++ b/selfdrive/can/packer.py @@ -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") diff --git a/selfdrive/can/parser.cc b/selfdrive/can/parser.cc index 7e7913bf43..839adf5222 100644 --- a/selfdrive/can/parser.cc +++ b/selfdrive/can/parser.cc @@ -18,7 +18,7 @@ #include #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 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-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; inum_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 query(uint64_t sec) { std::vector 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 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. - - # redundant safety check with the board - self.controls_allowed = False + self.brake_last = 0. - 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 = [] diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index d2c0f974ae..4eb8c6c386 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -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) + diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index c020cc5d4c..730f5ea032 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -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') - - ret.errors = errors + 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])) + + 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) diff --git a/selfdrive/car/honda/can_parser.py b/selfdrive/car/honda/old_can_parser.py similarity index 97% rename from selfdrive/car/honda/can_parser.py rename to selfdrive/car/honda/old_can_parser.py index d12a33fe1e..6f7f3dce3d 100644 --- a/selfdrive/car/honda/can_parser.py +++ b/selfdrive/car/honda/old_can_parser.py @@ -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 diff --git a/selfdrive/common/cereal.mk b/selfdrive/common/cereal.mk index f4603dc32d..637a104061 100644 --- a/selfdrive/common/cereal.mk +++ b/selfdrive/common/cereal.mk @@ -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 '$@' '$<' - diff --git a/selfdrive/common/mat.h b/selfdrive/common/mat.h index f429b0c5d3..1c20eae17f 100644 --- a/selfdrive/common/mat.h +++ b/selfdrive/common/mat.h @@ -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 diff --git a/selfdrive/common/params.c b/selfdrive/common/params.cc similarity index 83% rename from selfdrive/common/params.c rename to selfdrive/common/params.cc index 7cefc7b06b..5b0d216bdd 100644 --- a/selfdrive/common/params.c +++ b/selfdrive/common/params.cc @@ -11,6 +11,18 @@ #include #include +namespace { + +template +T* null_coalesce(T* a, T* b) { + return a != NULL ? a : b; +} + +static const char* default_params_path = null_coalesce( + const_cast(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(read_file(path, value_sz)); if (*value == NULL) { result = -22; goto cleanup; diff --git a/selfdrive/common/params.h b/selfdrive/common/params.h index 05ddb1dd79..bd4d86dcfe 100644 --- a/selfdrive/common/params.h +++ b/selfdrive/common/params.h @@ -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. diff --git a/selfdrive/common/swaglog.h b/selfdrive/common/swaglog.h index b972f98d18..33e77f618a 100644 --- a/selfdrive/common/swaglog.h +++ b/selfdrive/common/swaglog.h @@ -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 diff --git a/selfdrive/common/timing.h b/selfdrive/common/timing.h index 02e934dcb5..1a30ad6e1e 100644 --- a/selfdrive/common/timing.h +++ b/selfdrive/common/timing.h @@ -5,7 +5,7 @@ #include #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 diff --git a/selfdrive/common/util.c b/selfdrive/common/util.c index e5ab4d0a6c..172345869e 100644 --- a/selfdrive/common/util.c +++ b/selfdrive/common/util.c @@ -3,6 +3,10 @@ #include #include +#ifdef __linux__ +#include +#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 +} diff --git a/selfdrive/common/util.h b/selfdrive/common/util.h index 9b5da8e726..ce18068bbe 100644 --- a/selfdrive/common/util.h +++ b/selfdrive/common/util.h @@ -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 diff --git a/selfdrive/common/utilpp.h b/selfdrive/common/utilpp.h index 25abac6682..bc1b2fd267 100644 --- a/selfdrive/common/utilpp.h +++ b/selfdrive/common/utilpp.h @@ -9,6 +9,10 @@ #include #include +#ifdef __x86_64 +#include +#endif + namespace util { inline bool starts_with(std::string s, std::string prefix) { diff --git a/selfdrive/common/version.h b/selfdrive/common/version.h index 4c2a881ae9..de335592cd 100644 --- a/selfdrive/common/version.h +++ b/selfdrive/common/version.h @@ -1 +1 @@ -#define OPENPILOT_VERSION "0.3.6.1" +#define OPENPILOT_VERSION "0.3.7" diff --git a/selfdrive/common/visionipc.c b/selfdrive/common/visionipc.c index c371754f88..4299f512f9 100644 --- a/selfdrive/common/visionipc.c +++ b/selfdrive/common/visionipc.c @@ -1,6 +1,7 @@ #include #include #include +#include #include #include #include @@ -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; ibufs_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; diff --git a/selfdrive/common/visionipc.h b/selfdrive/common/visionipc.h index dab392ec54..24b882340f 100644 --- a/selfdrive/common/visionipc.h +++ b/selfdrive/common/visionipc.h @@ -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 diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index c93ad14339..867062c54b 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -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 - -class Controls(object): - def __init__(self, gctx, rate=100): - self.rate = rate - - # *** log *** - context = zmq.Context() - - # 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") - - # *** 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 state_transition(self): - pass # for now - - def state_control(self): - - # did it request to enable? - enable_request, enable_condition = False, False - - # 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) - - # handle button presses - for b in self.CS.buttonEvents: - print b - - # 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 + PRE_ENABLED = 2 + SOFT_DISABLING = 3 - # 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 - else: - enable_condition = enable_request +# True when actuators are controlled +def isActive(state): + return state in [State.ENABLED, State.SOFT_DISABLING] + + +# True if system is engaged +def isEnabled(state): + return (isActive(state) or state == State.PRE_ENABLED) - if self.CP.enableCruise and self.CS.cruiseState.enabled: - self.v_cruise_kph = self.CS.cruiseState.speed * CV.MS_TO_KPH - self.prof.checkpoint("AdaptiveCruise") +def data_sample(CI, CC, thermal, health, cal, 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 + # *** read can and compute car states *** + CS = CI.update(CC) + events = list(CS.events) - # 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) + # *** 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])) - 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) + # under 15% of space free + if td.thermal.freeSpace < 0.15: + events.append(create_event('outOfSpace', [ET.NO_ENTRY])) - if not self.plan.longitudinalValid: - self.AM.add("radarCommIssue", self.enabled) + # *** read calibration status *** + cal = messaging.recv_sock(cal) + if cal is not None: + cal_status = cal.liveCalibration.calStatus + cal_perc = cal.liveCalibration.calPerc - if self.cal_status != Calibration.CALIBRATED: - if self.cal_status == Calibration.UNCALIBRATED: - self.AM.add("calibrationInProgress", self.enabled, str(self.cal_perc) + '%') + if cal_status != Calibration.CALIBRATED: + if cal_status == Calibration.UNCALIBRATED: + events.append(create_event('calibrationInProgress', [ET.NO_ENTRY, ET.SOFT_DISABLE])) + else: + events.append(create_event('calibrationInvalid', [ET.NO_ENTRY, ET.SOFT_DISABLE])) + + # *** 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])) + + return CS, events, cal_status, cal_perc + + +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 + + # add events from planner + events += list(plan.events) + + # 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])) + + return plan, plan_ts + + +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) + + # 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) + + # 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) + + # ***** handle state transitions ***** + + # 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: + if get_events(events, [ET.PRE_ENABLE]): + state = State.PRE_ENABLED else: - self.AM.add("calibrationInvalid", self.enabled) - - 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) - - - # *** 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) - - # *** 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) - - # *** 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) - - # 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 + 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))) + + # ENABLED + elif state == State.ENABLED: + if get_events(events, [ET.USER_DISABLE]): + state = State.DISABLED + AM.add("disable", enabled) + + elif get_events(events, [ET.IMMEDIATE_DISABLE]): + state = State.DISABLED + for e in get_events(events, [ET.IMMEDIATE_DISABLE]): + AM.add(e, enabled) + + 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) + + # SOFT DISABLING + elif state == State.SOFT_DISABLING: + if get_events(events, [ET.USER_DISABLE]): + state = State.DISABLED + AM.add("disable", enabled) + + elif get_events(events, [ET.IMMEDIATE_DISABLE]): + state = State.DISABLED + for e in get_events(events, [ET.IMMEDIATE_DISABLE]): + AM.add(e, enabled) + + elif not get_events(events, [ET.SOFT_DISABLE]): + # no more soft disabling condition, so go back to ENABLED + state = State.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) + + 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) + + elif not get_events(events, [ET.PRE_ENABLE]): + state = State.ENABLED + + return state, soft_disable_timer, v_cruise_kph + + +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 + + # reset actuators to zero + actuators = car.CarControl.Actuators.new_message() + + enabled = isEnabled(state) + active = isActive(state) + + for b in CS.buttonEvents: + # any button event resets awarenesss_status + awareness_status = 1. + + # 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: - self.soft_disable_timer -= 1 - else: - self.soft_disable_timer = None - - if enable_condition and not self.enabled and not self.AM.alertPresent(): - print "*** enabling controls" - - # beep for enabling - self.AM.add("enable", self.enabled) - - # enable both lateral and longitudinal controls - self.enabled = True - - # 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))) - - # 6 minutes driver you're on - self.awareness_status = 1.0 - - # reset the PID loops - self.LaC.reset() - # start long control at actual speed - self.LoC.reset(v_pid = self.CS.vEgo) - - # *** 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 - - self.CC.gas = float(final_gas) - self.CC.brake = float(final_brake) - self.CC.steeringTorque = float(final_steer) - - 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)) - - # 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) - - #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 - - # 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) - - 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()) - - self.prof.checkpoint("CarControl") - - # 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 ***** - - # publish controls state at 100Hz - dat = messaging.new_message() - dat.init('live100') + rear_view_toggle = False - # 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 + if b.type == "altButton1" and b.pressed: + rear_view_toggle = not rear_view_toggle - # 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 + # send FCW alert if triggered by planner + if plan.fcw: + AM.add("fcw", enabled) - # car state - dat.live100.vEgo = self.CS.vEgo - dat.live100.angleSteers = self.CS.steeringAngle - dat.live100.steerOverride = self.CS.steeringPressed + # ***** state specific actions ***** - # 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) + # DISABLED + if state in [State.PRE_ENABLED, State.DISABLED]: + awareness_status = 1. + LaC.reset() + LoC.reset(v_pid=CS.vEgo) - # 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) + # ENABLED or SOFT_DISABLING + elif state in [State.ENABLED, State.SOFT_DISABLING]: - # 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) + if CS.steeringPressed: + # reset awareness status on steering + awareness_status = 1.0 - # log learned angle offset - dat.live100.angleOffset = float(self.angle_offset) + # 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) - # lag - dat.live100.cumLagMs = -self.rk.remaining*1000. + # parse warnings from car specific interface + for e in get_events(events, [ET.WARNING]): + AM.add(e, enabled) - self.live100.send(dat.to_bytes()) + # 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) - self.prof.checkpoint("Live100") + # *** angle offset learning *** - def wait(self): - # *** run loop at fixed rate *** - if self.rk.keep_time(): - self.prof.display() + 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('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()) + + 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() - diff --git a/selfdrive/controls/lib/adaptivecruise.py b/selfdrive/controls/lib/adaptivecruise.py index 1fcf6cad68..8d3ceec674 100644 --- a/selfdrive/controls/lib/adaptivecruise.py +++ b/selfdrive/controls/lib/adaptivecruise.py @@ -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 diff --git a/selfdrive/controls/lib/alertmanager.py b/selfdrive/controls/lib/alertmanager.py index 162a1977ed..a876673228 100644 --- a/selfdrive/controls/lib/alertmanager.py +++ b/selfdrive/controls/lib/alertmanager.py @@ -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 - diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 2d01b40ca0..699893134e 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -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 diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index 720d7947f8..ab81435fc7 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -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 - - 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 = (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 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 + + +# 100ms is a rule of thumb estimation of lag from image processing to actuator command +ACTUATORS_DELAY = 0.1 + + +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 get_steer_max(CP, v_ego): + return interp(v_ego, CP.steerMaxBP, CP.steerMaxV) + 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, 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 + + curvature_factor = VM.curvature_factor(v_ego) - def update(self, enabled, v_ego, angle_steers, steer_override, d_poly, angle_offset, VM): - rate = 100 + 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)) - steer_max = 1.0 + # account for actuation delay + self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers, curvature_factor, VM.CP.sR) - # how far we look ahead is function of speed and desired path - d_lookahead = calc_d_lookahead(v_ego, d_poly) + 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) - # desired lookahead offset - self.y_des = np.polyval(d_poly, d_lookahead) + delta_desired = self.mpc_solution[0].delta[1] + self.cur_state[0].delta = delta_desired - # 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.angle_steers_des = math.degrees(delta_desired * VM.CP.sR) + angle_offset + self.mpc_updated = True - 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) + 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) - final_steer = clip(output_steer, -steer_max, steer_max) - return final_steer, sat_flag + self.sat_flag = self.pid.saturated + return output_steer diff --git a/selfdrive/controls/lib/latcontrol_helpers.py b/selfdrive/controls/lib/latcontrol_helpers.py new file mode 100644 index 0000000000..982152d65c --- /dev/null +++ b/selfdrive/controls/lib/latcontrol_helpers.py @@ -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 diff --git a/selfdrive/controls/lib/lateral_mpc/Makefile b/selfdrive/controls/lib/lateral_mpc/Makefile new file mode 100644 index 0000000000..b0802f9a44 --- /dev/null +++ b/selfdrive/controls/lib/lateral_mpc/Makefile @@ -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) diff --git a/selfdrive/controls/lib/lateral_mpc/__init__.py b/selfdrive/controls/lib/lateral_mpc/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/controls/lib/lateral_mpc/generator.cpp b/selfdrive/controls/lib/lateral_mpc/generator.cpp new file mode 100644 index 0000000000..2e5c3a2ff4 --- /dev/null +++ b/selfdrive/controls/lib/lateral_mpc/generator.cpp @@ -0,0 +1,137 @@ +#include + +#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; +} diff --git a/selfdrive/controls/lib/lateral_mpc/libmpc_py.py b/selfdrive/controls/lib/lateral_mpc/libmpc_py.py new file mode 100644 index 0000000000..032567be59 --- /dev/null +++ b/selfdrive/controls/lib/lateral_mpc/libmpc_py.py @@ -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) diff --git a/selfdrive/controls/lib/lateral_mpc/mpc.c b/selfdrive/controls/lib/lateral_mpc/mpc.c new file mode 100644 index 0000000000..6823c554c2 --- /dev/null +++ b/selfdrive/controls/lib/lateral_mpc/mpc.c @@ -0,0 +1,100 @@ +#include "acado_common.h" +#include "acado_auxiliary_functions.h" + +#include + +#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; +} diff --git a/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_auxiliary_functions.c b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_auxiliary_functions.c new file mode 100644 index 0000000000..174ed75004 --- /dev/null +++ b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_auxiliary_functions.c @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:36c26a2590e54135f7f03b8c784b434d2bd5ef0d42e7e2a9022c2bb56d0e2357 +size 4906 diff --git a/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_auxiliary_functions.h b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_auxiliary_functions.h new file mode 100644 index 0000000000..ac98266ff4 --- /dev/null +++ b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_auxiliary_functions.h @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:606244b9b31362cc30c324793191d9bd34a098d5ebf526612749f437a75a4270 +size 3428 diff --git a/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_common.h b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_common.h new file mode 100644 index 0000000000..155d06bfae --- /dev/null +++ b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_common.h @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c78a95b2550fdfdc0182add44508c59ec7af9d1c58d776dcd32ee85ea52a771e +size 8680 diff --git a/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_integrator.c b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_integrator.c new file mode 100644 index 0000000000..ecce19d241 --- /dev/null +++ b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_integrator.c @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:963720039f3655eb3926e5bb3bbde21d92c824245055588ebaee223223accc02 +size 18490 diff --git a/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_qpoases_interface.cpp b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_qpoases_interface.cpp new file mode 100644 index 0000000000..85bf25f21d --- /dev/null +++ b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_qpoases_interface.cpp @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:10ff52f5d8dbe27def800fe490cdee82a7c054183d2fb1888752609ea00bbea1 +size 1949 diff --git a/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_qpoases_interface.hpp b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_qpoases_interface.hpp new file mode 100644 index 0000000000..fd3f8494e1 --- /dev/null +++ b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_qpoases_interface.hpp @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:55ef4b230fd392bb43a49b2bd4fbf6e478b6f7b6144f65df97fe3a952d6b0b49 +size 1822 diff --git a/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_solver.c b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_solver.c new file mode 100644 index 0000000000..8beb002f19 --- /dev/null +++ b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_solver.c @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:dd891792d5bc3c780941a0e3d8c37829d6f4ceef554a4704017f6024e29fba20 +size 194688 diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index ec9893293b..5b65fef7de 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -1,205 +1,117 @@ import numpy as np from common.numpy_fast import clip, interp from selfdrive.config import Conversions as CV +from selfdrive.controls.lib.pid import PIController + +STOPPING_EGO_SPEED = 0.5 +STOPPING_TARGET_SPEED = 0.3 +STARTING_TARGET_SPEED = 0.5 +BRAKE_THRESHOLD_TO_PID = 0.2 + class LongCtrlState: #*** this function handles the long control state transitions # long_control_state labels: - off = 0 # Off - pid = 1 # moving and tracking targets, with PID control running + off = 0 # Off + pid = 1 # moving and tracking targets, with PID control running stopping = 2 # stopping and changing controls to almost open loop as PID does not fit well at such a low speed starting = 3 # starting and releasing brakes in open loop before giving back to PID -def long_control_state_trans(enabled, long_control_state, v_ego, v_target, v_pid, output_gb): - stopping_speed = 0.5 - stopping_target_speed = 0.3 - starting_target_speed = 0.5 - brake_threshold_to_pid = 0.2 +def long_control_state_trans(active, long_control_state, v_ego, v_target, v_pid, + output_gb, brake_pressed): - stopping_condition = ((v_ego < stopping_speed) and (v_pid < stopping_target_speed) and (v_target < stopping_target_speed)) + stopping_condition = (v_ego < STOPPING_EGO_SPEED) and \ + (((v_pid < STOPPING_TARGET_SPEED) and (v_target < STOPPING_TARGET_SPEED)) or + (brake_pressed)) - if not enabled: + if not active: long_control_state = LongCtrlState.off + else: if long_control_state == LongCtrlState.off: - if enabled: + if active: long_control_state = LongCtrlState.pid + elif long_control_state == LongCtrlState.pid: if stopping_condition: long_control_state = LongCtrlState.stopping + elif long_control_state == LongCtrlState.stopping: - if (v_target > starting_target_speed): + if (v_target > STARTING_TARGET_SPEED): long_control_state = LongCtrlState.starting + elif long_control_state == LongCtrlState.starting: if stopping_condition: long_control_state = LongCtrlState.stopping - elif output_gb >= -brake_threshold_to_pid: + elif output_gb >= -BRAKE_THRESHOLD_TO_PID: long_control_state = LongCtrlState.pid return long_control_state -def get_compute_gb(): - # see debug/dump_accel_from_fiber.py - 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(dat): - #linearly extrap below v1 using v1 and v2 data - v1 = 5. - v2 = 10. - vx = dat[1] - if vx > 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 = (vx - v1) * (m4v2 - m4v1) / (v2 - v1) + m4v1 - return m4 - return _compute_gb - -# takes in [desired_accel, current_speed] -> [-1.0, 1.0] where -1.0 is max brake and 1.0 is max gas -compute_gb = get_compute_gb() - _KP_BP = [0., 5., 35.] -_KP_V = [1.2, 0.8, 0.5] - -_kI_BP = [0., 35.] -_kI_V = [0.18, 0.12] - -def pid_long_control(v_ego, v_pid, Ui_accel_cmd, gas_max, brake_max, jerk_factor, gear, rate): - #*** This function compute the gb pedal positions in order to track the desired speed - # proportional and integral terms. More precision at low speed - Kp = interp(v_ego, _KP_BP, _KP_V) - Ki = interp(v_ego, _kI_BP, _kI_V) - - # scale Kp and Ki by jerk factor from drive_thread - Kp = (1. + jerk_factor)*Kp - Ki = (1. + jerk_factor)*Ki - - # this is ugly but can speed reports 0 when speed<0.3m/s and we can't have that jump - v_ego_min = 0.3 - v_ego = max(v_ego, v_ego_min) - - v_error = v_pid - v_ego - - Up_accel_cmd = v_error*Kp - Ui_accel_cmd_new = Ui_accel_cmd + v_error*Ki*1.0/rate - accel_cmd_new = Ui_accel_cmd_new + Up_accel_cmd - output_gb_new = compute_gb([accel_cmd_new, v_ego]) - - # Anti-wind up for integrator: only update integrator if we not against the throttle and brake limits - # do not wind up if we are changing gear and we are on the gas pedal - if (((v_error >= 0. and (output_gb_new < gas_max or Ui_accel_cmd < 0)) or - (v_error <= 0. and (output_gb_new > - brake_max or Ui_accel_cmd > 0))) and - not (v_error >= 0. and gear == 11 and output_gb_new > 0)): - #update integrator - Ui_accel_cmd = Ui_accel_cmd_new +_KP_V = [1.2, 0.8, 0.5] - accel_cmd = Ui_accel_cmd + Up_accel_cmd +_KI_BP = [0., 35.] +_KI_V = [0.18, 0.12] - # go from accel to pedals - output_gb = compute_gb([accel_cmd, v_ego]) - output_gb = output_gb[0] - - # useful to know if control is against the limit - long_control_sat = False - if output_gb > gas_max or output_gb < -brake_max: - long_control_sat = True - - output_gb = clip(output_gb, -brake_max, gas_max) - - return output_gb, Up_accel_cmd, Ui_accel_cmd, long_control_sat - - -stopping_brake_rate = 0.2 # brake_travel/s while trying to stop -starting_brake_rate = 0.6 # brake_travel/s while releasing on restart -starting_Ui = 0.5 # Since we don't have much info about acceleration at this point, be conservative +stopping_brake_rate = 0.2 # brake_travel/s while trying to stop +starting_brake_rate = 0.6 # brake_travel/s while releasing on restart +starting_Ui = 0.5 # Since we don't have much info about acceleration at this point, be conservative brake_stopping_target = 0.5 # apply at least this amount of brake to maintain the vehicle stationary _MAX_SPEED_ERROR_BP = [0., 30.] # speed breakpoints -_MAX_SPEED_ERROR_V = [1.5, .8] # max positive v_pid error VS actual speed; this avoids controls windup due to slow pedal resp +_MAX_SPEED_ERROR_V = [1.5, .8] # max positive v_pid error VS actual speed; this avoids controls windup due to slow pedal resp + class LongControl(object): - def __init__(self): - self.long_control_state = LongCtrlState.off # initialized to off - self.long_control_sat = False - self.Up_accel_cmd = 0. - self.last_output_gb = 0. - self.reset(0.) + def __init__(self, compute_gb): + self.long_control_state = LongCtrlState.off # initialized to off + self.pid = PIController((_KP_BP, _KP_V), + (_KI_BP, _KI_V), + rate=100.0, + sat_limit=0.8, + convert=compute_gb) + self.v_pid = 0.0 + self.last_output_gb = 0.0 def reset(self, v_pid): - self.Ui_accel_cmd = 0. + self.pid.reset() self.v_pid = v_pid - def update(self, enabled, v_ego, v_cruise, v_target_lead, a_target, jerk_factor, CP): - brake_max_bp = [0., 5., 20., 100.] # speeds - brake_max_v = [1.0, 1.0, 0.8, 0.8] # values - - # brake and gas limits - brake_max = interp(v_ego, brake_max_bp, brake_max_v) + def update(self, active, v_ego, brake_pressed, v_cruise, v_target_lead, a_target, + jerk_factor, CP): - # TODO: not every time - if CP.enableGas: - gas_max_bp = [0., 100.] # speeds - gas_max_v = [0.6, 0.6] # values - gas_max = interp(v_ego, gas_max_bp, gas_max_v) - else: - gas_max = 0 + # actuation limits + gas_max = interp(v_ego, CP.gasMaxBP, CP.gasMaxV) + brake_max = interp(v_ego, CP.brakeMaxBP, CP.brakeMaxV) - overshoot_allowance = 2.0 # overshoot allowed when changing accel sign + overshoot_allowance = 2.0 # overshoot allowed when changing accel sign output_gb = self.last_output_gb - rate = 100 - # limit max target speed based on cruise setting: - v_cruise_mph = round(v_cruise * CV.KPH_TO_MPH) # what's displayed in mph on the IC - v_target = min(v_target_lead, v_cruise_mph * CV.MPH_TO_MS) + # limit max target speed based on cruise setting + v_target = min(v_target_lead, v_cruise * CV.KPH_TO_MS) + rate = 100.0 + max_speed_delta_up = a_target[1] * 1.0 / rate + max_speed_delta_down = a_target[0] * 1.0 / rate - max_speed_delta_up = a_target[1]*1.0/rate - max_speed_delta_down = a_target[0]*1.0/rate - - # *** long control substate transitions - self.long_control_state = long_control_state_trans(enabled, self.long_control_state, v_ego, v_target, self.v_pid, output_gb) + self.long_control_state = long_control_state_trans(active, self.long_control_state, v_ego,\ + v_target, self.v_pid, output_gb, brake_pressed) # *** long control behavior based on state - # TODO: move this to drive_helpers - # disabled if self.long_control_state == LongCtrlState.off: - self.v_pid = v_ego # do nothing + self.v_pid = v_ego # do nothing output_gb = 0. - self.Ui_accel_cmd = 0. + self.pid.reset() + # tracking objects and driving elif self.long_control_state == LongCtrlState.pid: #reset v_pid close to v_ego if it was too far and new v_target is closer to v_ego - if ((self.v_pid > v_ego + overshoot_allowance) and - (v_target < self.v_pid)): + if ((self.v_pid > v_ego + overshoot_allowance) and (v_target < self.v_pid)): self.v_pid = max(v_target, v_ego + overshoot_allowance) - elif ((self.v_pid < v_ego - overshoot_allowance) and - (v_target > self.v_pid)): + elif ((self.v_pid < v_ego - overshoot_allowance) and (v_target > self.v_pid)): self.v_pid = min(v_target, v_ego - overshoot_allowance) # move v_pid no faster than allowed accel limits @@ -215,25 +127,31 @@ class LongControl(object): max_speed_error = interp(v_ego, _MAX_SPEED_ERROR_BP, _MAX_SPEED_ERROR_V) self.v_pid = min(self.v_pid, v_ego + max_speed_error) - # TODO: removed anti windup on gear change, does it matter? - output_gb, self.Up_accel_cmd, self.Ui_accel_cmd, self.long_control_sat = pid_long_control(v_ego, self.v_pid, \ - self.Ui_accel_cmd, gas_max, brake_max, jerk_factor, 0, rate) + self.pid.pos_limit = gas_max + self.pid.neg_limit = - brake_max + v_ego_pid = max(v_ego, 0.3) # Without this we get jumps, CAN bus reports 0 when speed < 0.3 + output_gb = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, jerk_factor=jerk_factor) + # intention is to stop, switch to a different brake control until we stop elif self.long_control_state == LongCtrlState.stopping: + # TODO: use the standstill bit from CAN to detect movements, usually more accurate than looking at v_ego if v_ego > 0. or output_gb > -brake_stopping_target: - output_gb -= stopping_brake_rate/rate + output_gb -= stopping_brake_rate / rate output_gb = clip(output_gb, -brake_max, gas_max) + self.v_pid = v_ego - self.Ui_accel_cmd = 0. + self.pid.reset() + # intention is to move again, release brake fast before handling control to PID elif self.long_control_state == LongCtrlState.starting: if output_gb < -0.2: - output_gb += starting_brake_rate/rate + output_gb += starting_brake_rate / rate self.v_pid = v_ego - self.Ui_accel_cmd = starting_Ui + self.pid.reset() + self.pid.i = starting_Ui self.last_output_gb = output_gb final_gas = clip(output_gb, 0., gas_max) final_brake = -clip(output_gb, -brake_max, 0.) - return final_gas, final_brake + return final_gas, final_brake diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index d65367b995..68b4f22cfb 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -1,66 +1,9 @@ -import math -import numpy as np - from common.numpy_fast import interp -import selfdrive.messaging as messaging - - -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)) - -# 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.] +from selfdrive.controls.lib.latcontrol_helpers import model_polyfit, calc_desired_path, compute_path_pinv -def calc_desired_path(l_poly, r_poly, p_poly, l_prob, r_prob, p_prob, speed): - #*** this function computes the poly for the center of the lane, averaging left and right polys - 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 = math.sqrt((l_prob**2 + r_prob**2) / 2.) - 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 - -class OptPathPlanner(object): - def __init__(self, model): - self.model = model - self.dead = True - self.d_poly = [0., 0., 0., 0.] - self.last_model = 0. - self._path_pinv = compute_path_pinv() - - def update(self, cur_time, v_ego, md): - if md is not None: - # simple compute of the center of the lane - pts = [(x+y)/2 for x,y in zip(md.model.leftLane.points, md.model.rightLane.points)] - self.d_poly = model_polyfit(pts, self._path_pinv) - - self.last_model = cur_time - self.dead = False - elif cur_time - self.last_model > 0.5: - self.dead = True class PathPlanner(object): def __init__(self): - self.dead = True self.d_poly = [0., 0., 0., 0.] self.c_poly = [0., 0., 0., 0.] self.c_prob = 0. @@ -68,24 +11,47 @@ class PathPlanner(object): self.lead_dist, self.lead_prob, self.lead_var = 0, 0, 1 self._path_pinv = compute_path_pinv() - def update(self, cur_time, v_ego, md): + self.lane_width_estimate = 3.7 + self.lane_width_certainty = 1.0 + self.lane_width = 3.7 + + def update(self, v_ego, md): if md is not None: - p_poly = model_polyfit(md.model.path.points, self._path_pinv) # predicted path - l_poly = model_polyfit(md.model.leftLane.points, self._path_pinv) # left line + p_poly = model_polyfit(md.model.path.points, self._path_pinv) # predicted path + l_poly = model_polyfit(md.model.leftLane.points, self._path_pinv) # left line r_poly = model_polyfit(md.model.rightLane.points, self._path_pinv) # right line - p_prob = 1. # model does not tell this probability yet, so set to 1 for now - l_prob = md.model.leftLane.prob # left line prob + p_prob = 1. # model does not tell this probability yet, so set to 1 for now + l_prob = md.model.leftLane.prob # left line prob r_prob = md.model.rightLane.prob # right line prob + # Find current lanewidth + lr_prob = l_prob * r_prob + self.lane_width_certainty += 0.05 * (lr_prob - self.lane_width_certainty) + current_lane_width = abs(l_poly[3] - r_poly[3]) + self.lane_width_estimate += 0.005 * (current_lane_width - self.lane_width_estimate) + speed_lane_width = interp(v_ego, [0., 31.], [3., 3.8]) + self.lane_width = self.lane_width_certainty * self.lane_width_estimate + \ + (1 - self.lane_width_certainty) * speed_lane_width + + lane_width_diff = abs(self.lane_width - current_lane_width) + lane_r_prob = interp(lane_width_diff, [0.3, 1.0], [1.0, 0.0]) + + r_prob *= lane_r_prob + self.lead_dist = md.model.lead.dist self.lead_prob = md.model.lead.prob self.lead_var = md.model.lead.std**2 # compute target path - self.d_poly, self.c_poly, self.c_prob = calc_desired_path(l_poly, r_poly, p_poly, l_prob, r_prob, p_prob, v_ego) + self.d_poly, self.c_poly, self.c_prob = calc_desired_path( + l_poly, r_poly, p_poly, l_prob, r_prob, p_prob, v_ego, self.lane_width) + + self.r_poly = r_poly + self.r_prob = r_prob + + self.l_poly = l_poly + self.l_prob = l_prob - self.last_model = cur_time - self.dead = False - elif cur_time - self.last_model > 0.5: - self.dead = True + self.p_poly = p_poly + self.p_prob = p_prob diff --git a/selfdrive/controls/lib/pid.py b/selfdrive/controls/lib/pid.py new file mode 100644 index 0000000000..faf5355db5 --- /dev/null +++ b/selfdrive/controls/lib/pid.py @@ -0,0 +1,91 @@ +import numpy as np +from common.numpy_fast import clip, interp +import numbers + +class PIController(object): + def __init__(self, k_p, k_i, pos_limit=None, neg_limit=None, rate=100, sat_limit=0.8, convert=None): + self._k_p = k_p + self._k_i = k_i + + self.pos_limit = pos_limit + self.neg_limit = neg_limit + + self.sat_count_rate = 1.0 / rate + self.i_unwind_rate = 0.3 / rate + self.i_rate = 1.0 / rate + self.sat_limit = sat_limit + self.jerk_factor = 0.0 + self.convert = convert + + self.reset() + + @property + def k_p(self): + if isinstance(self._k_p, numbers.Number): + kp = self._k_p + else: + kp = interp(self.speed, self._k_p[0], self._k_p[1]) + + return (1.0 + self.jerk_factor) * kp + + @property + def k_i(self): + if isinstance(self._k_i, numbers.Number): + ki = self._k_i + else: + ki = interp(self.speed, self._k_i[0], self._k_i[1]) + + return (1.0 + self.jerk_factor) * ki + + def _check_saturation(self, control, override, error): + saturated = (control < self.neg_limit) or (control > self.pos_limit) + + if saturated and not override and abs(error) > 0.1: + self.sat_count += self.sat_count_rate + else: + self.sat_count -= self.sat_count_rate + + self.sat_count = clip(self.sat_count, 0.0, 1.0) + + return self.sat_count > self.sat_limit + + def reset(self): + self.p = 0.0 + self.i = 0.0 + self.sat_count = 0.0 + self.saturated = False + self.control = 0 + + def update(self, setpoint, measurement, speed=0.0, check_saturation=True, jerk_factor=0.0, override=False): + self.speed = speed + self.jerk_factor = jerk_factor + + error = float(setpoint - measurement) + self.p = error * self.k_p + + if override: + self.i -= self.i_unwind_rate * float(np.sign(self.i)) + else: + i = self.i + error * self.k_i * self.i_rate + control = self.p + i + + if self.convert is not None: + control = self.convert(control, speed=self.speed) + + # Update when changing i will move the control away from the limits + # or when i will move towards the sign of the error + if (error >= 0 and (control <= self.pos_limit or i < 0.0)) or \ + (error <= 0 and (control >= self.neg_limit or i > 0.0)): + self.i = i + + control = self.p + self.i + if self.convert is not None: + control = self.convert(control, speed=self.speed) + + if check_saturation: + self.saturated = self._check_saturation(control, override, error) + else: + self.saturated = False + + self.control = clip(control, self.neg_limit, self.pos_limit) + return self.control diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index e9e7adb671..9cc657c138 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -1,80 +1,93 @@ #!/usr/bin/env python -import os import zmq -import numpy as np +from common.realtime import sec_since_boot import selfdrive.messaging as messaging - from selfdrive.services import service_list -from common.realtime import sec_since_boot -from common.params import Params - -from selfdrive.swaglog import cloudlog -from cereal import car - -from selfdrive.controls.lib.pathplanner import OptPathPlanner +from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET from selfdrive.controls.lib.pathplanner import PathPlanner from selfdrive.controls.lib.adaptivecruise import AdaptiveCruise from selfdrive.controls.lib.fcw import ForwardCollisionWarning _DT = 0.01 # 100Hz + class Planner(object): def __init__(self, CP): context = zmq.Context() self.CP = CP self.live20 = messaging.sub_sock(context, service_list['live20'].port) self.model = messaging.sub_sock(context, service_list['model'].port) - + self.plan = messaging.pub_sock(context, service_list['plan'].port) - + self.last_md_ts = 0 self.last_l20_ts = 0 + self.last_model = 0. + self.last_l20 = 0. + self.model_dead = True + self.radar_dead = True + self.radar_errors = [] - if os.getenv("OPT") is not None: - self.PP = OptPathPlanner(self.model) - else: - self.PP = PathPlanner() + self.PP = PathPlanner() self.AC = AdaptiveCruise() self.FCW = ForwardCollisionWarning(_DT) # this runs whenever we get a packet that can change the plan - def update(self, CS, LoC): cur_time = sec_since_boot() md = messaging.recv_sock(self.model) if md is not None: self.last_md_ts = md.logMonoTime + self.last_model = cur_time + self.model_dead = False + if cur_time - self.last_model > 0.5: + self.model_dead = True + l20 = messaging.recv_sock(self.live20) if l20 is not None: self.last_l20_ts = l20.logMonoTime + self.last_l20 = cur_time + self.radar_dead = False + self.radar_errors = list(l20.live20.radarErrors) + if cur_time - self.last_l20 > 0.5: + self.radar_dead = True - self.PP.update(cur_time, CS.vEgo, md) + self.PP.update(CS.vEgo, md) # LoC.v_pid -> CS.vEgo # TODO: is this change okay? - self.AC.update(cur_time, CS.vEgo, CS.steeringAngle, LoC.v_pid, self.CP, l20) + self.AC.update(CS.vEgo, CS.steeringAngle, LoC.v_pid, self.CP, l20) # **** send the plan **** plan_send = messaging.new_message() plan_send.init('plan') + events = [] + if self.model_dead: + events.append(create_event('modelCommIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) + if self.radar_dead or 'commIssue' in self.radar_errors: + events.append(create_event('radarCommIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) + if 'fault' in self.radar_errors: + events.append(create_event('radarFault', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) + + plan_send.plan.events = events + plan_send.plan.mdMonoTime = self.last_md_ts plan_send.plan.l20MonoTime = self.last_l20_ts # lateral plan - plan_send.plan.lateralValid = not self.PP.dead - if plan_send.plan.lateralValid: - plan_send.plan.dPoly = map(float, self.PP.d_poly) + plan_send.plan.lateralValid = not self.model_dead + plan_send.plan.dPoly = map(float, self.PP.d_poly) + plan_send.plan.laneWidth = float(self.PP.lane_width) # longitudal plan - plan_send.plan.longitudinalValid = not self.AC.dead - if plan_send.plan.longitudinalValid: - plan_send.plan.vTarget = float(self.AC.v_target_lead) - plan_send.plan.aTargetMin = float(self.AC.a_target[0]) - plan_send.plan.aTargetMax = float(self.AC.a_target[1]) - plan_send.plan.jerkFactor = float(self.AC.jerk_factor) - plan_send.plan.hasLead = self.AC.has_lead + plan_send.plan.longitudinalValid = not self.radar_dead + plan_send.plan.vTarget = float(self.AC.v_target_lead) + plan_send.plan.aTargetMin = float(self.AC.a_target[0]) + plan_send.plan.aTargetMax = float(self.AC.a_target[1]) + plan_send.plan.jerkFactor = float(self.AC.jerk_factor) + plan_send.plan.hasLead = self.AC.has_lead # compute risk of collision events: fcw self.FCW.process(CS, self.AC) diff --git a/selfdrive/controls/lib/vehicle_model.py b/selfdrive/controls/lib/vehicle_model.py index 3c676fbeab..91e26e4ff4 100755 --- a/selfdrive/controls/lib/vehicle_model.py +++ b/selfdrive/controls/lib/vehicle_model.py @@ -1,33 +1,32 @@ #!/usr/bin/env python import numpy as np from numpy.linalg import inv -from selfdrive.car.honda.interface import CarInterface - -## dynamic bycicle model from "The Science of Vehicle Dynamics (2014), M. Guiggiani"## +# dynamic bycicle model from "The Science of Vehicle Dynamics (2014), M. Guiggiani"## # Xdot = A*X + B*U # where X = [v, r], with v and r lateral speed and rotational speed, respectively # and U is the steering angle (controller input) -# +# # A depends on longitudinal speed, u, and vehicle parameters CP + def create_dyn_state_matrices(u, CP): - A = np.zeros((2,2)) - B = np.zeros((2,1)) - A[0,0] = - (CP.cF + CP.cR)/(CP.m*u) - A[0,1] = - (CP.cF*CP.aF - CP.cR*CP.aR) / (CP.m*u) - u - A[1,0] = - (CP.cF*CP.aF - CP.cR*CP.aR) / (CP.j*u) - A[1,1] = - (CP.cF*CP.aF**2 + CP.cR*CP.aR**2) / (CP.j*u) - B[0,0] = (CP.cF + CP.chi*CP.cR) / CP.m / CP.sR - B[1,0] = (CP.cF*CP.aF - CP.chi*CP.cR*CP.aR) / CP.j / CP.sR + A = np.zeros((2, 2)) + B = np.zeros((2, 1)) + A[0, 0] = - (CP.cF + CP.cR) / (CP.m * u) + A[0, 1] = - (CP.cF * CP.aF - CP.cR * CP.aR) / (CP.m * u) - u + A[1, 0] = - (CP.cF * CP.aF - CP.cR * CP.aR) / (CP.j * u) + A[1, 1] = - (CP.cF * CP.aF**2 + CP.cR * CP.aR**2) / (CP.j * u) + B[0, 0] = (CP.cF + CP.chi * CP.cR) / CP.m / CP.sR + B[1, 0] = (CP.cF * CP.aF - CP.chi * CP.cR * CP.aR) / CP.j / CP.sR return A, B def kin_ss_sol(sa, u, CP): # kinematic solution, useful when speed ~ 0 - K = np.zeros((2,1)) - K[0,0] = CP.aR / CP.sR / CP.l * u - K[1,0] = 1. / CP.sR / CP.l * u + K = np.zeros((2, 1)) + K[0, 0] = CP.aR / CP.sR / CP.l * u + K[1, 0] = 1. / CP.sR / CP.l * u return K * sa @@ -36,13 +35,15 @@ def dyn_ss_sol(sa, u, CP): A, B = create_dyn_state_matrices(u, CP) return - np.matmul(inv(A), B) * sa + def calc_slip_factor(CP): # the slip factor is a measure of how the curvature changes with speed # it's positive for Oversteering vehicle, negative (usual case) otherwise return CP.m * (CP.cF * CP.aF - CP.cR * CP.aR) / (CP.l**2 * CP.cF * CP.cR) + class VehicleModel(object): - def __init__(self, CP, init_state=np.asarray([[0.],[0.]])): + def __init__(self, CP, init_state=np.asarray([[0.], [0.]])): self.dt = 0.1 lookahead = 2. # s self.steps = int(lookahead / self.dt) @@ -54,7 +55,7 @@ class VehicleModel(object): self.state = state def steady_state_sol(self, sa, u): - # if the speed is too small we can't use the dynamic model + # if the speed is too small we can't use the dynamic model # (tire slip is undefined), we then use the kinematic model if u > 0.1: return dyn_ss_sol(sa, u, self.CP) @@ -63,13 +64,14 @@ class VehicleModel(object): def calc_curvature(self, sa, u): # this formula can be derived from state equations in steady state conditions + return self.curvature_factor(u) * sa / self.CP.sR + + def curvature_factor(self, u): sf = calc_slip_factor(self.CP) - return (1. - self.CP.chi)/(1. - sf * u**2) * sa / self.CP.sR / self.CP.l + return (1. - self.CP.chi)/(1. - sf * u**2) / self.CP.l def get_steer_from_curvature(self, curv, u): - # this function is the exact inverse of calc_curvature, returning steer angle given curvature - sf = calc_slip_factor(self.CP) - return self.CP.l * self.CP.sR * (1. - sf * u**2) / (1. - self.CP.chi) * curv + return curv * self.CP.sR * 1.0 / self.curvature_factor(u) def state_prediction(self, sa, u): # U is the matrix of the controls @@ -79,9 +81,10 @@ class VehicleModel(object): if __name__ == '__main__': + from selfdrive.car.toyota.interface import CarInterface # load car params - CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING", {}) + CP = CarInterface.get_params("TOYOTA PRIUS 2017", {}) print CP VM = VehicleModel(CP) print VM.steady_state_sol(.1, 0.15) - + print calc_slip_factor(CP) diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 00ee9182df..d966ba59f2 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -7,7 +7,7 @@ from collections import defaultdict from fastcluster import linkage_vector import selfdrive.messaging as messaging from selfdrive.services import service_list -from selfdrive.controls.lib.latcontrol import calc_lookahead_offset +from selfdrive.controls.lib.latcontrol_helpers import calc_lookahead_offset from selfdrive.controls.lib.pathplanner import PathPlanner from selfdrive.controls.lib.radar_helpers import Track, Cluster, fcluster, RDR_TO_LDR from selfdrive.controls.lib.vehicle_model import VehicleModel @@ -84,7 +84,7 @@ def radard_thread(gctx=None): tracks = defaultdict(dict) - # Kalman filter stuff: + # Kalman filter stuff: ekfv = EKFV1D() speedSensorV = SimpleSensor(XV, 1, 2) @@ -121,7 +121,7 @@ def radard_thread(gctx=None): last_md_ts = md.logMonoTime # *** get path prediction from the model *** - PP.update(sec_since_boot(), v_ego, md) + PP.update(v_ego, md) # run kalman filter only if prob is high enough if PP.lead_prob > 0.7: @@ -156,7 +156,7 @@ def radard_thread(gctx=None): continue rpt = ar_pts[ids] - # align v_ego by a fixed time to align it with the radar measurement + # align v_ego by a fixed time to align it with the radar measurement cur_time = float(rk.frame)/rate v_ego_t_aligned = np.interp(cur_time - rdr_delay, v_ego_array[1], v_ego_array[0]) d_path = np.sqrt(np.amin((path_x - rpt[0]) ** 2 + (path_y - rpt[1]) ** 2)) @@ -224,6 +224,7 @@ def radard_thread(gctx=None): dat.init('live20') dat.live20.mdMonoTime = last_md_ts dat.live20.canMonoTimes = list(rr.canMonoTimes) + dat.live20.radarErrors = list(rr.errors) dat.live20.l100MonoTime = last_l100_ts if lead_len > 0: lead_clusters[0].toLive20(dat.live20.leadOne) diff --git a/selfdrive/debug/can_printer.py b/selfdrive/debug/can_printer.py index aeab94ed8e..115b54ea60 100755 --- a/selfdrive/debug/can_printer.py +++ b/selfdrive/debug/can_printer.py @@ -1,5 +1,6 @@ #!/usr/bin/env python import os +import sys import struct from collections import defaultdict from common.realtime import sec_since_boot @@ -8,14 +9,14 @@ import selfdrive.messaging as messaging from selfdrive.services import service_list -def can_printer(): +def can_printer(bus=0, max_msg=0x10000, addr="127.0.0.1"): context = zmq.Context() - logcan = messaging.sub_sock(context, service_list['can'].port) + logcan = messaging.sub_sock(context, service_list['can'].port, addr=addr) start = sec_since_boot() lp = sec_since_boot() msgs = defaultdict(list) - canbus = int(os.getenv("CAN", 0)) + canbus = int(os.getenv("CAN", bus)) while 1: can_recv = messaging.drain_sock(logcan, wait_for_one=True) for x in can_recv: @@ -27,10 +28,18 @@ def can_printer(): dd = chr(27) + "[2J" dd += "%5.2f\n" % (sec_since_boot() - start) for k,v in sorted(zip(msgs.keys(), map(lambda x: x[-1].encode("hex"), msgs.values()))): - dd += "%s(%6d) %s\n" % ("%04X(%4d)" % (k,k),len(msgs[k]), v) + if k < max_msg: + dd += "%s(%6d) %s\n" % ("%04X(%4d)" % (k,k),len(msgs[k]), v) print dd lp = sec_since_boot() if __name__ == "__main__": - can_printer() - + if len(sys.argv) > 3: + can_printer(int(sys.argv[1]), int(sys.argv[2]), sys.argv[3]) + elif len(sys.argv) > 2: + can_printer(int(sys.argv[1]), int(sys.argv[2])) + elif len(sys.argv) > 1: + can_printer(int(sys.argv[1])) + else: + can_printer() + diff --git a/selfdrive/debug/dump.py b/selfdrive/debug/dump.py index 2387d299fd..2d31240835 100755 --- a/selfdrive/debug/dump.py +++ b/selfdrive/debug/dump.py @@ -27,5 +27,5 @@ if __name__ == "__main__": if args.raw: hexdump(sock.recv()) else: - print messaging.recv_sock(sock) + print messaging.recv_one(sock) diff --git a/selfdrive/debug/getframes/getframes.py b/selfdrive/debug/getframes/getframes.py index 5c63b8986d..498716faf1 100755 --- a/selfdrive/debug/getframes/getframes.py +++ b/selfdrive/debug/getframes/getframes.py @@ -42,26 +42,26 @@ typedef struct VisionStreamBufs { } buf_info; } VisionStreamBufs; -typedef struct VisionBuf { +typedef struct VIPCBuf { int fd; size_t len; void* addr; -} VisionBuf; +} VIPCBuf; -typedef struct VisionBufExtra { +typedef struct VIPCBufExtra { uint32_t frame_id; // only for yuv -} VisionBufExtra; +} VIPCBufExtra; typedef struct VisionStream { int ipc_fd; 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); """ diff --git a/selfdrive/debug/test_carcontroller.py b/selfdrive/debug/test_carcontroller.py index 4d9dcbda86..d672cfb3d3 100755 --- a/selfdrive/debug/test_carcontroller.py +++ b/selfdrive/debug/test_carcontroller.py @@ -28,6 +28,7 @@ if __name__ == "__main__": sendcan = messaging.pub_sock(context, service_list['sendcan'].port) CI, CP = get_car(logcan, sendcan) + CC = car.CarControl.new_message() rk = Ratekeeper(100) @@ -56,11 +57,11 @@ if __name__ == "__main__": print axis_values, button_values # **** handle car **** - CS = CI.update() + CS = CI.update(CC) #print CS - CC = car.CarControl.new_message() + CC.enabled = True CC.gas = float(np.clip(-axis_values[1], 0, 1.0)) diff --git a/selfdrive/debug/test_carstate.py b/selfdrive/debug/test_carstate.py index f88c521d1f..98d2f7fc15 100755 --- a/selfdrive/debug/test_carstate.py +++ b/selfdrive/debug/test_carstate.py @@ -1,6 +1,7 @@ #!/usr/bin/env python import os import zmq +from cereal import car import selfdrive.messaging as messaging from selfdrive.services import service_list @@ -36,8 +37,9 @@ def test_loop(): "False"] while 1: + CC = car.CarControl.new_message() # read CAN - CS = CI.update() + CS = CI.update(CC) while eval(states[state]) == True: state += 1 diff --git a/selfdrive/logcatd/Makefile b/selfdrive/logcatd/Makefile index 0d3423647a..83684abe5b 100644 --- a/selfdrive/logcatd/Makefile +++ b/selfdrive/logcatd/Makefile @@ -2,6 +2,7 @@ CC = clang CXX = clang++ PHONELIBS = ../../phonelibs +BASEDIR = ../.. WARN_FLAGS = -Werror=implicit-function-declaration \ -Werror=incompatible-pointer-types \ diff --git a/selfdrive/loggerd/config.py b/selfdrive/loggerd/config.py index 0759ae2663..9d1c3120fb 100644 --- a/selfdrive/loggerd/config.py +++ b/selfdrive/loggerd/config.py @@ -1,4 +1,9 @@ import os -ROOT = '/data/media/0/realdata/' +if os.environ.get('LOGGERD_ROOT', False): + ROOT = os.environ['LOGGERD_ROOT'] + print("Custom loggerd root: ", ROOT) +else: + ROOT = '/data/media/0/realdata/' + SEGMENT_LENGTH = 60 diff --git a/selfdrive/loggerd/loggerd b/selfdrive/loggerd/loggerd index 4f560ad028..f93d4f674b 100755 --- a/selfdrive/loggerd/loggerd +++ b/selfdrive/loggerd/loggerd @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:51d01404e86f6e1f1d2ecea67bfea72c8e568f03b4f839b4480b40c27993e685 -size 1367088 +oid sha256:2aaef8016fb65911037340c5a7a4c340b7cc49ea67723d01c1129246f8221bbe +size 1364512 diff --git a/selfdrive/manager.py b/selfdrive/manager.py index 136cd73457..1c275cf8f5 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -1,30 +1,36 @@ #!/usr/bin/env python import os +import time -# check if NEOS update is required -while 1: - if ((not os.path.isfile("/VERSION") - or int(open("/VERSION").read()) < 3) - and not os.path.isfile("/data/media/0/noupdate")): - os.system("curl -o /tmp/updater https://openpilot.comma.ai/updater && chmod +x /tmp/updater && /tmp/updater") - else: - break +if os.path.isfile("/init.qcom.rc"): + # check if NEOS update is required + while 1: + if ((not os.path.isfile("/VERSION") + or int(open("/VERSION").read()) < 3) + and not os.path.isfile("/data/media/0/noupdate")): + os.system("curl -o /tmp/updater https://openpilot.comma.ai/updater && chmod +x /tmp/updater && /tmp/updater") + else: + break + time.sleep(10) import sys -import time import importlib import subprocess import signal import traceback -import usb1 from multiprocessing import Process -from selfdrive.services import service_list +from common.basedir import BASEDIR + +sys.path.append(os.path.join(BASEDIR, "pyextra")) +os.environ['BASEDIR'] = BASEDIR +import usb1 import hashlib import zmq - from setproctitle import setproctitle +from selfdrive.services import service_list + from selfdrive.swaglog import cloudlog import selfdrive.messaging as messaging from selfdrive.thermal import read_thermal @@ -36,22 +42,20 @@ from common.params import Params from selfdrive.loggerd.config import ROOT -BASEDIR = os.path.join(os.path.dirname(os.path.realpath(__file__)), "../") - # comment out anything you don't want to run managed_processes = { "uploader": "selfdrive.loggerd.uploader", "controlsd": "selfdrive.controls.controlsd", "radard": "selfdrive.controls.radard", - "loggerd": ("loggerd", ["./loggerd"]), + "loggerd": ("selfdrive/loggerd", ["./loggerd"]), "logmessaged": "selfdrive.logmessaged", "tombstoned": "selfdrive.tombstoned", - "logcatd": ("logcatd", ["./logcatd"]), - "proclogd": ("proclogd", ["./proclogd"]), - "boardd": ("boardd", ["./boardd"]), # switch to c++ boardd - "ui": ("ui", ["./ui"]), - "visiond": ("visiond", ["./visiond"]), - "sensord": ("sensord", ["./sensord"]), } + "logcatd": ("selfdrive/logcatd", ["./logcatd"]), + "proclogd": ("selfdrive/proclogd", ["./proclogd"]), + "boardd": ("selfdrive/boardd", ["./boardd"]), # switch to c++ boardd + "ui": ("selfdrive/ui", ["./ui"]), + "visiond": ("selfdrive/visiond", ["./visiond"]), + "sensord": ("selfdrive/sensord", ["./sensord"]), } running = {} def get_running(): @@ -116,13 +120,28 @@ def start_managed_process(name): running[name] = Process(name=name, target=launcher, args=(proc, gctx)) else: pdir, pargs = proc - cwd = os.path.join(BASEDIR, "selfdrive") - if pdir is not None: - cwd = os.path.join(cwd, pdir) + cwd = os.path.join(BASEDIR, pdir) cloudlog.info("starting process %s" % name) running[name] = Process(name=name, target=nativelauncher, args=(pargs, cwd)) running[name].start() +def prepare_managed_process(p): + proc = managed_processes[p] + if isinstance(proc, basestring): + # import this python + cloudlog.info("preimporting %s" % proc) + importlib.import_module(proc) + else: + # build this process + cloudlog.info("building %s" % (proc,)) + try: + subprocess.check_call(["make", "-j4"], cwd=os.path.join(BASEDIR, proc[0])) + except subprocess.CalledProcessError: + # make clean if the build failed + cloudlog.info("building %s failed, make clean" % (proc, )) + subprocess.check_call(["make", "clean"], cwd=os.path.join(BASEDIR, proc[0])) + subprocess.check_call(["make", "-j4"], cwd=os.path.join(BASEDIR, proc[0])) + def kill_managed_process(name): if name not in running or name not in managed_processes: return @@ -212,9 +231,67 @@ def system(cmd): except subprocess.CalledProcessError, e: cloudlog.event("running failed", cmd=e.cmd, - output=e.output, + output=e.output[-1024:], returncode=e.returncode) +# TODO: this is not proper gating for EON +try: + from smbus2 import SMBus + EON = True +except ImportError: + EON = False + +def setup_eon_fan(): + if not EON: + return + + os.system("echo 2 > /sys/module/dwc3_msm/parameters/otg_switch") + + bus = SMBus(7, force=True) + bus.write_byte_data(0x21, 0x10, 0xf) # mask all interrupts + bus.write_byte_data(0x21, 0x03, 0x1) # set drive current and global interrupt disable + bus.write_byte_data(0x21, 0x02, 0x2) # needed? + bus.write_byte_data(0x21, 0x04, 0x4) # manual override source + bus.close() + +last_eon_fan_val = None +def set_eon_fan(val): + global last_eon_fan_val + + if not EON: + return + + if last_eon_fan_val is None or last_eon_fan_val != val: + bus = SMBus(7, force=True) + bus.write_byte_data(0x21, 0x04, 0x2) + bus.write_byte_data(0x21, 0x03, (val*2)+1) + bus.write_byte_data(0x21, 0x04, 0x4) + bus.close() + last_eon_fan_val = val + + +# temp thresholds to control fan speed - high hysteresis +_TEMP_THRS_H = [50., 65., 80., 10000] +# temp thresholds to control fan speed - low hysteresis +_TEMP_THRS_L = [42.5, 57.5, 72.5, 10000] +# fan speed options +_FAN_SPEEDS = [0, 16384, 32768, 65535] + +def handle_fan(max_temp, fan_speed): + new_speed_h = next(speed for speed, temp_h in zip(_FAN_SPEEDS, _TEMP_THRS_H) if temp_h > max_temp) + new_speed_l = next(speed for speed, temp_l in zip(_FAN_SPEEDS, _TEMP_THRS_L) if temp_l > max_temp) + + if new_speed_h > fan_speed: + # update speed if using the high thresholds results in fan speed increment + fan_speed = new_speed_h + elif new_speed_l < fan_speed: + # update speed if using the low thresholds results in fan speed decrement + fan_speed = new_speed_l + + set_eon_fan(fan_speed/16384) + + return fan_speed + def manager_thread(): global baseui_running @@ -233,24 +310,18 @@ def manager_thread(): start_managed_process("ui") manage_baseui(True) - panda = False - if os.getenv("NOBOARD") is None: - # *** wait for the board *** - panda = wait_for_device() == 0x2300 - - # flash the device - if os.getenv("NOPROG") is None: - # flash the board - boarddir = os.path.join(BASEDIR, "panda/board/") - mkfile = "Makefile" if panda else "Makefile.legacy" - print "using", mkfile - system("cd %s && make -f %s" % (boarddir, mkfile)) + # do this before panda flashing + setup_eon_fan() - start_managed_process("boardd") + if os.getenv("NOBOARD") is None: + from panda import ensure_st_up_to_date + ensure_st_up_to_date() + start_managed_process("boardd") started = False logger_dead = False count = 0 + fan_speed = 0 # set 5 second timeout on health socket # 5x slower than expected @@ -274,12 +345,15 @@ def manager_thread(): msg.thermal.batteryPercent = int(f.read()) with open("/sys/class/power_supply/battery/status") as f: msg.thermal.batteryStatus = f.read().strip() - thermal_sock.send(msg.to_bytes()) - print msg # TODO: add car battery voltage check max_temp = max(msg.thermal.cpu0, msg.thermal.cpu1, msg.thermal.cpu2, msg.thermal.cpu3) / 10.0 + fan_speed = handle_fan(max_temp, fan_speed) + msg.thermal.fanSpeed = fan_speed + + thermal_sock.send(msg.to_bytes()) + print msg # uploader is gated based on the phone temperature if max_temp > 85.0: @@ -352,21 +426,7 @@ def manager_prepare(): # build all processes os.chdir(os.path.dirname(os.path.abspath(__file__))) for p in managed_processes: - proc = managed_processes[p] - if isinstance(proc, basestring): - # import this python - cloudlog.info("preimporting %s" % proc) - importlib.import_module(proc) - else: - # build this process - cloudlog.info("building %s" % (proc,)) - try: - subprocess.check_call(["make", "-j4"], cwd=proc[0]) - except subprocess.CalledProcessError: - # make clean if the build failed - cloudlog.info("building %s failed, make clean" % (proc, )) - subprocess.check_call(["make", "clean"], cwd=proc[0]) - subprocess.check_call(["make", "-j4"], cwd=proc[0]) + prepare_managed_process(p) # install apks installed = get_installed_apks() diff --git a/selfdrive/proclogd/Makefile b/selfdrive/proclogd/Makefile index cd970c97e7..e7a6a57f1d 100644 --- a/selfdrive/proclogd/Makefile +++ b/selfdrive/proclogd/Makefile @@ -2,6 +2,7 @@ CC = clang CXX = clang++ PHONELIBS = ../../phonelibs +BASEDIR = ../.. WARN_FLAGS = -Werror=implicit-function-declaration \ -Werror=incompatible-pointer-types \ diff --git a/selfdrive/radar/nidec/interface.py b/selfdrive/radar/nidec/interface.py index 554ce944c6..dc8b5d073e 100755 --- a/selfdrive/radar/nidec/interface.py +++ b/selfdrive/radar/nidec/interface.py @@ -2,8 +2,7 @@ import os import numpy as np -from selfdrive.car.honda.can_parser import CANParser -from selfdrive.can.parser import CANParser as CANParserC +from selfdrive.can.parser import CANParser from selfdrive.boardd.boardd import can_capnp_to_can_list @@ -14,29 +13,27 @@ import zmq from selfdrive.services import service_list import selfdrive.messaging as messaging -NEW_CAN = os.getenv("OLD_CAN") is None - -def _create_radard_can_parser(): +def _create_nidec_can_parser(): dbc_f = 'acura_ilx_2016_nidec.dbc' - radar_messages = range(0x430, 0x43A) + range(0x440, 0x446) - signals = zip(['LONG_DIST'] * 16 + ['NEW_TRACK'] * 16 + ['LAT_DIST'] * 16 + - ['REL_SPEED'] * 16, radar_messages * 4, - [255] * 16 + [1] * 16 + [0] * 16 + [0] * 16) - checks = zip(radar_messages, [20]*16) + radar_messages = [0x400] + range(0x430, 0x43A) + range(0x440, 0x446) + signals = zip(['RADAR_STATE'] + + ['LONG_DIST'] * 16 + ['NEW_TRACK'] * 16 + ['LAT_DIST'] * 16 + + ['REL_SPEED'] * 16, + [0x400] + radar_messages[1:] * 4, + [0] + [255] * 16 + [1] * 16 + [0] * 16 + [0] * 16) + checks = zip([0x445], [20]) - if NEW_CAN: - return CANParserC(os.path.splitext(dbc_f)[0], signals, checks, 1) - else: - return CANParser(dbc_f, signals, checks) + return CANParser(os.path.splitext(dbc_f)[0], signals, checks, 1) class RadarInterface(object): def __init__(self): # radar self.pts = {} self.track_id = 0 + self.radar_fault = False # Nidec - self.rcp = _create_radard_can_parser() + self.rcp = _create_nidec_can_parser() context = zmq.Context() self.logcan = messaging.sub_sock(context, service_list['can'].port) @@ -44,39 +41,19 @@ class RadarInterface(object): def update(self): canMonoTimes = [] - if NEW_CAN: - updated_messages = set() - while 1: - tm = int(sec_since_boot() * 1e9) - updated_messages.update(self.rcp.update(tm, True)) - if 0x445 in updated_messages: - break - else: - can_pub_radar = [] - - # TODO: can hang if no packets show up - while 1: - for a in messaging.drain_sock(self.logcan, wait_for_one=True): - canMonoTimes.append(a.logMonoTime) - can_pub_radar.extend(can_capnp_to_can_list(a.can, [1, 3])) - - # only run on the 0x445 packets, used for timing - if any(x[0] == 0x445 for x in can_pub_radar): - break - - updated_messages = self.rcp.update_can(can_pub_radar) - - ret = car.RadarState.new_message() - errors = [] - if not self.rcp.can_valid: - errors.append("notValid") - ret.errors = errors - ret.canMonoTimes = canMonoTimes + updated_messages = set() + while 1: + tm = int(sec_since_boot() * 1e9) + updated_messages.update(self.rcp.update(tm, True)) + if 0x445 in updated_messages: + break for ii in updated_messages: cpt = self.rcp.vl[ii] - #print cpt - if cpt['LONG_DIST'] < 255: + if ii == 0x400: + # check for radar faults + self.radar_fault = cpt['RADAR_STATE'] != 0x79 + elif cpt['LONG_DIST'] < 255: if ii not in self.pts or cpt['NEW_TRACK']: self.pts[ii] = car.RadarState.RadarPoint.new_message() self.pts[ii].trackId = self.track_id @@ -90,6 +67,15 @@ class RadarInterface(object): if ii in self.pts: del self.pts[ii] + ret = car.RadarState.new_message() + errors = [] + if not self.rcp.can_valid: + errors.append("commIssue") + if self.radar_fault: + errors.append("fault") + ret.errors = errors + ret.canMonoTimes = canMonoTimes + ret.points = self.pts.values() return ret diff --git a/selfdrive/sensord/sensord b/selfdrive/sensord/sensord index dacc7a12be..914ceaa97b 100755 --- a/selfdrive/sensord/sensord +++ b/selfdrive/sensord/sensord @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:fc4b862553d2ec9d63740b554997e5069808c531d9d722b4ce2cf83a2f4429ed -size 935360 +oid sha256:ba62e6a2641999da0ae564166ebbcf01e1922f69dbcf9d48dc94c3bb0886e558 +size 939496 diff --git a/selfdrive/service_list.yaml b/selfdrive/service_list.yaml index e6a1ce2144..b2d3cc0768 100644 --- a/selfdrive/service_list.yaml +++ b/selfdrive/service_list.yaml @@ -43,6 +43,9 @@ lidarPts: [8030, true] procLog: [8031, true] gpsLocationExternal: [8032, true] ubloxGnss: [8033, true] +clocks: [8034, true] +liveMpc: [8035, true] + testModel: [8040, false] # manager -- base process to manage starting and stopping of all others diff --git a/selfdrive/test/plant/maneuver.py b/selfdrive/test/plant/maneuver.py index 80b2fcd2b9..8117b8b193 100644 --- a/selfdrive/test/plant/maneuver.py +++ b/selfdrive/test/plant/maneuver.py @@ -66,6 +66,8 @@ class Maneuver(object): jerk_factor=last_live100.jerkFactor, a_target_min=last_live100.aTargetMin, a_target_max=last_live100.aTargetMax) + print "maneuver end" + return (None, plot) diff --git a/selfdrive/test/plant/plant.py b/selfdrive/test/plant/plant.py index ec9e5b590f..26dcc5b229 100755 --- a/selfdrive/test/plant/plant.py +++ b/selfdrive/test/plant/plant.py @@ -13,10 +13,10 @@ import selfdrive.messaging as messaging from selfdrive.services import service_list from selfdrive.config import CruiseButtons from selfdrive.car.honda.hondacan import fix -from selfdrive.car.honda.carstate import get_can_parser +from selfdrive.car.honda.carstate import get_can_signals from selfdrive.boardd.boardd import can_capnp_to_can_list, can_list_to_can_capnp -from selfdrive.car.honda.can_parser import CANParser +from selfdrive.car.honda.old_can_parser import CANParser from selfdrive.car.honda.interface import CarInterface from cereal import car @@ -81,6 +81,11 @@ def get_car_can_parser(): ] return CANParser(dbc_f, signals, checks) +def to_3_byte(x): + return struct.pack("!H", int(x)).encode("hex")[1:] + +def to_3s_byte(x): + return struct.pack("!h", int(x)).encode("hex")[1:] class Plant(object): messaging_initialized = False @@ -142,11 +147,12 @@ class Plant(object): return float(self.rk.frame) / self.rate def step(self, v_lead=0.0, cruise_buttons=None, grade=0.0, publish_model = True): - # dbc_f, sgs, ivs, msgs, cks_msgs, frqs = initialize_can_struct(self.civic, self.brake_only) - cp2 = get_can_parser(CP) - sgs = cp2._sgs - msgs = cp2._msgs - cks_msgs = cp2.msgs_ck + gen_dbc, gen_signals, gen_checks = get_can_signals(CP) + sgs = [s[0] for s in gen_signals] + msgs = [s[1] for s in gen_signals] + cks_msgs = set(check[0] for check in gen_checks) + cks_msgs.add(0x18F) + cks_msgs.add(0x30C) # ******** get messages sent to the car ******** can_msgs = [] @@ -212,10 +218,8 @@ class Plant(object): self.user_brake, self.steer_error, self.brake_error, self.brake_error, self.gear_shifter, self.main_on, self.acc_status, self.pedal_gas, self.cruise_setting, - # left_blinker, right_blinker, counter - 0,0,0, - # interceptor_gas - 0,0] + # append one more zero for gas interceptor + 0,0,0,0] # TODO: publish each message at proper frequency can_msgs = [] @@ -236,15 +240,12 @@ class Plant(object): # add the radar message # TODO: use the DBC - def to_3_byte(x): - return struct.pack("!H", int(x)).encode("hex")[1:] - - def to_3s_byte(x): - return struct.pack("!h", int(x)).encode("hex")[1:] + radar_state_msg = '\x79\x00\x00\x00\x00\x00\x00\x00' radar_msg = to_3_byte(d_rel*16.0) + \ to_3_byte(int(lateral_pos_rel*16.0)&0x3ff) + \ to_3s_byte(int(v_rel*32.0)) + \ "0f00000" + can_msgs.append([0x400, 0, radar_state_msg, 1]) can_msgs.append([0x445, 0, radar_msg.decode("hex"), 1]) Plant.logcan.send(can_list_to_can_capnp(can_msgs).to_bytes()) diff --git a/selfdrive/test/plant/plant_ui.py b/selfdrive/test/plant/plant_ui.py new file mode 100755 index 0000000000..fed1234ca7 --- /dev/null +++ b/selfdrive/test/plant/plant_ui.py @@ -0,0 +1,122 @@ +#!/usr/bin/env python +import pygame +from plant import Plant +from selfdrive.config import CruiseButtons +import numpy as np +import selfdrive.messaging as messaging +import math + +CAR_WIDTH = 2.0 +CAR_LENGTH = 4.5 + +METER = 8 + +def rot_center(image, angle): + """rotate an image while keeping its center and size""" + orig_rect = image.get_rect() + rot_image = pygame.transform.rotate(image, angle) + rot_rect = orig_rect.copy() + rot_rect.center = rot_image.get_rect().center + rot_image = rot_image.subsurface(rot_rect).copy() + return rot_image + +def car_w_color(c): + car = pygame.Surface((METER*CAR_LENGTH, METER*CAR_LENGTH)) + car.set_alpha(0) + car.fill((10,10,10)) + car.set_alpha(128) + pygame.draw.rect(car, c, (METER*1.25, 0, METER*CAR_WIDTH, METER*CAR_LENGTH), 1) + return car + +if __name__ == "__main__": + pygame.init() + display = pygame.display.set_mode((1000, 1000)) + pygame.display.set_caption('Plant UI') + + car = car_w_color((255,0,255)) + leadcar = car_w_color((255,0,0)) + + carx, cary, heading = 10.0, 50.0, 0.0 + + plant = Plant(100, distance_lead = 40.0) + + control_offset = 2.0 + control_pts = zip(np.arange(0, 100.0, 10.0), [50.0 + control_offset]*10) + + def pt_to_car(pt): + x,y = pt + x -= carx + y -= cary + rx = x * math.cos(-heading) + y * -math.sin(-heading) + ry = x * math.sin(-heading) + y * math.cos(-heading) + return rx, ry + + def pt_from_car(pt): + x,y = pt + rx = x * math.cos(heading) + y * -math.sin(heading) + ry = x * math.sin(heading) + y * math.cos(heading) + rx += carx + ry += cary + return rx, ry + + while 1: + if plant.rk.frame%100 >= 20 and plant.rk.frame%100 <= 25: + cruise_buttons = CruiseButtons.RES_ACCEL + else: + cruise_buttons = 0 + + md = messaging.new_message() + md.init('model') + md.model.frameId = 0 + for x in [md.model.path, md.model.leftLane, md.model.rightLane]: + x.points = [0.0]*50 + x.prob = 0.0 + x.std = 1.0 + + car_pts = map(pt_to_car, control_pts) + + print car_pts + + car_poly = np.polyfit([x[0] for x in car_pts], [x[1] for x in car_pts], 3) + md.model.path.points = np.polyval(car_poly, np.arange(0, 50)).tolist() + md.model.path.prob = 1.0 + Plant.model.send(md.to_bytes()) + + plant.step(cruise_buttons = cruise_buttons, v_lead = 2.0, publish_model = False) + + display.fill((10,10,10)) + + carx += plant.speed * plant.ts * math.cos(heading) + cary += plant.speed * plant.ts * math.sin(heading) + + # positive steering angle = steering right + print plant.angle_steer + heading += plant.angle_steer * plant.ts + print heading + + # draw my car + display.blit(pygame.transform.rotate(car, 90-math.degrees(heading)), (carx*METER, cary*METER)) + + # draw control pts + for x,y in control_pts: + pygame.draw.circle(display, (255,255,0), (int(x * METER),int(y * METER)), 2) + + # draw path + path_pts = zip(np.arange(0, 50), md.model.path.points) + + for x,y in path_pts: + x,y = pt_from_car((x,y)) + pygame.draw.circle(display, (0,255,0), (int(x * METER),int(y * METER)), 1) + + """ + # draw lead car + dl = (plant.distance_lead - plant.distance) + 4.5 + lx = carx + dl * math.cos(heading) + ly = cary + dl * math.sin(heading) + + display.blit(pygame.transform.rotate(leadcar, 90-math.degrees(heading)), (lx*METER, ly*METER)) + """ + + pygame.display.flip() + + diff --git a/selfdrive/test/plant/runtest.sh b/selfdrive/test/plant/runtest.sh deleted file mode 100755 index a955d9f936..0000000000 --- a/selfdrive/test/plant/runtest.sh +++ /dev/null @@ -1,14 +0,0 @@ -#!/bin/bash - -export OPTEST=1 -export OLD_CAN=1 - -pushd ../../controls -./controlsd.py & -pid1=$! -./radard.py & -pid2=$! -trap "trap - SIGTERM && kill $pid1 && kill $pid2" SIGINT SIGTERM EXIT -popd -mkdir -p out -MPLBACKEND=svg ./runtracks.py out diff --git a/selfdrive/test/plant/runtracks.py b/selfdrive/test/plant/runtracks.py deleted file mode 100755 index 7647e2f58a..0000000000 --- a/selfdrive/test/plant/runtracks.py +++ /dev/null @@ -1,207 +0,0 @@ -#!/usr/bin/env python -import sys -import time, json - -from selfdrive.test.plant import plant -from selfdrive.config import Conversions as CV, CruiseButtons as CB -from maneuver import * - -maneuvers = [ - Maneuver( - 'while cruising at 40 mph, change cruise speed to 50mph', - duration=30., - initial_speed = 40. * CV.MPH_TO_MS, - cruise_button_presses = [(CB.DECEL_SET, 2.), (0, 2.3), - (CB.RES_ACCEL, 10.), (0, 10.1), - (CB.RES_ACCEL, 10.2), (0, 10.3)] - ), - Maneuver( - 'while cruising at 60 mph, change cruise speed to 50mph', - duration=30., - initial_speed=60. * CV.MPH_TO_MS, - cruise_button_presses = [(CB.DECEL_SET, 2.), (0, 2.3), - (CB.DECEL_SET, 10.), (0, 10.1), - (CB.DECEL_SET, 10.2), (0, 10.3)] - ), - Maneuver( - 'while cruising at 20mph, grade change +10%', - duration=25., - initial_speed=20. * CV.MPH_TO_MS, - cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)], - grade_values = [0., 0., 1.0], - grade_breakpoints = [0., 10., 11.] - ), - Maneuver( - 'while cruising at 20mph, grade change -10%', - duration=25., - initial_speed=20. * CV.MPH_TO_MS, - cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)], - grade_values = [0., 0., -1.0], - grade_breakpoints = [0., 10., 11.] - ), - Maneuver( - 'approaching a 40mph car while cruising at 60mph from 100m away', - duration=30., - initial_speed = 60. * CV.MPH_TO_MS, - lead_relevancy=True, - initial_distance_lead=100., - speed_lead_values = [40.*CV.MPH_TO_MS, 40.*CV.MPH_TO_MS], - speed_lead_breakpoints = [0., 100.], - cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)] - ), - Maneuver( - 'approaching a 0mph car while cruising at 40mph from 150m away', - duration=30., - initial_speed = 40. * CV.MPH_TO_MS, - lead_relevancy=True, - initial_distance_lead=150., - speed_lead_values = [0.*CV.MPH_TO_MS, 0.*CV.MPH_TO_MS], - speed_lead_breakpoints = [0., 100.], - cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)] - ), - Maneuver( - 'steady state following a car at 20m/s, then lead decel to 0mph at 1m/s^2', - duration=50., - initial_speed = 20., - lead_relevancy=True, - initial_distance_lead=35., - speed_lead_values = [20.*CV.MPH_TO_MS, 20.*CV.MPH_TO_MS, 0.*CV.MPH_TO_MS], - speed_lead_breakpoints = [0., 15., 35.0], - cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)] - ), - Maneuver( - 'steady state following a car at 20m/s, then lead decel to 0mph at 2m/s^2', - duration=50., - initial_speed = 20., - lead_relevancy=True, - initial_distance_lead=35., - speed_lead_values = [20.*CV.MPH_TO_MS, 20.*CV.MPH_TO_MS, 0.*CV.MPH_TO_MS], - speed_lead_breakpoints = [0., 15., 25.0], - cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)] - ), - Maneuver( - 'starting at 0mph, approaching a stopped car 100m away', - duration=30., - initial_speed = 0., - lead_relevancy=True, - initial_distance_lead=100., - cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3), - (CB.RES_ACCEL, 1.4), (0.0, 1.5), - (CB.RES_ACCEL, 1.6), (0.0, 1.7), - (CB.RES_ACCEL, 1.8), (0.0, 1.9)] - ), - Maneuver( - "following a car at 60mph, lead accel and decel at 0.5m/s^2 every 2s", - duration=25., - initial_speed=30., - lead_relevancy=True, - initial_distance_lead=49., - speed_lead_values=[30.,30.,29.,31.,29.,31.,29.], - speed_lead_breakpoints=[0., 6., 8., 12.,16.,20.,24.], - cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3), - (CB.RES_ACCEL, 1.4), (0.0, 1.5), - (CB.RES_ACCEL, 1.6), (0.0, 1.7)] - ), - Maneuver( - "following a car at 10mph, stop and go at 1m/s2 lead dece1 and accel", - duration=70., - initial_speed=10., - lead_relevancy=True, - initial_distance_lead=20., - speed_lead_values=[10., 0., 0., 10., 0.,10.], - speed_lead_breakpoints=[10., 20., 30., 40., 50., 60.], - cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3), - (CB.RES_ACCEL, 1.4), (0.0, 1.5), - (CB.RES_ACCEL, 1.6), (0.0, 1.7)] - ), - Maneuver( - "green light: stopped behind lead car, lead car accelerates at 1.5 m/s", - duration=30., - initial_speed=0., - lead_relevancy=True, - initial_distance_lead=4., - speed_lead_values=[0, 0 , 45], - speed_lead_breakpoints=[0, 10., 40.], - cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3), - (CB.RES_ACCEL, 1.4), (0.0, 1.5), - (CB.RES_ACCEL, 1.6), (0.0, 1.7), - (CB.RES_ACCEL, 1.8), (0.0, 1.9), - (CB.RES_ACCEL, 2.0), (0.0, 2.1), - (CB.RES_ACCEL, 2.2), (0.0, 2.3)] - ), - Maneuver( - "stop and go with 1m/s2 lead decel and accel, with full stops", - duration=70., - initial_speed=0., - lead_relevancy=True, - initial_distance_lead=20., - speed_lead_values=[10., 0., 0., 10., 0., 0.] , - speed_lead_breakpoints=[10., 20., 30., 40., 50., 60.], - cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3), - (CB.RES_ACCEL, 1.4), (0.0, 1.5), - (CB.RES_ACCEL, 1.6), (0.0, 1.7)] - ), - Maneuver( - "accelerate from 20 while lead vehicle decelerates from 40 to 20 at 1m/s2", - duration=30., - initial_speed=10., - lead_relevancy=True, - initial_distance_lead=10., - speed_lead_values=[20., 10.], - speed_lead_breakpoints=[1., 11.], - cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3), - (CB.RES_ACCEL, 1.4), (0.0, 1.5), - (CB.RES_ACCEL, 1.6), (0.0, 1.7), - (CB.RES_ACCEL, 1.8), (0.0, 1.9), - (CB.RES_ACCEL, 2.0), (0.0, 2.1), - (CB.RES_ACCEL, 2.2), (0.0, 2.3)] - ), - Maneuver( - "accelerate from 20 while lead vehicle decelerates from 40 to 0 at 2m/s2", - duration=30., - initial_speed=10., - lead_relevancy=True, - initial_distance_lead=10., - speed_lead_values=[20., 0.], - speed_lead_breakpoints=[1., 11.], - cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3), - (CB.RES_ACCEL, 1.4), (0.0, 1.5), - (CB.RES_ACCEL, 1.6), (0.0, 1.7), - (CB.RES_ACCEL, 1.8), (0.0, 1.9), - (CB.RES_ACCEL, 2.0), (0.0, 2.1), - (CB.RES_ACCEL, 2.2), (0.0, 2.3)] - ) -] - -css_style = """ -.maneuver_title { - font-size: 24px; - text-align: center; -} -.maneuver_graph { - width: 100%; -} -""" - -def main(output_dir): - view_html = "" % (css_style,) - for i, man in enumerate(maneuvers): - view_html += "" % (man.title,) - for c in ['distance.svg', 'speeds.svg', 'acceleration.svg', 'pedals.svg', 'pid.svg']: - view_html += "" % (os.path.join("maneuver" + str(i+1).zfill(2), c), ) - view_html += "" - - with open(os.path.join(output_dir, "index.html"), "w") as f: - f.write(view_html) - - for i, man in enumerate(maneuvers): - score, plot = man.evaluate() - plot.write_plot(output_dir, "maneuver" + str(i+1).zfill(2)) - -if __name__ == "__main__": - if len(sys.argv) <= 1: - print "Usage:", sys.argv[0], "" - exit(1) - - main(sys.argv[1]) - diff --git a/selfdrive/test/test_openpilot.py b/selfdrive/test/test_openpilot.py index 732df0cfa9..2fcce2acbc 100644 --- a/selfdrive/test/test_openpilot.py +++ b/selfdrive/test/test_openpilot.py @@ -42,25 +42,25 @@ def with_processes(processes): return wrap return wrapper -@phone_only -@with_processes(['controlsd', 'radard']) -def test_controls(): - from selfdrive.test.plant.plant import Plant - - # start the fake car for 2 seconds - plant = Plant(100) - for i in range(200): - if plant.rk.frame >= 20 and plant.rk.frame <= 25: - cruise_buttons = CruiseButtons.RES_ACCEL - # rolling forward - assert plant.speed > 0 - else: - cruise_buttons = 0 - plant.step(cruise_buttons = cruise_buttons) - plant.close() - - # assert that we stopped - assert plant.speed == 0.0 +#@phone_only +#@with_processes(['controlsd', 'radard']) +#def test_controls(): +# from selfdrive.test.plant.plant import Plant +# +# # start the fake car for 2 seconds +# plant = Plant(100) +# for i in range(200): +# if plant.rk.frame >= 20 and plant.rk.frame <= 25: +# cruise_buttons = CruiseButtons.RES_ACCEL +# # rolling forward +# assert plant.speed > 0 +# else: +# cruise_buttons = 0 +# plant.step(cruise_buttons = cruise_buttons) +# plant.close() +# +# # assert that we stopped +# assert plant.speed == 0.0 @phone_only @with_processes(['loggerd', 'logmessaged', 'tombstoned', 'proclogd', 'logcatd']) diff --git a/selfdrive/test/tests/plant/__init__.py b/selfdrive/test/tests/plant/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/test/tests/plant/test_longitudinal.py b/selfdrive/test/tests/plant/test_longitudinal.py new file mode 100755 index 0000000000..3c8286b775 --- /dev/null +++ b/selfdrive/test/tests/plant/test_longitudinal.py @@ -0,0 +1,255 @@ +#!/usr/bin/env python +import os +os.environ['OLD_CAN'] = '1' +os.environ['NOCRASH'] = '1' + +import time +import unittest +import shutil + +import matplotlib +matplotlib.use('svg') + +from selfdrive.config import Conversions as CV, CruiseButtons as CB +from selfdrive.test.plant.maneuver import Maneuver +import selfdrive.manager as manager + +def create_dir(path): + try: + os.makedirs(path) + except OSError: + pass + +maneuvers = [ + Maneuver( + 'while cruising at 40 mph, change cruise speed to 50mph', + duration=30., + initial_speed = 40. * CV.MPH_TO_MS, + cruise_button_presses = [(CB.DECEL_SET, 2.), (0, 2.3), + (CB.RES_ACCEL, 10.), (0, 10.1), + (CB.RES_ACCEL, 10.2), (0, 10.3)] + ), + Maneuver( + 'while cruising at 60 mph, change cruise speed to 50mph', + duration=30., + initial_speed=60. * CV.MPH_TO_MS, + cruise_button_presses = [(CB.DECEL_SET, 2.), (0, 2.3), + (CB.DECEL_SET, 10.), (0, 10.1), + (CB.DECEL_SET, 10.2), (0, 10.3)] + ), + Maneuver( + 'while cruising at 20mph, grade change +10%', + duration=25., + initial_speed=20. * CV.MPH_TO_MS, + cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)], + grade_values = [0., 0., 1.0], + grade_breakpoints = [0., 10., 11.] + ), + Maneuver( + 'while cruising at 20mph, grade change -10%', + duration=25., + initial_speed=20. * CV.MPH_TO_MS, + cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)], + grade_values = [0., 0., -1.0], + grade_breakpoints = [0., 10., 11.] + ), + Maneuver( + 'approaching a 40mph car while cruising at 60mph from 100m away', + duration=30., + initial_speed = 60. * CV.MPH_TO_MS, + lead_relevancy=True, + initial_distance_lead=100., + speed_lead_values = [40.*CV.MPH_TO_MS, 40.*CV.MPH_TO_MS], + speed_lead_breakpoints = [0., 100.], + cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)] + ), + Maneuver( + 'approaching a 0mph car while cruising at 40mph from 150m away', + duration=30., + initial_speed = 40. * CV.MPH_TO_MS, + lead_relevancy=True, + initial_distance_lead=150., + speed_lead_values = [0.*CV.MPH_TO_MS, 0.*CV.MPH_TO_MS], + speed_lead_breakpoints = [0., 100.], + cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)] + ), + Maneuver( + 'steady state following a car at 20m/s, then lead decel to 0mph at 1m/s^2', + duration=50., + initial_speed = 20., + lead_relevancy=True, + initial_distance_lead=35., + speed_lead_values = [20.*CV.MPH_TO_MS, 20.*CV.MPH_TO_MS, 0.*CV.MPH_TO_MS], + speed_lead_breakpoints = [0., 15., 35.0], + cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)] + ), + Maneuver( + 'steady state following a car at 20m/s, then lead decel to 0mph at 2m/s^2', + duration=50., + initial_speed = 20., + lead_relevancy=True, + initial_distance_lead=35., + speed_lead_values = [20.*CV.MPH_TO_MS, 20.*CV.MPH_TO_MS, 0.*CV.MPH_TO_MS], + speed_lead_breakpoints = [0., 15., 25.0], + cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)] + ), + Maneuver( + 'starting at 0mph, approaching a stopped car 100m away', + duration=30., + initial_speed = 0., + lead_relevancy=True, + initial_distance_lead=100., + cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3), + (CB.RES_ACCEL, 1.4), (0.0, 1.5), + (CB.RES_ACCEL, 1.6), (0.0, 1.7), + (CB.RES_ACCEL, 1.8), (0.0, 1.9)] + ), + Maneuver( + "following a car at 60mph, lead accel and decel at 0.5m/s^2 every 2s", + duration=25., + initial_speed=30., + lead_relevancy=True, + initial_distance_lead=49., + speed_lead_values=[30.,30.,29.,31.,29.,31.,29.], + speed_lead_breakpoints=[0., 6., 8., 12.,16.,20.,24.], + cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3), + (CB.RES_ACCEL, 1.4), (0.0, 1.5), + (CB.RES_ACCEL, 1.6), (0.0, 1.7)] + ), + Maneuver( + "following a car at 10mph, stop and go at 1m/s2 lead dece1 and accel", + duration=70., + initial_speed=10., + lead_relevancy=True, + initial_distance_lead=20., + speed_lead_values=[10., 0., 0., 10., 0.,10.], + speed_lead_breakpoints=[10., 20., 30., 40., 50., 60.], + cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3), + (CB.RES_ACCEL, 1.4), (0.0, 1.5), + (CB.RES_ACCEL, 1.6), (0.0, 1.7)] + ), + Maneuver( + "green light: stopped behind lead car, lead car accelerates at 1.5 m/s", + duration=30., + initial_speed=0., + lead_relevancy=True, + initial_distance_lead=4., + speed_lead_values=[0, 0 , 45], + speed_lead_breakpoints=[0, 10., 40.], + cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3), + (CB.RES_ACCEL, 1.4), (0.0, 1.5), + (CB.RES_ACCEL, 1.6), (0.0, 1.7), + (CB.RES_ACCEL, 1.8), (0.0, 1.9), + (CB.RES_ACCEL, 2.0), (0.0, 2.1), + (CB.RES_ACCEL, 2.2), (0.0, 2.3)] + ), + Maneuver( + "stop and go with 1m/s2 lead decel and accel, with full stops", + duration=70., + initial_speed=0., + lead_relevancy=True, + initial_distance_lead=20., + speed_lead_values=[10., 0., 0., 10., 0., 0.] , + speed_lead_breakpoints=[10., 20., 30., 40., 50., 60.], + cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3), + (CB.RES_ACCEL, 1.4), (0.0, 1.5), + (CB.RES_ACCEL, 1.6), (0.0, 1.7)] + ), + Maneuver( + "accelerate from 20 while lead vehicle decelerates from 40 to 20 at 1m/s2", + duration=30., + initial_speed=10., + lead_relevancy=True, + initial_distance_lead=10., + speed_lead_values=[20., 10.], + speed_lead_breakpoints=[1., 11.], + cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3), + (CB.RES_ACCEL, 1.4), (0.0, 1.5), + (CB.RES_ACCEL, 1.6), (0.0, 1.7), + (CB.RES_ACCEL, 1.8), (0.0, 1.9), + (CB.RES_ACCEL, 2.0), (0.0, 2.1), + (CB.RES_ACCEL, 2.2), (0.0, 2.3)] + ), + Maneuver( + "accelerate from 20 while lead vehicle decelerates from 40 to 0 at 2m/s2", + duration=30., + initial_speed=10., + lead_relevancy=True, + initial_distance_lead=10., + speed_lead_values=[20., 0.], + speed_lead_breakpoints=[1., 11.], + cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3), + (CB.RES_ACCEL, 1.4), (0.0, 1.5), + (CB.RES_ACCEL, 1.6), (0.0, 1.7), + (CB.RES_ACCEL, 1.8), (0.0, 1.9), + (CB.RES_ACCEL, 2.0), (0.0, 2.1), + (CB.RES_ACCEL, 2.2), (0.0, 2.3)] + ) +] + +def setup_output(): + output_dir = os.path.join(os.getcwd(), 'out/longitudinal') + if not os.path.exists(os.path.join(output_dir, "index.html")): + # write test output header + + css_style = """ + .maneuver_title { + font-size: 24px; + text-align: center; + } + .maneuver_graph { + width: 100%; + } + """ + + view_html = "
%s
" % (css_style,) + for i, man in enumerate(maneuvers): + view_html += "" % (man.title,) + for c in ['distance.svg', 'speeds.svg', 'acceleration.svg', 'pedals.svg', 'pid.svg']: + view_html += "" % (os.path.join("maneuver" + str(i+1).zfill(2), c), ) + view_html += "" + + create_dir(output_dir) + with open(os.path.join(output_dir, "index.html"), "w") as f: + f.write(view_html) + +class LongitudinalControl(unittest.TestCase): + @classmethod + def setUpClass(cls): + + setup_output() + + shutil.rmtree('/data/params', ignore_errors=True) + + manager.gctx = {} + manager.prepare_managed_process('radard') + manager.prepare_managed_process('controlsd') + + manager.start_managed_process('radard') + manager.start_managed_process('controlsd') + + @classmethod + def tearDownClass(cls): + manager.kill_managed_process('radard') + manager.kill_managed_process('controlsd') + time.sleep(5) + + # hack + def test_longitudinal_setup(self): + pass + +WORKERS = 8 +def run_maneuver_worker(k): + output_dir = os.path.join(os.getcwd(), 'out/longitudinal') + for i, man in enumerate(maneuvers[k::WORKERS]): + score, plot = man.evaluate() + plot.write_plot(output_dir, "maneuver" + str(WORKERS * i + k+1).zfill(2)) + +for k in xrange(WORKERS): + setattr(LongitudinalControl, + "test_longitudinal_maneuvers_%d" % (k+1), + lambda self, k=k: run_maneuver_worker(k)) + +if __name__ == "__main__": + unittest.main() + diff --git a/selfdrive/ui/Makefile b/selfdrive/ui/Makefile index d2289228e3..d0a3778e21 100644 --- a/selfdrive/ui/Makefile +++ b/selfdrive/ui/Makefile @@ -55,9 +55,10 @@ ui: $(OBJS) $(OPENGL_LIBS) \ -lcutils -lm -llog -../common/framebuffer.o: ../common/framebuffer.cc +%.o: %.cc @echo "[ CXX ] $@" $(CXX) $(CXXFLAGS) -MMD \ + -Iinclude -I.. -I../.. \ -I$(PHONELIBS)/android_frameworks_native/include \ -I$(PHONELIBS)/android_system_core/include \ -I$(PHONELIBS)/android_hardware_libhardware/include \ diff --git a/selfdrive/ui/ui.c b/selfdrive/ui/ui.c index fc5c16a942..68d979ec99 100644 --- a/selfdrive/ui/ui.c +++ b/selfdrive/ui/ui.c @@ -38,16 +38,18 @@ typedef struct UIScene { int frontview; uint8_t *bgr_ptr; - int big_box_x, big_box_y, big_box_width, big_box_height; int transformed_width, transformed_height; uint64_t model_ts; ModelData model; + float mpc_x[50]; + float mpc_y[50]; + bool world_objects_visible; // TODO(mgraczyk): Remove and use full frame for everything. - mat3 warp_matrix; // transformed box -> big_box. + mat3 warp_matrix; // transformed box -> frame. mat4 extrinsic_matrix; // Last row is 0 so we can use mat4. float v_cruise; @@ -88,14 +90,16 @@ typedef struct UIState { void *livecalibration_sock_raw; zsock_t *live20_sock; void *live20_sock_raw; + zsock_t *livempc_sock; + void *livempc_sock_raw; // vision state bool vision_connected; bool vision_connect_firstrun; int ipc_fd; - VisionBuf bufs[UI_BUF_COUNT]; - VisionBuf front_bufs[UI_BUF_COUNT]; + VIPCBuf bufs[UI_BUF_COUNT]; + VIPCBuf front_bufs[UI_BUF_COUNT]; int cur_vision_idx; int cur_vision_front_idx; @@ -228,6 +232,11 @@ static void ui_init(UIState *s) { assert(s->live20_sock); s->live20_sock_raw = zsock_resolve(s->live20_sock); + s->livempc_sock = zsock_new_sub(">tcp://127.0.0.1:8035", ""); + assert(s->livempc_sock); + s->livempc_sock_raw = zsock_resolve(s->livempc_sock); + + s->ipc_fd = -1; // init display @@ -271,8 +280,7 @@ static void ui_init(UIState *s) { // intrinsic_matrix and returns true. Otherwise returns false. static bool try_load_intrinsics(mat3 *intrinsic_matrix) { char *value; - const int result = - read_db_value("/data/params", "CloudCalibration", &value, NULL); + const int result = read_db_value(NULL, "CloudCalibration", &value, NULL); if (result == 0) { JsonNode* calibration_json = json_decode(value); @@ -309,18 +317,14 @@ static void ui_init_vision(UIState *s, const VisionStreamBufs back_bufs, assert(num_back_fds == UI_BUF_COUNT); assert(num_front_fds == UI_BUF_COUNT); - visionbufs_load(s->bufs, &back_bufs, num_back_fds, back_fds); - visionbufs_load(s->front_bufs, &front_bufs, num_front_fds, front_fds); + vipc_bufs_load(s->bufs, &back_bufs, num_back_fds, back_fds); + vipc_bufs_load(s->front_bufs, &front_bufs, num_front_fds, front_fds); s->cur_vision_idx = -1; s->cur_vision_front_idx = -1; s->scene = (UIScene){ .frontview = 0, - .big_box_x = ui_info.big_box_x, - .big_box_y = ui_info.big_box_y, - .big_box_width = ui_info.big_box_width, - .big_box_height = ui_info.big_box_height, .transformed_width = ui_info.transformed_width, .transformed_height = ui_info.transformed_height, .front_box_x = ui_info.front_box_x, @@ -344,7 +348,7 @@ static void ui_init_vision(UIState *s, const VisionStreamBufs back_bufs, }}; char *value; - const int result = read_db_value("/data/params", "IsMetric", &value, NULL); + const int result = read_db_value(NULL, "IsMetric", &value, NULL); if (result == 0) { s->is_metric = value[0] == '1'; free(value); @@ -392,9 +396,8 @@ static void ui_draw_transformed_box(UIState *s, uint32_t color) { }; for (int i=0; ibig_box_x + verts[i].pos.v[0] / verts[i].pos.v[2]; - verts[i].pos.v[1] = s->rgb_height - (scene->big_box_y + - verts[i].pos.v[1] / verts[i].pos.v[2]); + verts[i].pos.v[0] = verts[i].pos.v[0] / verts[i].pos.v[2]; + verts[i].pos.v[1] = s->rgb_height - verts[i].pos.v[1] / verts[i].pos.v[2]; } glUseProgram(s->line_program); @@ -469,6 +472,51 @@ static void draw_cross(UIState *s, float x_in, float y_in, float sz, NVGcolor co nvgRestore(s->vg); } +static void draw_x_y(UIState *s, const float *x_coords, const float *y_coords, size_t num_points, + NVGcolor color) { + const UIScene *scene = &s->scene; + + nvgSave(s->vg); + + // path coords are worked out in rgb-box space + nvgTranslate(s->vg, 240.0f, 0.0); + + // zooom in 2x + nvgTranslate(s->vg, -1440.0f / 2, -1080.0f / 2); + nvgScale(s->vg, 2.0, 2.0); + + nvgScale(s->vg, 1440.0f / s->rgb_width, 1080.0f / s->rgb_height); + + nvgBeginPath(s->vg); + nvgStrokeColor(s->vg, color); + nvgStrokeWidth(s->vg, 2); + bool started = false; + + for (int i=0; ivg, x, y); + started = true; + } else { + nvgLineTo(s->vg, x, y); + } + } + + nvgStroke(s->vg); + + nvgRestore(s->vg); +} + static void draw_path(UIState *s, const float *points, float off, NVGcolor color) { const UIScene *scene = &s->scene; @@ -669,6 +717,8 @@ static void ui_draw_world(UIState *s) { draw_model_path( s, scene->model.right_lane, nvgRGBA(0, (int)(255 * scene->model.right_lane.prob), 0, 128)); + + draw_x_y(s, scene->mpc_x, scene->mpc_y, 50, nvgRGBA(255, 0, 0, 255)); } if (scene->lead_status) { @@ -939,7 +989,7 @@ static void ui_update(UIState *s) { // poll for events while (true) { - zmq_pollitem_t polls[5] = {{0}}; + zmq_pollitem_t polls[6] = {{0}}; polls[0].socket = s->live100_sock_raw; polls[0].events = ZMQ_POLLIN; polls[1].socket = s->livecalibration_sock_raw; @@ -948,15 +998,19 @@ static void ui_update(UIState *s) { polls[2].events = ZMQ_POLLIN; polls[3].socket = s->live20_sock_raw; polls[3].events = ZMQ_POLLIN; - - int num_polls = 4; + polls[4].socket = s->livempc_sock_raw; + polls[4].events = ZMQ_POLLIN; + + int num_polls = 5; if (s->vision_connected) { assert(s->ipc_fd >= 0); - polls[4].fd = s->ipc_fd; - polls[4].events = ZMQ_POLLIN; + polls[5].fd = s->ipc_fd; + polls[5].events = ZMQ_POLLIN; num_polls++; } + + int ret = zmq_poll(polls, num_polls, 0); if (ret < 0) { LOGW("poll failed (%d)", ret); @@ -969,7 +1023,7 @@ static void ui_update(UIState *s) { // awake on any activity set_awake(s, true); - if (s->vision_connected && polls[4].revents) { + if (s->vision_connected && polls[5].revents) { // vision ipc event VisionPacket rp; err = vipc_recv(s->ipc_fd, &rp); @@ -1021,7 +1075,7 @@ static void ui_update(UIState *s) { } else { // zmq messages void* which = NULL; - for (int i=0; i<4; i++) { + for (int i=0; i<5; i++) { if (polls[i].revents) { which = polls[i].socket; break; @@ -1085,7 +1139,7 @@ static void ui_update(UIState *s) { cereal_read_LiveCalibrationData(&datad, eventd.liveCalibration); // should we still even have this? - capn_list32 warpl = datad.warpMatrix; + capn_list32 warpl = datad.warpMatrix2; capn_resolve(&warpl.p); // is this a bug? for (int i = 0; i < 3 * 3; i++) { s->scene.warp_matrix.v[i] = capn_to_f32(capn_get32(warpl, i)); @@ -1100,6 +1154,23 @@ static void ui_update(UIState *s) { } else if (eventd.which == cereal_Event_model) { s->scene.model_ts = eventd.logMonoTime; s->scene.model = read_model(eventd.model); + } else if (eventd.which == cereal_Event_liveMpc) { + struct cereal_LiveMpcData datad; + cereal_read_LiveMpcData(&datad, eventd.liveMpc); + + capn_list32 x_list = datad.x; + capn_resolve(&x_list.p); + + for (int i = 0; i < 50; i++){ + s->scene.mpc_x[i] = capn_to_f32(capn_get32(x_list, i)); + } + + capn_list32 y_list = datad.y; + capn_resolve(&y_list.p); + + for (int i = 0; i < 50; i++){ + s->scene.mpc_y[i] = capn_to_f32(capn_get32(y_list, i)); + } } capn_free(&ctx); diff --git a/selfdrive/visiond/visiond b/selfdrive/visiond/visiond index f2b8f6bdb2..a1ca9e3477 100755 --- a/selfdrive/visiond/visiond +++ b/selfdrive/visiond/visiond @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:e2dc36a454c0447387826dd0de64ee47e170012df0ec041189ae77d64aa6de3f -size 16407976 +oid sha256:a5078532dfc830e278a83b885c19f8e7a2a7073ea3f3bb389f2323702f40cb6b +size 13285152
%s