diff --git a/.gitignore b/.gitignore
index f247c5651ece0e..5bf9742e9c1f15 100644
--- a/.gitignore
+++ b/.gitignore
@@ -46,3 +46,7 @@ openpilot
notebooks
xx
+/selfdrive/tinklad/params
+/selfdrive/tinklad/bb_openpilot.cfg
+/selfdrive/tinklad/tinklad-cache
+/selfdrive/tinklad/bb_openpilot_config.cfg
diff --git a/.python-version b/.python-version
index 35b46aeacab588..c1e43e6d45b265 100644
--- a/.python-version
+++ b/.python-version
@@ -1 +1 @@
-2.7.16
+3.7.3
diff --git a/README.md b/README.md
index fe82e2c5c113b8..13788d793a7ca7 100644
--- a/README.md
+++ b/README.md
@@ -67,15 +67,15 @@ Supported Cars
| ---------------------| ---------------------------| ---------------------| --------| ---------------| -----------------| ---------------|-------------------|
| Acura | ILX 2016-18 | AcuraWatch Plus | Yes | Yes | 25mph1| 25mph | Nidec |
| Acura | RDX 2016-18 | AcuraWatch Plus | Yes | Yes | 25mph1| 12mph | Nidec |
-| Buick3 | Regal 2018 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom7|
-| Chevrolet3| Malibu 2017 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom7|
-| Chevrolet3| Volt 2017-18 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom7|
-| Cadillac3 | ATS 2018 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom7|
+| Buick3 | Regal 2018 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom6|
+| Chevrolet3| Malibu 2017 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom6|
+| Chevrolet3| Volt 2017-18 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom6|
+| Cadillac3 | ATS 2018 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom6|
| Chrysler | Pacifica 2017-18 | Adaptive Cruise | Yes | Stock | 0mph | 9mph | FCA |
| Chrysler | Pacifica Hybrid 2017-18 | Adaptive Cruise | Yes | Stock | 0mph | 9mph | FCA |
| Chrysler | Pacifica Hybrid 2019 | Adaptive Cruise | Yes | Stock | 0mph | 39mph | FCA |
-| GMC3 | Acadia Denali 2018 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom7|
-| Holden3 | Astra 2017 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom7|
+| GMC3 | Acadia Denali 2018 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom6|
+| Holden3 | Astra 2017 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom6|
| Honda | Accord 2018-19 | All | Yes | Stock | 0mph | 3mph | Bosch |
| Honda | Accord Hybrid 2018-19 | All | Yes | Stock | 0mph | 3mph | Bosch |
| Honda | Civic Sedan/Coupe 2016-18 | Honda Sensing | Yes | Yes | 0mph | 12mph | Nidec |
@@ -90,30 +90,32 @@ Supported Cars
| Honda | Pilot 2016-18 | Honda Sensing | Yes | Yes | 25mph1| 12mph | Nidec |
| Honda | Pilot 2019 | All | Yes | Yes | 25mph1| 12mph | Inverted Nidec |
| Honda | Ridgeline 2017-19 | Honda Sensing | Yes | Yes | 25mph1| 12mph | Nidec |
-| Hyundai | Santa Fe 2019 | All | Yes | Stock | 0mph | 0mph | Custom6|
-| Hyundai | Elantra 2017-19 | SCC + LKAS | Yes | Stock | 19mph | 34mph | Custom6|
-| Hyundai | Genesis 2018 | All | Yes | Stock | 19mph | 34mph | Custom6|
+| Hyundai | Santa Fe 2019 | All | Yes | Stock | 0mph | 0mph | Custom5|
+| Hyundai | Elantra 2017-19 | SCC + LKAS | Yes | Stock | 19mph | 34mph | Custom5|
+| Hyundai | Genesis 2018 | All | Yes | Stock | 19mph | 34mph | Custom5|
| Jeep | Grand Cherokee 2016-18 | Adaptive Cruise | Yes | Stock | 0mph | 9mph | FCA |
| Jeep | Grand Cherokee 2019 | Adaptive Cruise | Yes | Stock | 0mph | 39mph | FCA |
-| Kia | Optima 2019 | SCC + LKAS | Yes | Stock | 0mph | 0mph | Custom6|
-| Kia | Sorento 2018 | All | Yes | Stock | 0mph | 0mph | Custom6|
-| Kia | Stinger 2018 | SCC + LKAS | Yes | Stock | 0mph | 0mph | Custom6|
+| Kia | Optima 2019 | SCC + LKAS | Yes | Stock | 0mph | 0mph | Custom5|
+| Kia | Sorento 2018 | All | Yes | Stock | 0mph | 0mph | Custom5|
+| Kia | Stinger 2018 | SCC + LKAS | Yes | Stock | 0mph | 0mph | Custom5|
| Lexus | CT Hybrid 2017-18 | All | Yes | Yes2| 0mph | 0mph | Toyota |
+| Lexus | ES 2019 | All | Yes | Yes | 0mph | 0mph | Toyota |
| Lexus | ES Hybrid 2019 | All | Yes | Yes | 0mph | 0mph | Toyota |
| Lexus | RX Hybrid 2016-19 | All | Yes | Yes2| 0mph | 0mph | Toyota |
| Lexus | IS 2017-2019 | All | Yes | Stock | 22mph | 0mph | Toyota |
| Lexus | IS Hybrid 2017 | All | Yes | Stock | 0mph | 0mph | Toyota |
-| Subaru | Crosstrek 2018 | EyeSight | Yes | Stock | 0mph | 0mph | Custom4|
-| Subaru | Impreza 2019 | EyeSight | Yes | Stock | 0mph | 0mph | Custom4|
+| Subaru | Crosstrek 2018-19 | EyeSight | Yes | Stock | 0mph | 0mph | Subaru |
+| Subaru | Impreza 2019-20 | EyeSight | Yes | Stock | 0mph | 0mph | Subaru |
| Toyota | Avalon 2016 | TSS-P | Yes | Yes2| 20mph1| 0mph | Toyota |
| Toyota | Avalon 2017-18 | All | Yes | Yes2| 20mph1| 0mph | Toyota |
-| Toyota | Camry 2018-19 | All | Yes | Stock | 0mph5 | 0mph | Toyota |
-| Toyota | Camry Hybrid 2018-19 | All | Yes | Stock | 0mph5 | 0mph | Toyota |
+| Toyota | Camry 2018-19 | All | Yes | Stock | 0mph4 | 0mph | Toyota |
+| Toyota | Camry Hybrid 2018-19 | All | Yes | Stock | 0mph4 | 0mph | Toyota |
| Toyota | C-HR 2017-19 | All | Yes | Stock | 0mph | 0mph | Toyota |
| Toyota | C-HR Hybrid 2017-19 | All | Yes | Stock | 0mph | 0mph | Toyota |
| Toyota | Corolla 2017-19 | All | Yes | Yes2| 20mph1| 0mph | Toyota |
| Toyota | Corolla 2020 | All | Yes | Yes | 0mph | 0mph | Toyota |
| Toyota | Corolla Hatchback 2019 | All | Yes | Yes | 0mph | 0mph | Toyota |
+| Toyota | Corolla Hybrid 2019 | All | Yes | Yes | 0mph | 0mph | Toyota |
| Toyota | Highlander 2017-19 | All | Yes | Yes2| 0mph | 0mph | Toyota |
| Toyota | Highlander Hybrid 2017-19 | All | Yes | Yes2| 0mph | 0mph | Toyota |
| Toyota | Prius 2016 | TSS-P | Yes | Yes2| 0mph | 0mph | Toyota |
@@ -129,20 +131,19 @@ Supported Cars
1[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).***
2When 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). ***NOTE: disconnecting the DSU disables Automatic Emergency Braking (AEB).***
3[GM installation guide](https://zoneos.com/volt/). ***NOTE: disconnecting the ASCM disables Automatic Emergency Braking (AEB).***
-4Subaru Giraffe is DIY.
-528mph for Camry 4CYL L, 4CYL LE and 4CYL SE which don't have Full-Speed Range Dynamic Radar Cruise Control.
-6Open 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.
-7Community built Giraffe, find more information [here](https://zoneos.com/shop/).
+428mph for Camry 4CYL L, 4CYL LE and 4CYL SE which don't have Full-Speed Range Dynamic Radar Cruise Control.
+5Open 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.
+6Community built Giraffe, find more information [here](https://zoneos.com/shop/).
Community Maintained Cars
------
| Make | Model | Supported Package | Lateral | Longitudinal | No Accel Below | No Steer Below | Giraffe |
| ---------------------| -------------------------| ---------------------| --------| ---------------| -----------------| ---------------|-------------------|
-| Tesla | Model S 2012-13 | All | Yes | Not yet | Not applicable | 0mph | Custom8|
+| Tesla | Model S 2012-13 | All | Yes | Not yet | Not applicable | 0mph | Custom7|
[[Tesla Model S Pull Request]](https://github.com/commaai/openpilot/pull/246)
-8Community built Giraffe, find more information here [Community Tesla Giraffe](https://github.com/jeankalud/neo/tree/tesla_giraffe/giraffe/tesla)
+7Community built Giraffe, find more information here [Community Tesla Giraffe](https://github.com/jeankalud/neo/tree/tesla_giraffe/giraffe/tesla)
Community Maintained Cars are not confirmed by comma.ai to meet our [safety model](https://github.com/commaai/openpilot/blob/devel/SAFETY.md). Be extra cautious using them.
diff --git a/cereal/tinkla.capnp b/cereal/tinkla.capnp
index 9e71cb789a07b7..ab35b430b4e7f9 100644
--- a/cereal/tinkla.capnp
+++ b/cereal/tinkla.capnp
@@ -7,7 +7,7 @@ $Java.outerClassname("Tinkla");
@0xfc8dda643156b95d;
-const interfaceVersion :Float32 = 2.2;
+const interfaceVersion :Float32 = 3; # Use integer values here
struct Interface {
diff --git a/common/dbc.py b/common/dbc.py
index e65ee38be87b16..aa52e40671aca2 100755
--- a/common/dbc.py
+++ b/common/dbc.py
@@ -101,13 +101,8 @@ def __init__(self, fn):
defvals = defvals.replace("?",r"\?") #escape sequence in C++
defvals = defvals.split('"')[:-1]
- defs = defvals[1::2]
- #cleanup, convert to UPPER_CASE_WITH_UNDERSCORES
- for i,d in enumerate(defs):
- d = defs[i].strip().upper()
- defs[i] = d.replace(" ","_")
-
- defvals[1::2] = defs
+ # convert strings to UPPER_CASE_WITH_UNDERSCORES
+ defvals[1::2] = [d.strip().upper().replace(" ","_") for d in defvals[1::2]]
defvals = '"'+"".join(str(i) for i in defvals)+'"'
self.def_vals[ids].append((sgname, defvals))
@@ -152,22 +147,20 @@ def encode(self, msg_id, dd):
ival = dd.get(s.name)
if ival is not None:
- b2 = s.size
- if s.is_little_endian:
- b1 = s.start_bit
- else:
- b1 = (s.start_bit // 8) * 8 + (-s.start_bit - 1) % 8
- bo = 64 - (b1 + s.size)
-
ival = (ival / s.factor) - s.offset
ival = int(round(ival))
if s.is_signed and ival < 0:
- ival = (1 << b2) + ival
+ ival = (1 << s.size) + ival
+
+ if s.is_little_endian:
+ shift = s.start_bit
+ else:
+ b1 = (s.start_bit // 8) * 8 + (-s.start_bit - 1) % 8
+ shift = 64 - (b1 + s.size)
- shift = b1 if s.is_little_endian else bo
- mask = ((1 << b2) - 1) << shift
- dat = (ival & ((1 << b2) - 1)) << shift
+ mask = ((1 << s.size) - 1) << shift
+ dat = (ival & ((1 << s.size) - 1)) << shift
if s.is_little_endian:
mask = self.reverse_bytes(mask)
@@ -227,30 +220,24 @@ def decode(self, x, arr=None, debug=False):
factor = s[5]
offset = s[6]
- b2 = signal_size
- if little_endian:
- b1 = start_bit
- else:
- b1 = (start_bit // 8) * 8 + (-start_bit - 1) % 8
- bo = 64 - (b1 + signal_size)
-
if little_endian:
if le is None:
le = struct.unpack("Q", st)[0]
- shift_amount = bo
tmp = be
+ b1 = (start_bit // 8) * 8 + (-start_bit - 1) % 8
+ shift_amount = 64 - (b1 + signal_size)
if shift_amount < 0:
continue
- tmp = (tmp >> shift_amount) & ((1 << b2) - 1)
- if signed and (tmp >> (b2 - 1)):
- tmp -= (1 << b2)
+ tmp = (tmp >> shift_amount) & ((1 << signal_size) - 1)
+ if signed and (tmp >> (signal_size - 1)):
+ tmp -= (1 << signal_size)
tmp = tmp * factor + offset
diff --git a/common/params.py b/common/params.py
index 7397347db84f1f..b2c7618e5dc215 100755
--- a/common/params.py
+++ b/common/params.py
@@ -81,6 +81,7 @@ class UnknownKeyName(Exception):
"Passive": [TxType.PERSISTENT],
"RecordFront": [TxType.PERSISTENT],
"ReleaseNotes": [TxType.PERSISTENT],
+ "SafetyModelLock": [TxType.PERSISTENT],
"ShouldDoUpdate": [TxType.CLEAR_ON_MANAGER_START],
"SpeedLimitOffset": [TxType.PERSISTENT],
"SubscriberInfo": [TxType.PERSISTENT],
diff --git a/opendbc/generator/toyota/lexus_is_2018_pt.dbc b/opendbc/generator/toyota/lexus_is_2018_pt.dbc
index e1aff7c4faead7..ffcd949f05fbd3 100644
--- a/opendbc/generator/toyota/lexus_is_2018_pt.dbc
+++ b/opendbc/generator/toyota/lexus_is_2018_pt.dbc
@@ -11,7 +11,7 @@ BO_ 705 GAS_PEDAL: 8 XXX
SG_ GAS_PEDAL : 55|8@0+ (0.005,0) [0|1] "" XXX
BO_ 608 STEER_TORQUE_SENSOR: 8 XXX
- SG_ STEER_TORQUE_EPS : 47|16@0- (0.66,0) [-20000|20000] "" XXX
+ SG_ STEER_TORQUE_EPS : 47|16@0- (0.77,0) [-20000|20000] "" XXX
SG_ STEER_TORQUE_DRIVER : 15|16@0- (1,0) [-32768|32767] "" XXX
SG_ STEER_OVERRIDE : 0|1@0+ (1,0) [0|1] "" XXX
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
diff --git a/opendbc/honda_accord_touring_2016_can.dbc b/opendbc/honda_accord_touring_2016_can.dbc
index e8f388de08692c..744e55a9b5e3b5 100644
--- a/opendbc/honda_accord_touring_2016_can.dbc
+++ b/opendbc/honda_accord_touring_2016_can.dbc
@@ -177,14 +177,14 @@ BO_ 597 ROUGH_WHEEL_SPEED: 8 VSA
SG_ SET_TO_X55_2 : 47|8@0+ (1,0) [0|255] "" NEO
SG_ LONG_COUNTER : 55|8@0+ (1,0) [0|255] "" XXX
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX
- SG_ CHECKSUM : 59|4@1+ (1,0) [0|15] "" XXX
+ SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX
BO_ 660 SCM_COMMANDS: 8 SCM
SG_ RIGHT_BLINKER : 6|1@0+ (1,0) [0|1] "" NEO
SG_ LEFT_BLINKER : 5|1@0+ (1,0) [0|1] "" NEO
SG_ WIPERS_SPEED : 4|2@0+ (1,0) [0|3] "" NEO
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX
- SG_ CHECKSUM : 59|4@1+ (1,0) [0|15] "" XXX
+ SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX
BO_ 661 XXX_10: 4 XXX
SG_ COUNTER : 29|2@0+ (1,0) [0|3] "" XXX
diff --git a/opendbc/lexus_is_2018_pt_generated.dbc b/opendbc/lexus_is_2018_pt_generated.dbc
index 9e4eb514545dff..ac6bdf3832e5dc 100644
--- a/opendbc/lexus_is_2018_pt_generated.dbc
+++ b/opendbc/lexus_is_2018_pt_generated.dbc
@@ -370,7 +370,7 @@ BO_ 705 GAS_PEDAL: 8 XXX
SG_ GAS_PEDAL : 55|8@0+ (0.005,0) [0|1] "" XXX
BO_ 608 STEER_TORQUE_SENSOR: 8 XXX
- SG_ STEER_TORQUE_EPS : 47|16@0- (0.66,0) [-20000|20000] "" XXX
+ SG_ STEER_TORQUE_EPS : 47|16@0- (0.77,0) [-20000|20000] "" XXX
SG_ STEER_TORQUE_DRIVER : 15|16@0- (1,0) [-32768|32767] "" XXX
SG_ STEER_OVERRIDE : 0|1@0+ (1,0) [0|1] "" XXX
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
diff --git a/panda/board/drivers/can.h b/panda/board/drivers/can.h
index 528f07f1d179e5..c45a3fe8d1d754 100644
--- a/panda/board/drivers/can.h
+++ b/panda/board/drivers/can.h
@@ -43,7 +43,7 @@ int can_live = 0, pending_can_live = 0, can_loopback = 0, can_silent = ALL_CAN_S
CAN_FIFOMailBox_TypeDef elems_##x[size]; \
can_ring can_##x = { .w_ptr = 0, .r_ptr = 0, .fifo_size = size, .elems = (CAN_FIFOMailBox_TypeDef *)&elems_##x };
-can_buffer(rx_q, 0x1500)
+can_buffer(rx_q, 0x1000)
can_buffer(tx1_q, 0x100)
can_buffer(tx2_q, 0x100)
can_buffer(tx3_q, 0x100)
diff --git a/panda/tests/pedal/enter_canloader.py b/panda/tests/pedal/enter_canloader.py
index 8d263a37275731..f76ac9597d67c4 100755
--- a/panda/tests/pedal/enter_canloader.py
+++ b/panda/tests/pedal/enter_canloader.py
@@ -25,7 +25,7 @@ def _handle_timeout(signum, frame):
finally:
signal.alarm(0)
- #print "R:",ret.encode("hex")
+ # "R:",ret.encode("hex")
return ret
def controlWrite(self, request_type, request, value, index, data, timeout=0):
@@ -58,7 +58,7 @@ def bulkRead(self, endpoint, length, timeout=0):
while 1:
if len(p.can_recv()) == 0:
break
- print "entering bootloader mode"
+ print("entering bootloader mode")
if args.recover:
p.can_send(0x200, b"\xce\xfa\xad\xde\x1e\x0b\xb0\x02", 0)
p.can_send(0x551, b"\xce\xfa\xad\xde\x1e\x0b\xb0\x02", 0)
diff --git a/phonelibs/json11/json11.o b/phonelibs/json11/json11.o
index 516adf37346d2f..160a552565d48b 100644
Binary files a/phonelibs/json11/json11.o and b/phonelibs/json11/json11.o differ
diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc
index 24e01e552b6002..a703fdf4ca7545 100644
--- a/selfdrive/boardd/boardd.cc
+++ b/selfdrive/boardd/boardd.cc
@@ -49,6 +49,7 @@ const uint32_t NO_IGNITION_CNT_MAX = 2 * 60 * 60 * 24 * 3; // turn off charge a
uint32_t no_ignition_cnt = 0;
bool connected_once = false;
uint8_t ignition_last = 0;
+bool safety_model_locked = false;
pthread_t safety_setter_thread_handle = -1;
pthread_t pigeon_thread_handle = -1;
@@ -74,12 +75,12 @@ void *safety_setter_thread(void *s) {
}
LOGW("got CarVin %s", value_vin);
- pthread_mutex_lock(&usb_lock);
-
// VIN query done, stop listening to OBDII
- libusb_control_transfer(dev_handle, 0x40, 0xdc, (uint16_t)(cereal::CarParams::SafetyModel::NO_OUTPUT), 0, NULL, 0, TIMEOUT);
-
- pthread_mutex_unlock(&usb_lock);
+ if (!safety_model_locked) {
+ pthread_mutex_lock(&usb_lock);
+ libusb_control_transfer(dev_handle, 0x40, 0xdc, (uint16_t)(cereal::CarParams::SafetyModel::NO_OUTPUT), 0, NULL, 0, TIMEOUT);
+ pthread_mutex_unlock(&usb_lock);
+ }
char *value;
size_t value_sz = 0;
@@ -125,6 +126,11 @@ void *safety_setter_thread(void *s) {
bool usb_connect() {
int err;
unsigned char hw_query[1] = {0};
+ char *value_safety_model;
+ size_t value_safety_model_sz = 0;
+ int safety_model;
+ const int result = read_db_value(NULL, "SafetyModelLock", &value_safety_model, &value_safety_model_sz);
+
ignition_last = 0;
dev_handle = libusb_open_device_with_vid_pid(ctx, 0xbbaa, 0xddcc);
@@ -140,6 +146,16 @@ bool usb_connect() {
libusb_control_transfer(dev_handle, 0xc0, 0xe5, 1, 0, NULL, 0, TIMEOUT);
}
+ // check if safety mode is forced (needed to support gm)
+ if (value_safety_model_sz > 0) {
+ sscanf(value_safety_model, "%d", &safety_model);
+ // sanity check that we are not setting all output
+ assert(safety_model != (int)(cereal::CarParams::SafetyModel::ALL_OUTPUT));
+ safety_model_locked = true;
+ LOGW("Setting Locked Safety Model %s", value_safety_model);
+ libusb_control_transfer(dev_handle, 0x40, 0xdc, (uint16_t)(cereal::CarParams::SafetyModel(safety_model)), 0, NULL, 0, TIMEOUT);
+ }
+
// power off ESP
libusb_control_transfer(dev_handle, 0xc0, 0xd9, 0, 0, NULL, 0, TIMEOUT);
@@ -297,10 +313,11 @@ void can_health(void *s) {
assert((result == 0) || (result == ERR_NO_VALUE));
// diagnostic only is the default, needed for VIN query
- pthread_mutex_lock(&usb_lock);
- libusb_control_transfer(dev_handle, 0x40, 0xdc, (uint16_t)(cereal::CarParams::SafetyModel::ELM327), 0, NULL, 0, TIMEOUT);
- pthread_mutex_unlock(&usb_lock);
-
+ if (!safety_model_locked) {
+ pthread_mutex_lock(&usb_lock);
+ libusb_control_transfer(dev_handle, 0x40, 0xdc, (uint16_t)(cereal::CarParams::SafetyModel::ELM327), 0, NULL, 0, TIMEOUT);
+ pthread_mutex_unlock(&usb_lock);
+ }
if (safety_setter_thread_handle == -1) {
err = pthread_create(&safety_setter_thread_handle, NULL, safety_setter_thread, NULL);
assert(err == 0);
diff --git a/selfdrive/can/can_define.py b/selfdrive/can/can_define.py
index d6ffc5c669e9fb..eb708869f09963 100644
--- a/selfdrive/can/can_define.py
+++ b/selfdrive/can/can_define.py
@@ -35,5 +35,5 @@ def __init__(self, dbc_name):
self.dv[msgname] = {}
# two ways to lookup: address or msg name
- self.dv[address][sgname] = {v: d for v, d in zip(values, defs)} #build dict
+ self.dv[address][sgname] = dict(zip(values, defs))
self.dv[msgname][sgname] = self.dv[address][sgname]
diff --git a/selfdrive/can/common.h b/selfdrive/can/common.h
index 4f097228432543..d6c2e4760bd40a 100644
--- a/selfdrive/can/common.h
+++ b/selfdrive/can/common.h
@@ -7,11 +7,13 @@
#define ARRAYSIZE(x) (sizeof(x)/sizeof(x[0]))
-
unsigned int honda_checksum(unsigned int address, uint64_t d, int l);
unsigned int toyota_checksum(unsigned int address, uint64_t d, int l);
unsigned int pedal_checksum(unsigned int address, uint64_t d, int l);
+void init_crc_lookup_tables();
+unsigned int volkswagen_crc(unsigned int address, uint64_t d, int l);
+
struct SignalPackValue {
const char* name;
double value;
@@ -44,6 +46,8 @@ enum SignalType {
TOYOTA_CHECKSUM,
PEDAL_CHECKSUM,
PEDAL_COUNTER,
+ VOLKSWAGEN_CHECKSUM,
+ VOLKSWAGEN_COUNTER,
};
struct Signal {
diff --git a/selfdrive/can/dbc_template.cc b/selfdrive/can/dbc_template.cc
index 776403b22f03ff..be249012e36d83 100644
--- a/selfdrive/can/dbc_template.cc
+++ b/selfdrive/can/dbc_template.cc
@@ -31,6 +31,10 @@ const Signal sigs_{{address}}[] = {
.type = SignalType::PEDAL_CHECKSUM,
{% elif address in [512, 513] and sig.name == "COUNTER_PEDAL" %}
.type = SignalType::PEDAL_COUNTER,
+ {% elif checksum_type == "volkswagen" and sig.name == "CHECKSUM" %}
+ .type = SignalType::VOLKSWAGEN_CHECKSUM,
+ {% elif checksum_type == "volkswagen" and sig.name == "COUNTER" %}
+ .type = SignalType::VOLKSWAGEN_COUNTER,
{% else %}
.type = SignalType::DEFAULT,
{% endif %}
diff --git a/selfdrive/can/libdbc_py.py b/selfdrive/can/libdbc_py.py
index fbf36a3d4d0764..36713eab0fd60c 100644
--- a/selfdrive/can/libdbc_py.py
+++ b/selfdrive/can/libdbc_py.py
@@ -41,6 +41,8 @@
TOYOTA_CHECKSUM,
PEDAL_CHECKSUM,
PEDAL_COUNTER,
+ VOLKSWAGEN_CHECKSUM,
+ VOLKSWAGEN_COUNTER,
} SignalType;
typedef struct {
diff --git a/selfdrive/can/packer.cc b/selfdrive/can/packer.cc
index d0e84197887a8f..88fca77da05c3f 100644
--- a/selfdrive/can/packer.cc
+++ b/selfdrive/can/packer.cc
@@ -51,6 +51,8 @@ namespace {
signal_lookup[std::make_pair(msg->address, std::string(sig->name))] = *sig;
}
}
+
+ init_crc_lookup_tables();
}
uint64_t pack(uint32_t address, const std::vector &signals, int counter) {
@@ -82,23 +84,30 @@ namespace {
}
auto sig = sig_it->second;
- if (sig.type != SignalType::HONDA_COUNTER){
+ if ((sig.type != SignalType::HONDA_COUNTER) && (sig.type != SignalType::VOLKSWAGEN_COUNTER)) {
WARN("COUNTER signal type not valid\n");
}
ret = set_value(ret, sig, counter);
}
- auto sig_it = signal_lookup.find(std::make_pair(address, "CHECKSUM"));
- if (sig_it != signal_lookup.end()) {
- auto sig = sig_it->second;
- if (sig.type == SignalType::HONDA_CHECKSUM){
+ auto sig_it_checksum = signal_lookup.find(std::make_pair(address, "CHECKSUM"));
+ if (sig_it_checksum != signal_lookup.end()) {
+ auto sig = sig_it_checksum->second;
+ if (sig.type == SignalType::HONDA_CHECKSUM) {
unsigned int chksm = honda_checksum(address, ret, message_lookup[address].size);
ret = set_value(ret, sig, chksm);
}
- else if (sig.type == SignalType::TOYOTA_CHECKSUM){
+ else if (sig.type == SignalType::TOYOTA_CHECKSUM) {
unsigned int chksm = toyota_checksum(address, ret, message_lookup[address].size);
ret = set_value(ret, sig, chksm);
+ }
+ else if (sig.type == SignalType::VOLKSWAGEN_CHECKSUM) {
+ // FIXME: Hackish fix for an endianness issue. The message is in reverse byte order
+ // until later in the pack process. Checksums can be run backwards, CRCs not so much.
+ // The correct fix is unclear but this works for the moment.
+ unsigned int chksm = volkswagen_crc(address, ReverseBytes(ret), message_lookup[address].size);
+ ret = set_value(ret, sig, chksm);
} else {
//WARN("CHECKSUM signal type not valid\n");
}
diff --git a/selfdrive/can/packer_impl.pyx b/selfdrive/can/packer_impl.pyx
index 7429338ac40721..d7c6119b753874 100644
--- a/selfdrive/can/packer_impl.pyx
+++ b/selfdrive/can/packer_impl.pyx
@@ -20,7 +20,9 @@ ctypedef enum SignalType:
HONDA_COUNTER,
TOYOTA_CHECKSUM,
PEDAL_CHECKSUM,
- PEDAL_COUNTER
+ PEDAL_COUNTER,
+ VOLKSWAGEN_CHECKSUM,
+ VOLKSWAGEN_COUNTER
cdef struct Signal:
const char* name
diff --git a/selfdrive/can/parser.cc b/selfdrive/can/parser.cc
index 6b9cf575869210..d925a0efb13df3 100644
--- a/selfdrive/can/parser.cc
+++ b/selfdrive/can/parser.cc
@@ -24,9 +24,11 @@
// #define DEBUG printf
#define INFO printf
-
#define MAX_BAD_COUNTER 5
+// Static lookup table for fast computation of CRC8 poly 0x2F, aka 8H2F/AUTOSAR
+uint8_t crc8_lut_8h2f[256];
+
unsigned int honda_checksum(unsigned int address, uint64_t d, int l) {
d >>= ((8-l)*8); // remove padding
d >>= 4; // remove checksum
@@ -75,6 +77,98 @@ unsigned int pedal_checksum(unsigned int address, uint64_t d, int l) {
return crc;
}
+void gen_crc_lookup_table(uint8_t poly, uint8_t crc_lut[])
+{
+ uint8_t crc;
+ int i, j;
+
+ for (i = 0; i < 256; i++) {
+ crc = i;
+ for (j = 0; j < 8; j++) {
+ if ((crc & 0x80) != 0)
+ crc = (uint8_t)((crc << 1) ^ poly);
+ else
+ crc <<= 1;
+ }
+ crc_lut[i] = crc;
+ }
+}
+
+void init_crc_lookup_tables()
+{
+ // At init time, set up static lookup tables for fast CRC computation.
+
+ gen_crc_lookup_table(0x2F, crc8_lut_8h2f); // CRC-8 8H2F/AUTOSAR for Volkswagen
+}
+
+unsigned int volkswagen_crc(unsigned int address, uint64_t d, int l)
+{
+ // Volkswagen uses standard CRC8 8H2F/AUTOSAR, but they compute it with
+ // a magic variable padding byte tacked onto the end of the payload.
+ // https://www.autosar.org/fileadmin/user_upload/standards/classic/4-3/AUTOSAR_SWS_CRCLibrary.pdf
+
+ uint8_t *dat = (uint8_t *)&d;
+ uint8_t crc = 0xFF; // Standard init value for CRC8 8H2F/AUTOSAR
+
+ // CRC the payload first, skipping over the first byte where the CRC lives.
+ for (int i = 1; i < l; i++) {
+ crc ^= dat[i];
+ crc = crc8_lut_8h2f[crc];
+ }
+
+ // Look up and apply the magic final CRC padding byte, which permutes by CAN
+ // address, and additionally (for SOME addresses) by the message counter.
+ uint8_t counter = dat[1] & 0x0F;
+ switch(address) {
+ case 0x86: // LWI_01 Steering Angle
+ crc ^= (uint8_t[]){0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86}[counter];
+ break;
+ case 0x9F: // EPS_01 Electric Power Steering
+ crc ^= (uint8_t[]){0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5}[counter];
+ break;
+ case 0xAD: // Getriebe_11 Automatic Gearbox
+ crc ^= (uint8_t[]){0x3F,0x69,0x39,0xDC,0x94,0xF9,0x14,0x64,0xD8,0x6A,0x34,0xCE,0xA2,0x55,0xB5,0x2C}[counter];
+ break;
+ case 0xFD: // ESP_21 Electronic Stability Program
+ crc ^= (uint8_t[]){0xB4,0xEF,0xF8,0x49,0x1E,0xE5,0xC2,0xC0,0x97,0x19,0x3C,0xC9,0xF1,0x98,0xD6,0x61}[counter];
+ break;
+ case 0x106: // ESP_05 Electronic Stability Program
+ crc ^= (uint8_t[]){0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07}[counter];
+ break;
+ case 0x117: // ACC_10 Automatic Cruise Control
+ crc ^= (uint8_t[]){0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC}[counter];
+ break;
+ case 0x122: // ACC_06 Automatic Cruise Control
+ crc ^= (uint8_t[]){0x37,0x7D,0xF3,0xA9,0x18,0x46,0x6D,0x4D,0x3D,0x71,0x92,0x9C,0xE5,0x32,0x10,0xB9}[counter];
+ break;
+ case 0x126: // HCA_01 Heading Control Assist
+ crc ^= (uint8_t[]){0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA}[counter];
+ break;
+ case 0x12B: // GRA_ACC_01 Steering wheel controls for ACC
+ crc ^= (uint8_t[]){0x6A,0x38,0xB4,0x27,0x22,0xEF,0xE1,0xBB,0xF8,0x80,0x84,0x49,0xC7,0x9E,0x1E,0x2B}[counter];
+ break;
+ case 0x187: // EV_Gearshift "Gear" selection data for EVs with no gearbox
+ crc ^= (uint8_t[]){0x7F,0xED,0x17,0xC2,0x7C,0xEB,0x44,0x21,0x01,0xFA,0xDB,0x15,0x4A,0x6B,0x23,0x05}[counter];
+ break;
+ case 0x30C: // ACC_02 Automatic Cruise Control
+ crc ^= (uint8_t[]){0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F}[counter];
+ break;
+ case 0x3C0: // Klemmen_Status_01 ignition and starting status
+ crc ^= (uint8_t[]){0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3}[counter];
+ break;
+ case 0x65D: // ESP_20 Electronic Stability Program
+ crc ^= (uint8_t[]){0xAC,0xB3,0xAB,0xEB,0x7A,0xE1,0x3B,0xF7,0x73,0xBA,0x7C,0x9E,0x06,0x5F,0x02,0xD9}[counter];
+ break;
+ default: // As-yet undefined CAN message, CRC check expected to fail
+ INFO("Attempt to CRC check undefined Volkswagen message 0x%02X\n", address);
+ crc ^= (uint8_t[]){0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}[counter];
+ break;
+ }
+ crc = crc8_lut_8h2f[crc];
+
+ return crc ^ 0xFF; // Return after standard final XOR for CRC8 8H2F/AUTOSAR
+}
+
namespace {
uint64_t read_u64_be(const uint8_t* v) {
@@ -129,16 +223,11 @@ struct MessageState {
tmp -= (tmp >> (sig.b2-1)) ? (1ULL << sig.b2) : 0; //signed
}
- // Testing both little and big endian signals (Tesla messages)
- //if ( (address == 0x318) || (address == 0x118)) {
- // INFO("parse %X %s -> %f, dat -> %lX\n", address, sig.name, tmp * sig.factor + sig.offset, dat);
- //}
-
- DEBUG("parse %X %s -> %lld\n", address, sig.name, tmp);
+ DEBUG("parse 0x%X %s -> %lld\n", address, sig.name, tmp);
if (sig.type == SignalType::HONDA_CHECKSUM) {
if (honda_checksum(address, dat, size) != tmp) {
- INFO("%X CHECKSUM FAIL\n", address);
+ INFO("0x%X CHECKSUM FAIL\n", address);
return false;
}
} else if (sig.type == SignalType::HONDA_COUNTER) {
@@ -147,12 +236,21 @@ struct MessageState {
}
} else if (sig.type == SignalType::TOYOTA_CHECKSUM) {
if (toyota_checksum(address, dat, size) != tmp) {
- INFO("%X CHECKSUM FAIL\n", address);
+ INFO("0x%X CHECKSUM FAIL\n", address);
+ return false;
+ }
+ } else if (sig.type == SignalType::VOLKSWAGEN_CHECKSUM) {
+ if (volkswagen_crc(address, dat, size) != tmp) {
+ INFO("0x%X CRC FAIL\n", address);
+ return false;
+ }
+ } else if (sig.type == SignalType::VOLKSWAGEN_COUNTER) {
+ if (!update_counter_generic(tmp, sig.b2)) {
return false;
}
} else if (sig.type == SignalType::PEDAL_CHECKSUM) {
if (pedal_checksum(address, dat, size) != tmp) {
- INFO("%X PEDAL CHECKSUM FAIL\n", address);
+ INFO("0x%X PEDAL CHECKSUM FAIL\n", address);
return false;
}
} else if (sig.type == SignalType::PEDAL_COUNTER) {
@@ -176,7 +274,7 @@ struct MessageState {
if (((old_counter+1) & ((1 << cnt_size) -1)) != v) {
counter_fail += 1;
if (counter_fail > 1) {
- INFO("%X COUNTER FAIL %d -- %d vs %d\n", address, counter_fail, old_counter, (int)v);
+ INFO("0x%X COUNTER FAIL %d -- %d vs %d\n", address, counter_fail, old_counter, (int)v);
}
if (counter_fail >= MAX_BAD_COUNTER) {
return false;
@@ -228,7 +326,9 @@ class CANParser {
}
dbc = dbc_lookup(dbc_name);
- assert(dbc);
+ assert(dbc);
+ init_crc_lookup_tables();
+
for (const auto& op : options) {
MessageState state = {
.address = op.address,
diff --git a/selfdrive/can/parser_pyx.pxd b/selfdrive/can/parser_pyx.pxd
index 28048bbc1f0e53..4d7879e1ca848a 100644
--- a/selfdrive/can/parser_pyx.pxd
+++ b/selfdrive/can/parser_pyx.pxd
@@ -14,7 +14,9 @@ ctypedef enum SignalType:
HONDA_COUNTER,
TOYOTA_CHECKSUM,
PEDAL_CHECKSUM,
- PEDAL_COUNTER
+ PEDAL_COUNTER,
+ VOLKSWAGEN_CHECKSUM,
+ VOLKSWAGEN_COUNTER
cdef struct Signal:
const char* name
diff --git a/selfdrive/can/plant_can_parser.py b/selfdrive/can/plant_can_parser.py
index a75435a4c8d5c4..03aaf10c5dc540 100644
--- a/selfdrive/can/plant_can_parser.py
+++ b/selfdrive/can/plant_can_parser.py
@@ -21,7 +21,7 @@ def __init__(self, dbc_f, signals, checks=None):
# - frequency is the frequency at which health should be monitored.
checks = [] if checks is None else checks
- self.msgs_ck = set([check[0] for check in checks])
+ self.msgs_ck = {check[0] for check in checks}
self.frqs = dict(checks)
self.can_valid = False # start with False CAN assumption
# list of received msg we want to monitor counter and checksum for
diff --git a/selfdrive/can/process_dbc.py b/selfdrive/can/process_dbc.py
index 187d413584dcd7..6da527f785bba1 100755
--- a/selfdrive/can/process_dbc.py
+++ b/selfdrive/can/process_dbc.py
@@ -1,5 +1,4 @@
#!/usr/bin/env python3
-from __future__ import print_function
import os
import glob
import sys
@@ -39,50 +38,78 @@ def main():
if dbc_mtime < out_mtime and template_mtime < out_mtime and this_file_mtime < out_mtime:
continue #skip output is newer than template and dbc
- msgs = [(address, msg_name, msg_size, sorted(msg_sigs, key=lambda s: s.name not in (b"COUNTER", b"CHECKSUM"))) # process counter and checksums first
+ msgs = [(address, msg_name, msg_size, sorted(msg_sigs, key=lambda s: s.name not in ("COUNTER", "CHECKSUM"))) # process counter and checksums first
for address, ((msg_name, msg_size), msg_sigs) in sorted(can_dbc.msgs.items()) if msg_sigs]
def_vals = {a: set(b) for a,b in can_dbc.def_vals.items()} #remove duplicates
- def_vals = [(address, sig) for address, sig in sorted(def_vals.items())]
+ def_vals = sorted(def_vals.items())
- if can_dbc.name.startswith("honda") or can_dbc.name.startswith("acura"):
+ if can_dbc.name.startswith(("honda_", "acura_")):
checksum_type = "honda"
checksum_size = 4
- elif can_dbc.name.startswith("toyota") or can_dbc.name.startswith("lexus"):
+ counter_size = 2
+ checksum_start_bit = 3
+ counter_start_bit = 5
+ little_endian = False
+ elif can_dbc.name.startswith(("toyota_", "lexus_")):
checksum_type = "toyota"
checksum_size = 8
+ counter_size = None
+ checksum_start_bit = 7
+ counter_start_bit = None
+ little_endian = False
+ elif can_dbc.name.startswith(("vw_", "volkswagen_", "audi_", "seat_", "skoda_")):
+ checksum_type = "volkswagen"
+ checksum_size = 8
+ counter_size = 4
+ checksum_start_bit = 0
+ counter_start_bit = 0
+ little_endian = True
else:
checksum_type = None
+ checksum_size = None
+ counter_size = None
+ checksum_start_bit = None
+ counter_start_bit = None
+ little_endian = None
+ # sanity checks on expected COUNTER and CHECKSUM rules, as packer and parser auto-compute those signals
for address, msg_name, msg_size, sigs in msgs:
+ dbc_msg_name = dbc_name + " " + msg_name
for sig in sigs:
- if checksum_type is not None and sig.name == b"CHECKSUM":
- if sig.size != checksum_size:
- sys.exit("CHECKSUM is not %d bits longs %s" % (checksum_size, msg_name))
- if checksum_type == "honda" and sig.start_bit % 8 != 3:
- sys.exit("CHECKSUM starts at wrong bit %s" % msg_name)
- if checksum_type == "toyota" and sig.start_bit % 8 != 7:
- sys.exit("CHECKSUM starts at wrong bit %s" % msg_name)
- if checksum_type == "honda" and sig.name == b"COUNTER":
- if sig.size != 2:
- sys.exit("COUNTER is not 2 bits longs %s" % msg_name)
- if sig.start_bit % 8 != 5:
- sys.exit("COUNTER starts at wrong bit %s" % msg_name)
+ if checksum_type is not None:
+ # checksum rules
+ if sig.name == "CHECKSUM":
+ if sig.size != checksum_size:
+ sys.exit("%s: CHECKSUM is not %d bits long" % (dbc_msg_name, checksum_size))
+ if sig.start_bit % 8 != checksum_start_bit:
+ sys.exit("%s: CHECKSUM starts at wrong bit" % dbc_msg_name)
+ if little_endian != sig.is_little_endian:
+ sys.exit("%s: CHECKSUM has wrong endianess" % dbc_msg_name)
+ # counter rules
+ if sig.name == "COUNTER":
+ if counter_size is not None and sig.size != counter_size:
+ sys.exit("%s: COUNTER is not %d bits long" % (dbc_msg_name, counter_size))
+ if counter_start_bit is not None and sig.start_bit % 8 != counter_start_bit:
+ print(counter_start_bit, sig.start_bit)
+ sys.exit("%s: COUNTER starts at wrong bit" % dbc_msg_name)
+ if little_endian != sig.is_little_endian:
+ sys.exit("%s: COUNTER has wrong endianess" % dbc_msg_name)
+ # pedal rules
if address in [0x200, 0x201]:
- if sig.name == b"COUNTER_PEDAL" and sig.size != 4:
- sys.exit("PEDAL COUNTER is not 4 bits longs %s" % msg_name)
- if sig.name == b"CHECKSUM_PEDAL" and sig.size != 8:
- sys.exit("PEDAL CHECKSUM is not 8 bits longs %s" % msg_name)
+ if sig.name == "COUNTER_PEDAL" and sig.size != 4:
+ sys.exit("%s: PEDAL COUNTER is not 4 bits long" % dbc_msg_name)
+ if sig.name == "CHECKSUM_PEDAL" and sig.size != 8:
+ sys.exit("%s: PEDAL CHECKSUM is not 8 bits long" % dbc_msg_name)
# Fail on duplicate message names
c = Counter([msg_name for address, msg_name, msg_size, sigs in msgs])
for name, count in c.items():
if count > 1:
- sys.exit("Duplicate message name in DBC file %s" % name)
+ sys.exit("%s: Duplicate message name in DBC file %s" % (dbc_name, name))
parser_code = template.render(dbc=can_dbc, checksum_type=checksum_type, msgs=msgs, def_vals=def_vals, len=len)
-
with open(out_fn, "w") as out_f:
out_f.write(parser_code)
diff --git a/selfdrive/car/lock_safety_model.py b/selfdrive/car/lock_safety_model.py
new file mode 100755
index 00000000000000..055d8c0c2b499d
--- /dev/null
+++ b/selfdrive/car/lock_safety_model.py
@@ -0,0 +1,27 @@
+#!/usr/bin/env python3
+import sys
+from cereal import car
+from common.params import Params
+
+# This script locks the safety model to a given value.
+# When the safety model is locked, boardd will preset panda to the locked safety model
+
+# run example:
+# ./lock_safety_model.py gm
+
+if __name__ == "__main__":
+
+ params = Params()
+
+ if len(sys.argv) < 2:
+ params.delete("SafetyModelLock")
+ print("Clear locked safety model")
+
+ else:
+ safety_model = getattr(car.CarParams.SafetyModel, sys.argv[1])
+ if type(safety_model) != int:
+ raise Exception("Invalid safety model: " + sys.argv[1])
+ if safety_model == car.CarParams.SafetyModel.allOutput:
+ raise Exception("Locking the safety model to allOutput is not allowed")
+ params.put("SafetyModelLock", str(safety_model))
+ print("Locked safety model: " + sys.argv[1])
diff --git a/selfdrive/car/modules/ALCA_module.py b/selfdrive/car/modules/ALCA_module.py
index d12f8f49c5b4f3..b686126cc7ec2d 100644
--- a/selfdrive/car/modules/ALCA_module.py
+++ b/selfdrive/car/modules/ALCA_module.py
@@ -31,6 +31,7 @@
HISTORY
-------
+v4.1 - OP 0.6.5 operating model
v4.0 - integrated into model_parser.py
v3.6 - moved parameters to carstate.py
v3.5 - changing the start angle to keep turning until we reach MAX_ANGLE_DELTA
@@ -56,73 +57,90 @@
WAIT_TIME_AFTER_TURN = 2.0
#ALCA
-ALCA_line_check_low_limit = 0.25
-ALCA_line_check_high_limit = 0.75
ALCA_line_min_prob = 0.01
ALCA_release_distance = 0.3
-ALCA_line_prob_low = 0.1
+ALCA_line_prob_low = 0.2
ALCA_line_prob_high = 0.4
-ALCA_zero_line = 2.5 #meters in front where to check for distance vs line
+ALCA_distance_jump = 1.1
+ALCA_lane_change_coefficient = 0.8
+ITERATIONS_AHEAD_TO_ESTIMATE = 4
+ALCA_duration_seconds = 5.
+ALCA_right_lane_multiplier = 1.1
-ALCA_DEBUG = True
+ALCA_DEBUG = False
DEBUG_INFO = "step {step} of {total_steps}: direction = {ALCA_direction} | using visual = {ALCA_use_visual} | over line = {ALCA_over_line} | lane width = {ALCA_lane_width} | left to move = {left_to_move} | from center = {from_center} | C2 offset = {ALCA_OFFSET_C2} | C1 offset = {ALCA_OFFSET_C1} | Prob Low = {prob_low} | Prob High = {prob_high}"
+
class ALCAController():
def __init__(self,carcontroller,alcaEnabled,steerByAngle):
#import settings
+ self.frame = 0
self.CC = carcontroller # added to start, will see if we need it actually
# variables for lane change
self.angle_offset = 0. #added when one needs to compensate for missalignment
self.alcaEnabled = alcaEnabled
- self.alca_duration = [2., 3.5, 5.]
- self.laneChange_strStartFactor = 2.
- self.laneChange_strStartMultiplier = 1.5
- self.laneChange_steerByAngle = steerByAngle # steer only by angle; do not call PID
- self.laneChange_last_actuator_angle = 0.
- self.laneChange_last_actuator_delta = 0.
- self.laneChange_last_sent_angle = 0.
self.laneChange_over_the_line = 0 # did we cross the line?
- self.laneChange_avg_angle = 0. # used if we do average entry angle over x frames
- self.laneChange_avg_count = 0. # used if we do average entry angle over x frames
self.laneChange_enabled = 1 # set to zero for no lane change
self.laneChange_counter = 0 # used to count frames during lane change
- self.laneChange_min_duration = 2. # min time to wait before looking for next lane
- self.laneChange_duration = 5.6 # how many max seconds to actually do the move; if lane not found after this then send error
- self.laneChange_after_lane_duration_mult = 1. # multiplier for time after we cross the line before we let OP take over; multiplied with CL_TIMEA_T
self.laneChange_wait = 1 # how many seconds to wait before it starts the change
- self.laneChange_lw = 3.7 # lane width in meters
- self.laneChange_angle = 0. # saves the last angle from actuators before lane change starts
- self.laneChange_angled = 0. # angle delta
- self.laneChange_steerr = 15.75 # steer ratio for lane change starting with the Tesla one
self.laneChange_direction = 0 # direction of the lane change
self.prev_right_blinker_on = False # local variable for prev position
self.prev_left_blinker_on = False # local variable for prev position
- self.keep_angle = False #local variable to keep certain angle delta vs. actuator
- self.last10delta = []
self.laneChange_cancelled = False
self.laneChange_cancelled_counter = 0
- self.last_time_enabled = 0
+ self.alcaStatusSocket = messaging.pub_sock(service_list['alcaStatus'].port)
+
+ def debug_alca(self,message):
+ if ALCA_DEBUG:
+ print (message)
+
+ def send_status(self,CS):
+ CS.ALCA_enabled = (self.laneChange_enabled > 1) and self.alcaEnabled
+ CS.ALCA_total_steps = int(20 * ALCA_duration_seconds)
+ if self.laneChange_enabled == 3:
+ CS.ALCA_direction = -self.laneChange_direction
+ else:
+ CS.ALCA_direction = 0
+ if not (self.frame % 20 == 0):
+ return
+ alca_status = tesla.ALCAStatus.new_message()
+ alca_status.alcaEnabled = bool(CS.ALCA_enabled)
+ alca_status.alcaTotalSteps = int(CS.ALCA_total_steps)
+ alca_status.alcaDirection = int(CS.ALCA_direction)
+ alca_status.alcaError = bool(CS.ALCA_error)
+ self.alcaStatusSocket.send(alca_status.to_bytes())
def update_status(self,alcaEnabled):
self.alcaEnabled = alcaEnabled
- def stop_ALCA(self, CS):
+ def stop_ALCA(self, CS, isDone):
# something is not right; ALCAModelParser is not engaged; cancel
- CS.UE.custom_alert_message(3,"Auto Lane Change Canceled! (d)",200,5)
- self.laneChange_cancelled = True
- self.laneChange_cancelled_counter = 200
+ self.debug_alca("ALCA canceled: stop_ALCA called")
+ if not isDone:
+ CS.UE.custom_alert_message(3,"Auto Lane Change Canceled! (d)",200,5)
+ self.laneChange_cancelled = True
+ self.laneChange_cancelled_counter = 200
+ else:
+ self.laneChange_cancelled = False
+ self.laneChange_cancelled_counter = 0
self.laneChange_enabled = 1
self.laneChange_counter = 0
self.laneChange_direction = 0
CS.cstm_btns.set_button_status("alca",1)
+ self.send_status(CS)
- def update(self,enabled,CS,actuators):
+ def update(self,enabled,CS,actuators,alcaStateData,frame):
cl_min_v = CS.CL_MIN_V
cl_max_a = CS.CL_MAX_A
- alca_mode = CS.cstm_btns.get_button_label2_index("alca")
+ self.frame = frame
+
+ if (alcaStateData is not None) and (((self.laneChange_direction != 0) and alcaStateData.alcaError) or (alcaStateData.alcaDirection == 100)):
+ self.debug_alca("ALCA canceled: stop_ALCA called (1)")
+ self.stop_ALCA(CS,alcaStateData.alcaDirection == 100)
+ return 0, False
if self.laneChange_cancelled_counter > 0:
self.laneChange_cancelled_counter -= 1
@@ -131,11 +149,11 @@ def update(self,enabled,CS,actuators):
# Basic highway lane change logic
actuator_delta = 0.
- laneChange_angle = 0.
turn_signal_needed = 0 # send 1 for left, 2 for right 0 for not needed
if (not CS.right_blinker_on) and (not CS.left_blinker_on) and \
(self.laneChange_enabled == 4):
+ self.debug_alca("ALCA reset: resetting 4 -> 1")
self.laneChange_enabled =1
self.laneChange_counter =0
self.laneChange_direction =0
@@ -158,49 +176,48 @@ def update(self,enabled,CS,actuators):
else:
CS.cstm_btns.set_button_status("alca",1)
- if self.alcaEnabled and enabled and (((not self.prev_right_blinker_on) and CS.right_blinker_on) or \
- ((not self.prev_left_blinker_on) and CS.left_blinker_on)) and \
+ if self.alcaEnabled and enabled and (self.laneChange_enabled > 1) and \
((CS.v_ego < cl_min_v) or (abs(actuators.steerAngle) >= cl_max_a) or (abs(CS.angle_steers) >=cl_max_a)):
# something is not right, the speed or angle is limitting
- CS.UE.custom_alert_message(3,"Auto Lane Change Unavailable!",500,3)
+ self.debug_alca("ALCA Unavailable (2)")
+ CS.UE.custom_alert_message(3,"Auto Lane Change Unavailable!",200,3)
CS.cstm_btns.set_button_status("alca",9)
-
+ self.stop_ALCA(CS, False)
+ return 0, False
if self.alcaEnabled and enabled and (((not self.prev_right_blinker_on) and CS.right_blinker_on) or \
((not self.prev_left_blinker_on) and CS.left_blinker_on)) and \
- (CS.v_ego >= cl_min_v) and (abs(actuators.steerAngle) < cl_max_a):
+ (CS.v_ego >= cl_min_v) and (abs(actuators.steerAngle) < cl_max_a) and (self.laneChange_enabled == 1):
# start blinker, speed and angle is within limits, let's go
laneChange_direction = 1
# changing lanes
if CS.left_blinker_on:
laneChange_direction = -1
+ self.debug_alca("ALCA blinker on detected")
- if (self.laneChange_enabled > 1) and (self.laneChange_direction != laneChange_direction):
- # something is not right; signal in oposite direction; cancel
- CS.UE.custom_alert_message(3,"Auto Lane Change Canceled! (s)",200,5)
- self.laneChange_cancelled = True
- self.laneChange_cancelled_counter = 200
- self.laneChange_enabled = 1
- self.laneChange_counter = 0
- self.laneChange_direction = 0
- CS.cstm_btns.set_button_status("alca",1)
- elif (self.laneChange_enabled == 1) :
- # compute angle delta for lane change
- CS.UE.custom_alert_message(2,"Auto Lane Change Engaged!",100)
- self.laneChange_enabled = 2
- self.laneChange_counter = 1
- self.laneChange_direction = laneChange_direction
- CS.cstm_btns.set_button_status("alca",2)
+ CS.UE.custom_alert_message(2,"Auto Lane Change Engaged!",100)
+ self.debug_alca("ALCA engaged")
+ self.laneChange_enabled = 2
+ self.laneChange_counter = 1
+ self.laneChange_direction = laneChange_direction
+ CS.cstm_btns.set_button_status("alca",2)
+
+ self.prev_right_blinker_on = CS.right_blinker_on
+ self.prev_left_blinker_on = CS.left_blinker_on
if (not self.alcaEnabled) and self.laneChange_enabled > 1:
+ self.debug_alca("ALCA canceled: not enabled")
self.laneChange_enabled = 1
self.laneChange_counter = 0
self.laneChange_direction = 0
+ self.stop_ALCA(CS, False)
+ return 0, False
# lane change in progress
if self.laneChange_enabled > 1:
if (CS.steer_override or (CS.v_ego < cl_min_v)):
CS.UE.custom_alert_message(4,"Auto Lane Change Canceled! (u)",200,3)
+ self.debug_alca("ALCA canceled: steer override")
self.laneChange_cancelled = True
self.laneChange_cancelled_counter = 200
# if any steer override cancel process or if speed less than min speed
@@ -208,33 +225,34 @@ def update(self,enabled,CS,actuators):
self.laneChange_enabled = 1
self.laneChange_direction = 0
CS.cstm_btns.set_button_status("alca",1)
+ self.stop_ALCA(CS, False)
+ return 0, False
if self.laneChange_enabled == 2:
if self.laneChange_counter == 1:
CS.UE.custom_alert_message(2,"Auto Lane Change Engaged! (1)",self.laneChange_wait * 100)
self.laneChange_counter += 1
+ self.debug_alca("ALCA phase 2: " + str(self.laneChange_counter))
if self.laneChange_counter == self.laneChange_wait * 100:
self.laneChange_enabled = 3
self.laneChange_counter = 0
if self.laneChange_enabled ==3:
if self.laneChange_counter == 1:
- CS.UE.custom_alert_message(2,"Auto Lane Change Engaged! (2)",int(self.alca_duration[alca_mode] * 100))
+ CS.UE.custom_alert_message(2,"Auto Lane Change Engaged! (2)",int(ALCA_duration_seconds * 100))
self.laneChange_counter += 1
- if self.laneChange_counter >= self.alca_duration[alca_mode] * 100:
+ self.debug_alca("ALCA phase 3: " + str(self.laneChange_counter))
+ if self.laneChange_counter >= ALCA_duration_seconds * 100.:
self.laneChange_enabled = 4
self.laneChange_counter = 0
if self.laneChange_enabled == 4:
+ self.debug_alca("ALCA phase 4: " +str(self.laneChange_counter))
if self.laneChange_counter == 1:
CS.UE.custom_alert_message(2,"Auto Lane Change Complete!",100)
self.laneChange_enabled = 1
self.laneChange_counter = 0
+ self.stop_ALCA(CS, True)
+ return 0, False
- CS.ALCA_enabled = (self.laneChange_enabled > 1) and self.alcaEnabled
- CS.ALCA_total_steps = int(20 * self.alca_duration[alca_mode])
- if self.laneChange_enabled == 3:
- CS.ALCA_direction = -self.laneChange_direction
- else:
- CS.ALCA_direction = 0
-
+ self.send_status(CS)
return turn_signal_needed, self.laneChange_enabled > 1
class ALCAModelParser():
@@ -242,9 +260,9 @@ def __init__(self):
#ALCA params
self.ALCA_error = False
self.ALCA_lane_width = 3.6
- self.ALCA_direction = 0 # left 1, right -1
+ self.ALCA_direction = 100 #none 0, left 1, right -1,reset 100
self.ALCA_step = 0
- self.ALCA_total_steps = 20 * 5 #20 Hz, 5 seconds, wifey mode
+ self.ALCA_total_steps = 20 * ALCA_duration_seconds #20 Hz, 5 seconds, wifey mode
self.ALCA_cancelling = False
self.ALCA_enabled = False
self.ALCA_OFFSET_C3 = 0.
@@ -267,18 +285,18 @@ def __init__(self):
self.prev_distance_to_line_R = 100.
- def reset_alca (self):
+ def reset_alca (self,v_ego):
self.ALCA_step = 0
- self.ALCA_direction = 0
+ self.ALCA_direction = 100
self.ALCA_cancelling = False
- self.ALCA_error = True
+ self.ALCA_error = False
self.ALCA_enabled = False
self.ALCA_OFFSET_C3 = 0.
self.ALCA_OFFSET_C2 = 0.
self.ALCA_OFFSET_C1 = 0.
self.ALCA_over_line = False
self.ALCA_use_visual = True
- self.ALCA_vego_prev = 0.
+ self.ALCA_vego_prev = v_ego
self.alcas = None
self.hit_prob_low = False
self.hit_prob_high = False
@@ -286,6 +304,7 @@ def reset_alca (self):
self.prev_distance_to_line_L = 100.
self.distance_to_line_R = 100.
self.prev_distance_to_line_R = 100.
+ self.send_state()
def debug_alca(self,message):
if ALCA_DEBUG:
@@ -309,126 +328,141 @@ def update(self, v_ego, md, r_poly, l_poly, r_prob, l_prob, lane_width, p_poly):
if socket is self.alcaStatus:
self.alcas = tesla.ALCAStatus.from_bytes(socket.recv())
-
#if we don't have yet ALCA status, return same values
if self.alcas is None:
self.send_state()
return np.array(r_poly),np.array(l_poly),r_prob, l_prob, lane_width, p_poly
-
- self.ALCA_direction = self.alcas.alcaDirection
- self.ALCA_enabled = self.alcas.alcaEnabled
- self.ALCA_total_steps = self.alcas.alcaTotalSteps
- self.ALCA_error = self.ALCA_error or (self.alcas.alcaError and not self.prev_CS_ALCA_error)
- self.prev_CS_ALCA_error = self.alcas.alcaError
+ if self.alcas.alcaDirection == 0:
+ self.ALCA_direction = 0
- if not self.ALCA_enabled:
- self.send_state()
- return np.array(r_poly),np.array(l_poly),r_prob, l_prob, lane_width, p_poly
+ if self.ALCA_direction < 100:
+ self.ALCA_direction = self.alcas.alcaDirection
+
+ self.ALCA_enabled = self.alcas.alcaEnabled
+
+ self.ALCA_total_steps = self.alcas.alcaTotalSteps
+ self.ALCA_error = self.ALCA_error or (self.alcas.alcaError and not self.prev_CS_ALCA_error)
+ self.prev_CS_ALCA_error = self.alcas.alcaError
+
+ if self.ALCA_enabled:
+ if self.ALCA_direction == 0:
+ self.ALCA_lane_width = lane_width
+ else:
+ lane_width = self.ALCA_lane_width
#if error but no direction, the carcontroller component is fine and we need to reset
if self.ALCA_error and (self.ALCA_direction == 0):
self.ALCA_error = False
- self.hit_prob_low = (self.ALCA_direction != 0) and (self.hit_prob_low or ((self.ALCA_direction == 1) and (l_prob < ALCA_line_prob_low)) or ((self.ALCA_direction == -1) and (r_prob < ALCA_line_prob_low)))
+ if (not self.ALCA_enabled) or (self.ALCA_direction == 100) or (self.ALCA_direction == 0):
+ self.ALCA_vego_prev = v_ego
+ self.send_state()
+ return np.array(r_poly),np.array(l_poly),r_prob, l_prob, lane_width,np.array(p_poly)
+
+ self.hit_prob_low = self.hit_prob_low and ((self.ALCA_direction != 0) and (self.hit_prob_low or ((self.ALCA_direction == 1) and (l_prob < ALCA_line_prob_low)) or ((self.ALCA_direction == -1) and (r_prob < ALCA_line_prob_low))))
self.hit_prob_high = (self.ALCA_direction != 0) and (self.hit_prob_low) and (self.hit_prob_high or ((self.ALCA_direction == 1) and (r_prob > ALCA_line_prob_high)) or ((self.ALCA_direction == -1) and (l_prob < ALCA_line_prob_high)))
if self.hit_prob_high:
self.debug_alca("Hit high probability for line, ALCA done, releasing...")
- self.reset_alca()
+ self.reset_alca(v_ego)
+ return np.array(r_poly),np.array(l_poly),r_prob, l_prob, lane_width,np.array(p_poly)
#where are we in alca as %
ALCA_perc_complete = float(self.ALCA_step) / float(self.ALCA_total_steps)
if self.ALCA_error and self.ALCA_cancelling:
self.debug_alca(" Error and Cancelling -> resetting...")
- self.reset_alca()
+ self.reset_alca(v_ego)
+ return np.array(r_poly),np.array(l_poly),r_prob, l_prob, lane_width,np.array(p_poly)
if self.ALCA_error and not self.ALCA_cancelling:
if (ALCA_perc_complete < 0.1) or (ALCA_perc_complete > 0.9):
self.debug_alca(" Error and less than 10% -> resetting...")
- self.reset_alca()
+ self.reset_alca(v_ego)
+ return np.array(r_poly),np.array(l_poly),r_prob, l_prob, lane_width,np.array(p_poly)
else:
self.debug_alca(" Error and not Cancelling -> rewinding...")
self.ALCA_cancelling = True
self.ALCA_error = False
- if self.ALCA_enabled and not (self.ALCA_direction == 0):
- self.ALCA_step += 1 #ALCA_increment
- if (self.ALCA_step < 0) or (self.ALCA_step >= self.ALCA_total_steps):
- #done so end ALCA
- self.debug_alca(" step out of bounds -> resetting...")
- self.reset_alca()
-
- #compute offset
- from_center = 0.
- left_to_move = 0.
- if self.ALCA_enabled and not (self.ALCA_direction == 0):
- #compute distances to lines
- self.distance_to_line_L = abs(np.polyval(l_poly,ALCA_zero_line))
- self.distance_to_line_R = abs(np.polyval(r_poly,ALCA_zero_line))
- if ((self.ALCA_direction == 1) and ((self.distance_to_line_L > 1.1 * self.prev_distance_to_line_L) or (self.distance_to_line_R < self.ALCA_lane_width / 3.))) or \
- ((self.ALCA_direction == -1) and ((self.distance_to_line_R > 1.1 * self.prev_distance_to_line_R) or (self.distance_to_line_L < self.ALCA_lane_width / 3.))):
- self.ALCA_over_line = True
- left_to_move = self.ALCA_lane_width / 5.
- if self.ALCA_over_line:
- left_to_move = self.ALCA_lane_width / 2. - (self.distance_to_line_L if self.ALCA_direction == 1 else self.distance_to_line_R)
- if left_to_move < ALCA_release_distance:
- self.reset_alca()
- self.send_state()
- return np.array(r_poly),np.array(l_poly),r_prob, l_prob, lane_width, p_poly
- distance_left = float((self.ALCA_total_steps) * 0.05 * (self.ALCA_vego_prev + v_ego) / 2.) #5m + distance left
- d1 = np.polyval(p_poly,distance_left)
- d2 = np.polyval(p_poly,distance_left + 1)
- cos = 1.
- if abs(d2 - d1) > 0.1:
- cos = abs(np.cos(np.arctan(1/abs(d2-d1))))
- ltm = cos * left_to_move
- #compute offsets
- self.ALCA_OFFSET_C1 = 0.
- self.ALCA_OFFSET_C2 = float(self.ALCA_direction * ltm) / (distance_left )
- self.prev_distance_to_line_R = self.distance_to_line_R
- self.prev_distance_to_line_L = self.distance_to_line_L
- if ALCA_DEBUG:
- debug_string = DEBUG_INFO.format(step=self.ALCA_step,total_steps=self.ALCA_total_steps,ALCA_direction=self.ALCA_direction,ALCA_use_visual=self.ALCA_use_visual,ALCA_over_line=self.ALCA_over_line,ALCA_lane_width=self.ALCA_lane_width, left_to_move=left_to_move, from_center=from_center, ALCA_OFFSET_C2=self.ALCA_OFFSET_C2, ALCA_OFFSET_C1=self.ALCA_OFFSET_C1,prob_low=self.hit_prob_low,prob_high=self.hit_prob_high)
- self.debug_alca(debug_string)
- else:
- self.ALCA_OFFSET_C2 = 0.
- self.ALCA_OFFSET_C1 = 0.
-
-
- if (not self.ALCA_error) and self.ALCA_use_visual:
- if self.ALCA_over_line:
- if (self.ALCA_total_steps - self.ALCA_step <= 1) or (self.ALCA_over_line and ((self.ALCA_direction == 1) and (r_poly[3] < -ALCA_release_distance)) or ((self.ALCA_direction == -1) and (l_poly[3] > ALCA_release_distance))):
- self.reset_alca()
- self.ALCA_error = False
-
- if l_prob > r_prob:
- r_poly = np.array(l_poly)
- if l_prob > ALCA_line_prob_low:
- l_prob = 1
- r_prob = l_prob
- else:
- l_poly = np.array(r_poly)
- if r_prob > ALCA_line_prob_low:
- r_prob = 1
- l_prob = r_prob
- l_poly[3] = self.ALCA_lane_width / 2
- r_poly[3] = -self.ALCA_lane_width / 2
- l_poly[2] += self.ALCA_OFFSET_C2
- r_poly[2] += self.ALCA_OFFSET_C2
- l_poly[1] += self.ALCA_OFFSET_C1
- r_poly[1] += self.ALCA_OFFSET_C1
+ self.ALCA_step += 1 #ALCA_increment
+
+
+ if (self.ALCA_step < 0) or (self.ALCA_step >= self.ALCA_total_steps):
+ #done so end ALCA
+ self.debug_alca(" step out of bounds -> resetting...")
+ self.reset_alca(v_ego)
+ return np.array(r_poly),np.array(l_poly),r_prob, l_prob, lane_width,np.array(p_poly)
+
+ #compute offset
+ from_center = 0.
+ left_to_move = 0.
+
+ #compute distances to lines
+ self.distance_to_line_L = abs(l_poly[3])
+ self.distance_to_line_R = abs(r_poly[3])
+ distance_left = float((self.ALCA_total_steps) * 0.05 * (self.ALCA_vego_prev + v_ego) / 2.) #5m + distance left
+ distance_estimate = float(ITERATIONS_AHEAD_TO_ESTIMATE * 0.05 * (self.ALCA_vego_prev + v_ego) / 2.)
+ estimate_curv_at = distance_estimate / distance_left
+ left_to_move = self.ALCA_lane_width * estimate_curv_at
+ # if ((self.ALCA_direction == 1) and ((self.distance_to_line_L > ALCA_distance_jump * self.prev_distance_to_line_L) or (self.distance_to_line_R < self.ALCA_lane_width / 3.))) or \
+ # ((self.ALCA_direction == -1) and ((self.distance_to_line_R > ALCA_distance_jump * self.prev_distance_to_line_R) or (self.distance_to_line_L < self.ALCA_lane_width / 3.))):
+ # self.ALCA_over_line = True
+ if ((self.ALCA_direction == 1) and (self.distance_to_line_R < self.ALCA_lane_width / 3.)) or \
+ ((self.ALCA_direction == -1) and (self.distance_to_line_L < self.ALCA_lane_width / 3.)):
+ self.ALCA_over_line = True
+ left_to_move = self.ALCA_lane_width * estimate_curv_at
+ if self.ALCA_over_line:
+ left_to_move2 = (self.distance_to_line_L if self.ALCA_direction == 1 else self.distance_to_line_R) - self.ALCA_lane_width / 2.
+ if left_to_move2 < ALCA_release_distance:
+ self.reset_alca(v_ego)
+ return np.array(r_poly),np.array(l_poly),r_prob, l_prob, lane_width, p_poly
+
+ d1 = np.polyval(p_poly,distance_estimate -1)
+ d2 = np.polyval(p_poly,distance_estimate + 1)
+ cos = 0.
+ if abs(d2 - d1) > 0.001:
+ #cos = abs(np.cos(np.arctan(2/abs(d2-d1))))
+ cos = np.cos(np.arctan(2/(d2-d1)))
+ d0 = (d2 + d1) / 2.0 - np.polyval(p_poly,distance_estimate)
+ ltm = left_to_move # * (1 - cos * self.ALCA_direction)
+ #compute offsets
+ self.ALCA_OFFSET_C1 = 0.
+ lane_multiplier = 1. if self.ALCA_direction == 1 else ALCA_right_lane_multiplier
+ self.ALCA_OFFSET_C2 = lane_multiplier * ALCA_lane_change_coefficient * float(self.ALCA_direction * ltm) / (distance_estimate )
+ self.prev_distance_to_line_R = self.distance_to_line_R
+ self.prev_distance_to_line_L = self.distance_to_line_L
+ if ALCA_DEBUG:
+ debug_string = DEBUG_INFO.format(step=self.ALCA_step,total_steps=self.ALCA_total_steps,ALCA_direction=self.ALCA_direction,ALCA_use_visual=self.ALCA_use_visual,ALCA_over_line=self.ALCA_over_line,ALCA_lane_width=self.ALCA_lane_width, left_to_move=left_to_move/estimate_curv_at, from_center=from_center, ALCA_OFFSET_C2=self.ALCA_OFFSET_C2, ALCA_OFFSET_C1=self.ALCA_OFFSET_C1,prob_low=self.hit_prob_low,prob_high=self.hit_prob_high)
+ self.debug_alca(debug_string)
+
+ if (not self.ALCA_error) and self.ALCA_use_visual:
+ if self.ALCA_over_line:
+ if (self.ALCA_total_steps - self.ALCA_step <= 1) or (self.ALCA_over_line and ((self.ALCA_direction == 1) and (r_poly[3] < -ALCA_release_distance)) or ((self.ALCA_direction == -1) and (l_poly[3] > ALCA_release_distance))):
+ self.ALCA_error = False
+ self.reset_alca(v_ego)
+ return np.array(r_poly),np.array(l_poly),r_prob, l_prob, lane_width, p_poly
+
+ if l_prob > r_prob:
+ r_poly = np.array(l_poly)
+ if l_prob > ALCA_line_prob_low:
+ l_prob = 1
+ r_prob = l_prob
else:
- self.reset_alca()
- self.ALCA_error = False
+ l_poly = np.array(r_poly)
+ if r_prob > ALCA_line_prob_low:
+ r_prob = 1
+ l_prob = r_prob
+ l_poly[3] = self.ALCA_lane_width / 2
+ r_poly[3] = -self.ALCA_lane_width / 2
+ l_poly[2] += self.ALCA_OFFSET_C2
+ r_poly[2] += self.ALCA_OFFSET_C2
+ l_poly[1] += self.ALCA_OFFSET_C1
+ r_poly[1] += self.ALCA_OFFSET_C1
+ p_poly[3] = 0
+ p_poly[2] += self.ALCA_OFFSET_C2
+ p_poly[1] += self.ALCA_OFFSET_C1
self.ALCA_vego_prev = v_ego
-
- if self.ALCA_enabled:
- if self.ALCA_direction == 0:
- self.ALCA_lane_width = lane_width
- else:
- lane_width = self.ALCA_lane_width
-
self.send_state()
- return np.array(r_poly),np.array(l_poly),r_prob, l_prob, self.ALCA_lane_width, p_poly
+ return np.array(r_poly),np.array(l_poly),r_prob, l_prob, self.ALCA_lane_width, np.array(p_poly)
diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py
index 0fa12cf7d9bb89..e2195b00a19ca1 100644
--- a/selfdrive/car/subaru/interface.py
+++ b/selfdrive/car/subaru/interface.py
@@ -3,9 +3,9 @@
from selfdrive.config import Conversions as CV
from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET
from selfdrive.controls.lib.vehicle_model import VehicleModel
-from selfdrive.car.subaru.values import CAR, FINGERPRINTS, ECU_FINGERPRINT, ECU
+from selfdrive.car.subaru.values import CAR
from selfdrive.car.subaru.carstate import CarState, get_powertrain_can_parser, get_camera_can_parser
-from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, is_ecu_disconnected
+from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint
from selfdrive.car.interfaces import CarInterfaceBase
ButtonType = car.CarState.ButtonEvent.Type
@@ -48,7 +48,9 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay
ret.enableCruise = True
ret.steerLimitAlert = True
- ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, ECU.CAM) or has_relay
+ # force openpilot to fake the stock camera, since car harness is not supported yet and old style giraffe (with switches)
+ # was never released
+ ret.enableCamera = True
ret.steerRateCost = 0.7
diff --git a/selfdrive/car/tesla/ACC_module.py b/selfdrive/car/tesla/ACC_module.py
index 4445a5173fb4fa..3b76968bf00052 100644
--- a/selfdrive/car/tesla/ACC_module.py
+++ b/selfdrive/car/tesla/ACC_module.py
@@ -357,7 +357,7 @@ def _calc_follow_button(self, CS, lead_car,speed_limit_kph, speed_limit_valid, s
print ("Ratio: {0:.1f}% lead: {1:.1f}m avail: {2:.1f}kph vRel: {3:.1f}kph Angle: {4:.1f}deg".format(
ratio, lead_dist_m, available_speed_kph, lead_car.vRel * CV.MS_TO_KPH, CS.angle_steers))
self.last_update_time = current_time_ms
- if msg != None:
+ if msg is not None:
print ("ACC: " + msg)
return button
@@ -371,7 +371,7 @@ def _should_autoengage_cc(self, CS, lead_car=None):
and CS.v_ego >= self.MIN_CRUISE_SPEED_MS
and _current_time_millis() > self.fast_decel_time + 2000)
- slow_lead = lead_car and lead_car.dRel > 0 and lead_car.vRel < 0 or self._fast_decel_required(CS, lead_car)
+ slow_lead = lead_car and lead_car.dRel > 0 and lead_car.vRel < 0 or self._fast_decel_required(CS, lead_car) # pylint: disable=chained-comparison
# "Autoresume" mode allows cruise to engage even after brake events, but
# shouldn't trigger DURING braking.
diff --git a/selfdrive/car/tesla/HSO_module.py b/selfdrive/car/tesla/HSO_module.py
index fba539b5f2ef4d..daa20935112c59 100644
--- a/selfdrive/car/tesla/HSO_module.py
+++ b/selfdrive/car/tesla/HSO_module.py
@@ -1,7 +1,6 @@
#human steer override module
import time
-MAX_TIME_BLINKER_ON = 150 # in 0.01 seconds
TIME_REMEMBER_LAST_BLINKER = 50 # in 0.01 seconds
def _current_time_millis():
@@ -9,7 +8,6 @@ def _current_time_millis():
class HSOController():
def __init__(self,carcontroller):
- self.CC = carcontroller
self.human_control = False
self.frame_humanSteered = 0
self.turn_signal_needed = 0 # send 1 for left, 2 for right 0 for not needed
@@ -18,6 +16,7 @@ def __init__(self,carcontroller):
self.frame_blinker_on = 0
self.last_human_blinker_on = 0
self.frame_human_blinker_on = 0
+ self.HSO_frame_blinker_on = 0
def update_stat(self,CC,CS,enabled,actuators,frame):
@@ -29,6 +28,10 @@ def update_stat(self,CC,CS,enabled,actuators,frame):
elif CS.left_blinker_on:
self.blinker_on = 1
self.frame_human_blinker_on = frame
+ if (self.last_blinker_on == 0) and (self.blinker_on > 0) and (CC.ALCA.laneChange_enabled <= 1):
+ self.HSO_frame_blinker_on = frame
+ if (self.last_blinker_on == 0) and (self.blinker_on == 0):
+ self.HSO_frame_blinker_on = 0
if self.blinker_on > 0:
self.frame_blinker_on = frame
self.last_human_blinker_on = self.blinker_on
@@ -37,24 +40,24 @@ def update_stat(self,CC,CS,enabled,actuators,frame):
self.blinker_on = self.last_human_blinker_on
else:
self.last_human_blinker_on = 0
- if frame - self.frame_human_blinker_on > MAX_TIME_BLINKER_ON:
+ if frame - self.frame_human_blinker_on > 100 * CS.hsoBlinkerExtender:
self.blinker_on = 0
self.turn_signal_needed = 0
self.last_blinker_on = 0
if CS.enableHSO and enabled:
#if steering but not by ALCA
- if (CS.right_blinker_on or CS.left_blinker_on) and (self.CC.ALCA.laneChange_enabled <= 1):# and (self.last_blinker_on != self.blinker_on):
+ if (CS.right_blinker_on or CS.left_blinker_on) and (CC.ALCA.laneChange_enabled <= 1):# and (self.last_blinker_on != self.blinker_on):
self.frame_humanSteered = frame
- if (CS.steer_override > 0) and (frame - self.frame_humanSteered > 50):
+ if (CS.steer_override > 0): # and (frame - self.frame_humanSteered > 50): #let's try with human touch only
self.frame_humanSteered = frame
else:
- if (frame - self.frame_humanSteered < 50): # Need more human testing of handoff timing
+ if (CC.ALCA.laneChange_enabled <= 1) and (frame - self.frame_humanSteered < 50): # Need more human testing of handoff timing
# Find steering difference between visiond model and human (no need to do every frame if we run out of CPU):
steer_current=(CS.angle_steers) # Formula to convert current steering angle to match apply_steer calculated number
apply_steer = int(-actuators.steerAngle)
angle = abs(apply_steer-steer_current)
- if angle > 15.:
+ if frame < (self.HSO_frame_blinker_on + int(100 * CS.hsoNumbPeriod)) or angle > 15.:
self.frame_humanSteered = frame
if enabled:
if CS.enableHSO:
@@ -74,7 +77,7 @@ def update_stat(self,CC,CS,enabled,actuators,frame):
CC.DAS_219_lcTempUnavailableSpeed = 0
CC.warningNeeded = 1
self.turn_signal_needed = 0
- if (self.CC.ALCA.laneChange_enabled > 1):
+ if (CC.ALCA.laneChange_enabled > 1):
self.turn_signal_needed = 0
self.blinker_on = 0
self.last_blinker_on = 0
diff --git a/selfdrive/car/tesla/PCC_module.py b/selfdrive/car/tesla/PCC_module.py
index aec40a03f25fcd..bf286b4ae250a8 100644
--- a/selfdrive/car/tesla/PCC_module.py
+++ b/selfdrive/car/tesla/PCC_module.py
@@ -15,6 +15,7 @@
from collections import OrderedDict
from common.params import Params
from selfdrive.car.tesla.movingaverage import MovingAverage
+import json
_DT = 0.05 # 10Hz in our case, since we don't want to process more than once the same radarState message
_DT_MPC = _DT
@@ -49,6 +50,8 @@
# Pull the cruise stalk twice in this many ms for a 'double pull'
STALK_DOUBLE_PULL_MS = 750
+V_PID_FILE = '/data/params/pidParams'
+
class Mode():
label = None
@@ -177,6 +180,32 @@ def __init__(self,carcontroller):
self.params = Params()
self.average_speed_over_x_suggestions = 3 #10x a second
self.maxsuggestedspeed_avg = MovingAverage(self.average_speed_over_x_suggestions)
+
+ def load_pid(self):
+ try:
+ v_pid_json = open(V_PID_FILE)
+ data = json.load(v_pid_json)
+ if (self.LoC):
+ if self.LoC.pid:
+ self.LoC.pid.p = data['p']
+ self.LoC.pid.i = data['i']
+ self.LoC.pid.f = data['f']
+ else:
+ print("self.LoC not initialized!")
+ except IOError:
+ print("file not present, creating at next reset")
+
+ #Helper function for saving the PCC pid constants across drives
+ def save_pid(self, pid):
+ data = {}
+ data['p'] = pid.p
+ data['i'] = pid.i
+ data['f'] = pid.f
+ try:
+ with open(V_PID_FILE , 'w') as outfile :
+ json.dump(data, outfile)
+ except IOError:
+ print("PDD pid parameters could not be saved to file")
def max_v_by_speed_limit(self,pedal_set_speed_ms ,speed_limit_ms, CS):
# if more than 10 kph / 2.78 ms, consider we have speed limit
@@ -195,12 +224,19 @@ def max_v_by_speed_limit(self,pedal_set_speed_ms ,speed_limit_ms, CS):
return pedal_set_speed_ms
def reset(self, v_pid):
+ #save the pid parameters to params file
+ self.save_pid(self.LoC.pid)
+
if self.LoC and RESET_PID_ON_DISENGAGE:
self.LoC.reset(v_pid)
def update_stat(self, CS, enabled):
if not self.LoC:
self.LoC = LongControl(CS.CP, tesla_compute_gb)
+ # Get v_id from the stored file when initiating the LoC and reset_on_disengage==false
+ if (not RESET_PID_ON_DISENGAGE):
+ self.load_pid()
+
can_sends = []
if CS.pedal_interceptor_available and not CS.cstm_btns.get_button_status("pedal"):
@@ -252,7 +288,7 @@ def update_stat(self, CS, enabled):
if ready and double_pull:
# A double pull enables ACC. updating the max ACC speed if necessary.
self.enable_pedal_cruise = True
- self.LoC.reset(CS.v_ego)
+ self.reset(CS.v_ego)
# Increase PCC speed to match current, if applicable.
self.pedal_speed_kph = max(CS.v_ego * CV.MS_TO_KPH, self.speed_limit_kph)
# Handle pressing the cancel button.
@@ -379,6 +415,7 @@ def update_pdl(self, enabled, CS, frame, actuators, pcm_speed, speed_limit_ms, s
####################################################################
if PCCModes.is_selected(FollowMode(), CS.cstm_btns):
self.v_pid = self.calc_follow_speed_ms(CS,alca_enabled)
+
if mapd is not None:
v_curve = max_v_in_mapped_curve_ms(mapd.liveMapData, self.pedal_speed_kph)
if v_curve:
@@ -394,7 +431,7 @@ def update_pdl(self, enabled, CS, frame, actuators, pcm_speed, speed_limit_ms, s
self.pedal_state = CS.pedal_interceptor_value > 10
#reset PID if we just lifted foot of accelerator
if (not self.pedal_state) and self.prev_pedal_state:
- self.LoC.reset(v_pid=CS.v_ego)
+ self.reset(v_pid=CS.v_ego)
if self.enable_pedal_cruise and (not self.pedal_state):
jerk_min, jerk_max = _jerk_limits(CS.v_ego, self.lead_1, self.v_pid * CV.MS_TO_KPH, self.lead_last_seen_time_ms, CS)
@@ -432,7 +469,7 @@ def update_pdl(self, enabled, CS, frame, actuators, pcm_speed, speed_limit_ms, s
output_gb = t_go - t_brake
#print ("Output GB Follow:", output_gb)
else:
- self.LoC.reset(v_pid=CS.v_ego)
+ self.reset(v_pid=CS.v_ego)
#print ("PID reset")
output_gb = 0.
starting = self.LoC.long_control_state == LongCtrlState.starting
@@ -534,7 +571,7 @@ def calc_follow_speed_ms(self, CS, alca_enabled):
elif lead_dist_m < MIN_SAFE_DIST_M:
new_speed_kph = MIN_PCC_V_KPH
# In a 10 meter cruise zone, lets match the car in front
- elif lead_dist_m > MIN_SAFE_DIST_M and lead_dist_m < safe_dist_m + 2: # BB we might want to try this and rel_speed_kph > 0:
+ elif safe_dist_m + 2 > lead_dist_m > MIN_SAFE_DIST_M: # BB we might want to try this and rel_speed_kph > 0:
min_vrel_kph_map = OrderedDict([
# (distance in m, min allowed relative kph)
(0.5 * safe_dist_m, 3.0),
@@ -592,7 +629,7 @@ def calc_follow_speed_ms(self, CS, alca_enabled):
# Don't accelerate during manual turns, curves or ALCA.
new_speed_kph = min(new_speed_kph, self.last_speed_kph)
#BB Last safety check. Zero if below MIN_SAFE_DIST_M
- if (lead_dist_m > 0) and (lead_dist_m < MIN_SAFE_DIST_M) and (rel_speed_kph < 3.):
+ if (MIN_SAFE_DIST_M > lead_dist_m > 0) and (rel_speed_kph < 3.):
new_speed_kph = MIN_PCC_V_KPH
self.last_speed_kph = new_speed_kph
return new_speed_kph * CV.KPH_TO_MS
diff --git a/selfdrive/car/tesla/carcontroller.py b/selfdrive/car/tesla/carcontroller.py
index e8b63a5b9b6e03..c12d70646e1091 100644
--- a/selfdrive/car/tesla/carcontroller.py
+++ b/selfdrive/car/tesla/carcontroller.py
@@ -4,6 +4,7 @@
from collections import namedtuple
from common.numpy_fast import clip, interp
from common.realtime import DT_CTRL
+from common import realtime
from selfdrive.car.tesla import teslacan
from selfdrive.car.tesla.values import AH, CM
from selfdrive.can.packer import CANPacker
@@ -25,10 +26,6 @@
ANGLE_DELTA_BP = [0., 5., 15.]
ANGLE_DELTA_V = [5., .8, .25] # windup limit
ANGLE_DELTA_VU = [5., 3.5, 0.8] # unwind limit
-#steering adjustment with speed
-DES_ANGLE_ADJUST_FACTOR_BP = [0.,13., 44.]
-DES_ANGLE_ADJUST_FACTOR = [1.0, 1.0, 1.0]
-
#LDW WARNING LEVELS
LDW_WARNING_1 = 0.9
LDW_WARNING_2 = 0.5
@@ -161,11 +158,8 @@ def __init__(self, dbc_name):
self.curv2 = 0.
self.curv3 = 0.
self.visionCurvC0 = 0.
- self.laneRange = 50 #max is 160m but OP has issues with precision beyond 50
- self.useZeroC0 = False
- self.useMap = False
- self.clipC0 = False
- self.useMapOnly = False
+ self.laneRange = 75 #max is 160m but OP has issues with precision beyond 50
+
self.laneWidth = 0.
self.stopSign_visible = False
@@ -196,7 +190,11 @@ def __init__(self, dbc_name):
self.prev_ldwStatus = 0
self.radarVin_idx = 0
-
+ self.LDW_ENABLE_SPEED = 16
+ self.should_ldw = False
+ self.ldw_numb_frame_start = 0
+ self.prev_changing_lanes = False
+
self.isMetric = (self.params.get("IsMetric") == "1")
def reset_traffic_events(self):
@@ -317,7 +315,25 @@ def update(self, enabled, CS, frame, actuators, \
# Basic highway lane change logic
changing_lanes = CS.right_blinker_on or CS.left_blinker_on
-
+
+ if (frame % 5 == 0):
+ if (changing_lanes and not self.prev_changing_lanes): #we have a transition from blinkers off to blinkers on, save the frame
+ self.ldw_numb_frame_start = frame
+ if (CS.v_ego > self.LDW_ENABLE_SPEED):
+ CS.UE.custom_alert_message(3, "LDW Disabled", 150, 4)
+
+ # update the previous state of the blinkers (chaning_lanes if (self.ALCA.laneChange_enabled > 1):
+ self.ldw_numb_frame_start = frame
+ self.prev_changing_lanes = changing_lanes
+ if self.alca_enabled:
+ self.ldw_numb_frame_start = frame + 200 #we don't want LDW for 2 seconds after ALCA finishes
+ #Determine if we should have LDW or not
+ self.should_ldw = (frame > (self.ldw_numb_frame_start + int( 50 * CS.ldwNumbPeriod)) and CS.v_ego > self.LDW_ENABLE_SPEED)
+
+ if self.should_ldw and self.ldw_numb_frame_start != 0:
+ self.ldw_numb_frame_start = 0
+ CS.UE.custom_alert_message(2, "LDW Enabled", 150, 4)
+
#upodate custom UI buttons and alerts
CS.UE.update_custom_ui()
@@ -337,11 +353,11 @@ def update(self, enabled, CS, frame, actuators, \
self.speed_limit_offset = 0.
if not self.isMetric:
self.speed_limit_offset = self.speed_limit_offset * CV.MPH_TO_MS
- if CS.useTeslaGPS:
+ if CS.useTeslaGPS and (frame % 10 == 0):
if self.gpsLocationExternal is None:
self.gpsLocationExternal = messaging.pub_sock(service_list['gpsLocationExternal'].port)
sol = gen_solution(CS)
- sol.logMonoTime = int(frame * DT_CTRL * 1e9)
+ sol.logMonoTime = int(realtime.sec_since_boot() * 1e9)
self.gpsLocationExternal.send(sol.to_bytes())
#get pitch/roll/yaw every 0.1 sec
@@ -351,7 +367,6 @@ def update(self, enabled, CS, frame, actuators, \
# Update statuses for custom buttons every 0.1 sec.
if (frame % 10 == 0):
- #self.ALCA.update_status(False)
self.ALCA.update_status((CS.cstm_btns.get_button_status("alca") > 0) and ((CS.enableALCA and not CS.hasTeslaIcIntegration) or (CS.hasTeslaIcIntegration and CS.alcaEnabled)))
pedal_can_sends = []
@@ -362,8 +377,7 @@ def update(self, enabled, CS, frame, actuators, \
self.ACC.enable_adaptive_cruise = False
else:
# Update ACC module info.
- if frame % 5 == 0:
- self.ACC.update_stat(CS, True)
+ self.ACC.update_stat(CS, True)
self.PCC.enable_pedal_cruise = False
# Update HSO module info.
@@ -380,7 +394,7 @@ def update(self, enabled, CS, frame, actuators, \
else:
CS.v_cruise_pcm = max(0.,CS.v_ego * CV.MS_TO_KPH +0.5) * speed_uom_kph
# Get the turn signal from ALCA.
- turn_signal_needed, self.alca_enabled = self.ALCA.update(enabled, CS, actuators)
+ turn_signal_needed, self.alca_enabled = self.ALCA.update(enabled, CS, actuators, self.alcaStateData, frame)
apply_angle = -actuators.steerAngle # Tesla is reversed vs OP.
human_control,turn_signal_needed_hso = self.HSO.update_stat(self,CS, enabled, actuators, frame)
if turn_signal_needed == 0:
@@ -399,11 +413,8 @@ def update(self, enabled, CS, frame, actuators, \
else:
angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_VU)
- des_angle_factor = interp(CS.v_ego, DES_ANGLE_ADJUST_FACTOR_BP, DES_ANGLE_ADJUST_FACTOR )
- if self.alca_enabled or not CS.enableSpeedVariableDesAngle:
- des_angle_factor = 1.
#BB disable limits to test 0.5.8
- # apply_angle = clip(apply_angle * des_angle_factor, self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim)
+ # apply_angle = clip(apply_angle , self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim)
# If human control, send the steering angle as read at steering wheel.
if human_control:
apply_angle = CS.angle_steers
@@ -472,46 +483,6 @@ def update(self, enabled, CS, frame, actuators, \
can_messages = self.handleTrafficEvents(trafficEventsSocket = socket)
can_sends.extend(can_messages)
- if (CS.roadCurvRange > 20) and self.useMap:
- if self.useZeroC0:
- self.curv0 = 0.
- elif self.clipC0:
- self.curv0 = -clip(CS.roadCurvC0,-0.5,0.5)
- #else:
- # self.curv0 = -CS.roadCurvC0
- #if CS.v_ego > 9:
- # self.curv1 = -CS.roadCurvC1
- #else:
- # self.curv1 = 0.
- self.curv2 = -CS.roadCurvC2
- self.curv3 = -CS.roadCurvC3
- self.laneRange = CS.roadCurvRange
- #else:
- # self.curv0 = 0.
- # self.curv1 = 0.
- # self.curv2 = 0.
- # self.curv3 = 0.
- # self.laneRange = 0
-
- if (CS.csaRoadCurvRange > 2.) and self.useMap and not self.useMapOnly:
- self.curv2 = -CS.csaRoadCurvC2
- self.curv3 = -CS.csaRoadCurvC3
- #if self.laneRange > 0:
- # self.laneRange = min(self.laneRange,CS.csaRoadCurvRange)
- #else:
- self.laneRange = CS.csaRoadCurvRange
- elif (CS.csaOfframpCurvRange > 2.) and self.useMap and not self.useMapOnly:
- #self.curv2 = -CS.csaOfframpCurvC2
- #self.curv3 = -CS.csaOfframpCurvC3
- #self.curv0 = 0.
- #self.curv1 = 0.
- #if self.laneRange > 0:
- # self.laneRange = min(self.laneRange,CS.csaOfframpCurvRange)
- #else:
- self.laneRange = CS.csaOfframpCurvRange
- else:
- self.laneRange = 50
- self.laneRange = int(clip(self.laneRange,0,159))
op_status = 0x02
hands_on_state = 0x00
forward_collision_warning = 0 #1 if needed
@@ -726,19 +697,16 @@ def handlePathPlanSocketForCurvatureOnIC(self, pathPlanSocket, alcaStateData, CS
self.curv2 = -clip(pp.dPoly[1],-0.0025,0.0025) #self.curv2Matrix.add(-clip(pp.cPoly[1],-0.0025,0.0025))
self.curv3 = -clip(pp.dPoly[0],-0.00003,0.00003) #self.curv3Matrix.add(-clip(pp.cPoly[0],-0.00003,0.00003))
self.laneWidth = pp.laneWidth
- self.laneRange = 50 # it is fixed in OP at 50m pp.viewRange
self.visionCurvC0 = self.curv0
self.prev_ldwStatus = self.ldwStatus
self.ldwStatus = 0
- if (self.ALCA.laneChange_direction != 0) and alcaStateData.alcaError:
- self.ALCA.stop_ALCA(CS)
- if self.alca_enabled:
+ if alcaStateData.alcaEnabled:
#exagerate position a little during ALCA to make lane change look smoother on IC
- if self.ALCA.laneChange_over_the_line:
- self.curv0 = self.ALCA.laneChange_direction * self.laneWidth - self.curv0
+ self.curv1 = 0.0 #straighten the turn for ALCA
+ self.curv0 = -self.ALCA.laneChange_direction * alcaStateData.alcaLaneWidth * alcaStateData.alcaStep / alcaStateData.alcaTotalSteps #animas late change on IC
self.curv0 = clip(self.curv0, -3.5, 3.5)
else:
- if CS.enableLdw and (not CS.blinker_on) and (CS.v_ego > 15.6) and (turn_signal_needed == 0):
+ if self.should_ldw and (CS.enableLdw and (not CS.blinker_on) and (turn_signal_needed == 0)):
if pp.lProb > LDW_LANE_PROBAB:
lLaneC0 = -pp.lPoly[3]
if abs(lLaneC0) < LDW_WARNING_2:
diff --git a/selfdrive/car/tesla/carstate.py b/selfdrive/car/tesla/carstate.py
index 383176a713f0c5..72ac38ea6e88f4 100644
--- a/selfdrive/car/tesla/carstate.py
+++ b/selfdrive/car/tesla/carstate.py
@@ -201,7 +201,7 @@ def __init__(self, CP):
self.CL_MIN_V = 8.9
self.CL_MAX_A = 20.
# labels for buttons
- self.btns_init = [["alca", "ALC", ["MadMax", "Normal", "Calm"]],
+ self.btns_init = [["alca", "ALC", [""]],
[ACCMode.BUTTON_NAME, ACCMode.BUTTON_ABREVIATION, ACCMode.labels()],
["dsp", "DSP", ["OP","MIN","OFF","GYRO"]],
["", "", [""]],
@@ -215,9 +215,6 @@ def __init__(self, CP):
self.enableALCA = True
self.enableDasEmulation = True
self.enableRadarEmulation = True
- self.enableSpeedVariableDesAngle = False
- self.enableRollAngleCorrection = False
- self.enableFeedForwardAngleCorrection = True
self.enableDriverMonitor = True
self.enableShowCar = True
self.enableShowLogo = True
@@ -240,9 +237,10 @@ def __init__(self, CP):
self.radarEpasType = 0
self.fix1916 = False
self.forceFingerprintTesla = False
- self.eonToFront = 0.1
self.spinnerText = ""
- self.enableParamLearner = False
+ self.hsoNumbPeriod = 1.5
+ self.ldwNumbPeriod = 1.5
+ self.hsoBlinkerExtender = 1.0
#read config file
read_config_file(self)
### END OF MAIN CONFIG OPTIONS ###
diff --git a/selfdrive/car/tesla/interface.py b/selfdrive/car/tesla/interface.py
index 796e7331172abc..8eaa7ac3f42491 100644
--- a/selfdrive/car/tesla/interface.py
+++ b/selfdrive/car/tesla/interface.py
@@ -1,7 +1,7 @@
#!/usr/bin/env python
from cereal import car, tesla
from common.numpy_fast import clip, interp
-from common.realtime import sec_since_boot, DT_CTRL
+from common.realtime import DT_CTRL
from selfdrive.config import Conversions as CV
from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET, get_events
from selfdrive.controls.lib.vehicle_model import VehicleModel
@@ -35,8 +35,6 @@ def __init__(self, CP, CarController):
self.gas_pressed_prev = False
self.brake_pressed_prev = False
self.can_invalid_count = 0
- self.alca = messaging.pub_sock(service_list['alcaStatus'].port)
-
# *** init the major players ***
@@ -217,6 +215,7 @@ def get_params(candidate, fingerprint, vin="", is_panda_black=False):
ret.steerLimitAlert = False
ret.startAccel = 0.5
ret.steerRateCost = 1.0
+
ret.radarOffCan = not CarSettings().get_value("useTeslaRadar")
return ret
@@ -450,10 +449,10 @@ def update(self, c, can_strings):
# NO_ENTRY events, so controlsd will display alerts. Also not send enable events
# too close in time, so a no_entry will not be followed by another one.
# TODO: button press should be the only thing that triggers enble
- if ((cur_time - self.last_enable_pressed) < 0.2 and
+ if ((cur_time - self.last_enable_pressed) < 0.2 and # pylint: disable=chained-comparison
(cur_time - self.last_enable_sent) > 0.2 and
ret.cruiseState.enabled) or \
- (enable_pressed and get_events(events, [ET.NO_ENTRY])):
+ (enable_pressed and get_events(events, [ET.NO_ENTRY])):
if ret.seatbeltUnlatched:
self.CC.DAS_211_accNoSeatBelt = 1
self.CC.warningCounter = 300
@@ -494,15 +493,6 @@ def update(self, c, can_strings):
self.gas_pressed_prev = ret.gasPressed
self.brake_pressed_prev = self.CS.brake_pressed != 0
- #pass ALCA status
- alca_status = tesla.ALCAStatus.new_message()
-
- alca_status.alcaEnabled = bool(self.CS.ALCA_enabled)
- alca_status.alcaTotalSteps = int(self.CS.ALCA_total_steps)
- alca_status.alcaDirection = int(self.CS.ALCA_direction)
- alca_status.alcaError = bool(self.CS.ALCA_error)
-
- self.alca.send(alca_status.to_bytes())
# cast to reader so it can't be modified
return ret.as_reader()
diff --git a/selfdrive/car/tesla/radar_interface.py b/selfdrive/car/tesla/radar_interface.py
index b3111c286910e2..5ce6c6dbe647f7 100644
--- a/selfdrive/car/tesla/radar_interface.py
+++ b/selfdrive/car/tesla/radar_interface.py
@@ -47,6 +47,7 @@ def _create_radard_can_parser():
checks = list(zip(RADAR_A_MSGS + RADAR_B_MSGS, [6]*(msg_a_n + msg_b_n)))
+
return CANParser(os.path.splitext(dbc_f)[0].encode('utf8'), signals, checks, 1)
@@ -55,6 +56,7 @@ class RadarInterface(RadarInterfaceBase):
tinklaClient = TinklaClient()
def __init__(self,CP):
+ super().__init__(self)
# radar
self.pts = {}
self.extPts = {}
@@ -82,7 +84,8 @@ def update(self, can_strings):
if not self.useTeslaRadar:
time.sleep(0.05)
return car.RadarData.new_message(),self.extPts.values()
- if can_strings != None:
+
+ if can_strings is not None:
vls = self.rcp.update_strings(can_strings)
self.updated_messages.update(vls)
diff --git a/selfdrive/car/tesla/readconfig.py b/selfdrive/car/tesla/readconfig.py
index bf89f0b457d8cd..50a60cda0ee9aa 100644
--- a/selfdrive/car/tesla/readconfig.py
+++ b/selfdrive/car/tesla/readconfig.py
@@ -21,9 +21,11 @@ def read(self, into, config_path):
print("no config file, creating with defaults...")
main_section = 'OP_CONFIG'
+ pref_section = 'OP_PREFERENCES'
logging_section = 'LOGGING'
config = configparser.RawConfigParser(allow_no_value=True)
config.add_section(main_section)
+ config.add_section(pref_section)
config.add_section(logging_section)
#user_handle -> userHandle
@@ -44,15 +46,6 @@ def read(self, into, config_path):
)
file_changed |= didUpdate
- #eon_to_front -> eonToFront
- into.eonToFront, didUpdate = self.read_config_entry(
- config, configr, prev_file_contents, section = main_section,
- entry = 'eon_to_front', entry_type = float,
- default_value = 0.9,
- comment = 'Distance between EON plane and front of the car.'
- )
- file_changed |= didUpdate
-
#force_pedal_over_cc -> forcePedalOverCC
into.forcePedalOverCC, didUpdate = self.read_config_entry(
config, configr, prev_file_contents, section = main_section,
@@ -71,7 +64,7 @@ def read(self, into, config_path):
)
file_changed |= didUpdate
- #enable_das_emulation -> enableDasEmulation
+ #enable_alca -> enableALCA
into.enableALCA, didUpdate = self.read_config_entry(
config, configr, prev_file_contents, section = main_section,
entry = 'enable_alca', entry_type = bool,
@@ -98,33 +91,6 @@ def read(self, into, config_path):
)
file_changed |= didUpdate
- #enable_roll_angle_correction -> enableRollAngleCorrection
- into.enableSpeedVariableDesAngle, didUpdate = self.read_config_entry(
- config, configr, prev_file_contents, section = main_section,
- entry = 'enable_speed_variable_angle', entry_type = bool,
- default_value = True,
- comment = ''
- )
- file_changed |= didUpdate
-
- #enable_roll_angle_correction -> enableRollAngleCorrection
- into.enableRollAngleCorrection, didUpdate = self.read_config_entry(
- config, configr, prev_file_contents, section = main_section,
- entry = 'enable_roll_angle_correction', entry_type = bool,
- default_value = False,
- comment = ''
- )
- file_changed |= didUpdate
-
- #enable_feed_forward_angle_correction -> enableFeedForwardAngleCorrection
- into.enableFeedForwardAngleCorrection, didUpdate = self.read_config_entry(
- config, configr, prev_file_contents, section = main_section,
- entry = 'enable_feed_forward_angle_correction', entry_type = bool,
- default_value = True,
- comment = ''
- )
- file_changed |= didUpdate
-
#enable_driver_monitor -> enableDriverMonitor
into.enableDriverMonitor, didUpdate = self.read_config_entry(
config, configr, prev_file_contents, section = main_section,
@@ -134,24 +100,6 @@ def read(self, into, config_path):
)
file_changed |= didUpdate
- #enable_show_car -> enableShowCar
- into.enableShowCar, didUpdate = self.read_config_entry(
- config, configr, prev_file_contents, section = main_section,
- entry = 'enable_show_car', entry_type = bool,
- default_value = True,
- comment = 'Shows a Tesla car in the limitted UI mode instead of the triangle that identifies the lead car; this is only used if you do not have IC/CID integration'
- )
- file_changed |= didUpdate
-
- #enable_show_logo -> enableShowLogo
- into.enableShowLogo, didUpdate = self.read_config_entry(
- config, configr, prev_file_contents, section = main_section,
- entry = 'enable_show_logo', entry_type = bool,
- default_value = True,
- comment = 'Shows a Tesla red logo on the EON screen when OP is not enabled'
- )
- file_changed |= didUpdate
-
#has_noctua_fan -> hasNoctuaFan
into.hasNoctuaFan, didUpdate = self.read_config_entry(
config, configr, prev_file_contents, section = main_section,
@@ -264,15 +212,6 @@ def read(self, into, config_path):
into.radarVIN = default_radar_vin
file_changed = True
- #enable_ldw = enableLdw
- into.enableLdw, didUpdate = self.read_config_entry(
- config, configr, prev_file_contents, section = main_section,
- entry = 'enable_ldw', entry_type = bool,
- default_value = True,
- comment = 'Enable the Lane Departure Warning (LDW) feature; this feature warns the driver is the car gets too close to one of the lines when driving above 45 MPH (72 km/h) without touching the steering wheel and when the turn signal is off'
- )
- file_changed |= didUpdate
-
#radar_offset -> radarOffset
into.radarOffset, didUpdate = self.read_config_entry(
config, configr, prev_file_contents, section = main_section,
@@ -320,19 +259,64 @@ def read(self, into, config_path):
#spiner_text -> spinnerText
into.spinnerText, didUpdate = self.read_config_entry(
- config, configr, prev_file_contents, section = main_section,
+ config, configr, prev_file_contents, section = pref_section,
entry = 'spinner_text', entry_type = str,
default_value = '%d',
comment = 'The text that is shown for the spinner when spawning the managed services.'
)
file_changed |= didUpdate
- #enable_param_learner -> enableParamLearner
- into.enableParamLearner, didUpdate = self.read_config_entry(
- config, configr, prev_file_contents, section = main_section,
- entry = 'enable_param_learner', entry_type = bool,
- default_value = False,
- comment = 'Set this setting to True if you want OP to relearn steering rate automatically or will be using fixed rate when False'
+ #hso_numb_period -> hsoNumbPeriod
+ into.hsoNumbPeriod, didUpdate = self.read_config_entry(
+ config, configr, prev_file_contents, section = pref_section,
+ entry = 'hso_numb_period', entry_type = float,
+ default_value = 1.5,
+ comment = 'Period to delay (in seconds) the reengagement of LKAS after human turn signal has been used. Time starts when the turn signal is turned on.'
+ )
+ file_changed |= didUpdate
+
+ #enable_ldw = enableLdw
+ into.enableLdw, didUpdate = self.read_config_entry(
+ config, configr, prev_file_contents, section = pref_section,
+ entry = 'enable_ldw', entry_type = bool,
+ default_value = True,
+ comment = 'Enable the Lane Departure Warning (LDW) feature; this feature warns the driver is the car gets too close to one of the lines when driving above 45 MPH (72 km/h) without touching the steering wheel and when the turn signal is off'
+ )
+ file_changed |= didUpdate
+
+ #ldw_numb_period -> ldwNumbPeriod
+ into.ldwNumbPeriod, didUpdate = self.read_config_entry(
+ config, configr, prev_file_contents, section = pref_section,
+ entry = 'ldw_numb_period', entry_type = float,
+ default_value = 1.5,
+ comment = 'Period to delay (in seconds) the LDW warnings after human turn signal has been used. Time starts when the turn signal is turned on.'
+ )
+ file_changed |= didUpdate
+
+ #hso_blinker_extender -> hsoBlinkerExtender
+ into.hsoBlinkerExtender, didUpdate = self.read_config_entry(
+ config, configr, prev_file_contents, section = pref_section,
+ entry = 'hso_blinker_extender', entry_type = float,
+ default_value = 0.0,
+ comment = 'Period to keep the blinker on (in seconds). Time starts when the turn signal is turned off. If LKA is reengaged, the signal is turned off automatically.'
+ )
+ file_changed |= didUpdate
+
+ #enable_show_car -> enableShowCar
+ into.enableShowCar, didUpdate = self.read_config_entry(
+ config, configr, prev_file_contents, section = pref_section,
+ entry = 'enable_show_car', entry_type = bool,
+ default_value = True,
+ comment = 'Shows a Tesla car in the limitted UI mode instead of the triangle that identifies the lead car; this is only used if you do not have IC/CID integration'
+ )
+ file_changed |= didUpdate
+
+ #enable_show_logo -> enableShowLogo
+ into.enableShowLogo, didUpdate = self.read_config_entry(
+ config, configr, prev_file_contents, section = pref_section,
+ entry = 'enable_show_logo', entry_type = bool,
+ default_value = True,
+ comment = 'Shows a Tesla red logo on the EON screen when OP is not enabled'
)
file_changed |= didUpdate
@@ -391,15 +375,11 @@ class CarSettings():
userHandle = None
forceFingerprintTesla = None
- eonToFront = None
forcePedalOverCC = None
enableHSO = None
enableALCA = None
enableDasEmulation = None
enableRadarEmulation = None
- enableSpeedVariableDesAngle = None
- enableRollAngleCorrection = None
- enableFeedForwardAngleCorrection = None
enableDriverMonitor = None
enableShowCar = None
enableShowLogo = None
@@ -424,7 +404,9 @@ class CarSettings():
spinnerText = None
shouldLogProcessCommErrors = None
shouldLogCanErrors = None
- enableParamLearner = None
+ hsoNumbPeriod = None
+ ldwNumbPeriod = None
+ hsoBlinkerExtender = None
def __init__(self, optional_config_file_path = default_config_file_path):
config_file = ConfigFile()
@@ -436,4 +418,5 @@ def get_value(self, name_of_variable):
# Legacy support
def read_config_file(into, config_path = default_config_file_path):
config_file = ConfigFile()
- config_file.read(into, config_path)
\ No newline at end of file
+ config_file.read(into, config_path)
+
diff --git a/selfdrive/car/tesla/test_readconfig.py b/selfdrive/car/tesla/test_readconfig.py
index 40a67ad423e82f..10110209c7c1f2 100755
--- a/selfdrive/car/tesla/test_readconfig.py
+++ b/selfdrive/car/tesla/test_readconfig.py
@@ -187,9 +187,6 @@ def check_defaults(self, cs):
self.assertEqual(cs.enableALCA, True)
self.assertEqual(cs.enableDasEmulation, False)
self.assertEqual(cs.enableRadarEmulation, False)
- self.assertEqual(cs.enableSpeedVariableDesAngle, True)
- self.assertEqual(cs.enableRollAngleCorrection, False)
- self.assertEqual(cs.enableFeedForwardAngleCorrection, True)
self.assertEqual(cs.enableDriverMonitor, True)
self.assertEqual(cs.enableShowCar, True)
self.assertEqual(cs.enableShowLogo, True)
diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index 4c1f8169425e2b..3dcb3e3955c100 100755
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -180,7 +180,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay
ret.mass = 3370. * CV.LB_TO_KG + STD_CARGO_KG
ret.lateralTuning.pid.kf = 0.00007818594
- elif candidate == CAR.COROLLA_TSS2:
+ elif candidate in [CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2]:
stop_and_go = True
ret.safetyParam = 73
ret.wheelbase = 2.63906
@@ -190,7 +190,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]]
ret.lateralTuning.pid.kf = 0.00007818594
- elif candidate == CAR.LEXUS_ESH_TSS2:
+ elif candidate in [CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2]:
stop_and_go = True
ret.safetyParam = 73
ret.wheelbase = 2.8702
@@ -212,7 +212,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay
elif candidate == CAR.LEXUS_IS:
stop_and_go = False
- ret.safetyParam = 66
+ ret.safetyParam = 77
ret.wheelbase = 2.79908
ret.steerRatio = 13.3
tire_stiffness_factor = 0.444
diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py
index 4ec85d51e277ed..e3bacab9c937a9 100644
--- a/selfdrive/car/toyota/values.py
+++ b/selfdrive/car/toyota/values.py
@@ -15,6 +15,8 @@ class CAR:
AVALON = "TOYOTA AVALON 2016"
RAV4_TSS2 = "TOYOTA RAV4 2019"
COROLLA_TSS2 = "TOYOTA COROLLA TSS2 2019"
+ COROLLAH_TSS2 = "TOYOTA COROLLA HYBRID TSS2 2019"
+ LEXUS_ES_TSS2 = "LEXUS ES 2019"
LEXUS_ESH_TSS2 = "LEXUS ES 300H 2019"
SIENNA = "TOYOTA SIENNA XLE 2018"
LEXUS_IS = "LEXUS IS300 2018"
@@ -162,7 +164,8 @@ class ECU:
36: 8, 37: 8, 170: 8, 180: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 581: 5, 608: 8, 610: 5, 643: 7, 713: 8, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 7, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 3, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1112: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1184: 8, 1185: 8, 1186: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1212: 8, 1227: 8, 1232: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 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
},
{
- 36: 8, 37: 8, 170: 8, 180: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 581: 5, 608: 8, 610: 5, 643: 7, 713: 8, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 3, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1057: 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, 1184: 8, 1185: 8, 1186: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1212: 8, 1227: 8, 1232: 8, 1237: 8, 1279: 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
+ # 2019 Highlander Hybrid Limited Platinum
+ 36: 8, 37: 8, 170: 8, 180: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 581: 5, 608: 8, 610: 5, 643: 7, 713: 8, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 7, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 3, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1076: 8, 1077: 8, 1112: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1184: 8, 1185: 8, 1186: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1212: 8, 1227: 8, 1232: 8, 1235: 8, 1237: 8, 1263: 8, 1279: 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
}],
CAR.AVALON: [{
36: 8, 37: 8, 170: 8, 180: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 547: 8, 550: 8, 552: 4, 562: 6, 608: 8, 610: 5, 643: 7, 705: 8, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 905: 8, 911: 1, 916: 2, 921: 8, 933: 6, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 1005: 2, 1014: 8, 1017: 8, 1041: 8, 1042: 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, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1200: 8, 1201: 8, 1202: 8, 1203: 8, 1206: 8, 1227: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1558: 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, 1596: 8, 1597: 8, 1664: 8, 1728: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
@@ -181,12 +184,26 @@ class ECU:
{
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, 913: 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, 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, 1595: 8, 1649: 8, 1745: 8, 1775: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1808: 8, 1809: 8, 1816: 8, 1817: 8, 1840: 8, 1848: 8, 1904: 8, 1912: 8, 1940: 8, 1941: 8, 1948: 8, 1949: 8, 1952: 8, 1960: 8, 1981: 8, 1986: 8, 1990: 8, 1994: 8, 1998: 8, 2004: 8
}],
+ CAR.COROLLAH_TSS2: [
+ # 2019 Taiwan Altis Hybrid
+ {
+ 36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 401: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 713: 8, 728: 8, 740: 5, 742: 8, 743: 8, 761: 8, 765: 8, 800: 8, 810: 2, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 885: 8, 896: 8, 898: 8, 918: 7, 921: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 987: 8, 993: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1082: 8, 1112: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 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, 1745: 8, 1775: 8, 1779: 8
+ }],
+ CAR.LEXUS_ES_TSS2: [
+ {
+ 36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 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, 658: 8, 705: 8, 728: 8, 740: 5, 761: 8, 764: 8, 765: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 824: 8, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 882: 8, 885: 8, 889: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 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, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1696: 8, 1775: 8, 1777: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8
+ }],
CAR.LEXUS_ESH_TSS2: [
{
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 401: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 658: 8, 713: 8, 728: 8, 740: 5, 742: 8, 743: 8, 744: 8, 761: 8, 764: 8, 765: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 863: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 882: 8, 885: 8, 889: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 987: 8, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 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, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1696: 8, 1775: 8, 1777: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
}],
- CAR.SIENNA: [{
+ CAR.SIENNA: [
+ {
36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 545: 5, 548: 8, 550: 8, 552: 4, 562: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 764: 8, 800: 8, 824: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 888: 8, 896: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 1, 918: 7, 921: 8, 933: 8, 944: 6, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1008: 2, 1014: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1114: 8, 1160: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1200: 8, 1201: 8, 1202: 8, 1203: 8, 1212: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1279: 8, 1552: 8, 1553: 8, 1555: 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, 1656: 8, 1664: 8, 1666: 8, 1667: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
+ },
+ # XLE AWD 2018
+ {
+ 36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 238: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 545: 5, 548: 8, 550: 8, 552: 4, 562: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 764: 8, 800: 8, 824: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 1, 921: 8, 933: 8, 944: 6, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1008: 2, 1014: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1114: 8, 1160: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1200: 8, 1201: 8, 1202: 8, 1203: 8, 1212: 8, 1227: 8, 1235: 8, 1237: 8, 1279: 8, 1552: 8, 1553: 8, 1555: 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, 1656: 8, 1664: 8, 1666: 8, 1667: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
}],
CAR.LEXUS_IS: [
# IS300 2018
@@ -219,12 +236,14 @@ class ECU:
CAR.AVALON: dbc_dict('toyota_avalon_2017_pt_generated', 'toyota_adas'),
CAR.RAV4_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.COROLLA_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
+ CAR.COROLLAH_TSS2: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_tss2_adas'),
+ CAR.LEXUS_ES_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.LEXUS_ESH_TSS2: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_tss2_adas'),
CAR.SIENNA: dbc_dict('toyota_sienna_xle_2018_pt_generated', 'toyota_adas'),
CAR.LEXUS_IS: dbc_dict('lexus_is_2018_pt_generated', 'toyota_adas'),
CAR.LEXUS_CTH: dbc_dict('lexus_ct200h_2018_pt_generated', 'toyota_adas'),
}
-NO_DSU_CAR = [CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.LEXUS_ESH_TSS2]
-TSS2_CAR = [CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.LEXUS_ESH_TSS2]
-NO_STOP_TIMER_CAR = [CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.LEXUS_ESH_TSS2, CAR.SIENNA] # no resume button press required
+NO_DSU_CAR = [CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2]
+TSS2_CAR = [CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2]
+NO_STOP_TIMER_CAR = [CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.SIENNA] # no resume button press required
diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index 79001ec850ec2a..863869953cca2b 100755
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -522,7 +522,7 @@ def controlsd_thread(sm=None, pm=None, can_sock=None):
# FIXME: offroad alerts should not be created with negative severity
connectivity_alert = params.get("Offroad_ConnectivityNeeded", encoding='utf8')
- internet_needed = connectivity_alert is not None and json.loads(connectivity_alert.replace("'", "\""))["severity"] >= 0
+ internet_needed = connectivity_alert is not None and json.loads(connectivity_alert)["severity"] >= 0
prof = Profiler(False) # off by default
diff --git a/selfdrive/controls/lib/lane_planner.py b/selfdrive/controls/lib/lane_planner.py
index 454c8454f7acfa..2042cefd6d9b6e 100644
--- a/selfdrive/controls/lib/lane_planner.py
+++ b/selfdrive/controls/lib/lane_planner.py
@@ -27,7 +27,7 @@ def calc_d_poly(l_poly, r_poly, p_poly, l_prob, r_prob, lane_width):
path_from_right_lane = r_poly.copy()
path_from_right_lane[3] += lane_width / 2.0
- lr_prob = l_prob * r_prob
+ lr_prob = l_prob + r_prob - l_prob * r_prob
d_poly_lane = (l_prob * path_from_left_lane + r_prob * path_from_right_lane) / (l_prob + r_prob + 0.0001)
return lr_prob * d_poly_lane + (1.0 - lr_prob) * p_poly
diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py
index 8acdc27452b2fb..a7ecfee7f4473b 100644
--- a/selfdrive/controls/lib/pathplanner.py
+++ b/selfdrive/controls/lib/pathplanner.py
@@ -6,13 +6,12 @@
from selfdrive.controls.lib.drive_helpers import MPC_COST_LAT
from selfdrive.controls.lib.lane_planner import LanePlanner
import selfdrive.messaging as messaging
-from selfdrive.car.tesla.readconfig import CarSettings
LOG_MPC = os.environ.get('LOG_MPC', False)
-def calc_states_after_delay(states, v_ego, steer_angle, curvature_factor, steer_ratio, delay, long_camera_offset):
- states[0].x = max(v_ego * delay + long_camera_offset, 0.0)
+def calc_states_after_delay(states, v_ego, steer_angle, curvature_factor, steer_ratio, delay):
+ states[0].x = max(v_ego * delay, 0.0)
states[0].psi = v_ego * curvature_factor * math.radians(steer_angle) / steer_ratio * delay
return states
@@ -26,7 +25,6 @@ def __init__(self, CP):
self.setup_mpc(CP.steerRateCost)
self.solution_invalid_cnt = 0
self.path_offset_i = 0.0
- self.eonToFront = CarSettings().get_value("eonToFront")
def setup_mpc(self, steer_rate_cost):
self.libmpc = libmpc_py.libmpc
@@ -70,7 +68,7 @@ def update(self, sm, pm, CP, VM):
# account for actuation delay
- self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers - angle_offset, curvature_factor, VM.sR, CP.steerActuatorDelay, self.eonToFront)
+ self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers - angle_offset, curvature_factor, VM.sR, CP.steerActuatorDelay)
v_ego_mpc = max(v_ego, 5.0) # avoid mpc roughness due to low speed
self.libmpc.run_mpc(self.cur_state, self.mpc_solution,
diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py
index 8d66b4f71e1452..f45f9fa5c7b373 100755
--- a/selfdrive/controls/radard.py
+++ b/selfdrive/controls/radard.py
@@ -72,7 +72,10 @@ def get_lead(v_ego, ready, clusters, lead_msg, low_speed_override=True,use_tesla
cluster = None
lead_dict = {'status': False}
- lead_dict_ext = {'trackId': 1, 'oClass': 0, 'length': 0.}
+ lead_dict_ext = {'trackId': 1, 'oClass': 1, 'length': 0.}
+ # temporary for development purposes: we set the default lead vehicle type to truck (=0) to distinguish between vision (truck) and radar leads (car) in IC
+ if use_tesla_radar:
+ lead_dict_ext['oClass'] = 0
if cluster is not None:
lead_dict,lead_dict_ext = cluster.get_RadarState(lead_msg.prob)
elif (cluster is None) and ready and (lead_msg.prob > .5):
diff --git a/selfdrive/locationd/params_learner.cc b/selfdrive/locationd/params_learner.cc
index 778dc7096128f0..f66977e1eb0a09 100644
--- a/selfdrive/locationd/params_learner.cc
+++ b/selfdrive/locationd/params_learner.cc
@@ -28,6 +28,8 @@ ParamsLearner::ParamsLearner(cereal::CarParams::Reader car_params,
cF0 = car_params.getTireStiffnessFront();
cR0 = car_params.getTireStiffnessRear();
+ prev_u = 0;
+
l = car_params.getWheelbase();
m = car_params.getMass();
@@ -43,53 +45,31 @@ ParamsLearner::ParamsLearner(cereal::CarParams::Reader car_params,
alpha3 = 0.1 * learning_rate;
alpha4 = 1.0 * learning_rate;
cs_sr = car_params.getSteerRatio();
- learnerEnabled = false;
- char line[500];
- int linenum=0;
- FILE *stream;
- stream = fopen("/data/bb_openpilot.cfg","r");
- while(fgets(line, 500, stream) != NULL)
- {
- char setting[256], value[256], oper[10];
-
- linenum++;
- if(line[0] == '#') continue;
- if(sscanf(line, "%s %s %s", setting, oper , value) != 3)
- {
- continue;
- }
- if (strcmp("enable_param_learner",setting) == 0) {
- //printf("Found [%s] %s [%s]\n", setting, oper, value);
- learnerEnabled = (strcmp("True",value) == 0);
- printf("Set learnerEnabled to [%s]\n", learnerEnabled ? "true" : "false");
- sR = cs_sr;
- printf("Setting steerRatio to [%f]\n", sR);
- }
- }
- fclose(stream);
}
bool ParamsLearner::update(double psi, double u, double sa) {
- if (u > 10.0 && fabs(sa) < (DEGREES_TO_RADIANS * 90.)) {
+ //BB only learn when speed is constant; accel and decel in turns can affect learner
+ if (u > 10.0 && fabs(sa) < (DEGREES_TO_RADIANS * 90.)) {
double ao_diff = 2.0*cF0*cR0*l*u*x*(1.0*cF0*cR0*l*u*x*(ao - sa) + psi*sR*(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0)))/(pow(sR, 2)*pow(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0), 2));
double new_ao = ao - alpha1 * ao_diff;
double slow_ao_diff = 2.0*cF0*cR0*l*u*x*(1.0*cF0*cR0*l*u*x*(slow_ao - sa) + psi*sR*(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0)))/(pow(sR, 2)*pow(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0), 2));
double new_slow_ao = slow_ao - alpha2 * slow_ao_diff;
-
+
double new_x = x - alpha3 * (-2.0*cF0*cR0*l*m*pow(u, 3)*(slow_ao - sa)*(aF*cF0 - aR*cR0)*(1.0*cF0*cR0*l*u*x*(slow_ao - sa) + psi*sR*(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0)))/(pow(sR, 2)*pow(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0), 3)));
double new_sR = sR - alpha4 * (-2.0*cF0*cR0*l*u*x*(slow_ao - sa)*(1.0*cF0*cR0*l*u*x*(slow_ao - sa) + psi*sR*(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0)))/(pow(sR, 3)*pow(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0), 2)));
- ao = new_ao;
- slow_ao = new_slow_ao;
- x = new_x;
- if (learnerEnabled) {
+
+ //only consider if acceleration [abs(prev_speed - speed) * frequency] is less than MAX_ACCEL
+ double a = abs(prev_u - u) * FREQUENCY;
+ if (a < MAX_ACCEL) {
+ ao = new_ao;
+ slow_ao = new_slow_ao;
+ x = new_x;
sR = new_sR;
- } else {
- sR = cs_sr;
}
}
-
+ prev_u = u;
#ifdef DEBUG
std::cout << "Instant AO: " << (RADIANS_TO_DEGREES * ao) << "\tAverage AO: " << (RADIANS_TO_DEGREES * slow_ao);
std::cout << "\tStiffness: " << x << "\t sR: " << sR << std::endl;
diff --git a/selfdrive/locationd/params_learner.h b/selfdrive/locationd/params_learner.h
index a433275fc94b10..aab0f712cb9c8a 100644
--- a/selfdrive/locationd/params_learner.h
+++ b/selfdrive/locationd/params_learner.h
@@ -12,6 +12,9 @@
#define MIN_SR_TH 0.55
#define MAX_SR_TH 1.9
+#define FREQUENCY 100.0 //BB learner is called at 100 Hz
+#define MAX_ACCEL 0.2 //BB maximum acceleration m/s^2 accepted before ignoring the calculations
+
class ParamsLearner {
double cF0, cR0;
double aR, aF;
@@ -20,12 +23,12 @@ class ParamsLearner {
double min_sr, max_sr, min_sr_th, max_sr_th;
double alpha1, alpha2, alpha3, alpha4;
double cs_sr;
- bool learnerEnabled;
public:
double ao;
double slow_ao;
double x, sR;
+ double prev_u; //BB previous speed so we only learn when speed is constant between iterations
ParamsLearner(cereal::CarParams::Reader car_params,
double angle_offset,
diff --git a/selfdrive/manager.py b/selfdrive/manager.py
index 9cda418a0afa86..80048f1d868f66 100755
--- a/selfdrive/manager.py
+++ b/selfdrive/manager.py
@@ -5,6 +5,7 @@
import errno
import signal
import subprocess
+import time
from selfdrive.tinklad.tinkla_interface import TinklaClient
from cereal import tinkla
from selfdrive.car.tesla.readconfig import CarSettings
@@ -321,7 +322,7 @@ def system(cmd):
output=e.output[-1024:],
returncode=e.returncode)
-def sendUserInfoToTinkla(params):
+def sendUserInfoToTinkla(params, tinklaClient):
carSettings = CarSettings()
gitRemote = params.get("GitRemote")
gitBranch = params.get("GitBranch")
@@ -368,9 +369,9 @@ def manager_thread():
logger_dead = False
# Tinkla interface
- global tinklaClient
+ last_tinklad_send_attempt_time = 0
tinklaClient = TinklaClient()
- sendUserInfoToTinkla(params)
+ sendUserInfoToTinkla(params=params, tinklaClient=tinklaClient)
while 1:
msg = messaging.recv_sock(thermal_sock, wait=True)
@@ -382,7 +383,11 @@ def manager_thread():
start_managed_process("uploader")
# Attempt to send pending messages if there's any that queued while offline
- tinklaClient.attemptToSendPendingMessages()
+ # Seems this loop runs every second or so, throttle to once every 30s
+ now = time.time()
+ if now - last_tinklad_send_attempt_time >= 30:
+ tinklaClient.attemptToSendPendingMessages()
+ last_tinklad_send_attempt_time = now
if msg.thermal.freeSpace < 0.05:
logger_dead = True
diff --git a/selfdrive/registration.py b/selfdrive/registration.py
index 1b1705fe5116c6..e3f1ab72815e57 100644
--- a/selfdrive/registration.py
+++ b/selfdrive/registration.py
@@ -55,14 +55,6 @@ def get_subscriber_info():
return ""
return ret
-def get_git_remote():
- try:
- local_branch = subprocess.run(["git", "name-rev", "--name-only", "HEAD"], capture_output=True).stdout.decode("utf-8").strip()
- tracking_remote = subprocess.run(["git", "config", "branch."+local_branch+".remote"],capture_output=True).stdout.decode("utf-8").strip()
- return subprocess.run(["git", "config", "remote."+tracking_remote+".url"],capture_output=True).stdout.decode("utf-8").strip()
- except subprocess.CalledProcessError:
- return ""
-
def register():
params = Params()
params.put("Version", version)
diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit
index 557be7d88d35e9..780589159b43ac 100644
--- a/selfdrive/test/process_replay/ref_commit
+++ b/selfdrive/test/process_replay/ref_commit
@@ -1 +1 @@
-8def7e7391802f2e86498d764c953f487361f6a1
\ No newline at end of file
+a64d824f47e2f794512f1c9439a04e242e3d1676
\ No newline at end of file
diff --git a/selfdrive/test/test_car_models.py b/selfdrive/test/test_car_models.py
index fc4605f94acdf7..ab95fe7d0aafad 100755
--- a/selfdrive/test/test_car_models.py
+++ b/selfdrive/test/test_car_models.py
@@ -281,6 +281,11 @@ def get_route_logs(route_name):
'enableCamera': True,
'enableDsu': True,
},
+ "e6a24be49a6cd46e|2019-10-29--10-52-42": {
+ 'carFingerprint': TOYOTA.LEXUS_ES_TSS2,
+ 'enableCamera': True,
+ 'enableDsu': True,
+ },
"f49e8041283f2939|2019-05-29--13-48-33": {
'carFingerprint': TOYOTA.LEXUS_ESH_TSS2,
'enableCamera': False,
diff --git a/selfdrive/thermald.py b/selfdrive/thermald.py
old mode 100755
new mode 100644
index f728b310742e3d..20291a20acaf00
--- a/selfdrive/thermald.py
+++ b/selfdrive/thermald.py
@@ -162,7 +162,6 @@ def thermald_thread():
off_ts = None
started_ts = None
- ignition_seen = False
started_seen = False
thermal_status = ThermalStatus.green
thermal_status_prev = ThermalStatus.green
@@ -188,7 +187,6 @@ def thermald_thread():
# clear car params when panda gets disconnected
if health is None and health_prev is not None:
params.panda_disconnect()
- ignition_seen = False
health_prev = health
if health is not None:
@@ -273,12 +271,6 @@ def thermald_thread():
# start constellation of processes when the car starts
ignition = health is not None and health.health.started
- # print "Ignition from panda: ", ignition
- ignition_seen = ignition_seen or ignition
-
- # add voltage check for ignition
- #if not ignition_seen and health is not None and health.health.voltage > 13500:
- # ignition = True
do_uninstall = params.get("DoUninstall") == b"1"
accepted_terms = params.get("HasAcceptedTerms") == terms_version
diff --git a/selfdrive/tinklad/airtable_publisher.py b/selfdrive/tinklad/airtable_publisher.py
index 0d17224f37dba6..2ff3f776498fb3 100644
--- a/selfdrive/tinklad/airtable_publisher.py
+++ b/selfdrive/tinklad/airtable_publisher.py
@@ -1,3 +1,7 @@
+#!/usr/bin/env python3
+# Created by Raf 5/2019
+
+import asyncio
AIRTABLE_API_KEY = 'keyvqdsl2VIIr9Q2A'
AIRTABLE_BASE_ID = 'appht7GB4aJS2A0LD'
@@ -34,7 +38,7 @@ class Publisher():
latest_info_dict = None # current info published
userRecordId = None
- def send_info(self, info, isData= False):
+ async def send_info(self, info, isData= False):
data_dict = None
if isData:
data_dict = info
@@ -42,23 +46,23 @@ def send_info(self, info, isData= False):
data_dict = self.__generate_airtable_user_info_dict(info)
# Early return if no changes
- if self.latest_info_dict != None:
+ if self.latest_info_dict is not None:
print(LOG_PREFIX + "latest_info. data=%s" % (self.latest_info_dict))
if data_dict == self.latest_info_dict:
print(LOG_PREFIX + "send_info no update necessary*")
return
print(LOG_PREFIX + "Sending info. data=%s" % (data_dict))
- if self.userRecordId != None:
- self.__update_user(data_dict)
+ if self.userRecordId is not None:
+ await self.__update_user(data_dict)
- if info.openPilotId != None and info.openPilotId != '':
+ if (info.openPilotId is not None) and info.openPilotId != '':
self.openPilotId = info.openPilotId
- response = self.at.get(USERS_TABLE, limit=1, filter_by_formula=("{openPilotId} = '%s'" % (self.openPilotId)))
+ response = await self.at.get(USERS_TABLE, limit=1, filter_by_formula=("{openPilotId} = '%s'" % (self.openPilotId)))
if self.__is_notfound_response(response): # Not found, create:
print(LOG_PREFIX + "Creating record for openPilotId='%s'" % (info.openPilotId))
- response = self.at.create(USERS_TABLE, data_dict)
+ response = await self.at.create(USERS_TABLE, data_dict)
if self.__is_error_response(response):
raise Exception(response)
elif self.__is_error_response(response): #Unsupported error
@@ -66,25 +70,24 @@ def send_info(self, info, isData= False):
raise Exception(response)
else:
self.userRecordId = response["records"][0]["id"]
- self.__update_user(data_dict)
+ await self.__update_user(data_dict)
self.latest_info_dict = data_dict
print(LOG_PREFIX + "*send_info competed*")
- def send_event(self, event):
- if self.openPilotId is None and self.latest_info_dict != None:
+ async def send_event(self, event):
+ if self.openPilotId is None and self.latest_info_dict is not None:
self.openPilotId = self.latest_info_dict[self.userKeys.openPilotId]
event_dict = self.__generate_airtable_user_event_dict(event)
print(LOG_PREFIX + "Sending event. data=%s" % (event_dict))
- response = self.at.create(EVENTS_TABLE, event_dict)
+ response = await self.at.create(EVENTS_TABLE, event_dict)
if self.__is_error_response(response):
print(LOG_PREFIX + "Error sending airtable event. %s" % (response))
raise Exception(response)
print(LOG_PREFIX + "*send_event competed*")
-
def __generate_airtable_user_info_dict(self, info):
dictionary = info.to_dict()
dictionary.pop(self.userKeys.timestamp, None)
@@ -100,29 +103,29 @@ def __generate_airtable_user_event_dict(self, event):
value = event.value.intValue
elif value == self.eventValueTypes.floatValue:
value = event.value.floatValue
- openPilotId = self.openPilotId if (self.openPilotId != None) else ""
+ openPilotId = event.openPilotId if (event.openPilotId is not None) else (self.openPilotId if (self.openPilotId is not None) else "")
dictionary = event.to_dict()
dictionary[self.eventKeys.value] = value
dictionary[self.eventKeys.openPilotId] = openPilotId
# dictionary.pop("timestamp", None)
return dictionary
- def __update_user(self, data):
+ async def __update_user(self, data):
print(LOG_PREFIX + "Updating userRecordId='%s'" % (self.userRecordId))
- response = self.at.update(USERS_TABLE, self.userRecordId, data)
+ response = await self.at.update(USERS_TABLE, self.userRecordId, data)
if self.__is_error_response(response):
raise Exception(response)
def __is_notfound_response(self, response):
try:
- return response["error"] != None and response["error"]["code"] == 422
+ return response["error"] is not None and response["error"]["code"] == 422
except: # pylint: disable=bare-except
- count = response["records"].__len__()
- return count == 0
+ count = response["records"].__len__()
+ return count == 0
def __is_error_response(self, response):
try:
- return response["error"] != None
+ return response["error"] is not None
except: # pylint: disable=bare-except
return False
@@ -176,7 +179,7 @@ def create_payload(data):
return {'fields': data}
-class Airtable(object):
+class Airtable():
def __init__(self, base_id, api_key, dict_class=OrderedDict):
"""Create a client to connect to an Airtable Base.
@@ -193,14 +196,28 @@ def __init__(self, base_id, api_key, dict_class=OrderedDict):
self.headers = {'Authorization': 'Bearer %s' % api_key}
self._dict_class = dict_class
- def __request(self, method, url, params=None, payload=None):
+ def __perform_request(self, method, url, params, data, headers):
+ return requests.request(
+ method,
+ url,
+ params=params,
+ data=data,
+ headers=headers
+ )
+
+ async def __request(self, method, url, params=None, payload=None):
if method in ['POST', 'PUT', 'PATCH']:
self.headers.update({'Content-type': 'application/json'})
- r = requests.request(method,
- posixpath.join(self.base_url, url),
- params=params,
- data=payload,
- headers=self.headers)
+ loop = asyncio.get_event_loop()
+ r = await loop.run_in_executor(
+ None,
+ self.__perform_request,
+ method,
+ posixpath.join(self.base_url, url),
+ params,
+ payload,
+ self.headers
+ )
if r.status_code == requests.codes.ok: # pylint: disable=no-member
return r.json(object_pairs_hook=self._dict_class)
else:
@@ -272,27 +289,27 @@ def iterate( # pylint: disable=dangerous-default-value
else:
break
- def create(self, table_name, data): # pylint: disable=inconsistent-return-statements
+ async def create(self, table_name, data): # pylint: disable=inconsistent-return-statements
if check_string(table_name):
payload = create_payload(data)
- return self.__request('POST', table_name,
+ return await self.__request('POST', table_name,
payload=json.dumps(payload))
- def update(self, table_name, record_id, data): # pylint: disable=inconsistent-return-statements
+ async def update(self, table_name, record_id, data): # pylint: disable=inconsistent-return-statements
if check_string(table_name) and check_string(record_id):
url = posixpath.join(table_name, record_id)
payload = create_payload(data)
- return self.__request('PATCH', url,
+ return await self.__request('PATCH', url,
payload=json.dumps(payload))
- def update_all(self, table_name, record_id, data): # pylint: disable=inconsistent-return-statements
+ async def update_all(self, table_name, record_id, data): # pylint: disable=inconsistent-return-statements
if check_string(table_name) and check_string(record_id):
url = posixpath.join(table_name, record_id)
payload = create_payload(data)
- return self.__request('PUT', url,
+ return await self.__request('PUT', url,
payload=json.dumps(payload))
- def delete(self, table_name, record_id): # pylint: disable=inconsistent-return-statements
+ async def delete(self, table_name, record_id): # pylint: disable=inconsistent-return-statements
if check_string(table_name) and check_string(record_id):
url = posixpath.join(table_name, record_id)
- return self.__request('DELETE', url)
+ return await self.__request('DELETE', url)
diff --git a/selfdrive/tinklad/pqueue.py b/selfdrive/tinklad/pqueue.py
index 691cc88d3b8694..de5c241e42931d 100644
--- a/selfdrive/tinklad/pqueue.py
+++ b/selfdrive/tinklad/pqueue.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python3.7
+#!/usr/bin/env python3
#
# https://github.com/balena/python-pqueue
@@ -73,7 +73,7 @@ def _destroy(self):
shutil.rmtree(self.path)
os.makedirs(self.path)
- def _qsize(self, len=len): # pylint: disable=redefined-builtin
+ def _qsize(self, len=len): # pylint: disable=redefined-builtin,arguments-differ
return self.info['size']
def _put(self, item):
diff --git a/selfdrive/tinklad/tinkla_interface.py b/selfdrive/tinklad/tinkla_interface.py
index e6768913b53ea5..cc117bf4a37550 100644
--- a/selfdrive/tinklad/tinkla_interface.py
+++ b/selfdrive/tinklad/tinkla_interface.py
@@ -1,11 +1,11 @@
-#!/usr/bin/env python3.7
+#!/usr/bin/env python3
+# Created by Raf 5/2019
from cereal import tinkla
import os
import zmq
import datetime
-#import tinklad
-from selfdrive.tinklad.tinklad import TinklaInterfaceEventCategoryKeys, TinklaInterfaceMessageKeys, TinklaInterfaceActions
+from selfdrive.tinklad import tinklad
import time
## For helpers:
@@ -13,6 +13,7 @@
from selfdrive.car.tesla.readconfig import CarSettings
from common.params import Params
+LOG_PREFIX = "tinklad client: "
tinklaClient = None
@@ -25,9 +26,9 @@ class TinklaClient():
lastCanErrorTimestamp = 0
lastProcessErrorTimestamp = 0
- eventCategoryKeys = TinklaInterfaceEventCategoryKeys()
- messageTypeKeys = TinklaInterfaceMessageKeys()
- actions = TinklaInterfaceActions()
+ eventCategoryKeys = tinklad.TinklaInterfaceEventCategoryKeys()
+ messageTypeKeys = tinklad.TinklaInterfaceMessageKeys()
+ actions = tinklad.TinklaInterfaceActions()
# Configurable:
# Note: If throttling, events are dropped
@@ -50,7 +51,6 @@ def start_client(self):
print("Unable to connect to tinklad")
self.sock = None
-
def setUserInfo(self, info):
self.start_client()
if self.sock is None:
@@ -167,8 +167,14 @@ def print_msg(self, message):
print(message)
def __init__(self):
- params = Params()
- self.carSettings = CarSettings()
+ try:
+ params = Params()
+ except OSError:
+ params = Params(db="./params")
+ try:
+ self.carSettings = CarSettings()
+ except IOError:
+ self.carSettings = CarSettings(optional_config_file_path="./bb_openpilot.cfg")
self.openPilotId = params.get("DongleId")
self.userHandle = self.carSettings.userHandle
self.gitRemote = params.get("GitRemote")
diff --git a/selfdrive/tinklad/tinklad.py b/selfdrive/tinklad/tinklad.py
index aa6c22cfac8c7f..6a046748be2101 100644
--- a/selfdrive/tinklad/tinklad.py
+++ b/selfdrive/tinklad/tinklad.py
@@ -1,4 +1,5 @@
-#!/usr/bin/env python3.7
+#!/usr/bin/env python3
+# Created by Raf 5/2019
import zmq
import cereal
@@ -6,7 +7,8 @@
from selfdrive.tinklad.airtable_publisher import Publisher
import requests
import time
-import os
+import os
+import asyncio
LOG_PREFIX = "tinklad: "
@@ -73,7 +75,7 @@ class TinklaServer():
last_attempt_time = 0
- def attemptToSendPendingMessages(self):
+ async def attemptToSendPendingMessages(self):
# Throttle to once per minute
now = time.time()
if now - self.last_attempt_time < 60:
@@ -85,14 +87,15 @@ def attemptToSendPendingMessages(self):
if not self.isOnline():
return
print(LOG_PREFIX + "Attempting to send pending messages")
- self.publish_pending_userinfo()
- self.publish_pending_events()
+ await self.publish_pending_userinfo()
+ await self.publish_pending_events()
- def setUserInfo(self, info, **kwargs):
+ async def setUserInfo(self, info, **kwargs):
+ print(LOG_PREFIX + "Pushing user info to cache")
self.userInfoCache.push(info)
- self.publish_pending_userinfo()
+ await self.publish_pending_userinfo()
- def publish_pending_userinfo(self):
+ async def publish_pending_userinfo(self):
if self.userInfoCache.count() == 0:
return
@@ -107,15 +110,17 @@ def publish_pending_userinfo(self):
print(LOG_PREFIX + "Sending info to publisher: %s" % (info.to_dict()))
self.info = info
try:
- self.publisher.send_info(info)
+ await self.publisher.send_info(info)
+ self.userInfoCache.task_done()
except Exception as error: # pylint: disable=broad-except
- print(LOG_PREFIX + "Error attempting to publish user info (%s)" % (error))
+ self.userInfoCache.push(info)
+ print(LOG_PREFIX + "Error attempting to publish user info (%s) (Cache has %d elements)" % (error, self.userInfoCache.count()))
- def logUserEvent(self, event, **kwargs):
+ async def logUserEvent(self, event, **kwargs):
self.eventCache.push(event)
- self.publish_pending_events()
+ await self.publish_pending_events()
- def publish_pending_events(self):
+ async def publish_pending_events(self):
if self.eventCache.count() > 0:
print(LOG_PREFIX + 'Cache has %d elements, attempting to publish...' %(self.eventCache.count()))
@@ -126,7 +131,7 @@ def publish_pending_events(self):
return
try:
print(LOG_PREFIX + "Sending event to publisher: %s" % (event.to_dict()))
- self.publisher.send_event(event)
+ await self.publisher.send_event(event)
self.eventCache.task_done()
except AssertionError as error:
self.eventCache.push(event)
@@ -147,42 +152,52 @@ def isOnline(self):
return False
return False
- def __init__(self):
- self.publisher = Publisher()
- # set persitent cache for bad network / offline
- self.eventCache = Cache("events")
- self.userInfoCache = Cache("user_info")
- self.publish_pending_userinfo()
- self.publish_pending_events()
+ async def messageLoop(self, sock):
messageKeys = TinklaInterfaceMessageKeys()
actions = TinklaInterfaceActions()
- # Start server:
- ctx = zmq.Context()
- sock = ctx.socket(zmq.PULL)
- sock.bind("ipc:///tmp/tinklad")
- context = zmq.Context()
-
while True:
- tmp = sock.recv_multipart()
data = b''.join(sock.recv_multipart())
+ #print(LOG_PREFIX + "Received Data: " + repr(data) + "'")
tinklaInterface = cereal.tinkla.Interface.from_bytes(data)
if tinklaInterface.version != cereal.tinkla.interfaceVersion:
print(LOG_PREFIX + "Unsupported message version: %0.2f (supported version: %0.2f)" % (tinklaInterface.version, cereal.tinkla.interfaceVersion))
continue
messageType = tinklaInterface.message.which()
+ #if messageType != messageKeys.action:
+ print(LOG_PREFIX + "> Received message. Type: '%s'" % messageType)
if messageType == messageKeys.userInfo:
info = tinklaInterface.message.userInfo
- self.setUserInfo(info)
+ await self.setUserInfo(info)
elif messageType == messageKeys.event:
event = tinklaInterface.message.event
- self.logUserEvent(event)
+ await self.logUserEvent(event)
elif messageType == messageKeys.action:
if tinklaInterface.message.action == actions.attemptToSendPendingMessages:
- self.attemptToSendPendingMessages()
+ await self.attemptToSendPendingMessages()
else:
print(LOG_PREFIX + "Unsupported action: %s" % tinklaInterface.message.action)
+ def __init__(self):
+ loop = asyncio.get_event_loop()
+ self.publisher = Publisher()
+ # set persitent cache for bad network / offline
+ self.eventCache = Cache("events")
+ self.userInfoCache = Cache("user_info")
+ tasks = [
+ self.publish_pending_userinfo(),
+ self.publish_pending_events()
+ ]
+ loop.run_until_complete(asyncio.wait(tasks))
+
+ # Start server:
+ ctx = zmq.Context()
+ sock = ctx.socket(zmq.PULL)
+ sock.bind("ipc:///tmp/tinklad")
+ context = zmq.Context()
+
+ loop.run_until_complete(self.messageLoop(sock=sock))
+
def main(gctx=None):
print("Starting tinklad service ...")
@@ -190,5 +205,4 @@ def main(gctx=None):
if __name__ == "__main__":
- main()
-
\ No newline at end of file
+ main()
diff --git a/selfdrive/tinklad/tinkladTestClient.py b/selfdrive/tinklad/tinkladTestClient.py
index 1b0be87d8964e4..6da4a4460890b8 100644
--- a/selfdrive/tinklad/tinkladTestClient.py
+++ b/selfdrive/tinklad/tinkladTestClient.py
@@ -1,4 +1,5 @@
-#!/usr/bin/env python2.7
+#!/usr/bin/env python3
+# Created by Raf 5/2019
from cereal import tinkla
from tinkla_interface import TinklaClient
@@ -48,15 +49,25 @@ def __init__(self):
print("send crash log")
self.tinklaClient.logCrashStackTraceEvent(openPilotId=openPilotId)
- print("send can error")
- self.tinklaClient.logCANErrorEvent(source=source, canMessage=1, additionalInformation="test can error logging", openPilotId=openPilotId)
- time.sleep(1)
- self.tinklaClient.logCANErrorEvent(source=source, canMessage=2, additionalInformation="test can error logging", openPilotId=openPilotId)
+ print("send user info 2")
+ info = tinkla.Interface.UserInfo.new_message(
+ openPilotId=openPilotId,
+ userHandle=userHandle + "2",
+ gitRemote="test_github.com/something",
+ gitBranch="test_gitbranch",
+ gitHash="test_123456"
+ )
+ self.tinklaClient.setUserInfo(info)
+
+ #print("send can error")
+ #self.tinklaClient.logCANErrorEvent(source=source, canMessage=1, additionalInformation="test can error logging", openPilotId=openPilotId)
+ #time.sleep(1)
+ #self.tinklaClient.logCANErrorEvent(source=source, canMessage=2, additionalInformation="test can error logging", openPilotId=openPilotId)
- print("send process comm error")
- self.tinklaClient.logProcessCommErrorEvent(source=source, processName="processNameWouldBeHere1", count=10, eventType="Not Alive", openPilotId=openPilotId)
- time.sleep(1)
- self.tinklaClient.logProcessCommErrorEvent(source=source, processName="processNameWouldBeHere2", count=10, eventType="Not Alive", openPilotId=openPilotId)
+ #print("send process comm error")
+ #self.tinklaClient.logProcessCommErrorEvent(source=source, processName="processNameWouldBeHere1", count=10, eventType="Not Alive", openPilotId=openPilotId)
+ #time.sleep(1)
+ #self.tinklaClient.logProcessCommErrorEvent(source=source, processName="processNameWouldBeHere2", count=10, eventType="Not Alive", openPilotId=openPilotId)
if __name__ == "__main__":
TinklaTestClient()
diff --git a/selfdrive/tinklad/tinkladTestClient.sh b/selfdrive/tinklad/tinkladTestClient.sh
index 3c26accdcae1eb..32f70dfd4936e3 100755
--- a/selfdrive/tinklad/tinkladTestClient.sh
+++ b/selfdrive/tinklad/tinkladTestClient.sh
@@ -1 +1 @@
-PYTHONPATH="../../" python2.7 tinkladTestClient.py
+PYTHONPATH="../../" python3 tinkladTestClient.py
diff --git a/selfdrive/tinklad/tinkladTestServer.sh b/selfdrive/tinklad/tinkladTestServer.sh
index 4a7a9d1d071d82..57b366f9ea14a3 100755
--- a/selfdrive/tinklad/tinkladTestServer.sh
+++ b/selfdrive/tinklad/tinkladTestServer.sh
@@ -1,2 +1,2 @@
mkdir ./tinklad-cache 2> /dev/null
-PYTHONPATH="../../" python2.7 tinklad.py
+PYTHONPATH="../../" python3 tinklad.py