Vehicle Researcher
6 years ago
committed by
Willem Melching
parent
b4f4b92ecf
commit
d7f0b402a8
148 changed files with 4191 additions and 3323 deletions
File diff suppressed because it is too large
Load Diff
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@ -1,16 +1,19 @@ |
||||
#!/bin/bash |
||||
set -e |
||||
|
||||
SETUP="cd /tmp/openpilot && make -C cereal && " |
||||
|
||||
docker build -t tmppilot -f Dockerfile.openpilot . |
||||
|
||||
docker run --rm tmppilot /bin/sh -c 'cd /tmp/openpilot/ && make -C cereal && cd /tmp/openpilot/selfdrive/test/ && ./test_fingerprints.py' |
||||
docker run --rm tmppilot /bin/sh -c 'cd /tmp/openpilot/ && pyflakes $(find . -iname "*.py" | grep -vi "^\./pyextra.*" | grep -vi "^\./panda" | grep -vi "^\./tools")' |
||||
|
||||
docker run --rm tmppilot /bin/sh -c "$SETUP cd /tmp/openpilot/selfdrive/test/ && ./test_fingerprints.py" |
||||
docker run --rm tmppilot /bin/sh -c 'cd /tmp/openpilot/ && flake8 --select=F $(find . -iname "*.py" | grep -vi "^\./pyextra.*" | grep -vi "^\./panda" | grep -vi "^\./tools")' |
||||
docker run --rm tmppilot /bin/sh -c 'cd /tmp/openpilot/ && pylint $(find . -iname "*.py" | grep -vi "^\./pyextra.*" | grep -vi "^\./panda" | grep -vi "^\./tools"); exit $(($? & 3))' |
||||
docker run --rm tmppilot /bin/sh -c 'cd /tmp/openpilot/ && make -C cereal && python -m unittest discover common' |
||||
docker run --rm tmppilot /bin/sh -c 'cd /tmp/openpilot/ && make -C cereal && python -m unittest discover selfdrive/can' |
||||
docker run --rm tmppilot /bin/sh -c 'cd /tmp/openpilot/ && make -C cereal && python -m unittest discover selfdrive/boardd' |
||||
docker run --rm tmppilot /bin/sh -c 'cd /tmp/openpilot/ && make -C cereal && python -m unittest discover selfdrive/controls' |
||||
docker run --rm tmppilot /bin/sh -c 'cd /tmp/openpilot/ && python -m unittest discover selfdrive/loggerd' |
||||
docker run --rm -v "$(pwd)"/selfdrive/test/longitudinal_maneuvers/out:/tmp/openpilot/selfdrive/test/longitudinal_maneuvers/out tmppilot /bin/sh -c 'cd /tmp/openpilot/selfdrive/test/longitudinal_maneuvers && OPTEST=1 ./test_longitudinal.py' |
||||
docker run --rm tmppilot /bin/sh -c 'cd /tmp/openpilot/ && make -C cereal && cd /tmp/openpilot/selfdrive/test/process_replay/ && ./test_processes.py' |
||||
docker run --rm tmppilot /bin/sh -c 'mkdir -p /data/params && cd /tmp/openpilot/ && make -C cereal && cd /tmp/openpilot/selfdrive/test/ && ./test_car_models.py' |
||||
docker run --rm tmppilot /bin/sh -c "$SETUP python -m unittest discover common" |
||||
docker run --rm tmppilot /bin/sh -c "$SETUP make -C selfdrive/can -j4 && python -m unittest discover selfdrive/can" |
||||
docker run --rm tmppilot /bin/sh -c "$SETUP python -m unittest discover selfdrive/boardd" |
||||
docker run --rm tmppilot /bin/sh -c "$SETUP make -C selfdrive/can -j4 && python -m unittest discover selfdrive/controls" |
||||
docker run --rm tmppilot /bin/sh -c "$SETUP python -m unittest discover selfdrive/loggerd" |
||||
docker run --rm -v "$(pwd)"/selfdrive/test/longitudinal_maneuvers/out:/tmp/openpilot/selfdrive/test/longitudinal_maneuvers/out tmppilot /bin/sh -c "$SETUP make -C selfdrive/can -j4 && cd /tmp/openpilot/selfdrive/test/longitudinal_maneuvers && OPTEST=1 ./test_longitudinal.py" |
||||
docker run --rm tmppilot /bin/sh -c "$SETUP make -C selfdrive/can -j4 && cd /tmp/openpilot/selfdrive/test/process_replay/ && ./test_processes.py" |
||||
docker run --rm tmppilot /bin/sh -c "$SETUP make -C selfdrive/can -j4 && mkdir -p /data/params && cd /tmp/openpilot/selfdrive/test/ && ./test_car_models.py" |
||||
|
||||
@ -0,0 +1,36 @@ |
||||
#!/usr/bin/env python3 |
||||
|
||||
import time |
||||
from multiprocessing import Process |
||||
|
||||
import selfdrive.crash as crash |
||||
from common.params import Params |
||||
from selfdrive.manager import launcher |
||||
from selfdrive.swaglog import cloudlog |
||||
from selfdrive.version import version, dirty |
||||
|
||||
ATHENA_MGR_PID_PARAM = "AthenadPid" |
||||
|
||||
def main(): |
||||
params = Params() |
||||
dongle_id = params.get("DongleId").decode('utf-8') |
||||
cloudlog.bind_global(dongle_id=dongle_id, version=version, dirty=dirty, is_eon=True) |
||||
crash.bind_user(id=dongle_id) |
||||
crash.bind_extra(version=version, dirty=dirty, is_eon=True) |
||||
crash.install() |
||||
|
||||
try: |
||||
while 1: |
||||
cloudlog.info("starting athena daemon") |
||||
proc = Process(name='athenad', target=launcher, args=('selfdrive.athena.athenad',)) |
||||
proc.start() |
||||
proc.join() |
||||
cloudlog.event("athenad exited", exitcode=proc.exitcode) |
||||
time.sleep(5) |
||||
except: |
||||
cloudlog.exception("manage_athenad.exception") |
||||
finally: |
||||
params.delete(ATHENA_MGR_PID_PARAM) |
||||
|
||||
if __name__ == '__main__': |
||||
main() |
||||
@ -0,0 +1,165 @@ |
||||
#include "common.h" |
||||
|
||||
unsigned int honda_checksum(unsigned int address, uint64_t d, int l) { |
||||
d >>= ((8-l)*8); // remove padding
|
||||
d >>= 4; // remove checksum
|
||||
|
||||
int s = 0; |
||||
while (address) { s += (address & 0xF); address >>= 4; } |
||||
while (d) { s += (d & 0xF); d >>= 4; } |
||||
s = 8-s; |
||||
s &= 0xF; |
||||
|
||||
return s; |
||||
} |
||||
|
||||
unsigned int toyota_checksum(unsigned int address, uint64_t d, int l) { |
||||
d >>= ((8-l)*8); // remove padding
|
||||
d >>= 8; // remove checksum
|
||||
|
||||
unsigned int s = l; |
||||
while (address) { s += address & 0xff; address >>= 8; } |
||||
while (d) { s += d & 0xff; d >>= 8; } |
||||
|
||||
return s & 0xFF; |
||||
} |
||||
|
||||
// Static lookup table for fast computation of CRC8 poly 0x2F, aka 8H2F/AUTOSAR
|
||||
uint8_t crc8_lut_8h2f[256]; |
||||
|
||||
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
|
||||
printf("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
|
||||
} |
||||
|
||||
|
||||
unsigned int pedal_checksum(uint64_t d, int l) { |
||||
uint8_t crc = 0xFF; |
||||
uint8_t poly = 0xD5; // standard crc8
|
||||
|
||||
d >>= ((8-l)*8); // remove padding
|
||||
d >>= 8; // remove checksum
|
||||
|
||||
uint8_t *dat = (uint8_t *)&d; |
||||
|
||||
int i, j; |
||||
for (i = 0; i < l - 1; i++) { |
||||
crc ^= dat[i]; |
||||
for (j = 0; j < 8; j++) { |
||||
if ((crc & 0x80) != 0) { |
||||
crc = (uint8_t)((crc << 1) ^ poly); |
||||
} |
||||
else { |
||||
crc <<= 1; |
||||
} |
||||
} |
||||
} |
||||
return crc; |
||||
} |
||||
|
||||
|
||||
uint64_t read_u64_be(const uint8_t* v) { |
||||
return (((uint64_t)v[0] << 56) |
||||
| ((uint64_t)v[1] << 48) |
||||
| ((uint64_t)v[2] << 40) |
||||
| ((uint64_t)v[3] << 32) |
||||
| ((uint64_t)v[4] << 24) |
||||
| ((uint64_t)v[5] << 16) |
||||
| ((uint64_t)v[6] << 8) |
||||
| (uint64_t)v[7]); |
||||
} |
||||
|
||||
uint64_t read_u64_le(const uint8_t* v) { |
||||
return ((uint64_t)v[0] |
||||
| ((uint64_t)v[1] << 8) |
||||
| ((uint64_t)v[2] << 16) |
||||
| ((uint64_t)v[3] << 24) |
||||
| ((uint64_t)v[4] << 32) |
||||
| ((uint64_t)v[5] << 40) |
||||
| ((uint64_t)v[6] << 48) |
||||
| ((uint64_t)v[7] << 56)); |
||||
} |
||||
@ -0,0 +1,71 @@ |
||||
# distutils: language = c++ |
||||
#cython: language_level=3 |
||||
|
||||
from libc.stdint cimport uint32_t, uint64_t, uint16_t |
||||
from libcpp.vector cimport vector |
||||
from libcpp.map cimport map |
||||
from libcpp.string cimport string |
||||
from libcpp.unordered_set cimport unordered_set |
||||
from libcpp cimport bool |
||||
|
||||
|
||||
cdef extern from "common.h": |
||||
|
||||
ctypedef enum SignalType: |
||||
DEFAULT, |
||||
HONDA_CHECKSUM, |
||||
HONDA_COUNTER, |
||||
TOYOTA_CHECKSUM, |
||||
PEDAL_CHECKSUM, |
||||
PEDAL_COUNTER |
||||
|
||||
cdef struct Signal: |
||||
const char* name |
||||
int b1, b2, bo |
||||
bool is_signed |
||||
double factor, offset |
||||
SignalType type |
||||
|
||||
cdef struct Msg: |
||||
const char* name |
||||
uint32_t address |
||||
unsigned int size |
||||
size_t num_sigs |
||||
const Signal *sigs |
||||
|
||||
cdef struct Val: |
||||
const char* name |
||||
uint32_t address |
||||
const char* def_val |
||||
const Signal *sigs |
||||
|
||||
cdef struct DBC: |
||||
const char* name |
||||
size_t num_msgs |
||||
const Msg *msgs |
||||
const Val *vals |
||||
size_t num_vals |
||||
|
||||
cdef struct SignalParseOptions: |
||||
uint32_t address |
||||
const char* name |
||||
double default_value |
||||
|
||||
|
||||
cdef struct MessageParseOptions: |
||||
uint32_t address |
||||
int check_frequency |
||||
|
||||
cdef struct SignalValue: |
||||
uint32_t address |
||||
uint16_t ts |
||||
const char* name |
||||
double value |
||||
|
||||
cdef const DBC* dbc_lookup(const string); |
||||
|
||||
cdef cppclass CANParser: |
||||
bool can_valid |
||||
CANParser(int, string, vector[MessageParseOptions], vector[SignalParseOptions]) |
||||
void update_string(string) |
||||
vector[SignalValue] query_latest() |
||||
@ -1,98 +0,0 @@ |
||||
# distutils: language = c++ |
||||
#cython: language_level=3 |
||||
|
||||
from libc.stdint cimport uint32_t, uint64_t, uint16_t |
||||
from libcpp.vector cimport vector |
||||
from libcpp.map cimport map |
||||
from libcpp.string cimport string |
||||
from libcpp.unordered_set cimport unordered_set |
||||
from libcpp cimport bool |
||||
|
||||
ctypedef enum SignalType: |
||||
DEFAULT, |
||||
HONDA_CHECKSUM, |
||||
HONDA_COUNTER, |
||||
TOYOTA_CHECKSUM, |
||||
PEDAL_CHECKSUM, |
||||
PEDAL_COUNTER, |
||||
VOLKSWAGEN_CHECKSUM, |
||||
VOLKSWAGEN_COUNTER |
||||
|
||||
cdef struct Signal: |
||||
const char* name |
||||
int b1, b2, bo |
||||
bool is_signed |
||||
double factor, offset |
||||
SignalType type |
||||
|
||||
|
||||
|
||||
cdef struct Msg: |
||||
const char* name |
||||
uint32_t address |
||||
unsigned int size |
||||
size_t num_sigs |
||||
const Signal *sigs |
||||
|
||||
cdef struct Val: |
||||
const char* name |
||||
uint32_t address |
||||
const char* def_val |
||||
const Signal *sigs |
||||
|
||||
cdef struct DBC: |
||||
const char* name |
||||
size_t num_msgs |
||||
const Msg *msgs |
||||
const Val *vals |
||||
size_t num_vals |
||||
|
||||
cdef struct SignalParseOptions: |
||||
uint32_t address |
||||
const char* name |
||||
double default_value |
||||
|
||||
|
||||
cdef struct MessageParseOptions: |
||||
uint32_t address |
||||
int check_frequency |
||||
|
||||
cdef struct SignalValue: |
||||
uint32_t address |
||||
uint16_t ts |
||||
const char* name |
||||
double value |
||||
|
||||
ctypedef const DBC * (*dbc_lookup_func)(const char* dbc_name) |
||||
ctypedef void* (*can_init_with_vectors_func)(int bus, const char* dbc_name, |
||||
vector[MessageParseOptions] message_options, |
||||
vector[SignalParseOptions] signal_options, |
||||
bool sendcan, |
||||
const char* tcp_addr, |
||||
int timeout) |
||||
ctypedef int (*can_update_func)(void* can, uint64_t sec, bool wait); |
||||
ctypedef void (*can_update_string_func)(void* can, const char* dat, int len); |
||||
ctypedef size_t (*can_query_latest_func)(void* can, bool *out_can_valid, size_t out_values_size, SignalValue* out_values); |
||||
ctypedef void (*can_query_latest_vector_func)(void* can, bool *out_can_valid, vector[SignalValue] &values) |
||||
|
||||
cdef class CANParser: |
||||
cdef: |
||||
void *can |
||||
const DBC *dbc |
||||
dbc_lookup_func dbc_lookup |
||||
can_init_with_vectors_func can_init_with_vectors |
||||
can_update_func can_update |
||||
can_update_string_func can_update_string |
||||
can_query_latest_vector_func can_query_latest_vector |
||||
map[string, uint32_t] msg_name_to_address |
||||
map[uint32_t, string] address_to_msg_name |
||||
vector[SignalValue] can_values |
||||
bool test_mode_enabled |
||||
cdef public: |
||||
string dbc_name |
||||
dict vl |
||||
dict ts |
||||
bool can_valid |
||||
int can_invalid_cnt |
||||
|
||||
cdef unordered_set[uint32_t] update_vl(self) |
||||
@ -1,68 +0,0 @@ |
||||
import struct |
||||
from selfdrive.can.libdbc_py import libdbc, ffi |
||||
|
||||
|
||||
class CANPacker(): |
||||
def __init__(self, dbc_name): |
||||
dbc_name = dbc_name.encode('utf8') |
||||
self.packer = libdbc.canpack_init(dbc_name) |
||||
self.dbc = libdbc.dbc_lookup(dbc_name) |
||||
self.sig_names = {} |
||||
self.name_to_address_and_size = {} |
||||
|
||||
num_msgs = self.dbc[0].num_msgs |
||||
for i in range(num_msgs): |
||||
msg = self.dbc[0].msgs[i] |
||||
|
||||
name = ffi.string(msg.name).decode('utf8') |
||||
address = msg.address |
||||
self.name_to_address_and_size[name] = (address, msg.size) |
||||
self.name_to_address_and_size[address] = (address, msg.size) |
||||
|
||||
def pack(self, addr, values, counter): |
||||
values_thing = [] |
||||
for name, value in values.items(): |
||||
if name not in self.sig_names: |
||||
self.sig_names[name] = ffi.new("char[]", name.encode('utf8')) |
||||
|
||||
values_thing.append({ |
||||
'name': self.sig_names[name], |
||||
'value': value |
||||
}) |
||||
|
||||
values_c = ffi.new("SignalPackValue[]", values_thing) |
||||
|
||||
return libdbc.canpack_pack(self.packer, addr, len(values_thing), values_c, counter) |
||||
|
||||
def pack_bytes(self, addr, values, counter=-1): |
||||
addr, size = self.name_to_address_and_size[addr] |
||||
|
||||
val = self.pack(addr, values, counter) |
||||
r = struct.pack(">Q", val) |
||||
return addr, r[:size] |
||||
|
||||
def make_can_msg(self, addr, bus, values, counter=-1): |
||||
addr, msg = self.pack_bytes(addr, values, counter) |
||||
return [addr, 0, msg, bus] |
||||
|
||||
|
||||
if __name__ == "__main__": |
||||
## little endian test |
||||
cp = CANPacker("hyundai_santa_fe_2019_ccan") |
||||
s = cp.pack_bytes(0x340, { |
||||
"CR_Lkas_StrToqReq": -0.06, |
||||
#"CF_Lkas_FcwBasReq": 1, |
||||
"CF_Lkas_MsgCount": 7, |
||||
"CF_Lkas_HbaSysState": 0, |
||||
#"CF_Lkas_Chksum": 3, |
||||
}) |
||||
s = cp.pack_bytes(0x340, { |
||||
"CF_Lkas_MsgCount": 1, |
||||
}) |
||||
# big endian test |
||||
#cp = CANPacker("honda_civic_touring_2016_can_generated") |
||||
#s = cp.pack_bytes(0xe4, { |
||||
# "STEER_TORQUE": -2, |
||||
#}) |
||||
print([hex(ord(v)) for v in s[1]]) |
||||
print(s[1].encode("hex")) |
||||
@ -1,220 +0,0 @@ |
||||
import time |
||||
import numbers |
||||
|
||||
from selfdrive.can.libdbc_py import libdbc, ffi |
||||
|
||||
CAN_INVALID_CNT = 5 # after so many consecutive CAN data with wrong checksum, counter or frequency, flag CAN invalidity |
||||
|
||||
class CANParser(): |
||||
def __init__(self, dbc_name, signals, checks=None, bus=0, sendcan=False, tcp_addr="127.0.0.1", timeout=-1): |
||||
if checks is None: |
||||
checks = [] |
||||
|
||||
self.can_valid = True |
||||
self.can_invalid_cnt = CAN_INVALID_CNT |
||||
self.vl = {} |
||||
self.ts = {} |
||||
|
||||
self.dbc_name = dbc_name |
||||
self.dbc = libdbc.dbc_lookup(dbc_name.encode('utf8')) |
||||
self.msg_name_to_addres = {} |
||||
self.address_to_msg_name = {} |
||||
|
||||
num_msgs = self.dbc[0].num_msgs |
||||
for i in range(num_msgs): |
||||
msg = self.dbc[0].msgs[i] |
||||
|
||||
name = ffi.string(msg.name).decode('utf8') |
||||
address = msg.address |
||||
|
||||
self.msg_name_to_addres[name] = address |
||||
self.address_to_msg_name[address] = name |
||||
|
||||
self.vl[address] = {} |
||||
self.vl[name] = {} |
||||
self.ts[address] = {} |
||||
self.ts[name] = {} |
||||
|
||||
# Convert message names into addresses |
||||
for i in range(len(signals)): |
||||
s = signals[i] |
||||
if not isinstance(s[1], numbers.Number): |
||||
s = (s[0], self.msg_name_to_addres[s[1]], s[2]) |
||||
signals[i] = s |
||||
|
||||
for i in range(len(checks)): |
||||
c = checks[i] |
||||
if not isinstance(c[0], numbers.Number): |
||||
c = (self.msg_name_to_addres[c[0]], c[1]) |
||||
checks[i] = c |
||||
|
||||
sig_names = dict((name, ffi.new("char[]", name.encode('utf8'))) for name, _, _ in signals) |
||||
|
||||
signal_options_c = ffi.new("SignalParseOptions[]", [ |
||||
{ |
||||
'address': sig_address, |
||||
'name': sig_names[sig_name], |
||||
'default_value': sig_default, |
||||
} for sig_name, sig_address, sig_default in signals]) |
||||
|
||||
message_options = dict((address, 0) for _, address, _ in signals) |
||||
message_options.update(dict(checks)) |
||||
|
||||
message_options_c = ffi.new("MessageParseOptions[]", [ |
||||
{ |
||||
'address': msg_address, |
||||
'check_frequency': freq, |
||||
} for msg_address, freq in message_options.items()]) |
||||
|
||||
self.can = libdbc.can_init(bus, dbc_name.encode('utf8'), len(message_options_c), message_options_c, |
||||
len(signal_options_c), signal_options_c, sendcan, tcp_addr.encode('utf8'), timeout) |
||||
|
||||
self.p_can_valid = ffi.new("bool*") |
||||
|
||||
value_count = libdbc.can_query_latest(self.can, self.p_can_valid, 0, ffi.NULL) |
||||
self.can_values = ffi.new("SignalValue[%d]" % value_count) |
||||
self.update_vl(0) |
||||
|
||||
def update_vl(self, sec): |
||||
can_values_len = libdbc.can_query_latest(self.can, self.p_can_valid, len(self.can_values), self.can_values) |
||||
assert can_values_len <= len(self.can_values) |
||||
|
||||
self.can_invalid_cnt += 1 |
||||
if self.p_can_valid[0]: |
||||
self.can_invalid_cnt = 0 |
||||
self.can_valid = self.can_invalid_cnt < CAN_INVALID_CNT |
||||
|
||||
ret = set() |
||||
for i in range(can_values_len): |
||||
cv = self.can_values[i] |
||||
address = cv.address |
||||
# print("{0} {1}".format(hex(cv.address), ffi.string(cv.name))) |
||||
name = ffi.string(cv.name).decode('utf8') |
||||
self.vl[address][name] = cv.value |
||||
self.ts[address][name] = cv.ts |
||||
|
||||
sig_name = self.address_to_msg_name[address] |
||||
self.vl[sig_name][name] = cv.value |
||||
self.ts[sig_name][name] = cv.ts |
||||
ret.add(address) |
||||
return ret |
||||
|
||||
def update(self, sec, wait): |
||||
"""Returns if the update was successfull (e.g. no rcv timeout happened)""" |
||||
r = libdbc.can_update(self.can, sec, wait) |
||||
return bool(r >= 0), self.update_vl(sec) |
||||
|
||||
|
||||
if __name__ == "__main__": |
||||
from common.realtime import sec_since_boot |
||||
|
||||
radar_messages = range(0x430, 0x43A) + range(0x440, 0x446) |
||||
# signals = zip(['LONG_DIST'] * 16 + ['NEW_TRACK'] * 16 + ['LAT_DIST'] * 16 + |
||||
# ['REL_SPEED'] * 16, radar_messages * 4, |
||||
# [255] * 16 + [1] * 16 + [0] * 16 + [0] * 16) |
||||
# checks = zip(radar_messages, [20]*16) |
||||
|
||||
# cp = CANParser("acura_ilx_2016_nidec", signals, checks, 1) |
||||
|
||||
|
||||
# signals = [ |
||||
# ("XMISSION_SPEED", 0x158, 0), #sig_name, sig_address, default |
||||
# ("WHEEL_SPEED_FL", 0x1d0, 0), |
||||
# ("WHEEL_SPEED_FR", 0x1d0, 0), |
||||
# ("WHEEL_SPEED_RL", 0x1d0, 0), |
||||
# ("STEER_ANGLE", 0x14a, 0), |
||||
# ("STEER_TORQUE_SENSOR", 0x18f, 0), |
||||
# ("GEAR", 0x191, 0), |
||||
# ("WHEELS_MOVING", 0x1b0, 1), |
||||
# ("DOOR_OPEN_FL", 0x405, 1), |
||||
# ("DOOR_OPEN_FR", 0x405, 1), |
||||
# ("DOOR_OPEN_RL", 0x405, 1), |
||||
# ("DOOR_OPEN_RR", 0x405, 1), |
||||
# ("CRUISE_SPEED_PCM", 0x324, 0), |
||||
# ("SEATBELT_DRIVER_LAMP", 0x305, 1), |
||||
# ("SEATBELT_DRIVER_LATCHED", 0x305, 0), |
||||
# ("BRAKE_PRESSED", 0x17c, 0), |
||||
# ("CAR_GAS", 0x130, 0), |
||||
# ("CRUISE_BUTTONS", 0x296, 0), |
||||
# ("ESP_DISABLED", 0x1a4, 1), |
||||
# ("HUD_LEAD", 0x30c, 0), |
||||
# ("USER_BRAKE", 0x1a4, 0), |
||||
# ("STEER_STATUS", 0x18f, 5), |
||||
# ("WHEEL_SPEED_RR", 0x1d0, 0), |
||||
# ("BRAKE_ERROR_1", 0x1b0, 1), |
||||
# ("BRAKE_ERROR_2", 0x1b0, 1), |
||||
# ("GEAR_SHIFTER", 0x191, 0), |
||||
# ("MAIN_ON", 0x326, 0), |
||||
# ("ACC_STATUS", 0x17c, 0), |
||||
# ("PEDAL_GAS", 0x17c, 0), |
||||
# ("CRUISE_SETTING", 0x296, 0), |
||||
# ("LEFT_BLINKER", 0x326, 0), |
||||
# ("RIGHT_BLINKER", 0x326, 0), |
||||
# ("COUNTER", 0x324, 0), |
||||
# ("ENGINE_RPM", 0x17C, 0) |
||||
# ] |
||||
# checks = [ |
||||
# (0x14a, 100), # address, frequency |
||||
# (0x158, 100), |
||||
# (0x17c, 100), |
||||
# (0x191, 100), |
||||
# (0x1a4, 50), |
||||
# (0x326, 10), |
||||
# (0x1b0, 50), |
||||
# (0x1d0, 50), |
||||
# (0x305, 10), |
||||
# (0x324, 10), |
||||
# (0x405, 3), |
||||
# ] |
||||
|
||||
# cp = CANParser("honda_civic_touring_2016_can_generated", signals, checks, 0) |
||||
|
||||
|
||||
signals = [ |
||||
# sig_name, sig_address, default |
||||
("GEAR", 956, 0x20), |
||||
("BRAKE_PRESSED", 548, 0), |
||||
("GAS_PEDAL", 705, 0), |
||||
|
||||
("WHEEL_SPEED_FL", 170, 0), |
||||
("WHEEL_SPEED_FR", 170, 0), |
||||
("WHEEL_SPEED_RL", 170, 0), |
||||
("WHEEL_SPEED_RR", 170, 0), |
||||
("DOOR_OPEN_FL", 1568, 1), |
||||
("DOOR_OPEN_FR", 1568, 1), |
||||
("DOOR_OPEN_RL", 1568, 1), |
||||
("DOOR_OPEN_RR", 1568, 1), |
||||
("SEATBELT_DRIVER_UNLATCHED", 1568, 1), |
||||
("TC_DISABLED", 951, 1), |
||||
("STEER_ANGLE", 37, 0), |
||||
("STEER_FRACTION", 37, 0), |
||||
("STEER_RATE", 37, 0), |
||||
("GAS_RELEASED", 466, 0), |
||||
("CRUISE_STATE", 466, 0), |
||||
("MAIN_ON", 467, 0), |
||||
("SET_SPEED", 467, 0), |
||||
("STEER_TORQUE_DRIVER", 608, 0), |
||||
("STEER_TORQUE_EPS", 608, 0), |
||||
("TURN_SIGNALS", 1556, 3), # 3 is no blinkers |
||||
("LKA_STATE", 610, 0), |
||||
] |
||||
checks = [ |
||||
(548, 40), |
||||
(705, 33), |
||||
|
||||
(170, 80), |
||||
(37, 80), |
||||
(466, 33), |
||||
(608, 50), |
||||
] |
||||
|
||||
cp = CANParser("toyota_rav4_2017_pt_generated", signals, checks, 0) |
||||
|
||||
# print(cp.vl) |
||||
|
||||
while True: |
||||
cp.update(int(sec_since_boot()*1e9), True) |
||||
# print(cp.vl) |
||||
print(cp.ts) |
||||
print(cp.can_valid) |
||||
time.sleep(0.01) |
||||
@ -1,35 +0,0 @@ |
||||
import unittest |
||||
import random |
||||
|
||||
from selfdrive.can.tests.packer_old import CANPacker as CANPackerOld |
||||
from selfdrive.can.packer import CANPacker |
||||
import selfdrive.car.chrysler.chryslercan as chryslercan |
||||
|
||||
|
||||
class TestPackerMethods(unittest.TestCase): |
||||
def setUp(self): |
||||
self.chrysler_cp_old = CANPackerOld("chrysler_pacifica_2017_hybrid") |
||||
self.chrysler_cp = CANPacker("chrysler_pacifica_2017_hybrid") |
||||
|
||||
def test_correctness(self): |
||||
# Test all commands, randomize the params. |
||||
for _ in range(1000): |
||||
gear = ('drive', 'reverse', 'low')[random.randint(0, 3) % 3] |
||||
lkas_active = (random.randint(0, 2) % 2 == 0) |
||||
hud_alert = random.randint(0, 6) |
||||
hud_count = random.randint(0, 65536) |
||||
lkas_car_model = random.randint(0, 65536) |
||||
m_old = chryslercan.create_lkas_hud(self.chrysler_cp_old, gear, lkas_active, hud_alert, hud_count, lkas_car_model) |
||||
m = chryslercan.create_lkas_hud(self.chrysler_cp, gear, lkas_active, hud_alert, hud_count, lkas_car_model) |
||||
self.assertEqual(m_old, m) |
||||
|
||||
apply_steer = (random.randint(0, 2) % 2 == 0) |
||||
moving_fast = (random.randint(0, 2) % 2 == 0) |
||||
frame = random.randint(0, 65536) |
||||
m_old = chryslercan.create_lkas_command(self.chrysler_cp_old, apply_steer, moving_fast, frame) |
||||
m = chryslercan.create_lkas_command(self.chrysler_cp, apply_steer, moving_fast, frame) |
||||
self.assertEqual(m_old, m) |
||||
|
||||
|
||||
if __name__ == "__main__": |
||||
unittest.main() |
||||
@ -1,66 +0,0 @@ |
||||
import unittest |
||||
import random |
||||
|
||||
from selfdrive.can.tests.packer_old import CANPacker as CANPackerOld |
||||
from selfdrive.can.packer import CANPacker |
||||
import selfdrive.car.gm.gmcan as gmcan |
||||
from selfdrive.car.gm.interface import CanBus as GMCanBus |
||||
|
||||
|
||||
class TestPackerMethods(unittest.TestCase): |
||||
def setUp(self): |
||||
self.gm_cp_old = CANPackerOld("gm_global_a_powertrain") |
||||
self.gm_cp = CANPacker("gm_global_a_powertrain") |
||||
|
||||
self.ct6_cp_old = CANPackerOld("cadillac_ct6_chassis") |
||||
self.ct6_cp = CANPacker("cadillac_ct6_chassis") |
||||
|
||||
def test_correctness(self): |
||||
# Test all cars' commands, randomize the params. |
||||
for _ in range(1000): |
||||
bus = random.randint(0, 65536) |
||||
apply_steer = (random.randint(0, 2) % 2 == 0) |
||||
idx = random.randint(0, 65536) |
||||
lkas_active = (random.randint(0, 2) % 2 == 0) |
||||
m_old = gmcan.create_steering_control(self.gm_cp_old, bus, apply_steer, idx, lkas_active) |
||||
m = gmcan.create_steering_control(self.gm_cp, bus, apply_steer, idx, lkas_active) |
||||
self.assertEqual(m_old, m) |
||||
|
||||
canbus = GMCanBus() |
||||
apply_steer = (random.randint(0, 2) % 2 == 0) |
||||
v_ego = random.randint(0, 65536) |
||||
idx = random.randint(0, 65536) |
||||
enabled = (random.randint(0, 2) % 2 == 0) |
||||
m_old = gmcan.create_steering_control_ct6(self.ct6_cp_old, canbus, apply_steer, v_ego, idx, enabled) |
||||
m = gmcan.create_steering_control_ct6(self.ct6_cp, canbus, apply_steer, v_ego, idx, enabled) |
||||
self.assertEqual(m_old, m) |
||||
|
||||
bus = random.randint(0, 65536) |
||||
throttle = random.randint(0, 65536) |
||||
idx = random.randint(0, 65536) |
||||
acc_engaged = (random.randint(0, 2) % 2 == 0) |
||||
at_full_stop = (random.randint(0, 2) % 2 == 0) |
||||
m_old = gmcan.create_gas_regen_command(self.gm_cp_old, bus, throttle, idx, acc_engaged, at_full_stop) |
||||
m = gmcan.create_gas_regen_command(self.gm_cp, bus, throttle, idx, acc_engaged, at_full_stop) |
||||
self.assertEqual(m_old, m) |
||||
|
||||
bus = random.randint(0, 65536) |
||||
apply_brake = (random.randint(0, 2) % 2 == 0) |
||||
idx = random.randint(0, 65536) |
||||
near_stop = (random.randint(0, 2) % 2 == 0) |
||||
at_full_stop = (random.randint(0, 2) % 2 == 0) |
||||
m_old = gmcan.create_friction_brake_command(self.ct6_cp_old, bus, apply_brake, idx, near_stop, at_full_stop) |
||||
m = gmcan.create_friction_brake_command(self.ct6_cp, bus, apply_brake, idx, near_stop, at_full_stop) |
||||
self.assertEqual(m_old, m) |
||||
|
||||
bus = random.randint(0, 65536) |
||||
acc_engaged = (random.randint(0, 2) % 2 == 0) |
||||
target_speed_kph = random.randint(0, 65536) |
||||
lead_car_in_sight = (random.randint(0, 2) % 2 == 0) |
||||
m_old = gmcan.create_acc_dashboard_command(self.gm_cp_old, bus, acc_engaged, target_speed_kph, lead_car_in_sight) |
||||
m = gmcan.create_acc_dashboard_command(self.gm_cp, bus, acc_engaged, target_speed_kph, lead_car_in_sight) |
||||
self.assertEqual(m_old, m) |
||||
|
||||
|
||||
if __name__ == "__main__": |
||||
unittest.main() |
||||
@ -1,56 +0,0 @@ |
||||
import unittest |
||||
import random |
||||
|
||||
from selfdrive.can.tests.packer_old import CANPacker as CANPackerOld |
||||
from selfdrive.can.packer import CANPacker |
||||
import selfdrive.car.honda.hondacan as hondacan |
||||
from selfdrive.car.honda.values import HONDA_BOSCH |
||||
from selfdrive.car.honda.carcontroller import HUDData |
||||
|
||||
|
||||
class TestPackerMethods(unittest.TestCase): |
||||
def setUp(self): |
||||
self.honda_cp_old = CANPackerOld("honda_pilot_touring_2017_can_generated") |
||||
self.honda_cp = CANPacker("honda_pilot_touring_2017_can_generated") |
||||
|
||||
def test_correctness(self): |
||||
# Test all commands, randomize the params. |
||||
for _ in range(1000): |
||||
has_relay = False |
||||
car_fingerprint = HONDA_BOSCH[0] |
||||
|
||||
apply_brake = (random.randint(0, 2) % 2 == 0) |
||||
pump_on = (random.randint(0, 2) % 2 == 0) |
||||
pcm_override = (random.randint(0, 2) % 2 == 0) |
||||
pcm_cancel_cmd = (random.randint(0, 2) % 2 == 0) |
||||
fcw = random.randint(0, 65536) |
||||
idx = random.randint(0, 65536) |
||||
m_old = hondacan.create_brake_command(self.honda_cp_old, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, fcw, idx, car_fingerprint, has_relay) |
||||
m = hondacan.create_brake_command(self.honda_cp, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, fcw, idx, car_fingerprint, has_relay) |
||||
self.assertEqual(m_old, m) |
||||
|
||||
apply_steer = (random.randint(0, 2) % 2 == 0) |
||||
lkas_active = (random.randint(0, 2) % 2 == 0) |
||||
idx = random.randint(0, 65536) |
||||
m_old = hondacan.create_steering_control(self.honda_cp_old, apply_steer, lkas_active, car_fingerprint, idx, has_relay) |
||||
m = hondacan.create_steering_control(self.honda_cp, apply_steer, lkas_active, car_fingerprint, idx, has_relay) |
||||
self.assertEqual(m_old, m) |
||||
|
||||
pcm_speed = random.randint(0, 65536) |
||||
hud = HUDData(random.randint(0, 65536), random.randint(0, 65536), 1, random.randint(0, 65536), |
||||
0xc1, random.randint(0, 65536), random.randint(0, 65536), random.randint(0, 65536), random.randint(0, 65536)) |
||||
idx = random.randint(0, 65536) |
||||
is_metric = (random.randint(0, 2) % 2 == 0) |
||||
m_old = hondacan.create_ui_commands(self.honda_cp_old, pcm_speed, hud, car_fingerprint, is_metric, idx, has_relay) |
||||
m = hondacan.create_ui_commands(self.honda_cp, pcm_speed, hud, car_fingerprint, is_metric, idx, has_relay) |
||||
self.assertEqual(m_old, m) |
||||
|
||||
button_val = random.randint(0, 65536) |
||||
idx = random.randint(0, 65536) |
||||
m_old = hondacan.spam_buttons_command(self.honda_cp_old, button_val, idx, car_fingerprint, has_relay) |
||||
m = hondacan.spam_buttons_command(self.honda_cp, button_val, idx, car_fingerprint, has_relay) |
||||
self.assertEqual(m_old, m) |
||||
|
||||
|
||||
if __name__ == "__main__": |
||||
unittest.main() |
||||
@ -1,70 +0,0 @@ |
||||
import unittest |
||||
import random |
||||
|
||||
from selfdrive.can.tests.packer_old import CANPacker as CANPackerOld |
||||
from selfdrive.can.packer import CANPacker |
||||
import selfdrive.car.hyundai.hyundaican as hyundaican |
||||
from selfdrive.car.hyundai.values import CHECKSUM as hyundai_checksum |
||||
|
||||
|
||||
class TestPackerMethods(unittest.TestCase): |
||||
def setUp(self): |
||||
self.hyundai_cp_old = CANPackerOld("hyundai_kia_generic") |
||||
self.hyundai_cp = CANPacker("hyundai_kia_generic") |
||||
|
||||
def test_correctness(self): |
||||
# Test all commands, randomize the params. |
||||
for _ in range(1000): |
||||
# Hyundai |
||||
car_fingerprint = hyundai_checksum["crc8"][0] |
||||
apply_steer = (random.randint(0, 2) % 2 == 0) |
||||
steer_req = (random.randint(0, 2) % 2 == 0) |
||||
cnt = random.randint(0, 65536) |
||||
enabled = (random.randint(0, 2) % 2 == 0) |
||||
lkas11 = { |
||||
"CF_Lkas_LdwsSysState": random.randint(0,65536), |
||||
"CF_Lkas_SysWarning": random.randint(0,65536), |
||||
"CF_Lkas_LdwsLHWarning": random.randint(0,65536), |
||||
"CF_Lkas_LdwsRHWarning": random.randint(0,65536), |
||||
"CF_Lkas_HbaLamp": random.randint(0,65536), |
||||
"CF_Lkas_FcwBasReq": random.randint(0,65536), |
||||
"CF_Lkas_ToiFlt": random.randint(0,65536), |
||||
"CF_Lkas_HbaSysState": random.randint(0,65536), |
||||
"CF_Lkas_FcwOpt": random.randint(0,65536), |
||||
"CF_Lkas_HbaOpt": random.randint(0,65536), |
||||
"CF_Lkas_FcwSysState": random.randint(0,65536), |
||||
"CF_Lkas_FcwCollisionWarning": random.randint(0,65536), |
||||
"CF_Lkas_FusionState": random.randint(0,65536), |
||||
"CF_Lkas_FcwOpt_USM": random.randint(0,65536), |
||||
"CF_Lkas_LdwsOpt_USM": random.randint(0,65536) |
||||
} |
||||
hud_alert = random.randint(0, 65536) |
||||
keep_stock = (random.randint(0, 2) % 2 == 0) |
||||
m_old = hyundaican.create_lkas11(self.hyundai_cp_old, car_fingerprint, apply_steer, steer_req, cnt, enabled, |
||||
lkas11, hud_alert, keep_stock) |
||||
m = hyundaican.create_lkas11(self.hyundai_cp, car_fingerprint, apply_steer, steer_req, cnt, enabled, |
||||
lkas11, hud_alert, keep_stock) |
||||
self.assertEqual(m_old, m) |
||||
|
||||
clu11 = { |
||||
"CF_Clu_CruiseSwState": random.randint(0,65536), |
||||
"CF_Clu_CruiseSwMain": random.randint(0,65536), |
||||
"CF_Clu_SldMainSW": random.randint(0,65536), |
||||
"CF_Clu_ParityBit1": random.randint(0,65536), |
||||
"CF_Clu_VanzDecimal": random.randint(0,65536), |
||||
"CF_Clu_Vanz": random.randint(0,65536), |
||||
"CF_Clu_SPEED_UNIT": random.randint(0,65536), |
||||
"CF_Clu_DetentOut": random.randint(0,65536), |
||||
"CF_Clu_RheostatLevel": random.randint(0,65536), |
||||
"CF_Clu_CluInfo": random.randint(0,65536), |
||||
"CF_Clu_AmpInfo": random.randint(0,65536), |
||||
"CF_Clu_AliveCnt1": random.randint(0,65536), |
||||
} |
||||
button = random.randint(0, 65536) |
||||
m_old = hyundaican.create_clu11(self.hyundai_cp_old, clu11, button) |
||||
m = hyundaican.create_clu11(self.hyundai_cp, clu11, button) |
||||
self.assertEqual(m_old, m) |
||||
|
||||
|
||||
if __name__ == "__main__": |
||||
unittest.main() |
||||
@ -0,0 +1,92 @@ |
||||
#!/usr/bin/env python3 |
||||
|
||||
import unittest |
||||
|
||||
from selfdrive.can.parser import CANParser |
||||
from selfdrive.can.packer import CANPacker |
||||
|
||||
from selfdrive.boardd.boardd import can_list_to_can_capnp |
||||
|
||||
from selfdrive.car.honda.interface import CarInterface as HondaInterface |
||||
from selfdrive.car.honda.values import CAR as HONDA_CAR |
||||
from selfdrive.car.honda.values import DBC as HONDA_DBC |
||||
|
||||
from selfdrive.car.subaru.interface import CarInterface as SubaruInterface |
||||
from selfdrive.car.subaru.values import CAR as SUBARU_CAR |
||||
from selfdrive.car.subaru.values import DBC as SUBARU_DBC |
||||
|
||||
class TestCanParserPacker(unittest.TestCase): |
||||
def test_civic(self): |
||||
CP = HondaInterface.get_params(HONDA_CAR.CIVIC) |
||||
|
||||
signals = [ |
||||
("STEER_TORQUE", "STEERING_CONTROL", 0), |
||||
("STEER_TORQUE_REQUEST", "STEERING_CONTROL", 0), |
||||
] |
||||
checks = [] |
||||
|
||||
parser = CANParser(HONDA_DBC[CP.carFingerprint]['pt'], signals, checks, 0) |
||||
packer = CANPacker(HONDA_DBC[CP.carFingerprint]['pt']) |
||||
|
||||
idx = 0 |
||||
|
||||
for steer in range(0, 255): |
||||
for active in [1, 0]: |
||||
values = { |
||||
"STEER_TORQUE": steer, |
||||
"STEER_TORQUE_REQUEST": active, |
||||
} |
||||
|
||||
msgs = packer.make_can_msg("STEERING_CONTROL", 0, values, idx) |
||||
bts = can_list_to_can_capnp([msgs]) |
||||
|
||||
parser.update_string(bts) |
||||
|
||||
self.assertAlmostEqual(parser.vl["STEERING_CONTROL"]["STEER_TORQUE"], steer) |
||||
self.assertAlmostEqual(parser.vl["STEERING_CONTROL"]["STEER_TORQUE_REQUEST"], active) |
||||
self.assertAlmostEqual(parser.vl["STEERING_CONTROL"]["COUNTER"], idx % 4) |
||||
|
||||
idx += 1 |
||||
|
||||
def test_subaru(self): |
||||
# Subuaru is little endian |
||||
CP = SubaruInterface.get_params(SUBARU_CAR.IMPREZA) |
||||
|
||||
signals = [ |
||||
("Counter", "ES_LKAS", 0), |
||||
("LKAS_Output", "ES_LKAS", 0), |
||||
("LKAS_Request", "ES_LKAS", 0), |
||||
("SET_1", "ES_LKAS", 0), |
||||
|
||||
] |
||||
|
||||
checks = [] |
||||
|
||||
parser = CANParser(SUBARU_DBC[CP.carFingerprint]['pt'], signals, checks, 0) |
||||
packer = CANPacker(SUBARU_DBC[CP.carFingerprint]['pt']) |
||||
|
||||
idx = 0 |
||||
|
||||
for steer in range(0, 255): |
||||
for active in [1, 0]: |
||||
values = { |
||||
"Counter": idx, |
||||
"LKAS_Output": steer, |
||||
"LKAS_Request": active, |
||||
"SET_1": 1 |
||||
} |
||||
|
||||
msgs = packer.make_can_msg("ES_LKAS", 0, values) |
||||
bts = can_list_to_can_capnp([msgs]) |
||||
parser.update_string(bts) |
||||
|
||||
self.assertAlmostEqual(parser.vl["ES_LKAS"]["LKAS_Output"], steer) |
||||
self.assertAlmostEqual(parser.vl["ES_LKAS"]["LKAS_Request"], active) |
||||
self.assertAlmostEqual(parser.vl["ES_LKAS"]["SET_1"], 1) |
||||
self.assertAlmostEqual(parser.vl["ES_LKAS"]["Counter"], idx % 16) |
||||
|
||||
idx += 1 |
||||
|
||||
|
||||
if __name__ == "__main__": |
||||
unittest.main() |
||||
@ -1,37 +0,0 @@ |
||||
import unittest |
||||
import random |
||||
|
||||
from selfdrive.can.tests.packer_old import CANPacker as CANPackerOld |
||||
from selfdrive.can.packer import CANPacker |
||||
import selfdrive.car.subaru.subarucan as subarucan |
||||
from selfdrive.car.subaru.values import CAR as subaru_car |
||||
|
||||
|
||||
class TestPackerMethods(unittest.TestCase): |
||||
def setUp(self): |
||||
self.subaru_cp_old = CANPackerOld("subaru_global_2017") |
||||
self.subaru_cp = CANPacker("subaru_global_2017") |
||||
|
||||
def test_correctness(self): |
||||
# Test all cars' commands, randomize the params. |
||||
for _ in range(1000): |
||||
apply_steer = (random.randint(0, 2) % 2 == 0) |
||||
frame = random.randint(1, 65536) |
||||
steer_step = random.randint(1, 65536) |
||||
m_old = subarucan.create_steering_control(self.subaru_cp_old, subaru_car.IMPREZA, apply_steer, frame, steer_step) |
||||
m = subarucan.create_steering_control(self.subaru_cp, subaru_car.IMPREZA, apply_steer, frame, steer_step) |
||||
self.assertEqual(m_old, m) |
||||
|
||||
m_old = subarucan.create_steering_status(self.subaru_cp_old, subaru_car.IMPREZA, apply_steer, frame, steer_step) |
||||
m = subarucan.create_steering_status(self.subaru_cp, subaru_car.IMPREZA, apply_steer, frame, steer_step) |
||||
self.assertEqual(m_old, m) |
||||
|
||||
es_distance_msg = {} |
||||
pcm_cancel_cmd = (random.randint(0, 2) % 2 == 0) |
||||
m_old = subarucan.create_es_distance(self.subaru_cp_old, es_distance_msg, pcm_cancel_cmd) |
||||
m = subarucan.create_es_distance(self.subaru_cp, es_distance_msg, pcm_cancel_cmd) |
||||
self.assertEqual(m_old, m) |
||||
|
||||
|
||||
if __name__ == "__main__": |
||||
unittest.main() |
||||
@ -1,86 +0,0 @@ |
||||
import unittest |
||||
import random |
||||
|
||||
from selfdrive.can.tests.packer_old import CANPacker as CANPackerOld |
||||
from selfdrive.can.packer import CANPacker |
||||
from selfdrive.car.toyota.toyotacan import ( |
||||
create_ipas_steer_command, create_steer_command, create_accel_command, |
||||
create_fcw_command, create_ui_command |
||||
) |
||||
from common.realtime import sec_since_boot |
||||
|
||||
|
||||
class TestPackerMethods(unittest.TestCase): |
||||
def setUp(self): |
||||
self.cp_old = CANPackerOld("toyota_rav4_hybrid_2017_pt_generated") |
||||
self.cp = CANPacker("toyota_rav4_hybrid_2017_pt_generated") |
||||
|
||||
def test_correctness(self): |
||||
# Test all commands, randomize the params. |
||||
for _ in range(1000): |
||||
# Toyota |
||||
steer = random.randint(-1, 1) |
||||
enabled = (random.randint(0, 2) % 2 == 0) |
||||
apgs_enabled = (random.randint(0, 2) % 2 == 0) |
||||
m_old = create_ipas_steer_command(self.cp_old, steer, enabled, apgs_enabled) |
||||
m = create_ipas_steer_command(self.cp, steer, enabled, apgs_enabled) |
||||
self.assertEqual(m_old, m) |
||||
|
||||
steer = (random.randint(0, 2) % 2 == 0) |
||||
steer_req = (random.randint(0, 2) % 2 == 0) |
||||
raw_cnt = random.randint(1, 65536) |
||||
m_old = create_steer_command(self.cp_old, steer, steer_req, raw_cnt) |
||||
m = create_steer_command(self.cp, steer, steer_req, raw_cnt) |
||||
self.assertEqual(m_old, m) |
||||
|
||||
accel = (random.randint(0, 2) % 2 == 0) |
||||
pcm_cancel = (random.randint(0, 2) % 2 == 0) |
||||
standstill_req = (random.randint(0, 2) % 2 == 0) |
||||
lead = (random.randint(0, 2) % 2 == 0) |
||||
m_old = create_accel_command(self.cp_old, accel, pcm_cancel, standstill_req, lead) |
||||
m = create_accel_command(self.cp, accel, pcm_cancel, standstill_req, lead) |
||||
self.assertEqual(m_old, m) |
||||
|
||||
fcw = random.randint(1, 65536) |
||||
m_old = create_fcw_command(self.cp_old, fcw) |
||||
m = create_fcw_command(self.cp, fcw) |
||||
self.assertEqual(m_old, m) |
||||
|
||||
steer = (random.randint(0, 2) % 2 == 0) |
||||
chime = random.randint(1, 65536) |
||||
left_line = (random.randint(0, 2) % 2 == 0) |
||||
right_line = (random.randint(0, 2) % 2 == 0) |
||||
left_lane_depart = (random.randint(0, 2) % 2 == 0) |
||||
right_lane_depart = (random.randint(0, 2) % 2 == 0) |
||||
m_old = create_ui_command(self.cp_old, steer, chime, left_line, right_line, left_lane_depart, right_lane_depart) |
||||
m = create_ui_command(self.cp, steer, chime, left_line, right_line, left_lane_depart, right_lane_depart) |
||||
self.assertEqual(m_old, m) |
||||
|
||||
def test_performance(self): |
||||
n1 = sec_since_boot() |
||||
recursions = 100000 |
||||
steer = (random.randint(0, 2) % 2 == 0) |
||||
chime = random.randint(1, 65536) |
||||
left_line = (random.randint(0, 2) % 2 == 0) |
||||
right_line = (random.randint(0, 2) % 2 == 0) |
||||
left_lane_depart = (random.randint(0, 2) % 2 == 0) |
||||
right_lane_depart = (random.randint(0, 2) % 2 == 0) |
||||
|
||||
for _ in range(recursions): |
||||
create_ui_command(self.cp_old, steer, chime, left_line, right_line, left_lane_depart, right_lane_depart) |
||||
n2 = sec_since_boot() |
||||
elapsed_old = n2 - n1 |
||||
|
||||
# print('Old API, elapsed time: {} secs'.format(elapsed_old)) |
||||
n1 = sec_since_boot() |
||||
for _ in range(recursions): |
||||
create_ui_command(self.cp, steer, chime, left_line, right_line, left_lane_depart, right_lane_depart) |
||||
n2 = sec_since_boot() |
||||
elapsed_new = n2 - n1 |
||||
# print('New API, elapsed time: {} secs'.format(elapsed_new)) |
||||
self.assertTrue(elapsed_new < elapsed_old / 2) |
||||
|
||||
|
||||
|
||||
if __name__ == "__main__": |
||||
unittest.main() |
||||
@ -1,106 +0,0 @@ |
||||
#!/usr/bin/env python3 |
||||
|
||||
import os |
||||
import unittest |
||||
|
||||
import requests |
||||
|
||||
import selfdrive.messaging as messaging |
||||
from selfdrive.can.parser import CANParser as CANParserNew |
||||
from selfdrive.can.tests.parser_old import CANParser as CANParserOld |
||||
from selfdrive.car.honda.carstate import get_can_signals |
||||
from selfdrive.car.honda.interface import CarInterface |
||||
from selfdrive.car.honda.values import CAR, DBC |
||||
from selfdrive.services import service_list |
||||
from tools.lib.logreader import LogReader |
||||
|
||||
BASE_URL = "https://commadataci.blob.core.windows.net/openpilotci/" |
||||
DT = int(0.01 * 1e9) # ns |
||||
|
||||
|
||||
def dict_keys_differ(dict1, dict2): |
||||
keys1 = set(dict1.keys()) |
||||
keys2 = set(dict2.keys()) |
||||
|
||||
if keys1 != keys2: |
||||
return True |
||||
|
||||
for k in keys1: |
||||
keys1 = set(dict1[k].keys()) |
||||
keys2 = set(dict2[k].keys()) |
||||
|
||||
if keys1 != keys2: |
||||
return True |
||||
|
||||
return False |
||||
|
||||
def dicts_vals_differ(dict1, dict2): |
||||
for k_outer in dict1.keys(): |
||||
for k_inner in dict1[k_outer].keys(): |
||||
if dict1[k_outer][k_inner] != dict2[k_outer][k_inner]: |
||||
return True |
||||
|
||||
return False |
||||
|
||||
def run_route(route): |
||||
can = messaging.pub_sock(service_list['can'].port) |
||||
|
||||
CP = CarInterface.get_params(CAR.CIVIC) |
||||
signals, checks = get_can_signals(CP) |
||||
parser_old = CANParserOld(DBC[CP.carFingerprint]['pt'], signals, checks, 0, timeout=-1, tcp_addr="127.0.0.1") |
||||
parser_new = CANParserNew(DBC[CP.carFingerprint]['pt'], signals, checks, 0, timeout=-1, tcp_addr="127.0.0.1") |
||||
parser_string = CANParserNew(DBC[CP.carFingerprint]['pt'], signals, checks, 0, timeout=-1) |
||||
|
||||
if dict_keys_differ(parser_old.vl, parser_new.vl): |
||||
return False |
||||
|
||||
lr = LogReader(route + ".bz2") |
||||
|
||||
route_ok = True |
||||
|
||||
for msg in lr: |
||||
if msg.which() == 'can': |
||||
t = msg.logMonoTime |
||||
msg_bytes = msg.as_builder().to_bytes() |
||||
can.send(msg_bytes) |
||||
|
||||
_, updated_old = parser_old.update(t, True) |
||||
_, updated_new = parser_new.update(t, True) |
||||
updated_string = parser_string.update_string(msg_bytes) |
||||
|
||||
if updated_old != updated_new: |
||||
route_ok = False |
||||
print(t, "Diff in seen") |
||||
|
||||
if updated_new != updated_string: |
||||
route_ok = False |
||||
print(t, "Diff in seen string") |
||||
|
||||
if dicts_vals_differ(parser_old.vl, parser_new.vl): |
||||
print(t, "Diff in dict") |
||||
route_ok = False |
||||
|
||||
if dicts_vals_differ(parser_new.vl, parser_string.vl): |
||||
print(t, "Diff in dict string") |
||||
route_ok = False |
||||
|
||||
return route_ok |
||||
|
||||
class TestCanParser(unittest.TestCase): |
||||
def setUp(self): |
||||
self.routes = { |
||||
CAR.CIVIC: "b0c9d2329ad1606b|2019-05-30--20-23-57" |
||||
} |
||||
|
||||
for route in self.routes.values(): |
||||
route_filename = route + ".bz2" |
||||
if not os.path.isfile(route_filename): |
||||
with open(route + ".bz2", "wb") as f: |
||||
f.write(requests.get(BASE_URL + route_filename).content) |
||||
|
||||
def test_parser_civic(self): |
||||
self.assertTrue(run_route(self.routes[CAR.CIVIC])) |
||||
|
||||
|
||||
if __name__ == "__main__": |
||||
unittest.main() |
||||
@ -1,27 +0,0 @@ |
||||
#!/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]) |
||||
@ -0,0 +1 @@ |
||||
|
||||
@ -0,0 +1,188 @@ |
||||
from cereal import car |
||||
from selfdrive.car import apply_std_steer_torque_limits |
||||
from selfdrive.car.volkswagen import volkswagencan |
||||
from selfdrive.car.volkswagen.values import DBC, MQB_LDW_MESSAGES, BUTTON_STATES, CarControllerParams |
||||
from selfdrive.can.packer import CANPacker |
||||
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert |
||||
|
||||
|
||||
class CarController(): |
||||
def __init__(self, canbus, car_fingerprint): |
||||
self.apply_steer_last = 0 |
||||
self.car_fingerprint = car_fingerprint |
||||
|
||||
# Setup detection helper. Routes commands to an appropriate CAN bus number. |
||||
self.canbus = canbus |
||||
self.packer_gw = CANPacker(DBC[car_fingerprint]['pt']) |
||||
|
||||
self.hcaSameTorqueCount = 0 |
||||
self.hcaEnabledFrameCount = 0 |
||||
self.graButtonStatesToSend = None |
||||
self.graMsgSentCount = 0 |
||||
self.graMsgStartFramePrev = 0 |
||||
self.graMsgBusCounterPrev = 0 |
||||
|
||||
def update(self, enabled, CS, frame, actuators, visual_alert, audible_alert, leftLaneVisible, rightLaneVisible): |
||||
""" Controls thread """ |
||||
|
||||
P = CarControllerParams |
||||
|
||||
# Send CAN commands. |
||||
can_sends = [] |
||||
canbus = self.canbus |
||||
|
||||
#-------------------------------------------------------------------------- |
||||
# # |
||||
# Prepare HCA_01 Heading Control Assist messages with steering torque. # |
||||
# # |
||||
#-------------------------------------------------------------------------- |
||||
|
||||
# The factory camera sends at 50Hz while steering and 1Hz when not. When |
||||
# OP is active, Panda filters HCA_01 from the factory camera and OP emits |
||||
# HCA_01 at 50Hz. Rate switching creates some confusion in Cabana and |
||||
# doesn't seem to add value at this time. The rack will accept HCA_01 at |
||||
# 100Hz if we want to control at finer resolution in the future. |
||||
if frame % P.HCA_STEP == 0: |
||||
|
||||
# FAULT AVOIDANCE: HCA must not be enabled at standstill. Also stop |
||||
# commanding HCA if there's a fault, so the steering rack recovers. |
||||
if enabled and not (CS.standstill or CS.steeringFault): |
||||
|
||||
# FAULT AVOIDANCE: Requested HCA torque must not exceed 3.0 Nm. This |
||||
# is inherently handled by scaling to STEER_MAX. The rack doesn't seem |
||||
# to care about up/down rate, but we have some evidence it may do its |
||||
# own rate limiting, and matching OP helps for accurate tuning. |
||||
apply_steer = int(round(actuators.steer * P.STEER_MAX)) |
||||
apply_steer = apply_std_steer_torque_limits(apply_steer, self.apply_steer_last, CS.steeringTorque, P) |
||||
|
||||
# FAULT AVOIDANCE: HCA must not be enabled for >360 seconds. Sending |
||||
# a single frame with HCA disabled is an effective workaround. |
||||
if apply_steer == 0: |
||||
# We can usually reset the timer for free, just by disabling HCA |
||||
# when apply_steer is exactly zero, which happens by chance during |
||||
# many steer torque direction changes. This could be expanded with |
||||
# a small dead-zone to capture all zero crossings, but not seeing a |
||||
# major need at this time. |
||||
hcaEnabled = False |
||||
self.hcaEnabledFrameCount = 0 |
||||
else: |
||||
self.hcaEnabledFrameCount += 1 |
||||
if self.hcaEnabledFrameCount >= 118 * (100 / P.HCA_STEP): # 118s |
||||
# The Kansas I-70 Crosswind Problem: if we truly do need to steer |
||||
# in one direction for > 360 seconds, we have to disable HCA for a |
||||
# frame while actively steering. Testing shows we can just set the |
||||
# disabled flag, and keep sending non-zero torque, which keeps the |
||||
# Panda torque rate limiting safety happy. Do so 3x within the 360 |
||||
# second window for safety and redundancy. |
||||
hcaEnabled = False |
||||
self.hcaEnabledFrameCount = 0 |
||||
else: |
||||
hcaEnabled = True |
||||
# FAULT AVOIDANCE: HCA torque must not be static for > 6 seconds. |
||||
# This is to detect the sending camera being stuck or frozen. OP |
||||
# can trip this on a curve if steering is saturated. Avoid this by |
||||
# reducing torque 0.01 Nm for one frame. Do so 3x within the 6 |
||||
# second period for safety and redundancy. |
||||
if self.apply_steer_last == apply_steer: |
||||
self.hcaSameTorqueCount += 1 |
||||
if self.hcaSameTorqueCount > 1.9 * (100 / P.HCA_STEP): # 1.9s |
||||
apply_steer -= (1, -1)[apply_steer < 0] |
||||
self.hcaSameTorqueCount = 0 |
||||
else: |
||||
self.hcaSameTorqueCount = 0 |
||||
|
||||
else: |
||||
# Continue sending HCA_01 messages, with the enable flags turned off. |
||||
hcaEnabled = False |
||||
apply_steer = 0 |
||||
|
||||
self.apply_steer_last = apply_steer |
||||
idx = (frame / P.HCA_STEP) % 16 |
||||
can_sends.append(volkswagencan.create_mqb_steering_control(self.packer_gw, canbus.gateway, apply_steer, |
||||
idx, hcaEnabled)) |
||||
|
||||
#-------------------------------------------------------------------------- |
||||
# # |
||||
# Prepare LDW_02 HUD messages with lane borders, confidence levels, and # |
||||
# the LKAS status LED. # |
||||
# # |
||||
#-------------------------------------------------------------------------- |
||||
|
||||
# The factory camera emits this message at 10Hz. When OP is active, Panda |
||||
# filters LDW_02 from the factory camera and OP emits LDW_02 at 10Hz. |
||||
|
||||
if frame % P.LDW_STEP == 0: |
||||
hcaEnabled = True if enabled and not CS.standstill else False |
||||
|
||||
if visual_alert == VisualAlert.steerRequired: |
||||
hud_alert = MQB_LDW_MESSAGES["laneAssistTakeOverSilent"] |
||||
else: |
||||
hud_alert = MQB_LDW_MESSAGES["none"] |
||||
|
||||
can_sends.append(volkswagencan.create_mqb_hud_control(self.packer_gw, canbus.gateway, hcaEnabled, |
||||
CS.steeringPressed, hud_alert, leftLaneVisible, |
||||
rightLaneVisible)) |
||||
|
||||
#-------------------------------------------------------------------------- |
||||
# # |
||||
# Prepare GRA_ACC_01 ACC control messages with button press events. # |
||||
# # |
||||
#-------------------------------------------------------------------------- |
||||
|
||||
# The car sends this message at 33hz. OP sends it on-demand only for |
||||
# virtual button presses. |
||||
# |
||||
# First create any virtual button press event needed by openpilot, to sync |
||||
# stock ACC with OP disengagement, or to auto-resume from stop. |
||||
|
||||
if frame > self.graMsgStartFramePrev + P.GRA_VBP_STEP: |
||||
if not enabled and CS.accEnabled: |
||||
# Cancel ACC if it's engaged with OP disengaged. |
||||
self.graButtonStatesToSend = BUTTON_STATES.copy() |
||||
self.graButtonStatesToSend["cancel"] = True |
||||
elif enabled and CS.standstill: |
||||
# Blip the Resume button if we're engaged at standstill. |
||||
# FIXME: This is a naive implementation, improve with visiond or radar input. |
||||
# A subset of MQBs like to "creep" too aggressively with this implementation. |
||||
self.graButtonStatesToSend = BUTTON_STATES.copy() |
||||
self.graButtonStatesToSend["resumeCruise"] = True |
||||
|
||||
# OP/Panda can see this message but can't filter it when integrated at the |
||||
# R242 LKAS camera. It could do so if integrated at the J533 gateway, but |
||||
# we need a generalized solution that works for either. The message is |
||||
# counter-protected, so we need to time our transmissions very precisely |
||||
# to achieve fast and fault-free switching between message flows accepted |
||||
# at the J428 ACC radar. |
||||
# |
||||
# Example message flow on the bus, frequency of 33Hz (GRA_ACC_STEP): |
||||
# |
||||
# CAR: 0 1 2 3 4 5 6 7 8 9 A B C D E F 0 1 2 3 4 5 6 |
||||
# EON: 3 4 5 6 7 8 9 A B C D E F 0 1 2 GG^ |
||||
# |
||||
# If OP needs to send a button press, it waits to see a GRA_ACC_01 message |
||||
# counter change, and then immediately follows up with the next increment. |
||||
# The OP message will be sent within about 1ms of the car's message, which |
||||
# is about 2ms before the car's next message is expected. OP sends for an |
||||
# arbitrary duration of 16 messages / ~0.5 sec, in lockstep with each new |
||||
# message from the car. |
||||
# |
||||
# Because OP's counter is synced to the car, J428 immediately accepts the |
||||
# OP messages as valid. Further messages from the car get discarded as |
||||
# duplicates without a fault. When OP stops sending, the extra time gap |
||||
# (GG) to the next valid car message is less than 1 * GRA_ACC_STEP. J428 |
||||
# tolerates the gap just fine and control returns to the car immediately. |
||||
|
||||
if CS.graMsgBusCounter != self.graMsgBusCounterPrev: |
||||
self.graMsgBusCounterPrev = CS.graMsgBusCounter |
||||
if self.graButtonStatesToSend is not None: |
||||
if self.graMsgSentCount == 0: |
||||
self.graMsgStartFramePrev = frame |
||||
idx = (CS.graMsgBusCounter + 1) % 16 |
||||
can_sends.append(volkswagencan.create_mqb_acc_buttons_control(self.packer_gw, canbus.extended, self.graButtonStatesToSend, CS, idx)) |
||||
self.graMsgSentCount += 1 |
||||
if self.graMsgSentCount >= 16: |
||||
self.graButtonStatesToSend = None |
||||
self.graMsgSentCount = 0 |
||||
|
||||
return can_sends |
||||
@ -0,0 +1,229 @@ |
||||
import numpy as np |
||||
from cereal import car |
||||
from common.kalman.simple_kalman import KF1D |
||||
from selfdrive.config import Conversions as CV |
||||
from selfdrive.can.parser import CANParser |
||||
from selfdrive.can.can_define import CANDefine |
||||
from selfdrive.car.volkswagen.values import DBC, BUTTON_STATES |
||||
from selfdrive.car.volkswagen.carcontroller import CarControllerParams |
||||
|
||||
GEAR = car.CarState.GearShifter |
||||
|
||||
def get_mqb_gateway_can_parser(CP, canbus): |
||||
# this function generates lists for signal, messages and initial values |
||||
signals = [ |
||||
# sig_name, sig_address, default |
||||
("LWI_Lenkradwinkel", "LWI_01", 0), # Absolute steering angle |
||||
("LWI_VZ_Lenkradwinkel", "LWI_01", 0), # Steering angle sign |
||||
("LWI_Lenkradw_Geschw", "LWI_01", 0), # Absolute steering rate |
||||
("LWI_VZ_Lenkradw_Geschw", "LWI_01", 0), # Steering rate sign |
||||
("ESP_VL_Radgeschw_02", "ESP_19", 0), # ABS wheel speed, front left |
||||
("ESP_VR_Radgeschw_02", "ESP_19", 0), # ABS wheel speed, front right |
||||
("ESP_HL_Radgeschw_02", "ESP_19", 0), # ABS wheel speed, rear left |
||||
("ESP_HR_Radgeschw_02", "ESP_19", 0), # ABS wheel speed, rear right |
||||
("ESP_Gierrate", "ESP_02", 0), # Absolute yaw rate |
||||
("ESP_VZ_Gierrate", "ESP_02", 0), # Yaw rate sign |
||||
("ZV_FT_offen", "Gateway_72", 0), # Door open, driver |
||||
("ZV_BT_offen", "Gateway_72", 0), # Door open, passenger |
||||
("ZV_HFS_offen", "Gateway_72", 0), # Door open, rear left |
||||
("ZV_HBFS_offen", "Gateway_72", 0), # Door open, rear right |
||||
("ZV_HD_offen", "Gateway_72", 0), # Trunk or hatch open |
||||
("BH_Blinker_li", "Gateway_72", 0), # Left turn signal on |
||||
("BH_Blinker_re", "Gateway_72", 0), # Right turn signal on |
||||
("GE_Fahrstufe", "Getriebe_11", 0), # Auto trans gear selector position |
||||
("AB_Gurtschloss_FA", "Airbag_02", 0), # Seatbelt status, driver |
||||
("AB_Gurtschloss_BF", "Airbag_02", 0), # Seatbelt status, passenger |
||||
("ESP_Fahrer_bremst", "ESP_05", 0), # Brake pedal pressed |
||||
("ESP_Status_Bremsdruck", "ESP_05", 0), # Brakes applied |
||||
("ESP_Bremsdruck", "ESP_05", 0), # Brake pressure applied |
||||
("MO_Fahrpedalrohwert_01", "Motor_20", 0), # Accelerator pedal value |
||||
("MO_Kuppl_schalter", "Motor_14", 0), # Clutch switch |
||||
("Driver_Strain", "EPS_01", 0), # Absolute driver torque input |
||||
("Driver_Strain_VZ", "EPS_01", 0), # Driver torque input sign |
||||
("HCA_Ready", "EPS_01", 0), # Steering rack HCA support configured |
||||
("ESP_Tastung_passiv", "ESP_21", 0), # Stability control disabled |
||||
("KBI_MFA_v_Einheit_02", "Einheiten_01", 0), # MPH vs KMH speed display |
||||
("KBI_Handbremse", "Kombi_01", 0), # Manual handbrake applied |
||||
("TSK_Fahrzeugmasse_02", "Motor_16", 0), # Estimated vehicle mass from drivetrain coordinator |
||||
("GRA_Hauptschalter", "GRA_ACC_01", 0), # ACC button, on/off |
||||
("GRA_Abbrechen", "GRA_ACC_01", 0), # ACC button, cancel |
||||
("GRA_Tip_Setzen", "GRA_ACC_01", 0), # ACC button, set |
||||
("GRA_Tip_Hoch", "GRA_ACC_01", 0), # ACC button, increase or accel |
||||
("GRA_Tip_Runter", "GRA_ACC_01", 0), # ACC button, decrease or decel |
||||
("GRA_Tip_Wiederaufnahme", "GRA_ACC_01", 0), # ACC button, resume |
||||
("GRA_Verstellung_Zeitluecke", "GRA_ACC_01", 0), # ACC button, time gap adj |
||||
("GRA_Typ_Hauptschalter", "GRA_ACC_01", 0), # ACC main button type |
||||
("GRA_Tip_Stufe_2", "GRA_ACC_01", 0), # unknown related to stalk type |
||||
("GRA_ButtonTypeInfo", "GRA_ACC_01", 0), # unknown related to stalk type |
||||
("COUNTER", "GRA_ACC_01", 0), # GRA_ACC_01 CAN message counter |
||||
] |
||||
|
||||
checks = [ |
||||
# sig_address, frequency |
||||
("LWI_01", 100), # From J500 Steering Assist with integrated sensors |
||||
("EPS_01", 100), # From J500 Steering Assist with integrated sensors |
||||
("ESP_19", 100), # From J104 ABS/ESP controller |
||||
("ESP_05", 50), # From J104 ABS/ESP controller |
||||
("ESP_21", 50), # From J104 ABS/ESP controller |
||||
("Motor_20", 50), # From J623 Engine control module |
||||
("GRA_ACC_01", 33), # From J??? steering wheel control buttons |
||||
("Getriebe_11", 20), # From J743 Auto transmission control module |
||||
("Gateway_72", 10), # From J533 CAN gateway (aggregated data) |
||||
("Motor_14", 10), # From J623 Engine control module |
||||
("Airbag_02", 5), # From J234 Airbag control module |
||||
("Kombi_01", 2), # From J285 Instrument cluster |
||||
("Motor_16", 2), # From J623 Engine control module |
||||
("Einheiten_01", 1), # From J??? not known if gateway, cluster, or BCM |
||||
] |
||||
|
||||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, canbus.gateway) |
||||
|
||||
def get_mqb_extended_can_parser(CP, canbus): |
||||
|
||||
signals = [ |
||||
# sig_name, sig_address, default |
||||
("ACC_Status_ACC", "ACC_06", 0), # ACC engagement status |
||||
("ACC_Typ", "ACC_06", 0), # ACC type (follow to stop, stop&go) |
||||
("SetSpeed", "ACC_02", 0), # ACC set speed |
||||
] |
||||
|
||||
checks = [ |
||||
# sig_address, frequency |
||||
("ACC_06", 50), # From J428 ACC radar control module |
||||
("ACC_02", 17), # From J428 ACC radar control module |
||||
] |
||||
|
||||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, canbus.extended) |
||||
|
||||
def parse_gear_shifter(gear, vals): |
||||
# Return mapping of gearshift position to selected gear. |
||||
|
||||
val_to_capnp = {'P': GEAR.park, 'R': GEAR.reverse, 'N': GEAR.neutral, |
||||
'D': GEAR.drive, 'E': GEAR.eco, 'S': GEAR.sport, 'T': GEAR.manumatic} |
||||
try: |
||||
return val_to_capnp[vals[gear]] |
||||
except KeyError: |
||||
return "unknown" |
||||
|
||||
class CarState(): |
||||
def __init__(self, CP, canbus): |
||||
# initialize can parser |
||||
self.CP = CP |
||||
self.car_fingerprint = CP.carFingerprint |
||||
self.can_define = CANDefine(DBC[CP.carFingerprint]['pt']) |
||||
|
||||
self.shifter_values = self.can_define.dv["Getriebe_11"]['GE_Fahrstufe'] |
||||
|
||||
self.buttonStates = BUTTON_STATES.copy() |
||||
|
||||
# vEgo Kalman filter |
||||
dt = 0.01 |
||||
self.v_ego_kf = KF1D(x0=[[0.], [0.]], |
||||
A=[[1., dt], [0., 1.]], |
||||
C=[1., 0.], |
||||
K=[[0.12287673], [0.29666309]]) |
||||
|
||||
def update(self, gw_cp, ex_cp): |
||||
# Update vehicle speed and acceleration from ABS wheel speeds. |
||||
self.wheelSpeedFL = gw_cp.vl["ESP_19"]['ESP_VL_Radgeschw_02'] * CV.KPH_TO_MS |
||||
self.wheelSpeedFR = gw_cp.vl["ESP_19"]['ESP_VR_Radgeschw_02'] * CV.KPH_TO_MS |
||||
self.wheelSpeedRL = gw_cp.vl["ESP_19"]['ESP_HL_Radgeschw_02'] * CV.KPH_TO_MS |
||||
self.wheelSpeedRR = gw_cp.vl["ESP_19"]['ESP_HR_Radgeschw_02'] * CV.KPH_TO_MS |
||||
|
||||
self.vEgoRaw = float(np.mean([self.wheelSpeedFL, self.wheelSpeedFR, self.wheelSpeedRL, self.wheelSpeedRR])) |
||||
v_ego_x = self.v_ego_kf.update(self.vEgoRaw) |
||||
self.vEgo = float(v_ego_x[0]) |
||||
self.aEgo = float(v_ego_x[1]) |
||||
self.standstill = self.vEgoRaw < 0.1 |
||||
|
||||
# Update steering angle, rate, yaw rate, and driver input torque. VW send |
||||
# the sign/direction in a separate signal so they must be recombined. |
||||
self.steeringAngle = gw_cp.vl["LWI_01"]['LWI_Lenkradwinkel'] * (1,-1)[int(gw_cp.vl["LWI_01"]['LWI_VZ_Lenkradwinkel'])] |
||||
self.steeringRate = gw_cp.vl["LWI_01"]['LWI_Lenkradw_Geschw'] * (1,-1)[int(gw_cp.vl["LWI_01"]['LWI_VZ_Lenkradwinkel'])] |
||||
self.steeringTorque = gw_cp.vl["EPS_01"]['Driver_Strain'] * (1,-1)[int(gw_cp.vl["EPS_01"]['Driver_Strain_VZ'])] |
||||
self.steeringPressed = abs(self.steeringTorque) > CarControllerParams.STEER_DRIVER_ALLOWANCE |
||||
self.yawRate = gw_cp.vl["ESP_02"]['ESP_Gierrate'] * (1,-1)[int(gw_cp.vl["ESP_02"]['ESP_VZ_Gierrate'])] * CV.DEG_TO_RAD |
||||
|
||||
# Update gas, brakes, and gearshift. |
||||
self.gas = gw_cp.vl["Motor_20"]['MO_Fahrpedalrohwert_01'] / 100.0 |
||||
self.gasPressed = self.gas > 0 |
||||
self.brake = gw_cp.vl["ESP_05"]['ESP_Bremsdruck'] / 250.0 # FIXME: this is pressure in Bar, not sure what OP expects |
||||
self.brakePressed = bool(gw_cp.vl["ESP_05"]['ESP_Fahrer_bremst']) |
||||
self.brakeLights = bool(gw_cp.vl["ESP_05"]['ESP_Status_Bremsdruck']) |
||||
|
||||
# Update gear and/or clutch position data. |
||||
can_gear_shifter = int(gw_cp.vl["Getriebe_11"]['GE_Fahrstufe']) |
||||
self.gearShifter = parse_gear_shifter(can_gear_shifter, self.shifter_values) |
||||
|
||||
# Update door and trunk/hatch lid open status. |
||||
self.doorOpen = any([gw_cp.vl["Gateway_72"]['ZV_FT_offen'], |
||||
gw_cp.vl["Gateway_72"]['ZV_BT_offen'], |
||||
gw_cp.vl["Gateway_72"]['ZV_HFS_offen'], |
||||
gw_cp.vl["Gateway_72"]['ZV_HBFS_offen'], |
||||
gw_cp.vl["Gateway_72"]['ZV_HD_offen']]) |
||||
|
||||
# Update seatbelt fastened status. |
||||
self.seatbeltUnlatched = False if gw_cp.vl["Airbag_02"]["AB_Gurtschloss_FA"] == 3 else True |
||||
|
||||
# Update driver preference for metric. VW stores many different unit |
||||
# preferences, including separate units for for distance vs. speed. |
||||
# We use the speed preference for OP. |
||||
self.displayMetricUnits = not gw_cp.vl["Einheiten_01"]["KBI_MFA_v_Einheit_02"] |
||||
|
||||
# Update ACC radar status. |
||||
accStatus = ex_cp.vl["ACC_06"]['ACC_Status_ACC'] |
||||
if accStatus == 1: |
||||
# ACC okay but disabled |
||||
self.accFault = False |
||||
self.accAvailable = False |
||||
self.accEnabled = False |
||||
elif accStatus == 2: |
||||
# ACC okay and enabled, but not currently engaged |
||||
self.accFault = False |
||||
self.accAvailable = True |
||||
self.accEnabled = False |
||||
elif accStatus in [3, 4, 5]: |
||||
# ACC okay and enabled, currently engaged and regulating speed (3) or engaged with driver accelerating (4) or overrun (5) |
||||
self.accFault = False |
||||
self.accAvailable = True |
||||
self.accEnabled = True |
||||
else: |
||||
# ACC fault of some sort. Seen statuses 6 or 7 for CAN comms disruptions, visibility issues, etc. |
||||
self.accFault = True |
||||
self.accAvailable = False |
||||
self.accEnabled = False |
||||
|
||||
# Update ACC setpoint. When the setpoint is zero or there's an error, the |
||||
# radar sends a set-speed of ~90.69 m/s / 203mph. |
||||
self.accSetSpeed = ex_cp.vl["ACC_02"]['SetSpeed'] |
||||
if self.accSetSpeed > 90: self.accSetSpeed = 0 |
||||
|
||||
# Update control button states for turn signals and ACC controls. |
||||
self.buttonStates["leftBlinker"] = bool(gw_cp.vl["Gateway_72"]['BH_Blinker_li']) |
||||
self.buttonStates["leftBlinker"] = bool(gw_cp.vl["Gateway_72"]['BH_Blinker_re']) |
||||
self.buttonStates["accelCruise"] = bool(gw_cp.vl["GRA_ACC_01"]['GRA_Tip_Hoch']) |
||||
self.buttonStates["decelCruise"] = bool(gw_cp.vl["GRA_ACC_01"]['GRA_Tip_Runter']) |
||||
self.buttonStates["cancel"] = bool(gw_cp.vl["GRA_ACC_01"]['GRA_Abbrechen']) |
||||
self.buttonStates["setCruise"] = bool(gw_cp.vl["GRA_ACC_01"]['GRA_Tip_Setzen']) |
||||
self.buttonStates["resumeCruise"] = bool(gw_cp.vl["GRA_ACC_01"]['GRA_Tip_Wiederaufnahme']) |
||||
self.buttonStates["gapAdjustCruise"] = bool(gw_cp.vl["GRA_ACC_01"]['GRA_Verstellung_Zeitluecke']) |
||||
|
||||
# Read ACC hardware button type configuration info that has to pass thru |
||||
# to the radar. Ends up being different for steering wheel buttons vs |
||||
# third stalk type controls. |
||||
self.graHauptschalter = gw_cp.vl["GRA_ACC_01"]['GRA_Hauptschalter'] |
||||
self.graTypHauptschalter = gw_cp.vl["GRA_ACC_01"]['GRA_Typ_Hauptschalter'] |
||||
self.graButtonTypeInfo = gw_cp.vl["GRA_ACC_01"]['GRA_ButtonTypeInfo'] |
||||
self.graTipStufe2 = gw_cp.vl["GRA_ACC_01"]['GRA_Tip_Stufe_2'] |
||||
# Pick up the GRA_ACC_01 CAN message counter so we can sync to it for |
||||
# later cruise-control button spamming. |
||||
self.graMsgBusCounter = gw_cp.vl["GRA_ACC_01"]['COUNTER'] |
||||
|
||||
# Check to make sure the electric power steering rack is configured to |
||||
# accept and respond to HCA_01 messages and has not encountered a fault. |
||||
self.steeringFault = not gw_cp.vl["EPS_01"]["HCA_Ready"] |
||||
|
||||
# Additional safety checks performed in CarInterface. |
||||
self.parkingBrakeSet = bool(gw_cp.vl["Kombi_01"]['KBI_Handbremse']) # FIXME: need to include an EPB check as well |
||||
self.stabilityControlDisabled = gw_cp.vl["ESP_21"]['ESP_Tastung_passiv'] |
||||
|
||||
@ -0,0 +1,242 @@ |
||||
from cereal import car |
||||
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.volkswagen.values import CAR, BUTTON_STATES |
||||
from selfdrive.car.volkswagen.carstate import CarState, get_mqb_gateway_can_parser, get_mqb_extended_can_parser |
||||
from common.params import Params |
||||
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint |
||||
from selfdrive.car.interfaces import CarInterfaceBase |
||||
|
||||
GEAR = car.CarState.GearShifter |
||||
|
||||
class CANBUS: |
||||
gateway = 0 |
||||
extended = 2 |
||||
|
||||
class CarInterface(CarInterfaceBase): |
||||
def __init__(self, CP, CarController): |
||||
self.CP = CP |
||||
self.CC = None |
||||
|
||||
self.frame = 0 |
||||
|
||||
self.gasPressedPrev = False |
||||
self.brakePressedPrev = False |
||||
self.cruiseStateEnabledPrev = False |
||||
self.displayMetricUnitsPrev = None |
||||
self.buttonStatesPrev = BUTTON_STATES.copy() |
||||
|
||||
# *** init the major players *** |
||||
self.CS = CarState(CP, CANBUS) |
||||
self.VM = VehicleModel(CP) |
||||
self.gw_cp = get_mqb_gateway_can_parser(CP, CANBUS) |
||||
self.ex_cp = get_mqb_extended_can_parser(CP, CANBUS) |
||||
|
||||
# sending if read only is False |
||||
if CarController is not None: |
||||
self.CC = CarController(CANBUS, CP.carFingerprint) |
||||
|
||||
@staticmethod |
||||
def compute_gb(accel, speed): |
||||
return float(accel) / 4.0 |
||||
|
||||
@staticmethod |
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay=False): |
||||
ret = car.CarParams.new_message() |
||||
|
||||
ret.carFingerprint = candidate |
||||
ret.isPandaBlack = has_relay |
||||
ret.carVin = vin |
||||
|
||||
if candidate == CAR.GOLF: |
||||
# Set common MQB parameters that will apply globally |
||||
ret.carName = "volkswagen" |
||||
ret.safetyModel = car.CarParams.SafetyModel.volkswagen |
||||
ret.enableCruise = True # Stock ACC still controls acceleration and braking |
||||
ret.openpilotLongitudinalControl = False |
||||
ret.steerControlType = car.CarParams.SteerControlType.torque |
||||
# Steer torque is strongly rate limit and max value is decently high. Off to avoid false positives |
||||
ret.steerLimitAlert = False |
||||
|
||||
# Additional common MQB parameters that may be overridden per-vehicle |
||||
ret.steerRateCost = 0.5 |
||||
ret.steerActuatorDelay = 0.05 # Hopefully all MQB racks are similar here |
||||
ret.steerMaxBP = [0.] # m/s |
||||
ret.steerMaxV = [1.] |
||||
|
||||
# As a starting point for speed-adjusted lateral tuning, use the example |
||||
# map speed breakpoints from a VW Tiguan (SSP 399 page 9). It's unclear |
||||
# whether the driver assist map breakpoints have any direct bearing on |
||||
# HCA assist torque, but if they're good breakpoints for the driver, |
||||
# they're probably good breakpoints for HCA as well. OP won't be driving |
||||
# 250kph/155mph but it provides interpolation scaling above 100kmh/62mph. |
||||
ret.lateralTuning.pid.kpBP = [0., 15 * CV.KPH_TO_MS, 50 * CV.KPH_TO_MS] |
||||
ret.lateralTuning.pid.kiBP = [0., 15 * CV.KPH_TO_MS, 50 * CV.KPH_TO_MS] |
||||
|
||||
# FIXME: Per-vehicle parameters need to be reintegrated. |
||||
# For the time being, per-vehicle stuff is being archived since we |
||||
# can't auto-detect very well yet. Now that tuning is figured out, |
||||
# averaged params should work reasonably on a range of cars. Owners |
||||
# can tweak here, as needed, until we have car type auto-detection. |
||||
|
||||
ret.mass = 1700 + STD_CARGO_KG |
||||
ret.wheelbase = 2.75 |
||||
ret.centerToFront = ret.wheelbase * 0.45 |
||||
ret.steerRatio = 15.6 |
||||
ret.lateralTuning.pid.kf = 0.00006 |
||||
ret.lateralTuning.pid.kpV = [0.15, 0.25, 0.60] |
||||
ret.lateralTuning.pid.kiV = [0.05, 0.05, 0.05] |
||||
tire_stiffness_factor = 0.6 |
||||
|
||||
ret.enableCamera = True # Stock camera detection doesn't apply to VW |
||||
ret.transmissionType = car.CarParams.TransmissionType.automatic |
||||
ret.steerRatioRear = 0. |
||||
|
||||
# No support for OP longitudinal control on Volkswagen at this time. |
||||
ret.gasMaxBP = [0.] |
||||
ret.gasMaxV = [0.] |
||||
ret.brakeMaxBP = [0.] |
||||
ret.brakeMaxV = [0.] |
||||
ret.longitudinalTuning.deadzoneBP = [0.] |
||||
ret.longitudinalTuning.deadzoneV = [0.] |
||||
ret.longitudinalTuning.kpBP = [0.] |
||||
ret.longitudinalTuning.kpV = [0.] |
||||
ret.longitudinalTuning.kiBP = [0.] |
||||
ret.longitudinalTuning.kiV = [0.] |
||||
|
||||
# TODO: get actual value, for now starting with reasonable value for |
||||
# civic and scaling by mass and wheelbase |
||||
ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) |
||||
|
||||
# TODO: start from empirically derived lateral slip stiffness for the civic and scale by |
||||
# mass and CG position, so all cars will have approximately similar dyn behaviors |
||||
ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, |
||||
tire_stiffness_factor=tire_stiffness_factor) |
||||
|
||||
return ret |
||||
|
||||
# returns a car.CarState |
||||
def update(self, c, can_strings): |
||||
canMonoTimes = [] |
||||
events = [] |
||||
buttonEvents = [] |
||||
params = Params() |
||||
ret = car.CarState.new_message() |
||||
|
||||
# Process the most recent CAN message traffic, and check for validity |
||||
self.gw_cp.update_strings(can_strings) |
||||
self.ex_cp.update_strings(can_strings) |
||||
self.CS.update(self.gw_cp, self.ex_cp) |
||||
ret.canValid = self.gw_cp.can_valid and self.ex_cp.can_valid |
||||
|
||||
# Wheel and vehicle speed, yaw rate |
||||
ret.wheelSpeeds.fl = self.CS.wheelSpeedFL |
||||
ret.wheelSpeeds.fr = self.CS.wheelSpeedFR |
||||
ret.wheelSpeeds.rl = self.CS.wheelSpeedRL |
||||
ret.wheelSpeeds.rr = self.CS.wheelSpeedRR |
||||
ret.vEgoRaw = self.CS.vEgoRaw |
||||
ret.vEgo = self.CS.vEgo |
||||
ret.aEgo = self.CS.aEgo |
||||
ret.standstill = self.CS.standstill |
||||
|
||||
# Steering wheel position, movement, yaw rate, and driver input |
||||
ret.steeringAngle = self.CS.steeringAngle |
||||
ret.steeringRate = self.CS.steeringRate |
||||
ret.steeringTorque = self.CS.steeringTorque |
||||
ret.steeringPressed = self.CS.steeringPressed |
||||
ret.yawRate = self.CS.yawRate |
||||
|
||||
# Gas, brakes and shifting |
||||
ret.gas = self.CS.gas |
||||
ret.gasPressed = self.CS.gasPressed |
||||
ret.brake = self.CS.brake |
||||
ret.brakePressed = self.CS.brakePressed |
||||
ret.brakeLights = self.CS.brakeLights |
||||
ret.gearShifter = self.CS.gearShifter |
||||
|
||||
# Doors open, seatbelt unfastened |
||||
ret.doorOpen = self.CS.doorOpen |
||||
ret.seatbeltUnlatched = self.CS.seatbeltUnlatched |
||||
|
||||
# Update the EON metric configuration to match the car at first startup, |
||||
# or if there's been a change. |
||||
if self.CS.displayMetricUnits != self.displayMetricUnitsPrev: |
||||
params.put("IsMetric", "1" if self.CS.displayMetricUnits else "0") |
||||
|
||||
# Blinker switch updates |
||||
ret.leftBlinker = self.CS.buttonStates["leftBlinker"] |
||||
ret.rightBlinker = self.CS.buttonStates["rightBlinker"] |
||||
|
||||
# ACC cruise state |
||||
ret.cruiseState.available = self.CS.accAvailable |
||||
ret.cruiseState.enabled = self.CS.accEnabled |
||||
ret.cruiseState.speed = self.CS.accSetSpeed |
||||
|
||||
# Check for and process state-change events (button press or release) from |
||||
# the turn stalk switch or ACC steering wheel/control stalk buttons. |
||||
for button in self.CS.buttonStates: |
||||
if self.CS.buttonStates[button] != self.buttonStatesPrev[button]: |
||||
be = car.CarState.ButtonEvent.new_message() |
||||
be.type = button |
||||
be.pressed = self.CS.buttonStates[button] |
||||
buttonEvents.append(be) |
||||
|
||||
# Vehicle operation safety checks and events |
||||
if ret.doorOpen: |
||||
events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE])) |
||||
if ret.seatbeltUnlatched: |
||||
events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE])) |
||||
if ret.gearShifter == GEAR.reverse: |
||||
events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) |
||||
if not ret.gearShifter in [GEAR.drive, GEAR.eco, GEAR.sport]: |
||||
events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE])) |
||||
if self.CS.stabilityControlDisabled: |
||||
events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE])) |
||||
if self.CS.parkingBrakeSet: |
||||
events.append(create_event('parkBrake', [ET.NO_ENTRY, ET.USER_DISABLE])) |
||||
|
||||
# Vehicle health safety checks and events |
||||
if self.CS.accFault: |
||||
events.append(create_event('radarFault', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) |
||||
if self.CS.steeringFault: |
||||
events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING])) |
||||
|
||||
# Per the Comma safety model, disable on pedals rising edge or when brake |
||||
# is pressed and speed isn't zero. |
||||
if (ret.gasPressed and not self.gasPressedPrev) or \ |
||||
(ret.brakePressed and (not self.brakePressedPrev or not ret.standstill)): |
||||
events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) |
||||
if ret.gasPressed: |
||||
events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) |
||||
|
||||
# Engagement and longitudinal control using stock ACC. Make sure OP is |
||||
# disengaged if stock ACC is disengaged. |
||||
if not ret.cruiseState.enabled: |
||||
events.append(create_event('pcmDisable', [ET.USER_DISABLE])) |
||||
# Attempt OP engagement only on rising edge of stock ACC engagement. |
||||
elif not self.cruiseStateEnabledPrev: |
||||
events.append(create_event('pcmEnable', [ET.ENABLE])) |
||||
|
||||
ret.events = events |
||||
ret.buttonEvents = buttonEvents |
||||
ret.canMonoTimes = canMonoTimes |
||||
|
||||
# update previous car states |
||||
self.gasPressedPrev = ret.gasPressed |
||||
self.brakePressedPrev = ret.brakePressed |
||||
self.cruiseStateEnabledPrev = ret.cruiseState.enabled |
||||
self.displayMetricUnitsPrev = self.CS.displayMetricUnits |
||||
self.buttonStatesPrev = self.CS.buttonStates.copy() |
||||
|
||||
# cast to reader so it can't be modified |
||||
return ret.as_reader() |
||||
|
||||
def apply(self, c): |
||||
can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators, |
||||
c.hudControl.visualAlert, |
||||
c.hudControl.audibleAlert, |
||||
c.hudControl.leftLaneVisible, |
||||
c.hudControl.rightLaneVisible) |
||||
self.frame += 1 |
||||
return can_sends |
||||
@ -0,0 +1,5 @@ |
||||
#!/usr/bin/env python3 |
||||
from selfdrive.car.interfaces import RadarInterfaceBase |
||||
|
||||
class RadarInterface(RadarInterfaceBase): |
||||
pass |
||||
@ -0,0 +1,63 @@ |
||||
from selfdrive.car import dbc_dict |
||||
|
||||
class CarControllerParams: |
||||
HCA_STEP = 2 # HCA_01 message frequency 50Hz |
||||
LDW_STEP = 10 # LDW_02 message frequency 10Hz |
||||
GRA_ACC_STEP = 3 # GRA_ACC_01 message frequency 33Hz |
||||
|
||||
GRA_VBP_STEP = 100 # Send ACC virtual button presses once a second |
||||
GRA_VBP_COUNT = 16 # Send VBP messages for ~0.5s (GRA_ACC_STEP * 16) |
||||
|
||||
# Observed documented MQB limits: 3.00 Nm max, rate of change 5.00 Nm/sec. |
||||
# Limiting both torque and rate-of-change based on real-world testing and |
||||
# Comma's safety requirements for minimum time to lane departure. |
||||
STEER_MAX = 250 # Max heading control assist torque 2.50 Nm |
||||
STEER_DELTA_UP = 4 # Max HCA reached in 1.25s (STEER_MAX / (50Hz * 1.25)) |
||||
STEER_DELTA_DOWN = 10 # Min HCA reached in 0.60s (STEER_MAX / (50Hz * 0.60)) |
||||
STEER_DRIVER_ALLOWANCE = 80 |
||||
STEER_DRIVER_MULTIPLIER = 3 # weight driver torque heavily |
||||
STEER_DRIVER_FACTOR = 1 # from dbc |
||||
|
||||
BUTTON_STATES = { |
||||
"leftBlinker": False, |
||||
"rightBlinker": False, |
||||
"accelCruise": False, |
||||
"decelCruise": False, |
||||
"cancel": False, |
||||
"setCruise": False, |
||||
"resumeCruise": False, |
||||
"gapAdjustCruise": False |
||||
} |
||||
|
||||
MQB_LDW_MESSAGES = { |
||||
"none": 0, # Nothing to display |
||||
"laneAssistUnavailChime": 1, # "Lane Assist currently not available." with chime |
||||
"laneAssistUnavailNoSensorChime": 3, # "Lane Assist not available. No sensor view." with chime |
||||
"laneAssistTakeOverUrgent": 4, # "Lane Assist: Please Take Over Steering" with urgent beep |
||||
"emergencyAssistUrgent": 6, # "Emergency Assist: Please Take Over Steering" with urgent beep |
||||
"laneAssistTakeOverChime": 7, # "Lane Assist: Please Take Over Steering" with chime |
||||
"laneAssistTakeOverSilent": 8, # "Lane Assist: Please Take Over Steering" silent |
||||
"emergencyAssistChangingLanes": 9, # "Emergency Assist: Changing lanes..." with urgent beep |
||||
"laneAssistDeactivated": 10, # "Lane Assist deactivated." silent with persistent icon afterward |
||||
} |
||||
|
||||
class CAR: |
||||
GOLF = "Volkswagen Golf" |
||||
|
||||
FINGERPRINTS = { |
||||
CAR.GOLF: [ |
||||
# 76b83eb0245de90e|2019-10-21--17-40-42 - jyoung8607 car |
||||
{64: 8, 134: 8, 159: 8, 173: 8, 178: 8, 253: 8, 257: 8, 260: 8, 262: 8, 264: 8, 278: 8, 279: 8, 283: 8, 286: 8, 288: 8, 289: 8, 290: 8, 294: 8, 299: 8, 302: 8, 346: 8, 385: 8, 418: 8, 427: 8, 668: 8, 679: 8, 681: 8, 695: 8, 779: 8, 780: 8, 783: 8, 792: 8, 795: 8, 804: 8, 806: 8, 807: 8, 808: 8, 809: 8, 870: 8, 896: 8, 897: 8, 898: 8, 901: 8, 917: 8, 919: 8, 949: 8, 958: 8, 960: 4, 981: 8, 987: 8, 988: 8, 991: 8, 997: 8, 1000: 8, 1019: 8, 1120: 8, 1122: 8, 1123: 8, 1124: 8, 1153: 8, 1162: 8, 1175: 8, 1312: 8, 1385: 8, 1413: 8, 1440: 5, 1514: 8, 1515: 8, 1520: 8, 1600: 8, 1601: 8, 1603: 8, 1605: 8, 1624: 8, 1626: 8, 1629: 8, 1631: 8, 1646: 8, 1648: 8, 1712: 6, 1714: 8, 1716: 8, 1717: 8, 1719: 8, 1720: 8, 1721: 8 |
||||
}], |
||||
} |
||||
|
||||
class ECU: |
||||
CAM = 0 |
||||
|
||||
ECU_FINGERPRINT = { |
||||
ECU.CAM: [294, 919], # HCA_01 Heading Control Assist, LDW_02 Lane Departure Warning |
||||
} |
||||
|
||||
DBC = { |
||||
CAR.GOLF: dbc_dict('vw_mqb_2010', None), |
||||
} |
||||
@ -0,0 +1,52 @@ |
||||
# CAN controls for MQB platform Volkswagen, Audi, Skoda and SEAT. |
||||
# PQ35/PQ46/NMS, and any future MLB, to come later. |
||||
|
||||
def create_mqb_steering_control(packer, bus, apply_steer, idx, lkas_enabled): |
||||
values = { |
||||
"SET_ME_0X3": 0x3, |
||||
"Assist_Torque": abs(apply_steer), |
||||
"Assist_Requested": lkas_enabled, |
||||
"Assist_VZ": 1 if apply_steer < 0 else 0, |
||||
"HCA_Available": 1, |
||||
"HCA_Standby": not lkas_enabled, |
||||
"HCA_Active": lkas_enabled, |
||||
"SET_ME_0XFE": 0xFE, |
||||
"SET_ME_0X07": 0x07, |
||||
} |
||||
return packer.make_can_msg("HCA_01", bus, values, idx) |
||||
|
||||
def create_mqb_hud_control(packer, bus, hca_enabled, steering_pressed, hud_alert, leftLaneVisible, rightLaneVisible): |
||||
|
||||
if hca_enabled: |
||||
leftlanehud = 3 if leftLaneVisible else 1 |
||||
rightlanehud = 3 if rightLaneVisible else 1 |
||||
else: |
||||
leftlanehud = 2 if leftLaneVisible else 1 |
||||
rightlanehud = 2 if rightLaneVisible else 1 |
||||
|
||||
values = { |
||||
"LDW_Unknown": 2, # FIXME: possible speed or attention relationship |
||||
"Kombi_Lamp_Orange": 1 if hca_enabled and steering_pressed else 0, |
||||
"Kombi_Lamp_Green": 1 if hca_enabled and not steering_pressed else 0, |
||||
"Left_Lane_Status": leftlanehud, |
||||
"Right_Lane_Status": rightlanehud, |
||||
"Alert_Message": hud_alert, |
||||
} |
||||
return packer.make_can_msg("LDW_02", bus, values) |
||||
|
||||
def create_mqb_acc_buttons_control(packer, bus, buttonStatesToSend, CS, idx): |
||||
values = { |
||||
"GRA_Hauptschalter": CS.graHauptschalter, |
||||
"GRA_Abbrechen": buttonStatesToSend["cancel"], |
||||
"GRA_Tip_Setzen": buttonStatesToSend["setCruise"], |
||||
"GRA_Tip_Hoch": buttonStatesToSend["accelCruise"], |
||||
"GRA_Tip_Runter": buttonStatesToSend["decelCruise"], |
||||
"GRA_Tip_Wiederaufnahme": buttonStatesToSend["resumeCruise"], |
||||
"GRA_Verstellung_Zeitluecke": 3 if buttonStatesToSend["gapAdjustCruise"] else 0, |
||||
"GRA_Typ_Hauptschalter": CS.graTypHauptschalter, |
||||
"GRA_Codierung": 2, |
||||
"GRA_Tip_Stufe_2": CS.graTipStufe2, |
||||
"GRA_ButtonTypeInfo": CS.graButtonTypeInfo |
||||
} |
||||
|
||||
return packer.make_can_msg("GRA_ACC_01", bus, values, idx) |
||||
@ -1 +1 @@ |
||||
#define COMMA_VERSION "0.6.5-release" |
||||
#define COMMA_VERSION "0.6.6-release" |
||||
|
||||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in new issue