diff --git a/Dockerfile.openpilot b/Dockerfile.openpilot index 4efafd40df..98e1c0abf5 100644 --- a/Dockerfile.openpilot +++ b/Dockerfile.openpilot @@ -16,4 +16,4 @@ 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 +RUN mkdir -p /tmp/openpilot/selfdrive/test/out diff --git a/Makefile b/Makefile index 14dcee805c..4f690b2e7e 100644 --- a/Makefile +++ b/Makefile @@ -1,6 +1,9 @@ -.PHONY: all -# TODO: Add a global build system to openpilot +code_dir := $(shell pwd) + +# TODO: Add a global build system + +.PHONY: all all: - cd /data/openpilot/selfdrive && PYTHONPATH=/data/openpilot PREPAREONLY=1 /data/openpilot/selfdrive/manager.py + cd selfdrive && PYTHONPATH=$(code_dir) PREPAREONLY=1 ./manager.py diff --git a/README.md b/README.md index c70574e84a..f7c682c864 100644 --- a/README.md +++ b/README.md @@ -12,7 +12,7 @@ Here are [some](https://www.youtube.com/watch?v=9OwTJFuDI7g) [videos](https://ww Hardware ------ -Right now openpilot supports the [neo research platform](http://github.com/commaai/neo) for vehicle control. We'd like to support other platforms as well. +Right now openpilot supports the [NEO research platform](http://github.com/commaai/neo) and the [EON Dashcam DevKit](https://shop.comma.ai/products/eon-dashcam-devkit). We'd like to support other platforms as well. Install openpilot on a neo device by entering ``https://openpilot.comma.ai`` during NEOS setup. @@ -26,15 +26,25 @@ Supported Cars - 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!) +- Honda CR-V Touring 2015-2016 - Can only be enabled above 25 mph +- Toyota RAV-4 2016+ with TSS-P (alpha!) + - Can only be enabled above 20 mph + In Progress Cars ------ +- Toyota Prius 2017 + +- Probably all TSS-P Toyota + +Community Ported Cars +------ + - Chevy Volt 2016-2018 Premier with Driver Confidence II -- All 2017 Toyota Prius, Corolla, and RAV4 +- Classic Tesla Model S (pre-AP) Directory structure ------ diff --git a/RELEASES.md b/RELEASES.md index bf4baad56c..0aa52ec800 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,3 +1,9 @@ +Version 0.3.8.2 (2017-10-30) +========================== + * Add alpha support for 2017 Toyota RAV4 + * Stay silent if stock system is connected through giraffe + * Minor bug fixes + Version 0.3.7 (2017-09-30) ========================== * Improved lateral control using model predictive control diff --git a/cereal/Makefile b/cereal/Makefile index 3a580592b1..b384b4904e 100644 --- a/cereal/Makefile +++ b/cereal/Makefile @@ -1,8 +1,9 @@ +PWD := $(shell pwd) + 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 @@ -16,6 +17,12 @@ endif endif +ifeq ($(UNAME_M),aarch64) +CAPNPC=PATH=$(PWD)/../phonelibs/capnp-cpp/aarch64/bin/:$$PATH capnpc +else +CAPNPC=capnpc +endif + .PHONY: all all: $(GENS) @@ -26,17 +33,17 @@ clean: gen/c/%.capnp.c: %.capnp @echo "[ CAPNPC C ] $@" mkdir -p gen/c/ - capnpc '$<' -o c:gen/c/ + $(CAPNPC) '$<' -o c:gen/c/ gen/cpp/%.capnp.c++: %.capnp @echo "[ CAPNPC C++ ] $@" mkdir -p gen/cpp/ - capnpc '$<' -o c++: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 + $(CAPNPC) $^ -o java:gen/java # c-capnproto needs some empty headers gen/c/c++.capnp.h gen/c/java.capnp.h: diff --git a/cereal/car.capnp b/cereal/car.capnp index 6b5a019c7b..bafc77f80d 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -51,6 +51,8 @@ struct CarEvent @0x9b1657f34caf3ad3 { modelCommIssue @27; brakeHold @28; parkBrake @29; + manualRestart @30; + lowSpeedLockout @31; } } @@ -63,6 +65,9 @@ struct CarState { # car speed vEgo @1 :Float32; # best estimate of speed + aEgo @16 :Float32; # best estimate of acceleration + vEgoRaw @17 :Float32; # unfiltered speed + standstill @18 :Bool; wheelSpeeds @2 :WheelSpeeds; # gas pedal, 0.0-1.0 @@ -75,6 +80,7 @@ struct CarState { # steering wheel steeringAngle @7 :Float32; # deg + steeringRate @15 :Float32; # deg/s steeringTorque @8 :Float32; # TODO: standardize units steeringPressed @9 :Bool; # if the user is using the steering wheel @@ -242,6 +248,9 @@ struct CarParams { enableGas @4 :Bool; enableBrake @5 :Bool; enableCruise @6 :Bool; + enableCamera @27 :Bool; + enableDsu @28 :Bool; # driving support unit + enableApgs @29 :Bool; # advanced parking guidance system minEnableSpeed @18 :Float32; safetyModel @19 :Int16; @@ -254,10 +263,11 @@ struct CarParams { brakeMaxV @25 :List(Float32); enum SafetyModels { - # from board - default @0; + # does NOT match board setting + noOutput @0; honda @1; toyota @2; + elm327 @3; } # things about the car in the manual @@ -276,7 +286,9 @@ struct CarParams { # Kp and Ki for the lateral control steerKp @16 :Float32; steerKi @17 :Float32; + steerKf @26 :Float32; + + steerLimitAlert @30 :Bool; # TODO: Kp and Ki for long control, perhaps not needed? } - diff --git a/cereal/log.capnp b/cereal/log.capnp index 1496ff1356..6193d2c3ee 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -36,6 +36,7 @@ struct InitData { pandaInfo @8 :PandaInfo; dirty @9 :Bool; + passive @12 :Bool; enum DeviceType { unknown @0; @@ -327,8 +328,11 @@ struct Live100Data { mdMonoTimeDEPRECATED @18 :UInt64; planMonoTime @28 :UInt64; + state @31 :ControlState; vEgo @0 :Float32; + vEgoRaw @32 :Float32; aEgoDEPRECATED @1 :Float32; + longControlState @30 :LongControlState; vPid @2 :Float32; vTargetLead @3 :Float32; upAccelCmd @4 :Float32; @@ -356,6 +360,20 @@ struct Live100Data { awarenessStatus @26 :Float32; angleOffset @27 :Float32; + + enum ControlState { + disabled @0; + preEnabled @1; + enabled @2; + softDisabling @3; + } + + enum LongControlState { + off @0; + pid @1; + stopping @2; + starting @3; + } } struct LiveEventData { @@ -370,6 +388,7 @@ struct ModelData { leftLane @2 :PathData; rightLane @3 :PathData; lead @4 :LeadData; + leadNew @6 :List(Float32); settings @5 :ModelSettings; @@ -454,11 +473,13 @@ struct Plan { # longitudinal longitudinalValid @2 :Bool; vTarget @3 :Float32; + vTargetFuture @14 :Float32; aTargetMin @4 :Float32; aTargetMax @5 :Float32; jerkFactor @6 :Float32; hasLead @7 :Bool; fcw @8 :Bool; + longitudinalPlanSource @15 :LongitudinalPlanSource; # gps trajectory in car frame gpsTrajectory @12 :GpsTrajectory; @@ -467,6 +488,12 @@ struct Plan { x @0 :List(Float32); y @1 :List(Float32); } + + enum LongitudinalPlanSource { + cruise @0; + mpc1 @1; + mpc2 @2; + } } struct LiveLocationData { @@ -1073,6 +1100,7 @@ struct UbloxGnss { union { measurementReport @0 :MeasurementReport; ephemeris @1 :Ephemeris; + ionoData @2 :IonoData; } struct MeasurementReport { @@ -1175,9 +1203,24 @@ struct UbloxGnss { transmissionTime @34 :Float64; fitInterval @35 :Float64; + + toc @36 :Float64; + } + + struct IonoData { + svHealth @0 :UInt32; + tow @1 :Float64; + gpsWeek @2 :Float64; + + ionoAlpha @3 :List(Float64); + ionoBeta @4 :List(Float64); + + healthValid @5 :Bool; + ionoCoeffsValid @6 :Bool; } } + struct Clocks { bootTimeNanos @0 :UInt64; monotonicNanos @1 :UInt64; @@ -1191,6 +1234,21 @@ struct LiveMpcData { y @1 :List(Float32); psi @2 :List(Float32); delta @3 :List(Float32); + qpIterations @4 :UInt32; + calculationTime @5 :UInt64; +} + +struct LiveLongitudinalMpcData { + xEgo @0 :List(Float32); + vEgo @1 :List(Float32); + aEgo @2 :List(Float32); + xLead @3 :List(Float32); + vLead @4 :List(Float32); + aLead @5 :List(Float32); + aLeadTau @6 :Float32; # lead accel time constant + qpIterations @7 :UInt32; + mpcId @8 :UInt32; + calculationTime @9 :UInt64; } struct Event { @@ -1234,5 +1292,6 @@ struct Event { ubloxGnss @34 :UbloxGnss; clocks @35 :Clocks; liveMpc @36 :LiveMpcData; + liveLongitudinalMpc @37 :LiveLongitudinalMpcData; } } diff --git a/common/dbc.py b/common/dbc.py index 518890326b..6b93cf410c 100755 --- a/common/dbc.py +++ b/common/dbc.py @@ -192,4 +192,15 @@ class dbc(object): else: out[arr.index(s[0])] = ival return name, out + + + def get_signals(self, msg): + return [sgs.name for sgs in self.msgs[msg][1]] +if __name__ == "__main__": + import sys + import os + from opendbc import DBC_PATH + + dbc_test = dbc(os.path.join(DBC_PATH, sys.argv[1])) + print dbc_test.get_signals(0xe4) diff --git a/common/fingerprints.py b/common/fingerprints.py index 34c84b7449..101f6cb5d4 100644 --- a/common/fingerprints.py +++ b/common/fingerprints.py @@ -11,19 +11,23 @@ _FINGERPRINTS = { # sent messages 0xe4: 5, 0x1fa: 8, 0x200: 3, 0x30c: 8, 0x33d: 5, 0x35e: 8, 0x39f: 8, }, - "HONDA ACCORD 2016 TOURING": { - 1024L: 5, 929L: 8, 1027L: 5, 773L: 7, 1601L: 8, 777L: 8, 1036L: 8, 398L: 3, 1039L: 8, 401L: 8, 145L: 8, 1424L: 5, 660L: 8, 661L: 4, 918L: 7, 985L: 3, 923L: 2, 542L: 7, 927L: 8, 800L: 8, 545L: 4, 420L: 8, 422L: 8, 808L: 8, 426L: 8, 1029L: 8, 432L: 7, 57L: 3, 316L: 8, 829L: 5, 1600L: 5, 1089L: 8, 1057L: 5, 780L: 8, 1088L: 8, 464L: 8, 1108L: 8, 597L: 8, 342L: 6, 983L: 8, 344L: 8, 804L: 8, 476L: 4, 1296L: 3, 891L: 8, 1125L: 8, 487L: 4, 892L: 8, 490L: 8, 871L: 8, 1064L: 7, 882L: 2, 884L: 8, 506L: 8, 507L: 1, 380L: 8, 1365L: 5 - }, "HONDA CR-V 2016 TOURING": { 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 - } + "TOYOTA RAV4 2017": { + 36L: 8, 37L: 8, 170L: 8, 180L: 8, 186L: 4, 426L: 6, 452L: 8, 464L: 8, 466L: 8, 467L: 8, 547L: 8, 548L: 8, 552L: 4, 562L: 4, 608L: 8, 610L: 5, 643L: 7, 705L: 8, 725L: 2, 740L: 5, 800L: 8, 835L: 8, 836L: 8, 849L: 4, 869L: 7, 870L: 7, 871L: 2, 896L: 8, 897L: 8, 900L: 6, 902L: 6, 905L: 8, 911L: 8, 916L: 3, 918L: 7, 921L: 8, 933L: 8, 944L: 8, 945L: 8, 951L: 8, 955L: 4, 956L: 8, 979L: 2, 998L: 5, 999L: 7, 1000L: 8, 1001L: 8, 1008L: 2, 1014L: 8, 1017L: 8, 1041L: 8, 1042L: 8, 1043L: 8, 1044L: 8, 1056L: 8, 1059L: 1, 1114L: 8, 1161L: 8, 1162L: 8, 1163L: 8, 1176L: 8, 1177L: 8, 1178L: 8, 1179L: 8, 1180L: 8, 1181L: 8, 1190L: 8, 1191L: 8, 1192L: 8, 1196L: 8, 1227L: 8, 1228L: 8, 1235L: 8, 1237L: 8, 1263L: 8, 1279L: 8, 1408L: 8, 1409L: 8, 1410L: 8, 1552L: 8, 1553L: 8, 1554L: 8, 1555L: 8, 1556L: 8, 1557L: 8, 1561L: 8, 1562L: 8, 1568L: 8, 1569L: 8, 1570L: 8, 1571L: 8, 1572L: 8, 1584L: 8, 1589L: 8, 1592L: 8, 1593L: 8, 1595L: 8, 1596L: 8, 1597L: 8, 1600L: 8, 1656L: 8, 1664L: 8, 1728L: 8, 1745L: 8, 1779L: 8, 1904L: 8, 1912L: 8, 1990L: 8, 1998L: 8 + }, } +# support additional internal only fingerprints +try: + from common.fingerprints_internal import add_additional_fingerprints + add_additional_fingerprints(_FINGERPRINTS) +except ImportError: + pass + def eliminate_incompatible_cars(msg, candidate_cars): """Removes cars that could not have sent msg. @@ -50,36 +54,3 @@ def all_known_cars(): """Returns a list of all known car strings.""" return _FINGERPRINTS.keys() -# **** for use live only **** -def fingerprint(logcan): - import selfdrive.messaging as messaging - from cereal import car - from common.realtime import sec_since_boot - - if os.getenv("SIMULATOR") is not None or logcan is None: - return ("simulator", None) - elif os.getenv("SIMULATOR2") is not None: - return ("simulator2", None) - - print "waiting for fingerprint..." - candidate_cars = all_known_cars() - finger = {} - st = None - while 1: - for a in messaging.drain_sock(logcan, wait_for_one=True): - if st is None: - st = sec_since_boot() - for can in a.can: - if can.src == 0: - finger[can.address] = len(can.dat) - candidate_cars = eliminate_incompatible_cars(can, candidate_cars) - - # if we only have one car choice and it's been 100ms since we got our first message, exit - if len(candidate_cars) == 1 and st is not None and (sec_since_boot()-st) > 0.1: - break - elif len(candidate_cars) == 0: - print map(hex, finger.keys()) - raise Exception("car doesn't match any fingerprints") - - print "fingerprinted", candidate_cars[0] - return (candidate_cars[0], finger) diff --git a/common/params.py b/common/params.py index ca3ee99923..7e2ea7900a 100755 --- a/common/params.py +++ b/common/params.py @@ -64,7 +64,10 @@ keys = { "CloudCalibration": TxType.PERSISTANT, # written: controlsd # read: radard - "CarParams": TxType.CLEAR_ON_CAR_START} + "CarParams": TxType.CLEAR_ON_CAR_START, + + "Passive": TxType.PERSISTANT, +} def fsync_dir(path): fd = os.open(path, os.O_RDONLY) diff --git a/opendbc b/opendbc index b8c0034c5d..835a9739d6 160000 --- a/opendbc +++ b/opendbc @@ -1 +1 @@ -Subproject commit b8c0034c5d6ded3d85a4f0414b3509baae35a34b +Subproject commit 835a9739d6721e351e1814439b55b6c4212f7b85 diff --git a/panda b/panda index 5ea4df111b..849f68879d 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 5ea4df111b735ce9efb27ab2e0f87837210f6fc3 +Subproject commit 849f68879d1ceacbf1f9d4174e16e1cd14527383 diff --git a/selfdrive/boardd/Makefile b/selfdrive/boardd/Makefile index cb00d6ac1a..1573304e67 100644 --- a/selfdrive/boardd/Makefile +++ b/selfdrive/boardd/Makefile @@ -25,18 +25,16 @@ JSON_FLAGS = -I$(PHONELIBS)/json/src EXTRA_LIBS = -lusb -ifeq ($(OS),GNU/Linux) -# for Drive PX2 -ZMQ_LIBS = -lczmq -lzmq -CEREAL_LIBS = -lcapnp -lkj -lcapnp_c -EXTRA_LIBS = -lusb-1.0 -lpthread -endif +# ifeq ($(OS),GNU/Linux) +# # for Drive PX2 +# ZMQ_LIBS = -lczmq -lzmq +# CEREAL_LIBS = -lcapnp -lkj -lcapnp_c +# EXTRA_LIBS = -lusb-1.0 -lpthread +# endif ifeq ($(ARCH),x86_64) -ZMQ_LIBS = -L$(HOME)/one/external/zmq/lib/ \ +ZMQ_LIBS = -L$(BASEDIR)/external/zmq/lib/ \ -l:libczmq.a -l:libzmq.a -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 diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index c7e3d01603..48aecca0cd 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -25,7 +25,18 @@ #include "common/swaglog.h" #include "common/timing.h" -int do_exit = 0; +// double the FIFO size +#define RECV_SIZE (0x1000) +#define TIMEOUT 0 + +#define SAFETY_NOOUTPUT 0 +#define SAFETY_HONDA 1 +#define SAFETY_TOYOTA 2 +#define SAFETY_ELM327 0xE327 + +namespace { + +volatile int do_exit = 0; libusb_context *ctx = NULL; libusb_device_handle *dev_handle; @@ -35,12 +46,6 @@ bool spoofing_started = false; bool fake_send = false; bool loopback_can = false; -// double the FIFO size -#define RECV_SIZE (0x1000) -#define TIMEOUT 0 - -#define SAFETY_NOOUTPUT 0x0000 - pthread_t safety_setter_thread_handle = -1; void *safety_setter_thread(void *s) { @@ -49,6 +54,8 @@ void *safety_setter_thread(void *s) { LOGW("waiting for params to set safety model"); while (1) { + if (do_exit) return NULL; + const int result = read_db_value(NULL, "CarParams", &value, &value_sz); if (value_sz > 0) break; usleep(100*1000); @@ -62,15 +69,33 @@ void *safety_setter_thread(void *s) { capnp::FlatArrayMessageReader cmsg(amsg); cereal::CarParams::Reader car_params = cmsg.getRoot(); - int safety_model = car_params.getSafetyModel(); + auto safety_model = car_params.getSafetyModel(); LOGW("setting safety model: %d", safety_model); + int safety_setting = 0; + switch (safety_model) { + case (int)cereal::CarParams::SafetyModels::NO_OUTPUT: + safety_setting = SAFETY_NOOUTPUT; + break; + case (int)cereal::CarParams::SafetyModels::HONDA: + safety_setting = SAFETY_HONDA; + break; + case (int)cereal::CarParams::SafetyModels::TOYOTA: + safety_setting = SAFETY_TOYOTA; + break; + case (int)cereal::CarParams::SafetyModels::ELM327: + safety_setting = SAFETY_ELM327; + break; + default: + LOGE("unknown 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); + libusb_control_transfer(dev_handle, 0x40, 0xdc, safety_setting, 0, NULL, 0, TIMEOUT); pthread_mutex_unlock(&usb_lock); @@ -388,6 +413,8 @@ int set_realtime_priority(int level) { return sched_setscheduler(getpid(), SCHED_FIFO, &sa); } +} + int main() { int err; LOGW("starting boardd"); diff --git a/selfdrive/boardd/boardd.py b/selfdrive/boardd/boardd.py index 06abe8b3a5..0b428d04ee 100755 --- a/selfdrive/boardd/boardd.py +++ b/selfdrive/boardd/boardd.py @@ -116,6 +116,7 @@ def boardd_mock_loop(): handle.controlWrite(0x40, 0xdc, SAFETY_ALLOUTPUT, 0, b'') logcan = messaging.sub_sock(context, service_list['can'].port) + sendcan = messaging.pub_sock(context, service_list['sendcan'].port) while 1: tsc = messaging.drain_sock(logcan, wait_for_one=True) @@ -129,8 +130,8 @@ def boardd_mock_loop(): # recv @ 100hz can_msgs = can_recv() print "sent %d got %d" % (len(snd), len(can_msgs)) - - #print can_msgs + m = can_list_to_can_capnp(can_msgs) + sendcan.send(m.to_bytes()) def boardd_test_loop(): can_init() @@ -189,9 +190,45 @@ def boardd_loop(rate=200): rk.keep_time() +# *** main loop *** +def boardd_proxy_loop(rate=200, address="192.168.2.251"): + rk = Ratekeeper(rate) + context = zmq.Context() + + can_init() + + # *** subscribes can + logcan = messaging.sub_sock(context, service_list['can'].port, addr=address) + # *** publishes to can send + sendcan = messaging.pub_sock(context, service_list['sendcan'].port) + + while 1: + # recv @ 100hz + can_msgs = can_recv() + #for m in can_msgs: + # print "R:",hex(m[0]), str(m[2]).encode("hex") + + # publish to logger + # TODO: refactor for speed + if len(can_msgs) > 0: + dat = can_list_to_can_capnp(can_msgs, "sendcan") + sendcan.send(dat.to_bytes()) + + # send can if we have a packet + tsc = messaging.recv_sock(logcan) + if tsc is not None: + cl = can_capnp_to_can_list(tsc.can) + #for m in cl: + # print "S:",hex(m[0]), str(m[2]).encode("hex") + can_send_many(cl) + + rk.keep_time() + def main(gctx=None): if os.getenv("MOCK") is not None: boardd_mock_loop() + elif os.getenv("PROXY") is not None: + boardd_proxy_loop() elif os.getenv("BOARDTEST") is not None: boardd_test_loop() else: diff --git a/selfdrive/can/Makefile b/selfdrive/can/Makefile index 5a91e70291..8a1c6f6b7a 100644 --- a/selfdrive/can/Makefile +++ b/selfdrive/can/Makefile @@ -18,17 +18,13 @@ CFLAGS = -std=gnu11 -g -fPIC -O2 $(WARN_FLAGS) CXXFLAGS = -std=c++11 -g -fPIC -O2 $(WARN_FLAGS) ifeq ($(UNAME_S),Darwin) - CEREAL_LIBS := -L /usr/local/lib -lkj -lcapnp ZMQ_LIBS = -L/usr/local/lib -lzmq else ifeq ($(OPTEST),1) ZMQ_LIBS = -lzmq - CEREAL_LIBS = -lcapnp -lkj else ifeq ($(UNAME_M),x86_64) EXTERNAL := ../../external ZMQ_FLAGS = -I$(EXTERNAL)/zmq/include ZMQ_LIBS = -L$(EXTERNAL)/zmq/lib -l:libzmq.a - CEREAL_CXXFLAGS := -I$(EXTERNAL)/capnp/include - CEREAL_LIBS := -L$(EXTERNAL)/capnp/lib -l:libcapnp.a -l:libkj.a else ifeq ($(UNAME_M),aarch64) ZMQ_FLAGS = -I$(PHONELIBS)/zmq/aarch64/include ZMQ_LIBS = -L$(PHONELIBS)/zmq/aarch64/lib -l:libzmq.a @@ -44,9 +40,7 @@ CWD := $(shell pwd) .PHONY: all all: libdbc.so -ifeq ($(UNAME_M),aarch64) include ../common/cereal.mk -endif # make sure cereal is built libdbc.so:: ../../cereal/gen/cpp/log.capnp.h diff --git a/selfdrive/can/common.h b/selfdrive/can/common.h index 3790c1358a..6c4db60496 100644 --- a/selfdrive/can/common.h +++ b/selfdrive/can/common.h @@ -38,6 +38,7 @@ enum SignalType { DEFAULT, HONDA_CHECKSUM, HONDA_COUNTER, + TOYOTA_CHECKSUM, }; struct Signal { @@ -51,6 +52,7 @@ struct Signal { struct Msg { const char* name; uint32_t address; + unsigned int size; size_t num_sigs; const Signal *sigs; }; diff --git a/selfdrive/can/dbc_template.cc b/selfdrive/can/dbc_template.cc index 1d85b1e2f3..7d15691545 100644 --- a/selfdrive/can/dbc_template.cc +++ b/selfdrive/can/dbc_template.cc @@ -4,7 +4,7 @@ namespace { -{% for address, msg_name, sigs in msgs %} +{% for address, msg_name, msg_size, sigs in msgs %} const Signal sigs_{{address}}[] = { {% for sig in sigs %} { @@ -20,6 +20,8 @@ const Signal sigs_{{address}}[] = { .type = SignalType::HONDA_CHECKSUM, {% elif checksum_type == "honda" and sig.name == "COUNTER" %} .type = SignalType::HONDA_COUNTER, + {% elif checksum_type == "toyota" and sig.name == "CHECKSUM" %} + .type = SignalType::TOYOTA_CHECKSUM, {% else %} .type = SignalType::DEFAULT, {% endif %} @@ -29,11 +31,12 @@ const Signal sigs_{{address}}[] = { {% endfor %} const Msg msgs[] = { -{% for address, msg_name, sigs in msgs %} +{% for address, msg_name, msg_size, sigs in msgs %} {% set address_hex = "0x%X" % address %} { .name = "{{msg_name}}", .address = {{address_hex}}, + .size = {{msg_size}}, .num_sigs = ARRAYSIZE(sigs_{{address}}), .sigs = sigs_{{address}}, }, diff --git a/selfdrive/can/parser.cc b/selfdrive/can/parser.cc index 839adf5222..853b0b12a7 100644 --- a/selfdrive/can/parser.cc +++ b/selfdrive/can/parser.cc @@ -40,26 +40,33 @@ uint64_t read_u64_be(const uint8_t* v) { | (uint64_t)v[7]); } -bool honda_checksum(int address, uint64_t d, int l) { - int target = (d >> l) & 0xF; - - DEBUG("check checksum %16lx %d", d, l); - - // remove checksum from calculation - d &= ~(0xFLL << l); +unsigned int honda_checksum(unsigned int address, uint64_t d, int l) { + d >>= ((8-l)*8); // remove padding + d >>= 4; // remove checksum int s = 0; - while (address > 0) { s += (address & 0xF); address >>= 4; } - while (d > 0) { s += (d & 0xF); d >>= 4; } + while (address) { s += (address & 0xF); address >>= 4; } + while (d) { s += (d & 0xF); d >>= 4; } s = 8-s; s &= 0xF; - DEBUG(" %d = %d\n", target, s); - return target == s; + return s; +} + +unsigned int toyota_checksum(unsigned int address, uint64_t d, int l) { + d >>= ((8-l)*8); // remove padding + d >>= 8; // remove checksum + + unsigned int s = l; + while (address) { s += address & 0xff; address >>= 8; } + while (d) { s += d & 0xff; d >>= 8; } + + return s & 0xFF; } struct MessageState { uint32_t address; + unsigned int size; std::vector parse_sigs; std::vector vals; @@ -80,10 +87,10 @@ struct MessageState { tmp -= (tmp >> (sig.b2-1)) ? (1ULL << sig.b2) : 0; //signed } - DEBUG("parse %X %s -> %ld\n", address, sig.name, tmp); + DEBUG("parse %X %s -> %lld\n", address, sig.name, tmp); if (sig.type == SignalType::HONDA_CHECKSUM) { - if (!honda_checksum(address, dat, sig.bo)) { + if (honda_checksum(address, dat, size) != tmp) { INFO("%X CHECKSUM FAIL\n", address); return false; } @@ -91,6 +98,13 @@ struct MessageState { if (!honda_update_counter(tmp)) { return false; } + } else if (sig.type == SignalType::TOYOTA_CHECKSUM) { + // DEBUG("CHECKSUM %d %d %018llX - %lld vs %d\n", address, size, dat, tmp, toyota_checksum(address, dat, size)); + + if (toyota_checksum(address, dat, size) != tmp) { + INFO("%X CHECKSUM FAIL\n", address); + return false; + } } vals[i] = tmp * sig.factor + sig.offset; @@ -161,11 +175,12 @@ class CANParser { assert(false); } - // track checksums and for this message + state.size = msg->size; + + // track checksums and counters for this message for (int i=0; inum_sigs; i++) { const Signal *sig = &msg->sigs[i]; - if (sig->type == HONDA_CHECKSUM - || sig->type == HONDA_COUNTER) { + if (sig->type != SignalType::DEFAULT) { state.parse_sigs.push_back(*sig); state.vals.push_back(0); } @@ -178,8 +193,7 @@ class CANParser { for (int i=0; inum_sigs; i++) { const Signal *sig = &msg->sigs[i]; if (strcmp(sig->name, sigop.name) == 0 - && sig->type != HONDA_CHECKSUM - && sig->type != HONDA_COUNTER) { + && sig->type == SignalType::DEFAULT) { state.parse_sigs.push_back(*sig); state.vals.push_back(sigop.default_value); break; @@ -216,7 +230,7 @@ class CANParser { uint64_t p = read_u64_be(dat); - DEBUG(" proc %X: %lx\n", cmsg.getAddress(), p); + DEBUG(" proc %X: %llx\n", cmsg.getAddress(), p); state_it->second.parse(sec, cmsg.getBusTime(), p); } diff --git a/selfdrive/can/parser.py b/selfdrive/can/parser.py index 708d873350..b58abe1be6 100644 --- a/selfdrive/can/parser.py +++ b/selfdrive/can/parser.py @@ -72,58 +72,101 @@ if __name__ == "__main__": # cp = CANParser("acura_ilx_2016_nidec", signals, checks, 1) + + # signals = [ + # ("XMISSION_SPEED", 0x158, 0), #sig_name, sig_address, default + # ("WHEEL_SPEED_FL", 0x1d0, 0), + # ("WHEEL_SPEED_FR", 0x1d0, 0), + # ("WHEEL_SPEED_RL", 0x1d0, 0), + # ("STEER_ANGLE", 0x14a, 0), + # ("STEER_TORQUE_SENSOR", 0x18f, 0), + # ("GEAR", 0x191, 0), + # ("WHEELS_MOVING", 0x1b0, 1), + # ("DOOR_OPEN_FL", 0x405, 1), + # ("DOOR_OPEN_FR", 0x405, 1), + # ("DOOR_OPEN_RL", 0x405, 1), + # ("DOOR_OPEN_RR", 0x405, 1), + # ("CRUISE_SPEED_PCM", 0x324, 0), + # ("SEATBELT_DRIVER_LAMP", 0x305, 1), + # ("SEATBELT_DRIVER_LATCHED", 0x305, 0), + # ("BRAKE_PRESSED", 0x17c, 0), + # ("CAR_GAS", 0x130, 0), + # ("CRUISE_BUTTONS", 0x296, 0), + # ("ESP_DISABLED", 0x1a4, 1), + # ("HUD_LEAD", 0x30c, 0), + # ("USER_BRAKE", 0x1a4, 0), + # ("STEER_STATUS", 0x18f, 5), + # ("WHEEL_SPEED_RR", 0x1d0, 0), + # ("BRAKE_ERROR_1", 0x1b0, 1), + # ("BRAKE_ERROR_2", 0x1b0, 1), + # ("GEAR_SHIFTER", 0x191, 0), + # ("MAIN_ON", 0x326, 0), + # ("ACC_STATUS", 0x17c, 0), + # ("PEDAL_GAS", 0x17c, 0), + # ("CRUISE_SETTING", 0x296, 0), + # ("LEFT_BLINKER", 0x326, 0), + # ("RIGHT_BLINKER", 0x326, 0), + # ("COUNTER", 0x324, 0), + # ("ENGINE_RPM", 0x17C, 0) + # ] + # checks = [ + # (0x14a, 100), # address, frequency + # (0x158, 100), + # (0x17c, 100), + # (0x191, 100), + # (0x1a4, 50), + # (0x326, 10), + # (0x1b0, 50), + # (0x1d0, 50), + # (0x305, 10), + # (0x324, 10), + # (0x405, 3), + # ] + + # cp = CANParser("honda_civic_touring_2016_can", signals, checks, 0) + + signals = [ - ("XMISSION_SPEED", 0x158, 0), #sig_name, sig_address, default - ("WHEEL_SPEED_FL", 0x1d0, 0), - ("WHEEL_SPEED_FR", 0x1d0, 0), - ("WHEEL_SPEED_RL", 0x1d0, 0), - ("STEER_ANGLE", 0x14a, 0), - ("STEER_TORQUE_SENSOR", 0x18f, 0), - ("GEAR", 0x191, 0), - ("WHEELS_MOVING", 0x1b0, 1), - ("DOOR_OPEN_FL", 0x405, 1), - ("DOOR_OPEN_FR", 0x405, 1), - ("DOOR_OPEN_RL", 0x405, 1), - ("DOOR_OPEN_RR", 0x405, 1), - ("CRUISE_SPEED_PCM", 0x324, 0), - ("SEATBELT_DRIVER_LAMP", 0x305, 1), - ("SEATBELT_DRIVER_LATCHED", 0x305, 0), - ("BRAKE_PRESSED", 0x17c, 0), - ("CAR_GAS", 0x130, 0), - ("CRUISE_BUTTONS", 0x296, 0), - ("ESP_DISABLED", 0x1a4, 1), - ("HUD_LEAD", 0x30c, 0), - ("USER_BRAKE", 0x1a4, 0), - ("STEER_STATUS", 0x18f, 5), - ("WHEEL_SPEED_RR", 0x1d0, 0), - ("BRAKE_ERROR_1", 0x1b0, 1), - ("BRAKE_ERROR_2", 0x1b0, 1), - ("GEAR_SHIFTER", 0x191, 0), - ("MAIN_ON", 0x326, 0), - ("ACC_STATUS", 0x17c, 0), - ("PEDAL_GAS", 0x17c, 0), - ("CRUISE_SETTING", 0x296, 0), - ("LEFT_BLINKER", 0x326, 0), - ("RIGHT_BLINKER", 0x326, 0), - ("COUNTER", 0x324, 0), - ("ENGINE_RPM", 0x17C, 0) + # sig_name, sig_address, default + ("GEAR", 956, 0x20), + ("BRAKE_PRESSED", 548, 0), + ("GAS_PEDAL", 705, 0), + + ("WHEEL_SPEED_FL", 170, 0), + ("WHEEL_SPEED_FR", 170, 0), + ("WHEEL_SPEED_RL", 170, 0), + ("WHEEL_SPEED_RR", 170, 0), + ("DOOR_OPEN_FL", 1568, 1), + ("DOOR_OPEN_FR", 1568, 1), + ("DOOR_OPEN_RL", 1568, 1), + ("DOOR_OPEN_RR", 1568, 1), + ("SEATBELT_DRIVER_UNLATCHED", 1568, 1), + ("TC_DISABLED", 951, 1), + ("STEER_ANGLE", 37, 0), + ("STEER_FRACTION", 37, 0), + ("STEER_RATE", 37, 0), + ("GAS_RELEASED", 466, 0), + ("CRUISE_STATE", 466, 0), + ("MAIN_ON", 467, 0), + ("SET_SPEED", 467, 0), + ("STEER_TORQUE_DRIVER", 608, 0), + ("STEER_TORQUE_EPS", 608, 0), + ("TURN_SIGNALS", 1556, 3), # 3 is no blinkers + ("LKA_STATE", 610, 0), ] checks = [ - (0x14a, 100), # address, frequency - (0x158, 100), - (0x17c, 100), - (0x191, 100), - (0x1a4, 50), - (0x326, 10), - (0x1b0, 50), - (0x1d0, 50), - (0x305, 10), - (0x324, 10), - (0x405, 3), + (548, 40), + (705, 33), + + (170, 80), + (37, 80), + (466, 33), + (608, 50), ] - cp = CANParser("honda_civic_touring_2016_can", signals, checks, 0) - print cp.vl + cp = CANParser("toyota_rav4_2017_pt", signals, checks, 0) + + # print cp.vl while True: cp.update(int(sec_since_boot()*1e9), True) diff --git a/selfdrive/can/process_dbc.py b/selfdrive/can/process_dbc.py index 81646f78f9..c48afc8566 100755 --- a/selfdrive/can/process_dbc.py +++ b/selfdrive/can/process_dbc.py @@ -21,10 +21,15 @@ can_dbc = dbc(dbc_fn) with open(template_fn, "r") as template_f: template = jinja2.Template(template_f.read(), trim_blocks=True, lstrip_blocks=True) -msgs = [(address, msg_name, sorted(msg_sigs, key=lambda s: s.name not in ("COUNTER", "CHECKSUM"))) # process counter and checksums first - for address, ((msg_name, _), msg_sigs) in sorted(can_dbc.msgs.iteritems()) if msg_sigs] - -checksum_type = "honda" if can_dbc.name.startswith("honda") or can_dbc.name.startswith("acura") else None +msgs = [(address, msg_name, msg_size, sorted(msg_sigs, key=lambda s: s.name not in ("COUNTER", "CHECKSUM"))) # process counter and checksums first + for address, ((msg_name, msg_size), msg_sigs) in sorted(can_dbc.msgs.iteritems()) if msg_sigs] + +if can_dbc.name.startswith("honda") or can_dbc.name.startswith("acura"): + checksum_type = "honda" +elif can_dbc.name.startswith("toyota"): + checksum_type = "toyota" +else: + checksum_type = None parser_code = template.render(dbc=can_dbc, checksum_type=checksum_type, msgs=msgs, len=len) diff --git a/selfdrive/car/__init__.py b/selfdrive/car/__init__.py index 9443dab40a..f27e41ed16 100644 --- a/selfdrive/car/__init__.py +++ b/selfdrive/car/__init__.py @@ -1,5 +1,11 @@ -from common.fingerprints import fingerprint +import os +from cereal import car +from common.realtime import sec_since_boot +from common.fingerprints import eliminate_incompatible_cars, all_known_cars + +from selfdrive.swaglog import cloudlog +import selfdrive.messaging as messaging from .honda.interface import CarInterface as HondaInterface try: @@ -17,19 +23,65 @@ try: except ImportError: Sim2Interface = None + interfaces = { "HONDA CIVIC 2016 TOURING": HondaInterface, "ACURA ILX 2016 ACURAWATCH PLUS": HondaInterface, "HONDA ACCORD 2016 TOURING": HondaInterface, "HONDA CR-V 2016 TOURING": HondaInterface, "TOYOTA PRIUS 2017": ToyotaInterface, + "TOYOTA RAV4 2017": ToyotaInterface, "simulator": SimInterface, "simulator2": Sim2Interface } -def get_car(logcan, sendcan=None): - candidate, fingerprints = fingerprint(logcan) +# **** for use live only **** +def fingerprint(logcan, timeout): + if os.getenv("SIMULATOR") is not None or logcan is None: + return ("simulator", None) + elif os.getenv("SIMULATOR2") is not None: + return ("simulator2", None) + + finger_st = sec_since_boot() + + cloudlog.warning("waiting for fingerprint...") + candidate_cars = all_known_cars() + finger = {} + st = None + while 1: + for a in messaging.drain_sock(logcan, wait_for_one=True): + if st is None: + st = sec_since_boot() + for can in a.can: + if can.src == 0: + finger[can.address] = len(can.dat) + candidate_cars = eliminate_incompatible_cars(can, candidate_cars) + + ts = sec_since_boot() + # if we only have one car choice and the time_fingerprint since we got our first + # message has elapsed, exit. Toyota needs higher time_fingerprint, since DSU does not + # broadcast immediately + if len(candidate_cars) == 1 and st is not None: + time_fingerprint = 1.0 if "TOYOTA" in candidate_cars[0] else 0.1 + if (ts-st) > time_fingerprint: + break + + # bail if no cars left or we've been waiting too long + elif len(candidate_cars) == 0 or (timeout and ts-finger_st > timeout): + return None, finger + + cloudlog.warning("fingerprinted %s", candidate_cars[0]) + return (candidate_cars[0], finger) + + +def get_car(logcan, sendcan=None, timeout=None): + candidate, fingerprints = fingerprint(logcan, timeout) + + if candidate is None: + cloudlog.warning("car doesn't match any fingerprints: %r", fingerprints) + return None, None + interface_cls = interfaces[candidate] params = interface_cls.get_params(candidate, fingerprints) diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index caa15dfa07..b22ad701ab 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -7,7 +7,13 @@ from common.realtime import sec_since_boot from selfdrive.config import CruiseButtons from selfdrive.boardd.boardd import can_list_to_can_capnp from selfdrive.controls.lib.drive_helpers import rate_limit + from . import hondacan +from .values import AH + +# msgs sent for steering controller by camera module on can 0. +# those messages are mutually exclusive on non-rav4 and rav4 cars +CAMERA_MSGS = [0xe4, 0x194] def actuator_hystereses(brake, braking, brake_steady, v_ego, civic): @@ -41,17 +47,6 @@ def actuator_hystereses(brake, braking, brake_steady, v_ego, civic): return brake, braking, brake_steady -class AH: - #[alert_idx, value] - # See dbc files for info on values" - NONE = [0, 0] - FCW = [1, 0x8] - STEER = [2, 1] - BRAKE_PRESSED = [3, 10] - GEAR_NOT_D = [4, 6] - SEATBELT = [5, 5] - SPEED_TOO_HIGH = [6, 8] - def process_hud_alert(hud_alert): # initialize to no alert @@ -75,10 +70,11 @@ HUDData = namedtuple("HUDData", "lanes", "beep", "X8", "chime", "acc_alert"]) class CarController(object): - def __init__(self): + def __init__(self, enable_camera=True): self.braking = False self.brake_steady = 0. self.brake_last = 0. + self.enable_camera = enable_camera def update(self, sendcan, enabled, CS, frame, actuators, \ pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \ @@ -87,7 +83,7 @@ class CarController(object): """ Controls thread """ # TODO: Make the accord work. - if CS.accord: + if CS.accord or not self.enable_camera: return # *** apply brake hysteresis *** diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index 4eb8c6c386..8903afc89b 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -1,12 +1,12 @@ import os import time from cereal import car -import common.numpy_fast as np +from common.numpy_fast import interp 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 - +import numpy as np def parse_gear_shifter(can_gear_shifter, is_acura): @@ -59,6 +59,7 @@ def get_can_signals(CP): ("WHEEL_SPEED_RL", 0x1d0, 0), ("WHEEL_SPEED_RR", 0x1d0, 0), ("STEER_ANGLE", 0x14a, 0), + ("STEER_ANGLE_RATE", 0x14a, 0), ("STEER_TORQUE_SENSOR", 0x18f, 0), ("GEAR", 0x191, 0), ("WHEELS_MOVING", 0x1b0, 1), @@ -115,6 +116,7 @@ def get_can_signals(CP): ("WHEEL_SPEED_RL", 0x1d0, 0), ("WHEEL_SPEED_RR", 0x1d0, 0), ("STEER_ANGLE", 0x156, 0), + ("STEER_ANGLE_RATE", 0x156, 0), ("STEER_TORQUE_SENSOR", 0x18f, 0), ("GEAR", 0x1a3, 0), ("WHEELS_MOVING", 0x1b0, 1), @@ -167,6 +169,7 @@ def get_can_signals(CP): ("WHEEL_SPEED_RL", 0x1d0, 0), ("WHEEL_SPEED_RR", 0x1d0, 0), ("STEER_ANGLE", 0x156, 0), + ("STEER_ANGLE_RATE", 0x156, 0), #("STEER_TORQUE_SENSOR", 0x18f, 0), ("GEAR", 0x191, 0), ("WHEELS_MOVING", 0x1b0, 1), @@ -218,6 +221,7 @@ def get_can_signals(CP): ("WHEEL_SPEED_RL", 0x1d0, 0), ("WHEEL_SPEED_RR", 0x1d0, 0), ("STEER_ANGLE", 0x156, 0), + ("STEER_ANGLE_RATE", 0x156, 0), ("STEER_TORQUE_SENSOR", 0x18f, 0), ("GEAR", 0x191, 0), ("WHEELS_MOVING", 0x1b0, 1), @@ -304,8 +308,17 @@ class CarState(object): self.left_blinker_on = 0 self.right_blinker_on = 0 - # TODO: actually make this work - self.a_ego = 0. + # vEgo kalman filter + dt = 0.01 + self.v_ego_x = np.matrix([[0.0], [0.0]]) + self.v_ego_A = np.matrix([[1.0, dt], [0.0, 1.0]]) + self.v_ego_C = np.matrix([1.0, 0.0]) + self.v_ego_Q = np.matrix([[10.0, 0.0], [0.0, 100.0]]) + self.v_ego_R = 1e3 + # import control + # (x, l, K) = control.dare(np.transpose(A), np.transpose(C), Q, R) + # self.v_ego_K = np.transpose(K) + self.v_ego_K = np.matrix([[0.12287673], [0.29666309]]) def update(self, can_pub_main=None): cp = self.cp @@ -351,9 +364,16 @@ class CarState(object): 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 + self.v_weight = interp(self.v_wheel, v_weight_bp, v_weight_v) + speed = (1. - self.v_weight) * cp.vl[0x158]['XMISSION_SPEED'] + self.v_weight * self.v_wheel + self.v_ego_x = np.dot((self.v_ego_A - np.dot(self.v_ego_K, self.v_ego_C)), self.v_ego_x) + np.dot(self.v_ego_K, speed) + + self.v_ego_raw = speed + self.v_ego = float(self.v_ego_x[0]) + self.a_ego = float(self.v_ego_x[1]) + if self.CP.enableGas: # this is a hack self.user_gas = cp.vl[0x201]['INTERCEPTOR_GAS'] @@ -362,6 +382,7 @@ class CarState(object): if self.civic: can_gear_shifter = cp.vl[0x191]['GEAR_SHIFTER'] self.angle_steers = cp.vl[0x14A]['STEER_ANGLE'] + self.angle_steers_rate = cp.vl[0x14A]['STEER_ANGLE_RATE'] 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'] @@ -375,6 +396,7 @@ class CarState(object): elif self.accord: can_gear_shifter = cp.vl[0x191]['GEAR_SHIFTER'] self.angle_steers = cp.vl[0x156]['STEER_ANGLE'] + self.angle_steers_rate = cp.vl[0x156]['STEER_ANGLE_RATE'] 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'] @@ -388,6 +410,7 @@ class CarState(object): elif self.crv: can_gear_shifter = cp.vl[0x191]['GEAR_SHIFTER'] self.angle_steers = cp.vl[0x156]['STEER_ANGLE'] + self.angle_steers_rate = cp.vl[0x156]['STEER_ANGLE_RATE'] self.gear = cp.vl[0x191]['GEAR'] self.cruise_setting = cp.vl[0x1A6]['CRUISE_SETTING'] self.cruise_buttons = cp.vl[0x1A6]['CRUISE_BUTTONS'] @@ -401,6 +424,7 @@ class CarState(object): elif self.acura: can_gear_shifter = cp.vl[0x1A3]['GEAR_SHIFTER'] self.angle_steers = cp.vl[0x156]['STEER_ANGLE'] + self.angle_steers_rate = cp.vl[0x156]['STEER_ANGLE_RATE'] self.gear = cp.vl[0x1A3]['GEAR'] self.cruise_setting = cp.vl[0x1A6]['CRUISE_SETTING'] self.cruise_buttons = cp.vl[0x1A6]['CRUISE_BUTTONS'] @@ -448,7 +472,7 @@ if __name__ == '__main__': import time from selfdrive.services import service_list context = zmq.Context() - logcan = messaging.sub_sock(context, service_list['can'].port) + logcan = messaging.sub_sock(context, service_list['can'].port) class CarParams(object): def __init__(self): @@ -461,4 +485,3 @@ if __name__ == '__main__': while 1: CS.update() time.sleep(0.01) - diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 730f5ea032..397c905988 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -4,17 +4,19 @@ import time 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 +from selfdrive.car.honda.carstate import CarState +from selfdrive.car.honda.carcontroller import CAMERA_MSGS +from selfdrive.car.honda.values import CruiseButtons, CM, BP, AH +try: + from .carcontroller import CarController +except ImportError: + CarController = None def get_compute_gb(): # generate a function that takes in [desired_accel, current_speed] -> [-1.0, 1.0] @@ -61,31 +63,6 @@ def get_compute_gb(): return _compute_gb -# Car button codes -class CruiseButtons: - RES_ACCEL = 4 - DECEL_SET = 3 - CANCEL = 2 - MAIN = 1 - - -#car chimes: enumeration from dbc file. Chimes are for alerts and warnings -class CM: - MUTE = 0 - SINGLE = 3 - DOUBLE = 4 - REPEATED = 1 - CONTINUOUS = 2 - - -#car beepss: enumeration from dbc file. Beeps are for activ and deactiv -class BP: - MUTE = 0 - SINGLE = 3 - TRIPLE = 2 - REPEATED = 1 - - class CarInterface(object): def __init__(self, CP, logcan, sendcan=None): self.logcan = logcan @@ -104,7 +81,7 @@ class CarInterface(object): # sending if read only is False if sendcan is not None: self.sendcan = sendcan - self.CC = CarController() + self.CC = CarController(CP.enableCamera) if self.CS.accord: # self.accord_msg = [] @@ -127,11 +104,14 @@ class CarInterface(object): ret.enableSteer = True ret.enableBrake = True - # pedal + ret.enableCamera = not any(x for x in CAMERA_MSGS if x in fingerprint) ret.enableGas = 0x201 in fingerprint + print "ECU Camera Simulated: ", ret.enableCamera + print "ECU Gas Interceptor: ", ret.enableGas + ret.enableCruise = not ret.enableGas - # FIXME: hardcoding honda civic 2016 touring params so they 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 @@ -176,13 +156,15 @@ class CarInterface(object): 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 + ret.steerKf = 0. # TODO: investigate FF steer control for Honda + + # 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 + # 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) @@ -197,11 +179,14 @@ class CarInterface(object): # 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 + ret.steerLimitAlert = True + return ret compute_gb = staticmethod(get_compute_gb()) @@ -219,6 +204,9 @@ class CarInterface(object): # speeds ret.vEgo = self.CS.v_ego + ret.aEgo = self.CS.a_ego + ret.vEgoRaw = self.CS.v_ego_raw + ret.standstill = self.CS.standstill ret.wheelSpeeds.fl = self.CS.v_wheel_fl ret.wheelSpeeds.fr = self.CS.v_wheel_fr ret.wheelSpeeds.rl = self.CS.v_wheel_rl @@ -236,8 +224,8 @@ class CarInterface(object): ret.brakePressed = self.CS.brake_pressed != 0 # steering wheel - # TODO: units ret.steeringAngle = self.CS.angle_steers + ret.steeringRate = self.CS.angle_steers_rate # gear shifter lever ret.gearShifter = self.CS.gear_shifter @@ -249,7 +237,7 @@ class CarInterface(object): 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 + ret.cruiseState.speedOffset = self.CS.cruise_speed_offset # TODO: button presses buttonEvents = [] @@ -368,7 +356,7 @@ class CarInterface(object): 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 + # 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 diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py new file mode 100644 index 0000000000..ee5c08884b --- /dev/null +++ b/selfdrive/car/honda/values.py @@ -0,0 +1,34 @@ +# Car button codes +class CruiseButtons: + RES_ACCEL = 4 + DECEL_SET = 3 + CANCEL = 2 + MAIN = 1 + + +#car chimes: enumeration from dbc file. Chimes are for alerts and warnings +class CM: + MUTE = 0 + SINGLE = 3 + DOUBLE = 4 + REPEATED = 1 + CONTINUOUS = 2 + + +#car beepss: enumeration from dbc file. Beeps are for activ and deactiv +class BP: + MUTE = 0 + SINGLE = 3 + TRIPLE = 2 + REPEATED = 1 + +class AH: + #[alert_idx, value] + # See dbc files for info on values" + NONE = [0, 0] + FCW = [1, 0x8] + STEER = [2, 1] + BRAKE_PRESSED = [3, 10] + GEAR_NOT_D = [4, 6] + SEATBELT = [5, 5] + SPEED_TOO_HIGH = [6, 8] diff --git a/selfdrive/car/toyota/__init__.py b/selfdrive/car/toyota/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py new file mode 100644 index 0000000000..115a2599d7 --- /dev/null +++ b/selfdrive/car/toyota/carcontroller.py @@ -0,0 +1,236 @@ +from common.numpy_fast import clip, interp +from common.realtime import sec_since_boot +from selfdrive.boardd.boardd import can_list_to_can_capnp +from selfdrive.controls.lib.drive_helpers import rate_limit +from selfdrive.car.toyota.toyotacan import make_can_msg, create_video_target,\ + create_steer_command, create_ui_command, \ + create_ipas_steer_command, create_accel_command + + +ACCEL_HYST_GAP = 0.02 # don't change accel command for small oscilalitons within this value +ACCEL_MAX = 1500 # 1.5 m/s2 +ACCEL_MIN = -3000 # 3 m/s2 +ACCEL_SCALE = max(ACCEL_MAX, -ACCEL_MIN) + +STEER_MAX = 1500 +STEER_DELTA_UP = 10 # 1.5s time to peak torque +STEER_DELTA_DOWN = 45 # lower than 50 otherwise the Rav4 faults (Prius seems ok though) +STEER_ERROR_MAX = 500 # max delta between torque cmd and torque motor + +STEER_IPAS_MAX = 340 +STEER_IPAS_DELTA_MAX = 3 + +class CAR: + PRIUS = "TOYOTA PRIUS 2017" + RAV4 = "TOYOTA RAV4 2017" + +class ECU: + CAM = 0 # camera + DSU = 1 # driving support unit + APGS = 2 # advanced parking guidance system + + +TARGET_IDS = [0x340, 0x341, 0x342, 0x343, 0x344, 0x345, + 0x363, 0x364, 0x365, 0x370, 0x371, 0x372, + 0x373, 0x374, 0x375, 0x380, 0x381, 0x382, + 0x383] + +# addr, [ecu, bus, 1/freq*100, vl] +STATIC_MSGS = {0x141: (ECU.DSU, (CAR.PRIUS, CAR.RAV4), 1, 2, '\x00\x00\x00\x46'), + 0x128: (ECU.DSU, (CAR.PRIUS, CAR.RAV4), 1, 3, '\xf4\x01\x90\x83\x00\x37'), + + 0x292: (ECU.APGS, (CAR.PRIUS, CAR.RAV4), 0, 3, '\x00\x00\x00\x00\x00\x00\x00\x9e'), + 0x283: (ECU.DSU, (CAR.PRIUS, CAR.RAV4), 0, 3, '\x00\x00\x00\x00\x00\x00\x8c'), + 0x2E6: (ECU.DSU, (CAR.PRIUS,), 0, 3, '\xff\xf8\x00\x08\x7f\xe0\x00\x4e'), + 0x2E7: (ECU.DSU, (CAR.PRIUS,), 0, 3, '\xa8\x9c\x31\x9c\x00\x00\x00\x02'), + + 0x240: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'), + 0x241: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'), + 0x244: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'), + 0x245: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'), + 0x248: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 1, 5, '\x00\x00\x00\x00\x00\x00\x01'), + 0x344: (ECU.DSU, (CAR.PRIUS, CAR.RAV4), 0, 5, '\x00\x00\x01\x00\x00\x00\x00\x50'), + + 0x160: (ECU.DSU, (CAR.PRIUS, CAR.RAV4), 1, 7, '\x00\x00\x08\x12\x01\x31\x9c\x51'), + 0x161: (ECU.DSU, (CAR.PRIUS, CAR.RAV4), 1, 7, '\x00\x1e\x00\x00\x00\x80\x07'), + + 0x32E: (ECU.APGS, (CAR.PRIUS, CAR.RAV4), 0, 20, '\x00\x00\x00\x00\x00\x00\x00\x00'), + 0x33E: (ECU.DSU, (CAR.PRIUS,), 0, 20, '\x0f\xff\x26\x40\x00\x1f\x00'), + 0x365: (ECU.DSU, (CAR.PRIUS,), 0, 20, '\x00\x00\x00\x80\x03\x00\x08'), + 0x365: (ECU.DSU, (CAR.RAV4,), 0, 20, '\x00\x00\x00\x80\xfc\x00\x08'), + 0x366: (ECU.DSU, (CAR.PRIUS,), 0, 20, '\x00\x00\x4d\x82\x40\x02\x00'), + 0x366: (ECU.DSU, (CAR.RAV4,), 0, 20, '\x00\x72\x07\xff\x09\xfe\x00'), + + 0x367: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 0, 40, '\x06\x00'), + + 0x414: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x00\x00\x00\x00\x00\x00\x17\x00'), + 0x489: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x00\x00\x00\x00\x00\x00\x00'), + 0x48a: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x00\x00\x00\x00\x00\x00\x00'), + 0x48b: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x66\x06\x08\x0a\x02\x00\x00\x00'), + 0x4d3: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x1C\x00\x00\x01\x00\x00\x00\x00'), + 0x130: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 1, 100, '\x00\x00\x00\x00\x00\x00\x38'), + 0x466: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 1, 100, '\x20\x20\xAD'), + 0x396: (ECU.APGS, (CAR.PRIUS, CAR.RAV4), 0, 100, '\xBD\x00\x00\x00\x60\x0F\x02\x00'), + 0x43A: (ECU.APGS, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x84\x00\x00\x00\x00\x00\x00\x00'), + 0x43B: (ECU.APGS, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x00\x00\x00\x00\x00\x00\x00\x00'), + 0x497: (ECU.APGS, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x00\x00\x00\x00\x00\x00\x00\x00'), + 0x4CC: (ECU.APGS, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x0D\x00\x00\x00\x00\x00\x00\x00'), + 0x411: (ECU.DSU, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x00\x20\x00\x00\x10\x00\x80\x00'), + 0x4CB: (ECU.DSU, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x0c\x00\x00\x00\x00\x00\x00\x00'), + 0x470: (ECU.DSU, (CAR.PRIUS,), 1, 100, '\x00\x00\x02\x7a'), + } + + +def check_ecu_msgs(fingerprint, candidate, ecu): + # return True if fingerprint contains messages normally sent by a given ecu + ecu_msgs = [x for x in STATIC_MSGS if (ecu == STATIC_MSGS[x][0] and + candidate in STATIC_MSGS[x][1] and + STATIC_MSGS[x][2] == 0)] + + return any(msg for msg in fingerprint if msg in ecu_msgs) + + +def accel_hysteresis(accel, accel_steady, enabled): + + # for small accel oscillations within ACCEL_HYST_GAP, don't change the accel command + if not enabled: + # send 0 when disabled, otherwise acc faults + accel_steady = 0. + elif accel > accel_steady + ACCEL_HYST_GAP: + accel_steady = accel - ACCEL_HYST_GAP + elif accel < accel_steady - ACCEL_HYST_GAP: + accel_steady = accel + ACCEL_HYST_GAP + accel = accel_steady + + return accel, accel_steady + + +def process_hud_alert(hud_alert, audible_alert): + # initialize to no alert + hud1 = 0 + hud2 = 0 + if hud_alert in ['steerRequired', 'fcw']: + if audible_alert == 'chimeRepeated': + hud1 = 3 + else: + hud1 = 1 + if audible_alert in ['beepSingle', 'chimeSingle', 'chimeDouble']: + # TODO: find a way to send single chimes + hud2 = 1 + + return hud1, hud2 + + +class CarController(object): + def __init__(self, car_fingerprint, enable_camera, enable_dsu, enable_apg): + self.braking = False + # redundant safety check with the board + self.controls_allowed = True + self.last_steer = 0. + self.accel_steady = 0. + self.car_fingerprint = car_fingerprint + self.alert_active = False + + self.fake_ecus = set() + if enable_camera: self.fake_ecus.add(ECU.CAM) + if enable_dsu: self.fake_ecus.add(ECU.DSU) + if enable_apg: self.fake_ecus.add(ECU.APGS) + + def update(self, sendcan, enabled, CS, frame, actuators, + pcm_cancel_cmd, hud_alert, audible_alert): + + # *** compute control surfaces *** + tt = sec_since_boot() + + # steer torque is converted back to CAN reference (positive when steering right) + apply_accel = actuators.gas - actuators.brake + apply_accel, self.accel_steady = accel_hysteresis(apply_accel, self.accel_steady, enabled) + apply_accel = int(round(clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX))) + + # steer torque is converted back to CAN reference (positive when steering right) + apply_steer = int(round(actuators.steer * STEER_MAX)) + + max_lim = min(max(CS.steer_torque_motor + STEER_ERROR_MAX, STEER_ERROR_MAX), STEER_MAX) + min_lim = max(min(CS.steer_torque_motor - STEER_ERROR_MAX, -STEER_ERROR_MAX), -STEER_MAX) + + apply_steer = clip(apply_steer, min_lim, max_lim) + + # slow rate if steer torque increases in magnitude + if self.last_steer > 0: + apply_steer = clip(apply_steer, max(self.last_steer - STEER_DELTA_DOWN, - STEER_DELTA_UP), self.last_steer + STEER_DELTA_UP) + else: + apply_steer = clip(apply_steer, self.last_steer - STEER_DELTA_UP, min(self.last_steer + STEER_DELTA_DOWN, STEER_DELTA_UP)) + + 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 + pcm_cancel_cmd = 1 + + # dropping torque immediately might cause eps to temp fault. On the other hand, safety_toyota + # cuts steer torque immediately anyway TODO: monitor if this is a real issue + if not enabled or CS.steer_error: + apply_steer = 0 + + self.last_steer = apply_steer + self.last_accel = apply_accel + + can_sends = [] + + #*** control msgs *** + #print "steer", apply_steer, min_lim, max_lim, CS.steer_torque_motor + + # toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2; + # sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed + # on consecutive messages + if ECU.CAM in self.fake_ecus: + can_sends.append(create_steer_command(apply_steer, frame)) + + if ECU.APGS in self.fake_ecus: + can_sends.append(create_ipas_steer_command(apply_steer)) + + # accel cmd comes from DSU, but we can spam can to cancel the system even if we are using lat only control + if (frame % 3 == 0 and ECU.DSU in self.fake_ecus) or (pcm_cancel_cmd and ECU.CAM in self.fake_ecus): + if ECU.DSU in self.fake_ecus: + can_sends.append(create_accel_command(apply_accel, pcm_cancel_cmd)) + else: + can_sends.append(create_accel_command(0, pcm_cancel_cmd)) + + if frame % 10 == 0 and ECU.CAM in self.fake_ecus: + for addr in TARGET_IDS: + can_sends.append(create_video_target(frame/10, addr)) + + # ui mesg is at 100Hz but we send asap if: + # - there is something to display + # - there is something to stop displaying + hud1, hud2 = process_hud_alert(hud_alert, audible_alert) + if ((hud1 or hud2) and not self.alert_active) or \ + (not (hud1 or hud2) and self.alert_active): + send_ui = True + self.alert_active = not self.alert_active + else: + send_ui = False + + if (frame % 100 == 0 or send_ui) and ECU.CAM in self.fake_ecus: + can_sends.append(create_ui_command(hud1, hud2)) + + #*** static msgs *** + + for addr, (ecu, cars, bus, fr_step, vl) in STATIC_MSGS.iteritems(): + if frame % fr_step == 0 and ecu in self.fake_ecus and self.car_fingerprint in cars: + # send msg! + # special cases + + if fr_step == 5 and ecu == ECU.CAM and bus == 1: + cnt = (((frame / 5) % 7) + 1) << 5 + vl = chr(cnt) + vl + elif addr in (0x489, 0x48a) and bus == 0: + # add counter for those 2 messages (last 4 bits) + cnt = ((frame/100)%0xf) + 1 + if addr == 0x48a: + # 0x48a has a 8 preceding the counter + cnt += 1 << 7 + vl += chr(cnt) + + can_sends.append(make_can_msg(addr, vl, bus, False)) + + + sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes()) diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py new file mode 100644 index 0000000000..dcfc3fa633 --- /dev/null +++ b/selfdrive/car/toyota/carstate.py @@ -0,0 +1,187 @@ +import os +import time +from common.realtime import sec_since_boot +import selfdrive.messaging as messaging +from selfdrive.car.toyota.carcontroller import CAR +from selfdrive.can.parser import CANParser +from selfdrive.config import Conversions as CV +import numpy as np + +def parse_gear_shifter(can_gear, car_fingerprint): + + if car_fingerprint == CAR.PRIUS: + if can_gear == 0x0: + return "park" + elif can_gear == 0x1: + return "reverse" + elif can_gear == 0x2: + return "neutral" + elif can_gear == 0x3: + return "drive" + elif can_gear == 0x4: + return "brake" + elif car_fingerprint == CAR.RAV4: + if can_gear == 0x20: + return "park" + elif can_gear == 0x10: + return "reverse" + elif can_gear == 0x8: + return "neutral" + elif can_gear == 0x0: + return "drive" + elif can_gear == 0x1: + return "sport" + + return "unknown" + + +def get_can_parser(CP): + # this function generates lists for signal, messages and initial values + if CP.carFingerprint == CAR.PRIUS: + dbc_f = 'toyota_prius_2017_pt.dbc' + signals = [ + ("GEAR", 295, 0), + ("BRAKE_PRESSED", 550, 0), + ("GAS_PEDAL", 581, 0), + ] + checks = [ + (550, 40), + (581, 33) + ] + elif CP.carFingerprint == CAR.RAV4: + dbc_f = 'toyota_rav4_2017_pt.dbc' + signals = [ + ("GEAR", 956, 0x20), + ("BRAKE_PRESSED", 548, 0), + ("GAS_PEDAL", 705, 0), + ] + checks = [ + (548, 40), + (705, 33) + ] + + # TODO: DOORS, GAS_PEDAL, BRAKE_PRESSED for RAV4 + signals += [ + # sig_name, sig_address, default + ("WHEEL_SPEED_FL", 170, 0), + ("WHEEL_SPEED_FR", 170, 0), + ("WHEEL_SPEED_RL", 170, 0), + ("WHEEL_SPEED_RR", 170, 0), + ("DOOR_OPEN_FL", 1568, 1), + ("DOOR_OPEN_FR", 1568, 1), + ("DOOR_OPEN_RL", 1568, 1), + ("DOOR_OPEN_RR", 1568, 1), + ("SEATBELT_DRIVER_UNLATCHED", 1568, 1), + ("TC_DISABLED", 951, 1), + ("STEER_ANGLE", 37, 0), + ("STEER_FRACTION", 37, 0), + ("STEER_RATE", 37, 0), + ("GAS_RELEASED", 466, 0), + ("CRUISE_STATE", 466, 0), + ("MAIN_ON", 467, 0), + ("SET_SPEED", 467, 0), + ("LOW_SPEED_LOCKOUT", 467, 0), + ("STEER_TORQUE_DRIVER", 608, 0), + ("STEER_TORQUE_EPS", 608, 0), + ("TURN_SIGNALS", 1556, 3), # 3 is no blinkers + ("LKA_STATE", 610, 0), + ] + + checks += [ + (170, 80), + (37, 80), + (466, 33), + (467, 33), + (608, 50), + (610, 25), + ] + + return CANParser(os.path.splitext(dbc_f)[0], signals, checks, 0) + + +class CarState(object): + def __init__(self, CP, logcan): + + self.CP = CP + self.left_blinker_on = 0 + self.right_blinker_on = 0 + + # initialize can parser + self.cp = get_can_parser(CP) + self.car_fingerprint = CP.carFingerprint + + # vEgo kalman filter + dt = 0.01 + self.v_ego_x = np.matrix([[0.0], [0.0]]) + self.v_ego_A = np.matrix([[1.0, dt], [0.0, 1.0]]) + self.v_ego_C = np.matrix([1.0, 0.0]) + self.v_ego_Q = np.matrix([[10.0, 0.0], [0.0, 100.0]]) + self.v_ego_R = 1e3 + # import control + # (x, l, K) = control.dare(np.transpose(A), np.transpose(C), Q, R) + # self.v_ego_K = np.transpose(K) + self.v_ego_K = np.matrix([[0.12287673], [0.29666309]]) + + def update(self): + cp = self.cp + cp.update(int(sec_since_boot() * 1e9), False) + + # copy can_valid + self.can_valid = cp.can_valid + + if self.car_fingerprint == CAR.PRIUS: + can_gear = cp.vl[295]['GEAR'] + self.brake_pressed = cp.vl[550]['BRAKE_PRESSED'] + self.pedal_gas = cp.vl[581]['GAS_PEDAL'] + elif self.car_fingerprint == CAR.RAV4: + can_gear = cp.vl[956]['GEAR'] + self.brake_pressed = cp.vl[548]['BRAKE_PRESSED'] + self.pedal_gas = cp.vl[705]['GAS_PEDAL'] + + # update prevs, update must run once per loop + self.prev_left_blinker_on = self.left_blinker_on + self.prev_right_blinker_on = self.right_blinker_on + + # ******************* parse out can ******************* + self.door_all_closed = not any([cp.vl[1568]['DOOR_OPEN_FL'], cp.vl[1568]['DOOR_OPEN_FR'], + cp.vl[1568]['DOOR_OPEN_RL'], cp.vl[1568]['DOOR_OPEN_RR']]) + self.seatbelt = not cp.vl[1568]['SEATBELT_DRIVER_UNLATCHED'] + # whitelist instead of blacklist, safer at the expense of disengages + self.steer_error = False + self.brake_error = 0 + self.esp_disabled = cp.vl[951]['TC_DISABLED'] + # calc best v_ego estimate, by averaging two opposite corners + self.v_wheel_fl = cp.vl[170]['WHEEL_SPEED_FL'] + self.v_wheel_fr = cp.vl[170]['WHEEL_SPEED_FR'] + self.v_wheel_rl = cp.vl[170]['WHEEL_SPEED_RL'] + self.v_wheel_rr = cp.vl[170]['WHEEL_SPEED_RR'] + self.v_wheel = ( + cp.vl[170]['WHEEL_SPEED_FL'] + cp.vl[170]['WHEEL_SPEED_FR'] + + cp.vl[170]['WHEEL_SPEED_RL'] + cp.vl[170]['WHEEL_SPEED_RR']) / 4. * CV.KPH_TO_MS + + # Kalman filter + self.v_ego_x = np.dot((self.v_ego_A - np.dot(self.v_ego_K, self.v_ego_C)), self.v_ego_x) + np.dot(self.v_ego_K, self.v_wheel) + self.v_ego_raw = self.v_wheel + self.v_ego = float(self.v_ego_x[0]) + self.a_ego = float(self.v_ego_x[1]) + self.standstill = not self.v_wheel > 0.001 + + self.angle_steers = cp.vl[37]['STEER_ANGLE'] + cp.vl[37]['STEER_FRACTION'] + self.angle_steers_rate = cp.vl[37]['STEER_RATE'] + self.gear_shifter = parse_gear_shifter(can_gear, self.car_fingerprint) + self.main_on = cp.vl[467]['MAIN_ON'] + self.left_blinker_on = cp.vl[1556]['TURN_SIGNALS'] == 1 + self.right_blinker_on = cp.vl[1556]['TURN_SIGNALS'] == 2 + + # we could use the override bit from dbc, but it's triggered at too high torque values + self.steer_override = abs(cp.vl[608]['STEER_TORQUE_DRIVER']) > 100 + self.steer_error = cp.vl[610]['LKA_STATE'] == 50 + self.steer_torque_driver = cp.vl[608]['STEER_TORQUE_DRIVER'] + self.steer_torque_motor = cp.vl[608]['STEER_TORQUE_EPS'] + + self.user_brake = 0 + self.v_cruise_pcm = cp.vl[467]['SET_SPEED'] + self.pcm_acc_status = cp.vl[466]['CRUISE_STATE'] + self.car_gas = self.pedal_gas + self.gas_pressed = not cp.vl[466]['GAS_RELEASED'] + self.low_speed_lockout = cp.vl[467]['LOW_SPEED_LOCKOUT'] == 2 diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py new file mode 100755 index 0000000000..b7af7d05ec --- /dev/null +++ b/selfdrive/car/toyota/interface.py @@ -0,0 +1,246 @@ +#!/usr/bin/env python +import os +import time +import common.numpy_fast as np +from selfdrive.config import Conversions as CV +from selfdrive.car.toyota.carstate import CarState, CAR +from selfdrive.car.toyota.carcontroller import CarController, ECU, check_ecu_msgs +from cereal import car +from selfdrive.services import service_list +import selfdrive.messaging as messaging +from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event + + +class CarInterface(object): + def __init__(self, CP, logcan, sendcan=None): + self.logcan = logcan + self.CP = CP + + self.frame = 0 + self.can_invalid_count = 0 + self.gas_pressed_prev = False + self.brake_pressed_prev = False + self.cruise_enabled_prev = False + + # *** init the major players *** + self.CS = CarState(CP, self.logcan) + + # sending if read only is False + if sendcan is not None: + self.sendcan = sendcan + self.CC = CarController(CP.carFingerprint, CP.enableCamera, CP.enableDsu, CP.enableApgs) + + @staticmethod + def get_params(candidate, fingerprint): + + # kg of standard extra cargo to count for drive, gas, etc... + std_cargo = 136 + + ret = car.CarParams.new_message() + + ret.carName = "toyota" + ret.radarName = "toyota" + ret.carFingerprint = candidate + + ret.safetyModel = car.CarParams.SafetyModels.toyota + + ret.enableSteer = True + ret.enableBrake = True + + # pedal + ret.enableCruise = True + + # 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 + aF_civic = l_civic * 0.4 + aR_civic = l_civic - aF_civic + j_civic = 2500 + cF_civic = 85400 + cR_civic = 90000 + + stop_and_go = True + ret.m = 3045./2.205 + std_cargo + ret.l = 2.70 + ret.aF = ret.l * 0.44 + ret.sR = 14.5 #Rav4 2017, TODO: find exact value for Prius + if candidate == CAR.PRIUS: + ret.steerKp, ret.steerKi = 0.2, 0.01 + elif candidate == CAR.RAV4: # rav4 control seem to be ok with integrators + ret.steerKp, ret.steerKi = 0.2, 0.05 + ret.steerKf = 0.00007818594 # full torque for 10 deg at 80mph + + # 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. + if candidate == CAR.PRIUS: + ret.minEnableSpeed = -1. + elif candidate == CAR.RAV4: # TODO: hack Rav4 to do stop and go + ret.minEnableSpeed = 19. * 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, 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. + + # steer, gas, brake limitations VS speed + ret.steerMaxBP = [16. * CV.KPH_TO_MS, 45. * CV.KPH_TO_MS] # breakpoints at 1 and 40 kph + ret.steerMaxV = [1., 1.] # 2/3rd torque allowed above 45 kph + ret.gasMaxBP = [0.] + ret.gasMaxV = [0.5] + ret.brakeMaxBP = [5., 20.] + ret.brakeMaxV = [1., 0.8] + + ret.enableCamera = not check_ecu_msgs(fingerprint, candidate, ECU.CAM) + ret.enableDsu = not check_ecu_msgs(fingerprint, candidate, ECU.DSU) + ret.enableApgs = False # not check_ecu_msgs(fingerprint, candidate, ECU.APGS) + print "ECU Camera Simulated: ", ret.enableCamera + print "ECU DSU Simulated: ", ret.enableDsu + print "ECU APGS Simulated: ", ret.enableApgs + ret.enableGas = True + + ret.steerLimitAlert = False + + return ret + + @staticmethod + def compute_gb(accel, speed): + # toyota interface is already in accelration cmd, so conversion to gas-brake it's a pass-through. + return accel + + # returns a car.CarState + def update(self, c): + # ******************* do can recv ******************* + can_pub_main = [] + canMonoTimes = [] + + self.CS.update() + + # create message + ret = car.CarState.new_message() + + # speeds + ret.vEgo = self.CS.v_ego + ret.vEgoRaw = self.CS.v_ego_raw + ret.aEgo = self.CS.a_ego + ret.standstill = self.CS.standstill + 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 + + # gear shifter + ret.gearShifter = self.CS.gear_shifter + + # gas pedal + ret.gas = self.CS.car_gas / 256.0 + ret.gasPressed = self.CS.pedal_gas > 0 + + # brake pedal + ret.brake = self.CS.user_brake + ret.brakePressed = self.CS.brake_pressed != 0 + + # steering wheel + ret.steeringAngle = self.CS.angle_steers + ret.steeringRate = self.CS.angle_steers_rate + + ret.steeringTorque = 0 + 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 = 0. + + # TODO: button presses + buttonEvents = [] + + if self.CS.left_blinker_on != self.CS.prev_left_blinker_on: + be = car.CarState.ButtonEvent.new_message() + be.type = 'leftBlinker' + be.pressed = self.CS.left_blinker_on != 0 + buttonEvents.append(be) + + if self.CS.right_blinker_on != self.CS.prev_right_blinker_on: + be = car.CarState.ButtonEvent.new_message() + be.type = 'rightBlinker' + be.pressed = self.CS.right_blinker_on != 0 + buttonEvents.append(be) + + ret.buttonEvents = buttonEvents + + # events + events = [] + if not self.CS.can_valid: + self.can_invalid_count += 1 + if self.can_invalid_count >= 5: + events.append(create_event('commIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) + else: + self.can_invalid_count = 0 + if not ret.gearShifter == 'drive' and self.CP.enableDsu: + events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE])) + if not self.CS.door_all_closed: + events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE])) + if not self.CS.seatbelt: + events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE])) + if self.CS.esp_disabled and self.CP.enableDsu: + events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE])) + if not self.CS.main_on and self.CP.enableDsu: + events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE])) + if ret.gearShifter == 'reverse' and self.CP.enableDsu: + events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) + if self.CS.steer_error: + events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING])) + if self.CS.low_speed_lockout: + events.append(create_event('lowSpeedLockout', [ET.NO_ENTRY])) + if ret.vEgo < self.CP.minEnableSpeed and self.CP.enableDsu: + events.append(create_event('speedTooLow', [ET.NO_ENTRY])) + if c.actuators.gas > 0.1: + # some margin on the actuator to not false trigger cancellation while stopping + events.append(create_event('speedTooLow', [ET.IMMEDIATE_DISABLE])) + if ret.vEgo < 0.001: + # while in standstill, send a user alert + events.append(create_event('manualRestart', [ET.WARNING])) + + # enable request in prius is simple, as we activate when Toyota is active (rising edge) + if ret.cruiseState.enabled and not self.cruise_enabled_prev: + events.append(create_event('pcmEnable', [ET.ENABLE])) + elif not ret.cruiseState.enabled: + events.append(create_event('pcmDisable', [ET.USER_DISABLE])) + + # 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.gasPressed: + events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) + + ret.events = events + ret.canMonoTimes = canMonoTimes + + self.gas_pressed_prev = ret.gasPressed + self.brake_pressed_prev = ret.brakePressed + self.cruise_enabled_prev = ret.cruiseState.enabled + + return ret.as_reader() + + # pass in a car.CarControl + # to be called @ 100hz + def apply(self, c): + + self.CC.update(self.sendcan, c.enabled, self.CS, self.frame, + c.actuators, c.cruiseControl.cancel, c.hudControl.visualAlert, + c.hudControl.audibleAlert) + + self.frame += 1 + return False diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py new file mode 100644 index 0000000000..cfba3ce5af --- /dev/null +++ b/selfdrive/car/toyota/toyotacan.py @@ -0,0 +1,83 @@ +import struct +import common.numpy_fast as np +from selfdrive.config import Conversions as CV + + +# *** Toyota specific *** + +def fix(msg, addr): + checksum = 0 + idh = (addr & 0xff00) >> 8 + idl = (addr & 0xff) + + checksum = idh + idl + len(msg) + 1 + for d_byte in msg: + checksum += ord(d_byte) + + #return msg + chr(checksum & 0xFF) + return msg + struct.pack("B", checksum & 0xFF) + + +def make_can_msg(addr, dat, alt, cks=False): + if cks: + dat = fix(dat, addr) + return [addr, 0, dat, alt] + + +def create_video_target(frame, addr): + counter = frame & 0xff + msg = struct.pack("!BBBBBBB", counter, 0x03, 0xff, 0x00, 0x00, 0x00, 0x00) + return make_can_msg(addr, msg, 1, True) + + +def create_ipas_steer_command(steer): + + """Creates a CAN message for the Toyota Steer Command.""" + if steer < 0: + move = 0x60 + steer = 0xfff + steer + 1 + elif steer > 0: + move = 0x20 + else: + move = 0x40 + + mode = 0x30 if steer else 0x10 + + steer_h = (steer & 0xF00) >> 8 + steer_l = steer & 0xff + + msg = struct.pack("!BBBBBBB", mode | steer_h, steer_l, 0x10, 0x00, move, 0x40, 0x00) + + return make_can_msg(0x266, msg, 0, True) + +def create_steer_command(steer, raw_cnt): + """Creates a CAN message for the Toyota Steer Command.""" + # from 0x80 to 0xff + counter = ((raw_cnt & 0x3f) << 1) | 0x80 + if steer != 0: + counter |= 1 + + # hud + # 00 => Regular + # 40 => Actively Steering (with beep) + # 80 => Actively Steering (without beep) + hud = 0x00 + + msg = struct.pack("!BhB", counter, steer, hud) + + return make_can_msg(0x2e4, msg, 0, True) + + +def create_accel_command(accel, pcm_cancel): + # TODO: find the exact canceling bit + state = 0xc0 + pcm_cancel # this allows automatic restart from hold without driver cmd + + msg = struct.pack("!hBBBBB", accel, 0x63, state, 0x00, 0x00, 0x00) + + return make_can_msg(0x343, msg, 0, True) + + +def create_ui_command(hud1, hud2): + msg = struct.pack('!BBBBBBBB', 0x54, 0x04 + hud1 + (hud2 << 4), 0x0C, + 0x00, 0x00, 0x2C, 0x38, 0x02) + return make_can_msg(0x412, msg, 0, False) diff --git a/selfdrive/common/cereal.mk b/selfdrive/common/cereal.mk index 637a104061..46f23be551 100644 --- a/selfdrive/common/cereal.mk +++ b/selfdrive/common/cereal.mk @@ -2,27 +2,31 @@ UNAME_M ?= $(shell uname -m) UNAME_S ?= $(shell uname -s) -ifeq ($(UNAME_S),Darwin) CEREAL_CFLAGS = -I$(PHONELIBS)/capnp-c/include -CEREAL_CXXFLAGS = -I$(PHONELIBS)/capnp-cpp/mac/include +ifeq ($(OPTEST),1) + +CEREAL_LIBS = -lcapnp -lkj + +else ifeq ($(UNAME_S),Darwin) + +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) +CEREAL_CXXFLAGS = -I$(PHONELIBS)/capnp-cpp/include ifeq ($(CEREAL_LIBS),) - CEREAL_LIBS = -L$(BASEDIR)/external/capnp/lib \ - -l:libcapnp.a -l:libkj.a -l:libcapnp_c.a + CEREAL_LIBS = -L$(PHONELIBS)/capnp-cpp/x64/lib/ \ + -L$(PHONELIBS)/capnp-c/x64/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),) CEREAL_LIBS = -L$(PHONELIBS)/capnp-cpp/aarch64/lib/ \ diff --git a/selfdrive/common/modeldata.h b/selfdrive/common/modeldata.h index 521767775f..c3f48930f2 100644 --- a/selfdrive/common/modeldata.h +++ b/selfdrive/common/modeldata.h @@ -1,8 +1,10 @@ #ifndef MODELDATA_H #define MODELDATA_H +#define MODEL_PATH_DISTANCE 50 + typedef struct PathData { - float points[50]; + float points[MODEL_PATH_DISTANCE]; float prob; float std; } PathData; diff --git a/selfdrive/common/swaglog.c b/selfdrive/common/swaglog.c index 5ce88dcccb..70cba78167 100644 --- a/selfdrive/common/swaglog.c +++ b/selfdrive/common/swaglog.c @@ -57,7 +57,7 @@ static void cloudlog_init() { if (dongle_id) { cloudlog_bind_locked("dongle_id", dongle_id); } - cloudlog_bind_locked("version", OPENPILOT_VERSION); + cloudlog_bind_locked("version", COMMA_VERSION); bool dirty = !getenv("CLEAN"); json_append_member(s.ctx_j, "dirty", json_mkbool(dirty)); diff --git a/selfdrive/common/version.h b/selfdrive/common/version.h index de335592cd..fad8964840 100644 --- a/selfdrive/common/version.h +++ b/selfdrive/common/version.h @@ -1 +1 @@ -#define OPENPILOT_VERSION "0.3.7" +#define COMMA_VERSION "0.3.8.2-openpilot" diff --git a/selfdrive/config.py b/selfdrive/config.py index 7d923b8098..fc08f6292f 100644 --- a/selfdrive/config.py +++ b/selfdrive/config.py @@ -51,7 +51,7 @@ class ImageParams: self.VPY = self.VPY_R + to_int(shift[1]) # current vanishing point with shift class UIParams: - lidar_x, lidar_y, lidar_zoom = 384, 960, 8 + lidar_x, lidar_y, lidar_zoom = 384, 960, 6 lidar_car_x, lidar_car_y = lidar_x/2., lidar_y/1.1 car_hwidth = 1.7272/2 * lidar_zoom car_front = 2.6924 * lidar_zoom diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 80b25875d7..218845a73c 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -5,7 +5,6 @@ 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 @@ -42,10 +41,10 @@ class Calibration: class State: - DISABLED = 0 - ENABLED = 1 - PRE_ENABLED = 2 - SOFT_DISABLING = 3 + DISABLED = 'disabled' + ENABLED = 'enabled' + PRE_ENABLED = 'preEnabled' + SOFT_DISABLING = 'softDisabling' # True when actuators are controlled @@ -280,7 +279,7 @@ def state_control(plan, CS, CP, state, events, v_cruise_kph, AM, rk, awareness_s CS.steeringPressed) # *** gas/brake PID loop *** - actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.brakePressed, + actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.brakePressed, CS.standstill, v_cruise_kph, plan.vTarget, [plan.aTargetMin, plan.aTargetMax], plan.jerkFactor, CP) @@ -290,7 +289,7 @@ def state_control(plan, CS, CP, state, events, v_cruise_kph, AM, rk, awareness_s CS.steeringPressed, plan.dPoly, angle_offset, VM, PL) # send a "steering required alert" if saturation count has reached the limit - if LaC.sat_flag: + if LaC.sat_flag and CP.steerLimitAlert: AM.add("steerSaturated", enabled) if CP.enableCruise and CS.cruiseState.enabled: @@ -305,38 +304,40 @@ def state_control(plan, CS, CP, state, events, v_cruise_kph, AM, rk, awareness_s 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): + LaC, LoC, angle_offset, passive): # ***** control the car ***** CC = car.CarControl.new_message() - CC.enabled = isEnabled(state) + if not passive: - CC.actuators = actuators + CC.enabled = isEnabled(state) - 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) + CC.actuators = actuators - # 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) + 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) - # 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)) + # 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) - 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 + # 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)) - # send car controls over can - CI.apply(CC) + 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 @@ -358,10 +359,15 @@ def data_send(plan, plan_ts, CS, CI, CP, state, events, actuators, v_cruise_kph, # car state dat.live100.vEgo = CS.vEgo + dat.live100.vEgoRaw = CS.vEgoRaw dat.live100.angleSteers = CS.steeringAngle dat.live100.steerOverride = CS.steeringPressed + # high level control state + dat.live100.state = state + # longitudinal control state + dat.live100.longControlState = LoC.long_control_state dat.live100.vPid = float(LoC.v_pid) dat.live100.vCruise = float(v_cruise_kph) dat.live100.upAccelCmd = float(LoC.pid.p) @@ -420,13 +426,20 @@ def controlsd_thread(gctx, rate=100): context = zmq.Context() + params = Params() + # 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) + passive = params.get("Passive") != "0" + if not passive: + sendcan = messaging.pub_sock(context, service_list['sendcan'].port) + else: + sendcan = None + # sub thermal = messaging.sub_sock(context, service_list['thermal'].port) health = messaging.sub_sock(context, service_list['health'].port) @@ -434,15 +447,28 @@ def controlsd_thread(gctx, rate=100): logcan = messaging.sub_sock(context, service_list['can'].port) CC = car.CarControl.new_message() - CI, CP = get_car(logcan, sendcan) + + CI, CP = get_car(logcan, sendcan, 1.0 if passive else None) + + if CI is None: + if passive: + return + else: + raise Exception("unsupported car") + + if passive: + CP.safetyModel = car.CarParams.SafetyModels.noOutput + PL = Planner(CP) LoC = LongControl(CI.compute_gb) VM = VehicleModel(CP) LaC = LatControl(VM) AM = AlertManager() + if not passive: + AM.add("startup", False) + # write CarParams - params = Params() params.put("CarParams", CP.to_bytes()) state = State.DISABLED @@ -484,9 +510,10 @@ def controlsd_thread(gctx, rate=100): 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, AM) - prof.checkpoint("State transition") + if not passive: + # update control state + state, soft_disable_timer, v_cruise_kph = state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, 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, @@ -497,7 +524,7 @@ def controlsd_thread(gctx, rate=100): # 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) + rear_view_toggle, awareness_status, LaC, LoC, angle_offset, passive) prof.checkpoint("Sent") # *** run loop at fixed rate *** diff --git a/selfdrive/controls/lib/alertmanager.py b/selfdrive/controls/lib/alertmanager.py index 19512b579c..30422b20ee 100644 --- a/selfdrive/controls/lib/alertmanager.py +++ b/selfdrive/controls/lib/alertmanager.py @@ -59,9 +59,9 @@ class AlertManager(object): PT.MID, None, "beepSingle", .2, 0., 0.), "fcw": Alert( - "", - "", - PT.LOW, None, None, .1, .1, .1), + "Brake", + "Risk of Collision", + PT.HIGH, "fcw", "chimeRepeated", 1., 2., 2.), "steerSaturated": Alert( "Take Control", @@ -74,7 +74,7 @@ class AlertManager(object): PT.LOW, "steerRequired", "chimeDouble", .4, 2., 3.), "preDriverDistracted": Alert( - "Take Control ", + "Take Control", "User Distracted", PT.LOW, "steerRequired", "chimeDouble", .4, 2., 3.), @@ -98,6 +98,11 @@ class AlertManager(object): "Steer Temporary Unavailable", PT.LOW, None, "chimeDouble", .4, 0., 3.), + "manualRestart": Alert( + "Take Control", + "Resume Driving Manually", + PT.LOW, None, None, .0, 0., 1.), + # Non-entry only alerts "wrongCarModeNoEntry": Alert( "Comma Unavailable", @@ -134,6 +139,11 @@ class AlertManager(object): "Park Brake Engaged", PT.LOW, None, "chimeDouble", .4, 2., 3.), + "lowSpeedLockoutNoEntry": Alert( + "Comma Unavailable", + "Cruise Fault: Restart the Car", + PT.LOW, None, "chimeDouble", .4, 2., 3.), + # Cancellation alerts causing disabling "overheat": Alert( "Take Control Immediately", @@ -231,6 +241,11 @@ class AlertManager(object): "No Close Lead", PT.HIGH, None, "chimeDouble", .4, 2., 3.), + "speedTooLow": Alert( + "Comma Canceled", + "Speed Too Low", + PT.HIGH, None, "chimeDouble", .4, 2., 3.), + # Cancellation alerts causing non-entry "overheatNoEntry": Alert( "Comma Unavailable", @@ -331,7 +346,6 @@ class AlertManager(object): def __init__(self): self.activealerts = [] self.current_alert = None - self.add("startup", False) def alertPresent(self): return len(self.activealerts) > 0 diff --git a/selfdrive/controls/lib/fcw.py b/selfdrive/controls/lib/fcw.py index c9d6507057..15b6f245f4 100644 --- a/selfdrive/controls/lib/fcw.py +++ b/selfdrive/controls/lib/fcw.py @@ -8,13 +8,13 @@ def calc_ttc(l1): # if l1 is None, return max ttc immediately if not l1: return MAX_TTC - # this function returns the time to collision (ttc), assuming that + # this function returns the time to collision (ttc), assuming that # ARel will stay constant TODO: review this assumptions # change sign to rel quantities as it's going to be easier for calculations vRel = -l1.vRel aRel = -l1.aRel - # assuming that closing gap ARel comes from lead vehicle decel, + # assuming that closing gap ARel comes from lead vehicle decel, # then limit ARel so that v_lead will get to zero in no sooner than t_decel. # This helps underweighting ARel when v_lead is close to zero. t_decel = 2. @@ -44,10 +44,10 @@ class ForwardCollisionWarning(object): a_acc_on = -2.0 # with system on, above this limit of desired decel, we should trigger fcw a_acc_off = -2.5 # with system off, above this limit of desired decel, we should trigger fcw ttc_thrs = 2.5 # ttc threshold for fcw - v_fcw_min = 2. # no fcw below 2m/s + v_fcw_min = 5. # no fcw below 5m/s steer_angle_th = 40. # deg, no fcw above this steer angle cur_time = sec_since_boot() - + ttc = calc_ttc(AC.l1) a_fcw = a_acc_on if CS.cruiseState.enabled else a_acc_off @@ -59,8 +59,8 @@ class ForwardCollisionWarning(object): self.violation_time = np.minimum(self.violation_time + self.dt, violation_thrs) else: self.violation_time = np.maximum(self.violation_time - 2*self.dt, 0) - - # fire FCW + + # fire FCW self.active = self.violation_time >= violation_thrs and cur_time > (self.last_active + fcw_t_delta) if self.active: self.last_active = cur_time diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index ab81435fc7..5b86dc9063 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -20,7 +20,7 @@ def get_steer_max(CP, v_ego): class LatControl(object): def __init__(self, VM): - self.pid = PIController(VM.CP.steerKp, VM.CP.steerKi, pos_limit=1.0) + self.pid = PIController(VM.CP.steerKp, VM.CP.steerKi, k_f=VM.CP.steerKf, pos_limit=1.0) self.setup_mpc() self.y_des = -1 # Legacy @@ -57,14 +57,15 @@ class LatControl(object): # account for actuation delay self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers, curvature_factor, VM.CP.sR) + v_ego_mpc = max(v_ego, 5.0) # avoid mpc roughness due to low speed 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) + PL.PP.l_prob, PL.PP.r_prob, PL.PP.p_prob, curvature_factor, v_ego_mpc, PL.PP.lane_width) delta_desired = self.mpc_solution[0].delta[1] self.cur_state[0].delta = delta_desired - self.angle_steers_des = math.degrees(delta_desired * VM.CP.sR) + angle_offset + self.angle_steers_des = float(math.degrees(delta_desired * VM.CP.sR) + angle_offset) self.mpc_updated = True if v_ego < 0.3 or not active: @@ -74,7 +75,8 @@ class LatControl(object): 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) + steer_feedforward = self.angle_steers_des * v_ego**2 # proportional to realigning tire momentum (~ lateral accel) + output_steer = self.pid.update(self.angle_steers_des, angle_steers, check_saturation=(v_ego > 10), override=steer_override, feedforward=steer_feedforward) 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 index 982152d65c..90abcebf10 100644 --- a/selfdrive/controls/lib/latcontrol_helpers.py +++ b/selfdrive/controls/lib/latcontrol_helpers.py @@ -50,9 +50,9 @@ def calc_desired_steer_angle(v_ego, y_des, d_lookahead, VM, angle_offset): return steer_des, curvature -def compute_path_pinv(): +def compute_path_pinv(l=50): deg = 3 - x = np.arange(50.0) + x = np.arange(l*1.0) X = np.vstack(tuple(x**n for n in range(deg, -1, -1))).T pinv = np.linalg.pinv(X) return pinv diff --git a/selfdrive/controls/lib/lateral_mpc/Makefile b/selfdrive/controls/lib/lateral_mpc/Makefile index b0802f9a44..becef563e5 100644 --- a/selfdrive/controls/lib/lateral_mpc/Makefile +++ b/selfdrive/controls/lib/lateral_mpc/Makefile @@ -1,3 +1,4 @@ + CC = clang CXX = clang++ @@ -8,7 +9,7 @@ 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 +QPOASES_FLAGS = -I$(PHONELIBS)/qpoases -I$(PHONELIBS)/qpoases/INCLUDE -I$(PHONELIBS)/qpoases/SRC ACADO_FLAGS = -I$(PHONELIBS)/acado/include -I$(PHONELIBS)/acado/include/acado @@ -19,17 +20,17 @@ ACADO_LIBS := -L $(PHONELIBS)/acado/x64/lib -l:libacado_toolkit.a -l:libacado_ca 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 \ + qp/Bounds.o \ + qp/Constraints.o \ + qp/CyclingManager.o \ + qp/Indexlist.o \ + qp/MessageHandling.o \ + qp/QProblem.o \ + qp/QProblemB.o \ + qp/SubjectTo.o \ + qp/Utils.o \ + qp/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 \ @@ -43,6 +44,23 @@ all: libcommampc.so libcommampc.so: $(OBJS) $(CXX) -shared -o '$@' $^ -lm + +qp/%.o: $(PHONELIBS)/qpoases/SRC/%.cpp + @echo "[ CXX ] $@" + mkdir -p qp + $(CXX) $(CXXFLAGS) -MMD \ + -I mpc_export/ \ + $(QPOASES_FLAGS) \ + -c -o '$@' '$<' + +qp/EXTRAS/%.o: $(PHONELIBS)/qpoases/SRC/EXTRAS/%.cpp + @echo "[ CXX ] $@" + mkdir -p qp/EXTRAS + $(CXX) $(CXXFLAGS) -MMD \ + -I mpc_export/ \ + $(QPOASES_FLAGS) \ + -c -o '$@' '$<' + %.o: %.cpp @echo "[ CXX ] $@" $(CXX) $(CXXFLAGS) -MMD \ diff --git a/selfdrive/controls/lib/lateral_mpc/generator.cpp b/selfdrive/controls/lib/lateral_mpc/generator.cpp index 2e5c3a2ff4..350a783ee5 100644 --- a/selfdrive/controls/lib/lateral_mpc/generator.cpp +++ b/selfdrive/controls/lib/lateral_mpc/generator.cpp @@ -79,7 +79,7 @@ int main( ) Q(3,3) = 1.0; - Q(4,4) = 1.0; + Q(4,4) = 0.5; // Terminal cost Function hN; @@ -118,7 +118,8 @@ int main( ) 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( NUM_INTEGRATOR_STEPS, 1 * controlHorizon); + mpc.set( MAX_NUM_QP_ITERATIONS, 500); mpc.set( SPARSE_QP_SOLUTION, CONDENSING ); mpc.set( QP_SOLVER, QP_QPOASES ); diff --git a/selfdrive/controls/lib/lateral_mpc/libmpc_py.py b/selfdrive/controls/lib/lateral_mpc/libmpc_py.py index 032567be59..290fcb2436 100644 --- a/selfdrive/controls/lib/lateral_mpc/libmpc_py.py +++ b/selfdrive/controls/lib/lateral_mpc/libmpc_py.py @@ -4,7 +4,6 @@ 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) @@ -22,7 +21,7 @@ typedef struct { } log_t; void init(); -void run_mpc(state_t * x0, log_t * solution, +int 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); """) diff --git a/selfdrive/controls/lib/lateral_mpc/mpc.c b/selfdrive/controls/lib/lateral_mpc/mpc.c index 6823c554c2..6157b7653a 100644 --- a/selfdrive/controls/lib/lateral_mpc/mpc.c +++ b/selfdrive/controls/lib/lateral_mpc/mpc.c @@ -44,7 +44,7 @@ void init(){ for (i = 0; i < NX; ++i) acadoVariables.x0[ i ] = 0.0; } -void run_mpc(state_t * x0, log_t * solution, +int 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){ @@ -84,10 +84,8 @@ void run_mpc(state_t * x0, log_t * solution, acado_preparationStep(); - acado_feedbackStep( ); - - acado_shiftStates(2, 0, 0); - acado_shiftControls( 0 ); + acado_feedbackStep(); + /* printf("lat its: %d\n", acado_getNWSR()); */ for (i = 0; i <= N; i++){ solution->x[i] = acadoVariables.x[i*NX]; @@ -96,5 +94,9 @@ void run_mpc(state_t * x0, log_t * solution, solution->delta[i] = acadoVariables.x[i*NX+3]; } - return; + acado_shiftStates(2, 0, 0); + acado_shiftControls( 0 ); + + + return acado_getNWSR(); } diff --git a/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_common.h b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_common.h index a183304f01..4401190a78 100644 --- a/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_common.h +++ b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_common.h @@ -82,7 +82,7 @@ extern "C" /** Total number of QP optimization variables. */ #define ACADO_QP_NV 54 /** Number of integration steps per shooting interval. */ -#define ACADO_RK_NIS 5 +#define ACADO_RK_NIS 1 /** Number of Runge-Kutta stages per integration step. */ #define ACADO_RK_NSTAGES 4 /** Providing interface for arrival cost. */ diff --git a/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_integrator.c b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_integrator.c index f243d0ffa2..afff2ce7c6 100644 --- a/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_integrator.c +++ b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_integrator.c @@ -71,7 +71,7 @@ out[22] = ((od[1]*xd[23])*od[0]); out[23] = (real_t)(1.0000000000000000e+00); } -/* Fixed step size:0.01 */ +/* Fixed step size:0.05 */ int acado_integrate( real_t* const rk_eta, int resetIntegrator ) { int error; @@ -118,7 +118,7 @@ acadoWorkspace.rk_xxx[40] = rk_eta[40]; acadoWorkspace.rk_xxx[41] = rk_eta[41]; acadoWorkspace.rk_xxx[42] = rk_eta[42]; -for (run1 = 0; run1 < 5; ++run1) +for (run1 = 0; run1 < 1; ++run1) { acadoWorkspace.rk_xxx[0] = + rk_eta[0]; acadoWorkspace.rk_xxx[1] = + rk_eta[1]; @@ -145,106 +145,106 @@ acadoWorkspace.rk_xxx[21] = + rk_eta[21]; acadoWorkspace.rk_xxx[22] = + rk_eta[22]; acadoWorkspace.rk_xxx[23] = + rk_eta[23]; acado_rhs_forw( acadoWorkspace.rk_xxx, acadoWorkspace.rk_kkk ); -acadoWorkspace.rk_xxx[0] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[0] + rk_eta[0]; -acadoWorkspace.rk_xxx[1] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[1] + rk_eta[1]; -acadoWorkspace.rk_xxx[2] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[2] + rk_eta[2]; -acadoWorkspace.rk_xxx[3] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[3] + rk_eta[3]; -acadoWorkspace.rk_xxx[4] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[4] + rk_eta[4]; -acadoWorkspace.rk_xxx[5] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[5] + rk_eta[5]; -acadoWorkspace.rk_xxx[6] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[6] + rk_eta[6]; -acadoWorkspace.rk_xxx[7] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[7] + rk_eta[7]; -acadoWorkspace.rk_xxx[8] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[8] + rk_eta[8]; -acadoWorkspace.rk_xxx[9] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[9] + rk_eta[9]; -acadoWorkspace.rk_xxx[10] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[10] + rk_eta[10]; -acadoWorkspace.rk_xxx[11] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[11] + rk_eta[11]; -acadoWorkspace.rk_xxx[12] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[12] + rk_eta[12]; -acadoWorkspace.rk_xxx[13] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[13] + rk_eta[13]; -acadoWorkspace.rk_xxx[14] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[14] + rk_eta[14]; -acadoWorkspace.rk_xxx[15] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[15] + rk_eta[15]; -acadoWorkspace.rk_xxx[16] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[16] + rk_eta[16]; -acadoWorkspace.rk_xxx[17] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[17] + rk_eta[17]; -acadoWorkspace.rk_xxx[18] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[18] + rk_eta[18]; -acadoWorkspace.rk_xxx[19] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[19] + rk_eta[19]; -acadoWorkspace.rk_xxx[20] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[20] + rk_eta[20]; -acadoWorkspace.rk_xxx[21] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[21] + rk_eta[21]; -acadoWorkspace.rk_xxx[22] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[22] + rk_eta[22]; -acadoWorkspace.rk_xxx[23] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[23] + rk_eta[23]; +acadoWorkspace.rk_xxx[0] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[0] + rk_eta[0]; +acadoWorkspace.rk_xxx[1] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[1] + rk_eta[1]; +acadoWorkspace.rk_xxx[2] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[2] + rk_eta[2]; +acadoWorkspace.rk_xxx[3] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[3] + rk_eta[3]; +acadoWorkspace.rk_xxx[4] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[4] + rk_eta[4]; +acadoWorkspace.rk_xxx[5] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[5] + rk_eta[5]; +acadoWorkspace.rk_xxx[6] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[6] + rk_eta[6]; +acadoWorkspace.rk_xxx[7] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[7] + rk_eta[7]; +acadoWorkspace.rk_xxx[8] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[8] + rk_eta[8]; +acadoWorkspace.rk_xxx[9] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[9] + rk_eta[9]; +acadoWorkspace.rk_xxx[10] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[10] + rk_eta[10]; +acadoWorkspace.rk_xxx[11] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[11] + rk_eta[11]; +acadoWorkspace.rk_xxx[12] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[12] + rk_eta[12]; +acadoWorkspace.rk_xxx[13] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[13] + rk_eta[13]; +acadoWorkspace.rk_xxx[14] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[14] + rk_eta[14]; +acadoWorkspace.rk_xxx[15] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[15] + rk_eta[15]; +acadoWorkspace.rk_xxx[16] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[16] + rk_eta[16]; +acadoWorkspace.rk_xxx[17] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[17] + rk_eta[17]; +acadoWorkspace.rk_xxx[18] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[18] + rk_eta[18]; +acadoWorkspace.rk_xxx[19] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[19] + rk_eta[19]; +acadoWorkspace.rk_xxx[20] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[20] + rk_eta[20]; +acadoWorkspace.rk_xxx[21] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[21] + rk_eta[21]; +acadoWorkspace.rk_xxx[22] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[22] + rk_eta[22]; +acadoWorkspace.rk_xxx[23] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[23] + rk_eta[23]; acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 24 ]) ); -acadoWorkspace.rk_xxx[0] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[24] + rk_eta[0]; -acadoWorkspace.rk_xxx[1] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[25] + rk_eta[1]; -acadoWorkspace.rk_xxx[2] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[26] + rk_eta[2]; -acadoWorkspace.rk_xxx[3] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[27] + rk_eta[3]; -acadoWorkspace.rk_xxx[4] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[28] + rk_eta[4]; -acadoWorkspace.rk_xxx[5] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[29] + rk_eta[5]; -acadoWorkspace.rk_xxx[6] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[30] + rk_eta[6]; -acadoWorkspace.rk_xxx[7] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[31] + rk_eta[7]; -acadoWorkspace.rk_xxx[8] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[32] + rk_eta[8]; -acadoWorkspace.rk_xxx[9] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[33] + rk_eta[9]; -acadoWorkspace.rk_xxx[10] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[34] + rk_eta[10]; -acadoWorkspace.rk_xxx[11] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[35] + rk_eta[11]; -acadoWorkspace.rk_xxx[12] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[36] + rk_eta[12]; -acadoWorkspace.rk_xxx[13] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[37] + rk_eta[13]; -acadoWorkspace.rk_xxx[14] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[38] + rk_eta[14]; -acadoWorkspace.rk_xxx[15] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[39] + rk_eta[15]; -acadoWorkspace.rk_xxx[16] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[40] + rk_eta[16]; -acadoWorkspace.rk_xxx[17] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[41] + rk_eta[17]; -acadoWorkspace.rk_xxx[18] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[42] + rk_eta[18]; -acadoWorkspace.rk_xxx[19] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[43] + rk_eta[19]; -acadoWorkspace.rk_xxx[20] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[44] + rk_eta[20]; -acadoWorkspace.rk_xxx[21] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[45] + rk_eta[21]; -acadoWorkspace.rk_xxx[22] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[46] + rk_eta[22]; -acadoWorkspace.rk_xxx[23] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[47] + rk_eta[23]; +acadoWorkspace.rk_xxx[0] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[24] + rk_eta[0]; +acadoWorkspace.rk_xxx[1] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[25] + rk_eta[1]; +acadoWorkspace.rk_xxx[2] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[26] + rk_eta[2]; +acadoWorkspace.rk_xxx[3] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[27] + rk_eta[3]; +acadoWorkspace.rk_xxx[4] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[28] + rk_eta[4]; +acadoWorkspace.rk_xxx[5] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[29] + rk_eta[5]; +acadoWorkspace.rk_xxx[6] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[30] + rk_eta[6]; +acadoWorkspace.rk_xxx[7] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[31] + rk_eta[7]; +acadoWorkspace.rk_xxx[8] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[32] + rk_eta[8]; +acadoWorkspace.rk_xxx[9] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[33] + rk_eta[9]; +acadoWorkspace.rk_xxx[10] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[34] + rk_eta[10]; +acadoWorkspace.rk_xxx[11] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[35] + rk_eta[11]; +acadoWorkspace.rk_xxx[12] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[36] + rk_eta[12]; +acadoWorkspace.rk_xxx[13] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[37] + rk_eta[13]; +acadoWorkspace.rk_xxx[14] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[38] + rk_eta[14]; +acadoWorkspace.rk_xxx[15] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[39] + rk_eta[15]; +acadoWorkspace.rk_xxx[16] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[40] + rk_eta[16]; +acadoWorkspace.rk_xxx[17] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[41] + rk_eta[17]; +acadoWorkspace.rk_xxx[18] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[42] + rk_eta[18]; +acadoWorkspace.rk_xxx[19] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[43] + rk_eta[19]; +acadoWorkspace.rk_xxx[20] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[44] + rk_eta[20]; +acadoWorkspace.rk_xxx[21] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[45] + rk_eta[21]; +acadoWorkspace.rk_xxx[22] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[46] + rk_eta[22]; +acadoWorkspace.rk_xxx[23] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[47] + rk_eta[23]; acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 48 ]) ); -acadoWorkspace.rk_xxx[0] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[48] + rk_eta[0]; -acadoWorkspace.rk_xxx[1] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[49] + rk_eta[1]; -acadoWorkspace.rk_xxx[2] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[50] + rk_eta[2]; -acadoWorkspace.rk_xxx[3] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[51] + rk_eta[3]; -acadoWorkspace.rk_xxx[4] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[52] + rk_eta[4]; -acadoWorkspace.rk_xxx[5] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[53] + rk_eta[5]; -acadoWorkspace.rk_xxx[6] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[54] + rk_eta[6]; -acadoWorkspace.rk_xxx[7] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[55] + rk_eta[7]; -acadoWorkspace.rk_xxx[8] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[56] + rk_eta[8]; -acadoWorkspace.rk_xxx[9] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[57] + rk_eta[9]; -acadoWorkspace.rk_xxx[10] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[58] + rk_eta[10]; -acadoWorkspace.rk_xxx[11] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[59] + rk_eta[11]; -acadoWorkspace.rk_xxx[12] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[60] + rk_eta[12]; -acadoWorkspace.rk_xxx[13] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[61] + rk_eta[13]; -acadoWorkspace.rk_xxx[14] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[62] + rk_eta[14]; -acadoWorkspace.rk_xxx[15] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[63] + rk_eta[15]; -acadoWorkspace.rk_xxx[16] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[64] + rk_eta[16]; -acadoWorkspace.rk_xxx[17] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[65] + rk_eta[17]; -acadoWorkspace.rk_xxx[18] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[66] + rk_eta[18]; -acadoWorkspace.rk_xxx[19] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[67] + rk_eta[19]; -acadoWorkspace.rk_xxx[20] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[68] + rk_eta[20]; -acadoWorkspace.rk_xxx[21] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[69] + rk_eta[21]; -acadoWorkspace.rk_xxx[22] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[70] + rk_eta[22]; -acadoWorkspace.rk_xxx[23] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[71] + rk_eta[23]; +acadoWorkspace.rk_xxx[0] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[48] + rk_eta[0]; +acadoWorkspace.rk_xxx[1] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[49] + rk_eta[1]; +acadoWorkspace.rk_xxx[2] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[50] + rk_eta[2]; +acadoWorkspace.rk_xxx[3] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[51] + rk_eta[3]; +acadoWorkspace.rk_xxx[4] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[52] + rk_eta[4]; +acadoWorkspace.rk_xxx[5] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[53] + rk_eta[5]; +acadoWorkspace.rk_xxx[6] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[54] + rk_eta[6]; +acadoWorkspace.rk_xxx[7] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[55] + rk_eta[7]; +acadoWorkspace.rk_xxx[8] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[56] + rk_eta[8]; +acadoWorkspace.rk_xxx[9] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[57] + rk_eta[9]; +acadoWorkspace.rk_xxx[10] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[58] + rk_eta[10]; +acadoWorkspace.rk_xxx[11] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[59] + rk_eta[11]; +acadoWorkspace.rk_xxx[12] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[60] + rk_eta[12]; +acadoWorkspace.rk_xxx[13] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[61] + rk_eta[13]; +acadoWorkspace.rk_xxx[14] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[62] + rk_eta[14]; +acadoWorkspace.rk_xxx[15] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[63] + rk_eta[15]; +acadoWorkspace.rk_xxx[16] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[64] + rk_eta[16]; +acadoWorkspace.rk_xxx[17] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[65] + rk_eta[17]; +acadoWorkspace.rk_xxx[18] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[66] + rk_eta[18]; +acadoWorkspace.rk_xxx[19] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[67] + rk_eta[19]; +acadoWorkspace.rk_xxx[20] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[68] + rk_eta[20]; +acadoWorkspace.rk_xxx[21] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[69] + rk_eta[21]; +acadoWorkspace.rk_xxx[22] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[70] + rk_eta[22]; +acadoWorkspace.rk_xxx[23] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[71] + rk_eta[23]; acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 72 ]) ); -rk_eta[0] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[0] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[24] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[48] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[72]; -rk_eta[1] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[1] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[25] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[49] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[73]; -rk_eta[2] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[2] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[26] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[50] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[74]; -rk_eta[3] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[3] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[27] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[51] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[75]; -rk_eta[4] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[4] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[28] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[52] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[76]; -rk_eta[5] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[5] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[29] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[53] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[77]; -rk_eta[6] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[6] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[30] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[54] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[78]; -rk_eta[7] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[7] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[31] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[55] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[79]; -rk_eta[8] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[8] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[32] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[56] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[80]; -rk_eta[9] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[9] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[33] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[57] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[81]; -rk_eta[10] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[10] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[34] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[58] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[82]; -rk_eta[11] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[11] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[35] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[59] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[83]; -rk_eta[12] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[12] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[36] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[60] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[84]; -rk_eta[13] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[13] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[37] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[61] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[85]; -rk_eta[14] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[14] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[38] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[62] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[86]; -rk_eta[15] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[15] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[39] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[63] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[87]; -rk_eta[16] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[16] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[40] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[64] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[88]; -rk_eta[17] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[17] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[41] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[65] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[89]; -rk_eta[18] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[18] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[42] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[66] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[90]; -rk_eta[19] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[19] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[43] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[67] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[91]; -rk_eta[20] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[20] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[44] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[68] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[92]; -rk_eta[21] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[21] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[45] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[69] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[93]; -rk_eta[22] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[22] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[46] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[70] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[94]; -rk_eta[23] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[23] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[47] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[71] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[95]; -acadoWorkspace.rk_ttt += 2.0000000000000001e-01; +rk_eta[0] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[0] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[24] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[48] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[72]; +rk_eta[1] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[1] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[25] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[49] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[73]; +rk_eta[2] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[2] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[26] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[50] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[74]; +rk_eta[3] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[3] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[27] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[51] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[75]; +rk_eta[4] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[4] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[28] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[52] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[76]; +rk_eta[5] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[5] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[29] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[53] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[77]; +rk_eta[6] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[6] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[30] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[54] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[78]; +rk_eta[7] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[7] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[31] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[55] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[79]; +rk_eta[8] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[8] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[32] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[56] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[80]; +rk_eta[9] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[9] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[33] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[57] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[81]; +rk_eta[10] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[10] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[34] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[58] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[82]; +rk_eta[11] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[11] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[35] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[59] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[83]; +rk_eta[12] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[12] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[36] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[60] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[84]; +rk_eta[13] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[13] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[37] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[61] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[85]; +rk_eta[14] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[14] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[38] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[62] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[86]; +rk_eta[15] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[15] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[39] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[63] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[87]; +rk_eta[16] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[16] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[40] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[64] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[88]; +rk_eta[17] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[17] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[41] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[65] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[89]; +rk_eta[18] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[18] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[42] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[66] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[90]; +rk_eta[19] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[19] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[43] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[67] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[91]; +rk_eta[20] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[20] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[44] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[68] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[92]; +rk_eta[21] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[21] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[45] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[69] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[93]; +rk_eta[22] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[22] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[46] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[70] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[94]; +rk_eta[23] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[23] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[47] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[71] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[95]; +acadoWorkspace.rk_ttt += 1.0000000000000000e+00; } error = 0; return error; 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 index d14523d05a..30bbd8ab82 100644 --- a/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_qpoases_interface.hpp +++ b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_qpoases_interface.hpp @@ -41,7 +41,7 @@ /** Maximum number of constraints. */ #define QPOASES_NCMAX 100 /** Maximum number of working set recalculations. */ -#define QPOASES_NWSRMAX 462 +#define QPOASES_NWSRMAX 500 /** Print level for qpOASES. */ #define QPOASES_PRINTLEVEL PL_NONE /** The value of EPS */ diff --git a/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_solver.c b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_solver.c index 963a36430a..da671227a7 100644 --- a/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_solver.c +++ b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_solver.c @@ -213,22 +213,22 @@ tmpQ2[0] = + tmpFx[0]; tmpQ2[1] = + tmpFx[4]; tmpQ2[2] = + tmpFx[8]; tmpQ2[3] = + tmpFx[12]; -tmpQ2[4] = + tmpFx[16]; +tmpQ2[4] = + tmpFx[16]*(real_t)5.0000000000000000e-01; tmpQ2[5] = + tmpFx[1]; tmpQ2[6] = + tmpFx[5]; tmpQ2[7] = + tmpFx[9]; tmpQ2[8] = + tmpFx[13]; -tmpQ2[9] = + tmpFx[17]; +tmpQ2[9] = + tmpFx[17]*(real_t)5.0000000000000000e-01; tmpQ2[10] = + tmpFx[2]; tmpQ2[11] = + tmpFx[6]; tmpQ2[12] = + tmpFx[10]; tmpQ2[13] = + tmpFx[14]; -tmpQ2[14] = + tmpFx[18]; +tmpQ2[14] = + tmpFx[18]*(real_t)5.0000000000000000e-01; tmpQ2[15] = + tmpFx[3]; tmpQ2[16] = + tmpFx[7]; tmpQ2[17] = + tmpFx[11]; tmpQ2[18] = + tmpFx[15]; -tmpQ2[19] = + tmpFx[19]; +tmpQ2[19] = + tmpFx[19]*(real_t)5.0000000000000000e-01; tmpQ1[0] = + tmpQ2[0]*tmpFx[0] + tmpQ2[1]*tmpFx[4] + tmpQ2[2]*tmpFx[8] + tmpQ2[3]*tmpFx[12] + tmpQ2[4]*tmpFx[16]; tmpQ1[1] = + tmpQ2[0]*tmpFx[1] + tmpQ2[1]*tmpFx[5] + tmpQ2[2]*tmpFx[9] + tmpQ2[3]*tmpFx[13] + tmpQ2[4]*tmpFx[17]; tmpQ1[2] = + tmpQ2[0]*tmpFx[2] + tmpQ2[1]*tmpFx[6] + tmpQ2[2]*tmpFx[10] + tmpQ2[3]*tmpFx[14] + tmpQ2[4]*tmpFx[18]; @@ -253,7 +253,7 @@ tmpR2[0] = + tmpFu[0]; tmpR2[1] = + tmpFu[1]; tmpR2[2] = + tmpFu[2]; tmpR2[3] = + tmpFu[3]; -tmpR2[4] = + tmpFu[4]; +tmpR2[4] = + tmpFu[4]*(real_t)5.0000000000000000e-01; tmpR1[0] = + tmpR2[0]*tmpFu[0] + tmpR2[1]*tmpFu[1] + tmpR2[2]*tmpFu[2] + tmpR2[3]*tmpFu[3] + tmpR2[4]*tmpFu[4]; } @@ -1965,7 +1965,7 @@ tmpDy[0] = + acadoWorkspace.Dy[lRun1 * 5]; tmpDy[1] = + acadoWorkspace.Dy[lRun1 * 5 + 1]; tmpDy[2] = + acadoWorkspace.Dy[lRun1 * 5 + 2]; tmpDy[3] = + acadoWorkspace.Dy[lRun1 * 5 + 3]; -tmpDy[4] = + acadoWorkspace.Dy[lRun1 * 5 + 4]; +tmpDy[4] = + acadoWorkspace.Dy[lRun1 * 5 + 4]*(real_t)5.0000000000000000e-01; objVal += + acadoWorkspace.Dy[lRun1 * 5]*tmpDy[0] + acadoWorkspace.Dy[lRun1 * 5 + 1]*tmpDy[1] + acadoWorkspace.Dy[lRun1 * 5 + 2]*tmpDy[2] + acadoWorkspace.Dy[lRun1 * 5 + 3]*tmpDy[3] + acadoWorkspace.Dy[lRun1 * 5 + 4]*tmpDy[4]; } diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 5b65fef7de..536bd0c693 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -11,11 +11,11 @@ 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 - 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 + # long_control_state labels (see capnp enum): + off = 'off' # Off + pid = 'pid' # moving and tracking targets, with PID control running + stopping = 'stopping' # stopping and changing controls to almost open loop as PID does not fit well at such a low speed + starting = 'starting' # starting and releasing brakes in open loop before giving back to PID def long_control_state_trans(active, long_control_state, v_ego, v_target, v_pid, @@ -57,7 +57,7 @@ _KI_BP = [0., 35.] _KI_V = [0.18, 0.12] 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_brake_rate = 0.8 # 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 @@ -80,7 +80,7 @@ class LongControl(object): self.pid.reset() self.v_pid = v_pid - def update(self, active, v_ego, brake_pressed, v_cruise, v_target_lead, a_target, + def update(self, active, v_ego, brake_pressed, standstill, v_cruise, v_target_lead, a_target, jerk_factor, CP): # actuation limits @@ -100,9 +100,11 @@ class LongControl(object): self.long_control_state = long_control_state_trans(active, self.long_control_state, v_ego,\ v_target, self.v_pid, output_gb, brake_pressed) + v_ego_pid = max(v_ego, 0.3) # Without this we get jumps, CAN bus reports 0 when speed < 0.3 + # *** long control behavior based on state if self.long_control_state == LongCtrlState.off: - self.v_pid = v_ego # do nothing + self.v_pid = v_ego_pid # do nothing output_gb = 0. self.pid.reset() @@ -129,13 +131,12 @@ class LongControl(object): 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: + if not standstill or output_gb > -brake_stopping_target: output_gb -= stopping_brake_rate / rate output_gb = clip(output_gb, -brake_max, gas_max) diff --git a/selfdrive/controls/lib/pid.py b/selfdrive/controls/lib/pid.py index faf5355db5..8b53e46a85 100644 --- a/selfdrive/controls/lib/pid.py +++ b/selfdrive/controls/lib/pid.py @@ -3,9 +3,10 @@ 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 + def __init__(self, k_p, k_i, k_f=0., pos_limit=None, neg_limit=None, rate=100, sat_limit=0.8, convert=None): + self._k_p = k_p # proportional gain + self._k_i = k_i # integrale gain + self.k_f = k_f # feedforward gain self.pos_limit = pos_limit self.neg_limit = neg_limit @@ -56,18 +57,19 @@ class PIController(object): self.saturated = False self.control = 0 - def update(self, setpoint, measurement, speed=0.0, check_saturation=True, jerk_factor=0.0, override=False): + def update(self, setpoint, measurement, speed=0.0, check_saturation=True, jerk_factor=0.0, override=False, feedforward=0.): self.speed = speed self.jerk_factor = jerk_factor error = float(setpoint - measurement) self.p = error * self.k_p + f = feedforward * self.k_f 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 + control = self.p + f + i if self.convert is not None: control = self.convert(control, speed=self.speed) @@ -78,7 +80,7 @@ class PIController(object): (error <= 0 and (control >= self.neg_limit or i > 0.0)): self.i = i - control = self.p + self.i + control = self.p + f + self.i if self.convert is not None: control = self.convert(control, speed=self.speed) diff --git a/selfdrive/controls/lib/radar_helpers.py b/selfdrive/controls/lib/radar_helpers.py index 370534c40e..15cc871e8c 100644 --- a/selfdrive/controls/lib/radar_helpers.py +++ b/selfdrive/controls/lib/radar_helpers.py @@ -95,13 +95,15 @@ class Track(object): # rel speed is very hard to estimate from vision if dist_to_vision < 4.0 and rel_speed_diff < 10.: # vision point is never stationary - self.stationary = False - self.vision = True self.vision_cnt += 1 + # don't trust 1 or 2 fusions until model quality is much better + if self.vision_cnt >= 3: + self.vision = True + self.stationary = False def get_key_for_cluster(self): # Weigh y higher since radar is inaccurate in this dimension - return [self.dRel, self.dPath*2, self.vRel] + return [self.dRel, self.yRel*2, self.vRel] # ******************* Cluster ******************* @@ -208,7 +210,7 @@ class Cluster(object): lead.fcw = self.is_potential_fcw() def __str__(self): - ret = "x: %7.2f y: %7.2f v: %7.2f a: %7.2f" % (self.dRel, self.yRel, self.vRel, self.aRel) + ret = "x: %4.1f y: %4.1f v: %4.1f a: %4.1f d: %4.2f" % (self.dRel, self.yRel, self.vRel, self.aRel, self.dPath) if self.stationary: ret += " stationary" if self.vision: diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 353b7a73a5..ddfb7766de 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -18,6 +18,7 @@ from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper from common.kalman.ekf import EKF, SimpleSensor VISION_ONLY = False +DEBUG = False #vision point DIMSV = 2 @@ -76,7 +77,6 @@ def radard_thread(gctx=None): # Time-alignment rate = 20. # model and radar are both at 20Hz tsv = 1./rate - rdr_delay = 0.10 # radar data delay in s v_len = 20 # how many speed data points to remember for t alignment with rdr data enabled = 0 @@ -158,7 +158,7 @@ def radard_thread(gctx=None): # 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]) + v_ego_t_aligned = np.interp(cur_time - RI.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)) # create the track if it doesn't exist or it's a new track @@ -175,10 +175,17 @@ def radard_thread(gctx=None): # publish tracks (debugging) dat = messaging.new_message() dat.init('liveTracks', len(tracks)) - #print "NEW TRACKS" + + if DEBUG: + print "NEW CYCLE" + if VISION_POINT in ar_pts: + print "vision", ar_pts[VISION_POINT] + for cnt, ids in enumerate(tracks.keys()): - #print "%5s %5s %5s %5s" % \ - # (ids, round(tracks[ids].dRel, 2), round(tracks[ids].vRel, 2), round(tracks[ids].yRel, 2)) + if DEBUG: + print "id: %4.0f x: %4.1f y: %4.1f v: %4.1f d: %4.1f s: %1.0f" % \ + (ids, tracks[ids].dRel, tracks[ids].yRel, tracks[ids].vRel, + tracks[ids].dPath, tracks[ids].stationary) dat.liveTracks[cnt].trackId = ids dat.liveTracks[cnt].dRel = float(tracks[ids].dRel) dat.liveTracks[cnt].yRel = float(tracks[ids].yRel) @@ -210,6 +217,9 @@ def radard_thread(gctx=None): else: clusters = [] + if DEBUG: + for i in clusters: + print i # *** extract the lead car *** lead_clusters = [c for c in clusters if c.is_potential_lead(v_ego)] diff --git a/selfdrive/debug/dump.py b/selfdrive/debug/dump.py index 2d31240835..a58ac3a515 100755 --- a/selfdrive/debug/dump.py +++ b/selfdrive/debug/dump.py @@ -2,6 +2,7 @@ import sys import argparse import zmq +import json from hexdump import hexdump import selfdrive.messaging as messaging @@ -13,11 +14,19 @@ if __name__ == "__main__": parser = argparse.ArgumentParser(description='Sniff a communcation socket') parser.add_argument('--raw', action='store_true') + parser.add_argument('--json', action='store_true') + parser.add_argument('--addr', default='127.0.0.1') parser.add_argument("socket", type=str, nargs='*', help="socket name") args = parser.parse_args() for m in args.socket if len(args.socket) > 0 else service_list: - messaging.sub_sock(context, service_list[m].port, poller) + if m in service_list: + messaging.sub_sock(context, service_list[m].port, poller, addr=args.addr) + elif m.isdigit(): + messaging.sub_sock(context, int(m), poller, addr=args.addr) + else: + print("service not found") + exit(-1) while 1: polld = poller.poll(timeout=1000) @@ -26,6 +35,8 @@ if __name__ == "__main__": continue if args.raw: hexdump(sock.recv()) + elif args.json: + print(json.loads(sock.recv())) else: print messaging.recv_one(sock) diff --git a/selfdrive/logcatd/Makefile b/selfdrive/logcatd/Makefile index 83684abe5b..077f20f8cd 100644 --- a/selfdrive/logcatd/Makefile +++ b/selfdrive/logcatd/Makefile @@ -39,7 +39,7 @@ logcatd: $(OBJS) @echo "[ CXX ] $@" $(CXX) $(CXXFLAGS) \ -I$(PHONELIBS)/android_system_core/include \ - $(CEREAL_CFLAGS) \ + $(CEREAL_CXXFLAGS) \ $(ZMQ_FLAGS) \ -I../ \ -I../../ \ diff --git a/selfdrive/loggerd/loggerd b/selfdrive/loggerd/loggerd index 579b406284..e3811edeca 100755 Binary files a/selfdrive/loggerd/loggerd and b/selfdrive/loggerd/loggerd differ diff --git a/selfdrive/manager.py b/selfdrive/manager.py index 77b9395c38..e1b785ed52 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -1,36 +1,67 @@ #!/usr/bin/env python import os +import sys import time +import fcntl +import errno +import signal -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) +if __name__ == "__main__": + if os.path.isfile("/init.qcom.rc"): + # check if NEOS update is required + while ((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://neos.comma.ai/updater && chmod +x /tmp/updater && /tmp/updater") + time.sleep(10) + + # get a non-blocking stdout + child_pid, child_pty = os.forkpty() + if child_pid != 0: # parent + + # child is in its own process group, manually pass kill signals + signal.signal(signal.SIGINT, lambda signum, frame: os.kill(child_pid, signal.SIGINT)) + signal.signal(signal.SIGTERM, lambda signum, frame: os.kill(child_pid, signal.SIGTERM)) + + fcntl.fcntl(sys.stdout, fcntl.F_SETFL, + fcntl.fcntl(sys.stdout, fcntl.F_GETFL) | os.O_NONBLOCK) + + while True: + try: + dat = os.read(child_pty, 4096) + except OSError as e: + if e.errno == errno.EIO: + break + continue + + if not dat: + break + + try: + sys.stdout.write(dat) + except (OSError, IOError): + pass + + os._exit(os.wait()[1]) -import sys +import hashlib import importlib import subprocess -import signal import traceback from multiprocessing import Process -from common.basedir import BASEDIR +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 common.params import Params +from common.realtime import sec_since_boot +from selfdrive.services import service_list from selfdrive.swaglog import cloudlog import selfdrive.messaging as messaging from selfdrive.thermal import read_thermal @@ -38,8 +69,6 @@ from selfdrive.registration import register from selfdrive.version import version import selfdrive.crash as crash -from common.params import Params - from selfdrive.loggerd.config import ROOT # comment out anything you don't want to run @@ -52,10 +81,14 @@ managed_processes = { "tombstoned": "selfdrive.tombstoned", "logcatd": ("selfdrive/logcatd", ["./logcatd"]), "proclogd": ("selfdrive/proclogd", ["./proclogd"]), - "boardd": ("selfdrive/boardd", ["./boardd"]), # switch to c++ boardd + "boardd": ("selfdrive/boardd", ["./boardd"]), # not used directly + "pandad": "selfdrive.pandad", "ui": ("selfdrive/ui", ["./ui"]), "visiond": ("selfdrive/visiond", ["./visiond"]), - "sensord": ("selfdrive/sensord", ["./sensord"]), } + "sensord": ("selfdrive/sensord", ["./sensord"]), + "gpsd": ("selfdrive/sensord", ["./gpsd"]), + "updated": "selfdrive.updated", +} running = {} def get_running(): @@ -67,6 +100,16 @@ unkillable_processes = ['visiond'] # processes to end with SIGINT instead of SIGTERM interrupt_processes = [] +persistent_processes = [ + 'logmessaged', + 'logcatd', + 'tombstoned', + 'uploader', + 'ui', + 'gpsd', + 'updated', +] + car_started_processes = [ 'controlsd', 'loggerd', @@ -77,11 +120,13 @@ car_started_processes = [ ] def register_managed_process(name, desc, car_started=False): - global managed_processes, car_started_processes + global managed_processes, car_started_processes, persistent_processes print "registering", name managed_processes[name] = desc if car_started: car_started_processes.append(name) + else: + persistent_processes.append(name) # ****************** process management functions ****************** def launcher(proc, gctx): @@ -95,7 +140,7 @@ def launcher(proc, gctx): # exec the process mod.main(gctx) except KeyboardInterrupt: - cloudlog.info("child %s got ctrl-c" % proc) + cloudlog.warning("child %s got SIGINT" % proc) except Exception: # can't install the crash handler becuase sys.excepthook doesn't play nice # with threads, so catch it here. @@ -138,7 +183,7 @@ def prepare_managed_process(p): 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, )) + cloudlog.warning("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])) @@ -161,7 +206,7 @@ def kill_managed_process(name): running[name].join(15.0) if running[name].exitcode is None: cloudlog.critical("FORCE REBOOTING PHONE!") - os.system("date > /sdcard/unkillable_reboot") + os.system("date >> /sdcard/unkillable_reboot") os.system("reboot") raise RuntimeError else: @@ -191,6 +236,7 @@ def manage_baseui(start): os.system("am force-stop com.baseui") baseui_running = False + # ****************** run loop ****************** def manager_init(): @@ -292,6 +338,36 @@ def handle_fan(max_temp, fan_speed): return fan_speed +class LocationStarter(object): + def __init__(self): + self.last_good_loc = 0 + def update(self, started_ts, location): + rt = sec_since_boot() + + if location is None or location.accuracy > 50 or location.speed < 2: + # bad location, stop if we havent gotten a location in a while + # dont stop if we're been going for less than a minute + if started_ts: + if rt-self.last_good_loc > 60. and rt-started_ts > 60: + cloudlog.event("location_stop", + ts=rt, + started_ts=started_ts, + last_good_loc=self.last_good_loc, + location=location.to_dict() if location else None) + return False + else: + return True + else: + return False + + self.last_good_loc = rt + + if started_ts: + return True + else: + cloudlog.event("location_start", location=location.to_dict() if location else None) + return location.speed*3.6 > 10 + def manager_thread(): global baseui_running @@ -299,37 +375,40 @@ def manager_thread(): context = zmq.Context() thermal_sock = messaging.pub_sock(context, service_list['thermal'].port) health_sock = messaging.sub_sock(context, service_list['health'].port) + location_sock = messaging.sub_sock(context, service_list['gpsLocation'].port) cloudlog.info("manager start") - cloudlog.info(dict(os.environ)) + cloudlog.info({"environ": os.environ}) + + for p in persistent_processes: + start_managed_process(p) - start_managed_process("logmessaged") - start_managed_process("logcatd") - start_managed_process("tombstoned") - start_managed_process("uploader") - start_managed_process("ui") manage_baseui(True) # do this before panda flashing setup_eon_fan() if os.getenv("NOBOARD") is None: - from panda import ensure_st_up_to_date - ensure_st_up_to_date() - start_managed_process("boardd") + start_managed_process("pandad") + + passive = bool(os.getenv("PASSIVE")) + passive_starter = LocationStarter() - started = False + started_ts = None logger_dead = False count = 0 fan_speed = 0 + ignition_seen = False - # set 5 second timeout on health socket - # 5x slower than expected - health_sock.RCVTIMEO = 5000 + health_sock.RCVTIMEO = 1500 while 1: # get health of board, log this in "thermal" td = messaging.recv_sock(health_sock, wait=True) + location = messaging.recv_sock(location_sock) + + location = location.gpsLocation if location else None + print td # replace thermald @@ -357,7 +436,7 @@ def manager_thread(): # uploader is gated based on the phone temperature if max_temp > 85.0: - cloudlog.info("over temp: %r", max_temp) + cloudlog.warning("over temp: %r", max_temp) kill_managed_process("uploader") elif max_temp < 70.0: start_managed_process("uploader") @@ -366,11 +445,23 @@ def manager_thread(): logger_dead = True # start constellation of processes when the car starts + ignition = td is not None and td.health.started + ignition_seen = ignition_seen or ignition + + should_start = ignition + + # start on gps in passive mode + if passive and not ignition_seen: + should_start = should_start or passive_starter.update(started_ts, location) + # with 2% left, we killall, otherwise the phone is bricked - if td is not None and td.health.started and avail > 0.02: - if not started: + should_start = should_start and avail > 0.02 + + + if should_start: + if not started_ts: Params().car_start() - started = True + started_ts = sec_since_boot() for p in car_started_processes: if p == "loggerd" and logger_dead: kill_managed_process(p) @@ -379,7 +470,7 @@ def manager_thread(): manage_baseui(False) else: manage_baseui(True) - started = False + started_ts = None logger_dead = False for p in car_started_processes: kill_managed_process(p) @@ -416,9 +507,10 @@ def get_installed_apks(): # optional, build the c++ binaries and preimport the python for speed def manager_prepare(): - # update submodules - system("cd %s && git submodule init panda opendbc pyextra" % BASEDIR) - system("cd %s && git submodule update panda opendbc pyextra" % BASEDIR) + if os.path.exists(os.path.join(BASEDIR, ".gitmodules")): + # update submodules + system("cd %s && git submodule init panda opendbc pyextra" % BASEDIR) + system("cd %s && git submodule update panda opendbc pyextra" % BASEDIR) # build cereal first subprocess.check_call(["make", "-j4"], cwd=os.path.join(BASEDIR, "cereal")) @@ -455,24 +547,6 @@ def manager_prepare(): break assert ret == 0 -def wait_for_device(): - while 1: - try: - context = usb1.USBContext() - for device in context.getDeviceList(skip_on_error=True): - if (device.getVendorID() == 0xbbaa and device.getProductID() == 0xddcc) or \ - (device.getVendorID() == 0x0483 and device.getProductID() == 0xdf11): - bcd = device.getbcdDevice() - handle = device.open() - handle.claimInterface(0) - cloudlog.info("found board") - handle.close() - return bcd - except Exception as e: - print "exception", e, - print "waiting..." - time.sleep(1) - def main(): if os.getenv("NOLOG") is not None: del managed_processes['loggerd'] @@ -481,8 +555,6 @@ def main(): del managed_processes['uploader'] if os.getenv("NOVISION") is not None: del managed_processes['visiond'] - if os.getenv("NOBOARD") is not None: - del managed_processes['boardd'] if os.getenv("LEAN") is not None: del managed_processes['uploader'] del managed_processes['loggerd'] @@ -510,11 +582,13 @@ def main(): if params.get("IsRearViewMirror") is None: params.put("IsRearViewMirror", "1") + params.put("Passive", "1" if os.getenv("PASSIVE") else "0") + # put something on screen while we set things up if os.getenv("PREPAREONLY") is not None: spinner_proc = None else: - spinner_proc = subprocess.Popen(["./spinner", "loading openpilot..."], + spinner_proc = subprocess.Popen(["./spinner", "loading..."], cwd=os.path.join(BASEDIR, "selfdrive", "ui", "spinner"), close_fds=True) try: @@ -525,7 +599,7 @@ def main(): spinner_proc.terminate() if os.getenv("PREPAREONLY") is not None: - sys.exit(0) + return try: manager_thread() @@ -537,3 +611,5 @@ def main(): if __name__ == "__main__": main() + # manual exit because we are forked + sys.exit(0) diff --git a/selfdrive/pandad.py b/selfdrive/pandad.py new file mode 100644 index 0000000000..7f05a078c9 --- /dev/null +++ b/selfdrive/pandad.py @@ -0,0 +1,13 @@ +#!/usr/bin/env python +# simple boardd wrapper that updates the panda first +import os +from panda import ensure_st_up_to_date + +def main(gctx=None): + ensure_st_up_to_date() + + os.chdir("boardd") + os.execvp("./boardd", ["./boardd"]) + +if __name__ == "__main__": + main() diff --git a/selfdrive/proclogd/Makefile b/selfdrive/proclogd/Makefile index e7a6a57f1d..a7cd3682b2 100644 --- a/selfdrive/proclogd/Makefile +++ b/selfdrive/proclogd/Makefile @@ -38,7 +38,7 @@ proclogd: $(OBJS) %.o: %.cc @echo "[ CXX ] $@" $(CXX) $(CXXFLAGS) \ - $(CEREAL_CFLAGS) \ + $(CEREAL_CXXFLAGS) \ $(ZMQ_FLAGS) \ -I../ \ -I../../ \ diff --git a/selfdrive/radar/nidec/interface.py b/selfdrive/radar/nidec/interface.py index dc8b5d073e..00b02c2fc3 100755 --- a/selfdrive/radar/nidec/interface.py +++ b/selfdrive/radar/nidec/interface.py @@ -4,8 +4,6 @@ import numpy as np from selfdrive.can.parser import CANParser -from selfdrive.boardd.boardd import can_capnp_to_can_list - from cereal import car from common.realtime import sec_since_boot @@ -13,10 +11,11 @@ import zmq from selfdrive.services import service_list import selfdrive.messaging as messaging + def _create_nidec_can_parser(): dbc_f = 'acura_ilx_2016_nidec.dbc' radar_messages = [0x400] + range(0x430, 0x43A) + range(0x440, 0x446) - signals = zip(['RADAR_STATE'] + + signals = zip(['RADAR_STATE'] + ['LONG_DIST'] * 16 + ['NEW_TRACK'] * 16 + ['LAT_DIST'] * 16 + ['REL_SPEED'] * 16, [0x400] + radar_messages[1:] * 4, @@ -25,6 +24,7 @@ def _create_nidec_can_parser(): return CANParser(os.path.splitext(dbc_f)[0], signals, checks, 1) + class RadarInterface(object): def __init__(self): # radar @@ -32,6 +32,8 @@ class RadarInterface(object): self.track_id = 0 self.radar_fault = False + self.delay = 0.1 # Delay of radar + # Nidec self.rcp = _create_nidec_can_parser() @@ -79,11 +81,10 @@ class RadarInterface(object): ret.points = self.pts.values() return ret + if __name__ == "__main__": RI = RadarInterface() while 1: ret = RI.update() print(chr(27) + "[2J") print ret - - diff --git a/selfdrive/radar/toyota/__init__.py b/selfdrive/radar/toyota/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/radar/toyota/interface.py b/selfdrive/radar/toyota/interface.py new file mode 100755 index 0000000000..a16a972f97 --- /dev/null +++ b/selfdrive/radar/toyota/interface.py @@ -0,0 +1,81 @@ +#!/usr/bin/env python +import os +import numpy as np +from selfdrive.can.parser import CANParser +from cereal import car +from common.realtime import sec_since_boot +import zmq +from selfdrive.services import service_list +import selfdrive.messaging as messaging + +def _create_radard_can_parser(): + dbc_f = 'toyota_prius_2017_adas.dbc' + radar_messages = range(0x210, 0x220) + msg_n = len(radar_messages) + msg_last = radar_messages[-1] + signals = zip(['LONG_DIST'] * msg_n + ['NEW_TRACK'] * msg_n + ['LAT_DIST'] * msg_n + + ['REL_SPEED'] * msg_n + ['VALID'] * msg_n, + radar_messages * 5, + [255] * msg_n + [1] * msg_n + [0] * msg_n + [0] * msg_n + [0] * msg_n) + checks = zip(radar_messages, [20]*msg_n) + + 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.delay = 0.0 # Delay of radar + + # Nidec + self.rcp = _create_radard_can_parser() + + context = zmq.Context() + self.logcan = messaging.sub_sock(context, service_list['can'].port) + + def update(self): + canMonoTimes = [] + + updated_messages = set() + while 1: + tm = int(sec_since_boot() * 1e9) + updated_messages.update(self.rcp.update(tm, True)) + # TODO: use msg_last + if 0x21f in updated_messages: + break + + ret = car.RadarState.new_message() + errors = [] + if not self.rcp.can_valid: + errors.append("commIssue") + ret.errors = errors + ret.canMonoTimes = canMonoTimes + #print "NEW TRACKS" + for ii in updated_messages: + cpt = self.rcp.vl[ii] + if cpt['LONG_DIST'] < 255 and cpt['VALID']: + #print "%5s %5s %5s" % (round(cpt['LONG_DIST'], 1), round(cpt['LAT_DIST'], 1), round(cpt['REL_SPEED'], 1)) + 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 + self.track_id += 1 + self.pts[ii].dRel = cpt['LONG_DIST'] # from front of car + self.pts[ii].yRel = -cpt['LAT_DIST'] # in car frame's y axis, left is positive + self.pts[ii].vRel = cpt['REL_SPEED'] + self.pts[ii].aRel = float('nan') + self.pts[ii].yvRel = float('nan') + else: + if ii in self.pts: + del self.pts[ii] + + ret.points = self.pts.values() + return ret + +if __name__ == "__main__": + RI = RadarInterface() + while 1: + ret = RI.update() + print(chr(27) + "[2J") + print ret diff --git a/selfdrive/sensord/gpsd b/selfdrive/sensord/gpsd new file mode 100755 index 0000000000..8679e4771a Binary files /dev/null and b/selfdrive/sensord/gpsd differ diff --git a/selfdrive/sensord/sensord b/selfdrive/sensord/sensord index fc78d8ff80..ca6ee6c476 100755 Binary files a/selfdrive/sensord/sensord and b/selfdrive/sensord/sensord differ diff --git a/selfdrive/test/plant/plant.py b/selfdrive/test/plant/plant.py index 26dcc5b229..5b52db24cb 100755 --- a/selfdrive/test/plant/plant.py +++ b/selfdrive/test/plant/plant.py @@ -21,10 +21,10 @@ from selfdrive.car.honda.interface import CarInterface from cereal import car from common.dbc import dbc -acura = dbc(os.path.join(DBC_PATH, "acura_ilx_2016_can.dbc")) +honda = dbc(os.path.join(DBC_PATH, "honda_civic_touring_2016_can.dbc")) # Trick: set 0x201 (interceptor) in fingerprints for gas is controlled like if there was an interceptor -CP = CarInterface.get_params("ACURA ILX 2016 ACURAWATCH PLUS", {0x201}) +CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING", {0x201}) def car_plant(pos, speed, grade, gas, brake): @@ -41,7 +41,7 @@ def car_plant(pos, speed, grade, gas, brake): frontal_area = 2.2 air_density = 1.225 gas_to_peak_linear_slope = 3.33 - brake_to_peak_linear_slope = 0.2 + brake_to_peak_linear_slope = 0.3 creep_accel_v = [1., 0.] creep_accel_bp = [0., 1.5] @@ -66,7 +66,7 @@ def car_plant(pos, speed, grade, gas, brake): return speed, acceleration def get_car_can_parser(): - dbc_f = 'acura_ilx_2016_can.dbc' + dbc_f = 'honda_civic_touring_2016_can.dbc' signals = [ ("STEER_TORQUE", 0xe4, 0), ("STEER_TORQUE_REQUEST", 0xe4, 0), @@ -92,7 +92,7 @@ class Plant(object): def __init__(self, lead_relevancy=False, rate=100, speed=0.0, distance_lead=2.0): self.rate = rate - self.civic = False + self.civic = True self.brake_only = False if not Plant.messaging_initialized: @@ -113,10 +113,11 @@ class Plant(object): self.user_gas = 0 self.computer_brake,self.user_brake = 0,0 self.brake_pressed = 0 + self.angle_steer_rate = 0 self.distance, self.distance_prev = 0., 0. self.speed, self.speed_prev = speed, speed self.steer_error, self.brake_error, self.steer_not_allowed = 0, 0, 0 - self.gear_shifter = 4 # D gear + self.gear_shifter = 8 # D gear self.pedal_gas = 0 self.cruise_setting = 0 @@ -211,7 +212,7 @@ class Plant(object): # ******** publish the car ******** vls = [self.speed_sensor(speed), self.speed_sensor(speed), self.speed_sensor(speed), self.speed_sensor(speed), self.speed_sensor(speed), - self.angle_steer, 0, self.gear_choice, speed!=0, + self.angle_steer, self.angle_steer_rate, 0, self.gear_choice, speed!=0, 0, 0, 0, 0, self.v_cruise, not self.seatbelt, self.seatbelt, self.brake_pressed, 0., self.user_gas, cruise_buttons, self.esp_disabled, 0, @@ -219,7 +220,7 @@ class Plant(object): self.brake_error, self.gear_shifter, self.main_on, self.acc_status, self.pedal_gas, self.cruise_setting, # append one more zero for gas interceptor - 0,0,0,0] + 0,0,0,0,0,0] # TODO: publish each message at proper frequency can_msgs = [] @@ -228,25 +229,27 @@ class Plant(object): indxs = [i for i, x in enumerate(msgs) if msg == msgs[i]] for i in indxs: msg_struct[sgs[i]] = vls[i] - if msg in cks_msgs: + + if "COUNTER" in honda.get_signals(msg): msg_struct["COUNTER"] = self.rk.frame % 4 - msg_data = acura.encode(msg, msg_struct) + msg_data = honda.encode(msg, msg_struct) - if msg in cks_msgs: + if "CHECKSUM" in honda.get_signals(msg): msg_data = fix(msg_data, msg) can_msgs.append([msg, 0, msg_data, 0]) # add the radar message # TODO: use the DBC - 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]) + if self.rk.frame % 5 == 0: + 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()) # ******** publish a fake model going straight and fake calibration ******** diff --git a/selfdrive/test/tests/plant/test_longitudinal.py b/selfdrive/test/tests/plant/test_longitudinal.py index 3c8286b775..886b767b95 100755 --- a/selfdrive/test/tests/plant/test_longitudinal.py +++ b/selfdrive/test/tests/plant/test_longitudinal.py @@ -6,13 +6,14 @@ 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 +from common.params import Params + def create_dir(path): try: @@ -79,7 +80,7 @@ maneuvers = [ 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_values = [20., 20., 0.], speed_lead_breakpoints = [0., 15., 35.0], cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)] ), @@ -89,7 +90,7 @@ maneuvers = [ 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_values = [20., 20., 0.], speed_lead_breakpoints = [0., 15., 25.0], cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)] ), @@ -220,6 +221,8 @@ class LongitudinalControl(unittest.TestCase): setup_output() shutil.rmtree('/data/params', ignore_errors=True) + params = Params() + params.put("Passive", "1" if os.getenv("PASSIVE") else "0") manager.gctx = {} manager.prepare_managed_process('radard') diff --git a/selfdrive/tombstoned.py b/selfdrive/tombstoned.py index 65d925e0d5..430b31d881 100644 --- a/selfdrive/tombstoned.py +++ b/selfdrive/tombstoned.py @@ -11,7 +11,8 @@ from selfdrive.version import version from selfdrive.swaglog import cloudlog def get_tombstones(): - return [fn for fn in os.listdir("/data/tombstones") if fn.startswith("tombstone")] + return [("/data/tombstones/"+fn, int(os.stat("/data/tombstones/"+fn).st_ctime) ) + for fn in os.listdir("/data/tombstones") if fn.startswith("tombstone")] def report_tombstone(fn, client): mtime = os.path.getmtime(fn) @@ -73,8 +74,7 @@ def main(gctx): while True: now_tombstones = set(get_tombstones()) - for ts in (now_tombstones - initial_tombstones): - fn = "/data/tombstones/"+ts + for fn, ctime in (now_tombstones - initial_tombstones): cloudlog.info("reporting new tombstone %s", fn) report_tombstone(fn, client) diff --git a/selfdrive/ui/ui.c b/selfdrive/ui/ui.c index 3e1d5ea12f..d93311118a 100644 --- a/selfdrive/ui/ui.c +++ b/selfdrive/ui/ui.c @@ -136,6 +136,7 @@ typedef struct UIState { int awake_timeout; bool is_metric; + bool passive; } UIState; static void set_awake(UIState *s, bool awake) { @@ -281,6 +282,15 @@ static void ui_init(UIState *s) { glDisable(GL_DEPTH_TEST); assert(glGetError() == GL_NO_ERROR); + + { + char *value; + const int result = read_db_value(NULL, "Passive", &value, NULL); + if (result == 0) { + s->passive = value[0] == '1'; + free(value); + } + } } @@ -701,25 +711,9 @@ static void ui_draw_world(UIState *s) { return; } - /****************************************** - * Add background rect so it's easier to see in - * light background scenes - ******************************************/ - // Draw background around speed text - - // Left side - ui_draw_rounded_rect(s->vg, -15, 0, 570, 180, 20, nvgRGBA(10,10,10,170)); - - // Right side - ui_draw_rounded_rect(s->vg, 1920-530, 0, 150, 180, 20, nvgRGBA(10,10,10,170)); - /******************************************/ - draw_steering(s, scene->v_ego, scene->angle_steers); - // draw paths if ((nanos_since_boot() - scene->model_ts) < 1000000000ULL) { - draw_path(s, scene->model.path.points, 0.0f, nvgRGBA(128, 0, 255, 255)); - draw_model_path( s, scene->model.left_lane, nvgRGBA(0, (int)(255 * scene->model.left_lane.prob), 0, 128)); @@ -727,19 +721,19 @@ static void ui_draw_world(UIState *s) { 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)); + // draw paths + if (!s->passive) { + draw_path(s, scene->model.path.points, 0.0f, nvgRGBA(128, 0, 255, 255)); + + draw_x_y(s, scene->mpc_x, scene->mpc_y, 50, nvgRGBA(255, 0, 0, 255)); + } } if (scene->lead_status) { char radar_str[16]; - /****************************************** - * Add background rect so it's easier to see in - * light background scenes - ******************************************/ // Draw background for radar text ui_draw_rounded_rect(s->vg, 578, 0, 195, 180, 20, nvgRGBA(10,10,10,170)); - /******************************************/ if (s->is_metric) { int lead_v_rel = (int)(3.6 * scene->lead_v_rel); @@ -790,38 +784,48 @@ static void ui_draw_vision(UIState *s) { float defaultfontsize = 128.0f; float labelfontsize = 65.0f; - if (scene->engaged) { - nvgFillColor(s->vg, nvgRGBA(255, 128, 0, 192)); + if (!s->passive) { - // Add label - nvgFontSize(s->vg, labelfontsize); - nvgTextAlign(s->vg, NVG_ALIGN_LEFT | NVG_ALIGN_BASELINE); - nvgText(s->vg, 20, 175-30, "OpenPilot: On", NULL); - } else { - nvgFillColor(s->vg, nvgRGBA(195, 195, 195, 192)); + // background + ui_draw_rounded_rect(s->vg, -15, 0, 570, 180, 20, nvgRGBA(10,10,10,170)); - // Add label - nvgFontSize(s->vg, labelfontsize); - nvgTextAlign(s->vg, NVG_ALIGN_LEFT | NVG_ALIGN_BASELINE); - nvgText(s->vg, 20, 175-30, "OpenPilot: Off", NULL); - } + if (scene->engaged) { + nvgFillColor(s->vg, nvgRGBA(255, 128, 0, 192)); - nvgFontSize(s->vg, defaultfontsize); - if (scene->v_cruise != 255 && scene->v_cruise != 0) { - if (s->is_metric) { - snprintf(speed_str, sizeof(speed_str), "%3d KPH", - (int)(scene->v_cruise + 0.5)); + // Add label + nvgFontSize(s->vg, labelfontsize); + nvgTextAlign(s->vg, NVG_ALIGN_LEFT | NVG_ALIGN_BASELINE); + nvgText(s->vg, 20, 175-30, "OpenPilot: On", NULL); } else { - /* Convert KPH to MPH. Using an approximated mph to kph - conversion factor of 1.609 because this is what the Honda - hud seems to be using */ - snprintf(speed_str, sizeof(speed_str), "%3d MPH", - (int)(scene->v_cruise * 0.621504 + 0.5)); + nvgFillColor(s->vg, nvgRGBA(195, 195, 195, 192)); + + // Add label + nvgFontSize(s->vg, labelfontsize); + nvgTextAlign(s->vg, NVG_ALIGN_LEFT | NVG_ALIGN_BASELINE); + nvgText(s->vg, 20, 175-30, "OpenPilot: Off", NULL); + } + + nvgFontSize(s->vg, defaultfontsize); + if (scene->v_cruise != 255 && scene->v_cruise != 0) { + if (s->is_metric) { + snprintf(speed_str, sizeof(speed_str), "%3d KPH", + (int)(scene->v_cruise + 0.5)); + } else { + /* Convert KPH to MPH. Using an approximated mph to kph + conversion factor of 1.609 because this is what the Honda + hud seems to be using */ + snprintf(speed_str, sizeof(speed_str), "%3d MPH", + (int)(scene->v_cruise * 0.621504 + 0.5)); + } + nvgTextAlign(s->vg, NVG_ALIGN_RIGHT | NVG_ALIGN_BASELINE); + nvgText(s->vg, 480, 95, speed_str, NULL); } - nvgTextAlign(s->vg, NVG_ALIGN_RIGHT | NVG_ALIGN_BASELINE); - nvgText(s->vg, 480, 95, speed_str, NULL); } + + // speed background + ui_draw_rounded_rect(s->vg, 1920-530, 0, 150, 180, 20, nvgRGBA(10,10,10,170)); + // Add label nvgFontSize(s->vg, labelfontsize); nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 192)); diff --git a/selfdrive/updated.py b/selfdrive/updated.py new file mode 100644 index 0000000000..2790342b6c --- /dev/null +++ b/selfdrive/updated.py @@ -0,0 +1,29 @@ +#!/usr/bin/env python + +# simple service that waits for network access and tries to update every 3 hours + +import os +import time +import subprocess + +def main(gctx=None): + if not os.getenv("CLEAN"): + return + + while True: + # try network + r = subprocess.call(["ping", "-W", "4", "-c", "1", "8.8.8.8"]) + if r: + time.sleep(60) + continue + + # try fetch + r = subprocess.call(["nice", "-n", "19", "git", "fetch", "--depth=1"]) + if r: + time.sleep(60) + continue + + time.sleep(60*60*3) + +if __name__ == "__main__": + main() diff --git a/selfdrive/visiond/visiond b/selfdrive/visiond/visiond index 10ed806933..eb53c12bc0 100755 Binary files a/selfdrive/visiond/visiond and b/selfdrive/visiond/visiond differ