sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

openpilot v0.4.3.2 release

Browse Source
pull/219/merge
Vehicle Researcher 7 years ago
parent d0c9cd28d1
commit 78df63a6af
36 changed files with 412 additions and 424 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 9
      RELEASES.md
  2. BIN
      apk/ai.comma.plus.frame.apk
  3. BIN
      apk/ai.comma.plus.offroad.apk
  4. 6
      cereal/Makefile
  5. 17
      cereal/__init__.py
  6. 5
      cereal/log.capnp
  7. 18
      cereal/mapd.capnp
  8. 6
      common/dbc.py
  9. 4
      common/fingerprints.py
  10. 1
      common/kalman/ekf.py
  11. 2
      common/params.py
  12. 4
      common/transformations/coordinates.py
  13. 5
      launch_chffrplus.sh
  14. 11
      selfdrive/boardd/boardd.cc
  15. 2
      selfdrive/boardd/boardd.py
  16. 2
      selfdrive/can/packer.py
  17. 7
      selfdrive/car/honda/carstate.py
  18. 3
      selfdrive/car/honda/old_can_parser.py
  19. 8
      selfdrive/car/mock/interface.py
  20. 30
      selfdrive/car/toyota/carcontroller.py
  21. 4
      selfdrive/car/toyota/carstate.py
  22. 2
      selfdrive/common/version.h
  23. 5
      selfdrive/controls/controlsd.py
  24. 20
      selfdrive/controls/lib/lateral_mpc/generator.cpp
  25. 4
      selfdrive/controls/lib/lateral_mpc/mpc_export/acado_common.h
  26. 92
      selfdrive/controls/lib/lateral_mpc/mpc_export/acado_solver.c
  27. 3
      selfdrive/controls/lib/radar_helpers.py
  28. 5
      selfdrive/debug/dump.py
  29. BIN
      selfdrive/loggerd/loggerd
  30. 2
      selfdrive/manager.py
  31. BIN
      selfdrive/sensord/gpsd
  32. BIN
      selfdrive/sensord/sensord
  33. 8
      selfdrive/test/plant/plant_ui.py
  34. 522
      selfdrive/ui/ui.c
  35. 29
      selfdrive/updated.py
  36. BIN
      selfdrive/visiond/visiond

9
RELEASES.md
Unescape View File

@ -1,3 +1,12 @@
Version 0.4.3.2 (2018-03-29)
============================
* Improve autofocus
* Improve driving when only one lane line is detected
* Added fingerprint for Toyota Corolla LE
* Fixed Toyota Corolla steer error
* Full-screen driving UI
* Improved path drawing
Version 0.4.3.1 (2018-03-19) Version 0.4.3.1 (2018-03-19)
============================ ============================
* Improve autofocus * Improve autofocus

BIN
apk/ai.comma.plus.frame.apk
View File

Binary file not shown.

BIN
apk/ai.comma.plus.offroad.apk
View File

Binary file not shown.

6
cereal/Makefile
Unescape View File

@ -1,14 +1,14 @@
PWD := $(shell pwd) PWD := $(shell pwd)
SRCS := log.capnp car.capnp map.capnp SRCS := log.capnp car.capnp mapd.capnp
GENS := gen/cpp/car.capnp.c++ gen/cpp/log.capnp.c++ GENS := gen/cpp/car.capnp.c++ gen/cpp/log.capnp.c++ gen/cpp/mapd.capnp.c++
UNAME_M ?= $(shell uname -m) UNAME_M ?= $(shell uname -m)
# only generate C++ for docker tests # only generate C++ for docker tests
ifneq ($(OPTEST),1) ifneq ($(OPTEST),1)
GENS += gen/c/car.capnp.c gen/c/log.capnp.c gen/c/c++.capnp.h gen/c/java.capnp.h GENS += gen/c/car.capnp.c gen/c/log.capnp.c gen/c/mapd.capnp.c gen/c/c++.capnp.h gen/c/java.capnp.h
# Dont build java on the phone... # Dont build java on the phone...
ifeq ($(UNAME_M),x86_64) ifeq ($(UNAME_M),x86_64)

17
cereal/__init__.py
Unescape View File

@ -4,17 +4,6 @@ import capnp
CEREAL_PATH = os.path.dirname(os.path.abspath(__file__)) CEREAL_PATH = os.path.dirname(os.path.abspath(__file__))
capnp.remove_import_hook() capnp.remove_import_hook()
if os.getenv("NEWCAPNP"): log = capnp.load(os.path.join(CEREAL_PATH, "log.capnp"))
import tempfile car = capnp.load(os.path.join(CEREAL_PATH, "car.capnp"))
import pyximport mapd = capnp.load(os.path.join(CEREAL_PATH, "mapd.capnp"))
importers = pyximport.install(build_dir=os.path.join(tempfile.gettempdir(), ".pyxbld"))
try:
import cereal.gen.cython.log_capnp_cython as log
import cereal.gen.cython.car_capnp_cython as car
finally:
pyximport.uninstall(*importers)
del importers
else:
log = capnp.load(os.path.join(CEREAL_PATH, "log.capnp"))
car = capnp.load(os.path.join(CEREAL_PATH, "car.capnp"))

5
cereal/log.capnp
Unescape View File

