Vehicle Researcher
6 years ago
committed by
Willem Melching
parent
bb3ef635d2
commit
b713eae9f8
148 changed files with 3706 additions and 2935 deletions
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:8ed482abc0c75c5606769e049088894156f68864dd2b85fc769311de781e3998 |
||||
size 2507 |
||||
oid sha256:8c6eff576ba07fc45d0257241e024075ac4e3d6c3391ed75d496f140d146e72b |
||||
size 2535 |
||||
|
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:a9dfee10cd1c1bacb443032aecaa4e785ad28b5f407f1c2dc42c0d54f7a42b6e |
||||
size 150547 |
||||
oid sha256:eeb5d8af1db839a0bf1a594af50c52d0d79dc6e764569819dc8e78dbb039090c |
||||
size 159193 |
||||
|
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:8cf70342d6d92801517e068eee81f10df6b52de2222efd5df0f17b3e600e98b7 |
||||
size 17786424 |
||||
oid sha256:930595cab227f8288a5dafaa04d3e71c98166c55cf6399e25e46f51237f1ac98 |
||||
size 17796274 |
||||
|
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:cb1eea1b9d0e69da7aa07612bc6a72c36da9dd24b14eaa80191ecb8b9eca1841 |
||||
size 16950452 |
||||
oid sha256:78aa889fef50c33dd1020db85621dc28479ba0a346e75763e29539a36adcfbd7 |
||||
size 16950491 |
||||
|
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:4e22c4152adf797e0b2aab048037871f1988ffce9339e618c9bb950f38b4ca99 |
||||
size 601956 |
||||
oid sha256:e6ea68609a1477fb55e27b066af5c65f343b930eaf8ade38d91df68ec594c5a7 |
||||
size 158713 |
||||
|
||||
Binary file not shown.
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:125c5a5d7287a958f8287b7aa5af5b987e687ceb96dc7bc31704597efc87db6c |
||||
size 1363604 |
||||
oid sha256:c2adec93de45ff7175884afcef36b875d0c47d3d28f0d04c42bff825500cd4f1 |
||||
size 1277638 |
||||
|
||||
@ -0,0 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:0429a115567270191288ad578fa2a784396216a90622eb29a9815164dcef9315 |
||||
size 1140804 |
||||
@ -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