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 |
version https://git-lfs.github.com/spec/v1 |
||||||
oid sha256:8ed482abc0c75c5606769e049088894156f68864dd2b85fc769311de781e3998 |
oid sha256:8c6eff576ba07fc45d0257241e024075ac4e3d6c3391ed75d496f140d146e72b |
||||||
size 2507 |
size 2535 |
||||||
|
|||||||
@ -1,3 +1,3 @@ |
|||||||
version https://git-lfs.github.com/spec/v1 |
version https://git-lfs.github.com/spec/v1 |
||||||
oid sha256:a9dfee10cd1c1bacb443032aecaa4e785ad28b5f407f1c2dc42c0d54f7a42b6e |
oid sha256:eeb5d8af1db839a0bf1a594af50c52d0d79dc6e764569819dc8e78dbb039090c |
||||||
size 150547 |
size 159193 |
||||||
|
|||||||
@ -1,3 +1,3 @@ |
|||||||
version https://git-lfs.github.com/spec/v1 |
version https://git-lfs.github.com/spec/v1 |
||||||
oid sha256:8cf70342d6d92801517e068eee81f10df6b52de2222efd5df0f17b3e600e98b7 |
oid sha256:930595cab227f8288a5dafaa04d3e71c98166c55cf6399e25e46f51237f1ac98 |
||||||
size 17786424 |
size 17796274 |
||||||
|
|||||||
@ -1,3 +1,3 @@ |
|||||||
version https://git-lfs.github.com/spec/v1 |
version https://git-lfs.github.com/spec/v1 |
||||||
oid sha256:cb1eea1b9d0e69da7aa07612bc6a72c36da9dd24b14eaa80191ecb8b9eca1841 |
oid sha256:78aa889fef50c33dd1020db85621dc28479ba0a346e75763e29539a36adcfbd7 |
||||||
size 16950452 |
size 16950491 |
||||||
|
|||||||
@ -1,3 +1,3 @@ |
|||||||
version https://git-lfs.github.com/spec/v1 |
version https://git-lfs.github.com/spec/v1 |
||||||
oid sha256:4e22c4152adf797e0b2aab048037871f1988ffce9339e618c9bb950f38b4ca99 |
oid sha256:e6ea68609a1477fb55e27b066af5c65f343b930eaf8ade38d91df68ec594c5a7 |
||||||
size 601956 |
size 158713 |
||||||
|
|||||||
Binary file not shown.
@ -1,3 +1,3 @@ |
|||||||
version https://git-lfs.github.com/spec/v1 |
version https://git-lfs.github.com/spec/v1 |
||||||
oid sha256:125c5a5d7287a958f8287b7aa5af5b987e687ceb96dc7bc31704597efc87db6c |
oid sha256:c2adec93de45ff7175884afcef36b875d0c47d3d28f0d04c42bff825500cd4f1 |
||||||
size 1363604 |
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 |
#!/bin/bash |
||||||
set -e |
set -e |
||||||
|
|
||||||
|
SETUP="cd /tmp/openpilot && make -C cereal && " |
||||||
|
|
||||||
docker build -t tmppilot -f Dockerfile.openpilot . |
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/ && 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 "$SETUP 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 "$SETUP make -C selfdrive/can -j4 && 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 "$SETUP 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 "$SETUP make -C selfdrive/can -j4 && 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 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 'cd /tmp/openpilot/selfdrive/test/longitudinal_maneuvers && OPTEST=1 ./test_longitudinal.py' |
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 'cd /tmp/openpilot/ && make -C cereal && cd /tmp/openpilot/selfdrive/test/process_replay/ && ./test_processes.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 '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 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