mawei
/
tsl
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

openpilot v0.5.13 release 

old-commit-hash: dd34ccfe288ebda8e2568cf550994ae890379f45
This commit is contained in:
Vehicle Researcher 2019-06-06 04:38:45 +00:00
parent 86fb001d62
commit e47a2e6e30
106 changed files with 3754 additions and 1927 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
.gitignore vendored
View File

@ -34,3 +34,6 @@ selfdrive/visiond/visiond
/src/
one
openpilot
xx

17
README.md
View File

@ -92,8 +92,6 @@ Supported Cars
| Kia | Sorento 2018 | All | Yes | Stock | 0mph | 0mph | Custom<sup>6</sup>|
| Kia | Stinger 2018 | SCC + LKAS | Yes | Stock | 0mph | 0mph | Custom<sup>6</sup>|
| Lexus | RX Hybrid 2016-19 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
| Subaru | Crosstrek 2018 | EyeSight | Yes | Stock | 0mph | 0mph | Subaru |
| Subaru | Impreza 2019 | EyeSight | Yes | Stock | 0mph | 0mph | Subaru |
| Toyota | Avalon 2016 | TSS-P | Yes | Yes<sup>2</sup>| 20mph<sup>1</sup>| 0mph | Toyota |
| Toyota | Camry 2018<sup>4</sup> | All | Yes | Stock | 0mph<sup>5</sup> | 0mph | Toyota |
| Toyota | C-HR 2017-18<sup>4</sup> | All | Yes | Stock | 0mph | 0mph | Toyota |
@ -109,13 +107,13 @@ Supported Cars
| Toyota | Rav4 2019 | All | Yes | Yes | 0mph | 0mph | Toyota |
| Toyota | Rav4 Hybrid 2017-18 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
<sup>1</sup>[Comma Pedal](https://community.comma.ai/wiki/index.php/Comma_Pedal) is used to provide stop-and-go capability to some of the openpilot-supported cars that don't currently support stop-and-go. Here is how to [build a Comma Pedal](https://medium.com/@jfrux/comma-pedal-building-with-macrofab-6328bea791e8). ***NOTE: The Comma Pedal is not officially supported by [comma.ai](https://comma.ai)***
<sup>2</sup>When disconnecting the Driver Support Unit (DSU), otherwise longitudinal control is stock ACC. For DSU locations, see [Toyota Wiki page](https://community.comma.ai/wiki/index.php/Toyota)
<sup>3</sup>[GM installation guide](https://zoneos.com/volt/).
<sup>4</sup>It needs an extra 120Ohm resistor ([pic1](https://i.imgur.com/CmdKtTP.jpg), [pic2](https://i.imgur.com/s2etUo6.jpg)) on bus 3 and giraffe switches set to 01X1 (11X1 for stock LKAS), where X depends on if you have the [comma power](https://comma.ai/shop/products/power/).
<sup>5</sup>28mph for Camry 4CYL L, 4CYL LE and 4CYL SE which don't have Full-Speed Range Dynamic Radar Cruise Control.
<sup>6</sup>Open sourced [Hyundai Giraffe](https://github.com/commaai/neo/tree/master/giraffe/hyundai) is designed for the 2019 Sante Fe; pinout may differ for other Hyundais.
<sup>7</sup>Community built Giraffe, find more information [here](https://zoneos.com/shop/).
<sup>1</sup>[Comma Pedal](https://community.comma.ai/wiki/index.php/Comma_Pedal) is used to provide stop-and-go capability to some of the openpilot-supported cars that don't currently support stop-and-go. Here is how to [build a Comma Pedal](https://medium.com/@jfrux/comma-pedal-building-with-macrofab-6328bea791e8). ***NOTE: The Comma Pedal is not officially supported by [comma.ai](https://comma.ai)***
<sup>2</sup>When disconnecting the Driver Support Unit (DSU), otherwise longitudinal control is stock ACC. For DSU locations, see [Toyota Wiki page](https://community.comma.ai/wiki/index.php/Toyota)
<sup>3</sup>[GM installation guide](https://zoneos.com/volt/).
<sup>4</sup>It needs an extra 120Ohm resistor ([pic1](https://i.imgur.com/CmdKtTP.jpg), [pic2](https://i.imgur.com/s2etUo6.jpg)) on bus 3 and giraffe switches set to 01X1 (11X1 for stock LKAS), where X depends on if you have the [comma power](https://comma.ai/shop/products/power/).
<sup>5</sup>28mph for Camry 4CYL L, 4CYL LE and 4CYL SE which don't have Full-Speed Range Dynamic Radar Cruise Control.
<sup>6</sup>Open sourced [Hyundai Giraffe](https://github.com/commaai/neo/tree/master/giraffe/hyundai) is designed for the 2019 Sante Fe; pinout may differ for other Hyundais.
<sup>7</sup>Community built Giraffe, find more information [here](https://zoneos.com/shop/).
Community Maintained Cars
------
@ -173,7 +171,6 @@ Directory structure
├── locationd # Soon to be home of precise location
├── logcatd # Android logcat as a service
├── loggerd # Logger and uploader of car data
├── mapd # Fetches map data and computes next global path
├── proclogd # Logs information from proc
├── sensord # IMU / GPS interface code
├── test # Car simulator running code through virtual maneuvers

10
RELEASES.md
View File

@ -1,3 +1,13 @@
Version 0.5.13 (2019-05-31)
==========================
* Reduce panda power consumption by 70%, down to 80mW, when car is off (not for GM)
* Reduce EON power consumption by 40%, down to 1100mW, when car is off
* Reduce CPU utilization by 20% and improve stability
* Temporarily remove mapd functionalities to improve stability
* Add openpilot record-only mode for unsupported cars
* Synchronize controlsd to boardd to reduce latency
* Remove panda support for Subaru giraffe
Version 0.5.12 (2019-05-16)
==========================
* Improve lateral control for the Prius and Prius Prime

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

@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:1c82f4ea28008aad9a25cd434e5f817c84d621423739e7bca814f4dcff2b9714
size 18376958
oid sha256:6976978d140b928a499f26a6e8d879a752034a762154dde2d9bde55ef59a9c29
size 18310604

4
cereal/car.capnp
View File

@ -74,6 +74,7 @@ struct CarEvent @0x9b1657f34caf3ad3 {
invalidGiraffeHonda @49;
vehicleModelInvalid @50;
controlsFailed @51;
sensorDataInvalid @52;
}
}
@ -175,7 +176,7 @@ struct CarState {
# ******* radar state @ 20hz *******
struct RadarState {
struct RadarData @0x888ad6581cf0aacb {
errors @0 :List(Error);
points @1 :List(RadarPoint);
@ -333,6 +334,7 @@ struct CarParams {
steerActuatorDelay @36 :Float32; # Steering wheel actuator delay in seconds
openpilotLongitudinalControl @37 :Bool; # is openpilot doing the longitudinal control?
carVin @38 :Text; # VIN number queried during fingerprinting
struct LateralPIDTuning {
kpBP @0 :List(Float32);

23
cereal/log.capnp
View File

@ -305,12 +305,12 @@ struct LiveUI {
awarenessStatus @3 :Float32;
}
struct Live20Data {
struct RadarState @0x9a185389d6fdd05f {
canMonoTimes @10 :List(UInt64);
mdMonoTime @6 :UInt64;
ftMonoTimeDEPRECATED @7 :UInt64;
l100MonoTime @11 :UInt64;
radarErrors @12 :List(Car.RadarState.Error);
controlsStateMonoTime @11 :UInt64;
radarErrors @12 :List(Car.RadarData.Error);
# all deprecated
warpMatrixDEPRECATED @0 :List(Float32);
@ -368,15 +368,15 @@ struct LiveTracks {
oncoming @9 :Bool;
}
struct Live100Data {
struct ControlsState @0x97ff69c53601abf1 {
canMonoTimeDEPRECATED @16 :UInt64;
canMonoTimes @21 :List(UInt64);
l20MonoTimeDEPRECATED @17 :UInt64;
radarStateMonoTimeDEPRECATED @17 :UInt64;
mdMonoTimeDEPRECATED @18 :UInt64;
planMonoTime @28 :UInt64;
pathPlanMonoTime @50 :UInt64;
state @31 :ControlState;
state @31 :OpenpilotState;
vEgo @0 :Float32;
vEgoRaw @32 :Float32;
aEgoDEPRECATED @1 :Float32;
@ -433,7 +433,7 @@ struct Live100Data {
pidState @53 :LateralPIDState;
}
enum ControlState {
enum OpenpilotState @0xdbe58b96d2d1ac61 {
disabled @0;
preEnabled @1;
enabled @2;
@ -507,6 +507,7 @@ struct ModelData {
points @0 :List(Float32);
prob @1 :Float32;
std @2 :Float32;
stds @3 :List(Float32);
}
struct LeadData {
@ -574,7 +575,7 @@ struct LogRotate {
struct Plan {
mdMonoTime @9 :UInt64;
l20MonoTime @10 :UInt64;
radarStateMonoTime @10 :UInt64;
eventsDEPRECATED @13 :List(Car.CarEvent);
# lateral, 3rd order polynomial
@ -648,6 +649,7 @@ struct PathPlan {
paramsValid @10 :Bool;
modelValid @12 :Bool;
angleOffset @11 :Float32;
sensorValid @14 :Bool;
}
struct LiveLocationData {
@ -1643,6 +1645,7 @@ struct LiveParametersData {
angleOffsetAverage @3 :Float32;
stiffnessFactor @4 :Float32;
steerRatio @5 :Float32;
sensorValid @6 :Bool;
}
struct LiveMapData {
@ -1690,13 +1693,13 @@ struct Event {
sensorEventDEPRECATED @4 :SensorEventData;
can @5 :List(CanData);
thermal @6 :ThermalData;
live100 @7 :Live100Data;
controlsState @7 :ControlsState;
liveEventDEPRECATED @8 :List(LiveEventData);
model @9 :ModelData;
features @10 :CalibrationFeatures;
sensorEvents @11 :List(SensorEventData);
health @12 :HealthData;
live20 @13 :Live20Data;
radarState @13 :RadarState;
liveUIDEPRECATED @14 :LiveUI;
encodeIdx @15 :EncodeIndex;
liveTracks @16 :List(LiveTracks);

16
common/clock.pyx Normal file
View File

@ -0,0 +1,16 @@
from posix.time cimport clock_gettime, timespec, CLOCK_BOOTTIME, CLOCK_MONOTONIC_RAW
cdef double readclock(int clock_id):
cdef timespec ts
cdef double current
clock_gettime(clock_id, &ts)
current = ts.tv_sec + (ts.tv_nsec / 1000000000.)
return current
def monotonic_time():
return readclock(CLOCK_MONOTONIC_RAW)
def sec_since_boot():
return readclock(CLOCK_BOOTTIME)

103
common/file_helpers.py Normal file
View File

@ -0,0 +1,103 @@
import os
import shutil
import tempfile
from atomicwrites import AtomicWriter
def mkdirs_exists_ok(path):
try:
os.makedirs(path)
except OSError:
if not os.path.isdir(path):
raise
def rm_not_exists_ok(path):
try:
os.remove(path)
except OSError:
if os.path.exists(path):
raise
def rm_tree_or_link(path):
if os.path.islink(path):
os.unlink(path)
elif os.path.isdir(path):
shutil.rmtree(path)
def get_tmpdir_on_same_filesystem(path):
# TODO(mgraczyk): HACK, we should actually check for which filesystem.
normpath = os.path.normpath(path)
parts = normpath.split("/")
if len(parts) > 1:
if parts[1].startswith("raid"):
if len(parts) > 2 and parts[2] == "runner":
return "/{}/runner/tmp".format(parts[1])
elif len(parts) > 2 and parts[2] == "aws":
return "/{}/aws/tmp".format(parts[1])
else:
return "/{}/tmp".format(parts[1])
elif parts[1] == "aws":
return "/aws/tmp"
elif parts[1] == "scratch":
return "/scratch/tmp"
return "/tmp"
class AutoMoveTempdir(object):
def __init__(self, target_path, temp_dir=None):
self._target_path = target_path
self._path = tempfile.mkdtemp(dir=temp_dir)
@property
def name(self):
return self._path
def close(self):
os.rename(self._path, self._target_path)
def __enter__(self): return self
def __exit__(self, type, value, traceback):
if type is None:
self.close()
else:
shutil.rmtree(self._path)
class NamedTemporaryDir(object):
def __init__(self, temp_dir=None):
self._path = tempfile.mkdtemp(dir=temp_dir)
@property
def name(self):
return self._path
def close(self):
shutil.rmtree(self._path)
def __enter__(self): return self
def __exit__(self, type, value, traceback):
self.close()
def _get_fileobject_func(writer, temp_dir):
def _get_fileobject():
file_obj = writer.get_fileobject(dir=temp_dir)
os.chmod(file_obj.name, 0o644)
return file_obj
return _get_fileobject
def atomic_write_on_fs_tmp(path, **kwargs):
"""Creates an atomic writer using a temporary file in a temporary directory
on the same filesystem as path.
"""
# TODO(mgraczyk): This use of AtomicWriter relies on implementation details to set the temp
# directory.
writer = AtomicWriter(path, **kwargs)
return writer._open(_get_fileobject_func(writer, get_tmpdir_on_same_filesystem(path)))
def atomic_write_in_dir(path, **kwargs):
"""Creates an atomic writer using a temporary file in the same directory
as the destination file.
"""
writer = AtomicWriter(path, **kwargs)
return writer._open(_get_fileobject_func(writer, os.path.dirname(path)))

61
common/params.py
View File

@ -41,7 +41,7 @@ def mkdirs_exists_ok(path):
class TxType(Enum):
PERSISTENT = 1
CLEAR_ON_MANAGER_START = 2
CLEAR_ON_CAR_START = 3
CLEAR_ON_PANDA_DISCONNECT = 3
class UnknownKeyName(Exception):
@ -49,32 +49,33 @@ class UnknownKeyName(Exception):
keys = {
"AccessToken": TxType.PERSISTENT,
"CalibrationParams": TxType.PERSISTENT,
"CarParams": TxType.CLEAR_ON_CAR_START,
"CompletedTrainingVersion": TxType.PERSISTENT,
"ControlsParams": TxType.PERSISTENT,
"DoUninstall": TxType.CLEAR_ON_MANAGER_START,
"DongleId": TxType.PERSISTENT,
"GitBranch": TxType.PERSISTENT,
"GitCommit": TxType.PERSISTENT,
"GitRemote": TxType.PERSISTENT,
"HasAcceptedTerms": TxType.PERSISTENT,
"IsDriverMonitoringEnabled": TxType.PERSISTENT,
"IsFcwEnabled": TxType.PERSISTENT,
"IsGeofenceEnabled": TxType.PERSISTENT,
"IsMetric": TxType.PERSISTENT,
"IsUpdateAvailable": TxType.PERSISTENT,
"IsUploadVideoOverCellularEnabled": TxType.PERSISTENT,
"LimitSetSpeed": TxType.PERSISTENT,
"LiveParameters": TxType.PERSISTENT,
"LongitudinalControl": TxType.PERSISTENT,
"Passive": TxType.PERSISTENT,
"RecordFront": TxType.PERSISTENT,
"ShouldDoUpdate": TxType.CLEAR_ON_MANAGER_START,
"SpeedLimitOffset": TxType.PERSISTENT,
"TrainingVersion": TxType.PERSISTENT,
"Version": TxType.PERSISTENT,
"AccessToken": [TxType.PERSISTENT],
"CalibrationParams": [TxType.PERSISTENT],
"CarParams": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT],
"CompletedTrainingVersion": [TxType.PERSISTENT],
"ControlsParams": [TxType.PERSISTENT],
"DoUninstall": [TxType.CLEAR_ON_MANAGER_START],
"DongleId": [TxType.PERSISTENT],
"GitBranch": [TxType.PERSISTENT],
"GitCommit": [TxType.PERSISTENT],
"GitRemote": [TxType.PERSISTENT],
"HasAcceptedTerms": [TxType.PERSISTENT],
"IsDriverMonitoringEnabled": [TxType.PERSISTENT],
"IsFcwEnabled": [TxType.PERSISTENT],
"IsGeofenceEnabled": [TxType.PERSISTENT],
"IsMetric": [TxType.PERSISTENT],
"IsUpdateAvailable": [TxType.PERSISTENT],
"IsUploadVideoOverCellularEnabled": [TxType.PERSISTENT],
"LimitSetSpeed": [TxType.PERSISTENT],
"LiveParameters": [TxType.PERSISTENT],
"LongitudinalControl": [TxType.PERSISTENT],
"Passive": [TxType.PERSISTENT],
"RecordFront": [TxType.PERSISTENT],
"ShouldDoUpdate": [TxType.CLEAR_ON_MANAGER_START],
"SpeedLimitOffset": [TxType.PERSISTENT],
"SubscriberInfo": [TxType.PERSISTENT],
"TrainingVersion": [TxType.PERSISTENT],
"Version": [TxType.PERSISTENT],
}
@ -308,14 +309,14 @@ class Params(object):
def _clear_keys_with_type(self, tx_type):
with self.transaction(write=True) as txn:
for key in keys:
if keys[key] == tx_type:
if tx_type in keys[key]:
txn.delete(key)
def manager_start(self):
self._clear_keys_with_type(TxType.CLEAR_ON_MANAGER_START)
def car_start(self):
self._clear_keys_with_type(TxType.CLEAR_ON_CAR_START)
def panda_disconnect(self):
self._clear_keys_with_type(TxType.CLEAR_ON_PANDA_DISCONNECT)
def delete(self, key):
with self.transaction(write=True) as txn:

59
common/realtime.py
View File

@ -2,58 +2,24 @@
import os
import time
import platform
import threading
import subprocess
import multiprocessing
from cffi import FFI
# Build and load cython module
import pyximport
installer = pyximport.install(inplace=True, build_dir='/tmp')
from common.clock import monotonic_time, sec_since_boot # pylint: disable=no-name-in-module, import-error
pyximport.uninstall(*installer)
assert monotonic_time
assert sec_since_boot
ffi = FFI()
ffi.cdef("""
typedef int clockid_t;
struct timespec {
long tv_sec; /* Seconds. */
long tv_nsec; /* Nanoseconds. */
};
int clock_gettime (clockid_t clk_id, struct timespec *tp);
long syscall(long number, ...);
"""
)
ffi.cdef("long syscall(long number, ...);")
libc = ffi.dlopen(None)
# see <linux/time.h>
CLOCK_MONOTONIC_RAW = 4
CLOCK_BOOTTIME = 7
if platform.system() != 'Darwin' and hasattr(libc, 'clock_gettime'):
c_clock_gettime = libc.clock_gettime
tlocal = threading.local()
def clock_gettime(clk_id):
if not hasattr(tlocal, 'ts'):
tlocal.ts = ffi.new('struct timespec *')
ts = tlocal.ts
r = c_clock_gettime(clk_id, ts)
if r != 0:
raise OSError("clock_gettime")
return ts.tv_sec + ts.tv_nsec * 1e-9
else:
# hack. only for OS X < 10.12
def clock_gettime(clk_id):
return time.time()
def monotonic_time():
return clock_gettime(CLOCK_MONOTONIC_RAW)
def sec_since_boot():
return clock_gettime(CLOCK_BOOTTIME)
def set_realtime_priority(level):
if os.getuid() != 0:
print("not setting priority, not root")
@ -99,10 +65,9 @@ class Ratekeeper(object):
lagged = False
remaining = self._next_frame_time - sec_since_boot()
self._next_frame_time += self._interval
if remaining < -self._print_delay_threshold:
if self._print_delay_threshold is not None and remaining < -self._print_delay_threshold:
print("%s lagging by %.2f ms" % (self._process_name, -remaining * 1000))
lagged = True
self._frame += 1
self._remaining = remaining
return lagged

95
common/vin.py Executable file
View File

@ -0,0 +1,95 @@
#!/usr/bin/env python
import time
from common.realtime import sec_since_boot
import selfdrive.messaging as messaging
from selfdrive.boardd.boardd import can_list_to_can_capnp
def get_vin(logcan, sendcan):
# works on standard 11-bit addresses for diagnostic. Tested on Toyota and Subaru;
# Honda uses the extended 29-bit addresses, and unfortunately only works from OBDII
query_msg = [[0x7df, 0, '\x02\x09\x02'.ljust(8, "\x00"), 0],
[0x7e0, 0, '\x30'.ljust(8, "\x00"), 0]]
cnts = [1, 2] # Number of messages to wait for at each iteration
vin_valid = True
dat = []
for i in range(len(query_msg)):
cnt = 0
sendcan.send(can_list_to_can_capnp([query_msg[i]], msgtype='sendcan'))
got_response = False
t_start = sec_since_boot()
while sec_since_boot() - t_start < 0.05 and not got_response:
for a in messaging.drain_sock(logcan):
for can in a.can:
if can.src == 0 and can.address == 0x7e8:
vin_valid = vin_valid and is_vin_response_valid(can.dat, i, cnt)
dat += can.dat[2:] if i == 0 else can.dat[1:]
cnt += 1
if cnt == cnts[i]:
got_response = True
time.sleep(0.01)
return "".join(dat[3:]) if vin_valid else ""
"""
if 'vin' not in gctx:
print "getting vin"
gctx['vin'] = query_vin()[3:]
print "got VIN %s" % (gctx['vin'],)
cloudlog.info("got VIN %s" % (gctx['vin'],))
# *** determine platform based on VIN ****
if vin.startswith("19UDE2F36G"):
print "ACURA ILX 2016"
self.civic = False
else:
# TODO: add Honda check explicitly
print "HONDA CIVIC 2016"
self.civic = True
# *** special case VIN of Acura test platform
if vin == "19UDE2F36GA001322":
print "comma.ai test platform detected"
# it has a gas interceptor and a torque mod
self.torque_mod = True
"""
# sanity checks on response messages from vin query
def is_vin_response_valid(can_dat, step, cnt):
can_dat = [ord(i) for i in can_dat]
if len(can_dat) != 8:
# ISO-TP meesages are all 8 bytes
return False
if step == 0:
# VIN does not fit in a single message and it's 20 bytes of data
if can_dat[0] != 0x10 or can_dat[1] != 0x14:
return False
if step == 1 and cnt == 0:
# first response after a CONTINUE query is sent
if can_dat[0] != 0x21:
return False
if step == 1 and cnt == 1:
# second response after a CONTINUE query is sent
if can_dat[0] != 0x22:
return False
return True
if __name__ == "__main__":
import zmq
from selfdrive.services import service_list
context = zmq.Context()
logcan = messaging.sub_sock(context, service_list['can'].port)
sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
time.sleep(1.) # give time to sendcan socket to start
print get_vin(logcan, sendcan)

2
installer/updater/updater
View File

@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:4e8c704ed800b27d8ac0dda64aafcbc93239012ab65f3ac7540db5965844b2f0
oid sha256:206205b51ce5bfd3c387cb961302879b72d1e1ea3e81bb8b59dbbb7109197142
size 2501608

1
requirements_openpilot.txt
View File

@ -4,6 +4,7 @@
Cython==0.27.3
Flask==1.0.2
#PyGObject==3.28.2 This is installed on the EON, but requires a ton of dependencies to install
PyJWT==1.4.1
PyYAML==3.12
appdirs==1.4.0
atomicwrites==1.1.5

2
selfdrive/boardd/Makefile
View File

@ -64,7 +64,7 @@ boardd: $(OBJS)
boardd.o: boardd.cc
@echo "[ CXX ] $@"
$(CXX) $(CXXFLAGS) \
$(CXX) $(CXXFLAGS) -MMD \
-I$(PHONELIBS)/android_system_core/include \
$(CEREAL_CFLAGS) \
$(CEREAL_CXXFLAGS) \

98
selfdrive/boardd/boardd.cc
View File

@ -21,6 +21,7 @@
#include "cereal/gen/cpp/log.capnp.h"
#include "cereal/gen/cpp/car.capnp.h"
#include "common/messaging.h"
#include "common/params.h"
#include "common/swaglog.h"
#include "common/timing.h"
@ -34,7 +35,6 @@
#define SAFETY_NOOUTPUT 0
#define SAFETY_HONDA 1
#define SAFETY_TOYOTA 2
#define SAFETY_ELM327 0xE327
#define SAFETY_GM 3
#define SAFETY_HONDA_BOSCH 4
#define SAFETY_FORD 5
@ -46,7 +46,7 @@
#define SAFETY_TOYOTA_IPAS 0x1335
#define SAFETY_TOYOTA_NOLIMITS 0x1336
#define SAFETY_ALLOUTPUT 0x1337
#define SAFETY_ELM327 0xE327
#define SAFETY_ELM327 0xE327 // diagnostic only
namespace {
@ -138,6 +138,9 @@ void *safety_setter_thread(void *s) {
// set in the mutex to avoid race
safety_setter_thread_handle = -1;
// set if long_control is allowed by openpilot. Hardcoded to True for now
libusb_control_transfer(dev_handle, 0x40, 0xdf, 1, 0, NULL, 0, TIMEOUT);
libusb_control_transfer(dev_handle, 0x40, 0xdc, safety_setting, safety_param, NULL, 0, TIMEOUT);
pthread_mutex_unlock(&usb_lock);
@ -173,12 +176,8 @@ bool usb_connect() {
LOGW("not enabling charging on x86_64");
#endif
// no output is the default
if (getenv("RECVMOCK")) {
libusb_control_transfer(dev_handle, 0x40, 0xdc, SAFETY_ELM327, 0, NULL, 0, TIMEOUT);
} else {
libusb_control_transfer(dev_handle, 0x40, 0xdc, SAFETY_NOOUTPUT, 0, NULL, 0, TIMEOUT);
}
// diagnostic only is the default, needed for VIN query
libusb_control_transfer(dev_handle, 0x40, 0xdc, SAFETY_ELM327, 0, NULL, 0, TIMEOUT);
if (safety_setter_thread_handle == -1) {
err = pthread_create(&safety_setter_thread_handle, NULL, safety_setter_thread, NULL);
@ -223,6 +222,8 @@ void can_recv(void *s) {
int recv;
uint32_t f1, f2;
uint64_t start_time = nanos_since_boot();
// do recv
pthread_mutex_lock(&usb_lock);
@ -245,12 +246,13 @@ void can_recv(void *s) {
// create message
capnp::MallocMessageBuilder msg;
cereal::Event::Builder event = msg.initRoot<cereal::Event>();
event.setLogMonoTime(nanos_since_boot());
event.setLogMonoTime(start_time);
size_t num_msg = recv / 0x10;
auto canData = event.initCan(recv/0x10);
auto canData = event.initCan(num_msg);
// populate message
for (int i = 0; i<(recv/0x10); i++) {
for (int i = 0; i < num_msg; i++) {
if (data[i*4] & 4) {
// extended
canData[i].setAddress(data[i*4] >> 3);
@ -380,59 +382,14 @@ void can_send(void *s) {
free(send);
}
// **** threads ****
void *thermal_thread(void *crap) {
int err;
LOGD("start thermal thread");
// thermal = 8005
void *context = zmq_ctx_new();
void *subscriber = zmq_socket(context, ZMQ_SUB);
zmq_setsockopt(subscriber, ZMQ_SUBSCRIBE, "", 0);
zmq_connect(subscriber, "tcp://127.0.0.1:8005");
// run as fast as messages come in
while (!do_exit) {
// recv from thermal
zmq_msg_t msg;
zmq_msg_init(&msg);
err = zmq_msg_recv(&msg, subscriber, 0);
assert(err >= 0);
// format for board, make copy due to alignment issues, will be freed on out of scope
// copied from send thread...
auto amsg = kj::heapArray<capnp::word>((zmq_msg_size(&msg) / sizeof(capnp::word)) + 1);
memcpy(amsg.begin(), zmq_msg_data(&msg), zmq_msg_size(&msg));
capnp::FlatArrayMessageReader cmsg(amsg);
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
uint16_t target_fan_speed = event.getThermal().getFanSpeed();
//LOGW("setting fan speed %d", target_fan_speed);
pthread_mutex_lock(&usb_lock);
libusb_control_transfer(dev_handle, 0xc0, 0xd3, target_fan_speed, 0, NULL, 0, TIMEOUT);
pthread_mutex_unlock(&usb_lock);
zmq_msg_close(&msg);
}
// turn the fan off when we exit
libusb_control_transfer(dev_handle, 0xc0, 0xd3, 0, 0, NULL, 0, TIMEOUT);
return NULL;
}
void *can_send_thread(void *crap) {
LOGD("start send thread");
// sendcan = 8017
void *context = zmq_ctx_new();
void *subscriber = zmq_socket(context, ZMQ_SUB);
zmq_setsockopt(subscriber, ZMQ_SUBSCRIBE, "", 0);
zmq_connect(subscriber, "tcp://127.0.0.1:8017");
void *subscriber = sub_sock(context, "tcp://127.0.0.1:8017");
// drain sendcan to delete any stale messages from previous runs
zmq_msg_t msg;
@ -457,11 +414,24 @@ void *can_recv_thread(void *crap) {
void *publisher = zmq_socket(context, ZMQ_PUB);
zmq_bind(publisher, "tcp://*:8006");
// run at ~200hz
// run at 100hz
const uint64_t dt = 10000000ULL;
uint64_t next_frame_time = nanos_since_boot() + dt;
while (!do_exit) {
can_recv(publisher);
// 5ms
usleep(5*1000);
uint64_t cur_time = nanos_since_boot();
int64_t remaining = next_frame_time - cur_time;
if (remaining > 0){
useconds_t sleep = remaining / 1000;
usleep(sleep);
} else {
LOGW("missed cycle");
next_frame_time = cur_time;
}
next_frame_time += dt;
}
return NULL;
}
@ -686,16 +656,8 @@ int main() {
can_recv_thread, NULL);
assert(err == 0);
pthread_t thermal_thread_handle;
err = pthread_create(&thermal_thread_handle, NULL,
thermal_thread, NULL);
assert(err == 0);
// join threads
err = pthread_join(thermal_thread_handle, NULL);
assert(err == 0);
err = pthread_join(can_recv_thread_handle, NULL);
assert(err == 0);

4
selfdrive/can/libdbc_py.py
View File

@ -78,9 +78,9 @@ typedef struct {
void* can_init(int bus, const char* dbc_name,
size_t num_message_options, const MessageParseOptions* message_options,
size_t num_signal_options, const SignalParseOptions* signal_options, bool sendcan,
const char* tcp_addr);
const char* tcp_addr, int timeout);
void can_update(void* can, uint64_t sec, bool wait);
int can_update(void* can, uint64_t sec, bool wait);
size_t can_query(void* can, uint64_t sec, bool *out_can_valid, size_t out_values_size, SignalValue* out_values);

22
selfdrive/can/parser.cc
View File

@ -190,12 +190,13 @@ class CANParser {
CANParser(int abus, const std::string& dbc_name,
const std::vector<MessageParseOptions> &options,
const std::vector<SignalParseOptions> &sigoptions,
bool sendcan, const std::string& tcp_addr)
bool sendcan, const std::string& tcp_addr, int timeout=-1)
: bus(abus) {
// connect to can on 8006
context = zmq_ctx_new();
subscriber = zmq_socket(context, ZMQ_SUB);
zmq_setsockopt(subscriber, ZMQ_SUBSCRIBE, "", 0);
zmq_setsockopt(subscriber, ZMQ_RCVTIMEO, &timeout, sizeof(int));
std::string tcp_addr_str;
@ -325,8 +326,9 @@ class CANParser {
}
}
void update(uint64_t sec, bool wait) {
int update(uint64_t sec, bool wait) {
int err;
int result = 0;
// recv from can
zmq_msg_t msg;
@ -338,6 +340,11 @@ class CANParser {
if (first) {
err = zmq_msg_recv(&msg, subscriber, 0);
first = false;
// When we timeout on the first message, return error
if (err < 0){
result = -1;
}
} else {
err = zmq_msg_recv(&msg, subscriber, ZMQ_DONTWAIT);
}
@ -352,13 +359,12 @@ class CANParser {
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
auto cans = event.getCan();
UpdateCans(sec, cans);
}
UpdateValid(sec);
zmq_msg_close(&msg);
return result;
}
std::vector<SignalValue> query(uint64_t sec) {
@ -401,18 +407,18 @@ extern "C" {
void* can_init(int bus, const char* dbc_name,
size_t num_message_options, const MessageParseOptions* message_options,
size_t num_signal_options, const SignalParseOptions* signal_options,
bool sendcan, const char* tcp_addr) {
bool sendcan, const char* tcp_addr, int timeout) {
CANParser* ret = new CANParser(bus, std::string(dbc_name),
(message_options ? std::vector<MessageParseOptions>(message_options, message_options+num_message_options)
: std::vector<MessageParseOptions>{}),
(signal_options ? std::vector<SignalParseOptions>(signal_options, signal_options+num_signal_options)
: std::vector<SignalParseOptions>{}), sendcan, std::string(tcp_addr));
: std::vector<SignalParseOptions>{}), sendcan, std::string(tcp_addr), timeout);
return (void*)ret;
}
void can_update(void* can, uint64_t sec, bool wait) {
int can_update(void* can, uint64_t sec, bool wait) {
CANParser* cp = (CANParser*)can;
cp->update(sec, wait);
return cp->update(sec, wait);
}
size_t can_query(void* can, uint64_t sec, bool *out_can_valid, size_t out_values_size, SignalValue* out_values) {

10
selfdrive/can/parser.py
View File

@ -6,7 +6,7 @@ from selfdrive.can.libdbc_py import libdbc, ffi
class CANParser(object):
def __init__(self, dbc_name, signals, checks=None, bus=0, sendcan=False, tcp_addr="127.0.0.1"):
def __init__(self, dbc_name, signals, checks=None, bus=0, sendcan=False, tcp_addr="127.0.0.1", timeout=-1):
if checks is None:
checks = []
@ -61,7 +61,7 @@ class CANParser(object):
} for msg_address, freq in message_options.items()])
self.can = libdbc.can_init(bus, dbc_name, len(message_options_c), message_options_c,
len(signal_options_c), signal_options_c, sendcan, tcp_addr)
len(signal_options_c), signal_options_c, sendcan, tcp_addr, timeout)
self.p_can_valid = ffi.new("bool*")
@ -94,8 +94,10 @@ class CANParser(object):
return ret
def update(self, sec, wait):
libdbc.can_update(self.can, sec, wait)
return self.update_vl(sec)
"""Returns if the update was successfull (e.g. no rcv timeout happened)"""
r = libdbc.can_update(self.can, sec, wait)
return bool(r >= 0), self.update_vl(sec)
class CANDefine(object):
def __init__(self, dbc_name):

5
selfdrive/can/tests/test_packer_honda.py
View File

@ -41,8 +41,9 @@ class TestPackerMethods(unittest.TestCase):
random.randint(0, 65536), random.randint(0, 65536), random.randint(0, 65536))
car_fingerprint = HONDA_BOSCH[0]
idx = random.randint(0, 65536)
m_old = hondacan.create_ui_commands(self.honda_cp_old, pcm_speed, hud, car_fingerprint, idx)
m = hondacan.create_ui_commands(self.honda_cp, pcm_speed, hud, car_fingerprint, idx)
is_metric = (random.randint(0, 2) % 2 == 0)
m_old = hondacan.create_ui_commands(self.honda_cp_old, pcm_speed, hud, car_fingerprint, is_metric, idx)
m = hondacan.create_ui_commands(self.honda_cp, pcm_speed, hud, car_fingerprint, is_metric, idx)
self.assertEqual(m_old, m)
button_val = random.randint(0, 65536)

134
selfdrive/car/car_helpers.py
View File

@ -1,20 +1,34 @@
import os
import time
from common.vin import is_vin_response_valid
from common.basedir import BASEDIR
from common.realtime import sec_since_boot
from common.fingerprints import eliminate_incompatible_cars, all_known_cars
from selfdrive.boardd.boardd import can_list_to_can_capnp
from selfdrive.swaglog import cloudlog
import selfdrive.messaging as messaging
def load_interfaces(x):
def get_startup_alert(car_recognized, controller_available):
alert = 'startup'
if not car_recognized:
alert = 'startupNoCar'
elif car_recognized and not controller_available:
alert = 'startupNoControl'
return alert
def load_interfaces(brand_names):
ret = {}
for interface in x:
try:
imp = __import__('selfdrive.car.%s.interface' % interface, fromlist=['CarInterface']).CarInterface
except ImportError:
imp = None
for car in x[interface]:
ret[car] = imp
for brand_name in brand_names:
path = ('selfdrive.car.%s' % brand_name)
CarInterface = __import__(path + '.interface', fromlist=['CarInterface']).CarInterface
if os.path.exists(BASEDIR + '/' + path.replace('.', '/') + '/carcontroller.py'):
CarController = __import__(path + '.carcontroller', fromlist=['CarController']).CarController
else:
CarController = None
for model_name in brand_names[brand_name]:
ret[model_name] = (CarInterface, CarController)
return ret
@ -22,17 +36,17 @@ def _get_interface_names():
# read all the folders in selfdrive/car and return a dict where:
# - keys are all the car names that which we have an interface for
# - values are lists of spefic car models for a given car
interface_names = {}
brand_names = {}
for car_folder in [x[0] for x in os.walk(BASEDIR + '/selfdrive/car')]:
try:
car_name = car_folder.split('/')[-1]
model_names = __import__('selfdrive.car.%s.values' % car_name, fromlist=['CAR']).CAR
brand_name = car_folder.split('/')[-1]
model_names = __import__('selfdrive.car.%s.values' % brand_name, fromlist=['CAR']).CAR
model_names = [getattr(model_names, c) for c in model_names.__dict__.keys() if not c.startswith("__")]
interface_names[car_name] = model_names
brand_names[brand_name] = model_names
except (ImportError, IOError):
pass
return interface_names
return brand_names
# imports from directory selfdrive/car/<name>/
@ -41,68 +55,96 @@ interfaces = load_interfaces(_get_interface_names())
# BOUNTY: every added fingerprint in selfdrive/car/*/values.py is a $100 coupon code on shop.comma.ai
# **** for use live only ****
def fingerprint(logcan, timeout):
def fingerprint(logcan, sendcan):
if os.getenv("SIMULATOR2") is not None:
return ("simulator2", None)
return ("simulator2", None, "")
elif os.getenv("SIMULATOR") is not None:
return ("simulator", None)
return ("simulator", None, "")
finger = {}
cloudlog.warning("waiting for fingerprint...")
candidate_cars = all_known_cars()
finger = {}
st = None
st_passive = sec_since_boot() # only relevant when passive
can_seen_ts = None
can_seen = False
# works on standard 11-bit addresses for diagnostic. Tested on Toyota and Subaru;
# Honda uses the extended 29-bit addresses, and unfortunately only works from OBDII
vin_query_msg = [[0x7df, 0, '\x02\x09\x02'.ljust(8, "\x00"), 0],
[0x7e0, 0, '\x30'.ljust(8, "\x00"), 0]]
vin_cnts = [1, 2] # number of messages to wait for at each iteration
vin_step = 0
vin_cnt = 0
vin_responded = False
vin_never_responded = True
vin_dat = []
vin = ""
while 1:
for a in messaging.drain_sock(logcan):
for can in a.can:
can_seen = True
# have we got a VIN query response?
if can.src == 0 and can.address == 0x7e8:
vin_never_responded = False
# basic sanity checks on ISO-TP response
if is_vin_response_valid(can.dat, vin_step, vin_cnt):
vin_dat += can.dat[2:] if vin_step == 0 else can.dat[1:]
vin_cnt += 1
if vin_cnt == vin_cnts[vin_step]:
vin_responded = True
vin_step += 1
# ignore everything not on bus 0 and with more than 11 bits,
# which are ussually sporadic and hard to include in fingerprints
if can.src == 0 and can.address < 0x800:
# which are ussually sporadic and hard to include in fingerprints.
# also exclude VIN query response on 0x7e8
if can.src == 0 and can.address < 0x800 and can.address != 0x7e8:
finger[can.address] = len(can.dat)
candidate_cars = eliminate_incompatible_cars(can, candidate_cars)
if st is None and can_seen:
st = sec_since_boot() # start time
if can_seen_ts is None and can_seen:
can_seen_ts = sec_since_boot() # start time
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:
# TODO: better way to decide to wait more if Toyota
if len(candidate_cars) == 1 and can_seen_ts is not None:
time_fingerprint = 1.0 if ("TOYOTA" in candidate_cars[0] or "LEXUS" in candidate_cars[0]) else 0.1
if (ts-st) > time_fingerprint:
if (ts - can_seen_ts) > time_fingerprint:
break
# bail if no cars left or we've been waiting too long
elif len(candidate_cars) == 0 or (timeout and (ts - st_passive) > timeout):
return None, finger
# bail if no cars left or we've been waiting for more than 2s since can_seen
elif len(candidate_cars) == 0 or (can_seen_ts is not None and (ts - can_seen_ts) > 2.):
return None, finger, ""
# keep sending VIN qury if ECU isn't responsing.
# sendcan is probably not ready due to the zmq slow joiner syndrome
if can_seen and (vin_never_responded or (vin_responded and vin_step < len(vin_cnts))):
sendcan.send(can_list_to_can_capnp([vin_query_msg[vin_step]], msgtype='sendcan'))
vin_responded = False
vin_cnt = 0
time.sleep(0.01)
# only report vin if procedure is finished
if vin_step == len(vin_cnts) and vin_cnt == vin_cnts[-1]:
vin = "".join(vin_dat[3:])
cloudlog.warning("fingerprinted %s", candidate_cars[0])
return (candidate_cars[0], finger)
cloudlog.warning("VIN %s", vin)
return candidate_cars[0], finger, vin
def get_car(logcan, sendcan=None, passive=True):
# TODO: timeout only useful for replays so controlsd can start before unlogger
timeout = 2. if passive else None
candidate, fingerprints = fingerprint(logcan, timeout)
def get_car(logcan, sendcan):
candidate, fingerprints, vin = fingerprint(logcan, sendcan)
if candidate is None:
cloudlog.warning("car doesn't match any fingerprints: %r", fingerprints)
if passive:
candidate = "mock"
else:
return None, None
candidate = "mock"
interface_cls = interfaces[candidate]
CarInterface, CarController = interfaces[candidate]
params = CarInterface.get_params(candidate, fingerprints, vin)
if interface_cls is None:
cloudlog.warning("car matched %s, but interface wasn't available or failed to import" % candidate)
return None, None
params = interface_cls.get_params(candidate, fingerprints)
return interface_cls(params, sendcan), params
return CarInterface(params, CarController), params

9
selfdrive/car/chrysler/carcontroller.py
View File

@ -1,5 +1,4 @@
from cereal import car
from selfdrive.boardd.boardd import can_list_to_can_capnp
from selfdrive.car import apply_toyota_steer_torque_limits
from selfdrive.car.chrysler.chryslercan import create_lkas_hud, create_lkas_command, \
create_wheel_buttons, \
@ -38,12 +37,11 @@ class CarController(object):
self.packer = CANPacker(dbc_name)
def update(self, sendcan, enabled, CS, frame, actuators,
pcm_cancel_cmd, hud_alert, audible_alert):
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert, audible_alert):
# this seems needed to avoid steering faults and to force the sync with the EPS counter
frame = CS.lkas_counter
if self.prev_frame == frame:
return
return []
# *** compute control surfaces ***
# steer torque
@ -97,4 +95,5 @@ class CarController(object):
self.ccframe += 1
self.prev_frame = frame
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
return can_sends

4
selfdrive/car/chrysler/carstate.py
View File

@ -60,7 +60,7 @@ def get_can_parser(CP):
("ACC_2", 50),
]
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0, timeout=100)
def get_camera_parser(CP):
signals = [
@ -72,7 +72,7 @@ def get_camera_parser(CP):
]
checks = []
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2, timeout=100)
class CarState(object):

37
selfdrive/car/chrysler/interface.py
View File

@ -7,14 +7,9 @@ from selfdrive.controls.lib.vehicle_model import VehicleModel
from selfdrive.car.chrysler.carstate import CarState, get_can_parser, get_camera_parser
from selfdrive.car.chrysler.values import ECU, check_ecu_msgs, CAR
try:
from selfdrive.car.chrysler.carcontroller import CarController
except ImportError:
CarController = None
class CarInterface(object):
def __init__(self, CP, sendcan=None):
def __init__(self, CP, CarController):
self.CP = CP
self.VM = VehicleModel(CP)
@ -30,9 +25,8 @@ class CarInterface(object):
self.cp = get_can_parser(CP)
self.cp_cam = get_camera_parser(CP)
# sending if read only is False
if sendcan is not None:
self.sendcan = sendcan
self.CC = None
if CarController is not None:
self.CC = CarController(self.cp.dbc_name, CP.carFingerprint, CP.enableCamera)
@staticmethod
@ -44,7 +38,7 @@ class CarInterface(object):
return 1.0
@staticmethod
def get_params(candidate, fingerprint):
def get_params(candidate, fingerprint, vin=""):
# kg of standard extra cargo to count for drive, gas, etc...
std_cargo = 136
@ -53,6 +47,7 @@ class CarInterface(object):
ret.carName = "chrysler"
ret.carFingerprint = candidate
ret.carVin = vin
ret.safetyModel = car.CarParams.SafetyModels.chrysler
@ -140,8 +135,10 @@ class CarInterface(object):
def update(self, c):
# ******************* do can recv *******************
canMonoTimes = []
self.cp.update(int(sec_since_boot() * 1e9), False)
self.cp_cam.update(int(sec_since_boot() * 1e9), False)
can_valid, _ = self.cp.update(int(sec_since_boot() * 1e9), True)
cam_valid, _ = self.cp_cam.update(int(sec_since_boot() * 1e9), False)
can_rcv_error = not can_valid or not cam_valid
self.CS.update(self.cp, self.cp_cam)
# create message
@ -217,10 +214,12 @@ class CarInterface(object):
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 can_rcv_error or self.can_invalid_count >= 5:
events.append(create_event('commIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if not (ret.gearShifter in ('drive', 'low')):
events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if ret.doorOpen:
@ -263,11 +262,11 @@ class CarInterface(object):
def apply(self, c):
if (self.CS.frame == -1):
return False # if we haven't seen a frame 220, then do not update.
return [] # if we haven't seen a frame 220, then do not update.
self.frame = self.CS.frame
self.CC.update(self.sendcan, c.enabled, self.CS, self.frame,
c.actuators, c.cruiseControl.cancel, c.hudControl.visualAlert,
c.hudControl.audibleAlert)
can_sends = self.CC.update(c.enabled, self.CS, self.frame,
c.actuators, c.cruiseControl.cancel, c.hudControl.visualAlert,
c.hudControl.audibleAlert)
return False
return can_sends

11
selfdrive/car/chrysler/radar_interface.py
View File

@ -13,7 +13,7 @@ RADAR_MSGS_D = range(0x2a2, 0x2b4+2, 2) # d_ messages
LAST_MSG = max(RADAR_MSGS_C + RADAR_MSGS_D)
NUMBER_MSGS = len(RADAR_MSGS_C) + len(RADAR_MSGS_D)
def _create_radard_can_parser():
def _create_radar_can_parser():
dbc_f = 'chrysler_pacifica_2017_hybrid_private_fusion.dbc'
msg_n = len(RADAR_MSGS_C)
# list of [(signal name, message name or number, initial values), (...)]
@ -54,7 +54,7 @@ class RadarInterface(object):
def __init__(self, CP):
self.pts = {}
self.delay = 0.0 # Delay of radar #TUNE
self.rcp = _create_radard_can_parser()
self.rcp = _create_radar_can_parser()
context = zmq.Context()
self.logcan = messaging.sub_sock(context, service_list['can'].port)
@ -65,11 +65,12 @@ class RadarInterface(object):
while 1:
tm = int(sec_since_boot() * 1e9)
updated_messages.update(self.rcp.update(tm, True))
_, vls = self.rcp.update(tm, True)
updated_messages.update(vls)
if LAST_MSG in updated_messages:
break
ret = car.RadarState.new_message()
ret = car.RadarData.new_message()
errors = []
if not self.rcp.can_valid:
errors.append("commIssue")
@ -81,7 +82,7 @@ class RadarInterface(object):
trackId = _address_to_track(ii)
if trackId not in self.pts:
self.pts[trackId] = car.RadarState.RadarPoint.new_message()
self.pts[trackId] = car.RadarData.RadarPoint.new_message()
self.pts[trackId].trackId = trackId
self.pts[trackId].aRel = float('nan')
self.pts[trackId].yvRel = float('nan')

2
selfdrive/car/ford/carstate.py
View File

@ -29,7 +29,7 @@ def get_can_parser(CP):
checks = [
]
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0, timeout=100)
class CarState(object):

29
selfdrive/car/ford/interface.py
View File

@ -8,14 +8,9 @@ from selfdrive.controls.lib.vehicle_model import VehicleModel
from selfdrive.car.ford.carstate import CarState, get_can_parser
from selfdrive.car.ford.values import MAX_ANGLE
try:
from selfdrive.car.ford.carcontroller import CarController
except ImportError:
CarController = None
class CarInterface(object):
def __init__(self, CP, sendcan=None):
def __init__(self, CP, CarController):
self.CP = CP
self.VM = VehicleModel(CP)
@ -30,9 +25,8 @@ class CarInterface(object):
self.cp = get_can_parser(CP)
# sending if read only is False
if sendcan is not None:
self.sendcan = sendcan
self.CC = None
if CarController is not None:
self.CC = CarController(self.cp.dbc_name, CP.enableCamera, self.VM)
@staticmethod
@ -44,7 +38,7 @@ class CarInterface(object):
return 1.0
@staticmethod
def get_params(candidate, fingerprint):
def get_params(candidate, fingerprint, vin=""):
# kg of standard extra cargo to count for drive, gas, etc...
std_cargo = 136
@ -53,6 +47,7 @@ class CarInterface(object):
ret.carName = "ford"
ret.carFingerprint = candidate
ret.carVin = vin
ret.safetyModel = car.CarParams.SafetyModels.ford
@ -138,7 +133,8 @@ class CarInterface(object):
# ******************* do can recv *******************
canMonoTimes = []
self.cp.update(int(sec_since_boot() * 1e9), False)
can_valid, _ = self.cp.update(int(sec_since_boot() * 1e9), True)
can_rcv_error = not can_valid
self.CS.update(self.cp)
@ -174,11 +170,12 @@ class CarInterface(object):
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 can_rcv_error or self.can_invalid_count >= 5:
events.append(create_event('commIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if self.CS.steer_error:
events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
@ -212,8 +209,8 @@ class CarInterface(object):
# to be called @ 100hz
def apply(self, c):
self.CC.update(self.sendcan, c.enabled, self.CS, self.frame, c.actuators,
c.hudControl.visualAlert, c.cruiseControl.cancel)
can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,
c.hudControl.visualAlert, c.cruiseControl.cancel)
self.frame += 1
return False
return can_sends

12
selfdrive/car/ford/radar_interface.py
View File

@ -11,7 +11,7 @@ import selfdrive.messaging as messaging
RADAR_MSGS = range(0x500, 0x540)
def _create_radard_can_parser():
def _create_radar_can_parser():
dbc_f = 'ford_fusion_2018_adas.dbc'
msg_n = len(RADAR_MSGS)
signals = list(zip(['X_Rel'] * msg_n + ['Angle'] * msg_n + ['V_Rel'] * msg_n,
@ -31,7 +31,7 @@ class RadarInterface(object):
self.delay = 0.0 # Delay of radar
# Nidec
self.rcp = _create_radard_can_parser()
self.rcp = _create_radar_can_parser()
context = zmq.Context()
self.logcan = messaging.sub_sock(context, service_list['can'].port)
@ -42,12 +42,14 @@ class RadarInterface(object):
updated_messages = set()
while 1:
tm = int(sec_since_boot() * 1e9)
updated_messages.update(self.rcp.update(tm, True))
_, vls = self.rcp.update(tm, True)
updated_messages.update(vls)
# TODO: do not hardcode last msg
if 0x53f in updated_messages:
break
ret = car.RadarState.new_message()
ret = car.RadarData.new_message()
errors = []
if not self.rcp.can_valid:
errors.append("commIssue")
@ -69,7 +71,7 @@ class RadarInterface(object):
# radar point only valid if there have been enough valid measurements
if self.validCnt[ii] > 0:
if ii not in self.pts:
self.pts[ii] = car.RadarState.RadarPoint.new_message()
self.pts[ii] = car.RadarData.RadarPoint.new_message()
self.pts[ii].trackId = self.track_id
self.track_id += 1
self.pts[ii].dRel = cpt['X_Rel'] # from front of car

28
selfdrive/car/gm/carcontroller.py
View File

@ -1,12 +1,13 @@
from cereal import car
from common.numpy_fast import interp
from common.realtime import sec_since_boot
from selfdrive.config import Conversions as CV
from selfdrive.boardd.boardd import can_list_to_can_capnp
from selfdrive.car import apply_std_steer_torque_limits
from selfdrive.car.gm import gmcan
from selfdrive.car.gm.values import DBC, SUPERCRUISE_CARS
from selfdrive.can.packer import CANPacker
VisualAlert = car.CarControl.HUDControl.VisualAlert
class CarControllerParams():
def __init__(self, car_fingerprint):
@ -59,16 +60,21 @@ def actuator_hystereses(final_pedal, pedal_steady):
return final_pedal, pedal_steady
def process_hud_alert(hud_alert):
# initialize to no alert
steer = 0
if hud_alert == VisualAlert.steerRequired:
steer = 1
return steer
class CarController(object):
def __init__(self, canbus, car_fingerprint, allow_controls):
def __init__(self, canbus, car_fingerprint):
self.pedal_steady = 0.
self.start_time = sec_since_boot()
self.chime = 0
self.steer_idx = 0
self.apply_steer_last = 0
self.car_fingerprint = car_fingerprint
self.allow_controls = allow_controls
self.lka_icon_status_last = (False, False)
# Setup detection helper. Routes commands to
@ -79,13 +85,8 @@ class CarController(object):
self.packer_pt = CANPacker(DBC[car_fingerprint]['pt'])
self.packer_ch = CANPacker(DBC[car_fingerprint]['chassis'])
def update(self, sendcan, enabled, CS, frame, actuators, \
hud_v_cruise, hud_show_lanes, hud_show_car, chime, chime_cnt):
""" Controls thread """
# Sanity check.
if not self.allow_controls:
return
def update(self, enabled, CS, frame, actuators, \
hud_v_cruise, hud_show_lanes, hud_show_car, chime, chime_cnt, hud_alert):
P = self.params
@ -93,6 +94,9 @@ class CarController(object):
can_sends = []
canbus = self.canbus
alert_out = process_hud_alert(hud_alert)
steer = alert_out
### STEER ###
if (frame % P.STEER_STEP) == 0:
@ -175,7 +179,7 @@ class CarController(object):
lka_icon_status = (lka_active, lka_critical)
if frame % P.CAMERA_KEEPALIVE_STEP == 0 \
or lka_icon_status != self.lka_icon_status_last:
can_sends.append(gmcan.create_lka_icon_command(canbus.sw_gmlan, lka_active, lka_critical))
can_sends.append(gmcan.create_lka_icon_command(canbus.sw_gmlan, lka_active, lka_critical, steer))
self.lka_icon_status_last = lka_icon_status
# Send chimes
@ -195,4 +199,4 @@ class CarController(object):
# issued for the same chime type and duration
self.chime = chime
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
return can_sends

3
selfdrive/car/gm/carstate.py
View File

@ -47,7 +47,8 @@ def get_powertrain_can_parser(CP, canbus):
("CruiseState", "AcceleratorPedal2", 0),
]
return CANParser(DBC[CP.carFingerprint]['pt'], signals, [], canbus.powertrain)
return CANParser(DBC[CP.carFingerprint]['pt'], signals, [], canbus.powertrain, timeout=100)
class CarState(object):
def __init__(self, CP, canbus):

9
selfdrive/car/gm/gmcan.py
View File

@ -134,8 +134,13 @@ def create_chime_command(bus, chime_type, duration, repeat_cnt):
dat = [chime_type, duration, repeat_cnt, 0xff, 0]
return [0x10400060, 0, "".join(map(chr, dat)), bus]
def create_lka_icon_command(bus, active, critical):
if active:
def create_lka_icon_command(bus, active, critical, steer):
if active and steer == 1:
if critical:
dat = "\x50\xc0\x14"
else:
dat = "\x50\x40\x18"
elif active:
if critical:
dat = "\x40\xc0\x14"
else:

39
selfdrive/car/gm/interface.py
View File

@ -8,10 +8,6 @@ from selfdrive.car.gm.values import DBC, CAR, STOCK_CONTROL_MSGS, AUDIO_HUD, \
SUPERCRUISE_CARS, AccState
from selfdrive.car.gm.carstate import CarState, CruiseButtons, get_powertrain_can_parser
try:
from selfdrive.car.gm.carcontroller import CarController
except ImportError:
CarController = None
class CanBus(object):
def __init__(self):
@ -21,7 +17,7 @@ class CanBus(object):
self.sw_gmlan = 3
class CarInterface(object):
def __init__(self, CP, sendcan=None):
def __init__(self, CP, CarController):
self.CP = CP
self.frame = 0
@ -37,10 +33,9 @@ class CarInterface(object):
self.pt_cp = get_powertrain_can_parser(CP, canbus)
self.ch_cp_dbc_name = DBC[CP.carFingerprint]['chassis']
# sending if read only is False
if sendcan is not None:
self.sendcan = sendcan
self.CC = CarController(canbus, CP.carFingerprint, CP.enableCamera)
self.CC = None
if CarController is not None:
self.CC = CarController(canbus, CP.carFingerprint)
@staticmethod
def compute_gb(accel, speed):
@ -51,16 +46,17 @@ class CarInterface(object):
return 1.0
@staticmethod
def get_params(candidate, fingerprint):
def get_params(candidate, fingerprint, vin=""):
ret = car.CarParams.new_message()
ret.carName = "gm"
ret.carFingerprint = candidate
ret.carVin = vin
ret.enableCruise = False
# Presence of a camera on the object bus is ok.
# Have to go passive if ASCM is online (ACC-enabled cars),
# Have to go to read_only if ASCM is online (ACC-enabled cars),
# or camera is on powertrain bus (LKA cars without ACC).
ret.enableCamera = not any(x for x in STOCK_CONTROL_MSGS[candidate] if x in fingerprint)
ret.openpilotLongitudinalControl = ret.enableCamera
@ -195,8 +191,8 @@ class CarInterface(object):
# returns a car.CarState
def update(self, c):
self.pt_cp.update(int(sec_since_boot() * 1e9), False)
can_valid, _ = self.pt_cp.update(int(sec_since_boot() * 1e9), True)
can_rcv_error = not can_valid
self.CS.update(self.pt_cp)
# create message
@ -281,10 +277,12 @@ class CarInterface(object):
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 can_rcv_error or self.can_invalid_count >= 5:
events.append(create_event('commIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if self.CS.steer_error:
events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
if self.CS.steer_not_allowed:
@ -358,10 +356,11 @@ class CarInterface(object):
# In GM, PCM faults out if ACC command overlaps user gas.
enabled = c.enabled and not self.CS.user_gas_pressed
self.CC.update(self.sendcan, enabled, self.CS, self.frame, \
c.actuators,
hud_v_cruise, c.hudControl.lanesVisible, \
c.hudControl.leadVisible, \
chime, chime_count)
can_sends = self.CC.update(enabled, self.CS, self.frame, \
c.actuators,
hud_v_cruise, c.hudControl.lanesVisible, \
c.hudControl.leadVisible, \
chime, chime_count, c.hudControl.visualAlert)
self.frame += 1
return can_sends

11
selfdrive/car/gm/radar_interface.py
View File

@ -19,7 +19,7 @@ NUM_SLOTS = 20
# messages that are present in DBC
LAST_RADAR_MSG = RADAR_HEADER_MSG + NUM_SLOTS
def create_radard_can_parser(canbus, car_fingerprint):
def create_radar_can_parser(canbus, car_fingerprint):
dbc_f = DBC[car_fingerprint]['radar']
if car_fingerprint in (CAR.VOLT, CAR.MALIBU, CAR.HOLDEN_ASTRA, CAR.ACADIA, CAR.CADILLAC_ATS):
@ -53,14 +53,14 @@ class RadarInterface(object):
canbus = CanBus()
print "Using %d as obstacle CAN bus ID" % canbus.obstacle
self.rcp = create_radard_can_parser(canbus, CP.carFingerprint)
self.rcp = create_radar_can_parser(canbus, CP.carFingerprint)
context = zmq.Context()
self.logcan = messaging.sub_sock(context, service_list['can'].port)
def update(self):
updated_messages = set()
ret = car.RadarState.new_message()
ret = car.RadarData.new_message()
while 1:
if self.rcp is None:
@ -68,7 +68,8 @@ class RadarInterface(object):
return ret
tm = int(sec_since_boot() * 1e9)
updated_messages.update(self.rcp.update(tm, True))
_, vls = self.rcp.update(tm, True)
updated_messages.update(vls)
if LAST_RADAR_MSG in updated_messages:
break
@ -101,7 +102,7 @@ class RadarInterface(object):
targetId = cpt['TrkObjectID']
currentTargets.add(targetId)
if targetId not in self.pts:
self.pts[targetId] = car.RadarState.RadarPoint.new_message()
self.pts[targetId] = car.RadarData.RadarPoint.new_message()
self.pts[targetId].trackId = targetId
distance = cpt['TrkRange']
self.pts[targetId].dRel = distance # from front of car

15
selfdrive/car/honda/carcontroller.py
View File

@ -1,6 +1,5 @@
from collections import namedtuple
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 common.numpy_fast import clip
from selfdrive.car import create_gas_command
@ -75,26 +74,20 @@ HUDData = namedtuple("HUDData",
class CarController(object):
def __init__(self, dbc_name, enable_camera=True):
def __init__(self, dbc_name):
self.braking = False
self.brake_steady = 0.
self.brake_last = 0.
self.apply_brake_last = 0
self.last_pump_ts = 0
self.enable_camera = enable_camera
self.packer = CANPacker(dbc_name)
self.new_radar_config = False
def update(self, sendcan, enabled, CS, frame, actuators, \
def update(self, enabled, CS, frame, actuators, \
pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \
hud_v_cruise, hud_show_lanes, hud_show_car, \
hud_alert, snd_beep, snd_chime):
""" Controls thread """
if not self.enable_camera:
return
# *** apply brake hysteresis ***
brake, self.braking, self.brake_steady = actuator_hystereses(actuators.brake, self.braking, self.brake_steady, CS.v_ego, CS.CP.carFingerprint)
@ -161,7 +154,7 @@ class CarController(object):
# Send dashboard UI commands.
if (frame % 10) == 0:
idx = (frame//10) % 4
can_sends.extend(hondacan.create_ui_commands(self.packer, pcm_speed, hud, CS.CP.carFingerprint, idx))
can_sends.extend(hondacan.create_ui_commands(self.packer, pcm_speed, hud, CS.CP.carFingerprint, CS.is_metric, idx))
if CS.CP.radarOffCan:
# If using stock ACC, spam cancel command to kill gas when OP disengages.
@ -184,4 +177,4 @@ class CarController(object):
# This prevents unexpected pedal range rescaling
can_sends.append(create_gas_command(self.packer, apply_gas, idx))
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
return can_sends

12
selfdrive/car/honda/carstate.py
View File

@ -46,7 +46,6 @@ def get_can_signals(CP):
("BRAKE_SWITCH", "POWERTRAIN_DATA", 0),
("CRUISE_BUTTONS", "SCM_BUTTONS", 0),
("ESP_DISABLED", "VSA_STATUS", 1),
("HUD_LEAD", "ACC_HUD", 0),
("USER_BRAKE", "VSA_STATUS", 0),
("BRAKE_HOLD_ACTIVE", "VSA_STATUS", 0),
("STEER_STATUS", "STEER_STATUS", 5),
@ -124,6 +123,7 @@ def get_can_signals(CP):
if CP.carFingerprint == CAR.CIVIC:
signals += [("CAR_GAS", "GAS_PEDAL_2", 0),
("MAIN_ON", "SCM_FEEDBACK", 0),
("IMPERIAL_UNIT", "HUD_SETTING", 0),
("EPB_STATE", "EPB_STATUS", 0)]
elif CP.carFingerprint == CAR.ACURA_ILX:
signals += [("CAR_GAS", "GAS_PEDAL_2", 0),
@ -152,7 +152,8 @@ def get_can_signals(CP):
def get_can_parser(CP):
signals, checks = get_can_signals(CP)
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0, timeout=100)
def get_cam_can_parser(CP):
signals = []
@ -164,7 +165,7 @@ def get_cam_can_parser(CP):
cam_bus = 1 if CP.carFingerprint in HONDA_BOSCH else 2
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, cam_bus)
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, cam_bus, timeout=100)
class CarState(object):
def __init__(self, CP):
@ -332,14 +333,15 @@ class CarState(object):
self.user_brake = cp.vl["VSA_STATUS"]['USER_BRAKE']
self.pcm_acc_status = cp.vl["POWERTRAIN_DATA"]['ACC_STATUS']
self.hud_lead = cp.vl["ACC_HUD"]['HUD_LEAD']
# Gets rid of Pedal Grinding noise when brake is pressed at slow speeds for some models
# TODO: this should be ok for all cars. Verify it.
if self.CP.carFingerprint in (CAR.PILOT, CAR.PILOT_2019, CAR.RIDGELINE):
if self.user_brake > 0.05:
self.brake_pressed = 1
# TODO: discover the CAN msg that has the imperial unit bit for all other cars
self.is_metric = not cp.vl["HUD_SETTING"]['IMPERIAL_UNIT'] if self.CP.carFingerprint in (CAR.CIVIC) else False
# carstate standalone tester
if __name__ == '__main__':
import zmq

9
selfdrive/car/honda/hondacan.py
View File

@ -51,7 +51,7 @@ def create_steering_control(packer, apply_steer, lkas_active, car_fingerprint, i
return packer.make_can_msg("STEERING_CONTROL", bus, values, idx)
def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, idx):
def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, is_metric, idx):
commands = []
bus = 0
@ -65,9 +65,10 @@ def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, idx):
'CRUISE_SPEED': hud.v_cruise,
'ENABLE_MINI_CAR': hud.mini_car,
'HUD_LEAD': hud.car,
'SET_ME_X03': 0x01 if car_fingerprint == CAR.ODYSSEY_CHN else 0x03,
'SET_ME_X03_2': 0x02 if car_fingerprint == CAR.ODYSSEY_CHN else 0x03,
'SET_ME_X01': 0x01,
'HUD_DISTANCE': 3, # max distance setting on display
'IMPERIAL_UNIT': int(not is_metric),
'SET_ME_X01_2': 1,
'SET_ME_X01': 1,
}
commands.append(packer.make_can_msg("ACC_HUD", 0, acc_hud_values, idx))

53
selfdrive/car/honda/interface.py
View File

@ -12,11 +12,6 @@ from selfdrive.car.honda.carstate import CarState, get_can_parser, get_cam_can_p
from selfdrive.car.honda.values import CruiseButtons, CAR, HONDA_BOSCH, AUDIO_HUD, VISUAL_HUD
from selfdrive.controls.lib.planner import _A_CRUISE_MAX_V_FOLLOWING
try:
from selfdrive.car.honda.carcontroller import CarController
except ImportError:
CarController = None
# msgs sent for steering controller by camera module on can 0.
# those messages are mutually exclusive on CRV and non-CRV cars
@ -80,7 +75,7 @@ def get_compute_gb_acura():
class CarInterface(object):
def __init__(self, CP, sendcan=None):
def __init__(self, CP, CarController):
self.CP = CP
self.frame = 0
@ -98,10 +93,9 @@ class CarInterface(object):
self.CS = CarState(CP)
self.VM = VehicleModel(CP)
# sending if read only is False
if sendcan is not None:
self.sendcan = sendcan
self.CC = CarController(self.cp.dbc_name, CP.enableCamera)
self.CC = None
if CarController is not None:
self.CC = CarController(self.cp.dbc_name)
if self.CS.CP.carFingerprint == CAR.ACURA_ILX:
self.compute_gb = get_compute_gb_acura()
@ -141,11 +135,12 @@ class CarInterface(object):
return float(max(max_accel, a_target / A_ACC_MAX)) * min(speedLimiter, accelLimiter)
@staticmethod
def get_params(candidate, fingerprint):
def get_params(candidate, fingerprint, vin=""):
ret = car.CarParams.new_message()
ret.carName = "honda"
ret.carFingerprint = candidate
ret.carVin = vin
if candidate in HONDA_BOSCH:
ret.safetyModel = car.CarParams.SafetyModels.hondaBosch
@ -391,9 +386,9 @@ class CarInterface(object):
def update(self, c):
# ******************* do can recv *******************
canMonoTimes = []
self.cp.update(int(sec_since_boot() * 1e9), False)
self.cp_cam.update(int(sec_since_boot() * 1e9), False)
can_valid, _ = self.cp.update(int(sec_since_boot() * 1e9), True)
cam_valid, _ = self.cp_cam.update(int(sec_since_boot() * 1e9), False)
can_rcv_error = not can_valid or not cam_valid
self.CS.update(self.cp, self.cp_cam)
@ -503,11 +498,12 @@ class CarInterface(object):
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 can_rcv_error or self.can_invalid_count >= 5:
events.append(create_event('commIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if not self.CS.cam_can_valid and self.CP.enableCamera:
self.cam_can_invalid_count += 1
# wait 1.0s before throwing the alert to avoid it popping when you turn off the car
@ -612,17 +608,18 @@ class CarInterface(object):
pcm_accel = int(clip(c.cruiseControl.accelOverride, 0, 1) * 0xc6)
self.CC.update(self.sendcan, c.enabled, self.CS, self.frame,
c.actuators,
c.cruiseControl.speedOverride,
c.cruiseControl.override,
c.cruiseControl.cancel,
pcm_accel,
hud_v_cruise,
c.hudControl.lanesVisible,
hud_show_car=c.hudControl.leadVisible,
hud_alert=hud_alert,
snd_beep=snd_beep,
snd_chime=snd_chime)
can_sends = self.CC.update(c.enabled, self.CS, self.frame,
c.actuators,
c.cruiseControl.speedOverride,
c.cruiseControl.override,
c.cruiseControl.cancel,
pcm_accel,
hud_v_cruise,
c.hudControl.lanesVisible,
hud_show_car=c.hudControl.leadVisible,
hud_alert=hud_alert,
snd_beep=snd_beep,
snd_chime=snd_chime)
self.frame += 1
return can_sends

7
selfdrive/car/honda/radar_interface.py
View File

@ -43,7 +43,7 @@ class RadarInterface(object):
canMonoTimes = []
updated_messages = set()
ret = car.RadarState.new_message()
ret = car.RadarData.new_message()
# in Bosch radar and we are only steering for now, so sleep 0.05s to keep
# radard at 20Hz and return no points
@ -53,7 +53,8 @@ class RadarInterface(object):
while 1:
tm = int(sec_since_boot() * 1e9)
updated_messages.update(self.rcp.update(tm, True))
_, vls = self.rcp.update(tm, True)
updated_messages.update(vls)
if 0x445 in updated_messages:
break
@ -65,7 +66,7 @@ class RadarInterface(object):
self.radar_wrong_config = cpt['RADAR_STATE'] == 0x69
elif cpt['LONG_DIST'] < 255:
if ii not in self.pts or cpt['NEW_TRACK']:
self.pts[ii] = car.RadarState.RadarPoint.new_message()
self.pts[ii] = car.RadarData.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

14
selfdrive/car/hyundai/carcontroller.py
View File

@ -1,5 +1,4 @@
from selfdrive.car import apply_std_steer_torque_limits
from selfdrive.boardd.boardd import can_list_to_can_capnp
from selfdrive.car.hyundai.hyundaican import create_lkas11, create_lkas12, \
create_1191, create_1156, \
create_clu11
@ -18,23 +17,19 @@ class SteerLimitParams:
STEER_DRIVER_FACTOR = 1
class CarController(object):
def __init__(self, dbc_name, car_fingerprint, enable_camera):
def __init__(self, dbc_name, car_fingerprint):
self.apply_steer_last = 0
self.car_fingerprint = car_fingerprint
self.lkas11_cnt = 0
self.cnt = 0
self.last_resume_cnt = 0
self.enable_camera = enable_camera
# True when giraffe switch 2 is low and we need to replace all the camera messages
# otherwise we forward the camera msgs and we just replace the lkas cmd signals
self.camera_disconnected = False
self.packer = CANPacker(dbc_name)
def update(self, sendcan, enabled, CS, actuators, pcm_cancel_cmd, hud_alert):
if not self.enable_camera:
return
def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert):
### Steering Torque
apply_steer = actuators.steer * SteerLimitParams.STEER_MAX
@ -70,7 +65,6 @@ class CarController(object):
self.last_resume_cnt = self.cnt
can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.RES_ACCEL))
### Send messages to canbus
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
self.cnt += 1
return can_sends

6
selfdrive/car/hyundai/carstate.py
View File

@ -93,7 +93,8 @@ def get_can_parser(CP):
("SAS11", 100)
]
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0, timeout=100)
def get_camera_parser(CP):
@ -118,7 +119,8 @@ def get_camera_parser(CP):
checks = []
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2, timeout=100)
class CarState(object):
def __init__(self, CP):

35
selfdrive/car/hyundai/interface.py
View File

@ -7,14 +7,9 @@ from selfdrive.controls.lib.vehicle_model import VehicleModel
from selfdrive.car.hyundai.carstate import CarState, get_can_parser, get_camera_parser
from selfdrive.car.hyundai.values import CAMERA_MSGS, CAR, get_hud_alerts, FEATURES
try:
from selfdrive.car.hyundai.carcontroller import CarController
except ImportError:
CarController = None
class CarInterface(object):
def __init__(self, CP, sendcan=None):
def __init__(self, CP, CarController):
self.CP = CP
self.VM = VehicleModel(CP)
self.idx = 0
@ -32,10 +27,9 @@ class CarInterface(object):
self.cp = get_can_parser(CP)
self.cp_cam = get_camera_parser(CP)
# sending if read only is False
if sendcan is not None:
self.sendcan = sendcan
self.CC = CarController(self.cp.dbc_name, CP.carFingerprint, CP.enableCamera)
self.CC = None
if CarController is not None:
self.CC = CarController(self.cp.dbc_name, CP.carFingerprint)
@staticmethod
def compute_gb(accel, speed):
@ -46,7 +40,7 @@ class CarInterface(object):
return 1.0
@staticmethod
def get_params(candidate, fingerprint):
def get_params(candidate, fingerprint, vin=""):
# kg of standard extra cargo to count for drive, gas, etc...
std_cargo = 136
@ -55,6 +49,7 @@ class CarInterface(object):
ret.carName = "hyundai"
ret.carFingerprint = candidate
ret.carVin = vin
ret.radarOffCan = True
ret.safetyModel = car.CarParams.SafetyModels.hyundai
ret.enableCruise = True # stock acc
@ -179,8 +174,10 @@ class CarInterface(object):
def update(self, c):
# ******************* do can recv *******************
canMonoTimes = []
self.cp.update(int(sec_since_boot() * 1e9), False)
self.cp_cam.update(int(sec_since_boot() * 1e9), False)
can_rcv_error = not self.cp.update(int(sec_since_boot() * 1e9), True)
cam_rcv_error = not self.cp_cam.update(int(sec_since_boot() * 1e9), False)
can_rcv_error = can_rcv_error or cam_rcv_error
self.CS.update(self.cp, self.cp_cam)
# create message
ret = car.CarState.new_message()
@ -261,10 +258,12 @@ class CarInterface(object):
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 can_rcv_error or self.can_invalid_count >= 5:
events.append(create_event('commIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if not ret.gearShifter == 'drive':
events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if ret.doorOpen:
@ -310,7 +309,7 @@ class CarInterface(object):
hud_alert = get_hud_alerts(c.hudControl.visualAlert, c.hudControl.audibleAlert)
self.CC.update(self.sendcan, c.enabled, self.CS, c.actuators,
c.cruiseControl.cancel, hud_alert)
can_sends = self.CC.update(c.enabled, self.CS, c.actuators,
c.cruiseControl.cancel, hud_alert)
return False
return can_sends

2
selfdrive/car/hyundai/radar_interface.py
View File

@ -11,7 +11,7 @@ class RadarInterface(object):
def update(self):
ret = car.RadarState.new_message()
ret = car.RadarData.new_message()
time.sleep(0.05) # radard runs on RI updates
return ret

11
selfdrive/car/mock/interface.py
View File

@ -5,6 +5,7 @@ from selfdrive.config import Conversions as CV
from selfdrive.services import service_list
from selfdrive.swaglog import cloudlog
import selfdrive.messaging as messaging
from common.realtime import Ratekeeper
# mocked car interface to work with chffrplus
TS = 0.01 # 100Hz
@ -14,9 +15,10 @@ LPG = 2 * 3.1415 * YAW_FR * TS / (1 + 2 * 3.1415 * YAW_FR * TS)
class CarInterface(object):
def __init__(self, CP, sendcan=None):
def __init__(self, CP, CarController):
self.CP = CP
self.CC = CarController
cloudlog.debug("Using Mock Car Interface")
context = zmq.Context()
@ -30,6 +32,8 @@ class CarInterface(object):
self.yaw_rate = 0.
self.yaw_rate_meas = 0.
self.rk = Ratekeeper(100, print_delay_threshold=2. / 1000)
@staticmethod
def compute_gb(accel, speed):
return accel
@ -39,7 +43,7 @@ class CarInterface(object):
return 1.0
@staticmethod
def get_params(candidate, fingerprint):
def get_params(candidate, fingerprint, vin=""):
ret = car.CarParams.new_message()
@ -79,6 +83,7 @@ class CarInterface(object):
# returns a car.CarState
def update(self, c):
self.rk.keep_time()
# get basic data from phone and gps since CAN isn't connected
sensors = messaging.recv_sock(self.sensor)
@ -120,4 +125,4 @@ class CarInterface(object):
def apply(self, c):
# in mock no carcontrols
return False
return []

2
selfdrive/car/mock/radar_interface.py
View File

@ -11,7 +11,7 @@ class RadarInterface(object):
def update(self):
ret = car.RadarState.new_message()
ret = car.RadarData.new_message()
time.sleep(0.05) # radard runs on RI updates
return ret

5
selfdrive/car/subaru/carcontroller.py
View File

@ -1,6 +1,5 @@
#from common.numpy_fast import clip
from common.realtime import sec_since_boot
from selfdrive.boardd.boardd import can_list_to_can_capnp
from selfdrive.car import apply_std_steer_torque_limits
from selfdrive.car.subaru import subarucan
from selfdrive.car.subaru.values import CAR, DBC
@ -36,7 +35,7 @@ class CarController(object):
print(DBC)
self.packer = CANPacker(DBC[car_fingerprint]['pt'])
def update(self, sendcan, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, left_line, right_line):
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, left_line, right_line):
""" Controls thread """
P = self.params
@ -73,4 +72,4 @@ class CarController(object):
can_sends.append(subarucan.create_es_lkas(self.packer, CS.es_lkas_msg, visual_alert, left_line, right_line))
self.es_lkas_cnt = CS.es_lkas_msg["Counter"]
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
return can_sends

6
selfdrive/car/subaru/carstate.py
View File

@ -37,7 +37,8 @@ def get_powertrain_can_parser(CP):
("BodyInfo", 10),
]
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0, timeout=100)
def get_camera_can_parser(CP):
signals = [
@ -78,7 +79,8 @@ def get_camera_can_parser(CP):
("ES_DashStatus", 10),
]
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2, timeout=100)
class CarState(object):
def __init__(self, CP):

32
selfdrive/car/subaru/interface.py
View File

@ -7,14 +7,9 @@ from selfdrive.controls.lib.vehicle_model import VehicleModel
from selfdrive.car.subaru.values import CAR
from selfdrive.car.subaru.carstate import CarState, get_powertrain_can_parser, get_camera_can_parser
try:
from selfdrive.car.subaru.carcontroller import CarController
except ImportError:
CarController = None
class CarInterface(object):
def __init__(self, CP, sendcan=None):
def __init__(self, CP, CarController):
self.CP = CP
self.frame = 0
@ -30,9 +25,8 @@ class CarInterface(object):
self.gas_pressed_prev = False
# sending if read only is False
if sendcan is not None:
self.sendcan = sendcan
self.CC = None
if CarController is not None:
self.CC = CarController(CP.carFingerprint)
@staticmethod
@ -44,11 +38,12 @@ class CarInterface(object):
return 1.0
@staticmethod
def get_params(candidate, fingerprint):
def get_params(candidate, fingerprint, vin=""):
ret = car.CarParams.new_message()
ret.carName = "subaru"
ret.carFingerprint = candidate
ret.carVin = vin
ret.safetyModel = car.CarParams.SafetyModels.subaru
ret.enableCruise = True
@ -119,9 +114,10 @@ class CarInterface(object):
# returns a car.CarState
def update(self, c):
can_rcv_error = not self.pt_cp.update(int(sec_since_boot() * 1e9), True)
cam_rcv_error = not self.cam_cp.update(int(sec_since_boot() * 1e9), False)
can_rcv_error = can_rcv_error or cam_rcv_error
self.pt_cp.update(int(sec_since_boot() * 1e9), False)
self.cam_cp.update(int(sec_since_boot() * 1e9), False)
self.CS.update(self.pt_cp, self.cam_cp)
# create message
@ -183,11 +179,12 @@ class CarInterface(object):
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 can_rcv_error or self.can_invalid_count >= 5:
events.append(create_event('commIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if ret.seatbeltUnlatched:
events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
@ -216,7 +213,8 @@ class CarInterface(object):
return ret.as_reader()
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.leftLaneVisible, c.hudControl.rightLaneVisible)
can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,
c.cruiseControl.cancel, c.hudControl.visualAlert,
c.hudControl.leftLaneVisible, c.hudControl.rightLaneVisible)
self.frame += 1
return can_sends

2
selfdrive/car/subaru/radar_interface.py
View File

@ -11,7 +11,7 @@ class RadarInterface(object):
def update(self):
ret = car.RadarState.new_message()
ret = car.RadarData.new_message()
time.sleep(0.05) # radard runs on RI updates
return ret

6
selfdrive/car/toyota/carcontroller.py
View File

@ -1,6 +1,5 @@
from cereal import car
from common.numpy_fast import clip, interp
from selfdrive.boardd.boardd import can_list_to_can_capnp
from selfdrive.car import apply_toyota_steer_torque_limits
from selfdrive.car import create_gas_command
from selfdrive.car.toyota.toyotacan import make_can_msg, create_video_target,\
@ -124,7 +123,7 @@ class CarController(object):
self.packer = CANPacker(dbc_name)
def update(self, sendcan, enabled, CS, frame, actuators,
def update(self, enabled, CS, frame, actuators,
pcm_cancel_cmd, hud_alert, audible_alert, forwarding_camera,
left_line, right_line, lead, left_lane_depart, right_lane_depart):
@ -270,5 +269,4 @@ class CarController(object):
can_sends.append(make_can_msg(addr, vl, bus, False))
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
return can_sends

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

@ -68,7 +68,7 @@ def get_can_parser(CP):
signals.append(("INTERCEPTOR_GAS", "GAS_SENSOR", 0))
checks.append(("GAS_SENSOR", 50))
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0, timeout=100)
def get_cam_can_parser(CP):
@ -78,7 +78,7 @@ def get_cam_can_parser(CP):
# use steering message to check if panda is connected to frc
checks = [("STEERING_LKA", 42)]
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2, timeout=100)
class CarState(object):

35
selfdrive/car/toyota/interface.py
View File

@ -8,14 +8,9 @@ from selfdrive.car.toyota.carstate import CarState, get_can_parser, get_cam_can_
from selfdrive.car.toyota.values import ECU, check_ecu_msgs, CAR, NO_STOP_TIMER_CAR
from selfdrive.swaglog import cloudlog
try:
from selfdrive.car.toyota.carcontroller import CarController
except ImportError:
CarController = None
class CarInterface(object):
def __init__(self, CP, sendcan=None):
def __init__(self, CP, CarController):
self.CP = CP
self.VM = VehicleModel(CP)
@ -34,9 +29,8 @@ class CarInterface(object):
self.forwarding_camera = False
# sending if read only is False
if sendcan is not None:
self.sendcan = sendcan
self.CC = None
if CarController is not None:
self.CC = CarController(self.cp.dbc_name, CP.carFingerprint, CP.enableCamera, CP.enableDsu, CP.enableApgs)
@staticmethod
@ -48,7 +42,7 @@ class CarInterface(object):
return 1.0
@staticmethod
def get_params(candidate, fingerprint):
def get_params(candidate, fingerprint, vin=""):
# kg of standard extra cargo to count for drive, gas, etc...
std_cargo = 136
@ -57,6 +51,7 @@ class CarInterface(object):
ret.carName = "toyota"
ret.carFingerprint = candidate
ret.carVin = vin
ret.safetyModel = car.CarParams.SafetyModels.toyota
@ -256,7 +251,8 @@ class CarInterface(object):
# ******************* do can recv *******************
canMonoTimes = []
self.cp.update(int(sec_since_boot() * 1e9), False)
can_valid, _ = self.cp.update(int(sec_since_boot() * 1e9), True)
can_rcv_error = not can_valid
# run the cam can update for 10s as we just need to know if the camera is alive
if self.frame < 1000:
@ -341,11 +337,12 @@ class CarInterface(object):
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 can_rcv_error or self.can_invalid_count >= 5:
events.append(create_event('commIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if self.CS.cam_can_valid:
self.cam_can_valid_count += 1
if self.cam_can_valid_count >= 5:
@ -403,11 +400,11 @@ class CarInterface(object):
# 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.forwarding_camera,
c.hudControl.leftLaneVisible, c.hudControl.rightLaneVisible, c.hudControl.leadVisible,
c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart)
can_sends = self.CC.update(c.enabled, self.CS, self.frame,
c.actuators, c.cruiseControl.cancel, c.hudControl.visualAlert,
c.hudControl.audibleAlert, self.forwarding_camera,
c.hudControl.leftLaneVisible, c.hudControl.rightLaneVisible, c.hudControl.leadVisible,
c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart)
self.frame += 1
return False
return can_sends

11
selfdrive/car/toyota/radar_interface.py
View File

@ -9,7 +9,7 @@ from selfdrive.services import service_list
import selfdrive.messaging as messaging
from selfdrive.car.toyota.values import NO_DSU_CAR, DBC, TSSP2_CAR
def _create_radard_can_parser(car_fingerprint):
def _create_radar_can_parser(car_fingerprint):
dbc_f = DBC[car_fingerprint]['radar']
if car_fingerprint in TSSP2_CAR:
@ -48,7 +48,7 @@ class RadarInterface(object):
self.valid_cnt = {key: 0 for key in self.RADAR_A_MSGS}
self.rcp = _create_radard_can_parser(CP.carFingerprint)
self.rcp = _create_radar_can_parser(CP.carFingerprint)
self.no_dsu_car = CP.carFingerprint in NO_DSU_CAR
context = zmq.Context()
@ -56,7 +56,7 @@ class RadarInterface(object):
def update(self):
ret = car.RadarState.new_message()
ret = car.RadarData.new_message()
if self.no_dsu_car:
# TODO: make a adas dbc file for dsu-less models
@ -67,7 +67,8 @@ class RadarInterface(object):
updated_messages = set()
while 1:
tm = int(sec_since_boot() * 1e9)
updated_messages.update(self.rcp.update(tm, True))
_, vls = self.rcp.update(tm, True)
updated_messages.update(vls)
if self.RADAR_B_MSGS[-1] in updated_messages:
break
@ -94,7 +95,7 @@ class RadarInterface(object):
# radar point only valid if it's a valid measurement and score is above 50
if cpt['VALID'] or (score > 50 and cpt['LONG_DIST'] < 255 and self.valid_cnt[ii] > 0):
if ii not in self.pts or cpt['NEW_TRACK']:
self.pts[ii] = car.RadarState.RadarPoint.new_message()
self.pts[ii] = car.RadarData.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

10
selfdrive/car/toyota/values.py
View File

@ -141,10 +141,10 @@ FINGERPRINTS = {
CAR.HIGHLANDER: [{
36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 238: 4, 355: 5, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 545: 5, 550: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 921: 8, 922: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1008: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1207: 8, 1212: 8, 1227: 8, 1235: 8, 1237: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 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, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1666: 8, 1667: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
},
# 2019 Highlander XLE
# 2019 Highlander XLE
{
36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 238: 4, 355: 5, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 545: 5, 550: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 921: 8, 922: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1008: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1207: 8, 1212: 8, 1227: 8, 1235: 8, 1237: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 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, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
},
},
# 2017 Highlander Limited
{
36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 238: 4, 355: 5, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 545: 5, 550: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 7, 921: 8, 922: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1008: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1207: 8, 1212: 8, 1227: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 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, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
@ -160,12 +160,12 @@ FINGERPRINTS = {
{
36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 355: 5, 401: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 565: 8, 608: 8, 610: 8, 643: 7, 705: 8, 728: 8, 740: 5, 742: 8, 743: 8, 761: 8, 764: 8, 765: 8, 800: 8, 810: 2, 812: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 882: 8, 885: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 8, 1076: 8, 1077: 8,1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1235: 8, 1279: 8, 1541: 8, 1552: 8, 1553:8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1745: 8, 1775: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
},
# XLE
# XLE and AWD
{
36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 401: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 565: 8, 608: 8, 610: 8, 643: 7, 705: 8, 728: 8, 740: 5, 742: 8, 743: 8, 761: 8, 764: 8, 765: 8, 800: 8, 810: 2, 812: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 882: 8, 885: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 8, 1076: 8, 1077: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1235: 8, 1237: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1745: 8, 1775: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 401: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 565: 8, 608: 8, 610: 8, 643: 7, 658: 8, 705: 8, 728: 8, 740: 5, 742: 8, 743: 8, 761: 8, 764: 8, 765: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 822: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 882: 8, 885: 8, 889: 8, 891: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 987: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 8, 1076: 8, 1077: 8, 1082: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1696: 8, 1745: 8, 1775: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
}],
CAR.COROLLA_HATCH: [{
36: 8, 37: 8, 114: 5, 170: 8, 180: 8, 186: 4, 401: 8, 426: 6, 452: 8 , 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7 , 705: 8, 728: 8, 740: 5, 742: 8, 743: 8, 761: 8, 764: 8, 765: 8, 800: 8, 810: 2, 812: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 998: 5, 999: 7, 1000:8, 1001: 8, 1002: 8, 1017:8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059:1, 1076: 8, 1077: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1235: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1595: 8, 1745: 8, 1775: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
36: 8, 37: 8, 114: 5, 170: 8, 180: 8, 186: 4, 401: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7, 705: 8, 728: 8, 740: 5, 742: 8, 743: 8, 761: 8, 764: 8, 765: 8, 800: 8, 810: 2, 812: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1235: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1595: 8, 1649: 8, 1745: 8, 1775: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
}]
}

15
selfdrive/common/messaging.h Normal file
View File

@ -0,0 +1,15 @@
// the c version of selfdrive/messaging.py
#include <zmq.h>
// TODO: refactor to take in service instead of endpoint?
void *sub_sock(void *ctx, const char *endpoint) {
void* sock = zmq_socket(ctx, ZMQ_SUB);
assert(sock);
zmq_setsockopt(sock, ZMQ_SUBSCRIBE, "", 0);
int reconnect_ivl = 500;
zmq_setsockopt(sock, ZMQ_RECONNECT_IVL_MAX, &reconnect_ivl, sizeof(reconnect_ivl));
zmq_connect(sock, endpoint);
return sock;
}

2
selfdrive/common/version.h
View File

@ -1 +1 @@
#define COMMA_VERSION "0.5.12-release"
#define COMMA_VERSION "0.5.13-release"

80
selfdrive/controls/controlsd.py
View File

@ -12,7 +12,8 @@ from common.params import Params
import selfdrive.messaging as messaging
from selfdrive.config import Conversions as CV
from selfdrive.services import service_list
from selfdrive.car.car_helpers import get_car
from selfdrive.boardd.boardd import can_list_to_can_capnp
from selfdrive.car.car_helpers import get_car, get_startup_alert
from selfdrive.controls.lib.model_parser import CAMERA_OFFSET
from selfdrive.controls.lib.drive_helpers import learn_angle_model_bias, \
get_events, \
@ -30,7 +31,7 @@ from selfdrive.controls.lib.planner import _DT_MPC
from selfdrive.locationd.calibration_helpers import Calibration, Filter
ThermalStatus = log.ThermalData.ThermalStatus
State = log.Live100Data.ControlState
State = log.ControlsState.OpenpilotState
def isActive(state):
@ -209,7 +210,7 @@ def state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM
def state_control(rcv_times, plan, path_plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk,
driver_status, LaC, LoC, VM, angle_model_bias, passive, is_metric, cal_perc):
driver_status, LaC, LoC, VM, angle_model_bias, read_only, is_metric, cal_perc):
"""Given the state, this function returns an actuators packet"""
actuators = car.CarControl.Actuators.new_message()
@ -288,15 +289,15 @@ def state_control(rcv_times, plan, path_plan, CS, CP, state, events, v_cruise_kp
def data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate,
carcontrol, live100, AM, driver_status,
LaC, LoC, angle_model_bias, passive, start_time, v_acc, a_acc, lac_log):
"""Send actuators and hud commands to the car, send live100 and MPC logging"""
carcontrol, controlsstate, sendcan, AM, driver_status,
LaC, LoC, angle_model_bias, read_only, start_time, v_acc, a_acc, lac_log):
"""Send actuators and hud commands to the car, send controlsstate and MPC logging"""
plan_ts = plan.logMonoTime
plan = plan.plan
CC = car.CarControl.new_message()
if not passive:
if not read_only:
CC.enabled = isEnabled(state)
CC.actuators = actuators
@ -331,14 +332,15 @@ def data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruis
CC.hudControl.audibleAlert = AM.audible_alert
# send car controls over can
CI.apply(CC)
can_sends = CI.apply(CC)
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
force_decel = driver_status.awareness < 0.
# live100
# controlsState
dat = messaging.new_message()
dat.init('live100')
dat.live100 = {
dat.init('controlsState')
dat.controlsState = {
"alertText1": AM.alert_text_1,
"alertText2": AM.alert_text_2,
"alertSize": AM.alert_size,
@ -381,10 +383,10 @@ def data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruis
}
if CP.lateralTuning.which() == 'pid':
dat.live100.lateralControlState.pidState = lac_log
dat.controlsState.lateralControlState.pidState = lac_log
else:
dat.live100.lateralControlState.indiState = lac_log
live100.send(dat.to_bytes())
dat.controlsState.lateralControlState.indiState = lac_log
controlsstate.send(dat.to_bytes())
# carState
cs_send = messaging.new_message()
@ -402,7 +404,7 @@ def data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruis
return CC
def controlsd_thread(gctx=None, rate=100):
def controlsd_thread(gctx=None):
gc.disable()
# start the loop
@ -412,18 +414,14 @@ def controlsd_thread(gctx=None, rate=100):
params = Params()
# Pub Sockets
live100 = messaging.pub_sock(context, service_list['live100'].port)
controlsstate = messaging.pub_sock(context, service_list['controlsState'].port)
carstate = messaging.pub_sock(context, service_list['carState'].port)
carcontrol = messaging.pub_sock(context, service_list['carControl'].port)
is_metric = params.get("IsMetric") == "1"
passive = params.get("Passive") != "0"
# No sendcan if passive
if not passive:
sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
else:
sendcan = None
sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
# Sub sockets
poller = zmq.Poller()
@ -436,18 +434,18 @@ def controlsd_thread(gctx=None, rate=100):
logcan = messaging.sub_sock(context, service_list['can'].port)
CC = car.CarControl.new_message()
CI, CP = get_car(logcan, sendcan, 1.0 if passive else None)
CI, CP = get_car(logcan, sendcan)
AM = AlertManager()
if CI is None:
raise Exception("unsupported car")
car_recognized = CP.carName != 'mock'
# If stock camera is disconnected, we loaded car controls and it's not chffrplus
controller_available = CP.enableCamera and CI.CC is not None and not passive
read_only = not car_recognized or not controller_available
if read_only:
CP.safetyModel = car.CarParams.SafetyModels.elm327 # diagnostic only
# if stock camera is connected, then force passive behavior
if not CP.enableCamera:
passive = True
sendcan = None
if passive:
CP.safetyModel = car.CarParams.SafetyModels.noOutput
startup_alert = get_startup_alert(car_recognized, controller_available)
AM.add(startup_alert, False)
LoC = LongControl(CP, CI.compute_gb)
VM = VehicleModel(CP)
@ -457,12 +455,8 @@ def controlsd_thread(gctx=None, rate=100):
else:
LaC = LatControlINDI(CP)
AM = AlertManager()
driver_status = DriverStatus()
if not passive:
AM.add("startup", False)
# Write CarParams for radard and boardd safety mode
params.put("CarParams", CP.to_bytes())
params.put("LongitudinalControl", "1" if CP.openpilotLongitudinalControl else "0")
@ -484,8 +478,10 @@ def controlsd_thread(gctx=None, rate=100):
plan.init('plan')
path_plan = messaging.new_message()
path_plan.init('pathPlan')
path_plan.pathPlan.sensorValid = True
rk = Ratekeeper(rate, print_delay_threshold=2. / 1000)
# controlsd is driven by can recv, expected at 100Hz
rk = Ratekeeper(100, print_delay_threshold=None)
controls_params = params.get("ControlsParams")
# Read angle offset from previous drive
@ -516,6 +512,8 @@ def controlsd_thread(gctx=None, rate=100):
if not path_plan.pathPlan.valid or plan_age > 0.5 or path_plan_age > 0.5:
events.append(create_event('plannerError', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not path_plan.pathPlan.sensorValid:
events.append(create_event('sensorDataInvalid', [ET.NO_ENTRY, ET.PERMANENT]))
if not path_plan.pathPlan.paramsValid:
events.append(create_event('vehicleModelInvalid', [ET.WARNING]))
if not path_plan.pathPlan.modelValid:
@ -529,7 +527,7 @@ def controlsd_thread(gctx=None, rate=100):
if CS.brakePressed and plan.plan.vTargetFuture >= STARTING_TARGET_SPEED and not CP.radarOffCan and CS.vEgo < 0.3:
events.append(create_event('noTarget', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if not passive:
if not read_only:
# update control state
state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = \
state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM)
@ -539,21 +537,21 @@ def controlsd_thread(gctx=None, rate=100):
actuators, v_cruise_kph, driver_status, angle_model_bias, v_acc, a_acc, lac_log = \
state_control(rcv_times, plan.plan, path_plan.pathPlan, CS, CP, state, events, v_cruise_kph,
v_cruise_kph_last, AM, rk, driver_status,
LaC, LoC, VM, angle_model_bias, passive, is_metric, cal_perc)
LaC, LoC, VM, angle_model_bias, read_only, is_metric, cal_perc)
prof.checkpoint("State Control")
# Publish data
CC = data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate, carcontrol,
live100, AM, driver_status, LaC, LoC, angle_model_bias, passive, start_time, v_acc, a_acc, lac_log)
controlsstate, sendcan, AM, driver_status, LaC, LoC, angle_model_bias, read_only, start_time, v_acc, a_acc, lac_log)
prof.checkpoint("Sent")
rk.keep_time() # Run at 100Hz
rk.monitor_time()
prof.display()
def main(gctx=None):
controlsd_thread(gctx, 100)
controlsd_thread(gctx)
if __name__ == "__main__":

4
selfdrive/controls/lib/alertmanager.py
View File

@ -5,8 +5,8 @@ from common.realtime import sec_since_boot
import copy
AlertSize = log.Live100Data.AlertSize
AlertStatus = log.Live100Data.AlertStatus
AlertSize = log.ControlsState.AlertSize
AlertStatus = log.ControlsState.AlertStatus
class AlertManager(object):

32
selfdrive/controls/lib/alerts.py
View File

@ -9,8 +9,8 @@ class Priority:
HIGH = 4
HIGHEST = 5
AlertSize = log.Live100Data.AlertSize
AlertStatus = log.Live100Data.AlertStatus
AlertSize = log.ControlsState.AlertSize
AlertStatus = log.ControlsState.AlertStatus
AudibleAlert = car.CarControl.HUDControl.AudibleAlert
VisualAlert = car.CarControl.HUDControl.VisualAlert
@ -171,6 +171,20 @@ ALERTS = [
AlertStatus.normal, AlertSize.mid,
Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., 15.),
Alert(
"startupNoControl",
"Dashcam mode",
"Always keep hands on wheel and eyes on road",
AlertStatus.normal, AlertSize.mid,
Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., 15.),
Alert(
"startupNoCar",
"Dashcam mode with unsupported car",
"Always keep hands on wheel and eyes on road",
AlertStatus.normal, AlertSize.mid,
Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., 15.),
Alert(
"ethicalDilemma",
"TAKE CONTROL IMMEDIATELY",
@ -277,6 +291,13 @@ ALERTS = [
AlertStatus.normal, AlertSize.mid,
Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
Alert(
"sensorDataInvalidNoEntry",
"openpilot Unavailable",
"No Data from EON Sensors",
AlertStatus.normal, AlertSize.mid,
Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
# Cancellation alerts causing soft disabling
Alert(
"overheat",
@ -625,6 +646,13 @@ ALERTS = [
AlertStatus.normal, AlertSize.mid,
Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2),
Alert(
"sensorDataInvalidPermanent",
"No Data from EON Sensors",
"Reboot your EON",
AlertStatus.normal, AlertSize.mid,
Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2),
Alert(
"vehicleModelInvalid",
"Vehicle Parameter Identification Failed",

13
selfdrive/controls/lib/cluster/LICENSE Normal file
View File

@ -0,0 +1,13 @@
Copyright:
* fastcluster_dm.cpp & fastcluster_R_dm.cpp:
© 2011 Daniel Müllner <http://danifold.net>
* fastcluster.(h|cpp) & demo.cpp & plotresult.r:
© 2018 Christoph Dalitz <http://www.hsnr.de/ipattern/>
All rights reserved.
Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

28
selfdrive/controls/lib/cluster/Makefile Normal file
View File

@ -0,0 +1,28 @@
CC = clang
CXX = clang++
CPPFLAGS = -Wall -g -fPIC -std=c++11 -O2
OBJS = fastcluster.o test.o
DEPS := $(OBJS:.o=.d)
all: libfastcluster.so
test: libfastcluster.so test.o
$(CXX) -g -L. -lfastcluster -o $@ $+
valgrind: test
valgrind --leak-check=full ./test
libfastcluster.so: fastcluster.o
$(CXX) -g -shared -o $@ $+
%.o: %.cpp
$(CXX) $(CPPFLAGS) -MMD -c $*.cpp
clean:
rm -f $(OBJS) $(DEPS) libfastcluster.so test
-include $(DEPS)

79
selfdrive/controls/lib/cluster/README Normal file
View File

@ -0,0 +1,79 @@
C++ interface to fast hierarchical clustering algorithms
========================================================
This is a simplified C++ interface to fast implementations of hierarchical
clustering by Daniel Müllner. The original library with interfaces to R
and Python is described in:
Daniel Müllner: "fastcluster: Fast Hierarchical, Agglomerative Clustering
Routines for R and Python." Journal of Statistical Software 53 (2013),
no. 9, pp. 1–18, http://www.jstatsoft.org/v53/i09/
Usage of the library
--------------------
For using the library, the following source files are needed:
fastcluster_dm.cpp, fastcluster_R_dm.cpp
original code by Daniel Müllner
these are included by fastcluster.cpp via #include, and therefore
need not be compiled to object code
fastcluster.[h|cpp]
simplified C++ interface
fastcluster.cpp is the only file that must be compiled
The library provides the clustering function *hclust_fast* for
creating the dendrogram information in an encoding as used by the
R function *hclust*. For a description of the parameters, see fastcluster.h.
Its parameter *method* can be one of
HCLUST_METHOD_SINGLE
single link with the minimum spanning tree algorithm (Rohlf, 1973)
HHCLUST_METHOD_COMPLETE
complete link with the nearest-neighbor-chain algorithm (Murtagh, 1984)
HCLUST_METHOD_AVERAGE
complete link with the nearest-neighbor-chain algorithm (Murtagh, 1984)
HCLUST_METHOD_MEDIAN
median link with the generic algorithm (Müllner, 2011)
For splitting the dendrogram into clusters, the two functions *cutree_k*
and *cutree_cdist* are provided.
Note that output parameters must be allocated beforehand, e.g.
int* merge = new int[2*(npoints-1)];
For a complete usage example, see lines 135-142 of demo.cpp.
Demonstration program
---------------------
A simple demo is implemented in demo.cpp, which can be compiled and run with
make
./hclust-demo -m complete lines.csv
It creates two clusters of line segments such that the segment angle between
line segments of different clusters have a maximum (cosine) dissimilarity.
For visualizing the result, plotresult.r can be used as follows
(requires R <https://r-project.org> to be installed):
./hclust-demo -m complete lines.csv | Rscript plotresult.r
Authors & Copyright
-------------------
Daniel Müllner, 2011, <http://danifold.net>
Christoph Dalitz, 2018, <http://www.hsnr.de/ipattern/>
License
-------
This code is provided under a BSD-style license.
See the file LICENSE for details.

0
selfdrive/mapd/__init__.py → selfdrive/controls/lib/cluster/__init__.py
View File

218
selfdrive/controls/lib/cluster/fastcluster.cpp Normal file
View File

@ -0,0 +1,218 @@
//
// C++ standalone verion of fastcluster by Daniel Müllner
//
// Copyright: Christoph Dalitz, 2018
// Daniel Müllner, 2011
// License: BSD style license
// (see the file LICENSE for details)
//
#include <vector>
#include <algorithm>
#include <cmath>
extern "C" {
#include "fastcluster.h"
}
// Code by Daniel Müllner
// workaround to make it usable as a standalone version (without R)
bool fc_isnan(double x) { return false; }
#include "fastcluster_dm.cpp"
#include "fastcluster_R_dm.cpp"
extern "C" {
//
// Assigns cluster labels (0, ..., nclust-1) to the n points such
// that the cluster result is split into nclust clusters.
//
// Input arguments:
// n = number of observables
// merge = clustering result in R format
// nclust = number of clusters
// Output arguments:
// labels = allocated integer array of size n for result
//
void cutree_k(int n, const int* merge, int nclust, int* labels) {
int k,m1,m2,j,l;
if (nclust > n || nclust < 2) {
for (j=0; j<n; j++) labels[j] = 0;
return;
}
// assign to each observable the number of its last merge step
// beware: indices of observables in merge start at 1 (R convention)
std::vector<int> last_merge(n, 0);
for (k=1; k<=(n-nclust); k++) {
// (m1,m2) = merge[k,]
m1 = merge[k-1];
m2 = merge[n-1+k-1];
if (m1 < 0 && m2 < 0) { // both single observables
last_merge[-m1-1] = last_merge[-m2-1] = k;
}
else if (m1 < 0 || m2 < 0) { // one is a cluster
if(m1 < 0) { j = -m1; m1 = m2; } else j = -m2;
// merging single observable and cluster
for(l = 0; l < n; l++)
if (last_merge[l] == m1)
last_merge[l] = k;
last_merge[j-1] = k;
}
else { // both cluster
for(l=0; l < n; l++) {
if( last_merge[l] == m1 || last_merge[l] == m2 )
last_merge[l] = k;
}
}
}
// assign cluster labels
int label = 0;
std::vector<int> z(n,-1);
for (j=0; j<n; j++) {
if (last_merge[j] == 0) { // still singleton
labels[j] = label++;
} else {
if (z[last_merge[j]] < 0) {
z[last_merge[j]] = label++;
}
labels[j] = z[last_merge[j]];
}
}
}
//
// Assigns cluster labels (0, ..., nclust-1) to the n points such
// that the hierarchical clustering is stopped when cluster distance >= cdist
//
// Input arguments:
// n = number of observables
// merge = clustering result in R format
// height = cluster distance at each merge step
// cdist = cutoff cluster distance
// Output arguments:
// labels = allocated integer array of size n for result
//
void cutree_cdist(int n, const int* merge, double* height, double cdist, int* labels) {
int k;
for (k=0; k<(n-1); k++) {
if (height[k] >= cdist) {
break;
}
}
cutree_k(n, merge, n-k, labels);
}
//
// Hierarchical clustering with one of Daniel Muellner's fast algorithms
//
// Input arguments:
// n = number of observables
// distmat = condensed distance matrix, i.e. an n*(n-1)/2 array representing
// the upper triangle (without diagonal elements) of the distance
// matrix, e.g. for n=4:
// d00 d01 d02 d03
// d10 d11 d12 d13 -> d01 d02 d03 d12 d13 d23
// d20 d21 d22 d23
// d30 d31 d32 d33
// method = cluster metric (see enum method_code)
// Output arguments:
// merge = allocated (n-1)x2 matrix (2*(n-1) array) for storing result.
// Result follows R hclust convention:
// - observabe indices start with one
// - merge[i][] contains the merged nodes in step i
// - merge[i][j] is negative when the node is an atom
// height = allocated (n-1) array with distances at each merge step
// Return code:
// 0 = ok
// 1 = invalid method
//
int hclust_fast(int n, double* distmat, int method, int* merge, double* height) {
// call appropriate culstering function
cluster_result Z2(n-1);
if (method == HCLUST_METHOD_SINGLE) {
// single link
MST_linkage_core(n, distmat, Z2);
}
else if (method == HCLUST_METHOD_COMPLETE) {
// complete link
NN_chain_core<METHOD_METR_COMPLETE, t_float>(n, distmat, NULL, Z2);
}
else if (method == HCLUST_METHOD_AVERAGE) {
// best average distance
double* members = new double[n];
for (int i=0; i<n; i++) members[i] = 1;
NN_chain_core<METHOD_METR_AVERAGE, t_float>(n, distmat, members, Z2);
delete[] members;
}
else if (method == HCLUST_METHOD_MEDIAN) {
// best median distance (beware: O(n^3))
generic_linkage<METHOD_METR_MEDIAN, t_float>(n, distmat, NULL, Z2);
}
else if (method == HCLUST_METHOD_CENTROID) {
// best centroid distance (beware: O(n^3))
double* members = new double[n];
for (int i=0; i<n; i++) members[i] = 1;
generic_linkage<METHOD_METR_CENTROID, t_float>(n, distmat, members, Z2);
delete[] members;
}
else {
return 1;
}
int* order = new int[n];
if (method == HCLUST_METHOD_MEDIAN || method == HCLUST_METHOD_CENTROID) {
generate_R_dendrogram<true>(merge, height, order, Z2, n);
} else {
generate_R_dendrogram<false>(merge, height, order, Z2, n);
}
delete[] order; // only needed for visualization
return 0;
}
// Build condensed distance matrix
// Input arguments:
// n = number of observables
// m = dimension of observable
// Output arguments:
// out = allocated integer array of size n * (n - 1) / 2 for result
void hclust_pdist(int n, int m, double* pts, double* out) {
int ii = 0;
for (int i = 0; i < n; i++){
for (int j = i + 1; j < n; j++){
// Compute euclidian distance
double d = 0;
for (int k = 0; k < m; k ++){
double error = pts[i * m + k] - pts[j * m + k];
d += (error * error);
}
out[ii] = d;//sqrt(d);
ii++;
}
}
}
void cluster_points_centroid(int n, int m, double* pts, double dist, int* idx) {
double* pdist = new double[n * (n - 1) / 2];
int* merge = new int[2 * (n - 1)];
double* height = new double[n - 1];
hclust_pdist(n, m, pts, pdist);
hclust_fast(n, pdist, HCLUST_METHOD_CENTROID, merge, height);
cutree_cdist(n, merge, height, dist, idx);
delete[] pdist;
delete[] merge;
delete[] height;
}
}

77
selfdrive/controls/lib/cluster/fastcluster.h Normal file
View File

@ -0,0 +1,77 @@
//
// C++ standalone verion of fastcluster by Daniel Muellner
//
// Copyright: Daniel Muellner, 2011
// Christoph Dalitz, 2018
// License: BSD style license
// (see the file LICENSE for details)
//
#ifndef fastclustercpp_H
#define fastclustercpp_H
//
// Assigns cluster labels (0, ..., nclust-1) to the n points such
// that the cluster result is split into nclust clusters.
//
// Input arguments:
// n = number of observables
// merge = clustering result in R format
// nclust = number of clusters
// Output arguments:
// labels = allocated integer array of size n for result
//
void cutree_k(int n, const int* merge, int nclust, int* labels);
//
// Assigns cluster labels (0, ..., nclust-1) to the n points such
// that the hierarchical clsutering is stopped at cluster distance cdist
//
// Input arguments:
// n = number of observables
// merge = clustering result in R format
// height = cluster distance at each merge step
// cdist = cutoff cluster distance
// Output arguments:
// labels = allocated integer array of size n for result
//
void cutree_cdist(int n, const int* merge, double* height, double cdist, int* labels);
//
// Hierarchical clustering with one of Daniel Muellner's fast algorithms
//
// Input arguments:
// n = number of observables
// distmat = condensed distance matrix, i.e. an n*(n-1)/2 array representing
// the upper triangle (without diagonal elements) of the distance
// matrix, e.g. for n=4:
// d00 d01 d02 d03
// d10 d11 d12 d13 -> d01 d02 d03 d12 d13 d23
// d20 d21 d22 d23
// d30 d31 d32 d33
// method = cluster metric (see enum method_code)
// Output arguments:
// merge = allocated (n-1)x2 matrix (2*(n-1) array) for storing result.
// Result follows R hclust convention:
// - observabe indices start with one
// - merge[i][] contains the merged nodes in step i
// - merge[i][j] is negative when the node is an atom
// height = allocated (n-1) array with distances at each merge step
// Return code:
// 0 = ok
// 1 = invalid method
//
int hclust_fast(int n, double* distmat, int method, int* merge, double* height);
enum hclust_fast_methods {
HCLUST_METHOD_SINGLE = 0,
HCLUST_METHOD_COMPLETE = 1,
HCLUST_METHOD_AVERAGE = 2,
HCLUST_METHOD_MEDIAN = 3,
HCLUST_METHOD_CENTROID = 5,
};
void hclust_pdist(int n, int m, double* pts, double* out);
void cluster_points_centroid(int n, int m, double* pts, double dist, int* idx);
#endif

115
selfdrive/controls/lib/cluster/fastcluster_R_dm.cpp Normal file
View File

@ -0,0 +1,115 @@
//
// Excerpt from fastcluster_R.cpp
//
// Copyright: Daniel Müllner, 2011 <http://danifold.net>
//
struct pos_node {
t_index pos;
int node;
};
void order_nodes(const int N, const int * const merge, const t_index * const node_size, int * const order) {
/* Parameters:
N : number of data points
merge : (N-1)×2 array which specifies the node indices which are
merged in each step of the clustering procedure.
Negative entries -1...-N point to singleton nodes, while
positive entries 1...(N-1) point to nodes which are themselves
parents of other nodes.
node_size : array of node sizes - makes it easier
order : output array of size N
Runtime: Θ(N)
*/
auto_array_ptr<pos_node> queue(N/2);
int parent;
int child;
t_index pos = 0;
queue[0].pos = 0;
queue[0].node = N-2;
t_index idx = 1;
do {
--idx;
pos = queue[idx].pos;
parent = queue[idx].node;
// First child
child = merge[parent];
if (child<0) { // singleton node, write this into the 'order' array.
order[pos] = -child;
++pos;
}
else { /* compound node: put it on top of the queue and decompose it
in a later iteration. */
queue[idx].pos = pos;
queue[idx].node = child-1; // convert index-1 based to index-0 based
++idx;
pos += node_size[child-1];
}
// Second child
child = merge[parent+N-1];
if (child<0) {
order[pos] = -child;
}
else {
queue[idx].pos = pos;
queue[idx].node = child-1;
++idx;
}
} while (idx>0);
}
#define size_(r_) ( ((r_<N) ? 1 : node_size[r_-N]) )
template <const bool sorted>
void generate_R_dendrogram(int * const merge, double * const height, int * const order, cluster_result & Z2, const int N) {
// The array "nodes" is a union-find data structure for the cluster
// identites (only needed for unsorted cluster_result input).
union_find nodes(sorted ? 0 : N);
if (!sorted) {
std::stable_sort(Z2[0], Z2[N-1]);
}
t_index node1, node2;
auto_array_ptr<t_index> node_size(N-1);
for (t_index i=0; i<N-1; ++i) {
// Get two data points whose clusters are merged in step i.
// Find the cluster identifiers for these points.
if (sorted) {
node1 = Z2[i]->node1;
node2 = Z2[i]->node2;
}
else {
node1 = nodes.Find(Z2[i]->node1);
node2 = nodes.Find(Z2[i]->node2);
// Merge the nodes in the union-find data structure by making them
// children of a new node.
nodes.Union(node1, node2);
}
// Sort the nodes in the output array.
if (node1>node2) {
t_index tmp = node1;
node1 = node2;
node2 = tmp;
}
/* Conversion between labeling conventions.
Input: singleton nodes 0,...,N-1
compound nodes N,...,2N-2
Output: singleton nodes -1,...,-N
compound nodes 1,...,N
*/
merge[i] = (node1<N) ? -static_cast<int>(node1)-1
: static_cast<int>(node1)-N+1;
merge[i+N-1] = (node2<N) ? -static_cast<int>(node2)-1
: static_cast<int>(node2)-N+1;
height[i] = Z2[i]->dist;
node_size[i] = size_(node1) + size_(node2);
}
order_nodes(N, merge, node_size, order);
}

1794
selfdrive/controls/lib/cluster/fastcluster_dm.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

30
selfdrive/controls/lib/cluster/fastcluster_py.py Normal file
View File

@ -0,0 +1,30 @@
import os
import numpy as np
from cffi import FFI
import subprocess
cluster_dir = os.path.join(os.path.dirname(os.path.abspath(__file__)))
subprocess.check_call(["make", "-j4"], cwd=cluster_dir)
cluster_fn = os.path.join(cluster_dir, "libfastcluster.so")
ffi = FFI()
ffi.cdef("""
int hclust_fast(int n, double* distmat, int method, int* merge, double* height);
void cutree_cdist(int n, const int* merge, double* height, double cdist, int* labels);
void hclust_pdist(int n, int m, double* pts, double* out);
void cluster_points_centroid(int n, int m, double* pts, double dist, int* idx);
""")
hclust = ffi.dlopen(cluster_fn)
def cluster_points_centroid(pts, dist):
pts = np.ascontiguousarray(pts, dtype=np.float64)
pts_ptr = ffi.cast("double *", pts.ctypes.data)
n, m = pts.shape
labels_ptr = ffi.new("int[]", n)
hclust.cluster_points_centroid(n, m, pts_ptr, dist**2, labels_ptr)
return list(labels_ptr)

35
selfdrive/controls/lib/cluster/test.cpp Normal file
View File

@ -0,0 +1,35 @@
#include <cassert>
extern "C" {
#include "fastcluster.h"
}
int main(int argc, const char* argv[]){
const int n = 11;
const int m = 3;
double* pts = new double[n*m]{59.26000137, -9.35999966, -5.42500019,
91.61999817, -0.31999999, -2.75,
31.38000031, 0.40000001, -0.2,
89.57999725, -8.07999992, -18.04999924,
53.42000122, 0.63999999, -0.175,
31.38000031, 0.47999999, -0.2,
36.33999939, 0.16, -0.2,
53.33999939, 0.95999998, -0.175,
59.26000137, -9.76000023, -5.44999981,
33.93999977, 0.40000001, -0.22499999,
106.74000092, -5.76000023, -18.04999924};
int * idx = new int[n];
int * correct_idx = new int[n]{0, 1, 2, 3, 4, 2, 5, 4, 0, 5, 6};
cluster_points_centroid(n, m, pts, 2.5 * 2.5, idx);
for (int i = 0; i < n; i++){
assert(idx[i] == correct_idx[i]);
}
delete[] idx;
delete[] correct_idx;
delete[] pts;
}

2
selfdrive/controls/lib/latcontrol_indi.py
View File

@ -51,7 +51,7 @@ class LatControlINDI(object):
y = np.matrix([[math.radians(angle_steers)], [math.radians(angle_steers_rate)]])
self.x = np.dot(self.A_K, self.x) + np.dot(self.K, y)
indi_log = log.Live100Data.LateralINDIState.new_message()
indi_log = log.ControlsState.LateralINDIState.new_message()
indi_log.steerAngle = math.degrees(self.x[0])
indi_log.steerRate = math.degrees(self.x[1])
indi_log.steerAccel = math.degrees(self.x[2])

2
selfdrive/controls/lib/latcontrol_pid.py
View File

@ -15,7 +15,7 @@ class LatControlPID(object):
self.pid.reset()
def update(self, active, v_ego, angle_steers, angle_steers_rate, steer_override, CP, VM, path_plan):
pid_log = log.Live100Data.LateralPIDState.new_message()
pid_log = log.ControlsState.LateralPIDState.new_message()
pid_log.steerAngle = float(angle_steers)
pid_log.steerRate = float(angle_steers_rate)

2
selfdrive/controls/lib/longcontrol.py
View File

@ -2,7 +2,7 @@ from cereal import log
from common.numpy_fast import clip, interp
from selfdrive.controls.lib.pid import PIController
LongCtrlState = log.Live100Data.LongControlState
LongCtrlState = log.ControlsState.LongControlState
STOPPING_EGO_SPEED = 0.5
MIN_CAN_SPEED = 0.3 # TODO: parametrize this in car interface

11
selfdrive/controls/lib/pathplanner.py
View File

@ -20,10 +20,10 @@ def calc_states_after_delay(states, v_ego, steer_angle, curvature_factor, steer_
class PathPlanner(object):
def __init__(self, CP):
self.MP = ModelParser()
self.l_poly = [0., 0., 0., 0.]
self.r_poly = [0., 0., 0., 0.]
self.last_cloudlog_t = 0
context = zmq.Context()
@ -49,13 +49,13 @@ class PathPlanner(object):
self.angle_steers_des_prev = 0.0
self.angle_steers_des_time = 0.0
def update(self, rcv_times, CP, VM, CS, md, live100, live_parameters):
def update(self, rcv_times, CP, VM, CS, md, controls_state, live_parameters):
v_ego = CS.carState.vEgo
angle_steers = CS.carState.steeringAngle
active = live100.live100.active
active = controls_state.controlsState.active
angle_offset_average = live_parameters.liveParameters.angleOffsetAverage
angle_offset_bias = live100.live100.angleModelBias + angle_offset_average
angle_offset_bias = controls_state.controlsState.angleModelBias + angle_offset_average
self.MP.update(v_ego, md)
@ -123,6 +123,7 @@ class PathPlanner(object):
plan_send.pathPlan.angleOffset = float(angle_offset_average)
plan_send.pathPlan.valid = bool(plan_valid)
plan_send.pathPlan.paramsValid = bool(live_parameters.liveParameters.valid)
plan_send.pathPlan.sensorValid = bool(live_parameters.liveParameters.sensorValid)
plan_send.pathPlan.modelValid = bool(not model_dead)
self.plan.send(plan_send.to_bytes())

26
selfdrive/controls/lib/planner.py
View File

@ -114,18 +114,18 @@ class Planner(object):
self.v_acc_future = min([self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint])
def update(self, rcv_times, CS, CP, VM, PP, live20, live100, md, live_map_data):
"""Gets called when new live20 is available"""
def update(self, rcv_times, CS, CP, VM, PP, radar_state, controls_state, md, live_map_data):
"""Gets called when new radarState is available"""
cur_time = sec_since_boot()
v_ego = CS.carState.vEgo
long_control_state = live100.live100.longControlState
v_cruise_kph = live100.live100.vCruise
force_slow_decel = live100.live100.forceDecel
long_control_state = controls_state.controlsState.longControlState
v_cruise_kph = controls_state.controlsState.vCruise
force_slow_decel = controls_state.controlsState.forceDecel
v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS
lead_1 = live20.live20.leadOne
lead_2 = live20.live20.leadTwo
lead_1 = radar_state.radarState.leadOne
lead_2 = radar_state.radarState.leadTwo
enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping)
following = lead_1.status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0
@ -209,18 +209,18 @@ class Planner(object):
if fcw:
cloudlog.info("FCW triggered %s", self.fcw_checker.counters)
radar_dead = cur_time - rcv_times['live20'] > 0.5
radar_dead = cur_time - rcv_times['radarState'] > 0.5
radar_errors = list(live20.live20.radarErrors)
radar_fault = car.RadarState.Error.fault in radar_errors
radar_comm_issue = car.RadarState.Error.commIssue in radar_errors
radar_errors = list(radar_state.radarState.radarErrors)
radar_fault = car.RadarData.Error.fault in radar_errors
radar_comm_issue = car.RadarData.Error.commIssue in radar_errors
# **** send the plan ****
plan_send = messaging.new_message()
plan_send.init('plan')
plan_send.plan.mdMonoTime = md.logMonoTime
plan_send.plan.l20MonoTime = live20.logMonoTime
plan_send.plan.radarStateMonoTime = radar_state.logMonoTime
# longitudal plan
plan_send.plan.vCruise = self.v_cruise
@ -241,7 +241,7 @@ class Planner(object):
plan_send.plan.radarValid = bool(radar_valid)
plan_send.plan.radarCommIssue = bool(radar_comm_issue)
plan_send.plan.processingDelay = (plan_send.logMonoTime / 1e9) - rcv_times['live20']
plan_send.plan.processingDelay = (plan_send.logMonoTime / 1e9) - rcv_times['radarState']
# Send out fcw
fcw = fcw and (self.fcw_enabled or long_control_state != LongCtrlState.off)

26
selfdrive/controls/lib/radar_helpers.py
View File

@ -1,6 +1,3 @@
import os
import sys
import platform
import numpy as np
from common.numpy_fast import clip, interp
@ -121,27 +118,6 @@ class Track(object):
return [self.dRel, self.yRel*2, self.vRel]
# ******************* Cluster *******************
if platform.machine() == 'aarch64':
for x in sys.path:
pp = os.path.join(x, "phonelibs/hierarchy/lib")
if os.path.isfile(os.path.join(pp, "_hierarchy.so")):
sys.path.append(pp)
break
import _hierarchy #pylint: disable=import-error
else:
from scipy.cluster import _hierarchy
def fcluster(Z, t, criterion='inconsistent', depth=2, R=None, monocrit=None):
# supersimplified function to get fast clustering. Got it from scipy
Z = np.asarray(Z, order='c')
n = Z.shape[0] + 1
T = np.zeros((n,), dtype='i')
_hierarchy.cluster_dist(Z, T, float(t), int(n))
return T
def mean(l):
return sum(l) / len(l)
@ -215,7 +191,7 @@ class Cluster(object):
def oncoming(self):
return all([t.oncoming for t in self.tracks])
def toLive20(self):
def toRadarState(self):
return {
"dRel": float(self.dRel) - RDR_TO_LDR,
"yRel": float(self.yRel),

39
selfdrive/controls/plannerd.py
View File

@ -1,10 +1,11 @@
#!/usr/bin/env python
import gc
import zmq
from collections import defaultdict
from cereal import car
from common.params import Params
from common.realtime import sec_since_boot
from common.realtime import sec_since_boot, set_realtime_priority
from selfdrive.swaglog import cloudlog
from selfdrive.services import service_list
from selfdrive.controls.lib.planner import Planner
@ -14,6 +15,11 @@ import selfdrive.messaging as messaging
def plannerd_thread():
gc.disable()
# start the loop
set_realtime_priority(2)
context = zmq.Context()
params = Params()
@ -31,26 +37,27 @@ def plannerd_thread():
poller = zmq.Poller()
car_state_sock = messaging.sub_sock(context, service_list['carState'].port, conflate=True, poller=poller)
live100_sock = messaging.sub_sock(context, service_list['live100'].port, conflate=True, poller=poller)
live20_sock = messaging.sub_sock(context, service_list['live20'].port, conflate=True, poller=poller)
controls_state_sock = messaging.sub_sock(context, service_list['controlsState'].port, conflate=True, poller=poller)
radar_state_sock = messaging.sub_sock(context, service_list['radarState'].port, conflate=True, poller=poller)
model_sock = messaging.sub_sock(context, service_list['model'].port, conflate=True, poller=poller)
live_map_data_sock = messaging.sub_sock(context, service_list['liveMapData'].port, conflate=True, poller=poller)
live_parameters_sock = messaging.sub_sock(context, service_list['liveParameters'].port, conflate=True, poller=poller)
# live_map_data_sock = messaging.sub_sock(context, service_list['liveMapData'].port, conflate=True, poller=poller)
car_state = messaging.new_message()
car_state.init('carState')
live100 = messaging.new_message()
live100.init('live100')
controls_state = messaging.new_message()
controls_state.init('controlsState')
model = messaging.new_message()
model.init('model')
live20 = messaging.new_message()
live20.init('live20')
radar_state = messaging.new_message()
radar_state.init('radarState')
live_map_data = messaging.new_message()
live_map_data.init('liveMapData')
live_parameters = messaging.new_message()
live_parameters.init('liveParameters')
live_parameters.liveParameters.valid = True
live_parameters.liveParameters.sensorValid = True
live_parameters.liveParameters.steerRatio = CP.steerRatio
live_parameters.liveParameters.stiffnessFactor = 1.0
@ -61,20 +68,20 @@ def plannerd_thread():
msg = messaging.recv_one(socket)
rcv_times[msg.which()] = sec_since_boot()
if socket is live100_sock:
live100 = msg
if socket is controls_state_sock:
controls_state = msg
elif socket is car_state_sock:
car_state = msg
elif socket is live_parameters_sock:
live_parameters = msg
elif socket is model_sock:
model = msg
PP.update(rcv_times, CP, VM, car_state, model, live100, live_parameters)
elif socket is live_map_data_sock:
live_map_data = msg
elif socket is live20_sock:
live20 = msg
PL.update(rcv_times, car_state, CP, VM, PP, live20, live100, model, live_map_data)
PP.update(rcv_times, CP, VM, car_state, model, controls_state, live_parameters)
elif socket is radar_state_sock:
radar_state = msg
PL.update(rcv_times, car_state, CP, VM, PP, radar_state, controls_state, model, live_map_data)
# elif socket is live_map_data_sock:
# live_map_data = msg
def main(gctx=None):

72
selfdrive/controls/radard.py
View File

@ -4,14 +4,15 @@ import numpy as np
import numpy.matlib
import importlib
from collections import defaultdict, deque
from fastcluster import linkage_vector
import selfdrive.messaging as messaging
from selfdrive.services import service_list
from selfdrive.controls.lib.latcontrol_helpers import calc_lookahead_offset
from selfdrive.controls.lib.model_parser import ModelParser
from selfdrive.controls.lib.radar_helpers import Track, Cluster, fcluster, \
from selfdrive.controls.lib.radar_helpers import Track, Cluster, \
RDR_TO_LDR, NO_FUSION_SCORE
from selfdrive.controls.lib.cluster.fastcluster_py import cluster_points_centroid
from selfdrive.controls.lib.vehicle_model import VehicleModel
from selfdrive.swaglog import cloudlog
from cereal import car
@ -45,8 +46,6 @@ class EKFV1D(EKF):
## fuses camera and radar data for best lead detection
# FIXME: radard has a memory leak of about 50MB/hr
# BOUNTY: $100 coupon on shop.comma.ai
def radard_thread(gctx=None):
set_realtime_priority(2)
@ -65,7 +64,7 @@ def radard_thread(gctx=None):
# *** subscribe to features and model from visiond
poller = zmq.Poller()
model = messaging.sub_sock(context, service_list['model'].port, conflate=True, poller=poller)
live100 = messaging.sub_sock(context, service_list['live100'].port, conflate=True, poller=poller)
controls_state_sock = messaging.sub_sock(context, service_list['controlsState'].port, conflate=True, poller=poller)
live_parameters_sock = messaging.sub_sock(context, service_list['liveParameters'].port, conflate=True, poller=poller)
# Default parameters
@ -79,10 +78,10 @@ def radard_thread(gctx=None):
RI = RadarInterface(CP)
last_md_ts = 0
last_l100_ts = 0
last_controls_state_ts = 0
# *** publish live20 and liveTracks
live20 = messaging.pub_sock(context, service_list['live20'].port)
# *** publish radarState and liveTracks
radarState = messaging.pub_sock(context, service_list['radarState'].port)
liveTracks = messaging.pub_sock(context, service_list['liveTracks'].port)
path_x = np.arange(0.0, 140.0, 0.1) # 140 meters is max
@ -108,7 +107,7 @@ def radard_thread(gctx=None):
v_ego_hist_v = deque(maxlen=v_len)
v_ego_t_aligned = 0.
rk = Ratekeeper(rate, print_delay_threshold=np.inf)
rk = Ratekeeper(rate, print_delay_threshold=None)
while 1:
rr = RI.update()
@ -116,29 +115,29 @@ def radard_thread(gctx=None):
for pt in rr.points:
ar_pts[pt.trackId] = [pt.dRel + RDR_TO_LDR, pt.yRel, pt.vRel, pt.measured]
# receive the live100s
l100 = None
# receive the controlsStates
controls_state = None
md = None
for socket, event in poller.poll(0):
if socket is live100:
l100 = messaging.recv_one(socket)
if socket is controls_state_sock:
controls_state = messaging.recv_one(socket)
elif socket is model:
md = messaging.recv_one(socket)
elif socket is live_parameters_sock:
live_parameters = messaging.recv_one(socket)
VM.update_params(live_parameters.liveParameters.stiffnessFactor, live_parameters.liveParameters.steerRatio)
if l100 is not None:
active = l100.live100.active
v_ego = l100.live100.vEgo
steer_angle = l100.live100.angleSteers
steer_override = l100.live100.steerOverride
if controls_state is not None:
active = controls_state.controlsState.active
v_ego = controls_state.controlsState.vEgo
steer_angle = controls_state.controlsState.angleSteers
steer_override = controls_state.controlsState.steerOverride
v_ego_hist_v.append(v_ego)
v_ego_hist_t.append(float(rk.frame)/rate)
last_l100_ts = l100.logMonoTime
last_controls_state_ts = controls_state.logMonoTime
if v_ego is None:
continue
@ -231,16 +230,15 @@ def radard_thread(gctx=None):
# If we have multiple points, cluster them
if len(track_pts) > 1:
link = linkage_vector(track_pts, method='centroid')
cluster_idxs = fcluster(link, 2.5, criterion='distance')
clusters = [None]*max(cluster_idxs)
cluster_idxs = cluster_points_centroid(track_pts, 2.5)
clusters = [None] * (max(cluster_idxs) + 1)
for idx in xrange(len(track_pts)):
cluster_i = cluster_idxs[idx]-1
if clusters[cluster_i] == None:
cluster_i = cluster_idxs[idx]
if clusters[cluster_i] is None:
clusters[cluster_i] = Cluster()
clusters[cluster_i].add(tracks[idens[idx]])
elif len(track_pts) == 1:
# TODO: why do we need this?
clusters = [Cluster()]
@ -263,24 +261,24 @@ def radard_thread(gctx=None):
lead2_clusters.sort(key=lambda x: x.dRel)
lead2_len = len(lead2_clusters)
# *** publish live20 ***
# *** publish radarState ***
dat = messaging.new_message()
dat.init('live20')
dat.live20.mdMonoTime = last_md_ts
dat.live20.canMonoTimes = list(rr.canMonoTimes)
dat.live20.radarErrors = list(rr.errors)
dat.live20.l100MonoTime = last_l100_ts
dat.init('radarState')
dat.radarState.mdMonoTime = last_md_ts
dat.radarState.canMonoTimes = list(rr.canMonoTimes)
dat.radarState.radarErrors = list(rr.errors)
dat.radarState.controlsStateMonoTime = last_controls_state_ts
if lead_len > 0:
dat.live20.leadOne = lead_clusters[0].toLive20()
dat.radarState.leadOne = lead_clusters[0].toRadarState()
if lead2_len > 0:
dat.live20.leadTwo = lead2_clusters[0].toLive20()
dat.radarState.leadTwo = lead2_clusters[0].toRadarState()
else:
dat.live20.leadTwo.status = False
dat.radarState.leadTwo.status = False
else:
dat.live20.leadOne.status = False
dat.radarState.leadOne.status = False
dat.live20.cumLagMs = -rk.remaining*1000.
live20.send(dat.to_bytes())
dat.radarState.cumLagMs = -rk.remaining*1000.
radarState.send(dat.to_bytes())
# *** publish tracks for UI debugging (keep last) ***
dat = messaging.new_message()

138
selfdrive/controls/tests/test_clustering.py Normal file
View File

@ -0,0 +1,138 @@
import time
import unittest
import numpy as np
from fastcluster import linkage_vector
from scipy.cluster import _hierarchy
from scipy.spatial.distance import pdist
from selfdrive.controls.lib.cluster.fastcluster_py import hclust, ffi
from selfdrive.controls.lib.cluster.fastcluster_py import cluster_points_centroid
def fcluster(Z, t, criterion='inconsistent', depth=2, R=None, monocrit=None):
# supersimplified function to get fast clustering. Got it from scipy
Z = np.asarray(Z, order='c')
n = Z.shape[0] + 1
T = np.zeros((n,), dtype='i')
_hierarchy.cluster_dist(Z, T, float(t), int(n))
return T
TRACK_PTS = np.array([[59.26000137, -9.35999966, -5.42500019],
[91.61999817, -0.31999999, -2.75],
[31.38000031, 0.40000001, -0.2],
[89.57999725, -8.07999992, -18.04999924],
[53.42000122, 0.63999999, -0.175],
[31.38000031, 0.47999999, -0.2],
[36.33999939, 0.16, -0.2],
[53.33999939, 0.95999998, -0.175],
[59.26000137, -9.76000023, -5.44999981],
[33.93999977, 0.40000001, -0.22499999],
[106.74000092, -5.76000023, -18.04999924]])
CORRECT_LINK = np.array([[2., 5., 0.07999998, 2.],
[4., 7., 0.32984889, 2.],
[0., 8., 0.40078104, 2.],
[6., 9., 2.41209933, 2.],
[11., 14., 3.76342275, 4.],
[12., 13., 13.02297651, 4.],
[1., 3., 17.27626057, 2.],
[10., 17., 17.92918845, 3.],
[15., 16., 23.68525366, 8.],
[18., 19., 52.52351319, 11.]])
CORRECT_LABELS = np.array([7, 1, 4, 2, 6, 4, 5, 6, 7, 5, 3], dtype=np.int32)
def plot_cluster(pts, idx_old, idx_new):
import matplotlib.pyplot as plt
m = 'Set1'
plt.figure()
plt.subplot(1, 2, 1)
plt.scatter(pts[:, 0], pts[:, 1], c=idx_old, cmap=m)
plt.title("Old")
plt.colorbar()
plt.subplot(1, 2, 2)
plt.scatter(pts[:, 0], pts[:, 1], c=idx_new, cmap=m)
plt.title("New")
plt.colorbar()
plt.show()
def same_clusters(correct, other):
correct = np.asarray(correct)
other = np.asarray(other)
if len(correct) != len(other):
return False
for i in range(len(correct)):
c = np.where(correct == correct[i])
o = np.where(other == other[i])
if not np.array_equal(c, o):
return False
return True
class TestClustering(unittest.TestCase):
def test_scipy_clustering(self):
old_link = linkage_vector(TRACK_PTS, method='centroid')
old_cluster_idxs = fcluster(old_link, 2.5, criterion='distance')
np.testing.assert_allclose(old_link, CORRECT_LINK)
np.testing.assert_allclose(old_cluster_idxs, CORRECT_LABELS)
def test_pdist(self):
pts = np.ascontiguousarray(TRACK_PTS, dtype=np.float64)
pts_ptr = ffi.cast("double *", pts.ctypes.data)
n, m = pts.shape
out = np.zeros((n * (n - 1) / 2, ), dtype=np.float64)
out_ptr = ffi.cast("double *", out.ctypes.data)
hclust.hclust_pdist(n, m, pts_ptr, out_ptr)
np.testing.assert_allclose(out, np.power(pdist(TRACK_PTS), 2))
def test_cpp_clustering(self):
pts = np.ascontiguousarray(TRACK_PTS, dtype=np.float64)
pts_ptr = ffi.cast("double *", pts.ctypes.data)
n, m = pts.shape
labels = np.zeros((n, ), dtype=np.int32)
labels_ptr = ffi.cast("int *", labels.ctypes.data)
hclust.cluster_points_centroid(n, m, pts_ptr, 2.5**2, labels_ptr)
self.assertTrue(same_clusters(CORRECT_LABELS, labels))
def test_cpp_wrapper_clustering(self):
labels = cluster_points_centroid(TRACK_PTS, 2.5)
self.assertTrue(same_clusters(CORRECT_LABELS, labels))
def test_random_cluster(self):
np.random.seed(1337)
N = 1000
t_old = 0.
t_new = 0.
for _ in range(N):
n = int(np.random.uniform(2, 32))
x = np.random.uniform(-10, 50, (n, 1))
y = np.random.uniform(-5, 5, (n, 1))
vrel = np.random.uniform(-5, 5, (n, 1))
pts = np.hstack([x, y, vrel])
t = time.time()
old_link = linkage_vector(pts, method='centroid')
old_cluster_idx = fcluster(old_link, 2.5, criterion='distance')
t_old += time.time() - t
t = time.time()
cluster_idx = cluster_points_centroid(pts, 2.5)
t_new += time.time() - t
self.assertTrue(same_clusters(old_cluster_idx, cluster_idx))
if __name__ == "__main__":
unittest.main()

2
selfdrive/controls/tests/test_following_distance.py
View File

@ -51,7 +51,7 @@ def run_following_distance_simulation(v_lead, t_end=200.0):
CS.carState.aEgo = a_ego
# Setup lead packet
lead = log.Live20Data.LeadData.new_message()
lead = log.RadarState.LeadData.new_message()
lead.status = True
lead.dRel = x_lead - x_ego
lead.vLead = v_lead

1
selfdrive/debug/cpu_usage_stat.py Normal file → Executable file
View File

@ -1,3 +1,4 @@
#!/usr/bin/env python
import psutil
import time
import os

21
selfdrive/locationd/kalman/chi2_lookup.py Normal file
View File

@ -0,0 +1,21 @@
import numpy as np
import os
def gen_chi2_ppf_lookup(max_dim=200):
from scipy.stats import chi2
table = np.zeros((max_dim, 98))
for dim in range(1,max_dim):
table[dim] = chi2.ppf(np.arange(.01, .99, .01), dim)
#outfile = open('chi2_lookup_table', 'w')
np.save('chi2_lookup_table', table)
def chi2_ppf(p, dim):
table = np.load(os.path.dirname(os.path.realpath(__file__)) + '/chi2_lookup_table.npy')
result = np.interp(p, np.arange(.01, .99, .01), table[dim])
return result
if __name__== "__main__":
gen_chi2_ppf_lookup()

3
selfdrive/locationd/kalman/chi2_lookup_table.npy Normal file
View File

@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:0cc301b3a918cce76bd119a219e31722c6a03fa837927eb10fd84e877966cc3e
size 156928

16
selfdrive/locationd/kalman/ekf_sym.py
View File

@ -5,8 +5,7 @@ import numpy as np
from numpy import dot
from common.ffi_wrapper import compile_code, wrap_compiled
from common.sympy_helpers import sympy_into_c
import scipy
from scipy.stats import chi2
from chi2_lookup import chi2_ppf
EXTERNAL_PATH = os.path.dirname(os.path.abspath(__file__))
@ -19,12 +18,11 @@ def solve(a, b):
return np.linalg.solve(a, b)
def null(H, eps=1e-12):
from scipy import linalg
u, s, vh = linalg.svd(H)
u, s, vh = np.linalg.svd(H)
padding = max(0,np.shape(H)[1]-np.shape(s)[0])
null_mask = np.concatenate(((s <= eps), np.ones((padding,),dtype=bool)),axis=0)
null_space = scipy.compress(null_mask, vh, axis=0)
return scipy.transpose(null_space)
null_space = np.compress(null_mask, vh, axis=0)
return np.transpose(null_space)
def gen_code(name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_params=None, msckf_params=None, maha_test_kinds=[]):
# optional state transition matrix, H modifier
@ -121,7 +119,7 @@ def gen_code(name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_params=No
else:
He_str = 'NULL'
# ea_dim = 1 # not really dim of ea but makes c function work
maha_thresh = chi2.ppf(0.95, int(h_sym.shape[0])) # mahalanobis distance for outlier detection
maha_thresh = chi2_ppf(0.95, int(h_sym.shape[0])) # mahalanobis distance for outlier detection
maha_test = kind in maha_test_kinds
extra_post += """
void update_%d(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) {
@ -471,7 +469,7 @@ class EKF_sym(object):
if self.msckf and kind in self.maha_test_kinds:
a = np.linalg.inv(H.dot(P).dot(H.T) + R)
maha_dist = y.T.dot(a.dot(y))
if maha_dist > chi2.ppf(0.95, y.shape[0]):
if maha_dist > chi2_ppf(0.95, y.shape[0]):
R = 10e16*R
# *** same below this line ***
@ -513,7 +511,7 @@ class EKF_sym(object):
a = np.linalg.inv(H.dot(P).dot(H.T) + R)
maha_dist = y.T.dot(a.dot(y))
if maha_dist > chi2.ppf(maha_thresh, y.shape[0]):
if maha_dist > chi2_ppf(maha_thresh, y.shape[0]):
return False
else:
return True

36
selfdrive/locationd/locationd_local.py
View File

@ -32,11 +32,13 @@ MAX_SR_TH = 1.9
LEARNING_RATE = 3
class Localizer(object):
def __init__(self, disabled_logs=None, dog=None):
self.kf = LocLocalKalman()
self.reset_kalman()
self.sensor_data_t = 0.0
self.max_age = .2 # seconds
self.calibration_valid = False
@ -73,10 +75,10 @@ class Localizer(object):
self.update_kalman(current_time, ObservationKind.CAMERA_ODO_TRANSLATION, np.concatenate([log.cameraOdometry.trans,
log.cameraOdometry.transStd]))
def handle_live100(self, log, current_time):
def handle_controls_state(self, log, current_time):
self.speed_counter += 1
if self.speed_counter % 5 == 0:
self.update_kalman(current_time, ObservationKind.ODOMETRIC_SPEED, np.array([log.live100.vEgo]))
self.update_kalman(current_time, ObservationKind.ODOMETRIC_SPEED, np.array([log.controlsState.vEgo]))
def handle_sensors(self, log, current_time):
for sensor_reading in log.sensorEvents:
@ -92,9 +94,10 @@ class Localizer(object):
if typ in self.disabled_logs:
return
if typ == "sensorEvents":
self.sensor_data_t = current_time
self.handle_sensors(log, current_time)
elif typ == "live100":
self.handle_live100(log, current_time)
elif typ == "controlsState":
self.handle_controls_state(log, current_time)
elif typ == "cameraOdometry":
self.handle_cam_odo(log, current_time)
@ -173,7 +176,7 @@ def locationd_thread(gctx, addr, disabled_logs):
ctx = zmq.Context()
poller = zmq.Poller()
live100_socket = messaging.sub_sock(ctx, service_list['live100'].port, poller, addr=addr, conflate=True)
controls_state_socket = messaging.sub_sock(ctx, service_list['controlsState'].port, poller, addr=addr, conflate=True)
sensor_events_socket = messaging.sub_sock(ctx, service_list['sensorEvents'].port, poller, addr=addr, conflate=True)
camera_odometry_socket = messaging.sub_sock(ctx, service_list['cameraOdometry'].port, poller, addr=addr, conflate=True)
@ -191,17 +194,19 @@ def locationd_thread(gctx, addr, disabled_logs):
# Check if car model matches
if params is not None:
params = json.loads(params)
if params.get('carFingerprint', None) != CP.carFingerprint:
if (params.get('carFingerprint', None) != CP.carFingerprint) or (params.get('carVin', CP.carVin) != CP.carVin):
cloudlog.info("Parameter learner found parameters for wrong car.")
params = None
if params is None:
params = {
'carFingerprint': CP.carFingerprint,
'carVin': CP.carVin,
'angleOffsetAverage': 0.0,
'stiffnessFactor': 1.0,
'steerRatio': VM.sR,
}
params_reader.put("LiveParameters", json.dumps(params))
cloudlog.info("Parameter learner resetting to default values")
cloudlog.info("Parameter starting with: %s" % str(params))
@ -213,29 +218,33 @@ def locationd_thread(gctx, addr, disabled_logs):
steer_ratio=params['steerRatio'],
learning_rate=LEARNING_RATE)
i = 0
i = 1
while True:
for socket, event in poller.poll(timeout=1000):
log = messaging.recv_one(socket)
localizer.handle_log(log)
if socket is live100_socket:
if socket is controls_state_socket:
if not localizer.kf.t:
continue
if i % LEARNING_RATE == 0:
# live100 is not updating the Kalman Filter, so update KF manually
# controlsState is not updating the Kalman Filter, so update KF manually
localizer.kf.predict(1e-9 * log.logMonoTime)
predicted_state = localizer.kf.x
yaw_rate = -float(predicted_state[5])
steering_angle = math.radians(log.live100.angleSteers)
params_valid = learner.update(yaw_rate, log.live100.vEgo, steering_angle)
steering_angle = math.radians(log.controlsState.angleSteers)
params_valid = learner.update(yaw_rate, log.controlsState.vEgo, steering_angle)
log_t = 1e-9 * log.logMonoTime
sensor_data_age = log_t - localizer.sensor_data_t
params = messaging.new_message()
params.init('liveParameters')
params.liveParameters.valid = bool(params_valid)
params.liveParameters.sensorValid = bool(sensor_data_age < 5.0)
params.liveParameters.angleOffset = float(math.degrees(learner.ao))
params.liveParameters.angleOffsetAverage = float(math.degrees(learner.slow_ao))
params.liveParameters.stiffnessFactor = float(learner.x)
@ -245,8 +254,9 @@ def locationd_thread(gctx, addr, disabled_logs):
if i % 6000 == 0: # once a minute
params = learner.get_values()
params['carFingerprint'] = CP.carFingerprint
params['carVin'] = CP.carVin
params_reader.put("LiveParameters", json.dumps(params))
params_reader.put("ControlsParams", json.dumps({'angle_model_bias': log.live100.angleModelBias}))
params_reader.put("ControlsParams", json.dumps({'angle_model_bias': log.controlsState.angleModelBias}))
i += 1
elif socket is camera_odometry_socket:
@ -264,7 +274,7 @@ def main(gctx=None, addr="127.0.0.1"):
disabled_logs = os.getenv("DISABLED_LOGS", "").split(",")
# No speed for now
disabled_logs.append('live100')
disabled_logs.append('controlsState')
if IN_CAR:
addr = "192.168.5.11"

6
selfdrive/locationd/ubloxd_main.cc
View File

@ -72,7 +72,7 @@ int ubloxd_main(poll_ubloxraw_msg_func poll_func, send_gps_event_func send_func)
// New message available
if(parser.msg_class() == CLASS_NAV) {
if(parser.msg_id() == MSG_NAV_PVT) {
LOGD("MSG_NAV_PVT");
//LOGD("MSG_NAV_PVT");
auto words = parser.gen_solution();
if(words.size() > 0) {
auto bytes = words.asBytes();
@ -82,14 +82,14 @@ int ubloxd_main(poll_ubloxraw_msg_func poll_func, send_gps_event_func send_func)
LOGW("Unknown nav msg id: 0x%02X", parser.msg_id());
} else if(parser.msg_class() == CLASS_RXM) {
if(parser.msg_id() == MSG_RXM_RAW) {
LOGD("MSG_RXM_RAW");
//LOGD("MSG_RXM_RAW");
auto words = parser.gen_raw();
if(words.size() > 0) {
auto bytes = words.asBytes();
send_func(parser.msg_class(), parser.msg_id(), ubloxGnss, bytes.begin(), bytes.size(), 0);
}
} else if(parser.msg_id() == MSG_RXM_SFRBX) {
LOGD("MSG_RXM_SFRBX");
//LOGD("MSG_RXM_SFRBX");
auto words = parser.gen_nav_data();
if(words.size() > 0) {
auto bytes = words.asBytes();

10
selfdrive/loggerd/config.py
View File

@ -7,3 +7,13 @@ else:
ROOT = '/data/media/0/realdata/'
SEGMENT_LENGTH = 60
def get_available_percent():
try:
statvfs = os.statvfs(ROOT)
available_percent = 100.0 * statvfs.f_bavail / statvfs.f_blocks
except OSError:
available_percent = 100.0
return available_percent

5
selfdrive/loggerd/deleter.py
View File

@ -3,14 +3,13 @@ import os
import shutil
import threading
from selfdrive.swaglog import cloudlog
from selfdrive.loggerd.config import ROOT
from selfdrive.loggerd.config import ROOT, get_available_percent
from selfdrive.loggerd.uploader import listdir_by_creation_date
def deleter_thread(exit_event):
while not exit_event.is_set():
statvfs = os.statvfs(ROOT)
available_percent = 100.0 * statvfs.f_bavail / statvfs.f_blocks
available_percent = get_available_percent()
if available_percent < 10.0:
# remove the earliest directory we can

2
selfdrive/loggerd/loggerd
View File

@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:ac05170d2fbe9f5b65080edbb77cf408a1c90099575c1a3409d0a1ef75cd7a24
oid sha256:f340b8fa3d09329639271febcec3ba0c372e540a98ef47f7454811ddd575c253
size 1703704

5
selfdrive/loggerd/tests/fill_eon.py
View File

@ -3,7 +3,7 @@
import os
from selfdrive.loggerd.config import ROOT
from selfdrive.loggerd.config import ROOT, get_available_percent
from selfdrive.loggerd.tests.loggerd_tests_common import create_random_file
@ -21,7 +21,6 @@ if __name__ == "__main__":
segment_idx += 1
# Fill up to 99 percent
statvfs = os.statvfs(ROOT)
available_percent = 100.0 * statvfs.f_bavail / statvfs.f_blocks
available_percent = get_available_percent()
if available_percent < 1.0:
break

1
selfdrive/logmessaged.py
View File

@ -28,6 +28,7 @@ def main(gctx):
if levelnum >= le_level:
# push to logentries
# TODO: push to athena instead
le_handler.emit_raw(dat)
# then we publish them

15
selfdrive/manager.py
View File

@ -93,7 +93,6 @@ managed_processes = {
"plannerd": "selfdrive.controls.plannerd",
"radard": "selfdrive.controls.radard",
"ubloxd": ("selfdrive/locationd", ["./ubloxd"]),
"mapd": "selfdrive.mapd.mapd",
"loggerd": ("selfdrive/loggerd", ["./loggerd"]),
"logmessaged": "selfdrive.logmessaged",
"tombstoned": "selfdrive.tombstoned",
@ -101,7 +100,7 @@ managed_processes = {
"proclogd": ("selfdrive/proclogd", ["./proclogd"]),
"boardd": ("selfdrive/boardd", ["./boardd"]), # not used directly
"pandad": "selfdrive.pandad",
"ui": ("selfdrive/ui", ["./start.sh"]),
"ui": ("selfdrive/ui", ["./start.py"]),
"calibrationd": "selfdrive.locationd.calibrationd",
"locationd": "selfdrive.locationd.locationd_local",
"visiond": ("selfdrive/visiond", ["./visiond"]),
@ -128,11 +127,9 @@ persistent_processes = [
'logcatd',
'tombstoned',
'uploader',
'deleter',
'ui',
'gpsd',
'updated',
'athena'
'athena',
]
car_started_processes = [
@ -146,7 +143,8 @@ car_started_processes = [
'visiond',
'proclogd',
'ubloxd',
'mapd',
'gpsd',
'deleter',
]
def register_managed_process(name, desc, car_started=False):
@ -318,6 +316,7 @@ def manager_thread():
# save boot log
subprocess.call(["./loggerd", "--bootlog"], cwd=os.path.join(BASEDIR, "selfdrive/loggerd"))
# start persistent processes
for p in persistent_processes:
start_managed_process(p)
@ -356,8 +355,8 @@ def manager_thread():
kill_managed_process(p)
# check the status of all processes, did any of them die?
for p in running:
cloudlog.debug(" running %s %s" % (p, running[p]))
running_list = [" running %s %s" % (p, running[p]) for p in running]
cloudlog.debug('\n'.join(running_list))
# is this still needed?
if params.get("DoUninstall") == "1":

106
selfdrive/mapd/default_speeds.json
View File

@ -1,106 +0,0 @@
{
"_comment": "These speeds are from https://wiki.openstreetmap.org/wiki/Speed_limits Special cases have been stripped",
"AR:urban": "40",
"AR:urban:primary": "60",
"AR:urban:secondary": "60",
"AR:rural": "110",
"AT:urban": "50",
"AT:rural": "100",
"AT:trunk": "100",
"AT:motorway": "130",
"BE:urban": "50",
"BE-VLG:rural": "70",
"BE-WAL:rural": "90",
"BE:trunk": "120",
"BE:motorway": "120",
"CH:urban[1]": "50",
"CH:rural": "80",
"CH:trunk": "100",
"CH:motorway": "120",
"CZ:pedestrian_zone": "20",
"CZ:living_street": "20",
"CZ:urban": "50",
"CZ:urban_trunk": "80",
"CZ:urban_motorway": "80",
"CZ:rural": "90",
"CZ:trunk": "110",
"CZ:motorway": "130",
"DK:urban": "50",
"DK:rural": "80",
"DK:motorway": "130",
"DE:living_street": "7",
"DE:residential": "30",
"DE:urban": "50",
"DE:rural": "100",
"DE:trunk": "none",
"DE:motorway": "none",
"FI:urban": "50",
"FI:rural": "80",
"FI:trunk": "100",
"FI:motorway": "120",
"FR:urban": "50",
"FR:rural": "80",
"FR:trunk": "110",
"FR:motorway": "130",
"GR:urban": "50",
"GR:rural": "90",
"GR:trunk": "110",
"GR:motorway": "130",
"HU:urban": "50",
"HU:rural": "90",
"HU:trunk": "110",
"HU:motorway": "130",
"IT:urban": "50",
"IT:rural": "90",
"IT:trunk": "110",
"IT:motorway": "130",
"JP:national": "60",
"JP:motorway": "100",
"LT:living_street": "20",
"LT:urban": "50",
"LT:rural": "90",
"LT:trunk": "120",
"LT:motorway": "130",
"PL:living_street": "20",
"PL:urban": "50",
"PL:rural": "90",
"PL:trunk": "100",
"PL:motorway": "140",
"RO:urban": "50",
"RO:rural": "90",
"RO:trunk": "100",
"RO:motorway": "130",
"RU:living_street": "20",
"RU:urban": "60",
"RU:rural": "90",
"RU:motorway": "110",
"SK:urban": "50",
"SK:rural": "90",
"SK:trunk": "90",
"SK:motorway": "90",
"SI:urban": "50",
"SI:rural": "90",
"SI:trunk": "110",
"SI:motorway": "130",
"ES:living_street": "20",
"ES:urban": "50",
"ES:rural": "50",
"ES:trunk": "90",
"ES:motorway": "120",
"SE:urban": "50",
"SE:rural": "70",
"SE:trunk": "90",
"SE:motorway": "110",
"GB:nsl_restricted": "30 mph",
"GB:nsl_single": "60 mph",
"GB:nsl_dual": "70 mph",
"GB:motorway": "70 mph",
"UA:urban": "50",
"UA:rural": "90",
"UA:trunk": "110",
"UA:motorway": "130",
"UZ:living_street": "30",
"UZ:urban": "70",
"UZ:rural": "100",
"UZ:motorway": "110"
}

240
selfdrive/mapd/default_speeds_generator.py
View File

@ -1,240 +0,0 @@
#!/usr/bin/env python
import json
DEFAULT_OUTPUT_FILENAME = "default_speeds_by_region.json"
def main(filename = DEFAULT_OUTPUT_FILENAME):
countries = []
"""
--------------------------------------------------
US - United State of America
--------------------------------------------------
"""
US = Country("US") # First step, create the country using the ISO 3166 two letter code
countries.append(US) # Second step, add the country to countries list
""" Default rules """
# Third step, add some default rules for the country
# Speed limit rules are based on OpenStreetMaps (OSM) tags.
# The dictionary {...} defines the tag_name: value
# if a road in OSM has a tag with the name tag_name and this value, the speed limit listed below will be applied.
# The text at the end is the speed limit (use no unit for km/h)
# Rules apply in the order in which they are written for each country
# Rules for specific regions (states) take priority over country rules
# If you modify existing country rules, you must update all existing states without that rule to use the old rule
US.add_rule({"highway": "motorway"}, "65 mph") # On US roads with the tag highway and value motorway, the speed limit will default to 65 mph
US.add_rule({"highway": "trunk"}, "55 mph")
US.add_rule({"highway": "primary"}, "55 mph")
US.add_rule({"highway": "secondary"}, "45 mph")
US.add_rule({"highway": "tertiary"}, "35 mph")
US.add_rule({"highway": "unclassified"}, "55 mph")
US.add_rule({"highway": "residential"}, "25 mph")
US.add_rule({"highway": "service"}, "25 mph")
US.add_rule({"highway": "motorway_link"}, "55 mph")
US.add_rule({"highway": "trunk_link"}, "55 mph")
US.add_rule({"highway": "primary_link"}, "55 mph")
US.add_rule({"highway": "secondary_link"}, "45 mph")
US.add_rule({"highway": "tertiary_link"}, "35 mph")
US.add_rule({"highway": "living_street"}, "15 mph")
""" States """
new_york = US.add_region("New York") # Fourth step, add a state/region to country
new_york.add_rule({"highway": "primary"}, "45 mph") # Fifth step , add rules to the state. See the text above for how to write rules
new_york.add_rule({"highway": "secondary"}, "55 mph")
new_york.add_rule({"highway": "tertiary"}, "55 mph")
new_york.add_rule({"highway": "residential"}, "30 mph")
new_york.add_rule({"highway": "primary_link"}, "45 mph")
new_york.add_rule({"highway": "secondary_link"}, "55 mph")
new_york.add_rule({"highway": "tertiary_link"}, "55 mph")
# All if not written by the state, the rules will default to the country rules
#california = US.add_region("California")
# California uses only the default US rules
michigan = US.add_region("Michigan")
michigan.add_rule({"highway": "motorway"}, "70 mph")
oregon = US.add_region("Oregon")
oregon.add_rule({"highway": "motorway"}, "55 mph")
oregon.add_rule({"highway": "secondary"}, "35 mph")
oregon.add_rule({"highway": "tertiary"}, "30 mph")
oregon.add_rule({"highway": "service"}, "15 mph")
oregon.add_rule({"highway": "secondary_link"}, "35 mph")
oregon.add_rule({"highway": "tertiary_link"}, "30 mph")
south_dakota = US.add_region("South Dakota")
south_dakota.add_rule({"highway": "motorway"}, "80 mph")
south_dakota.add_rule({"highway": "trunk"}, "70 mph")
south_dakota.add_rule({"highway": "primary"}, "65 mph")
south_dakota.add_rule({"highway": "trunk_link"}, "70 mph")
south_dakota.add_rule({"highway": "primary_link"}, "65 mph")
wisconsin = US.add_region("Wisconsin")
wisconsin.add_rule({"highway": "trunk"}, "65 mph")
wisconsin.add_rule({"highway": "tertiary"}, "45 mph")
wisconsin.add_rule({"highway": "unclassified"}, "35 mph")
wisconsin.add_rule({"highway": "trunk_link"}, "65 mph")
wisconsin.add_rule({"highway": "tertiary_link"}, "45 mph")
"""
--------------------------------------------------
AU - Australia
--------------------------------------------------
"""
AU = Country("AU")
countries.append(AU)
""" Default rules """
AU.add_rule({"highway": "motorway"}, "100")
AU.add_rule({"highway": "trunk"}, "80")
AU.add_rule({"highway": "primary"}, "80")
AU.add_rule({"highway": "secondary"}, "50")
AU.add_rule({"highway": "tertiary"}, "50")
AU.add_rule({"highway": "unclassified"}, "80")
AU.add_rule({"highway": "residential"}, "50")
AU.add_rule({"highway": "service"}, "40")
AU.add_rule({"highway": "motorway_link"}, "90")
AU.add_rule({"highway": "trunk_link"}, "80")
AU.add_rule({"highway": "primary_link"}, "80")
AU.add_rule({"highway": "secondary_link"}, "50")
AU.add_rule({"highway": "tertiary_link"}, "50")
AU.add_rule({"highway": "living_street"}, "30")
"""
--------------------------------------------------
CA - Canada
--------------------------------------------------
"""
CA = Country("CA")
countries.append(CA)
""" Default rules """
CA.add_rule({"highway": "motorway"}, "100")
CA.add_rule({"highway": "trunk"}, "80")
CA.add_rule({"highway": "primary"}, "80")
CA.add_rule({"highway": "secondary"}, "50")
CA.add_rule({"highway": "tertiary"}, "50")
CA.add_rule({"highway": "unclassified"}, "80")
CA.add_rule({"highway": "residential"}, "40")
CA.add_rule({"highway": "service"}, "40")
CA.add_rule({"highway": "motorway_link"}, "90")
CA.add_rule({"highway": "trunk_link"}, "80")
CA.add_rule({"highway": "primary_link"}, "80")
CA.add_rule({"highway": "secondary_link"}, "50")
CA.add_rule({"highway": "tertiary_link"}, "50")
CA.add_rule({"highway": "living_street"}, "20")
"""
--------------------------------------------------
DE - Germany
--------------------------------------------------
"""
DE = Country("DE")
countries.append(DE)
""" Default rules """
DE.add_rule({"highway": "motorway"}, "none")
DE.add_rule({"highway": "living_street"}, "10")
DE.add_rule({"highway": "residential"}, "30")
DE.add_rule({"zone:traffic": "DE:rural"}, "100")
DE.add_rule({"zone:traffic": "DE:urban"}, "50")
DE.add_rule({"zone:maxspeed": "DE:30"}, "30")
DE.add_rule({"zone:maxspeed": "DE:urban"}, "50")
DE.add_rule({"zone:maxspeed": "DE:rural"}, "100")
DE.add_rule({"zone:maxspeed": "DE:motorway"}, "none")
DE.add_rule({"bicycle_road": "yes"}, "30")
"""
--------------------------------------------------
EE - Estonia
--------------------------------------------------
"""
EE = Country("EE")
countries.append(EE)
""" Default rules """
EE.add_rule({"highway": "motorway"}, "90")
EE.add_rule({"highway": "trunk"}, "90")
EE.add_rule({"highway": "primary"}, "90")
EE.add_rule({"highway": "secondary"}, "50")
EE.add_rule({"highway": "tertiary"}, "50")
EE.add_rule({"highway": "unclassified"}, "90")
EE.add_rule({"highway": "residential"}, "40")
EE.add_rule({"highway": "service"}, "40")
EE.add_rule({"highway": "motorway_link"}, "90")
EE.add_rule({"highway": "trunk_link"}, "70")
EE.add_rule({"highway": "primary_link"}, "70")
EE.add_rule({"highway": "secondary_link"}, "50")
EE.add_rule({"highway": "tertiary_link"}, "50")
EE.add_rule({"highway": "living_street"}, "20")
""" --- DO NOT MODIFY CODE BELOW THIS LINE --- """
""" --- ADD YOUR COUNTRY OR STATE ABOVE --- """
# Final step
write_json(countries, filename)
def write_json(countries, filename = DEFAULT_OUTPUT_FILENAME):
out_dict = {}
for country in countries:
out_dict.update(country.jsonify())
json_string = json.dumps(out_dict, indent=2)
with open(filename, "wb") as f:
f.write(json_string)
class Region(object):
ALLOWABLE_TAG_KEYS = ["highway", "zone:traffic", "bicycle_road", "zone:maxspeed"]
ALLOWABLE_HIGHWAY_TYPES = ["motorway", "trunk", "primary", "secondary", "tertiary", "unclassified", "residential", "service", "motorway_link", "trunk_link", "primary_link", "secondary_link", "tertiary_link", "living_street"]
def __init__(self, name):
self.name = name
self.rules = []
def add_rule(self, tag_conditions, speed):
new_rule = {}
if not isinstance(tag_conditions, dict):
raise TypeError("Rule tag conditions must be dictionary")
if not all(tag_key in self.ALLOWABLE_TAG_KEYS for tag_key in tag_conditions):
raise ValueError("Rule tag keys must be in allowable tag kesy") # If this is by mistake, please update ALLOWABLE_TAG_KEYS
if 'highway' in tag_conditions:
if not tag_conditions['highway'] in self.ALLOWABLE_HIGHWAY_TYPES:
raise ValueError("Invalid Highway type {}".format(tag_conditions["highway"]))
new_rule['tags'] = tag_conditions
try:
new_rule['speed'] = str(speed)
except ValueError:
raise ValueError("Rule speed must be string")
self.rules.append(new_rule)
def jsonify(self):
ret_dict = {}
ret_dict[self.name] = self.rules
return ret_dict
class Country(Region):
ALLOWABLE_COUNTRY_CODES = ["AF","AX","AL","DZ","AS","AD","AO","AI","AQ","AG","AR","AM","AW","AU","AT","AZ","BS","BH","BD","BB","BY","BE","BZ","BJ","BM","BT","BO","BQ","BA","BW","BV","BR","IO","BN","BG","BF","BI","KH","CM","CA","CV","KY","CF","TD","CL","CN","CX","CC","CO","KM","CG","CD","CK","CR","CI","HR","CU","CW","CY","CZ","DK","DJ","DM","DO","EC","EG","SV","GQ","ER","EE","ET","FK","FO","FJ","FI","FR","GF","PF","TF","GA","GM","GE","DE","GH","GI","GR","GL","GD","GP","GU","GT","GG","GN","GW","GY","HT","HM","VA","HN","HK","HU","IS","IN","ID","IR","IQ","IE","IM","IL","IT","JM","JP","JE","JO","KZ","KE","KI","KP","KR","KW","KG","LA","LV","LB","LS","LR","LY","LI","LT","LU","MO","MK","MG","MW","MY","MV","ML","MT","MH","MQ","MR","MU","YT","MX","FM","MD","MC","MN","ME","MS","MA","MZ","MM","NA","NR","NP","NL","NC","NZ","NI","NE","NG","NU","NF","MP","NO","OM","PK","PW","PS","PA","PG","PY","PE","PH","PN","PL","PT","PR","QA","RE","RO","RU","RW","BL","SH","KN","LC","MF","PM","VC","WS","SM","ST","SA","SN","RS","SC","SL","SG","SX","SK","SI","SB","SO","ZA","GS","SS","ES","LK","SD","SR","SJ","SZ","SE","CH","SY","TW","TJ","TZ","TH","TL","TG","TK","TO","TT","TN","TR","TM","TC","TV","UG","UA","AE","GB","US","UM","UY","UZ","VU","VE","VN","VG","VI","WF","EH","YE","ZM","ZW"]
def __init__(self, ISO_3166_alpha_2):
Region.__init__(self, ISO_3166_alpha_2)
if ISO_3166_alpha_2 not in self.ALLOWABLE_COUNTRY_CODES:
raise ValueError("Not valid IOS 3166 country code")
self.regions = {}
def add_region(self, name):
self.regions[name] = Region(name)
return self.regions[name]
def jsonify(self):
ret_dict = {}
ret_dict[self.name] = {}
for r_name, region in self.regions.items():
ret_dict[self.name].update(region.jsonify())
ret_dict[self.name]['Default'] = self.rules
return ret_dict
if __name__ == '__main__':
main()

297
selfdrive/mapd/mapd.py
View File

@ -1,297 +0,0 @@
#!/usr/bin/env python
# Add phonelibs openblas to LD_LIBRARY_PATH if import fails
from common.basedir import BASEDIR
try:
from scipy import spatial
except ImportError as e:
import os
import sys
openblas_path = os.path.join(BASEDIR, "phonelibs/openblas/")
os.environ['LD_LIBRARY_PATH'] += ':' + openblas_path
args = [sys.executable]
args.extend(sys.argv)
os.execv(sys.executable, args)
DEFAULT_SPEEDS_BY_REGION_JSON_FILE = BASEDIR + "/selfdrive/mapd/default_speeds_by_region.json"
from selfdrive.mapd import default_speeds_generator
default_speeds_generator.main(DEFAULT_SPEEDS_BY_REGION_JSON_FILE)
import os
import sys
import time
import zmq
import threading
import numpy as np
import overpy
from collections import defaultdict
from common.params import Params
from common.transformations.coordinates import geodetic2ecef
from selfdrive.services import service_list
import selfdrive.messaging as messaging
from selfdrive.mapd.mapd_helpers import MAPS_LOOKAHEAD_DISTANCE, Way, circle_through_points
import selfdrive.crash as crash
from selfdrive.version import version, dirty
OVERPASS_API_URL = "https://overpass.kumi.systems/api/interpreter"
OVERPASS_HEADERS = {
'User-Agent': 'NEOS (comma.ai)',
'Accept-Encoding': 'gzip'
}
last_gps = None
query_lock = threading.Lock()
last_query_result = None
last_query_pos = None
cache_valid = False
def build_way_query(lat, lon, radius=50):
"""Builds a query to find all highways within a given radius around a point"""
pos = " (around:%f,%f,%f)" % (radius, lat, lon)
lat_lon = "(%f,%f)" % (lat, lon)
q = """(
way
""" + pos + """
[highway][highway!~"^(footway|path|bridleway|steps|cycleway|construction|bus_guideway|escape)$"];
>;);out;""" + """is_in""" + lat_lon + """;area._[admin_level~"[24]"];
convert area ::id = id(), admin_level = t['admin_level'],
name = t['name'], "ISO3166-1:alpha2" = t['ISO3166-1:alpha2'];out;
"""
return q
def query_thread():
global last_query_result, last_query_pos, cache_valid
api = overpy.Overpass(url=OVERPASS_API_URL, headers=OVERPASS_HEADERS, timeout=10.)
while True:
time.sleep(1)
if last_gps is not None:
fix_ok = last_gps.flags & 1
if not fix_ok:
continue
if last_query_pos is not None:
cur_ecef = geodetic2ecef((last_gps.latitude, last_gps.longitude, last_gps.altitude))
prev_ecef = geodetic2ecef((last_query_pos.latitude, last_query_pos.longitude, last_query_pos.altitude))
dist = np.linalg.norm(cur_ecef - prev_ecef)
if dist < 1000: #updated when we are 1km from the edge of the downloaded circle
continue
if dist > 3000:
cache_valid = False
q = build_way_query(last_gps.latitude, last_gps.longitude, radius=3000)
try:
new_result = api.query(q)
# Build kd-tree
nodes = []
real_nodes = []
node_to_way = defaultdict(list)
location_info = {}
for n in new_result.nodes:
nodes.append((float(n.lat), float(n.lon), 0))
real_nodes.append(n)
for way in new_result.ways:
for n in way.nodes:
node_to_way[n.id].append(way)
for area in new_result.areas:
if area.tags.get('admin_level', '') == "2":
location_info['country'] = area.tags.get('ISO3166-1:alpha2', '')
if area.tags.get('admin_level', '') == "4":
location_info['region'] = area.tags.get('name', '')
nodes = np.asarray(nodes)
nodes = geodetic2ecef(nodes)
tree = spatial.cKDTree(nodes)
query_lock.acquire()
last_query_result = new_result, tree, real_nodes, node_to_way, location_info
last_query_pos = last_gps
cache_valid = True
query_lock.release()
except Exception as e:
print(e)
query_lock.acquire()
last_query_result = None
query_lock.release()
def mapsd_thread():
global last_gps
context = zmq.Context()
gps_sock = messaging.sub_sock(context, service_list['gpsLocation'].port, conflate=True)
gps_external_sock = messaging.sub_sock(context, service_list['gpsLocationExternal'].port, conflate=True)
map_data_sock = messaging.pub_sock(context, service_list['liveMapData'].port)
cur_way = None
curvature_valid = False
curvature = None
upcoming_curvature = 0.
dist_to_turn = 0.
road_points = None
while True:
gps = messaging.recv_one(gps_sock)
gps_ext = messaging.recv_one_or_none(gps_external_sock)
if gps_ext is not None:
gps = gps_ext.gpsLocationExternal
else:
gps = gps.gpsLocation
last_gps = gps
fix_ok = gps.flags & 1
if not fix_ok or last_query_result is None or not cache_valid:
cur_way = None
curvature = None
curvature_valid = False
upcoming_curvature = 0.
dist_to_turn = 0.
road_points = None
map_valid = False
else:
map_valid = True
lat = gps.latitude
lon = gps.longitude
heading = gps.bearing
speed = gps.speed
query_lock.acquire()
cur_way = Way.closest(last_query_result, lat, lon, heading, cur_way)
if cur_way is not None:
pnts, curvature_valid = cur_way.get_lookahead(lat, lon, heading, MAPS_LOOKAHEAD_DISTANCE)
xs = pnts[:, 0]
ys = pnts[:, 1]
road_points = [float(x) for x in xs], [float(y) for y in ys]
if speed < 10:
curvature_valid = False
if curvature_valid and pnts.shape[0] <= 3:
curvature_valid = False
# The curvature is valid when at least MAPS_LOOKAHEAD_DISTANCE of road is found
if curvature_valid:
# Compute the curvature for each point
with np.errstate(divide='ignore'):
circles = [circle_through_points(*p) for p in zip(pnts, pnts[1:], pnts[2:])]
circles = np.asarray(circles)
radii = np.nan_to_num(circles[:, 2])
radii[radii < 10] = np.inf
curvature = 1. / radii
# Index of closest point
closest = np.argmin(np.linalg.norm(pnts, axis=1))
dist_to_closest = pnts[closest, 0] # We can use x distance here since it should be close
# Compute distance along path
dists = list()
dists.append(0)
for p, p_prev in zip(pnts, pnts[1:, :]):
dists.append(dists[-1] + np.linalg.norm(p - p_prev))
dists = np.asarray(dists)
dists = dists - dists[closest] + dist_to_closest
dists = dists[1:-1]
close_idx = np.logical_and(dists > 0, dists < 500)
dists = dists[close_idx]
curvature = curvature[close_idx]
if len(curvature):
# TODO: Determine left or right turn
curvature = np.nan_to_num(curvature)
# Outlier rejection
new_curvature = np.percentile(curvature, 90, interpolation='lower')
k = 0.6
upcoming_curvature = k * upcoming_curvature + (1 - k) * new_curvature
in_turn_indices = curvature > 0.8 * new_curvature
if np.any(in_turn_indices):
dist_to_turn = np.min(dists[in_turn_indices])
else:
dist_to_turn = 999
else:
upcoming_curvature = 0.
dist_to_turn = 999
query_lock.release()
dat = messaging.new_message()
dat.init('liveMapData')
if last_gps is not None:
dat.liveMapData.lastGps = last_gps
if cur_way is not None:
dat.liveMapData.wayId = cur_way.id
# Speed limit
max_speed = cur_way.max_speed()
if max_speed is not None:
dat.liveMapData.speedLimitValid = True
dat.liveMapData.speedLimit = max_speed
# TODO: use the function below to anticipate upcoming speed limits
#max_speed_ahead, max_speed_ahead_dist = cur_way.max_speed_ahead(max_speed, lat, lon, heading, MAPS_LOOKAHEAD_DISTANCE)
#if max_speed_ahead is not None and max_speed_ahead_dist is not None:
# dat.liveMapData.speedLimitAheadValid = True
# dat.liveMapData.speedLimitAhead = float(max_speed_ahead)
# dat.liveMapData.speedLimitAheadDistance = float(max_speed_ahead_dist)
advisory_max_speed = cur_way.advisory_max_speed()
if advisory_max_speed is not None:
dat.liveMapData.speedAdvisoryValid = True
dat.liveMapData.speedAdvisory = advisory_max_speed
# Curvature
dat.liveMapData.curvatureValid = curvature_valid
dat.liveMapData.curvature = float(upcoming_curvature)
dat.liveMapData.distToTurn = float(dist_to_turn)
if road_points is not None:
dat.liveMapData.roadX, dat.liveMapData.roadY = road_points
if curvature is not None:
dat.liveMapData.roadCurvatureX = [float(x) for x in dists]
dat.liveMapData.roadCurvature = [float(x) for x in curvature]
dat.liveMapData.mapValid = map_valid
map_data_sock.send(dat.to_bytes())
def main(gctx=None):
params = Params()
dongle_id = params.get("DongleId")
crash.bind_user(id=dongle_id)
crash.bind_extra(version=version, dirty=dirty, is_eon=True)
crash.install()
main_thread = threading.Thread(target=mapsd_thread)
main_thread.daemon = True
main_thread.start()
q_thread = threading.Thread(target=query_thread)
q_thread.daemon = True
q_thread.start()
while True:
time.sleep(0.1)
if __name__ == "__main__":
main()

364
selfdrive/mapd/mapd_helpers.py
View File

@ -1,364 +0,0 @@
import math
import json
import numpy as np
from datetime import datetime
from common.basedir import BASEDIR
from selfdrive.config import Conversions as CV
from common.transformations.coordinates import LocalCoord, geodetic2ecef
LOOKAHEAD_TIME = 10.
MAPS_LOOKAHEAD_DISTANCE = 50 * LOOKAHEAD_TIME
DEFAULT_SPEEDS_JSON_FILE = BASEDIR + "/selfdrive/mapd/default_speeds.json"
DEFAULT_SPEEDS = {}
with open(DEFAULT_SPEEDS_JSON_FILE, "rb") as f:
DEFAULT_SPEEDS = json.loads(f.read())
DEFAULT_SPEEDS_BY_REGION_JSON_FILE = BASEDIR + "/selfdrive/mapd/default_speeds_by_region.json"
DEFAULT_SPEEDS_BY_REGION = {}
with open(DEFAULT_SPEEDS_BY_REGION_JSON_FILE, "rb") as f:
DEFAULT_SPEEDS_BY_REGION = json.loads(f.read())
def circle_through_points(p1, p2, p3):
"""Fits a circle through three points
Formulas from: http://www.ambrsoft.com/trigocalc/circle3d.htm"""
x1, y1, _ = p1
x2, y2, _ = p2
x3, y3, _ = p3
A = x1 * (y2 - y3) - y1 * (x2 - x3) + x2 * y3 - x3 * y2
B = (x1**2 + y1**2) * (y3 - y2) + (x2**2 + y2**2) * (y1 - y3) + (x3**2 + y3**2) * (y2 - y1)
C = (x1**2 + y1**2) * (x2 - x3) + (x2**2 + y2**2) * (x3 - x1) + (x3**2 + y3**2) * (x1 - x2)
D = (x1**2 + y1**2) * (x3 * y2 - x2 * y3) + (x2**2 + y2**2) * (x1 * y3 - x3 * y1) + (x3**2 + y3**2) * (x2 * y1 - x1 * y2)
return (-B / (2 * A), - C / (2 * A), np.sqrt((B**2 + C**2 - 4 * A * D) / (4 * A**2)))
def parse_speed_unit(max_speed):
"""Converts a maxspeed string to m/s based on the unit present in the input.
OpenStreetMap defaults to kph if no unit is present. """
if not max_speed:
return None
conversion = CV.KPH_TO_MS
if 'mph' in max_speed:
max_speed = max_speed.replace(' mph', '')
conversion = CV.MPH_TO_MS
try:
return float(max_speed) * conversion
except ValueError:
return None
def parse_speed_tags(tags):
"""Parses tags on a way to find the maxspeed string"""
max_speed = None
if 'maxspeed' in tags:
max_speed = tags['maxspeed']
if 'maxspeed:conditional' in tags:
try:
max_speed_cond, cond = tags['maxspeed:conditional'].split(' @ ')
cond = cond[1:-1]
start, end = cond.split('-')
now = datetime.now() # TODO: Get time and timezone from gps fix so this will work correctly on replays
start = datetime.strptime(start, "%H:%M").replace(year=now.year, month=now.month, day=now.day)
end = datetime.strptime(end, "%H:%M").replace(year=now.year, month=now.month, day=now.day)
if start <= now <= end:
max_speed = max_speed_cond
except ValueError:
pass
if not max_speed and 'source:maxspeed' in tags:
max_speed = DEFAULT_SPEEDS.get(tags['source:maxspeed'], None)
if not max_speed and 'maxspeed:type' in tags:
max_speed = DEFAULT_SPEEDS.get(tags['maxspeed:type'], None)
max_speed = parse_speed_unit(max_speed)
return max_speed
def geocode_maxspeed(tags, location_info):
max_speed = None
try:
geocode_country = location_info.get('country', '')
geocode_region = location_info.get('region', '')
country_rules = DEFAULT_SPEEDS_BY_REGION.get(geocode_country, {})
country_defaults = country_rules.get('Default', [])
for rule in country_defaults:
rule_valid = all(
tag_name in tags
and tags[tag_name] == value
for tag_name, value in rule['tags'].items()
)
if rule_valid:
max_speed = rule['speed']
break #stop searching country
region_rules = country_rules.get(geocode_region, [])
for rule in region_rules:
rule_valid = all(
tag_name in tags
and tags[tag_name] == value
for tag_name, value in rule['tags'].items()
)
if rule_valid:
max_speed = rule['speed']
break #stop searching region
except KeyError:
pass
max_speed = parse_speed_unit(max_speed)
return max_speed
class Way:
def __init__(self, way, query_results):
self.id = way.id
self.way = way
self.query_results = query_results
points = list()
for node in self.way.get_nodes(resolve_missing=False):
points.append((float(node.lat), float(node.lon), 0.))
self.points = np.asarray(points)
@classmethod
def closest(cls, query_results, lat, lon, heading, prev_way=None):
results, tree, real_nodes, node_to_way, location_info = query_results
cur_pos = geodetic2ecef((lat, lon, 0))
nodes = tree.query_ball_point(cur_pos, 500)
# If no nodes within 500m, choose closest one
if not nodes:
nodes = [tree.query(cur_pos)[1]]
ways = []
for n in nodes:
real_node = real_nodes[n]
ways += node_to_way[real_node.id]
ways = set(ways)
closest_way = None
best_score = None
for way in ways:
way = Way(way, query_results)
points = way.points_in_car_frame(lat, lon, heading)
on_way = way.on_way(lat, lon, heading, points)
if not on_way:
continue
# Create mask of points in front and behind
x = points[:, 0]
y = points[:, 1]
angles = np.arctan2(y, x)
front = np.logical_and((-np.pi / 2) < angles,
angles < (np.pi / 2))
behind = np.logical_not(front)
dists = np.linalg.norm(points, axis=1)
# Get closest point behind the car
dists_behind = np.copy(dists)
dists_behind[front] = np.NaN
closest_behind = points[np.nanargmin(dists_behind)]
# Get closest point in front of the car
dists_front = np.copy(dists)
dists_front[behind] = np.NaN
closest_front = points[np.nanargmin(dists_front)]
# fit line: y = a*x + b
x1, y1, _ = closest_behind
x2, y2, _ = closest_front
a = (y2 - y1) / max((x2 - x1), 1e-5)
b = y1 - a * x1
# With a factor of 60 a 20m offset causes the same error as a 20 degree heading error
# (A 20 degree heading offset results in an a of about 1/3)
score = abs(a) * 60. + abs(b)
# Prefer same type of road
if prev_way is not None:
if way.way.tags.get('highway', '') == prev_way.way.tags.get('highway', ''):
score *= 0.5
if closest_way is None or score < best_score:
closest_way = way
best_score = score
# Normal score is < 5
if best_score > 50:
return None
return closest_way
def __str__(self):
return "%s %s" % (self.id, self.way.tags)
def max_speed(self):
"""Extracts the (conditional) speed limit from a way"""
if not self.way:
return None
max_speed = parse_speed_tags(self.way.tags)
if not max_speed:
location_info = self.query_results[4]
max_speed = geocode_maxspeed(self.way.tags, location_info)
return max_speed
def max_speed_ahead(self, current_speed_limit, lat, lon, heading, lookahead):
"""Look ahead for a max speed"""
if not self.way:
return None
speed_ahead = None
speed_ahead_dist = None
lookahead_ways = 5
way = self
for i in range(lookahead_ways):
way_pts = way.points_in_car_frame(lat, lon, heading)
# Check current lookahead distance
max_dist = np.linalg.norm(way_pts[-1, :])
if max_dist > 2 * lookahead:
break
if 'maxspeed' in way.way.tags:
spd = parse_speed_tags(way.way.tags)
if not spd:
location_info = self.query_results[4]
spd = geocode_maxspeed(way.way.tags, location_info)
if spd < current_speed_limit:
speed_ahead = spd
min_dist = np.linalg.norm(way_pts[1, :])
speed_ahead_dist = min_dist
break
# Find next way
way = way.next_way()
if not way:
break
return speed_ahead, speed_ahead_dist
def advisory_max_speed(self):
if not self.way:
return None
tags = self.way.tags
adv_speed = None
if 'maxspeed:advisory' in tags:
adv_speed = tags['maxspeed:advisory']
adv_speed = parse_speed_unit(adv_speed)
return adv_speed
def on_way(self, lat, lon, heading, points=None):
if points is None:
points = self.points_in_car_frame(lat, lon, heading)
x = points[:, 0]
return np.min(x) < 0. and np.max(x) > 0.
def closest_point(self, lat, lon, heading, points=None):
if points is None:
points = self.points_in_car_frame(lat, lon, heading)
i = np.argmin(np.linalg.norm(points, axis=1))
return points[i]
def distance_to_closest_node(self, lat, lon, heading, points=None):
if points is None:
points = self.points_in_car_frame(lat, lon, heading)
return np.min(np.linalg.norm(points, axis=1))
def points_in_car_frame(self, lat, lon, heading):
lc = LocalCoord.from_geodetic([lat, lon, 0.])
# Build rotation matrix
heading = math.radians(-heading + 90)
c, s = np.cos(heading), np.sin(heading)
rot = np.array([[c, s, 0.], [-s, c, 0.], [0., 0., 1.]])
# Convert to local coordinates
points_carframe = lc.geodetic2ned(self.points).T
# Rotate with heading of car
points_carframe = np.dot(rot, points_carframe[(1, 0, 2), :]).T
return points_carframe
def next_way(self, backwards=False):
results, tree, real_nodes, node_to_way, location_info = self.query_results
if backwards:
node = self.way.nodes[0]
else:
node = self.way.nodes[-1]
ways = node_to_way[node.id]
way = None
try:
# Simple heuristic to find next way
ways = [w for w in ways if w.id != self.id]
ways = [w for w in ways if w.nodes[0] == node]
# Filter on highway tag
acceptable_tags = list()
cur_tag = self.way.tags['highway']
acceptable_tags.append(cur_tag)
if cur_tag == 'motorway_link':
acceptable_tags.append('motorway')
acceptable_tags.append('trunk')
acceptable_tags.append('primary')
ways = [w for w in ways if w.tags['highway'] in acceptable_tags]
# Filter on number of lanes
cur_num_lanes = int(self.way.tags['lanes'])
if len(ways) > 1:
ways_same_lanes = [w for w in ways if int(w.tags['lanes']) == cur_num_lanes]
if len(ways_same_lanes) == 1:
ways = ways_same_lanes
if len(ways) > 1:
ways = [w for w in ways if int(w.tags['lanes']) > cur_num_lanes]
if len(ways) == 1:
way = Way(ways[0], self.query_results)
except (KeyError, ValueError):
pass
return way
def get_lookahead(self, lat, lon, heading, lookahead):
pnts = None
way = self
valid = False
for i in range(5):
# Get new points and append to list
new_pnts = way.points_in_car_frame(lat, lon, heading)
if pnts is None:
pnts = new_pnts
else:
pnts = np.vstack([pnts, new_pnts])
# Check current lookahead distance
max_dist = np.linalg.norm(pnts[-1, :])
if max_dist > lookahead:
valid = True
if max_dist > 2 * lookahead:
break
# Find next way
way = way.next_way()
if not way:
break
return pnts, valid

78
selfdrive/registration.py
View File

@ -1,10 +1,16 @@
import os
import json
import subprocess
import struct
import jwt
from datetime import datetime, timedelta
from selfdrive.swaglog import cloudlog
from selfdrive.version import version, training_version
from selfdrive.version import version, training_version, get_git_commit, get_git_branch, get_git_remote
from common.api import api_get
from common.params import Params
from common.file_helpers import mkdirs_exists_ok
def get_imei():
ret = subprocess.check_output(["getprop", "oem.device.imeicache"]).strip()
@ -12,17 +18,38 @@ def get_imei():
ret = "000000000000000"
return ret
def get_serial():
return subprocess.check_output(["getprop", "ro.serialno"]).strip()
def get_git_commit():
return subprocess.check_output(["git", "rev-parse", "HEAD"]).strip()
def get_git_branch():
return subprocess.check_output(["git", "rev-parse", "--abbrev-ref", "HEAD"]).strip()
# TODO: move this to a library
def parse_service_call(call):
ret = subprocess.check_output(call).strip()
if 'Parcel' not in ret:
return None
try:
def fh(x):
if len(x) != 8:
return []
return [x[6:8], x[4:6], x[2:4], x[0:2]]
hd = []
for x in ret.split("\n")[1:]:
for k in map(fh, x.split(": ")[1].split(" '")[0].split(" ")):
hd.extend(k)
return ''.join([chr(int(x, 16)) for x in hd])
except Exception:
return None
def get_subscriber_info():
ret = parse_service_call(["service", "call", "iphonesubinfo", "7"])
if ret is None or len(ret) < 8:
return ""
if struct.unpack("I", ret[4:8]) == -1:
return ""
return ret[8:-2:2]
def get_git_remote():
return subprocess.check_output(["git", "config", "--get", "remote.origin.url"]).strip()
def register():
params = Params()
@ -31,23 +58,42 @@ def register():
params.put("GitCommit", get_git_commit())
params.put("GitBranch", get_git_branch())
params.put("GitRemote", get_git_remote())
params.put("SubscriberInfo", get_subscriber_info())
# create a key for auth
# your private key is kept on your device persist partition and never sent to our servers
# do not erase your persist partition
if not os.path.isfile("/persist/comma/id_rsa.pub"):
cloudlog.warning("generating your personal RSA key")
mkdirs_exists_ok("/persist/comma")
assert os.system("echo -e 'y\n' | ssh-keygen -t rsa -b 2048 -f /persist/comma/id_rsa.tmp -N ''") == 0
os.rename("/persist/comma/id_rsa.tmp", "/persist/comma/id_rsa")
os.rename("/persist/comma/id_rsa.tmp.pub", "/persist/comma/id_rsa.pub")
dongle_id, access_token = params.get("DongleId"), params.get("AccessToken")
public_key = open("/persist/comma/id_rsa.pub").read()
# create registration token
# in the future, this key will make JWTs directly
private_key = open("/persist/comma/id_rsa").read()
register_token = jwt.encode({'register':True, 'exp': datetime.utcnow() + timedelta(hours=1)}, private_key, algorithm='RS256')
try:
if dongle_id is None or access_token is None:
cloudlog.info("getting pilotauth")
resp = api_get("v1/pilotauth/", method='POST', timeout=15,
imei=get_imei(), serial=get_serial())
dongleauth = json.loads(resp.text)
dongle_id, access_token = dongleauth["dongle_id"].encode('ascii'), dongleauth["access_token"].encode('ascii')
cloudlog.info("getting pilotauth")
resp = api_get("v2/pilotauth/", method='POST', timeout=15,
imei=get_imei(), serial=get_serial(), public_key=public_key, register_token=register_token)
dongleauth = json.loads(resp.text)
dongle_id, access_token = dongleauth["dongle_id"].encode('ascii'), dongleauth["access_token"].encode('ascii')
params.put("DongleId", dongle_id)
params.put("AccessToken", access_token)
params.put("DongleId", dongle_id)
params.put("AccessToken", access_token)
return dongle_id, access_token
except Exception:
cloudlog.exception("failed to authenticate")
return None
if dongle_id is not None and access_token is not None:
return dongle_id, access_token
else:
return None
if __name__ == "__main__":
print(api_get("").text)

2
selfdrive/sensord/gpsd
View File

@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:125944cbfd9300a663ec5c01c48a67b3bdb0d64d253423656c47f3dcc7886e68
oid sha256:9ba2abd00e366eafd598ef10054783de945fb21f22d4c2780e1e4c20d28cdc93
size 1171544

2
selfdrive/sensord/sensord
View File

@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:43447776956dfc2b120ee525b489dc8a9090bc02975130ee6bcaa8557356de52
oid sha256:010fbcc81ddc121424d688568d98bbcbe1f5671ce8af21dba424b89d84e3b214
size 1159016

18
selfdrive/service_list.yaml
View File

@ -14,7 +14,7 @@ gpsNMEA: [8004, true]
thermal: [8005, true]
# List(CanData), list of can messages
can: [8006, true]
live100: [8007, true]
controlsState: [8007, true]
# random events we want to log
@ -22,7 +22,7 @@ live100: [8007, true]
model: [8009, true]
features: [8010, true]
health: [8011, true]
live20: [8012, true]
radarState: [8012, true]
#liveUI: [8014, true]
encodeIdx: [8015, true]
liveTracks: [8016, true]
@ -97,23 +97,23 @@ testJoystick: [8056, false]
# publishes: sensorEvents, gpsNMEA
# visiond -- talks to the cameras, runs the model, saves the videos
# subscribes: liveCalibration, sensorEvents, live100
# subscribes: liveCalibration, sensorEvents, controlsState
# publishes: frame, encodeIdx, model, liveCalibration
# **** stateful data transformers ****
# plannerd -- decides where to drive the car
# subscribes: carState, model, live20
# subscribes: carState, model, radarState
# publishes: plan
# controlsd -- actually drives the car
# subscribes: can, thermal, plan
# publishes: carState, carControl, sendcan, live100
# publishes: carState, carControl, sendcan, controlsState
# radard -- processes the radar data
# radard -- processes the radar and vision data
# blocks: CarParams
# subscribes: can, live100, model
# publishes: live20, liveTracks
# subscribes: can, controlsState, model
# publishes: radarState, liveTracks
# **** LOGGING SERVICE ****
@ -123,7 +123,7 @@ testJoystick: [8056, false]
# **** NON VITAL SERVICES ****
# ui
# subscribes: live100, live20, liveCalibration, model, (raw frames)
# subscribes: controlsState, radarState, liveCalibration, model, (raw frames)
# uploader
# communicates through file system with loggerd

24
selfdrive/test/plant/maneuver.py
View File

@ -28,7 +28,7 @@ class Maneuver(object):
distance_lead = self.distance_lead
)
last_live100 = None
last_controls_state = None
plot = ManeuverPlot(self.title)
buttons_sorted = sorted(self.cruise_button_presses, key=lambda a: a[1])
@ -42,27 +42,27 @@ class Maneuver(object):
grade = np.interp(plant.current_time(), self.grade_breakpoints, self.grade_values)
speed_lead = np.interp(plant.current_time(), self.speed_lead_breakpoints, self.speed_lead_values)
distance, speed, acceleration, distance_lead, brake, gas, steer_torque, fcw, live100= plant.step(speed_lead, current_button, grade)
if live100:
last_live100 = live100[-1]
distance, speed, acceleration, distance_lead, brake, gas, steer_torque, fcw, controls_state= plant.step(speed_lead, current_button, grade)
if controls_state:
last_controls_state = controls_state[-1]
d_rel = distance_lead - distance if self.lead_relevancy else 200.
v_rel = speed_lead - speed if self.lead_relevancy else 0.
if last_live100:
# print(last_live100)
if last_controls_state:
# print(last_controls_state)
#develop plots
plot.add_data(
time=plant.current_time(),
gas=gas, brake=brake, steer_torque=steer_torque,
distance=distance, speed=speed, acceleration=acceleration,
up_accel_cmd=last_live100.upAccelCmd, ui_accel_cmd=last_live100.uiAccelCmd,
uf_accel_cmd=last_live100.ufAccelCmd,
up_accel_cmd=last_controls_state.upAccelCmd, ui_accel_cmd=last_controls_state.uiAccelCmd,
uf_accel_cmd=last_controls_state.ufAccelCmd,
d_rel=d_rel, v_rel=v_rel, v_lead=speed_lead,
v_target_lead=last_live100.vTargetLead, pid_speed=last_live100.vPid,
cruise_speed=last_live100.vCruise,
jerk_factor=last_live100.jerkFactor,
a_target=last_live100.aTarget,
v_target_lead=last_controls_state.vTargetLead, pid_speed=last_controls_state.vPid,
cruise_speed=last_controls_state.vCruise,
jerk_factor=last_controls_state.jerkFactor,
a_target=last_controls_state.aTarget,
fcw=fcw)
print("maneuver end")

16
selfdrive/test/plant/plant.py
View File

@ -98,7 +98,7 @@ class Plant(object):
Plant.sendcan = messaging.sub_sock(context, service_list['sendcan'].port)
Plant.model = messaging.pub_sock(context, service_list['model'].port)
Plant.cal = messaging.pub_sock(context, service_list['liveCalibration'].port)
Plant.live100 = messaging.sub_sock(context, service_list['live100'].port)
Plant.controls_state = messaging.sub_sock(context, service_list['controlsState'].port)
Plant.plan = messaging.sub_sock(context, service_list['plan'].port)
Plant.messaging_initialized = True
@ -159,10 +159,10 @@ class Plant(object):
can_msgs.extend(can_capnp_to_can_list(a.sendcan, [0,2]))
self.cp.update_can(can_msgs)
# ******** get live100 messages for plotting ***
live100_msgs = []
for a in messaging.drain_sock(Plant.live100):
live100_msgs.append(a.live100)
# ******** get controlsState messages for plotting ***
controls_state_msgs = []
for a in messaging.drain_sock(Plant.controls_state):
controls_state_msgs.append(a.controlsState)
fcw = None
for a in messaging.drain_sock(Plant.plan):
@ -241,6 +241,7 @@ class Plant(object):
'EPB_STATE',
'BRAKE_HOLD_ACTIVE',
'INTERCEPTOR_GAS',
'IMPERIAL_UNIT',
])
vls = vls_tuple(
self.speed_sensor(speed),
@ -271,7 +272,8 @@ class Plant(object):
self.main_on,
0, # EPB State
0, # Brake hold
0 # Interceptor feedback
0, # Interceptor feedback
False
)
# TODO: publish each message at proper frequency
@ -351,7 +353,7 @@ class Plant(object):
self.distance_lead_prev = distance_lead
self.rk.keep_time()
return (distance, speed, acceleration, distance_lead, brake, gas, steer_torque, fcw, live100_msgs)
return (distance, speed, acceleration, distance_lead, brake, gas, steer_torque, fcw, controls_state_msgs)
# simple engage in standalone mode
def plant_thread(rate=100):

55
selfdrive/thermald.py
View File

@ -7,7 +7,7 @@ from selfdrive.version import training_version
from selfdrive.swaglog import cloudlog
import selfdrive.messaging as messaging
from selfdrive.services import service_list
from selfdrive.loggerd.config import ROOT
from selfdrive.loggerd.config import get_available_percent
from common.params import Params
from common.realtime import sec_since_boot
from common.numpy_fast import clip
@ -121,37 +121,6 @@ def check_car_battery_voltage(should_start, health, charging_disabled):
return charging_disabled
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 thermald_thread():
setup_eon_fan()
@ -170,10 +139,10 @@ def thermald_thread():
started_ts = None
ignition_seen = False
started_seen = False
passive_starter = LocationStarter()
thermal_status = ThermalStatus.green
health_sock.RCVTIMEO = 1500
current_filter = FirstOrderFilter(0., CURRENT_TAU, 1.)
health_prev = None
# Make sure charging is enabled
charging_disabled = False
@ -187,9 +156,13 @@ def thermald_thread():
location = location.gpsLocation if location else None
msg = read_thermal()
# clear car params when panda gets disconnected
if health is None and health_prev is not None:
params.panda_disconnect()
health_prev = health
# loggerd is gated based on free space
statvfs = os.statvfs(ROOT)
avail = (statvfs.f_bavail * 1.0)/statvfs.f_blocks
avail = get_available_percent() / 100.0
# thermal message now also includes free space
msg.thermal.freeSpace = avail
@ -253,17 +226,9 @@ def thermald_thread():
# have we seen a panda?
passive = (params.get("Passive") == "1")
# start on gps movement if we haven't seen ignition and are in passive mode
should_start = should_start or (not (ignition_seen and health) # seen ignition and panda is connected
and passive
and passive_starter.update(started_ts, location))
# with 2% left, we killall, otherwise the phone will take a long time to boot
should_start = should_start and msg.thermal.freeSpace > 0.02
# require usb power in passive mode
should_start = should_start and (not passive or msg.thermal.usbOnline)
# confirm we have completed training and aren't uninstalling
should_start = should_start and accepted_terms and (passive or completed_training) and (not do_uninstall)
@ -276,7 +241,6 @@ def thermald_thread():
if should_start:
off_ts = None
if started_ts is None:
params.car_start()
started_ts = sec_since_boot()
started_seen = True
os.system('echo performance > /sys/class/devfreq/soc:qcom,cpubw/governor')
@ -295,7 +259,7 @@ def thermald_thread():
#charging_disabled = check_car_battery_voltage(should_start, health, charging_disabled)
msg.thermal.chargingDisabled = charging_disabled
msg.thermal.chargingError = current_filter.x > 0. # if current is positive, then battery is being discharged
msg.thermal.chargingError = current_filter.x > 0. and msg.thermal.batteryPercent < 90 # if current is positive, then battery is being discharged
msg.thermal.started = started_ts is not None
msg.thermal.startedTs = int(1e9*(started_ts or 0))
@ -319,4 +283,3 @@ def main(gctx=None):
if __name__ == "__main__":
main()

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