@ -1487,6 +1487,11 @@ struct OrbFeatures {
ys @2 :List(Float32); ys @2 :List(Float32);
descriptors @3 :Data; descriptors @3 :Data;
octaves @4 :List(Int8); octaves @4 :List(Int8);
# match index to last OrbFeatures
# -1 if no match
timestampLastEof @5 :UInt64;
matches @6: List(Int16);
} }
struct OrbKeyFrame { struct OrbKeyFrame {

18
cereal/mapd.capnp
Unescape View File

@ -0,0 +1,18 @@
using Cxx = import "c++.capnp";
$Cxx.namespace("cereal");
using Java = import "java.capnp";
$Java.package("ai.comma.openpilot.cereal");
$Java.outerClassname("Log");
using Log = import "log.capnp";
@0xe1a6ab330ea7205f;
struct MapdRequest {
pos @0 :Log.ECEFPoint;
}
struct MapdResponse {
kfs @0 :List(Log.OrbKeyFrame);
}

6
common/dbc.py
Unescape View File

@ -25,9 +25,9 @@ class dbc(object):
self._warned_addresses = set() self._warned_addresses = set()
# regexps from https://github.com/ebroecker/canmatrix/blob/master/canmatrix/importdbc.py # regexps from https://github.com/ebroecker/canmatrix/blob/master/canmatrix/importdbc.py
bo_regexp = re.compile("^BO\_ (\w+) (\w+) *: (\w+) (\w+)") bo_regexp = re.compile(r"^BO\_ (\w+) (\w+) *: (\w+) (\w+)")
sg_regexp = re.compile("^SG\_ (\w+) : (\d+)\|(\d+)@(\d+)([\+|\-]) \(([0-9.+\-eE]+),([0-9.+\-eE]+)\) \[([0-9.+\-eE]+)\|([0-9.+\-eE]+)\] \"(.*)\" (.*)") sg_regexp = re.compile(r"^SG\_ (\w+) : (\d+)\|(\d+)@(\d+)([\+|\-]) \(([0-9.+\-eE]+),([0-9.+\-eE]+)\) \[([0-9.+\-eE]+)\|([0-9.+\-eE]+)\] \"(.*)\" (.*)")
sgm_regexp = re.compile("^SG\_ (\w+) (\w+) *: (\d+)\|(\d+)@(\d+)([\+|\-]) \(([0-9.+\-eE]+),([0-9.+\-eE]+)\) \[([0-9.+\-eE]+)\|([0-9.+\-eE]+)\] \"(.*)\" (.*)") sgm_regexp = re.compile(r"^SG\_ (\w+) (\w+) *: (\d+)\|(\d+)@(\d+)([\+|\-]) \(([0-9.+\-eE]+),([0-9.+\-eE]+)\) \[([0-9.+\-eE]+)\|([0-9.+\-eE]+)\] \"(.*)\" (.*)")
# A dictionary which maps message ids to tuples ((name, size), signals). # A dictionary which maps message ids to tuples ((name, size), signals).
# name is the ASCII name of the message. # name is the ASCII name of the message.

4
common/fingerprints.py
Unescape View File

@ -64,6 +64,10 @@ _FINGERPRINTS = {
}], }],
TOYOTA.COROLLA: [{ TOYOTA.COROLLA: [{
36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 547: 8, 548: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 2, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 4, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1196: 8, 1227: 8, 1235: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1596: 8, 1597: 8, 1600: 8, 1664: 8, 1728: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 547: 8, 548: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 2, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 4, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1196: 8, 1227: 8, 1235: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1596: 8, 1597: 8, 1600: 8, 1664: 8, 1728: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
},
# Corolla LE 2017
{
36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 547: 8, 548: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 2, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 4, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1196: 8, 1227: 8, 1235: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1596: 8, 1597: 8, 1600: 8, 1664: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2016: 8, 2017: 8, 2018: 8, 2019: 8, 2020: 8, 2021: 8, 2022: 8, 2023: 8, 2024: 8
}], }],
TOYOTA.LEXUS_RXH: [{ TOYOTA.LEXUS_RXH: [{
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 5, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 800: 8, 810: 2, 812: 3, 814: 8, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 6, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 8, 1071: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1595: 8, 1777: 8, 1779: 8, 1808: 8, 1810: 8, 1816: 8, 1818: 8, 1840: 8, 1848: 8, 1904: 8, 1912: 8, 1940: 8, 1941: 8, 1948: 8, 1949: 8, 1952: 8, 1956: 8, 1960: 8, 1964: 8, 1986: 8, 1990: 8, 1994: 8, 1998: 8, 2004: 8, 2012: 8 36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 5, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 800: 8, 810: 2, 812: 3, 814: 8, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 6, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 8, 1071: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1595: 8, 1777: 8, 1779: 8, 1808: 8, 1810: 8, 1816: 8, 1818: 8, 1840: 8, 1848: 8, 1904: 8, 1912: 8, 1940: 8, 1941: 8, 1948: 8, 1949: 8, 1952: 8, 1956: 8, 1960: 8, 1964: 8, 1986: 8, 1990: 8, 1994: 8, 1998: 8, 2004: 8, 2012: 8

1
common/kalman/ekf.py
Unescape View File

@ -1,3 +1,4 @@
# pylint: skip-file
import abc import abc
import numpy as np import numpy as np
# The EKF class contains the framework for an Extended Kalman Filter, but must be subclassed to use. # The EKF class contains the framework for an Extended Kalman Filter, but must be subclassed to use.

2
common/params.py
Unescape View File

@ -72,6 +72,8 @@ keys = {
"Passive": TxType.PERSISTANT, "Passive": TxType.PERSISTANT,
"DoUninstall": TxType.CLEAR_ON_MANAGER_START, "DoUninstall": TxType.CLEAR_ON_MANAGER_START,
"ShouldDoUpdate": TxType.CLEAR_ON_MANAGER_START,
"IsUpdateAvailable": TxType.PERSISTANT,
} }
def fsync_dir(path): def fsync_dir(path):

4
common/transformations/coordinates.py
Unescape View File

@ -78,12 +78,12 @@ class LocalCoord(object):
self.ecef2ned_matrix = self.ned2ecef_matrix.T self.ecef2ned_matrix = self.ned2ecef_matrix.T
@classmethod @classmethod
def from_geodetic(self, init_geodetic): def from_geodetic(cls, init_geodetic):
init_ecef = geodetic2ecef(init_geodetic) init_ecef = geodetic2ecef(init_geodetic)
return LocalCoord(init_geodetic, init_ecef) return LocalCoord(init_geodetic, init_ecef)
@classmethod @classmethod
def from_ecef(self, init_ecef): def from_ecef(cls, init_ecef):
init_geodetic = ecef2geodetic(init_ecef) init_geodetic = ecef2geodetic(init_ecef)
return LocalCoord(init_geodetic, init_ecef) return LocalCoord(init_geodetic, init_ecef)

5
launch_chffrplus.sh
Unescape View File

@ -5,11 +5,14 @@ if [ -z "$PASSIVE" ]; then
fi fi
function launch { function launch {
DO_UPDATE=$(cat /data/params/d/ShouldDoUpdate)
# apply update # apply update
if [ "$(git rev-parse HEAD)" != "$(git rev-parse @{u})" ]; then if [ "$DO_UPDATE" == "1" ] && [ "$(git rev-parse HEAD)" != "$(git rev-parse @{u})" ]; then
git reset --hard @{u} && git reset --hard @{u} &&
git clean -xdf && git clean -xdf &&
exec "${BASH_SOURCE[0]}" exec "${BASH_SOURCE[0]}"
echo -n 0 > /data/params/d/ShouldDoUpdate
echo -n 0 > /data/params/d/IsUpdateAvailable
fi fi
# no cpu rationing for now # no cpu rationing for now

11
selfdrive/boardd/boardd.cc
Unescape View File

@ -37,6 +37,8 @@
#define SAFETY_ELM327 0xE327 #define SAFETY_ELM327 0xE327
#define SAFETY_GM 3 #define SAFETY_GM 3
#define SAFETY_HONDA_BOSCH 4 #define SAFETY_HONDA_BOSCH 4
#define SAFETY_TOYOTA_NOLIMITS 0x1336
#define SAFETY_ALLOUTPUT 0x1337
namespace { namespace {
@ -570,8 +572,13 @@ void *pigeon_thread(void *crap) {
//printf("got %d\n", len); //printf("got %d\n", len);
alen += len; alen += len;
} }
if (alen > 0) { if (alen > 0) {
pigeon_publish_raw(publisher, dat, alen); if (dat[0] == (char)0x00){
LOGW("received invalid ublox message, resetting pigeon");
pigeon_init();
} else {
pigeon_publish_raw(publisher, dat, alen);
}
} }
// 10ms // 10ms

2
selfdrive/boardd/boardd.py
Unescape View File

@ -12,7 +12,7 @@ from selfdrive.swaglog import cloudlog
# USB is optional # USB is optional
try: try:
import usb1 import usb1
from usb1 import USBErrorIO, USBErrorOverflow from usb1 import USBErrorIO, USBErrorOverflow #pylint: disable=no-name-in-module
except Exception: except Exception:
pass pass

2
selfdrive/can/packer.py
Unescape View File

@ -62,4 +62,4 @@ if __name__ == "__main__":
("PCM_SPEED", 123), ("PCM_SPEED", 123),
("PCM_GAS", 10), ("PCM_GAS", 10),
]) ])
print s.encode("hex") print s[1].encode("hex")

7
selfdrive/car/honda/carstate.py
Unescape View File

@ -292,7 +292,6 @@ class CarState(object):
# carstate standalone tester # carstate standalone tester
if __name__ == '__main__': if __name__ == '__main__':
import zmq import zmq
import time
context = zmq.Context() context = zmq.Context()
class CarParams(object): class CarParams(object):
@ -303,6 +302,6 @@ if __name__ == '__main__':
CP = CarParams() CP = CarParams()
CS = CarState(CP) CS = CarState(CP)
while 1: # while 1:
CS.update() # CS.update()
time.sleep(0.01) # time.sleep(0.01)

3
selfdrive/car/honda/old_can_parser.py
Unescape View File

@ -7,7 +7,7 @@ from common.realtime import sec_since_boot
from common.dbc import dbc from common.dbc import dbc
class CANParser(object): class CANParser(object):
def __init__(self, dbc_f, signals, checks=[]): def __init__(self, dbc_f, signals, checks=None):
### input: ### input:
# dbc_f : dbc file # dbc_f : dbc file
# signals : List of tuples (name, address, ival) where # signals : List of tuples (name, address, ival) where
@ -19,6 +19,7 @@ class CANParser(object):
# monitored. # monitored.
# - frequency is the frequency at which health should be monitored. # - frequency is the frequency at which health should be monitored.
checks = [] if checks is None else checks
self.msgs_ck = set([check[0] for check in checks]) self.msgs_ck = set([check[0] for check in checks])
self.frqs = dict(checks) self.frqs = dict(checks)
self.can_valid = False # start with False CAN assumption self.can_valid = False # start with False CAN assumption

8
selfdrive/car/mock/interface.py
Unescape View File

@ -25,6 +25,7 @@ class CarInterface(object):
self.gps = messaging.sub_sock(context, service_list['gpsLocation'].port) self.gps = messaging.sub_sock(context, service_list['gpsLocation'].port)
self.speed = 0. self.speed = 0.
self.prev_speed = 0.
self.yaw_rate = 0. self.yaw_rate = 0.
self.yaw_rate_meas = 0. self.yaw_rate_meas = 0.
@ -85,6 +86,7 @@ class CarInterface(object):
gps = messaging.recv_sock(self.gps) gps = messaging.recv_sock(self.gps)
if gps is not None: if gps is not None:
self.prev_speed = self.speed
self.speed = gps.gpsLocation.speed self.speed = gps.gpsLocation.speed
# create message # create message
@ -93,7 +95,11 @@ class CarInterface(object):
# speeds # speeds
ret.vEgo = self.speed ret.vEgo = self.speed
ret.vEgoRaw = self.speed ret.vEgoRaw = self.speed
ret.aEgo = 0. a = self.speed - self.prev_speed
ret.aEgo = a
ret.brakePressed = a < -0.5
self.yawRate = LPG * self.yaw_rate_meas + (1. - LPG) * self.yaw_rate self.yawRate = LPG * self.yaw_rate_meas + (1. - LPG) * self.yaw_rate
ret.yawRate = self.yaw_rate ret.yawRate = self.yaw_rate
ret.standstill = self.speed < 0.01 ret.standstill = self.speed < 0.01

30
selfdrive/car/toyota/carcontroller.py
Unescape View File

@ -1,4 +1,4 @@
from common.numpy_fast import clip, interp, int_rnd from common.numpy_fast import clip, interp
from selfdrive.boardd.boardd import can_list_to_can_capnp from selfdrive.boardd.boardd import can_list_to_can_capnp
from selfdrive.car.toyota.toyotacan import make_can_msg, create_video_target,\ from selfdrive.car.toyota.toyotacan import make_can_msg, create_video_target,\
create_steer_command, create_ui_command, \ create_steer_command, create_ui_command, \
@ -19,11 +19,12 @@ STEER_DELTA_UP = 10 # 1.5s time to peak torque
STEER_DELTA_DOWN = 25 # always lower than 45 otherwise the Rav4 faults (Prius seems ok with 50) STEER_DELTA_DOWN = 25 # always lower than 45 otherwise the Rav4 faults (Prius seems ok with 50)
STEER_ERROR_MAX = 350 # max delta between torque cmd and torque motor STEER_ERROR_MAX = 350 # max delta between torque cmd and torque motor
# Steer angle limits # Steer angle limits (tested at the Crows Landing track and considered ok)
ANGLE_MAX_BP = [0., 5.] ANGLE_MAX_BP = [0., 5.]
ANGLE_MAX_V = [510., 300.] ANGLE_MAX_V = [510., 300.]
ANGLE_DELTA_BP = [0., 5.] ANGLE_DELTA_BP = [0., 5., 15.]
ANGLE_DELTA_V = [3., 1.] ANGLE_DELTA_V = [5., .8, .15] # windup limit
ANGLE_DELTA_VU = [5., 3.5, 0.4] # unwind limit
TARGET_IDS = [0x340, 0x341, 0x342, 0x343, 0x344, 0x345, TARGET_IDS = [0x340, 0x341, 0x342, 0x343, 0x344, 0x345,
0x363, 0x364, 0x365, 0x370, 0x371, 0x372, 0x363, 0x364, 0x365, 0x370, 0x371, 0x372,
@ -66,7 +67,7 @@ def process_hud_alert(hud_alert, audible_alert):
return steer, fcw, sound1, sound2 return steer, fcw, sound1, sound2
def ipas_state_transition(steer_angle_enabled, enabled, ipas_state, ipas_reset_counter): def ipas_state_transition(steer_angle_enabled, enabled, ipas_active, ipas_reset_counter):
if enabled and not steer_angle_enabled: if enabled and not steer_angle_enabled:
#ipas_reset_counter = max(0, ipas_reset_counter - 1) #ipas_reset_counter = max(0, ipas_reset_counter - 1)
@ -78,7 +79,7 @@ def ipas_state_transition(steer_angle_enabled, enabled, ipas_state, ipas_reset_c
return True, 0 return True, 0
elif enabled and steer_angle_enabled: elif enabled and steer_angle_enabled:
if steer_angle_enabled and ipas_state != 3: if steer_angle_enabled and not ipas_active:
ipas_reset_counter += 1 ipas_reset_counter += 1
else: else:
ipas_reset_counter = 0 ipas_reset_counter = 0
@ -141,20 +142,25 @@ class CarController(object):
# dropping torque immediately might cause eps to temp fault. On the other hand, safety_toyota # 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 # cuts steer torque immediately anyway TODO: monitor if this is a real issue
# only cut torque when steer state is a known fault # only cut torque when steer state is a known fault
if not enabled or CS.steer_state in [18, 50]: if not enabled or CS.steer_state in [9, 25]:
apply_steer = 0 apply_steer = 0
self.steer_angle_enabled, self.ipas_reset_counter = \ self.steer_angle_enabled, self.ipas_reset_counter = \
ipas_state_transition(self.steer_angle_enabled, enabled, CS.ipas_state, self.ipas_reset_counter) ipas_state_transition(self.steer_angle_enabled, enabled, CS.ipas_active, self.ipas_reset_counter)
#print self.steer_angle_enabled, self.ipas_reset_counter, CS.ipas_state #print self.steer_angle_enabled, self.ipas_reset_counter, CS.ipas_active
# steer angle # steer angle
if self.steer_angle_enabled: if self.steer_angle_enabled and CS.ipas_active:
apply_angle = actuators.steerAngle apply_angle = actuators.steerAngle
angle_lim = int_rnd(interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_MAX_V)) angle_lim = interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_MAX_V)
apply_angle = clip(apply_angle, -angle_lim, angle_lim) apply_angle = clip(apply_angle, -angle_lim, angle_lim)
angle_rate_lim = int_rnd(interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_V)) # windup slower
if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle):
angle_rate_lim = interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_DELTA_V)
else:
angle_rate_lim = interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_DELTA_VU)
apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim) apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim)
else: else:
apply_angle = CS.angle_steers apply_angle = CS.angle_steers

4
selfdrive/car/toyota/carstate.py
Unescape View File

@ -160,8 +160,8 @@ class CarState(object):
self.steer_override = abs(cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_DRIVER']) > 100 self.steer_override = abs(cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_DRIVER']) > 100
# 2 is standby, 10 is active. TODO: check that everything else is really a faulty state # 2 is standby, 10 is active. TODO: check that everything else is really a faulty state
self.steer_state = cp.vl["EPS_STATUS"]['LKA_STATE'] self.steer_state = cp.vl["EPS_STATUS"]['LKA_STATE']
self.steer_error = cp.vl["EPS_STATUS"]['LKA_STATE'] not in [2, 10] self.steer_error = cp.vl["EPS_STATUS"]['LKA_STATE'] not in [1, 5]
self.ipas_state = cp.vl['EPS_STATUS']['IPAS_STATE'] self.ipas_active = cp.vl['EPS_STATUS']['IPAS_STATE'] == 3
self.brake_error = 0 self.brake_error = 0
self.steer_torque_driver = cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_DRIVER'] self.steer_torque_driver = cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_DRIVER']
self.steer_torque_motor = cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_EPS'] self.steer_torque_motor = cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_EPS']

2
selfdrive/common/version.h
Unescape View File

@ -1 +1 @@
#define COMMA_VERSION "0.4.3.1-release" #define COMMA_VERSION "0.4.3.2-release"

5
selfdrive/controls/controlsd.py
Unescape View File

@ -224,10 +224,7 @@ def state_control(plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last,
for b in CS.buttonEvents: for b in CS.buttonEvents:
# button presses for rear view # button presses for rear view
if b.type == "leftBlinker" or b.type == "rightBlinker": if b.type == "leftBlinker" or b.type == "rightBlinker":
if b.pressed and rear_view_allowed: rear_view_toggle = b.pressed and rear_view_allowed
rear_view_toggle = True
else:
rear_view_toggle = False
if (b.type == "altButton1" and b.pressed) and not passive: if (b.type == "altButton1" and b.pressed) and not passive:
rear_view_toggle = not rear_view_toggle rear_view_toggle = not rear_view_toggle

20
selfdrive/controls/lib/lateral_mpc/generator.cpp
Unescape View File

@ -45,25 +45,29 @@ int main( )
auto angle_r = atan(3*r_poly_r0*xx*xx + 2*r_poly_r1*xx + r_poly_r2); auto angle_r = atan(3*r_poly_r0*xx*xx + 2*r_poly_r1*xx + r_poly_r2);
auto angle_p = atan(3*p_poly_r0*xx*xx + 2*p_poly_r1*xx + p_poly_r2); auto angle_p = atan(3*p_poly_r0*xx*xx + 2*p_poly_r1*xx + p_poly_r2);
auto c_left_lane = exp(-(poly_l - yy)); // given the lane width estimate, this is where we estimate the path given lane lines
auto c_right_lane = exp(poly_r - yy); auto l_phantom = poly_l - lane_width/2.0;
auto r_phantom = poly_r + lane_width/2.0;
auto r_phantom = poly_l - lane_width/2.0; // best path estimate path is a linear combination of poly_p and the path estimate
auto l_phantom = poly_r + lane_width/2.0; // given the lane lines
auto path = lr_prob * (l_prob * l_phantom + r_prob * r_phantom) / (l_prob + r_prob + 0.0001)
auto path = lr_prob * (l_prob * r_phantom + r_prob * l_phantom) / (l_prob + r_prob + 0.0001)
+ (1-lr_prob) * poly_p; + (1-lr_prob) * poly_p;
auto angle = lr_prob * (l_prob * angle_l + r_prob * angle_r) / (l_prob + r_prob + 0.0001) auto angle = lr_prob * (l_prob * angle_l + r_prob * angle_r) / (l_prob + r_prob + 0.0001)
+ (1-lr_prob) * angle_p; + (1-lr_prob) * angle_p;
// instead of using actual lane lines, use their estimated distance from path given lane_width
auto c_left_lane = exp(-(path + lane_width/2.0 - yy));
auto c_right_lane = exp(path - lane_width/2.0 - yy);
// Running cost // Running cost
Function h; Function h;
// Distance errors // Distance errors
h << path - yy; h << path - yy;
h << l_prob * c_left_lane; h << lr_prob * c_left_lane;
h << r_prob * c_right_lane; h << lr_prob * c_right_lane;
// Heading error // Heading error
h << (v_ref + 1.0 ) * (angle - psi); h << (v_ref + 1.0 ) * (angle - psi);

4
selfdrive/controls/lib/lateral_mpc/mpc_export/acado_common.h
Unescape View File

@ -184,8 +184,8 @@ real_t evGx[ 320 ];
/** Column vector of size: 80 */ /** Column vector of size: 80 */
real_t evGu[ 80 ]; real_t evGu[ 80 ];
/** Column vector of size: 19 */ /** Column vector of size: 21 */
real_t objAuxVar[ 19 ]; real_t objAuxVar[ 21 ];
/** Row vector of size: 23 */ /** Row vector of size: 23 */
real_t objValueIn[ 23 ]; real_t objValueIn[ 23 ];

92
selfdrive/controls/lib/lateral_mpc/mpc_export/acado_solver.c
Unescape View File

@ -99,49 +99,51 @@ void acado_evaluateLSQ(const real_t* in, real_t* out)
const real_t* xd = in; const real_t* xd = in;
const real_t* u = in + 4; const real_t* u = in + 4;
const real_t* od = in + 5; const real_t* od = in + 5;
/* Vector of auxiliary variables; number of elements: 19. */ /* Vector of auxiliary variables; number of elements: 21. */
real_t* a = acadoWorkspace.objAuxVar; real_t* a = acadoWorkspace.objAuxVar;
/* Compute intermediate quantities: */ /* Compute intermediate quantities: */
a[0] = (exp(((real_t)(0.0000000000000000e+00)-(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-xd[1])))); a[0] = (exp(((real_t)(0.0000000000000000e+00)-(((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))+(od[17]/(real_t)(2.0000000000000000e+00)))-xd[1]))));
a[1] = (exp((((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])-xd[1]))); a[1] = (exp((((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))-(od[17]/(real_t)(2.0000000000000000e+00)))-xd[1])));
a[2] = (atan(((((((real_t)(3.0000000000000000e+00)*od[2])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[3])*xd[0]))+od[4]))); a[2] = (atan(((((((real_t)(3.0000000000000000e+00)*od[2])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[3])*xd[0]))+od[4])));
a[3] = (atan(((((((real_t)(3.0000000000000000e+00)*od[6])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[7])*xd[0]))+od[8]))); a[3] = (atan(((((((real_t)(3.0000000000000000e+00)*od[6])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[7])*xd[0]))+od[8])));
a[4] = (atan(((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[11])*xd[0]))+od[12]))); a[4] = (atan(((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[11])*xd[0]))+od[12])));
a[5] = ((real_t)(1.0000000000000000e+00)/((od[14]+od[15])+(real_t)(1.0000000000000000e-04))); a[5] = ((real_t)(1.0000000000000000e+00)/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)));
a[6] = (exp(((real_t)(0.0000000000000000e+00)-(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-xd[1])))); a[6] = ((real_t)(1.0000000000000000e+00)/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)));
a[7] = (((real_t)(0.0000000000000000e+00)-(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))*a[6]); a[7] = (exp(((real_t)(0.0000000000000000e+00)-(((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))+(od[17]/(real_t)(2.0000000000000000e+00)))-xd[1]))));
a[8] = (((real_t)(0.0000000000000000e+00)-((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)))*a[6]); a[8] = (((real_t)(0.0000000000000000e+00)-(((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))))*a[6])+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12]))))*a[7]);
a[9] = (exp((((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])-xd[1]))); a[9] = (((real_t)(0.0000000000000000e+00)-((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)))*a[7]);
a[10] = ((((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8])*a[9]); a[10] = ((real_t)(1.0000000000000000e+00)/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)));
a[11] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[9]); a[11] = (exp((((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))-(od[17]/(real_t)(2.0000000000000000e+00)))-xd[1])));
a[12] = ((real_t)(1.0000000000000000e+00)/((real_t)(1.0000000000000000e+00)+(pow(((((((real_t)(3.0000000000000000e+00)*od[2])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[3])*xd[0]))+od[4]),2)))); a[12] = ((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))))*a[10])+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12])))*a[11]);
a[13] = ((((((real_t)(3.0000000000000000e+00)*od[2])*xd[0])+(((real_t)(3.0000000000000000e+00)*od[2])*xd[0]))+((real_t)(2.0000000000000000e+00)*od[3]))*a[12]); a[13] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[11]);
a[14] = ((real_t)(1.0000000000000000e+00)/((real_t)(1.0000000000000000e+00)+(pow(((((((real_t)(3.0000000000000000e+00)*od[6])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[7])*xd[0]))+od[8]),2)))); a[14] = ((real_t)(1.0000000000000000e+00)/((real_t)(1.0000000000000000e+00)+(pow(((((((real_t)(3.0000000000000000e+00)*od[2])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[3])*xd[0]))+od[4]),2))));
a[15] = ((((((real_t)(3.0000000000000000e+00)*od[6])*xd[0])+(((real_t)(3.0000000000000000e+00)*od[6])*xd[0]))+((real_t)(2.0000000000000000e+00)*od[7]))*a[14]); a[15] = ((((((real_t)(3.0000000000000000e+00)*od[2])*xd[0])+(((real_t)(3.0000000000000000e+00)*od[2])*xd[0]))+((real_t)(2.0000000000000000e+00)*od[3]))*a[14]);
a[16] = ((real_t)(1.0000000000000000e+00)/((od[14]+od[15])+(real_t)(1.0000000000000000e-04))); a[16] = ((real_t)(1.0000000000000000e+00)/((real_t)(1.0000000000000000e+00)+(pow(((((((real_t)(3.0000000000000000e+00)*od[6])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[7])*xd[0]))+od[8]),2))));
a[17] = ((real_t)(1.0000000000000000e+00)/((real_t)(1.0000000000000000e+00)+(pow(((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[11])*xd[0]))+od[12]),2)))); a[17] = ((((((real_t)(3.0000000000000000e+00)*od[6])*xd[0])+(((real_t)(3.0000000000000000e+00)*od[6])*xd[0]))+((real_t)(2.0000000000000000e+00)*od[7]))*a[16]);
a[18] = ((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])+(((real_t)(3.0000000000000000e+00)*od[10])*xd[0]))+((real_t)(2.0000000000000000e+00)*od[11]))*a[17]); a[18] = ((real_t)(1.0000000000000000e+00)/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)));
a[19] = ((real_t)(1.0000000000000000e+00)/((real_t)(1.0000000000000000e+00)+(pow(((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[11])*xd[0]))+od[12]),2))));
a[20] = ((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])+(((real_t)(3.0000000000000000e+00)*od[10])*xd[0]))+((real_t)(2.0000000000000000e+00)*od[11]))*a[19]);
/* Compute outputs: */ /* Compute outputs: */
out[0] = ((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))-xd[1]); out[0] = ((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))-xd[1]);
out[1] = (od[14]*a[0]); out[1] = (((od[14]+od[15])-(od[14]*od[15]))*a[0]);
out[2] = (od[15]*a[1]); out[2] = (((od[14]+od[15])-(od[14]*od[15]))*a[1]);
out[3] = ((od[1]+(real_t)(1.0000000000000000e+00))*((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*a[2])+(od[15]*a[3])))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*a[4]))-xd[2])); out[3] = ((od[1]+(real_t)(1.0000000000000000e+00))*((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*a[2])+(od[15]*a[3])))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*a[4]))-xd[2]));
out[4] = ((od[1]+(real_t)(1.0000000000000000e+00))*u[0]); out[4] = ((od[1]+(real_t)(1.0000000000000000e+00))*u[0]);
out[5] = (((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))))*a[5])+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12]))); out[5] = (((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))))*a[5])+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12])));
out[6] = ((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)); out[6] = ((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00));
out[7] = (real_t)(0.0000000000000000e+00); out[7] = (real_t)(0.0000000000000000e+00);
out[8] = (real_t)(0.0000000000000000e+00); out[8] = (real_t)(0.0000000000000000e+00);
out[9] = (od[14]*a[7]); out[9] = (((od[14]+od[15])-(od[14]*od[15]))*a[8]);
out[10] = (od[14]*a[8]); out[10] = (((od[14]+od[15])-(od[14]*od[15]))*a[9]);
out[11] = (real_t)(0.0000000000000000e+00); out[11] = (real_t)(0.0000000000000000e+00);
out[12] = (real_t)(0.0000000000000000e+00); out[12] = (real_t)(0.0000000000000000e+00);
out[13] = (od[15]*a[10]); out[13] = (((od[14]+od[15])-(od[14]*od[15]))*a[12]);
out[14] = (od[15]*a[11]); out[14] = (((od[14]+od[15])-(od[14]*od[15]))*a[13]);
out[15] = (real_t)(0.0000000000000000e+00); out[15] = (real_t)(0.0000000000000000e+00);
out[16] = (real_t)(0.0000000000000000e+00); out[16] = (real_t)(0.0000000000000000e+00);
out[17] = ((od[1]+(real_t)(1.0000000000000000e+00))*(((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*a[13])+(od[15]*a[15])))*a[16])+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*a[18]))); out[17] = ((od[1]+(real_t)(1.0000000000000000e+00))*(((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*a[15])+(od[15]*a[17])))*a[18])+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*a[20])));
out[18] = (real_t)(0.0000000000000000e+00); out[18] = (real_t)(0.0000000000000000e+00);
out[19] = ((od[1]+(real_t)(1.0000000000000000e+00))*((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))); out[19] = ((od[1]+(real_t)(1.0000000000000000e+00))*((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)));
out[20] = (real_t)(0.0000000000000000e+00); out[20] = (real_t)(0.0000000000000000e+00);
@ -160,29 +162,31 @@ void acado_evaluateLSQEndTerm(const real_t* in, real_t* out)
{ {
const real_t* xd = in; const real_t* xd = in;
const real_t* od = in + 4; const real_t* od = in + 4;
/* Vector of auxiliary variables; number of elements: 19. */ /* Vector of auxiliary variables; number of elements: 21. */
real_t* a = acadoWorkspace.objAuxVar; real_t* a = acadoWorkspace.objAuxVar;
/* Compute intermediate quantities: */ /* Compute intermediate quantities: */
a[0] = (exp(((real_t)(0.0000000000000000e+00)-(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-xd[1])))); a[0] = (exp(((real_t)(0.0000000000000000e+00)-(((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))+(od[17]/(real_t)(2.0000000000000000e+00)))-xd[1]))));
a[1] = (exp((((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])-xd[1]))); a[1] = (exp((((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))-(od[17]/(real_t)(2.0000000000000000e+00)))-xd[1])));
a[2] = (atan(((((((real_t)(3.0000000000000000e+00)*od[2])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[3])*xd[0]))+od[4]))); a[2] = (atan(((((((real_t)(3.0000000000000000e+00)*od[2])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[3])*xd[0]))+od[4])));
a[3] = (atan(((((((real_t)(3.0000000000000000e+00)*od[6])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[7])*xd[0]))+od[8]))); a[3] = (atan(((((((real_t)(3.0000000000000000e+00)*od[6])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[7])*xd[0]))+od[8])));
a[4] = (atan(((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[11])*xd[0]))+od[12]))); a[4] = (atan(((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[11])*xd[0]))+od[12])));
a[5] = ((real_t)(1.0000000000000000e+00)/((od[14]+od[15])+(real_t)(1.0000000000000000e-04))); a[5] = ((real_t)(1.0000000000000000e+00)/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)));
a[6] = (exp(((real_t)(0.0000000000000000e+00)-(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-xd[1])))); a[6] = ((real_t)(1.0000000000000000e+00)/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)));
a[7] = (((real_t)(0.0000000000000000e+00)-(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))*a[6]); a[7] = (exp(((real_t)(0.0000000000000000e+00)-(((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))+(od[17]/(real_t)(2.0000000000000000e+00)))-xd[1]))));
a[8] = (((real_t)(0.0000000000000000e+00)-((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)))*a[6]); a[8] = (((real_t)(0.0000000000000000e+00)-(((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))))*a[6])+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12]))))*a[7]);
a[9] = (exp((((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])-xd[1]))); a[9] = (((real_t)(0.0000000000000000e+00)-((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)))*a[7]);
a[10] = ((((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8])*a[9]); a[10] = ((real_t)(1.0000000000000000e+00)/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)));
a[11] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[9]); a[11] = (exp((((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))-(od[17]/(real_t)(2.0000000000000000e+00)))-xd[1])));
a[12] = ((real_t)(1.0000000000000000e+00)/((real_t)(1.0000000000000000e+00)+(pow(((((((real_t)(3.0000000000000000e+00)*od[2])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[3])*xd[0]))+od[4]),2)))); a[12] = ((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))))*a[10])+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12])))*a[11]);
a[13] = ((((((real_t)(3.0000000000000000e+00)*od[2])*xd[0])+(((real_t)(3.0000000000000000e+00)*od[2])*xd[0]))+((real_t)(2.0000000000000000e+00)*od[3]))*a[12]); a[13] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[11]);
a[14] = ((real_t)(1.0000000000000000e+00)/((real_t)(1.0000000000000000e+00)+(pow(((((((real_t)(3.0000000000000000e+00)*od[6])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[7])*xd[0]))+od[8]),2)))); a[14] = ((real_t)(1.0000000000000000e+00)/((real_t)(1.0000000000000000e+00)+(pow(((((((real_t)(3.0000000000000000e+00)*od[2])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[3])*xd[0]))+od[4]),2))));
a[15] = ((((((real_t)(3.0000000000000000e+00)*od[6])*xd[0])+(((real_t)(3.0000000000000000e+00)*od[6])*xd[0]))+((real_t)(2.0000000000000000e+00)*od[7]))*a[14]); a[15] = ((((((real_t)(3.0000000000000000e+00)*od[2])*xd[0])+(((real_t)(3.0000000000000000e+00)*od[2])*xd[0]))+((real_t)(2.0000000000000000e+00)*od[3]))*a[14]);
a[16] = ((real_t)(1.0000000000000000e+00)/((od[14]+od[15])+(real_t)(1.0000000000000000e-04))); a[16] = ((real_t)(1.0000000000000000e+00)/((real_t)(1.0000000000000000e+00)+(pow(((((((real_t)(3.0000000000000000e+00)*od[6])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[7])*xd[0]))+od[8]),2))));
a[17] = ((real_t)(1.0000000000000000e+00)/((real_t)(1.0000000000000000e+00)+(pow(((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[11])*xd[0]))+od[12]),2)))); a[17] = ((((((real_t)(3.0000000000000000e+00)*od[6])*xd[0])+(((real_t)(3.0000000000000000e+00)*od[6])*xd[0]))+((real_t)(2.0000000000000000e+00)*od[7]))*a[16]);
a[18] = ((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])+(((real_t)(3.0000000000000000e+00)*od[10])*xd[0]))+((real_t)(2.0000000000000000e+00)*od[11]))*a[17]); a[18] = ((real_t)(1.0000000000000000e+00)/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)));
a[19] = ((real_t)(1.0000000000000000e+00)/((real_t)(1.0000000000000000e+00)+(pow(((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[11])*xd[0]))+od[12]),2))));
a[20] = ((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])+(((real_t)(3.0000000000000000e+00)*od[10])*xd[0]))+((real_t)(2.0000000000000000e+00)*od[11]))*a[19]);
/* Compute outputs: */ /* Compute outputs: */
out[0] = ((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))-xd[1]); out[0] = ((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))-xd[1]);
@ -193,15 +197,15 @@ out[4] = (((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((od[2]*(((xd[0]+xd[0])*
out[5] = ((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)); out[5] = ((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00));
out[6] = (real_t)(0.0000000000000000e+00); out[6] = (real_t)(0.0000000000000000e+00);
out[7] = (real_t)(0.0000000000000000e+00); out[7] = (real_t)(0.0000000000000000e+00);
out[8] = (od[14]*a[7]); out[8] = (od[14]*a[8]);
out[9] = (od[14]*a[8]); out[9] = (od[14]*a[9]);
out[10] = (real_t)(0.0000000000000000e+00); out[10] = (real_t)(0.0000000000000000e+00);
out[11] = (real_t)(0.0000000000000000e+00); out[11] = (real_t)(0.0000000000000000e+00);
out[12] = (od[15]*a[10]); out[12] = (od[15]*a[12]);
out[13] = (od[15]*a[11]); out[13] = (od[15]*a[13]);
out[14] = (real_t)(0.0000000000000000e+00); out[14] = (real_t)(0.0000000000000000e+00);
out[15] = (real_t)(0.0000000000000000e+00); out[15] = (real_t)(0.0000000000000000e+00);
out[16] = ((((real_t)(2.0000000000000000e+00)*od[1])+(real_t)(1.0000000000000000e+00))*(((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*a[13])+(od[15]*a[15])))*a[16])+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*a[18]))); out[16] = ((((real_t)(2.0000000000000000e+00)*od[1])+(real_t)(1.0000000000000000e+00))*(((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*a[15])+(od[15]*a[17])))*a[18])+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*a[20])));
out[17] = (real_t)(0.0000000000000000e+00); out[17] = (real_t)(0.0000000000000000e+00);
out[18] = ((((real_t)(2.0000000000000000e+00)*od[1])+(real_t)(1.0000000000000000e+00))*((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))); out[18] = ((((real_t)(2.0000000000000000e+00)*od[1])+(real_t)(1.0000000000000000e+00))*((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)));
out[19] = (real_t)(0.0000000000000000e+00); out[19] = (real_t)(0.0000000000000000e+00);

3
selfdrive/controls/lib/radar_helpers.py
Unescape View File

@ -41,6 +41,7 @@ class Track(object):
def update(self, d_rel, y_rel, v_rel, d_path, v_ego_t_aligned, measured, steer_override): def update(self, d_rel, y_rel, v_rel, d_path, v_ego_t_aligned, measured, steer_override):
if self.initted: if self.initted:
# pylint: disable=access-member-before-definition
self.dPathPrev = self.dPath self.dPathPrev = self.dPath
self.vLeadPrev = self.vLead self.vLeadPrev = self.vLead
self.vRelPrev = self.vRel self.vRelPrev = self.vRel
@ -117,7 +118,7 @@ if platform.machine() == 'aarch64':
if os.path.isfile(os.path.join(pp, "_hierarchy.so")): if os.path.isfile(os.path.join(pp, "_hierarchy.so")):
sys.path.append(pp) sys.path.append(pp)
break break
import _hierarchy import _hierarchy #pylint: disable=import-error
else: else:
from scipy.cluster import _hierarchy from scipy.cluster import _hierarchy

5
selfdrive/debug/dump.py
Unescape View File

@ -44,7 +44,7 @@ if __name__ == "__main__":
republish_socks[sock] = messaging.pub_sock(context, port) republish_socks[sock] = messaging.pub_sock(context, port)
if args.map: if args.map:
from flask.ext.socketio import SocketIO from flask.ext.socketio import SocketIO #pylint: disable=no-name-in-module, import-error
from flask import Flask from flask import Flask
app = Flask(__name__) app = Flask(__name__)
socketio = SocketIO(app, async_mode='threading') socketio = SocketIO(app, async_mode='threading')
@ -64,7 +64,7 @@ if __name__ == "__main__":
republish_socks[sock].send(msg) republish_socks[sock].send(msg)
if args.map and evt.which() == 'liveLocation': if args.map and evt.which() == 'liveLocation':
print 'send loc' print 'send loc'
socketio.emit('location', { socketio.emit('location', {
'lat': evt.liveLocation.lat, 'lat': evt.liveLocation.lat,
'lon': evt.liveLocation.lon, 'lon': evt.liveLocation.lon,
'alt': evt.liveLocation.alt, 'alt': evt.liveLocation.alt,
@ -81,4 +81,3 @@ if __name__ == "__main__":
print json.dumps(evt.to_dict()) print json.dumps(evt.to_dict())
else: else:
print evt print evt

BIN
selfdrive/loggerd/loggerd
View File

Binary file not shown.

2
selfdrive/manager.py
Unescape View File

@ -52,7 +52,7 @@ sys.path.append(os.path.join(BASEDIR, "pyextra"))
os.environ['BASEDIR'] = BASEDIR os.environ['BASEDIR'] = BASEDIR
import zmq import zmq
from setproctitle import setproctitle from setproctitle import setproctitle #pylint: disable=no-name-in-module
from common.params import Params from common.params import Params
from common.realtime import sec_since_boot from common.realtime import sec_since_boot

BIN
selfdrive/sensord/gpsd
View File

Binary file not shown.

BIN
selfdrive/sensord/sensord
View File

Binary file not shown.

8
selfdrive/test/plant/plant_ui.py
Unescape View File

@ -21,7 +21,7 @@ def rot_center(image, angle):
return rot_image return rot_image
def car_w_color(c): def car_w_color(c):
car = pygame.Surface((METER*CAR_LENGTH, METER*CAR_LENGTH)) car = pygame.Surface((METER*CAR_LENGTH, METER*CAR_LENGTH)) # pylint: disable=too-many-function-args
car.set_alpha(0) car.set_alpha(0)
car.fill((10,10,10)) car.fill((10,10,10))
car.set_alpha(128) car.set_alpha(128)
@ -101,9 +101,9 @@ if __name__ == "__main__":
for x,y in control_pts: for x,y in control_pts:
pygame.draw.circle(display, (255,255,0), (int(x * METER),int(y * METER)), 2) pygame.draw.circle(display, (255,255,0), (int(x * METER),int(y * METER)), 2)
# draw path # draw path
path_pts = zip(np.arange(0, 50), md.model.path.points) path_pts = zip(np.arange(0, 50), md.model.path.points)
for x,y in path_pts: for x,y in path_pts:
x,y = pt_from_car((x,y)) x,y = pt_from_car((x,y))
pygame.draw.circle(display, (0,255,0), (int(x * METER),int(y * METER)), 1) pygame.draw.circle(display, (0,255,0), (int(x * METER),int(y * METER)), 1)
@ -118,5 +118,3 @@ if __name__ == "__main__":
""" """
pygame.display.flip() pygame.display.flip()

522
selfdrive/ui/ui.c
Unescape View File

@ -53,19 +53,16 @@
const int vwp_w = 1920; const int vwp_w = 1920;
const int vwp_h = 1080; const int vwp_h = 1080;
const int nav_w = 0; const int nav_w = 640;
const int nav_ww= 0; const int nav_ww= 760;
const int sbr_w = 300; const int sbr_w = 300;
const int bdr_s = 30; const int bdr_s = 30;
const int box_x = sbr_w+bdr_s; const int box_x = sbr_w+bdr_s;
const int box_y = bdr_s; const int box_y = bdr_s;
const int box_w = vwp_w-sbr_w-(bdr_s*2); const int box_w = vwp_w-sbr_w-(bdr_s*2);
const int box_h = vwp_h-(bdr_s*2); const int box_h = vwp_h-(bdr_s*2);
const int viz_x = box_x+nav_w; const int viz_w = vwp_w-(bdr_s*2);
const int viz_y = box_y; const int header_h = 420;
const int viz_w = box_w-nav_w;
const int viz_h = box_h;
const int viz_header_h = 420;
const uint8_t bg_colors[][4] = { const uint8_t bg_colors[][4] = {
[STATUS_STOPPED] = {0x07, 0x23, 0x39, 0xff}, [STATUS_STOPPED] = {0x07, 0x23, 0x39, 0xff},
@ -77,7 +74,7 @@ const uint8_t bg_colors[][4] = {
const uint8_t alert_colors[][4] = { const uint8_t alert_colors[][4] = {
[STATUS_STOPPED] = {0x07, 0x23, 0x39, 0xf1}, [STATUS_STOPPED] = {0x07, 0x23, 0x39, 0xf1},
[STATUS_DISENGAGED] = {0x17, 0x33, 0x49, 0xf1}, [STATUS_DISENGAGED] = {0x17, 0x33, 0x49, 0xc8},
[STATUS_ENGAGED] = {0x17, 0x86, 0x44, 0xf1}, [STATUS_ENGAGED] = {0x17, 0x86, 0x44, 0xf1},
[STATUS_WARNING] = {0xDA, 0x6F, 0x25, 0xf1}, [STATUS_WARNING] = {0xDA, 0x6F, 0x25, 0xf1},
[STATUS_ALERT] = {0xC9, 0x22, 0x31, 0xf1}, [STATUS_ALERT] = {0xC9, 0x22, 0x31, 0xf1},
@ -115,12 +112,10 @@ typedef struct UIScene {
bool uilayout_sidebarcollapsed; bool uilayout_sidebarcollapsed;
bool uilayout_mapenabled; bool uilayout_mapenabled;
// responsive sizes for sidebar // responsive layout
int ui_viz_rx; int ui_viz_rx;
int ui_viz_rw; int ui_viz_rw;
// responsize offset for frame + projections int ui_viz_ro;
int ui_frame_offset;
int ui_world_offset;
int lead_status; int lead_status;
float lead_d_rel, lead_y_rel, lead_v_rel; float lead_d_rel, lead_y_rel, lead_v_rel;
@ -228,8 +223,6 @@ typedef struct UIState {
static int last_brightness = -1; static int last_brightness = -1;
static void set_brightness(int brightness) { static void set_brightness(int brightness) {
if (last_brightness != brightness) { if (last_brightness != brightness) {
//printf("setting brightness %d\n", brightness);
// can't hurt
FILE *f = fopen("/sys/class/leds/lcd-backlight/brightness", "wb"); FILE *f = fopen("/sys/class/leds/lcd-backlight/brightness", "wb");
if (f != NULL) { if (f != NULL) {
fprintf(f, "%d", brightness); fprintf(f, "%d", brightness);
@ -309,7 +302,7 @@ static const mat4 device_transform = {{
// frame from 4/3 to box size with a 2x zoon // frame from 4/3 to box size with a 2x zoon
static const mat4 frame_transform = {{ static const mat4 frame_transform = {{
2*(4./3.)/((float)box_w/viz_h), 0.0, 0.0, 0.0, 2*(4./3.)/((float)viz_w/box_h), 0.0, 0.0, 0.0,
0.0, 2.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0,
0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0,
0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0,
@ -445,7 +438,6 @@ static bool try_load_intrinsics(mat3 *intrinsic_matrix) {
} }
} }
static void ui_init_vision(UIState *s, const VisionStreamBufs back_bufs, static void ui_init_vision(UIState *s, const VisionStreamBufs back_bufs,
int num_back_fds, const int *back_fds, int num_back_fds, const int *back_fds,
const VisionStreamBufs front_bufs, int num_front_fds, const VisionStreamBufs front_bufs, int num_front_fds,
@ -495,12 +487,6 @@ static void ui_init_vision(UIState *s, const VisionStreamBufs back_bufs,
} }
} }
static bool ui_alert_active(UIState *s) {
return (nanos_since_boot() - s->scene.alert_ts) < 20000000000ULL &&
strlen(s->scene.alert_text1) > 0 &&
s->scene.alert_size == cereal_Live100Data_AlertSize_full;
}
static void ui_update_frame(UIState *s) { static void ui_update_frame(UIState *s) {
assert(glGetError() == GL_NO_ERROR); assert(glGetError() == GL_NO_ERROR);
@ -580,8 +566,6 @@ vec3 car_space_to_full_frame(const UIState *s, vec4 car_space_projective) {
return p_image; return p_image;
} }
// TODO: refactor with draw_path
static void draw_cross(UIState *s, float x_in, float y_in, float sz, NVGcolor color) { static void draw_cross(UIState *s, float x_in, float y_in, float sz, NVGcolor color) {
const UIScene *scene = &s->scene; const UIScene *scene = &s->scene;
@ -626,8 +610,59 @@ static void draw_cross(UIState *s, float x_in, float y_in, float sz, NVGcolor co
nvgRestore(s->vg); nvgRestore(s->vg);
} }
static void draw_x_y(UIState *s, const float *x_coords, const float *y_coords, size_t num_points, static void draw_chevron(UIState *s, float x_in, float y_in, float sz,
NVGcolor color) { NVGcolor fillColor, NVGcolor glowColor) {
const UIScene *scene = &s->scene;
nvgSave(s->vg);
nvgTranslate(s->vg, 240.0f, 0.0);
nvgTranslate(s->vg, -1440.0f / 2, -1080.0f / 2);
nvgScale(s->vg, 2.0, 2.0);
nvgScale(s->vg, 1440.0f / s->rgb_width, 1080.0f / s->rgb_height);
const vec4 p_car_space = (vec4){{x_in, y_in, 0., 1.}};
const vec3 p_full_frame = car_space_to_full_frame(s, p_car_space);
sz *= 30;
sz /= (x_in / 3 + 30);
if (sz > 30) sz = 30;
if (sz < 15) sz = 15;
float x = p_full_frame.v[0];
float y = p_full_frame.v[1];
// glow
nvgBeginPath(s->vg);
float g_xo = 5;
float g_yo = 2;
if (x >= 0 && y >= 0.) {
nvgMoveTo(s->vg, x+(sz*1.35)+g_xo, y+sz+g_yo);
nvgLineTo(s->vg, x, y-g_xo);
nvgLineTo(s->vg, x-(sz*1.35)-g_xo, y+sz+g_yo);
nvgLineTo(s->vg, x+(sz*1.35)+g_xo, y+sz+g_yo);
nvgClosePath(s->vg);
}
nvgFillColor(s->vg, glowColor);
nvgFill(s->vg);
// chevron
nvgBeginPath(s->vg);
if (x >= 0 && y >= 0.) {
nvgMoveTo(s->vg, x+(sz*1.25), y+sz);
nvgLineTo(s->vg, x, y);
nvgLineTo(s->vg, x-(sz*1.25), y+sz);
nvgLineTo(s->vg, x+(sz*1.25), y+sz);
nvgClosePath(s->vg);
}
nvgFillColor(s->vg, fillColor);
nvgFill(s->vg);
nvgRestore(s->vg);
}
static void ui_draw_lane_edge(UIState *s, const float *points, float off,
NVGcolor color, int width) {
const UIScene *scene = &s->scene; const UIScene *scene = &s->scene;
nvgSave(s->vg); nvgSave(s->vg);
@ -643,12 +678,13 @@ static void draw_x_y(UIState *s, const float *x_coords, const float *y_coords, s
nvgBeginPath(s->vg); nvgBeginPath(s->vg);
nvgStrokeColor(s->vg, color); nvgStrokeColor(s->vg, color);
nvgStrokeWidth(s->vg, 2); nvgStrokeWidth(s->vg, width);
bool started = false; bool started = false;
for (int i=0; i<num_points; i++) { for (int i=0; i<50; i++) {
float px = x_coords[i]; float px = (float)i;
float py = y_coords[i]; float py = points[i] + off;
vec4 p_car_space = (vec4){{px, py, 0., 1.}}; vec4 p_car_space = (vec4){{px, py, 0., 1.}};
vec3 p_full_frame = car_space_to_full_frame(s, p_car_space); vec3 p_full_frame = car_space_to_full_frame(s, p_car_space);
@ -671,33 +707,81 @@ static void draw_x_y(UIState *s, const float *x_coords, const float *y_coords, s
nvgRestore(s->vg); nvgRestore(s->vg);
} }
static void draw_path(UIState *s, const float *points, float off, static void ui_draw_lane(UIState *s, const PathData path, NVGcolor color) {
NVGcolor color, int width) { ui_draw_lane_edge(s, path.points, 0.0, color, 5*path.prob);
float var = min(path.std, 0.7);
color.a /= 4;
ui_draw_lane_edge(s, path.points, -var, color, 2);
ui_draw_lane_edge(s, path.points, var, color, 2);
}
static void ui_draw_track(UIState *s, bool is_mpc) {
const UIScene *scene = &s->scene; const UIScene *scene = &s->scene;
const PathData path = scene->model.path;
const float *mpc_x_coords = &scene->mpc_x[0];
const float *mpc_y_coords = &scene->mpc_y[0];
nvgSave(s->vg); nvgSave(s->vg);
nvgTranslate(s->vg, 240.0f, 0.0); // rgb-box space
// path coords are worked out in rgb-box space nvgTranslate(s->vg, -1440.0f / 2, -1080.0f / 2); // zoom 2x
nvgTranslate(s->vg, 240.0f, 0.0);
// zooom in 2x
nvgTranslate(s->vg, -1440.0f / 2, -1080.0f / 2);
nvgScale(s->vg, 2.0, 2.0); nvgScale(s->vg, 2.0, 2.0);
nvgScale(s->vg, 1440.0f / s->rgb_width, 1080.0f / s->rgb_height); nvgScale(s->vg, 1440.0f / s->rgb_width, 1080.0f / s->rgb_height);
nvgBeginPath(s->vg); nvgBeginPath(s->vg);
nvgStrokeColor(s->vg, color);
nvgStrokeWidth(s->vg, width);
bool started = false; bool started = false;
for (int i=0; i<50; i++) { int track_start_x = 0;
float px = (float)i; int track_start_y = 0;
float py = points[i] + off; int track_end_x = 0;
int track_end_y = 0;
int lead_d = (int)scene->lead_d_rel*2.;
int path_height = is_mpc?((lead_d>5)?min(lead_d, 25)-min(lead_d/5,5):20)
:((lead_d>0)?min(lead_d, 50)-min(lead_d/2.5,15):50);
float off = is_mpc?0.3:0.5;
// left side up
for (int i=0; i<path_height; i++) {
float px, py;
if (is_mpc) {
px = mpc_x_coords[i];
py = mpc_y_coords[i] - off;
} else {
px = (float)i;
py = path.points[i] - off;
}
vec4 p_car_space = (vec4){{px, py, 0., 1.}}; vec4 p_car_space = (vec4){{px, py, 0., 1.}};
vec3 p_full_frame = car_space_to_full_frame(s, p_car_space); vec3 p_full_frame = car_space_to_full_frame(s, p_car_space);
float x = p_full_frame.v[0];
float y = p_full_frame.v[1];
if (x < 0 || y < 0.) {
continue;
}
if (!started) {
nvgMoveTo(s->vg, x, y);
track_start_x = x;
track_start_y = y;
started = true;
} else {
nvgLineTo(s->vg, x, y);
}
}
// right side down
for (int i=path_height; i>0; i--) {
float px, py;
if (is_mpc) {
px = mpc_x_coords[i];
py = mpc_y_coords[i] + off;
} else {
px = (float)i;
py = path.points[i] + off;
}
vec4 p_car_space = (vec4){{px, py, 0., 1.}};
vec3 p_full_frame = car_space_to_full_frame(s, p_car_space);
float x = p_full_frame.v[0]; float x = p_full_frame.v[0];
float y = p_full_frame.v[1]; float y = p_full_frame.v[1];
if (x < 0 || y < 0.) { if (x < 0 || y < 0.) {
@ -706,25 +790,31 @@ static void draw_path(UIState *s, const float *points, float off,
if (!started) { if (!started) {
nvgMoveTo(s->vg, x, y); nvgMoveTo(s->vg, x, y);
track_end_y = y;
track_end_x = x;
started = true; started = true;
} else { } else {
nvgLineTo(s->vg, x, y); nvgLineTo(s->vg, x, y);
} }
} }
nvgStroke(s->vg); NVGpaint track_bg;
if (is_mpc) {
// green track
const uint8_t *clr = bg_colors[s->status];
track_bg = nvgLinearGradient(s->vg, track_start_x, track_start_y, track_end_x, track_end_y,
nvgRGBA(clr[0], clr[1], clr[2], 255), nvgRGBA(clr[0], clr[1], clr[2], 150));
} else {
// white track
track_bg = nvgLinearGradient(s->vg, track_start_x, track_start_y, track_end_x, track_end_y,
nvgRGBA(255, 255, 255, 150), nvgRGBA(255, 255, 255, 75));
}
nvgFillPaint(s->vg, track_bg);
nvgFill(s->vg);
nvgRestore(s->vg); nvgRestore(s->vg);
} }
static void draw_model_path(UIState *s, const PathData path, NVGcolor color) {
draw_path(s, path.points, 0.0, color, 4*path.prob);
float var = min(path.std, 0.7);
color.a /= 4;
draw_path(s, path.points, -var, color, 2);
draw_path(s, path.points, var, color, 2);
}
static void draw_steering(UIState *s, float curvature) { static void draw_steering(UIState *s, float curvature) {
float points[50]; float points[50];
for (int i = 0; i < 50; i++) { for (int i = 0; i < 50; i++) {
@ -732,27 +822,23 @@ static void draw_steering(UIState *s, float curvature) {
points[i] = y_actual; points[i] = y_actual;
} }
draw_path(s, points, 0.0, nvgRGBA(0, 0, 255, 128), 5); ui_draw_lane_edge(s, points, 0.0, nvgRGBA(0, 0, 255, 128), 5);
} }
static void draw_frame(UIState *s) { static void draw_frame(UIState *s) {
// draw frame texture
const UIScene *scene = &s->scene; const UIScene *scene = &s->scene;
mat4 out_mat; mat4 out_mat;
float x1, x2, y1, y2; float x1, x2, y1, y2;
if (s->scene.frontview) { if (s->scene.frontview) {
out_mat = device_transform; // full 16/9 out_mat = device_transform; // full 16/9
// flip horizontally so it looks like a mirror // flip horizontally so it looks like a mirror
x2 = (float)scene->front_box_x / s->rgb_front_width; x2 = (float)scene->front_box_x / s->rgb_front_width;
x1 = (float)(scene->front_box_x + scene->front_box_width) / s->rgb_front_width; x1 = (float)(scene->front_box_x + scene->front_box_width) / s->rgb_front_width;
y1 = (float)scene->front_box_y / s->rgb_front_height; y1 = (float)scene->front_box_y / s->rgb_front_height;
y2 = (float)(scene->front_box_y + scene->front_box_height) / s->rgb_front_height; y2 = (float)(scene->front_box_y + scene->front_box_height) / s->rgb_front_height;
} else { } else {
out_mat = matmul(device_transform, frame_transform); out_mat = matmul(device_transform, frame_transform);
x1 = 1.0; x1 = 1.0;
x2 = 0.0; x2 = 0.0;
y1 = 1.0; y1 = 1.0;
@ -791,137 +877,25 @@ static void draw_frame(UIState *s) {
glDrawElements(GL_TRIANGLES, 6, GL_UNSIGNED_BYTE, &frame_indicies[0]); glDrawElements(GL_TRIANGLES, 6, GL_UNSIGNED_BYTE, &frame_indicies[0]);
} }
/*
* Draw a rect at specific position with specific dimensions
*/
static void ui_draw_rounded_rect(
NVGcontext* c,
int x,
int y,
int width,
int height,
int radius,
NVGcolor color
) {
int bottom_x = x + width;
int bottom_y = y + height;
nvgBeginPath(c);
// Position the rect
nvgRoundedRect(c, x, y, bottom_x, bottom_y, radius);
// Color the rect
nvgFillColor(c, color);
// Draw the rect
nvgFill(c);
// Draw white border around rect
nvgStrokeColor(c, nvgRGBA(255,255,255,200));
nvgStroke(c);
}
static void fill_lane(UIState *s, const float *l_points, const float *r_points, float off,
NVGcolor color) {
const UIScene *scene = &s->scene;
nvgSave(s->vg);
// path coords are worked out in rgb-box space
nvgTranslate(s->vg, 240.0f, 0.0);
// zoom in 2x
nvgTranslate(s->vg, -1440.0f / 2, -1080.0f / 2);
nvgScale(s->vg, 2.0, 2.0);
nvgScale(s->vg, 1440.0f / s->rgb_width, 1080.0f / s->rgb_height);
nvgBeginPath(s->vg);
bool started = false;
int gradient_start_x = 0;
int gradient_start_y = 0;
int gradient_end_x = 0;
int gradient_end_y = 0;
// left side of lane
for (int i=0; i<50; i++) {
float px = (float)i;
float py = l_points[i] + off;
vec4 p_car_space = (vec4){{px, py, 0., 1.}};
vec3 p_full_frame = car_space_to_full_frame(s, p_car_space);
float x = p_full_frame.v[0];
float y = p_full_frame.v[1];
if (x < 0 || y < 0.) {
continue;
}
if (!started) {
nvgMoveTo(s->vg, x, y);
gradient_start_x = x;
gradient_start_y = y;
started = true;
} else {
nvgLineTo(s->vg, x, y);
}
}
// right side of lane
for (int i=50; i>0; i--) {
float px = (float)i;
float py = r_points[i] + off;
vec4 p_car_space = (vec4){{px, py, 0., 1.}};
vec3 p_full_frame = car_space_to_full_frame(s, p_car_space);
float x = p_full_frame.v[0];
float y = p_full_frame.v[1];
if (x < 0 || y < 0.) {
continue;
}
if (!started) {
nvgMoveTo(s->vg, x, y);
started = true;
} else {
nvgLineTo(s->vg, x, y);
gradient_end_y = y;
gradient_end_x = x;
}
}
NVGpaint bg = nvgLinearGradient(s->vg, gradient_start_x, gradient_start_y, gradient_end_x, gradient_end_y,
nvgRGBAf(0.6, 0.6, 0.6, 1.0*scene->model.left_lane.prob), nvgRGBAf(0.6, 0.6, 0.6, 1.0*scene->model.right_lane.prob));
nvgFillPaint(s->vg, bg);
nvgFill(s->vg);
nvgRestore(s->vg);
}
static void ui_draw_vision_lanes(UIState *s) { static void ui_draw_vision_lanes(UIState *s) {
const UIScene *scene = &s->scene; const UIScene *scene = &s->scene;
if ((nanos_since_boot() - scene->model_ts) < 1000000000ULL) { if ((nanos_since_boot() - scene->model_ts) < 1000000000ULL) {
// draw left lane edge // draw left lane edge
draw_model_path( ui_draw_lane(
s, scene->model.left_lane, s, scene->model.left_lane,
nvgRGBAf(1.0, 1.0, 1.0, scene->model.left_lane.prob)); nvgRGBAf(1.0, 1.0, 1.0, scene->model.left_lane.prob));
// draw right lane edge // draw right lane edge
draw_model_path( ui_draw_lane(
s, scene->model.right_lane, s, scene->model.right_lane,
nvgRGBAf(1.0, 1.0, 1.0, scene->model.right_lane.prob)); nvgRGBAf(1.0, 1.0, 1.0, scene->model.right_lane.prob));
// draw projected path line // draw vision path
draw_path(s, scene->model.path.points, 0.0f, nvgRGBA(0xc0, 0xc0, 0xc0, 255), 5); ui_draw_track(s, false);
// draw MPC only if engaged // draw MPC path when engaged
if (scene->engaged) { if (scene->engaged) {
draw_x_y(s, &scene->mpc_x[0], &scene->mpc_y[0], 20, nvgRGBA(255, 0, 0, 255)); ui_draw_track(s, true);
} }
} }
} }
@ -1057,39 +1031,6 @@ static void ui_draw_vision_topbar(UIState *s) {
nvgText(s->vg, right_x+right_width/2, 185, "LEAD CAR", NULL); nvgText(s->vg, right_x+right_width/2, 185, "LEAD CAR", NULL);
} }
static void ui_draw_calibration_status(UIState *s) {
const UIScene *scene = &s->scene;
int ui_viz_rx = scene->ui_viz_rx;
int ui_viz_rw = scene->ui_viz_rw;
int viz_calib_w = 1120;
int viz_calib_x = (ui_viz_rx + ((ui_viz_rw/2) - (viz_calib_w/2)));
int viz_calib_y = 760;
int viz_calib_h = 250;
int viz_calibtext_x = (ui_viz_rx + (ui_viz_rw/2));
nvgBeginPath(s->vg);
nvgStrokeWidth(s->vg, 10);
nvgRoundedRect(s->vg, viz_calib_x, viz_calib_y, viz_calib_w, viz_calib_h, 20);
nvgStroke(s->vg);
nvgFillColor(s->vg, nvgRGBA(0,0,0,180));
nvgFill(s->vg);
nvgFontSize(s->vg, 40*2.5);
nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE);
nvgFontFace(s->vg, "sans-semibold");
nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 220));
char calib_status_str[64];
snprintf(calib_status_str, sizeof(calib_status_str), "Calibration in Progress: %d%%", scene->cal_perc);
nvgText(s->vg, viz_calibtext_x, viz_calib_y+100, calib_status_str, NULL);
if (s->is_metric) {
nvgText(s->vg, viz_calibtext_x, viz_calib_y+200, "Drive above 72 km/h", NULL);
} else {
nvgText(s->vg, viz_calibtext_x, viz_calib_y+200, "Drive above 45 mph", NULL);
}
}
static void ui_draw_gpsplanner_status(UIState *s) { static void ui_draw_gpsplanner_status(UIState *s) {
const UIScene *scene = &s->scene; const UIScene *scene = &s->scene;
@ -1129,7 +1070,7 @@ static void ui_draw_vision_maxspeed(UIState *s) {
float maxspeed = s->scene.v_cruise; float maxspeed = s->scene.v_cruise;
const int viz_maxspeed_x = (ui_viz_rx + (bdr_s*2)); const int viz_maxspeed_x = (ui_viz_rx + (bdr_s*2));
const int viz_maxspeed_y = (viz_y + (bdr_s*1.5)); const int viz_maxspeed_y = (box_y + (bdr_s*1.5));
const int viz_maxspeed_w = 180; const int viz_maxspeed_w = 180;
const int viz_maxspeed_h = 202; const int viz_maxspeed_h = 202;
char maxspeed_str[32]; char maxspeed_str[32];
@ -1174,7 +1115,7 @@ static void ui_draw_vision_speed(UIState *s) {
char speed_str[32]; char speed_str[32];
nvgBeginPath(s->vg); nvgBeginPath(s->vg);
nvgRect(s->vg, viz_speed_x, viz_y, viz_speed_w, viz_header_h); nvgRect(s->vg, viz_speed_x, box_y, viz_speed_w, header_h);
nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE); nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE);
if (s->is_metric) { if (s->is_metric) {
@ -1204,21 +1145,21 @@ static void ui_draw_vision_wheel(UIState *s) {
const int ui_viz_rw = scene->ui_viz_rw; const int ui_viz_rw = scene->ui_viz_rw;
const int viz_event_w = 220; const int viz_event_w = 220;
const int viz_event_x = ((ui_viz_rx + ui_viz_rw) - (viz_event_w + (bdr_s*2))); const int viz_event_x = ((ui_viz_rx + ui_viz_rw) - (viz_event_w + (bdr_s*2)));
const int viz_event_y = (viz_y + (bdr_s*1.5)); const int viz_event_y = (box_y + (bdr_s*1.5));
const int viz_event_h = (viz_header_h - (bdr_s*1.5)); const int viz_event_h = (header_h - (bdr_s*1.5));
// draw steering wheel // draw steering wheel
const int viz_wheel_size = 96; const int bg_wheel_size = 96;
const int viz_wheel_x = viz_event_x + (viz_event_w-viz_wheel_size); const int bg_wheel_x = viz_event_x + (viz_event_w-bg_wheel_size);
const int viz_wheel_y = viz_event_y + (viz_wheel_size/2); const int bg_wheel_y = viz_event_y + (bg_wheel_size/2);
const int img_wheel_size = viz_wheel_size*1.5; const int img_wheel_size = bg_wheel_size*1.5;
const int img_wheel_x = viz_wheel_x-(img_wheel_size/2); const int img_wheel_x = bg_wheel_x-(img_wheel_size/2);
const int img_wheel_y = viz_wheel_y-25; const int img_wheel_y = bg_wheel_y-25;
float img_wheel_alpha = 0.5f; float img_wheel_alpha = 0.5f;
bool is_engaged = (s->status == STATUS_ENGAGED); bool is_engaged = (s->status == STATUS_ENGAGED);
bool is_warning = (s->status == STATUS_WARNING); bool is_warning = (s->status == STATUS_WARNING);
if (is_engaged || is_warning) { if (is_engaged || is_warning) {
nvgBeginPath(s->vg); nvgBeginPath(s->vg);
nvgCircle(s->vg, viz_wheel_x, (viz_wheel_y + (bdr_s*1.5)), viz_wheel_size); nvgCircle(s->vg, bg_wheel_x, (bg_wheel_y + (bdr_s*1.5)), bg_wheel_size);
if (is_engaged) { if (is_engaged) {
nvgFillColor(s->vg, nvgRGBA(23, 134, 68, 255)); nvgFillColor(s->vg, nvgRGBA(23, 134, 68, 255));
} else if (is_warning) { } else if (is_warning) {
@ -1242,11 +1183,11 @@ static void ui_draw_vision_header(UIState *s) {
nvgBeginPath(s->vg); nvgBeginPath(s->vg);
NVGpaint gradient = nvgLinearGradient(s->vg, ui_viz_rx, NVGpaint gradient = nvgLinearGradient(s->vg, ui_viz_rx,
(viz_y+(viz_header_h-(viz_header_h/2.5))), (box_y+(header_h-(header_h/2.5))),
ui_viz_rx, viz_y+viz_header_h, ui_viz_rx, box_y+header_h,
nvgRGBAf(0,0,0,0.45), nvgRGBAf(0,0,0,0)); nvgRGBAf(0,0,0,0.45), nvgRGBAf(0,0,0,0));
nvgFillPaint(s->vg, gradient); nvgFillPaint(s->vg, gradient);
nvgRect(s->vg, ui_viz_rx, viz_y, ui_viz_rw, viz_header_h); nvgRect(s->vg, ui_viz_rx, box_y, ui_viz_rw, header_h);
nvgFill(s->vg); nvgFill(s->vg);
ui_draw_vision_maxspeed(s); ui_draw_vision_maxspeed(s);
@ -1254,19 +1195,20 @@ static void ui_draw_vision_header(UIState *s) {
ui_draw_vision_wheel(s); ui_draw_vision_wheel(s);
} }
static void ui_draw_vision_alert(UIState *s) { static void ui_draw_vision_alert(UIState *s, int va_size, int va_color,
const char* va_text1, const char* va_text2) {
const UIScene *scene = &s->scene; const UIScene *scene = &s->scene;
int ui_viz_rx = scene->ui_viz_rx; int ui_viz_rx = scene->ui_viz_rx;
int ui_viz_rw = scene->ui_viz_rw; int ui_viz_rw = scene->ui_viz_rw;
bool hasSidebar = !s->scene.uilayout_sidebarcollapsed; bool hasSidebar = !s->scene.uilayout_sidebarcollapsed;
bool mapEnabled = s->scene.uilayout_mapenabled; bool mapEnabled = s->scene.uilayout_mapenabled;
bool longAlert1 = strlen(scene->alert_text1) > 15; bool longAlert1 = strlen(va_text1) > 15;
const uint8_t *color = alert_colors[s->status]; const uint8_t *color = alert_colors[va_color];
const int alr_s = alert_sizes[s->alert_size]; const int alr_s = alert_sizes[va_size];
const int alr_x = ui_viz_rx-(mapEnabled?(hasSidebar?nav_w:(nav_ww+(bdr_s*2))):0)-bdr_s; const int alr_x = ui_viz_rx-(mapEnabled?(hasSidebar?nav_w:(nav_ww)):0)-bdr_s;
const int alr_w = ui_viz_rw+(mapEnabled?(hasSidebar?nav_w:(nav_ww+(bdr_s*2))):0)+(bdr_s*2); const int alr_w = ui_viz_rw+(mapEnabled?(hasSidebar?nav_w:(nav_ww)):0)+(bdr_s*2);
const int alr_h = alr_s+(s->alert_size==ALERTSIZE_NONE?0:bdr_s); const int alr_h = alr_s+(va_size==ALERTSIZE_NONE?0:bdr_s);
const int alr_y = vwp_h-alr_h; const int alr_y = vwp_h-alr_h;
nvgBeginPath(s->vg); nvgBeginPath(s->vg);
@ -1284,41 +1226,52 @@ static void ui_draw_vision_alert(UIState *s) {
nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255)); nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255));
nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE); nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE);
if (s->alert_size == ALERTSIZE_SMALL) { if (va_size == ALERTSIZE_SMALL) {
nvgFontFace(s->vg, "sans-semibold"); nvgFontFace(s->vg, "sans-semibold");
nvgFontSize(s->vg, 40*2.5); nvgFontSize(s->vg, 40*2.5);
nvgText(s->vg, alr_x+alr_w/2, alr_y+alr_h/2+15, scene->alert_text1, NULL); nvgText(s->vg, alr_x+alr_w/2, alr_y+alr_h/2+15, va_text1, NULL);
} else if (s->alert_size == ALERTSIZE_MID) { } else if (va_size== ALERTSIZE_MID) {
nvgFontFace(s->vg, "sans-bold"); nvgFontFace(s->vg, "sans-bold");
nvgFontSize(s->vg, 48*2.5); nvgFontSize(s->vg, 48*2.5);
nvgText(s->vg, alr_x+alr_w/2, alr_y+alr_h/2-45, scene->alert_text1, NULL); nvgText(s->vg, alr_x+alr_w/2, alr_y+alr_h/2-45, va_text1, NULL);
nvgFontFace(s->vg, "sans-regular"); nvgFontFace(s->vg, "sans-regular");
nvgFontSize(s->vg, 36*2.5); nvgFontSize(s->vg, 36*2.5);
nvgText(s->vg, alr_x+alr_w/2, alr_y+alr_h/2+75, scene->alert_text2, NULL); nvgText(s->vg, alr_x+alr_w/2, alr_y+alr_h/2+75, va_text2, NULL);
} else if (s->alert_size == ALERTSIZE_FULL) { } else if (va_size== ALERTSIZE_FULL) {
nvgFontSize(s->vg, (longAlert1?72:96)*2.5); nvgFontSize(s->vg, (longAlert1?72:96)*2.5);
nvgFontFace(s->vg, "sans-bold"); nvgFontFace(s->vg, "sans-bold");
nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_MIDDLE); nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_MIDDLE);
nvgTextBox(s->vg, alr_x, alr_y+(longAlert1?360:420), alr_w-60, scene->alert_text1, NULL); nvgTextBox(s->vg, alr_x, alr_y+(longAlert1?360:420), alr_w-60, va_text1, NULL);
nvgFontSize(s->vg, 48*2.5); nvgFontSize(s->vg, 48*2.5);
nvgFontFace(s->vg, "sans-regular"); nvgFontFace(s->vg, "sans-regular");
nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BOTTOM); nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BOTTOM);
nvgTextBox(s->vg, alr_x, alr_h-(longAlert1?300:360), alr_w-60, scene->alert_text2, NULL); nvgTextBox(s->vg, alr_x, alr_h-(longAlert1?300:360), alr_w-60, va_text2, NULL);
} }
} }
static void ui_draw_calibration_status(UIState *s) {
const UIScene *scene = &s->scene;
char calib_str1[64];
char calib_str2[64];
snprintf(calib_str1, sizeof(calib_str1), "Calibration in Progress: %d%%", scene->cal_perc);
snprintf(calib_str2, sizeof(calib_str2), (s->is_metric?"Drive above 72 km/h":"Drive above 45 mph"));
ui_draw_vision_alert(s, ALERTSIZE_MID, s->status, calib_str1, calib_str2);
}
static void ui_draw_vision(UIState *s) { static void ui_draw_vision(UIState *s) {
const UIScene *scene = &s->scene; const UIScene *scene = &s->scene;
int ui_viz_rx = scene->ui_viz_rx; int ui_viz_rx = scene->ui_viz_rx;
int ui_viz_rw = scene->ui_viz_rw; int ui_viz_rw = scene->ui_viz_rw;
int ui_viz_ro = scene->ui_viz_ro;
glClearColor(0.0, 0.0, 0.0, 0.0); glClearColor(0.0, 0.0, 0.0, 0.0);
glClear(GL_STENCIL_BUFFER_BIT | GL_COLOR_BUFFER_BIT); glClear(GL_STENCIL_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
// scissor for EON UI // Draw video frames
glEnable(GL_SCISSOR_TEST); glEnable(GL_SCISSOR_TEST);
glScissor(ui_viz_rx, s->fb_h-(viz_y+viz_h), ui_viz_rw, viz_h); glViewport(ui_viz_rx+ui_viz_ro, s->fb_h-(box_y+box_h), viz_w, box_h);
glViewport(ui_viz_rx, s->fb_h-(viz_y+viz_h), ui_viz_rw, viz_h); glScissor(ui_viz_rx, s->fb_h-(box_y+box_h), ui_viz_rw, box_h);
draw_frame(s); draw_frame(s);
glViewport(0, 0, s->fb_w, s->fb_h); glViewport(0, 0, s->fb_w, s->fb_h);
glDisable(GL_SCISSOR_TEST); glDisable(GL_SCISSOR_TEST);
@ -1330,72 +1283,39 @@ static void ui_draw_vision(UIState *s) {
nvgBeginFrame(s->vg, s->fb_w, s->fb_h, 1.0f); nvgBeginFrame(s->vg, s->fb_w, s->fb_h, 1.0f);
nvgSave(s->vg); nvgSave(s->vg);
// hack for eon // Draw augmented elements
const int inner_height = ui_viz_rw*9/16; const int inner_height = viz_w*9/16;
nvgScissor(s->vg, ui_viz_rx, viz_y, ui_viz_rw, viz_h); nvgScissor(s->vg, ui_viz_rx, box_y, ui_viz_rw, box_h);
nvgTranslate(s->vg, ui_viz_rx, viz_y + (viz_h-inner_height)/2.0); nvgTranslate(s->vg, ui_viz_rx+ui_viz_ro, box_y + (box_h-inner_height)/2.0);
nvgScale(s->vg, (float)ui_viz_rw / s->fb_w, (float)inner_height / s->fb_h); nvgScale(s->vg, (float)viz_w / s->fb_w, (float)inner_height / s->fb_h);
if (!scene->frontview) { if (!scene->frontview) {
ui_draw_world(s); ui_draw_world(s);
if (scene->lead_status) { if (scene->lead_status) {
draw_cross(s, scene->lead_d_rel + 2.7, scene->lead_y_rel, 30, draw_chevron(s, scene->lead_d_rel+2.7, scene->lead_y_rel, 30,
nvgRGBA(255, 0, 0, 128)); nvgRGBA(201, 34, 49, 255), nvgRGBA(218, 202, 37, 255));
} }
} }
nvgRestore(s->vg); nvgRestore(s->vg);
// Set Speed, Current Speed, Status/Events
ui_draw_vision_header(s); ui_draw_vision_header(s);
if (scene->cal_status == CALIBRATION_UNCALIBRATED) {
ui_draw_calibration_status(s);
}
ui_draw_vision_alert(s);
nvgEndFrame(s->vg);
glDisable(GL_BLEND);
}
static void ui_draw_alerts(UIState *s) {
const UIScene *scene = &s->scene;
if (!ui_alert_active(s)) return;
assert(s->status < ARRAYSIZE(alert_colors));
const uint8_t *color = alert_colors[s->status];
char alert_text1_upper[1024] = {0}; if (s->scene.alert_size != ALERTSIZE_NONE) {
for (int i=0; scene->alert_text1[i] && i < sizeof(alert_text1_upper)-1; i++) { // Controls Alerts
alert_text1_upper[i] = toupper(scene->alert_text1[i]); ui_draw_vision_alert(s, s->scene.alert_size, s->status,
} s->scene.alert_text1, s->scene.alert_text2);
nvgBeginPath(s->vg);
nvgRect(s->vg, box_x, box_y, box_w, box_h);
nvgFillColor(s->vg, nvgRGBA(color[0], color[1], color[2], color[3]));
nvgFill(s->vg);
nvgFontFace(s->vg, "sans-semibold");
if (strlen(alert_text1_upper) > 15) {
nvgFontSize(s->vg, 72.0*2.5);
} else { } else {
nvgFontSize(s->vg, 96.0*2.5); // Calibration Status
if (scene->cal_status == CALIBRATION_UNCALIBRATED) {
ui_draw_calibration_status(s);
}
} }
nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255));
nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_MIDDLE);
nvgTextBox(s->vg, box_x + 50, box_y + 287, box_w - 50, alert_text1_upper, NULL);
if (strlen(scene->alert_text2) > 0) { nvgEndFrame(s->vg);
glDisable(GL_BLEND);
nvgFontFace(s->vg, "sans-regular");
nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255));
nvgFontSize(s->vg, 44.0*2.5);
nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BOTTOM);
nvgTextBox(s->vg, box_x + 50, box_y + box_h - 250, box_w - 50, scene->alert_text2, NULL);
}
} }
static void ui_draw_blank(UIState *s) { static void ui_draw_blank(UIState *s) {
@ -1453,11 +1373,6 @@ static void ui_draw(UIState *s) {
nvgBeginFrame(s->vg, s->fb_w, s->fb_h, 1.0f); nvgBeginFrame(s->vg, s->fb_w, s->fb_h, 1.0f);
if (s->vision_connected && !s->scene.uilayout_sidebarcollapsed) {
ui_draw_aside(s);
}
// ui_draw_alerts(s);
nvgEndFrame(s->vg); nvgEndFrame(s->vg);
glDisable(GL_BLEND); glDisable(GL_BLEND);
} }
@ -1553,8 +1468,7 @@ static void ui_update(UIState *s) {
// Default UI Measurements (Assumes sidebar visible) // Default UI Measurements (Assumes sidebar visible)
s->scene.ui_viz_rx = box_x; s->scene.ui_viz_rx = box_x;
s->scene.ui_viz_rw = box_w; s->scene.ui_viz_rw = box_w;
s->scene.ui_frame_offset = -(sbr_w - 4*bdr_s); s->scene.ui_viz_ro = -(sbr_w - 4*bdr_s);
s->scene.ui_world_offset = -(sbr_w - 6*bdr_s);
s->vision_connect_firstrun = false; s->vision_connect_firstrun = false;
} }
@ -1814,13 +1728,13 @@ static void ui_update(UIState *s) {
bool hasSidebar = !s->scene.uilayout_sidebarcollapsed; bool hasSidebar = !s->scene.uilayout_sidebarcollapsed;
bool mapEnabled = s->scene.uilayout_mapenabled; bool mapEnabled = s->scene.uilayout_mapenabled;
if (mapEnabled) { if (mapEnabled) {
s->scene.ui_viz_rx = hasSidebar ? viz_x : (viz_x-(bdr_s*2)); s->scene.ui_viz_rx = hasSidebar ? (box_x+nav_w) : (box_x+nav_w-(bdr_s*4));
s->scene.ui_viz_rw = hasSidebar ? viz_w : (viz_w+(bdr_s*2)); s->scene.ui_viz_rw = hasSidebar ? (box_w-nav_w) : (box_w-nav_w+(bdr_s*4));
s->scene.ui_viz_ro = -(sbr_w + 4*bdr_s);
} else { } else {
s->scene.ui_viz_rx = hasSidebar ? box_x : (box_x-sbr_w+bdr_s*2); s->scene.ui_viz_rx = hasSidebar ? box_x : (box_x-sbr_w+bdr_s*2);
s->scene.ui_viz_rw = hasSidebar ? box_w : (box_w+sbr_w-(bdr_s*2)); s->scene.ui_viz_rw = hasSidebar ? box_w : (box_w+sbr_w-(bdr_s*2));
s->scene.ui_frame_offset = hasSidebar ? -(sbr_w - 4*bdr_s) : -(bdr_s*2); s->scene.ui_viz_ro = hasSidebar ? -(sbr_w - 6*bdr_s) : 0;
s->scene.ui_world_offset = hasSidebar ? -(sbr_w - 6*bdr_s) : -(bdr_s*2);
} }
} }
capn_free(&ctx); capn_free(&ctx);

29
selfdrive/updated.py
Unescape View File

@ -7,10 +7,14 @@ import time
import subprocess import subprocess
from common.basedir import BASEDIR from common.basedir import BASEDIR
from common.params import Params
from selfdrive.swaglog import cloudlog from selfdrive.swaglog import cloudlog
from selfdrive.version import dirty from selfdrive.version import dirty
NICE_LOW_PRIORITY = ["nice", "-n", "19"]
def main(gctx=None): def main(gctx=None):
params = Params()
while True: while True:
# try network # try network
r = subprocess.call(["ping", "-W", "4", "-c", "1", "8.8.8.8"]) r = subprocess.call(["ping", "-W", "4", "-c", "1", "8.8.8.8"])
@ -18,13 +22,19 @@ def main(gctx=None):
time.sleep(60) time.sleep(60)
continue continue
# If there are modifications we want to full history # If there are modifications we preserve full history
# otherwise only store head to save space # and disable update prompting.
# Otherwise, only store head to save space
if dirty: if dirty:
r = subprocess.call(["nice", "-n", "19", "git", "fetch", "--unshallow"]) r = subprocess.call(NICE_LOW_PRIORITY + ["git", "fetch", "--unshallow"])
is_update_available = False
else: else:
r = subprocess.call(["nice", "-n", "19", "git", "fetch", "--depth=1"]) r = subprocess.call(NICE_LOW_PRIORITY + ["git", "fetch", "--depth=1"])
is_update_available = check_is_update_available()
is_update_available_str = "1" if is_update_available else "0"
params.put("IsUpdateAvailable", is_update_available_str)
cloudlog.info("IsUpdateAvailable: %s", is_update_available_str)
cloudlog.info("git fetch: %r", r) cloudlog.info("git fetch: %r", r)
if r: if r:
time.sleep(60) time.sleep(60)
@ -36,5 +46,16 @@ def main(gctx=None):
time.sleep(60*60*3) time.sleep(60*60*3)
def check_is_update_available():
try:
local_rev = subprocess.check_output(NICE_LOW_PRIORITY + ["git", "rev-parse", "@"])
upstream_rev = subprocess.check_output(NICE_LOW_PRIORITY + ["git", "rev-parse", "@{u}"])
return upstream_rev != local_rev
except subprocess.CalledProcessError:
cloudlog.exception("updated: failed to compare local and upstream")
return False
if __name__ == "__main__": if __name__ == "__main__":
main() main()

BIN
selfdrive/visiond/visiond
View File

Binary file not shown.
Write Preview
Loading…
Cancel
Save