sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

openpilot release

Browse Source 
old-commit-hash: e94a30bec0
commatwo_master v0.1
Vehicle Researcher 9 years ago
commit 79ca8c9ec9
118 changed files with 23940 additions and 0 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 87
      .gitattributes
  2. 7
      LICENSE.openpilot
  3. 93
      README.md
  4. 36
      SAFETY.md
  5. 14
      TODO.md
  6. 43
      board/Makefile
  7. 38
      board/adc.h
  8. 75
      board/can.h
  9. 16
      board/dac.h
  10. 3
      board/inc/core_cm3.h
  11. 3
      board/inc/core_cmFunc.h
  12. 3
      board/inc/core_cmInstr.h
  13. 3
      board/inc/stm32f205xx.h
  14. 3
      board/inc/stm32f2xx.h
  15. 3
      board/inc/system_stm32f2xx.h
  16. 226
      board/libc.h
  17. 499
      board/main.c
  18. 511
      board/startup_stm32f205xx.s
  19. 163
      board/stm32_flash.ld
  20. 7
      board/timer.h
  21. 3
      board/tools/dfu-util-aarch64
  22. 3
      board/tools/dfu-util-x86_64
  23. 20
      board/tools/enter_download_mode.py
  24. 510
      board/usb.h
  25. 7
      cereal/__init__.py
  26. 26
      cereal/c++.capnp
  27. 0
      cereal/gen/c/c++.capnp.h
  28. 1016
      cereal/gen/c/log.capnp.c
  29. 667
      cereal/gen/c/log.capnp.h
  30. 3732
      cereal/gen/cpp/log.capnp.c++
  31. 7251
      cereal/gen/cpp/log.capnp.h
  32. 272
      cereal/log.capnp
  33. 0
      common/__init__.py
  34. 8
      common/api/__init__.py
  35. 26
      common/crash.py
  36. 182
      common/dbc.py
  37. 17
      common/filters.py
  38. 0
      common/kalman/__init__.py
  39. 300
      common/kalman/ekf.py
  40. 134
      common/logging_extra.py
  41. 2
      common/numpy_fast.py
  42. 94
      common/realtime.py
  43. 82
      common/services.py
  44. 2
      dbcs/__init__.py
  45. 295
      dbcs/acura_ilx_2016_can.dbc
  46. 175
      dbcs/acura_ilx_2016_nidec.dbc
  47. 319
      dbcs/honda_civic_touring_2016_can.dbc
  48. 28
      installation/files/continue.sh
  49. 27
      installation/files/id_rsa_openpilot_ro
  50. 11
      installation/install.sh
  51. 3
      phonelibs/hierarchy/lib/_hierarchy.so
  52. 3
      phonelibs/nanovg/fontstash.h
  53. 3
      phonelibs/nanovg/nanovg.c
  54. 3
      phonelibs/nanovg/nanovg.h
  55. 3
      phonelibs/nanovg/nanovg_gl.h
  56. 3
      phonelibs/nanovg/nanovg_gl_utils.h
  57. 3
      phonelibs/nanovg/stb_image.h
  58. 3
      phonelibs/nanovg/stb_truetype.h
  59. 0
      selfdrive/__init__.py
  60. 3
      selfdrive/assets/Roboto-Bold.ttf
  61. 3
      selfdrive/assets/courbd.ttf
  62. 73
      selfdrive/boardd/Makefile
  63. 0
      selfdrive/boardd/__init__.py
  64. 322
      selfdrive/boardd/boardd.cc
  65. 179
      selfdrive/boardd/boardd.py
  66. 0
      selfdrive/calibrationd/__init__.py
  67. 208
      selfdrive/calibrationd/calibration.py
  68. 116
      selfdrive/calibrationd/calibrationd.py
  69. 135
      selfdrive/common/framebuffer.cc
  70. 21
      selfdrive/common/framebuffer.h
  71. 68
      selfdrive/common/mat.h
  72. 23
      selfdrive/common/modeldata.h
  73. 13
      selfdrive/common/mutex.h
  74. 90
      selfdrive/common/swaglog.c
  75. 26
      selfdrive/common/swaglog.h
  76. 31
      selfdrive/common/timing.h
  77. 22
      selfdrive/common/util.h
  78. 127
      selfdrive/common/visionipc.c
  79. 50
      selfdrive/common/visionipc.h
  80. 68
      selfdrive/config.py
  81. 0
      selfdrive/controls/__init__.py
  82. 358
      selfdrive/controls/controlsd.py
  83. 0
      selfdrive/controls/lib/__init__.py
  84. 332
      selfdrive/controls/lib/adaptivecruise.py
  85. 178
      selfdrive/controls/lib/alert_database.py
  86. 123
      selfdrive/controls/lib/can_parser.py
  87. 185
      selfdrive/controls/lib/carcontroller.py
  88. 275
      selfdrive/controls/lib/carstate.py
  89. 52
      selfdrive/controls/lib/drive_helpers.py
  90. 8
      selfdrive/controls/lib/fingerprints.py
  91. 88
      selfdrive/controls/lib/hondacan.py
  92. 120
      selfdrive/controls/lib/latcontrol.py
  93. 232
      selfdrive/controls/lib/longcontrol.py
  94. 63
      selfdrive/controls/lib/pathplanner.py
  95. 256
      selfdrive/controls/lib/radar_helpers.py
  96. 268
      selfdrive/controls/radard.py
  97. 58
      selfdrive/logcatd/Makefile
  98. 68
      selfdrive/logcatd/logcatd.cc
  99. 0
      selfdrive/loggerd/__init__.py
  100. 9
      selfdrive/loggerd/config.py
  101. Some files were not shown because too many files have changed in this diff Show More

87
.gitattributes vendored
Unescape View File

@ -0,0 +1,87 @@
*.ico filter=lfs diff=lfs merge=lfs -text
*.dlc filter=lfs diff=lfs merge=lfs -text
*.onnx filter=lfs diff=lfs merge=lfs -text
*.svg filter=lfs diff=lfs merge=lfs -text
*.png filter=lfs diff=lfs merge=lfs -text
*.gif filter=lfs diff=lfs merge=lfs -text
*.ttf filter=lfs diff=lfs merge=lfs -text
*.wav filter=lfs diff=lfs merge=lfs -text
system/hardware/tici/updater filter=lfs diff=lfs merge=lfs -text
selfdrive/ui/qt/spinner_larch64 filter=lfs diff=lfs merge=lfs -text
selfdrive/ui/qt/text_larch64 filter=lfs diff=lfs merge=lfs -text
third_party/**/*.a filter=lfs diff=lfs merge=lfs -text
third_party/**/*.so filter=lfs diff=lfs merge=lfs -text
third_party/**/*.so.* filter=lfs diff=lfs merge=lfs -text
third_party/**/*.dylib filter=lfs diff=lfs merge=lfs -text
third_party/acados/*/t_renderer filter=lfs diff=lfs merge=lfs -text
third_party/qt5/larch64/bin/lrelease filter=lfs diff=lfs merge=lfs -text
third_party/qt5/larch64/bin/lupdate filter=lfs diff=lfs merge=lfs -text
third_party/catch2/include/catch2/catch.hpp filter=lfs diff=lfs merge=lfs -text
*.apk filter=lfs diff=lfs merge=lfs -text
*.apkpatch filter=lfs diff=lfs merge=lfs -text
*.jar filter=lfs diff=lfs merge=lfs -text
*.pdf filter=lfs diff=lfs merge=lfs -text
*.jpg filter=lfs diff=lfs merge=lfs -text
*.mp3 filter=lfs diff=lfs merge=lfs -text
*.thneed filter=lfs diff=lfs merge=lfs -text
*.tar.gz filter=lfs diff=lfs merge=lfs -text
*.npy filter=lfs diff=lfs merge=lfs -text
*.csv filter=lfs diff=lfs merge=lfs -text
*.a filter=lfs diff=lfs merge=lfs -text
*.so* filter=lfs diff=lfs merge=lfs -text
*.dylib filter=lfs diff=lfs merge=lfs -text
*.o filter=lfs diff=lfs merge=lfs -text
*.b64 filter=lfs diff=lfs merge=lfs -text
selfdrive/hardware/tici/updater filter=lfs diff=lfs merge=lfs -text
selfdrive/boardd/tests/test_boardd filter=lfs diff=lfs merge=lfs -text
selfdrive/ui/qt/spinner_aarch64 filter=lfs diff=lfs merge=lfs -text
installer/updater/updater filter=lfs diff=lfs merge=lfs -text
selfdrive/debug/profiling/simpleperf/**/* filter=lfs diff=lfs merge=lfs -text
selfdrive/hardware/eon/updater filter=lfs diff=lfs merge=lfs -text
selfdrive/ui/qt/text_aarch64 filter=lfs diff=lfs merge=lfs -text
selfdrive/debug/profiling/pyflame/**/* filter=lfs diff=lfs merge=lfs -text
installer/installers/installer_openpilot filter=lfs diff=lfs merge=lfs -text
installer/installers/installer_dashcam filter=lfs diff=lfs merge=lfs -text
selfdrive/ui/text/text filter=lfs diff=lfs merge=lfs -text
selfdrive/ui/android/text/text filter=lfs diff=lfs merge=lfs -text
selfdrive/ui/spinner/spinner filter=lfs diff=lfs merge=lfs -text
selfdrive/visiond/visiond filter=lfs diff=lfs merge=lfs -text
selfdrive/loggerd/loggerd filter=lfs diff=lfs merge=lfs -text
selfdrive/sensord/sensord filter=lfs diff=lfs merge=lfs -text
selfdrive/sensord/gpsd filter=lfs diff=lfs merge=lfs -text
selfdrive/ui/android/spinner/spinner filter=lfs diff=lfs merge=lfs -text
selfdrive/ui/qt/spinner filter=lfs diff=lfs merge=lfs -text
selfdrive/ui/qt/text filter=lfs diff=lfs merge=lfs -text
_stringdefs.py filter=lfs diff=lfs merge=lfs -text
dfu-util-aarch64-linux filter=lfs diff=lfs merge=lfs -text
dfu-util-aarch64 filter=lfs diff=lfs merge=lfs -text
dfu-util-x86_64-linux filter=lfs diff=lfs merge=lfs -text
dfu-util-x86_64 filter=lfs diff=lfs merge=lfs -text
stb_image.h filter=lfs diff=lfs merge=lfs -text
clpeak3 filter=lfs diff=lfs merge=lfs -text
clwaste filter=lfs diff=lfs merge=lfs -text
apk/**/* filter=lfs diff=lfs merge=lfs -text
external/**/* filter=lfs diff=lfs merge=lfs -text
phonelibs/**/* filter=lfs diff=lfs merge=lfs -text
third_party/boringssl/**/* filter=lfs diff=lfs merge=lfs -text
pyextra/**/* filter=lfs diff=lfs merge=lfs -text
panda/board/**/inc/*.h filter=lfs diff=lfs merge=lfs -text
panda/board/obj/*.elf filter=lfs diff=lfs merge=lfs -text
board/inc/*.h filter=lfs diff=lfs merge=lfs -text
third_party/nanovg/**/* filter=lfs diff=lfs merge=lfs -text
selfdrive/controls/lib/lateral_mpc/lib_mpc_export/**/* filter=lfs diff=lfs merge=lfs -text
third_party/android_hardware_libhardware/**/* filter=lfs diff=lfs merge=lfs -text
selfdrive/controls/lib/lead_mpc_lib/lib_mpc_export/**/* filter=lfs diff=lfs merge=lfs -text
*.pro filter=lfs diff=lfs merge=lfs -text
selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/**/* filter=lfs diff=lfs merge=lfs -text
selfdrive/controls/lib/lateral_mpc/mpc_export/**/* filter=lfs diff=lfs merge=lfs -text
third_party/curl/**/* filter=lfs diff=lfs merge=lfs -text
selfdrive/modeld/thneed/debug/**/* filter=lfs diff=lfs merge=lfs -text
selfdrive/modeld/thneed/include/**/* filter=lfs diff=lfs merge=lfs -text
third_party/openmax/**/* filter=lfs diff=lfs merge=lfs -text
selfdrive/controls/lib/longitudinal_mpc/mpc_export/**/* filter=lfs diff=lfs merge=lfs -text
selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/**/* filter=lfs diff=lfs merge=lfs -text
Pipfile filter=lfs diff=lfs merge=lfs -text
Pipfile.lock filter=lfs diff=lfs merge=lfs -text
poetry.lock filter=lfs diff=lfs merge=lfs -text
*.qm filter=lfs diff=lfs merge=lfs -text

7
LICENSE.openpilot
Unescape View File

@ -0,0 +1,7 @@
Copyright (c) 2016, comma.ai
Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

93
README.md
Unescape View File

@ -0,0 +1,93 @@
Welcome to openpilot
======
[openpilot](http://github.com/commaai/openpilot) is an open source driving agent.
Currently it performs the functions of Adaptive Cruise Control (ACC) and Lane Keeping Assist System (LKAS) for Hondas and Acuras. It's about on par with Tesla Autopilot at launch, and better than [all other manufacturers](http://www.thedrive.com/tech/5707/the-war-for-autonomous-driving-part-iii-us-vs-germany-vs-japan).
The openpilot codebase has been written to be concise and enable rapid prototyping. We look forward to your contributions - improving real vehicle automation has never been easier.
Hardware
------
Right now openpilot supports the [neo research platform](http://github.com/commaai/neo) for vehicle control. We'd like to support [Open Source Car Control](https://github.com/PolySync/OSCC) as well.
To install it on the NEO:
```bash
# Requires working adb in PATH
cd installation
./install.sh
```
Supported Cars
------
- Acura ILX 2016 with AcuraWatch Plus
- Limitations: Due to use of the cruise control for gas, it can only be enabled above 25 mph
- Honda Civic 2016 Touring Edition
- Limitations: Due to limitations in steering firmware, steering is disabled below 18 mph
Directory structure
------
- board -- Code that runs on the USB interface board
- cereal -- The messaging spec used for all logs on the phone
- common -- Library like functionality we've developed here
- dbcs -- Files showing how to interpret data from cars
- installation -- Installation on the neo platform
- phonelibs -- Libraries used on the phone
- selfdrive -- Code needed to drive the car
- assets -- Fonts for ui
- boardd -- Daemon to talk to the board
- calibrationd -- Camera calibration server
- common -- Shared C/C++ code for the daemons
- controls -- Python controls (PID loops etc) for the car
- logcatd -- Android logcat as a service
- loggerd -- Logger and uploader of car data
- sensord -- IMU / GPS interface code
- ui -- The UI
- visiond -- embedded vision pipeline
To understand how the services interact, see `common/services.py`
Adding Car Support
------
It should be relatively easy to add support for the Honda CR-V Touring. The brake message is the same. Steering has a slightly different message with a different message id. Sniff CAN while using LKAS to find it.
The Honda Accord uses different signalling for the steering and probably requires new hardware.
Adding other manufacturers besides Honda/Acura is doable but will be more of an undertaking.
User Data / chffr Account / Crash Reporting
------
By default openpilot creates an account and includes a client for chffr, our dashcam app. We use your data to train better models and improve openpilot for everyone.
It's open source software, so you are free to disable it if you wish.
It logs the road facing camera, CAN, GPS, IMU, magnetometer, thermal sensors, crashes, and operating system logs.
It does not log the user facing camera or the microphone.
By using it, you agree to [our privacy policy](https://beta.comma.ai/privacy.html). You understand that use of this software or its related services will generate certain types of user data, which may be logged and stored at the sole discretion of comma.ai. By accepting this agreement, you grant an irrevocable, perpetual, worldwide right to comma.ai for the use of this data.
Contributing
------
We welcome both pull requests and issues on
[github](http://github.com/commaai/openpilot). See the TODO file for a list of
good places to start.
Licensing
------
openpilot is released under the MIT license.
**THIS IS ALPHA QUALITY SOFTWARE FOR RESEARCH PURPOSES ONLY. THIS IS NOT A PRODUCT.
YOU ARE RESPONSIBLE FOR COMPLYING WITH LOCAL LAWS AND REGULATIONS.
NO WARRANTY EXPRESSED OR IMPLIED.**

36
SAFETY.md
Unescape View File

@ -0,0 +1,36 @@
openpilot Safety
======
openpilot is an Adaptive Cruise Control and Lane Keeping Assist System. Like
other ACC and LKAS systems, openpilot requires the driver to be alert and to pay
attention at all times. We repeat, **driver alertness is necessary, but not
sufficient, for openpilot to be used safely**.
Even with an attentive driver, we must make further efforts for the system to be
safe. We have designed openpilot with two other safety considerations.
1. The vehicle must always be controllable by the driver.
2. The vehicle must not alter its trajectory too quickly for the driver to safely
react.
To address these, we came up with two safety principles.
1. Enforced disengagements. Step on either pedal or press the cancel button to
retake manual control of the car immediately.
- These are hard enforced by the board, and soft enforced by the software. The
green led on the board signifies if the board is allowing control messages.
- Honda CAN uses both a counter and a checksum to ensure integrity and prevent
replay of the same message.
2. Actuation limits. While the system is engaged, the actuators are constrained
to operate within reasonable limits; the same limits used by the stock system on
the Honda.
- Without an interceptor, the gas is controlled by the PCM. The PCM limits
acceleration to what is reasonable for a cruise control system. With an
interceptor, the gas is clipped to 60% in longcontrol.py
- The brake is controlled by the 0x1FA CAN message. This message allows full
braking, although the board and the software clip it to 1/4th of the max.
This is around .3g of braking.
- Steering is controlled by the 0xE4 CAN message. The EPS controller in the
car limits the torque to a very small amount, so regardless of the message,
the controller cannot jerk the wheel.

14
TODO.md
Unescape View File

@ -0,0 +1,14 @@
TODO
======
An incomplete list of known issues and desired featues.
- TX and RX amounts on UI are wrong for a few frames at startup because we
subtract (total sent - 0). We should initialize sent bytes before displaying.
- Rewrite common/dbc.py to be faster and cleaner, potentially in C++.
- Add module and class level documentation where appropriate.
- Fix lock file cleanup so there isn't always 1 pending upload when the vehicle
shuts off.

43
board/Makefile
Unescape View File

@ -0,0 +1,43 @@
# :set noet
PROJ_NAME = comma
CFLAGS = -g -O0 -Wall
CFLAGS += -mlittle-endian -mthumb -mcpu=cortex-m3
CFLAGS += -msoft-float -DSTM32F2 -DSTM32F205xx
CFLAGS += -I inc -nostdlib
CFLAGS += -Tstm32_flash.ld
CC = arm-none-eabi-gcc
OBJCOPY = arm-none-eabi-objcopy
OBJDUMP = arm-none-eabi-objdump
MACHINE = $(shell uname -m)
all: obj/$(PROJ_NAME).bin
#$(OBJDUMP) -d obj/$(PROJ_NAME).elf
./tools/enter_download_mode.py
./tools/dfu-util-$(MACHINE) -a 0 -s 0x08000000 -D $<
./tools/dfu-util-$(MACHINE) --reset-stm32 -a 0 -s 0x08000000
ifneq ($(wildcard ../.git/HEAD),)
obj/gitversion.h: ../.git/HEAD ../.git/index
echo "const uint8_t gitversion[] = \"$(shell git rev-parse HEAD)\";" > $@
else
obj/gitversion.h:
echo "const uint8_t gitversion[] = \"RELEASE\";" > $@
endif
obj/main.o: main.c *.h obj/gitversion.h
$(CC) $(CFLAGS) -o $@ -c $<
obj/startup_stm32f205xx.o: startup_stm32f205xx.s
mkdir -p obj
$(CC) $(CFLAGS) -o $@ -c $<
obj/$(PROJ_NAME).bin: obj/startup_stm32f205xx.o obj/main.o
$(CC) $(CFLAGS) -o obj/$(PROJ_NAME).elf $^
$(OBJCOPY) -v -O binary obj/$(PROJ_NAME).elf $@
clean:
rm -f obj/*

38
board/adc.h
Unescape View File

@ -0,0 +1,38 @@
// ACCEL1 = ADC10
// ACCEL2 = ADC11
// VOLT_S = ADC12
// CURR_S = ADC13
#define ADCCHAN_ACCEL0 10
#define ADCCHAN_ACCEL1 11
#define ADCCHAN_VOLTAGE 12
#define ADCCHAN_CURRENT 13
void adc_init() {
// global setup
ADC->CCR = ADC_CCR_TSVREFE | ADC_CCR_VBATE;
//ADC1->CR2 = ADC_CR2_ADON | ADC_CR2_EOCS | ADC_CR2_DDS;
ADC1->CR2 = ADC_CR2_ADON;
// long
ADC1->SMPR1 = ADC_SMPR1_SMP10 | ADC_SMPR1_SMP11 | ADC_SMPR1_SMP12 | ADC_SMPR1_SMP13;
}
uint32_t adc_get(int channel) {
// includes length
//ADC1->SQR1 = 0;
// select channel
ADC1->JSQR = channel << 15;
//ADC1->CR1 = ADC_CR1_DISCNUM_0;
//ADC1->CR1 = ADC_CR1_EOCIE;
ADC1->SR &= ~(ADC_SR_JEOC);
ADC1->CR2 |= ADC_CR2_JSWSTART;
while (!(ADC1->SR & ADC_SR_JEOC));
return ADC1->JDR1;
}

75
board/can.h
Unescape View File

@ -0,0 +1,75 @@
void can_init(CAN_TypeDef *CAN) {
CAN->MCR = CAN_MCR_TTCM | CAN_MCR_INRQ;
while((CAN->MSR & CAN_MSR_INAK) != CAN_MSR_INAK);
puts("CAN initting\n");
// PCLK = 24000000, 500000 is 48 clocks
// from http://www.bittiming.can-wiki.ino/
CAN->BTR = 0x001c0002;
// loopback mode for debugging
#ifdef CAN_LOOPBACK_MODE
CAN->BTR |= CAN_BTR_SILM | CAN_BTR_LBKM;
#endif
// reset
CAN->MCR = CAN_MCR_TTCM;
while((CAN->MSR & CAN_MSR_INAK) == CAN_MSR_INAK);
puts("CAN init done\n");
// accept all filter
CAN->FMR |= CAN_FMR_FINIT;
// no mask
CAN->sFilterRegister[0].FR1 = 0;
CAN->sFilterRegister[0].FR2 = 0;
CAN->sFilterRegister[14].FR1 = 0;
CAN->sFilterRegister[14].FR2 = 0;
CAN->FA1R |= 1 | (1 << 14);
CAN->FMR &= ~(CAN_FMR_FINIT);
// enable all CAN interrupts
CAN->IER = 0xFFFFFFFF;
//CAN->IER = CAN_IER_TMEIE | CAN_IER_FMPIE0 | CAN_IER_FMPIE1;
}
// CAN error
void can_sce(CAN_TypeDef *CAN) {
#ifdef DEBUG
puts("MSR:");
puth(CAN->MSR);
puts(" TSR:");
puth(CAN->TSR);
puts(" RF0R:");
puth(CAN->RF0R);
puts(" RF1R:");
puth(CAN->RF1R);
puts(" ESR:");
puth(CAN->ESR);
puts("\n");
#endif
// clear
//CAN->sTxMailBox[0].TIR &= ~(CAN_TI0R_TXRQ);
CAN->TSR |= CAN_TSR_ABRQ0;
//CAN->ESR |= CAN_ESR_LEC;
//CAN->MSR &= ~(CAN_MSR_ERRI);
CAN->MSR = CAN->MSR;
}
int can_cksum(uint8_t *dat, int len, int addr, int idx) {
int i;
int s = 0;
for (i = 0; i < len; i++) {
s += (dat[i] >> 4);
s += dat[i] & 0xF;
}
s += (addr>>0)&0xF;
s += (addr>>4)&0xF;
s += (addr>>8)&0xF;
s += idx;
s = 8-s;
return s&0xF;
}

16
board/dac.h
Unescape View File

@ -0,0 +1,16 @@
void dac_init() {
// no buffers required since we have an opamp
//DAC->CR = DAC_CR_EN1 | DAC_CR_BOFF1 | DAC_CR_EN2 | DAC_CR_BOFF2;
DAC->DHR12R1 = 0;
DAC->DHR12R2 = 0;
DAC->CR = DAC_CR_EN1 | DAC_CR_EN2;
}
void dac_set(int channel, uint32_t value) {
if (channel == 0) {
DAC->DHR12R1 = value;
} else if (channel == 1) {
DAC->DHR12R2 = value;
}
}

3
board/inc/core_cm3.h
Unescape View File

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

3
board/inc/core_cmFunc.h
Unescape View File

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

3
board/inc/core_cmInstr.h
Unescape View File

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

3
board/inc/stm32f205xx.h
Unescape View File

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

3
board/inc/stm32f2xx.h
Unescape View File

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

3
board/inc/system_stm32f2xx.h
Unescape View File

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

226
board/libc.h
Unescape View File

@ -0,0 +1,226 @@
#define min(a,b) \
({ __typeof__ (a) _a = (a); \
__typeof__ (b) _b = (b); \
_a < _b ? _a : _b; })
#define max(a,b) \
({ __typeof__ (a) _a = (a); \
__typeof__ (b) _b = (b); \
_a > _b ? _a : _b; })
#define __DIV(_PCLK_, _BAUD_) (((_PCLK_)*25)/(4*(_BAUD_)))
#define __DIVMANT(_PCLK_, _BAUD_) (__DIV((_PCLK_), (_BAUD_))/100)
#define __DIVFRAQ(_PCLK_, _BAUD_) (((__DIV((_PCLK_), (_BAUD_)) - (__DIVMANT((_PCLK_), (_BAUD_)) * 100)) * 16 + 50) / 100)
#define __USART_BRR(_PCLK_, _BAUD_) ((__DIVMANT((_PCLK_), (_BAUD_)) << 4)|(__DIVFRAQ((_PCLK_), (_BAUD_)) & 0x0F))
#define GPIO_AF2_TIM3 ((uint8_t)0x02) /* TIM3 Alternate Function mapping */
#define GPIO_AF7_USART2 ((uint8_t)0x07) /* USART2 Alternate Function mapping */
#define GPIO_AF7_USART3 ((uint8_t)0x07) /* USART3 Alternate Function mapping */
#define GPIO_AF9_CAN1 ((uint8_t)0x09) /* CAN1 Alternate Function mapping */
#define GPIO_AF10_OTG_FS ((uint8_t)0xA) /* OTG_FS Alternate Function mapping */
#define GPIO_AF12_OTG_HS_FS ((uint8_t)0xC) /* OTG HS configured in FS */
#ifdef OLD_BOARD
#define USART USART2
#else
#define USART USART3
#endif
// **** shitty libc ****
void clock_init() {
#ifdef USE_INTERNAL_OSC
// enable internal oscillator
RCC->CR |= RCC_CR_HSION;
while ((RCC->CR & RCC_CR_HSIRDY) == 0);
#else
// enable external oscillator
RCC->CR |= RCC_CR_HSEON;
while ((RCC->CR & RCC_CR_HSERDY) == 0);
#endif
// divide shit
RCC->CFGR = RCC_CFGR_HPRE_DIV1 | RCC_CFGR_PPRE2_DIV2 | RCC_CFGR_PPRE1_DIV4;
#ifdef USE_INTERNAL_OSC
RCC->PLLCFGR = RCC_PLLCFGR_PLLQ_2 | RCC_PLLCFGR_PLLM_3 |
RCC_PLLCFGR_PLLN_6 | RCC_PLLCFGR_PLLN_5 | RCC_PLLCFGR_PLLSRC_HSI;
#else
RCC->PLLCFGR = RCC_PLLCFGR_PLLQ_2 | RCC_PLLCFGR_PLLM_3 |
RCC_PLLCFGR_PLLN_7 | RCC_PLLCFGR_PLLN_6 | RCC_PLLCFGR_PLLSRC_HSE;
#endif
// start PLL
RCC->CR |= RCC_CR_PLLON;
while ((RCC->CR & RCC_CR_PLLRDY) == 0);
// Configure Flash prefetch, Instruction cache, Data cache and wait state
// *** without this, it breaks ***
FLASH->ACR = FLASH_ACR_ICEN | FLASH_ACR_DCEN | FLASH_ACR_LATENCY_5WS;
// switch to PLL
RCC->CFGR |= RCC_CFGR_SW_PLL;
while ((RCC->CFGR & RCC_CFGR_SWS) != RCC_CFGR_SWS_PLL);
// *** running on PLL ***
// enable GPIOB, UART2, CAN, USB clock
RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN;
RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN;
RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN;
RCC->APB1ENR |= RCC_APB1ENR_USART2EN;
RCC->APB1ENR |= RCC_APB1ENR_USART3EN;
RCC->APB1ENR |= RCC_APB1ENR_CAN1EN;
RCC->APB1ENR |= RCC_APB1ENR_CAN2EN;
RCC->APB1ENR |= RCC_APB1ENR_DACEN;
RCC->APB1ENR |= RCC_APB1ENR_TIM3EN;
RCC->AHB2ENR |= RCC_AHB2ENR_OTGFSEN;
//RCC->APB2ENR |= RCC_APB2ENR_TIM1EN;
RCC->APB2ENR |= RCC_APB2ENR_ADC1EN;
// turn on alt USB
RCC->AHB1ENR |= RCC_AHB1ENR_OTGHSEN;
// fix interrupt vectors
}
// board specific
void gpio_init() {
// analog mode
GPIOC->MODER = GPIO_MODER_MODER3 | GPIO_MODER_MODER2 |
GPIO_MODER_MODER1 | GPIO_MODER_MODER0;
// FAN on C9, aka TIM3_CH4
#ifdef OLD_BOARD
GPIOC->MODER |= GPIO_MODER_MODER9_1;
GPIOC->AFR[1] = GPIO_AF2_TIM3 << ((9-8)*4);
#else
GPIOC->MODER |= GPIO_MODER_MODER8_1;
GPIOC->AFR[1] = GPIO_AF2_TIM3 << ((8-8)*4);
#endif
// IGNITION on C13
// set mode for LEDs and CAN
GPIOB->MODER = GPIO_MODER_MODER10_0 | GPIO_MODER_MODER11_0;
// CAN 2
GPIOB->MODER |= GPIO_MODER_MODER5_1 | GPIO_MODER_MODER6_1;
// CAN 1
GPIOB->MODER |= GPIO_MODER_MODER8_1 | GPIO_MODER_MODER9_1;
// CAN enables
GPIOB->MODER |= GPIO_MODER_MODER3_0 | GPIO_MODER_MODER4_0;
// set mode for SERIAL and USB (DAC should be configured to in)
GPIOA->MODER = GPIO_MODER_MODER2_1 | GPIO_MODER_MODER3_1;
GPIOA->AFR[0] = GPIO_AF7_USART2 << (2*4) | GPIO_AF7_USART2 << (3*4);
// GPIOC USART3
GPIOC->MODER |= GPIO_MODER_MODER10_1 | GPIO_MODER_MODER11_1;
GPIOC->AFR[1] |= GPIO_AF7_USART3 << ((10-8)*4) | GPIO_AF7_USART3 << ((11-8)*4);
if (USBx == USB_OTG_FS) {
GPIOA->MODER |= GPIO_MODER_MODER11_1 | GPIO_MODER_MODER12_1;
GPIOA->OSPEEDR = GPIO_OSPEEDER_OSPEEDR11 | GPIO_OSPEEDER_OSPEEDR12;
GPIOA->AFR[1] = GPIO_AF10_OTG_FS << ((11-8)*4) | GPIO_AF10_OTG_FS << ((12-8)*4);
}
GPIOA->PUPDR = GPIO_PUPDR_PUPDR2_0 | GPIO_PUPDR_PUPDR3_0;
// set mode for CAN / USB_HS pins
GPIOB->AFR[0] = GPIO_AF9_CAN1 << (5*4) | GPIO_AF9_CAN1 << (6*4);
GPIOB->AFR[1] = GPIO_AF9_CAN1 << ((8-8)*4) | GPIO_AF9_CAN1 << ((9-8)*4);
if (USBx == USB_OTG_HS) {
GPIOB->AFR[1] |= GPIO_AF12_OTG_HS_FS << ((15-8)*4) | GPIO_AF12_OTG_HS_FS << ((14-8)*4);
GPIOB->MODER |= GPIO_MODER_MODER14_1 | GPIO_MODER_MODER15_1;
}
GPIOB->OSPEEDR = GPIO_OSPEEDER_OSPEEDR14 | GPIO_OSPEEDER_OSPEEDR15;
// enable CAN busses
GPIOB->ODR |= (1 << 3) | (1 << 4);
// enable OTG out tied to ground
GPIOA->ODR = 0;
GPIOA->MODER |= GPIO_MODER_MODER1_0;
// enable USB power tied to +
GPIOA->ODR |= 1;
GPIOA->MODER |= GPIO_MODER_MODER0_0;
}
void uart_init() {
// enable uart and tx+rx mode
USART->CR1 = USART_CR1_UE;
USART->BRR = __USART_BRR(24000000, 115200);
USART->CR1 |= USART_CR1_TE | USART_CR1_RE;
USART->CR2 = USART_CR2_STOP_0 | USART_CR2_STOP_1;
// ** UART is ready to work **
// enable interrupts
USART->CR1 |= USART_CR1_RXNEIE;
}
void delay(int a) {
volatile int i;
for (i=0;i<a;i++);
}
void putch(const char a) {
while (!(USART->SR & USART_SR_TXE));
USART->DR = a;
}
int puts(const char *a) {
for (;*a;a++) {
if (*a == '\n') putch('\r');
putch(*a);
}
return 0;
}
void puth(unsigned int i) {
int pos;
char c[] = "0123456789abcdef";
for (pos = 28; pos != -4; pos -= 4) {
putch(c[(i >> pos) & 0xF]);
}
}
void puth2(unsigned int i) {
int pos;
char c[] = "0123456789abcdef";
for (pos = 4; pos != -4; pos -= 4) {
putch(c[(i >> pos) & 0xF]);
}
}
void hexdump(void *a, int l) {
int i;
for (i=0;i<l;i++) {
if (i != 0 && (i&0xf) == 0) puts("\n");
puth2(((unsigned char*)a)[i]);
puts(" ");
}
puts("\n");
}
void *memset(void *str, int c, unsigned int n) {
int i;
for (i = 0; i < n; i++) {
*((uint8_t*)str) = c;
++str;
}
return str;
}
void *memcpy(void *dest, const void *src, unsigned int n) {
int i;
// TODO: make not slow
for (i = 0; i < n; i++) {
((uint8_t*)dest)[i] = *(uint8_t*)src;
++src;
}
return dest;
}

499
board/main.c
Unescape View File

@ -0,0 +1,499 @@
//#define DEBUG
//#define CAN_LOOPBACK_MODE
//#define USE_INTERNAL_OSC
//#define OLD_BOARD
#define USB_VID 0xbbaa
#define USB_PID 0xddcc
// *** end config ***
#include "stm32f2xx.h"
#include "obj/gitversion.h"
#define ENTER_BOOTLOADER_MAGIC 0xdeadbeef
uint32_t enter_bootloader_mode;
USB_OTG_GlobalTypeDef *USBx = USB_OTG_FS;
#include "libc.h"
#include "adc.h"
#include "timer.h"
#include "usb.h"
#include "can.h"
// debug safety check: is controls allowed?
int controls_allowed = 0;
int gas_interceptor_detected = 0;
// ********************* instantiate queues *********************
#define FIFO_SIZE 0x100
typedef struct {
uint8_t w_ptr;
uint8_t r_ptr;
CAN_FIFOMailBox_TypeDef elems[FIFO_SIZE];
} can_ring;
can_ring can_rx_q = { .w_ptr = 0, .r_ptr = 0 };
can_ring can_tx1_q = { .w_ptr = 0, .r_ptr = 0 };
can_ring can_tx2_q = { .w_ptr = 0, .r_ptr = 0 };
// ********************* interrupt safe queue *********************
inline int pop(can_ring *q, CAN_FIFOMailBox_TypeDef *elem) {
if (q->w_ptr != q->r_ptr) {
*elem = q->elems[q->r_ptr];
q->r_ptr += 1;
return 1;
}
return 0;
}
inline int push(can_ring *q, CAN_FIFOMailBox_TypeDef *elem) {
uint8_t next_w_ptr = q->w_ptr + 1;
if (next_w_ptr != q->r_ptr) {
q->elems[q->w_ptr] = *elem;
q->w_ptr = next_w_ptr;
return 1;
}
return 0;
}
// ***************************** CAN *****************************
void process_can(CAN_TypeDef *CAN, can_ring *can_q, int can_number) {
#ifdef DEBUG
puts("process CAN TX\n");
#endif
// add successfully transmitted message to my fifo
if ((CAN->TSR & CAN_TSR_TXOK0) == CAN_TSR_TXOK0) {
CAN_FIFOMailBox_TypeDef to_push;
to_push.RIR = CAN->sTxMailBox[0].TIR;
to_push.RDTR = (CAN->sTxMailBox[0].TDTR & 0xFFFF000F) | ((can_number+2) << 4);
to_push.RDLR = CAN->sTxMailBox[0].TDLR;
to_push.RDHR = CAN->sTxMailBox[0].TDHR;
push(&can_rx_q, &to_push);
}
// check for empty mailbox
CAN_FIFOMailBox_TypeDef to_send;
if ((CAN->TSR & CAN_TSR_TME0) == CAN_TSR_TME0) {
if (pop(can_q, &to_send)) {
// BRAKE: safety check
if ((to_send.RIR>>21) == 0x1FA) {
if (controls_allowed) {
to_send.RDLR &= 0xFFFFFF3F;
} else {
to_send.RDLR &= 0xFFFF0000;
}
}
// STEER: safety check
if ((to_send.RIR>>21) == 0xE4) {
if (controls_allowed) {
to_send.RDLR &= 0xFFFFFFFF;
} else {
to_send.RDLR &= 0xFFFF0000;
}
}
// GAS: safety check
if ((to_send.RIR>>21) == 0x200) {
if (controls_allowed) {
to_send.RDLR &= 0xFFFFFFFF;
} else {
to_send.RDLR &= 0xFFFF0000;
}
}
// only send if we have received a packet
CAN->sTxMailBox[0].TDLR = to_send.RDLR;
CAN->sTxMailBox[0].TDHR = to_send.RDHR;
CAN->sTxMailBox[0].TDTR = to_send.RDTR;
CAN->sTxMailBox[0].TIR = to_send.RIR;
}
}
// clear interrupt
CAN->TSR |= CAN_TSR_RQCP0;
}
// send more, possible for these to not trigger?
void CAN1_TX_IRQHandler() {
process_can(CAN1, &can_tx1_q, 1);
}
void CAN2_TX_IRQHandler() {
process_can(CAN2, &can_tx2_q, 0);
}
// board enforces
// in-state
// accel set/resume
// out-state
// cancel button
// all commands: brake and steering
// if controls_allowed
// allow all commands up to limit
// else
// block all commands that produce actuation
// CAN receive handlers
void can_rx(CAN_TypeDef *CAN, int can_number) {
while (CAN->RF0R & CAN_RF0R_FMP0) {
// add to my fifo
CAN_FIFOMailBox_TypeDef to_push;
to_push.RIR = CAN->sFIFOMailBox[0].RIR;
// top 16-bits is the timestamp
to_push.RDTR = (CAN->sFIFOMailBox[0].RDTR & 0xFFFF000F) | (can_number << 4);
to_push.RDLR = CAN->sFIFOMailBox[0].RDLR;
to_push.RDHR = CAN->sFIFOMailBox[0].RDHR;
// state machine to enter and exit controls
// 0x1A6 for the ILX, 0x296 for the Civic Touring
if ((to_push.RIR>>21) == 0x1A6 || (to_push.RIR>>21) == 0x296) {
int buttons = (to_push.RDLR & 0xE0) >> 5;
if (buttons == 4 || buttons == 3) {
controls_allowed = 1;
} else if (buttons == 2) {
controls_allowed = 0;
}
}
// exit controls on brake press
if ((to_push.RIR>>21) == 0x17C) {
// bit 50
if (to_push.RDHR & 0x200000) {
controls_allowed = 0;
}
}
// exit controls on gas press if interceptor
if ((to_push.RIR>>21) == 0x201) {
gas_interceptor_detected = 1;
int gas = ((to_push.RDLR & 0xFF) << 8) | ((to_push.RDLR & 0xFF00) >> 8);
if (gas > 328) {
controls_allowed = 0;
}
}
// exit controls on gas press if no interceptor
if (!gas_interceptor_detected) {
if ((to_push.RIR>>21) == 0x17C) {
if (to_push.RDLR & 0xFF) {
controls_allowed = 0;
}
}
}
push(&can_rx_q, &to_push);
// next
CAN->RF0R |= CAN_RF0R_RFOM0;
}
}
void CAN1_RX0_IRQHandler() {
//puts("CANRX1");
//delay(10000);
can_rx(CAN1, 1);
}
void CAN2_RX0_IRQHandler() {
//puts("CANRX0");
//delay(10000);
can_rx(CAN2, 0);
}
void CAN1_SCE_IRQHandler() {
//puts("CAN1_SCE\n");
can_sce(CAN1);
}
void CAN2_SCE_IRQHandler() {
//puts("CAN2_SCE\n");
can_sce(CAN2);
}
// ***************************** serial port *****************************
void USART_IRQHandler(void) {
puts("S");
// echo characters
if (USART->SR & USART_SR_RXNE) {
char rcv = USART->DR;
putch(rcv);
// jump to DFU flash
if (rcv == 'z') {
enter_bootloader_mode = ENTER_BOOTLOADER_MAGIC;
NVIC_SystemReset();
}
}
}
void USART2_IRQHandler(void) {
USART_IRQHandler();
}
void USART3_IRQHandler(void) {
USART_IRQHandler();
}
// ***************************** USB port *****************************
int get_health_pkt(void *dat) {
struct {
uint32_t voltage;
uint32_t current;
uint8_t started;
uint8_t controls_allowed;
uint8_t gas_interceptor_detected;
} *health = dat;
health->voltage = adc_get(ADCCHAN_VOLTAGE);
health->current = adc_get(ADCCHAN_CURRENT);
health->started = (GPIOC->IDR & (1 << 13)) != 0;
health->controls_allowed = controls_allowed;
health->gas_interceptor_detected = gas_interceptor_detected;
return sizeof(*health);
}
void set_fan_speed(int fan_speed) {
#ifdef OLD_BOARD
TIM3->CCR4 = fan_speed;
#else
TIM3->CCR3 = fan_speed;
#endif
}
void usb_cb_ep1_in(int len) {
CAN_FIFOMailBox_TypeDef reply[4];
int ilen = 0;
while (ilen < min(len/0x10, 4) && pop(&can_rx_q, &reply[ilen])) ilen++;
#ifdef DEBUG
puts("FIFO SENDING ");
puth(ilen);
puts("\n");
#endif
USB_WritePacket((void *)reply, ilen*0x10, 1);
}
void usb_cb_ep2_out(uint8_t *usbdata, int len) {
}
// send on CAN
void usb_cb_ep3_out(uint8_t *usbdata, int len) {
int dpkt = 0;
for (dpkt = 0; dpkt < len; dpkt += 0x10) {
uint32_t *tf = (uint32_t*)(&usbdata[dpkt]);
int flags = tf[1] >> 4;
CAN_TypeDef *CAN;
can_ring *can_q;
int can_number = 0;
if (flags & 1) {
CAN=CAN1;
can_q = &can_tx1_q;
can_number = 1;
} else {
CAN=CAN2;
can_q = &can_tx2_q;
}
// add CAN packet to send queue
CAN_FIFOMailBox_TypeDef to_push;
to_push.RDHR = tf[3];
to_push.RDLR = tf[2];
to_push.RDTR = tf[1] & 0xF;
to_push.RIR = tf[0];
push(can_q, &to_push);
process_can(CAN, can_q, can_number);
}
}
void usb_cb_control_msg() {
uint8_t resp[0x20];
int resp_len;
switch (setup.b.bRequest) {
case 0xd1:
enter_bootloader_mode = ENTER_BOOTLOADER_MAGIC;
NVIC_SystemReset();
break;
case 0xd2:
resp_len = get_health_pkt(resp);
USB_WritePacket(resp, resp_len, 0);
USBx_OUTEP(0)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK;
break;
case 0xd3:
set_fan_speed(setup.b.wValue.w);
USB_WritePacket(0, 0, 0);
USBx_OUTEP(0)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK;
break;
case 0xd6: // GET_VERSION
USB_WritePacket(gitversion, min(sizeof(gitversion), setup.b.wLength.w), 0);
USBx_OUTEP(0)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK;
break;
case 0xd8: // RESET
NVIC_SystemReset();
break;
default:
puts("NO HANDLER ");
puth(setup.b.bRequest);
puts("\n");
break;
}
}
void OTG_FS_IRQHandler(void) {
NVIC_DisableIRQ(OTG_FS_IRQn);
//__disable_irq();
usb_irqhandler();
//__enable_irq();
NVIC_EnableIRQ(OTG_FS_IRQn);
}
void OTG_HS_IRQHandler(void) {
//puts("HS_IRQ\n");
NVIC_DisableIRQ(OTG_FS_IRQn);
//__disable_irq();
usb_irqhandler();
//__enable_irq();
NVIC_EnableIRQ(OTG_FS_IRQn);
}
void ADC_IRQHandler(void) {
puts("ADC_IRQ\n");
}
// ***************************** main code *****************************
void __initialize_hardware_early() {
// set USB power + and OTG mode
RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN;
// enable OTG out tied to ground
GPIOA->ODR = 0;
GPIOA->MODER |= GPIO_MODER_MODER1_0;
// enable USB power tied to +
GPIOA->ODR |= 1;
GPIOA->MODER |= GPIO_MODER_MODER0_0;
// enable pull DOWN on OTG_FS_DP
// must be done a while before reading it
GPIOA->PUPDR = GPIO_PUPDR_PUPDR12_1;
if (enter_bootloader_mode == ENTER_BOOTLOADER_MAGIC) {
enter_bootloader_mode = 0;
void (*bootloader)(void) = (void (*)(void)) (*((uint32_t *)0x1fff0004));
// jump to bootloader
bootloader();
// LOOP
while(1);
}
}
int main() {
// init devices
clock_init();
// test the USB choice before GPIO init
if (GPIOA->IDR & (1 << 12)) {
USBx = USB_OTG_HS;
}
gpio_init();
uart_init();
usb_init();
can_init(CAN1);
can_init(CAN2);
adc_init();
// timer for fan PWM
#ifdef OLD_BOARD
TIM3->CCMR2 = TIM_CCMR2_OC4M_2 | TIM_CCMR2_OC4M_1;
TIM3->CCER = TIM_CCER_CC4E;
#else
TIM3->CCMR2 = TIM_CCMR2_OC3M_2 | TIM_CCMR2_OC3M_1;
TIM3->CCER = TIM_CCER_CC3E;
#endif
// max value of the timer
// 64 makes it above the audible range
//TIM3->ARR = 64;
// 10 prescale makes it below the audible range
timer_init(TIM3, 10);
// set PWM
set_fan_speed(65535);
puts("**** INTERRUPTS ON ****\n");
__disable_irq();
NVIC_EnableIRQ(USART2_IRQn);
NVIC_EnableIRQ(USART3_IRQn);
NVIC_EnableIRQ(OTG_FS_IRQn);
NVIC_EnableIRQ(OTG_HS_IRQn);
NVIC_EnableIRQ(ADC_IRQn);
// CAN has so many interrupts!
NVIC_EnableIRQ(CAN1_TX_IRQn);
NVIC_EnableIRQ(CAN1_RX0_IRQn);
NVIC_EnableIRQ(CAN1_SCE_IRQn);
NVIC_EnableIRQ(CAN2_TX_IRQn);
NVIC_EnableIRQ(CAN2_RX0_IRQn);
NVIC_EnableIRQ(CAN2_SCE_IRQn);
__enable_irq();
// LED should keep on blinking all the time
while (1) {
#ifdef DEBUG
puts("** blink ");
puth(can_rx_q.r_ptr); puts(" "); puth(can_rx_q.w_ptr); puts(" ");
puth(can_tx1_q.r_ptr); puts(" "); puth(can_tx1_q.w_ptr); puts(" ");
puth(can_tx2_q.r_ptr); puts(" "); puth(can_tx2_q.w_ptr); puts("\n");
#endif
/*puts("voltage: "); puth(adc_get(ADCCHAN_VOLTAGE)); puts(" ");
puts("current: "); puth(adc_get(ADCCHAN_CURRENT)); puts("\n");*/
// set LED to be controls allowed
GPIOB->ODR = (GPIOB->ODR | (1 << 11)) & ~(controls_allowed << 11);
// blink the other LED if in FS mode
if (USBx == USB_OTG_FS) {
GPIOB->ODR |= (1 << 10);
}
delay(1000000);
GPIOB->ODR &= ~(1 << 10);
delay(1000000);
if (GPIOC->IDR & (1 << 13)) {
// turn on fan at half speed
set_fan_speed(32768);
} else {
// turn off fan
set_fan_speed(0);
}
}
return 0;
}

511
board/startup_stm32f205xx.s
Unescape View File

@ -0,0 +1,511 @@
/**
******************************************************************************
* @file startup_stm32f205xx.s
* @author MCD Application Team
* @version V2.0.1
* @date 25-March-2014
* @brief STM32F205xx Devices vector table for Atollic TrueSTUDIO toolchain.
* This module performs:
* - Set the initial SP
* - Set the initial PC == Reset_Handler,
* - Set the vector table entries with the exceptions ISR address
* - Branches to main in the C library (which eventually
* calls main()).
* After Reset the Cortex-M3 processor is in Thread mode,
* priority is Privileged, and the Stack is set to Main.
******************************************************************************
* @attention
*
* <h2><center>&copy; COPYRIGHT 2014 STMicroelectronics</center></h2>
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. Neither the name of STMicroelectronics nor the names of its contributors
* may be used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
******************************************************************************
*/
.syntax unified
.cpu cortex-m3
.thumb
.global g_pfnVectors
.global Default_Handler
/* start address for the initialization values of the .data section.
defined in linker script */
.word _sidata
/* start address for the .data section. defined in linker script */
.word _sdata
/* end address for the .data section. defined in linker script */
.word _edata
/* start address for the .bss section. defined in linker script */
.word _sbss
/* end address for the .bss section. defined in linker script */
.word _ebss
/* stack used for SystemInit_ExtMemCtl; always internal RAM used */
/**
* @brief This is the code that gets called when the processor first
* starts execution following a reset event. Only the absolutely
* necessary set is performed, after which the application
* supplied main() routine is called.
* @param None
* @retval : None
*/
.section .text.Reset_Handler
.weak Reset_Handler
.type Reset_Handler, %function
Reset_Handler:
bl __initialize_hardware_early
ldr sp, =_estack /* set stack pointer */
/* Copy the data segment initializers from flash to SRAM */
movs r1, #0
b LoopCopyDataInit
CopyDataInit:
ldr r3, =_sidata
ldr r3, [r3, r1]
str r3, [r0, r1]
adds r1, r1, #4
LoopCopyDataInit:
ldr r0, =_sdata
ldr r3, =_edata
adds r2, r0, r1
cmp r2, r3
bcc CopyDataInit
ldr r2, =_sbss
b LoopFillZerobss
/* Zero fill the bss segment. */
FillZerobss:
movs r3, #0
str r3, [r2], #4
LoopFillZerobss:
ldr r3, = _ebss
cmp r2, r3
bcc FillZerobss
/* Call the clock system intitialization function.*/
/* bl SystemInit */
/* Call static constructors */
/* bl __libc_init_array */
/* Call the application's entry point.*/
bl main
bx lr
.size Reset_Handler, .-Reset_Handler
/**
* @brief This is the code that gets called when the processor receives an
* unexpected interrupt. This simply enters an infinite loop, preserving
* the system state for examination by a debugger.
* @param None
* @retval None
*/
.section .text.Default_Handler,"ax",%progbits
Default_Handler:
Infinite_Loop:
b Infinite_Loop
.size Default_Handler, .-Default_Handler
/******************************************************************************
*
* The minimal vector table for a Cortex M3. Note that the proper constructs
* must be placed on this to ensure that it ends up at physical address
* 0x0000.0000.
*
*******************************************************************************/
.section .isr_vector,"a",%progbits
.type g_pfnVectors, %object
.size g_pfnVectors, .-g_pfnVectors
g_pfnVectors:
.word _estack
.word Reset_Handler
.word NMI_Handler
.word HardFault_Handler
.word MemManage_Handler
.word BusFault_Handler
.word UsageFault_Handler
.word 0
.word 0
.word 0
.word 0
.word SVC_Handler
.word DebugMon_Handler
.word 0
.word PendSV_Handler
.word SysTick_Handler
/* External Interrupts */
.word WWDG_IRQHandler /* Window WatchDog */
.word PVD_IRQHandler /* PVD through EXTI Line detection */
.word TAMP_STAMP_IRQHandler /* Tamper and TimeStamps through the EXTI line */
.word RTC_WKUP_IRQHandler /* RTC Wakeup through the EXTI line */
.word FLASH_IRQHandler /* FLASH */
.word RCC_IRQHandler /* RCC */
.word EXTI0_IRQHandler /* EXTI Line0 */
.word EXTI1_IRQHandler /* EXTI Line1 */
.word EXTI2_IRQHandler /* EXTI Line2 */
.word EXTI3_IRQHandler /* EXTI Line3 */
.word EXTI4_IRQHandler /* EXTI Line4 */
.word DMA1_Stream0_IRQHandler /* DMA1 Stream 0 */
.word DMA1_Stream1_IRQHandler /* DMA1 Stream 1 */
.word DMA1_Stream2_IRQHandler /* DMA1 Stream 2 */
.word DMA1_Stream3_IRQHandler /* DMA1 Stream 3 */
.word DMA1_Stream4_IRQHandler /* DMA1 Stream 4 */
.word DMA1_Stream5_IRQHandler /* DMA1 Stream 5 */
.word DMA1_Stream6_IRQHandler /* DMA1 Stream 6 */
.word ADC_IRQHandler /* ADC1, ADC2 and ADC3s */
.word CAN1_TX_IRQHandler /* CAN1 TX */
.word CAN1_RX0_IRQHandler /* CAN1 RX0 */
.word CAN1_RX1_IRQHandler /* CAN1 RX1 */
.word CAN1_SCE_IRQHandler /* CAN1 SCE */
.word EXTI9_5_IRQHandler /* External Line[9:5]s */
.word TIM1_BRK_TIM9_IRQHandler /* TIM1 Break and TIM9 */
.word TIM1_UP_TIM10_IRQHandler /* TIM1 Update and TIM10 */
.word TIM1_TRG_COM_TIM11_IRQHandler /* TIM1 Trigger and Commutation and TIM11 */
.word TIM1_CC_IRQHandler /* TIM1 Capture Compare */
.word TIM2_IRQHandler /* TIM2 */
.word TIM3_IRQHandler /* TIM3 */
.word TIM4_IRQHandler /* TIM4 */
.word I2C1_EV_IRQHandler /* I2C1 Event */
.word I2C1_ER_IRQHandler /* I2C1 Error */
.word I2C2_EV_IRQHandler /* I2C2 Event */
.word I2C2_ER_IRQHandler /* I2C2 Error */
.word SPI1_IRQHandler /* SPI1 */
.word SPI2_IRQHandler /* SPI2 */
.word USART1_IRQHandler /* USART1 */
.word USART2_IRQHandler /* USART2 */
.word USART3_IRQHandler /* USART3 */
.word EXTI15_10_IRQHandler /* External Line[15:10]s */
.word RTC_Alarm_IRQHandler /* RTC Alarm (A and B) through EXTI Line */
.word OTG_FS_WKUP_IRQHandler /* USB OTG FS Wakeup through EXTI line */
.word TIM8_BRK_TIM12_IRQHandler /* TIM8 Break and TIM12 */
.word TIM8_UP_TIM13_IRQHandler /* TIM8 Update and TIM13 */
.word TIM8_TRG_COM_TIM14_IRQHandler /* TIM8 Trigger and Commutation and TIM14 */
.word TIM8_CC_IRQHandler /* TIM8 Capture Compare */
.word DMA1_Stream7_IRQHandler /* DMA1 Stream7 */
.word FSMC_IRQHandler /* FSMC */
.word SDIO_IRQHandler /* SDIO */
.word TIM5_IRQHandler /* TIM5 */
.word SPI3_IRQHandler /* SPI3 */
.word UART4_IRQHandler /* UART4 */
.word UART5_IRQHandler /* UART5 */
.word TIM6_DAC_IRQHandler /* TIM6 and DAC1&2 underrun errors */
.word TIM7_IRQHandler /* TIM7 */
.word DMA2_Stream0_IRQHandler /* DMA2 Stream 0 */
.word DMA2_Stream1_IRQHandler /* DMA2 Stream 1 */
.word DMA2_Stream2_IRQHandler /* DMA2 Stream 2 */
.word DMA2_Stream3_IRQHandler /* DMA2 Stream 3 */
.word DMA2_Stream4_IRQHandler /* DMA2 Stream 4 */
.word 0 /* Reserved */
.word 0 /* Reserved */
.word CAN2_TX_IRQHandler /* CAN2 TX */
.word CAN2_RX0_IRQHandler /* CAN2 RX0 */
.word CAN2_RX1_IRQHandler /* CAN2 RX1 */
.word CAN2_SCE_IRQHandler /* CAN2 SCE */
.word OTG_FS_IRQHandler /* USB OTG FS */
.word DMA2_Stream5_IRQHandler /* DMA2 Stream 5 */
.word DMA2_Stream6_IRQHandler /* DMA2 Stream 6 */
.word DMA2_Stream7_IRQHandler /* DMA2 Stream 7 */
.word USART6_IRQHandler /* USART6 */
.word I2C3_EV_IRQHandler /* I2C3 event */
.word I2C3_ER_IRQHandler /* I2C3 error */
.word OTG_HS_EP1_OUT_IRQHandler /* USB OTG HS End Point 1 Out */
.word OTG_HS_EP1_IN_IRQHandler /* USB OTG HS End Point 1 In */
.word OTG_HS_WKUP_IRQHandler /* USB OTG HS Wakeup through EXTI */
.word OTG_HS_IRQHandler /* USB OTG HS */
.word 0 /* Reserved */
.word 0 /* Reserved */
.word HASH_RNG_IRQHandler /* Hash and Rng */
/*******************************************************************************
*
* Provide weak aliases for each Exception handler to the Default_Handler.
* As they are weak aliases, any function with the same name will override
* this definition.
*
*******************************************************************************/
.weak NMI_Handler
.thumb_set NMI_Handler,Default_Handler
.weak HardFault_Handler
.thumb_set HardFault_Handler,Default_Handler
.weak MemManage_Handler
.thumb_set MemManage_Handler,Default_Handler
.weak BusFault_Handler
.thumb_set BusFault_Handler,Default_Handler
.weak UsageFault_Handler
.thumb_set UsageFault_Handler,Default_Handler
.weak SVC_Handler
.thumb_set SVC_Handler,Default_Handler
.weak DebugMon_Handler
.thumb_set DebugMon_Handler,Default_Handler
.weak PendSV_Handler
.thumb_set PendSV_Handler,Default_Handler
.weak SysTick_Handler
.thumb_set SysTick_Handler,Default_Handler
.weak WWDG_IRQHandler
.thumb_set WWDG_IRQHandler,Default_Handler
.weak PVD_IRQHandler
.thumb_set PVD_IRQHandler,Default_Handler
.weak TAMP_STAMP_IRQHandler
.thumb_set TAMP_STAMP_IRQHandler,Default_Handler
.weak RTC_WKUP_IRQHandler
.thumb_set RTC_WKUP_IRQHandler,Default_Handler
.weak FLASH_IRQHandler
.thumb_set FLASH_IRQHandler,Default_Handler
.weak RCC_IRQHandler
.thumb_set RCC_IRQHandler,Default_Handler
.weak EXTI0_IRQHandler
.thumb_set EXTI0_IRQHandler,Default_Handler
.weak EXTI1_IRQHandler
.thumb_set EXTI1_IRQHandler,Default_Handler
.weak EXTI2_IRQHandler
.thumb_set EXTI2_IRQHandler,Default_Handler
.weak EXTI3_IRQHandler
.thumb_set EXTI3_IRQHandler,Default_Handler
.weak EXTI4_IRQHandler
.thumb_set EXTI4_IRQHandler,Default_Handler
.weak DMA1_Stream0_IRQHandler
.thumb_set DMA1_Stream0_IRQHandler,Default_Handler
.weak DMA1_Stream1_IRQHandler
.thumb_set DMA1_Stream1_IRQHandler,Default_Handler
.weak DMA1_Stream2_IRQHandler
.thumb_set DMA1_Stream2_IRQHandler,Default_Handler
.weak DMA1_Stream3_IRQHandler
.thumb_set DMA1_Stream3_IRQHandler,Default_Handler
.weak DMA1_Stream4_IRQHandler
.thumb_set DMA1_Stream4_IRQHandler,Default_Handler
.weak DMA1_Stream5_IRQHandler
.thumb_set DMA1_Stream5_IRQHandler,Default_Handler
.weak DMA1_Stream6_IRQHandler
.thumb_set DMA1_Stream6_IRQHandler,Default_Handler
.weak ADC_IRQHandler
.thumb_set ADC_IRQHandler,Default_Handler
.weak CAN1_TX_IRQHandler
.thumb_set CAN1_TX_IRQHandler,Default_Handler
.weak CAN1_RX0_IRQHandler
.thumb_set CAN1_RX0_IRQHandler,Default_Handler
.weak CAN1_RX1_IRQHandler
.thumb_set CAN1_RX1_IRQHandler,Default_Handler
.weak CAN1_SCE_IRQHandler
.thumb_set CAN1_SCE_IRQHandler,Default_Handler
.weak EXTI9_5_IRQHandler
.thumb_set EXTI9_5_IRQHandler,Default_Handler
.weak TIM1_BRK_TIM9_IRQHandler
.thumb_set TIM1_BRK_TIM9_IRQHandler,Default_Handler
.weak TIM1_UP_TIM10_IRQHandler
.thumb_set TIM1_UP_TIM10_IRQHandler,Default_Handler
.weak TIM1_TRG_COM_TIM11_IRQHandler
.thumb_set TIM1_TRG_COM_TIM11_IRQHandler,Default_Handler
.weak TIM1_CC_IRQHandler
.thumb_set TIM1_CC_IRQHandler,Default_Handler
.weak TIM2_IRQHandler
.thumb_set TIM2_IRQHandler,Default_Handler
.weak TIM3_IRQHandler
.thumb_set TIM3_IRQHandler,Default_Handler
.weak TIM4_IRQHandler
.thumb_set TIM4_IRQHandler,Default_Handler
.weak I2C1_EV_IRQHandler
.thumb_set I2C1_EV_IRQHandler,Default_Handler
.weak I2C1_ER_IRQHandler
.thumb_set I2C1_ER_IRQHandler,Default_Handler
.weak I2C2_EV_IRQHandler
.thumb_set I2C2_EV_IRQHandler,Default_Handler
.weak I2C2_ER_IRQHandler
.thumb_set I2C2_ER_IRQHandler,Default_Handler
.weak SPI1_IRQHandler
.thumb_set SPI1_IRQHandler,Default_Handler
.weak SPI2_IRQHandler
.thumb_set SPI2_IRQHandler,Default_Handler
.weak USART1_IRQHandler
.thumb_set USART1_IRQHandler,Default_Handler
.weak USART2_IRQHandler
.thumb_set USART2_IRQHandler,Default_Handler
.weak USART3_IRQHandler
.thumb_set USART3_IRQHandler,Default_Handler
.weak EXTI15_10_IRQHandler
.thumb_set EXTI15_10_IRQHandler,Default_Handler
.weak RTC_Alarm_IRQHandler
.thumb_set RTC_Alarm_IRQHandler,Default_Handler
.weak OTG_FS_WKUP_IRQHandler
.thumb_set OTG_FS_WKUP_IRQHandler,Default_Handler
.weak TIM8_BRK_TIM12_IRQHandler
.thumb_set TIM8_BRK_TIM12_IRQHandler,Default_Handler
.weak TIM8_UP_TIM13_IRQHandler
.thumb_set TIM8_UP_TIM13_IRQHandler,Default_Handler
.weak TIM8_TRG_COM_TIM14_IRQHandler
.thumb_set TIM8_TRG_COM_TIM14_IRQHandler,Default_Handler
.weak TIM8_CC_IRQHandler
.thumb_set TIM8_CC_IRQHandler,Default_Handler
.weak DMA1_Stream7_IRQHandler
.thumb_set DMA1_Stream7_IRQHandler,Default_Handler
.weak FSMC_IRQHandler
.thumb_set FSMC_IRQHandler,Default_Handler
.weak SDIO_IRQHandler
.thumb_set SDIO_IRQHandler,Default_Handler
.weak TIM5_IRQHandler
.thumb_set TIM5_IRQHandler,Default_Handler
.weak SPI3_IRQHandler
.thumb_set SPI3_IRQHandler,Default_Handler
.weak UART4_IRQHandler
.thumb_set UART4_IRQHandler,Default_Handler
.weak UART5_IRQHandler
.thumb_set UART5_IRQHandler,Default_Handler
.weak TIM6_DAC_IRQHandler
.thumb_set TIM6_DAC_IRQHandler,Default_Handler
.weak TIM7_IRQHandler
.thumb_set TIM7_IRQHandler,Default_Handler
.weak DMA2_Stream0_IRQHandler
.thumb_set DMA2_Stream0_IRQHandler,Default_Handler
.weak DMA2_Stream1_IRQHandler
.thumb_set DMA2_Stream1_IRQHandler,Default_Handler
.weak DMA2_Stream2_IRQHandler
.thumb_set DMA2_Stream2_IRQHandler,Default_Handler
.weak DMA2_Stream3_IRQHandler
.thumb_set DMA2_Stream3_IRQHandler,Default_Handler
.weak DMA2_Stream4_IRQHandler
.thumb_set DMA2_Stream4_IRQHandler,Default_Handler
.weak CAN2_TX_IRQHandler
.thumb_set CAN2_TX_IRQHandler,Default_Handler
.weak CAN2_RX0_IRQHandler
.thumb_set CAN2_RX0_IRQHandler,Default_Handler
.weak CAN2_RX1_IRQHandler
.thumb_set CAN2_RX1_IRQHandler,Default_Handler
.weak CAN2_SCE_IRQHandler
.thumb_set CAN2_SCE_IRQHandler,Default_Handler
.weak OTG_FS_IRQHandler
.thumb_set OTG_FS_IRQHandler,Default_Handler
.weak DMA2_Stream5_IRQHandler
.thumb_set DMA2_Stream5_IRQHandler,Default_Handler
.weak DMA2_Stream6_IRQHandler
.thumb_set DMA2_Stream6_IRQHandler,Default_Handler
.weak DMA2_Stream7_IRQHandler
.thumb_set DMA2_Stream7_IRQHandler,Default_Handler
.weak USART6_IRQHandler
.thumb_set USART6_IRQHandler,Default_Handler
.weak I2C3_EV_IRQHandler
.thumb_set I2C3_EV_IRQHandler,Default_Handler
.weak I2C3_ER_IRQHandler
.thumb_set I2C3_ER_IRQHandler,Default_Handler
.weak OTG_HS_EP1_OUT_IRQHandler
.thumb_set OTG_HS_EP1_OUT_IRQHandler,Default_Handler
.weak OTG_HS_EP1_IN_IRQHandler
.thumb_set OTG_HS_EP1_IN_IRQHandler,Default_Handler
.weak OTG_HS_WKUP_IRQHandler
.thumb_set OTG_HS_WKUP_IRQHandler,Default_Handler
.weak OTG_HS_IRQHandler
.thumb_set OTG_HS_IRQHandler,Default_Handler
.weak HASH_RNG_IRQHandler
.thumb_set HASH_RNG_IRQHandler,Default_Handler
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/

163
board/stm32_flash.ld
Unescape View File

@ -0,0 +1,163 @@
/*
*****************************************************************************
**
** File : stm32_flash.ld
**
** Abstract : Linker script for STM32F407VG Device with
** 1024KByte FLASH, 192KByte RAM
**
** Set heap size, stack size and stack location according
** to application requirements.
**
** Set memory bank area and size if external memory is used.
**
** Target : STMicroelectronics STM32
**
** Environment : Atollic TrueSTUDIO(R)
**
** Distribution: The file is distributed “as is,” without any warranty
** of any kind.
**
** (c)Copyright Atollic AB.
** You may use this file as-is or modify it according to the needs of your
** project. Distribution of this file (unmodified or modified) is not
** permitted. Atollic AB permit registered Atollic TrueSTUDIO(R) users the
** rights to distribute the assembled, compiled & linked contents of this
** file as part of an application binary file, provided that it is built
** using the Atollic TrueSTUDIO(R) toolchain.
**
*****************************************************************************
*/
/* Entry Point */
ENTRY(Reset_Handler)
/* Highest address of the user mode stack */
_estack = 0x20020000; /* end of 128K RAM on AHB bus*/
/* Generate a link error if heap and stack don't fit into RAM */
_Min_Heap_Size = 0; /* required amount of heap */
_Min_Stack_Size = 0x400; /* required amount of stack */
/* Specify the memory areas */
MEMORY
{
FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 128K
RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 128K
MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K
}
/* Define output sections */
SECTIONS
{
/* The startup code goes first into FLASH */
.isr_vector :
{
. = ALIGN(4);
KEEP(*(.isr_vector)) /* Startup code */
. = ALIGN(4);
} >FLASH
/* The program code and other data goes into FLASH */
.text :
{
. = ALIGN(4);
*(.text) /* .text sections (code) */
*(.text*) /* .text* sections (code) */
*(.rodata) /* .rodata sections (constants, strings, etc.) */
*(.rodata*) /* .rodata* sections (constants, strings, etc.) */
*(.glue_7) /* glue arm to thumb code */
*(.glue_7t) /* glue thumb to arm code */
*(.eh_frame)
KEEP (*(.init))
KEEP (*(.fini))
. = ALIGN(4);
_etext = .; /* define a global symbols at end of code */
_exit = .;
} >FLASH
.ARM.extab : { *(.ARM.extab* .gnu.linkonce.armextab.*) } >FLASH
.ARM : {
__exidx_start = .;
*(.ARM.exidx*)
__exidx_end = .;
} >FLASH
.preinit_array :
{
PROVIDE_HIDDEN (__preinit_array_start = .);
KEEP (*(.preinit_array*))
PROVIDE_HIDDEN (__preinit_array_end = .);
} >FLASH
.init_array :
{
PROVIDE_HIDDEN (__init_array_start = .);
KEEP (*(SORT(.init_array.*)))
KEEP (*(.init_array*))
PROVIDE_HIDDEN (__init_array_end = .);
} >FLASH
.fini_array :
{
PROVIDE_HIDDEN (__fini_array_start = .);
KEEP (*(.fini_array*))
KEEP (*(SORT(.fini_array.*)))
PROVIDE_HIDDEN (__fini_array_end = .);
} >FLASH
/* used by the startup to initialize data */
_sidata = .;
/* Initialized data sections goes into RAM, load LMA copy after code */
.data : AT ( _sidata )
{
. = ALIGN(4);
_sdata = .; /* create a global symbol at data start */
*(.data) /* .data sections */
*(.data*) /* .data* sections */
. = ALIGN(4);
_edata = .; /* define a global symbol at data end */
} >RAM
/* Uninitialized data section */
. = ALIGN(4);
.bss :
{
/* This is used by the startup in order to initialize the .bss secion */
_sbss = .; /* define a global symbol at bss start */
__bss_start__ = _sbss;
*(.bss)
*(.bss*)
*(COMMON)
. = ALIGN(4);
_ebss = .; /* define a global symbol at bss end */
__bss_end__ = _ebss;
} >RAM
/* User_heap_stack section, used to check that there is enough RAM left */
._user_heap_stack :
{
. = ALIGN(4);
PROVIDE ( end = . );
PROVIDE ( _end = . );
. = . + _Min_Heap_Size;
. = . + _Min_Stack_Size;
. = ALIGN(4);
} >RAM
/* MEMORY_bank1 section, code must be located here explicitly */
/* Example: extern int foo(void) __attribute__ ((section (".mb1text"))); */
.memory_b1_text :
{
*(.mb1text) /* .mb1text sections (code) */
*(.mb1text*) /* .mb1text* sections (code) */
*(.mb1rodata) /* read-only data (constants) */
*(.mb1rodata*)
} >MEMORY_B1
.ARM.attributes 0 : { *(.ARM.attributes) }
}

7
board/timer.h
Unescape View File

@ -0,0 +1,7 @@
void timer_init(TIM_TypeDef *TIM, int psc) {
TIM->PSC = psc-1;
TIM->DIER = TIM_DIER_UIE;
TIM->CR1 = TIM_CR1_CEN;
TIM->SR = 0;
}

3
board/tools/dfu-util-aarch64
Unescape View File

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

3
board/tools/dfu-util-x86_64
Unescape View File

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

20
board/tools/enter_download_mode.py
Unescape View File

@ -0,0 +1,20 @@
#!/usr/bin/env python
import usb1
import time
import traceback
if __name__ == "__main__":
context = usb1.USBContext()
for device in context.getDeviceList(skip_on_error=True):
if device.getVendorID() == 0xbbaa and device.getProductID()&0xFF00 == 0xdd00:
print "found device"
handle = device.open()
handle.claimInterface(0)
try:
handle.controlWrite(usb1.TYPE_VENDOR | usb1.RECIPIENT_DEVICE, 0xd1, 0, 0, '')
except Exception:
traceback.print_exc()
print "expected error, exiting cleanly"
time.sleep(1)

510
board/usb.h
Unescape View File

@ -0,0 +1,510 @@
// **** supporting defines ****
typedef struct
{
__IO uint32_t HPRT;
}
USB_OTG_HostPortTypeDef;
#define USBx_HOST ((USB_OTG_HostTypeDef *)((uint32_t)USBx + USB_OTG_HOST_BASE))
#define USBx_HOST_PORT ((USB_OTG_HostPortTypeDef *)((uint32_t)USBx + USB_OTG_HOST_PORT_BASE))
#define USBx_DEVICE ((USB_OTG_DeviceTypeDef *)((uint32_t)USBx + USB_OTG_DEVICE_BASE))
#define USBx_INEP(i) ((USB_OTG_INEndpointTypeDef *)((uint32_t)USBx + USB_OTG_IN_ENDPOINT_BASE + (i)*USB_OTG_EP_REG_SIZE))
#define USBx_OUTEP(i) ((USB_OTG_OUTEndpointTypeDef *)((uint32_t)USBx + USB_OTG_OUT_ENDPOINT_BASE + (i)*USB_OTG_EP_REG_SIZE))
#define USBx_DFIFO(i) *(__IO uint32_t *)((uint32_t)USBx + USB_OTG_FIFO_BASE + (i) * USB_OTG_FIFO_SIZE)
#define USBx_PCGCCTL *(__IO uint32_t *)((uint32_t)USBx + USB_OTG_PCGCCTL_BASE)
#define USB_REQ_GET_STATUS 0x00
#define USB_REQ_CLEAR_FEATURE 0x01
#define USB_REQ_SET_FEATURE 0x03
#define USB_REQ_SET_ADDRESS 0x05
#define USB_REQ_GET_DESCRIPTOR 0x06
#define USB_REQ_SET_DESCRIPTOR 0x07
#define USB_REQ_GET_CONFIGURATION 0x08
#define USB_REQ_SET_CONFIGURATION 0x09
#define USB_REQ_GET_INTERFACE 0x0A
#define USB_REQ_SET_INTERFACE 0x0B
#define USB_REQ_SYNCH_FRAME 0x0C
#define USB_DESC_TYPE_DEVICE 1
#define USB_DESC_TYPE_CONFIGURATION 2
#define USB_DESC_TYPE_STRING 3
#define USB_DESC_TYPE_INTERFACE 4
#define USB_DESC_TYPE_ENDPOINT 5
#define USB_DESC_TYPE_DEVICE_QUALIFIER 6
#define USB_DESC_TYPE_OTHER_SPEED_CONFIGURATION 7
#define STS_GOUT_NAK 1
#define STS_DATA_UPDT 2
#define STS_XFER_COMP 3
#define STS_SETUP_COMP 4
#define STS_SETUP_UPDT 6
#define USBD_FS_TRDT_VALUE 5
// interfaces
void usb_cb_control_msg();
void usb_cb_ep1_in(int len);
void usb_cb_ep2_out(uint8_t *usbdata, int len);
void usb_cb_ep3_out(uint8_t *usbdata, int len);
uint8_t device_desc[] = {
0x12,0x01,0x00,0x01,
0xFF,0xFF,0xFF,0x40,
(USB_VID>>0)&0xFF,(USB_VID>>8)&0xFF,
(USB_PID>>0)&0xFF,(USB_PID>>8)&0xFF,
0x00,0x22,0x00,0x00,
0x00,0x01};
uint8_t configuration_desc[] = {
0x09, 0x02, 0x27, 0x00,
0x01, 0x01, 0x00, 0xc0,
0x32,
// interface 0
0x09, 0x04, 0x00, 0x00,
0x03, 0xff, 0xFF, 0xFF,
0x00,
// endpoint 1, read CAN
0x07, 0x05, 0x81, 0x02, 0x40, 0x00, 0x00,
// endpoint 2, AES load
0x07, 0x05, 0x02, 0x02, 0x10, 0x00, 0x00,
// endpoint 3, send CAN
0x07, 0x05, 0x03, 0x02, 0x40, 0x00, 0x00,
};
typedef union
{
uint16_t w;
struct BW
{
uint8_t msb;
uint8_t lsb;
}
bw;
}
uint16_t_uint8_t;
typedef union _USB_Setup
{
uint32_t d8[2];
struct _SetupPkt_Struc
{
uint8_t bmRequestType;
uint8_t bRequest;
uint16_t_uint8_t wValue;
uint16_t_uint8_t wIndex;
uint16_t_uint8_t wLength;
} b;
}
USB_Setup_TypeDef;
// current packet
USB_Setup_TypeDef setup;
uint8_t usbdata[0x100];
// packet read and write
void *USB_ReadPacket(void *dest, uint16_t len) {
uint32_t i=0;
uint32_t count32b = (len + 3) / 4;
for ( i = 0; i < count32b; i++, dest += 4 ) {
// packed?
*(__attribute__((__packed__)) uint32_t *)dest = USBx_DFIFO(0);
}
return ((void *)dest);
}
void USB_WritePacket(const uint8_t *src, uint16_t len, uint32_t ep) {
#ifdef DEBUG
puts("writing ");
hexdump(src, len);
#endif
uint32_t count32b = 0, i = 0;
count32b = (len + 3) / 4;
// bullshit
USBx_INEP(ep)->DIEPTSIZ = (USB_OTG_DIEPTSIZ_PKTCNT & (1 << 19)) | (len & USB_OTG_DIEPTSIZ_XFRSIZ);
USBx_INEP(ep)->DIEPCTL |= (USB_OTG_DIEPCTL_CNAK | USB_OTG_DIEPCTL_EPENA);
// load the FIFO
for (i = 0; i < count32b; i++, src += 4) {
USBx_DFIFO(ep) = *((__attribute__((__packed__)) uint32_t *)src);
}
}
void usb_reset() {
// unmask endpoint interrupts, so many sets
USBx_DEVICE->DAINT = 0xFFFFFFFF;
USBx_DEVICE->DAINTMSK = 0xFFFFFFFF;
//USBx_DEVICE->DOEPMSK = (USB_OTG_DOEPMSK_STUPM | USB_OTG_DOEPMSK_XFRCM | USB_OTG_DOEPMSK_EPDM);
//USBx_DEVICE->DIEPMSK = (USB_OTG_DIEPMSK_TOM | USB_OTG_DIEPMSK_XFRCM | USB_OTG_DIEPMSK_EPDM | USB_OTG_DIEPMSK_ITTXFEMSK);
//USBx_DEVICE->DIEPMSK = (USB_OTG_DIEPMSK_TOM | USB_OTG_DIEPMSK_XFRCM | USB_OTG_DIEPMSK_EPDM);
// all interrupts for debugging
USBx_DEVICE->DIEPMSK = 0xFFFFFFFF;
USBx_DEVICE->DOEPMSK = 0xFFFFFFFF;
// clear interrupts
USBx_INEP(0)->DIEPINT = 0xFF;
USBx_OUTEP(0)->DOEPINT = 0xFF;
// unset the address
USBx_DEVICE->DCFG &= ~USB_OTG_DCFG_DAD;
// set up USB FIFOs
// RX start address is fixed to 0
USBx->GRXFSIZ = 0x40;
// 0x100 to offset past GRXFSIZ
USBx->DIEPTXF0_HNPTXFSIZ = (0x40 << 16) | 0x40;
// EP1, massive
USBx->DIEPTXF[0] = (0x40 << 16) | 0x80;
// flush TX fifo
USBx->GRSTCTL = USB_OTG_GRSTCTL_TXFFLSH | USB_OTG_GRSTCTL_TXFNUM_4;
while ((USBx->GRSTCTL & USB_OTG_GRSTCTL_TXFFLSH) == USB_OTG_GRSTCTL_TXFFLSH);
// flush RX FIFO
USBx->GRSTCTL = USB_OTG_GRSTCTL_RXFFLSH;
while ((USBx->GRSTCTL & USB_OTG_GRSTCTL_RXFFLSH) == USB_OTG_GRSTCTL_RXFFLSH);
// no global NAK
USBx_DEVICE->DCTL |= USB_OTG_DCTL_CGINAK;
// ready to receive setup packets
USBx_OUTEP(0)->DOEPTSIZ = USB_OTG_DOEPTSIZ_STUPCNT | (USB_OTG_DOEPTSIZ_PKTCNT & (1 << 19)) | (3 * 8);
}
void usb_setup() {
uint8_t resp[0x20];
// setup packet is ready
switch (setup.b.bRequest) {
case USB_REQ_SET_CONFIGURATION:
// enable other endpoints, has to be here?
USBx_INEP(1)->DIEPCTL = (0x40 & USB_OTG_DIEPCTL_MPSIZ) | (2 << 18) | (1 << 22) |
USB_OTG_DIEPCTL_SD0PID_SEVNFRM | USB_OTG_DIEPCTL_USBAEP;
USBx_INEP(1)->DIEPINT = 0xFF;
USBx_OUTEP(2)->DOEPTSIZ = (1 << 19) | 0x10;
USBx_OUTEP(2)->DOEPCTL = (0x10 & USB_OTG_DOEPCTL_MPSIZ) | (2 << 18) |
USB_OTG_DOEPCTL_SD0PID_SEVNFRM | USB_OTG_DOEPCTL_USBAEP;
USBx_OUTEP(2)->DOEPINT = 0xFF;
USBx_OUTEP(3)->DOEPTSIZ = (1 << 19) | 0x40;
USBx_OUTEP(3)->DOEPCTL = (0x40 & USB_OTG_DOEPCTL_MPSIZ) | (2 << 18) |
USB_OTG_DOEPCTL_SD0PID_SEVNFRM | USB_OTG_DOEPCTL_USBAEP;
USBx_OUTEP(3)->DOEPINT = 0xFF;
// mark ready to receive
USBx_OUTEP(2)->DOEPCTL |= USB_OTG_DOEPCTL_EPENA | USB_OTG_DOEPCTL_CNAK;
USBx_OUTEP(3)->DOEPCTL |= USB_OTG_DOEPCTL_EPENA | USB_OTG_DOEPCTL_CNAK;
USB_WritePacket(0, 0, 0);
USBx_OUTEP(0)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK;
break;
case USB_REQ_SET_ADDRESS:
// set now?
USBx_DEVICE->DCFG |= ((setup.b.wValue.w & 0x7f) << 4);
#ifdef DEBUG
puts(" set address\n");
#endif
USB_WritePacket(0, 0, 0);
USBx_OUTEP(0)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK;
break;
case USB_REQ_GET_DESCRIPTOR:
switch (setup.b.wValue.bw.lsb) {
case USB_DESC_TYPE_DEVICE:
//puts(" writing device descriptor\n");
// setup transfer
USB_WritePacket(device_desc, min(sizeof(device_desc), setup.b.wLength.w), 0);
USBx_OUTEP(0)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK;
//puts("D");
break;
case USB_DESC_TYPE_CONFIGURATION:
USB_WritePacket(configuration_desc, min(sizeof(configuration_desc), setup.b.wLength.w), 0);
USBx_OUTEP(0)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK;
break;
default:
// nothing here?
USB_WritePacket(0, 0, 0);
USBx_OUTEP(0)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK;
break;
}
break;
case USB_REQ_GET_STATUS:
// empty resp?
resp[0] = 0;
resp[1] = 0;
USB_WritePacket((void*)&resp, 2, 0);
USBx_OUTEP(0)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK;
break;
default:
usb_cb_control_msg();
}
}
void usb_init() {
// internal PHY set before reset
USBx->GUSBCFG |= USB_OTG_GUSBCFG_PHYSEL;
// full speed PHY, do reset and remove power down
puth(USBx->GRSTCTL);
puts(" resetting PHY\n");
while ((USBx->GRSTCTL & USB_OTG_GRSTCTL_AHBIDL) == 0);
puts("AHB idle\n");
// reset PHY here?
USBx->GRSTCTL |= USB_OTG_GRSTCTL_CSRST;
while ((USBx->GRSTCTL & USB_OTG_GRSTCTL_CSRST) == USB_OTG_GRSTCTL_CSRST);
puts("reset done\n");
// power up the PHY
USBx->GCCFG = USB_OTG_GCCFG_PWRDWN | USB_OTG_GCCFG_NOVBUSSENS;
// be a device, slowest timings
//USBx->GUSBCFG = USB_OTG_GUSBCFG_FDMOD | USB_OTG_GUSBCFG_PHYSEL | USB_OTG_GUSBCFG_TRDT | USB_OTG_GUSBCFG_TOCAL;
USBx->GUSBCFG = USB_OTG_GUSBCFG_FDMOD | USB_OTG_GUSBCFG_PHYSEL;
USBx->GUSBCFG |= (uint32_t)((USBD_FS_TRDT_VALUE << 10) & USB_OTG_GUSBCFG_TRDT);
//USBx->GUSBCFG = USB_OTG_GUSBCFG_PHYSEL | USB_OTG_GUSBCFG_TRDT | USB_OTG_GUSBCFG_TOCAL;
// **** for debugging, doesn't seem to work ****
//USBx->GUSBCFG |= USB_OTG_GUSBCFG_CTXPKT;
// reset PHY clock
USBx_PCGCCTL = 0;
// enable the fancy OTG things
USBx->GUSBCFG |= USB_OTG_GUSBCFG_HNPCAP | USB_OTG_GUSBCFG_SRPCAP;
USBx_DEVICE->DCFG = USB_OTG_DCFG_NZLSOHSK | USB_OTG_DCFG_DSPD;
//USBx_DEVICE->DCFG = USB_OTG_DCFG_DSPD;
// setup USB interrupts
// all interrupts except TXFIFO EMPTY
//USBx->GINTMSK = 0xFFFFFFFF & ~(USB_OTG_GINTMSK_NPTXFEM | USB_OTG_GINTMSK_PTXFEM | USB_OTG_GINTSTS_SOF | USB_OTG_GINTSTS_EOPF);
USBx->GINTMSK = 0xFFFFFFFF & ~(USB_OTG_GINTMSK_NPTXFEM | USB_OTG_GINTMSK_PTXFEM);
USBx->GAHBCFG = USB_OTG_GAHBCFG_GINT;
USBx->GINTSTS = 0;
}
// ***************************** USB port *****************************
void usb_irqhandler(void) {
USBx->GINTMSK = 0;
unsigned int gintsts = USBx->GINTSTS;
// gintsts SUSPEND? 04008428
#ifdef DEBUG
unsigned int daint = USBx_DEVICE->DAINT;
puth(gintsts);
puts(" ep ");
puth(daint);
puts(" USB interrupt!\n");
#endif
if (gintsts & USB_OTG_GINTSTS_ESUSP) {
puts("ESUSP detected\n");
}
if (gintsts & USB_OTG_GINTSTS_USBRST) {
puts("USB reset\n");
usb_reset();
}
if (gintsts & USB_OTG_GINTSTS_ENUMDNE) {
puts("enumeration done ");
// Full speed, ENUMSPD
puth(USBx_DEVICE->DSTS);
puts("\n");
}
if (gintsts & USB_OTG_GINTSTS_OTGINT) {
puts("OTG int:");
puth(USBx->GOTGINT);
puts("\n");
// getting ADTOCHG
USBx->GOTGINT = USBx->GOTGINT;
}
// RX FIFO first
if (gintsts & USB_OTG_GINTSTS_RXFLVL) {
// 1. Read the Receive status pop register
volatile unsigned int rxst = USBx->GRXSTSP;
#ifdef DEBUG
puts(" RX FIFO:");
puth(rxst);
puts(" status: ");
puth((rxst & USB_OTG_GRXSTSP_PKTSTS) >> 17);
puts(" len: ");
puth((rxst & USB_OTG_GRXSTSP_BCNT) >> 4);
puts("\n");
#endif
if (((rxst & USB_OTG_GRXSTSP_PKTSTS) >> 17) == STS_DATA_UPDT) {
int endpoint = (rxst & USB_OTG_GRXSTSP_EPNUM);
int len = (rxst & USB_OTG_GRXSTSP_BCNT) >> 4;
USB_ReadPacket(&usbdata, len);
#ifdef DEBUG
puts(" data ");
puth(len);
puts("\n");
hexdump(&usbdata, len);
#endif
if (endpoint == 2) {
usb_cb_ep2_out(usbdata, len);
}
if (endpoint == 3) {
usb_cb_ep3_out(usbdata, len);
}
} else if (((rxst & USB_OTG_GRXSTSP_PKTSTS) >> 17) == STS_SETUP_UPDT) {
USB_ReadPacket(&setup, 8);
#ifdef DEBUG
puts(" setup ");
hexdump(&setup, 8);
puts("\n");
#endif
}
}
if (gintsts & USB_OTG_GINTSTS_HPRTINT) {
// host
puts("HPRT:");
puth(USBx_HOST_PORT->HPRT);
puts("\n");
if (USBx_HOST_PORT->HPRT & USB_OTG_HPRT_PCDET) {
USBx_HOST_PORT->HPRT |= USB_OTG_HPRT_PRST;
USBx_HOST_PORT->HPRT |= USB_OTG_HPRT_PCDET;
}
}
if (gintsts & USB_OTG_GINTSTS_BOUTNAKEFF) {
// no global NAK, why is this getting set?
#ifdef DEBUG
puts("GLOBAL NAK\n");
#endif
USBx_DEVICE->DCTL |= USB_OTG_DCTL_CGONAK | USB_OTG_DCTL_CGINAK;
}
if (gintsts & USB_OTG_GINTSTS_SRQINT) {
// we want to do "A-device host negotiation protocol" since we are the A-device
puts("start request\n");
//USBx->GUSBCFG |= USB_OTG_GUSBCFG_FDMOD;
//USBx_HOST_PORT->HPRT = USB_OTG_HPRT_PPWR | USB_OTG_HPRT_PENA;
}
// out endpoint hit
if (gintsts & USB_OTG_GINTSTS_OEPINT) {
#ifdef DEBUG
puts(" 0:");
puth(USBx_OUTEP(0)->DOEPINT);
puts(" 2:");
puth(USBx_OUTEP(2)->DOEPINT);
puts(" 3:");
puth(USBx_OUTEP(3)->DOEPINT);
puts(" ");
puth(USBx_OUTEP(3)->DOEPCTL);
puts(" 4:");
puth(USBx_OUTEP(4)->DOEPINT);
puts(" OUT ENDPOINT\n");
#endif
if (USBx_OUTEP(2)->DOEPINT & USB_OTG_DOEPINT_XFRC) {
#ifdef DEBUG
puts(" OUT2 PACKET XFRC\n");
#endif
USBx_OUTEP(2)->DOEPTSIZ = (1 << 19) | 0x10;
USBx_OUTEP(2)->DOEPCTL |= USB_OTG_DOEPCTL_EPENA | USB_OTG_DOEPCTL_CNAK;
}
if (USBx_OUTEP(3)->DOEPINT & USB_OTG_DOEPINT_XFRC) {
#ifdef DEBUG
puts(" OUT3 PACKET XFRC\n");
#endif
USBx_OUTEP(3)->DOEPTSIZ = (1 << 19) | 0x40;
USBx_OUTEP(3)->DOEPCTL |= USB_OTG_DOEPCTL_EPENA | USB_OTG_DOEPCTL_CNAK;
} else if (USBx_OUTEP(3)->DOEPINT & 0x2000) {
#ifdef DEBUG
puts(" OUT3 PACKET WTF\n");
#endif
// if NAK was set trigger this, unknown interrupt
USBx_OUTEP(3)->DOEPTSIZ = (1 << 19) | 0x40;
USBx_OUTEP(3)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK;
} else if (USBx_OUTEP(3)->DOEPINT) {
puts("OUTEP3 error ");
puth(USBx_OUTEP(3)->DOEPINT);
puts("\n");
}
if (USBx_OUTEP(0)->DOEPINT & USB_OTG_DIEPINT_XFRC) {
// ready for next packet
USBx_OUTEP(0)->DOEPTSIZ = USB_OTG_DOEPTSIZ_STUPCNT | (USB_OTG_DOEPTSIZ_PKTCNT & (1 << 19)) | (1 * 8);
}
// respond to setup packets
if (USBx_OUTEP(0)->DOEPINT & USB_OTG_DOEPINT_STUP) {
usb_setup();
}
USBx_OUTEP(0)->DOEPINT = USBx_OUTEP(0)->DOEPINT;
USBx_OUTEP(2)->DOEPINT = USBx_OUTEP(2)->DOEPINT;
USBx_OUTEP(3)->DOEPINT = USBx_OUTEP(3)->DOEPINT;
}
// in endpoint hit
if (gintsts & USB_OTG_GINTSTS_IEPINT) {
#ifdef DEBUG
puts(" ");
puth(USBx_INEP(0)->DIEPINT);
puts(" ");
puth(USBx_INEP(1)->DIEPINT);
puts(" IN ENDPOINT\n");
#endif
// this happens first
if (USBx_INEP(1)->DIEPINT & USB_OTG_DIEPINT_XFRC) {
#ifdef DEBUG
puts(" IN PACKET SEND\n");
#endif
//USBx_DEVICE->DIEPEMPMSK = ~(1 << 1);
}
// *** IN token received when TxFIFO is empty
if (USBx_INEP(1)->DIEPINT & USB_OTG_DIEPMSK_ITTXFEMSK) {
#ifdef DEBUG
puts(" IN PACKET QUEUE\n");
#endif
// TODO: always assuming max len, can we get the length?
usb_cb_ep1_in(0x40);
}
// clear interrupts
USBx_INEP(0)->DIEPINT = USBx_INEP(0)->DIEPINT;
USBx_INEP(1)->DIEPINT = USBx_INEP(1)->DIEPINT;
}
// clear all interrupts
USBx_DEVICE->DAINT = USBx_DEVICE->DAINT;
USBx->GINTSTS = USBx->GINTSTS;
USBx->GINTMSK = 0xFFFFFFFF & ~(USB_OTG_GINTMSK_NPTXFEM | USB_OTG_GINTMSK_PTXFEM | USB_OTG_GINTSTS_SOF | USB_OTG_GINTSTS_EOPF);
}

7
cereal/__init__.py
Unescape View File

@ -0,0 +1,7 @@
import os
import capnp
capnp.remove_import_hook()
CEREAL_PATH = os.path.dirname(os.path.abspath(__file__))
log = capnp.load(os.path.join(CEREAL_PATH, "log.capnp"))

26
cereal/c++.capnp
Unescape View File

@ -0,0 +1,26 @@
# Copyright (c) 2013-2014 Sandstorm Development Group, Inc. and contributors
# Licensed under the MIT License:
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in
# all copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
# THE SOFTWARE.
@0xbdf87d7bb8304e81;
$namespace("capnp::annotations");
annotation namespace(file): Text;
annotation name(field, enumerant, struct, enum, interface, method, param, group, union): Text;

0
cereal/gen/c/c++.capnp.h
Unescape View File

1016
cereal/gen/c/log.capnp.c
View File

File diff suppressed because it is too large Load Diff

667
cereal/gen/c/log.capnp.h
Unescape View File

@ -0,0 +1,667 @@
#ifndef CAPN_F3B1F17E25A4285B
#define CAPN_F3B1F17E25A4285B
/* AUTO GENERATED - DO NOT EDIT */
#include <capnp_c.h>
#if CAPN_VERSION != 1
#error "version mismatch between capnp_c.h and generated code"
#endif
#include "c++.capnp.h"
#ifdef __cplusplus
extern "C" {
#endif
struct cereal_InitData;
struct cereal_FrameData;
struct cereal_GPSNMEAData;
struct cereal_SensorEventData;
struct cereal_SensorEventData_SensorVec;
struct cereal_CanData;
struct cereal_ThermalData;
struct cereal_HealthData;
struct cereal_LiveUI;
struct cereal_Live20Data;
struct cereal_Live20Data_LeadData;
struct cereal_LiveCalibrationData;
struct cereal_LiveTracks;
struct cereal_Live100Data;
struct cereal_LiveEventData;
struct cereal_ModelData;
struct cereal_ModelData_PathData;
struct cereal_ModelData_LeadData;
struct cereal_ModelData_ModelSettings;
struct cereal_CalibrationFeatures;
struct cereal_EncodeIndex;
struct cereal_AndroidLogEntry;
struct cereal_LogRotate;
struct cereal_Event;
typedef struct {capn_ptr p;} cereal_InitData_ptr;
typedef struct {capn_ptr p;} cereal_FrameData_ptr;
typedef struct {capn_ptr p;} cereal_GPSNMEAData_ptr;
typedef struct {capn_ptr p;} cereal_SensorEventData_ptr;
typedef struct {capn_ptr p;} cereal_SensorEventData_SensorVec_ptr;
typedef struct {capn_ptr p;} cereal_CanData_ptr;
typedef struct {capn_ptr p;} cereal_ThermalData_ptr;
typedef struct {capn_ptr p;} cereal_HealthData_ptr;
typedef struct {capn_ptr p;} cereal_LiveUI_ptr;
typedef struct {capn_ptr p;} cereal_Live20Data_ptr;
typedef struct {capn_ptr p;} cereal_Live20Data_LeadData_ptr;
typedef struct {capn_ptr p;} cereal_LiveCalibrationData_ptr;
typedef struct {capn_ptr p;} cereal_LiveTracks_ptr;
typedef struct {capn_ptr p;} cereal_Live100Data_ptr;
typedef struct {capn_ptr p;} cereal_LiveEventData_ptr;
typedef struct {capn_ptr p;} cereal_ModelData_ptr;
typedef struct {capn_ptr p;} cereal_ModelData_PathData_ptr;
typedef struct {capn_ptr p;} cereal_ModelData_LeadData_ptr;
typedef struct {capn_ptr p;} cereal_ModelData_ModelSettings_ptr;
typedef struct {capn_ptr p;} cereal_CalibrationFeatures_ptr;
typedef struct {capn_ptr p;} cereal_EncodeIndex_ptr;
typedef struct {capn_ptr p;} cereal_AndroidLogEntry_ptr;
typedef struct {capn_ptr p;} cereal_LogRotate_ptr;
typedef struct {capn_ptr p;} cereal_Event_ptr;
typedef struct {capn_ptr p;} cereal_InitData_list;
typedef struct {capn_ptr p;} cereal_FrameData_list;
typedef struct {capn_ptr p;} cereal_GPSNMEAData_list;
typedef struct {capn_ptr p;} cereal_SensorEventData_list;
typedef struct {capn_ptr p;} cereal_SensorEventData_SensorVec_list;
typedef struct {capn_ptr p;} cereal_CanData_list;
typedef struct {capn_ptr p;} cereal_ThermalData_list;
typedef struct {capn_ptr p;} cereal_HealthData_list;
typedef struct {capn_ptr p;} cereal_LiveUI_list;
typedef struct {capn_ptr p;} cereal_Live20Data_list;
typedef struct {capn_ptr p;} cereal_Live20Data_LeadData_list;
typedef struct {capn_ptr p;} cereal_LiveCalibrationData_list;
typedef struct {capn_ptr p;} cereal_LiveTracks_list;
typedef struct {capn_ptr p;} cereal_Live100Data_list;
typedef struct {capn_ptr p;} cereal_LiveEventData_list;
typedef struct {capn_ptr p;} cereal_ModelData_list;
typedef struct {capn_ptr p;} cereal_ModelData_PathData_list;
typedef struct {capn_ptr p;} cereal_ModelData_LeadData_list;
typedef struct {capn_ptr p;} cereal_ModelData_ModelSettings_list;
typedef struct {capn_ptr p;} cereal_CalibrationFeatures_list;
typedef struct {capn_ptr p;} cereal_EncodeIndex_list;
typedef struct {capn_ptr p;} cereal_AndroidLogEntry_list;
typedef struct {capn_ptr p;} cereal_LogRotate_list;
typedef struct {capn_ptr p;} cereal_Event_list;
enum cereal_EncodeIndex_Type {
cereal_EncodeIndex_Type_bigBoxLossless = 0,
cereal_EncodeIndex_Type_fullHEVC = 1,
cereal_EncodeIndex_Type_bigBoxHEVC = 2
};
extern int32_t cereal_logVersion;
struct cereal_InitData {
capn_ptr kernelArgs;
capn_text gctx;
capn_text dongleId;
};
static const size_t cereal_InitData_word_count = 0;
static const size_t cereal_InitData_pointer_count = 3;
static const size_t cereal_InitData_struct_bytes_count = 24;
struct cereal_FrameData {
uint32_t frameId;
uint32_t encodeId;
uint64_t timestampEof;
int32_t frameLength;
int32_t integLines;
int32_t globalGain;
capn_data image;
};
static const size_t cereal_FrameData_word_count = 4;
static const size_t cereal_FrameData_pointer_count = 1;
static const size_t cereal_FrameData_struct_bytes_count = 40;
struct cereal_GPSNMEAData {
int64_t timestamp;
uint64_t localWallTime;
capn_text nmea;
};
static const size_t cereal_GPSNMEAData_word_count = 2;
static const size_t cereal_GPSNMEAData_pointer_count = 1;
static const size_t cereal_GPSNMEAData_struct_bytes_count = 24;
enum cereal_SensorEventData_which {
cereal_SensorEventData_acceleration = 0,
cereal_SensorEventData_magnetic = 1,
cereal_SensorEventData_orientation = 2,
cereal_SensorEventData_gyro = 3
};
struct cereal_SensorEventData {
int32_t version;
int32_t sensor;
int32_t type;
int64_t timestamp;
enum cereal_SensorEventData_which which;
union {
cereal_SensorEventData_SensorVec_ptr acceleration;
cereal_SensorEventData_SensorVec_ptr magnetic;
cereal_SensorEventData_SensorVec_ptr orientation;
cereal_SensorEventData_SensorVec_ptr gyro;
};
};
static const size_t cereal_SensorEventData_word_count = 3;
static const size_t cereal_SensorEventData_pointer_count = 1;
static const size_t cereal_SensorEventData_struct_bytes_count = 32;
struct cereal_SensorEventData_SensorVec {
capn_list32 v;
int8_t status;
};
static const size_t cereal_SensorEventData_SensorVec_word_count = 1;
static const size_t cereal_SensorEventData_SensorVec_pointer_count = 1;
static const size_t cereal_SensorEventData_SensorVec_struct_bytes_count = 16;
struct cereal_CanData {
uint32_t address;
uint16_t busTime;
capn_data dat;
int8_t src;
};
static const size_t cereal_CanData_word_count = 1;
static const size_t cereal_CanData_pointer_count = 1;
static const size_t cereal_CanData_struct_bytes_count = 16;
struct cereal_ThermalData {
uint16_t cpu0;
uint16_t cpu1;
uint16_t cpu2;
uint16_t cpu3;
uint16_t mem;
uint16_t gpu;
uint32_t bat;
};
static const size_t cereal_ThermalData_word_count = 2;
static const size_t cereal_ThermalData_pointer_count = 0;
static const size_t cereal_ThermalData_struct_bytes_count = 16;
struct cereal_HealthData {
uint32_t voltage;
uint32_t current;
unsigned started : 1;
unsigned controlsAllowed : 1;
unsigned gasInterceptorDetected : 1;
};
static const size_t cereal_HealthData_word_count = 2;
static const size_t cereal_HealthData_pointer_count = 0;
static const size_t cereal_HealthData_struct_bytes_count = 16;
struct cereal_LiveUI {
unsigned rearViewCam : 1;
capn_text alertText1;
capn_text alertText2;
float awarenessStatus;
};
static const size_t cereal_LiveUI_word_count = 1;
static const size_t cereal_LiveUI_pointer_count = 2;
static const size_t cereal_LiveUI_struct_bytes_count = 24;
struct cereal_Live20Data {
capn_list64 canMonoTimes;
uint64_t mdMonoTime;
uint64_t ftMonoTime;
capn_list32 warpMatrix;
float angleOffset;
int8_t calStatus;
int32_t calCycle;
int8_t calPerc;
cereal_Live20Data_LeadData_ptr leadOne;
cereal_Live20Data_LeadData_ptr leadTwo;
float cumLagMs;
};
static const size_t cereal_Live20Data_word_count = 4;
static const size_t cereal_Live20Data_pointer_count = 4;
static const size_t cereal_Live20Data_struct_bytes_count = 64;
struct cereal_Live20Data_LeadData {
float dRel;
float yRel;
float vRel;
float aRel;
float vLead;
float aLead;
float dPath;
float vLat;
float vLeadK;
float aLeadK;
unsigned fcw : 1;
unsigned status : 1;
};
static const size_t cereal_Live20Data_LeadData_word_count = 6;
static const size_t cereal_Live20Data_LeadData_pointer_count = 0;
static const size_t cereal_Live20Data_LeadData_struct_bytes_count = 48;
struct cereal_LiveCalibrationData {
capn_list32 warpMatrix;
int8_t calStatus;
int32_t calCycle;
int8_t calPerc;
};
static const size_t cereal_LiveCalibrationData_word_count = 1;
static const size_t cereal_LiveCalibrationData_pointer_count = 1;
static const size_t cereal_LiveCalibrationData_struct_bytes_count = 16;
struct cereal_LiveTracks {
int32_t trackId;
float dRel;
float yRel;
float vRel;
float aRel;
float timeStamp;
float status;
float currentTime;
unsigned stationary : 1;
unsigned oncoming : 1;
};
static const size_t cereal_LiveTracks_word_count = 5;
static const size_t cereal_LiveTracks_pointer_count = 0;
static const size_t cereal_LiveTracks_struct_bytes_count = 40;
struct cereal_Live100Data {
uint64_t canMonoTime;
capn_list64 canMonoTimes;
uint64_t l20MonoTime;
uint64_t mdMonoTime;
float vEgo;
float aEgo;
float vPid;
float vTargetLead;
float upAccelCmd;
float uiAccelCmd;
float yActual;
float yDes;
float upSteer;
float uiSteer;
float aTargetMin;
float aTargetMax;
float jerkFactor;
float angleSteers;
int32_t hudLead;
float cumLagMs;
unsigned enabled : 1;
unsigned steerOverride : 1;
float vCruise;
unsigned rearViewCam : 1;
capn_text alertText1;
capn_text alertText2;
float awarenessStatus;
};
static const size_t cereal_Live100Data_word_count = 13;
static const size_t cereal_Live100Data_pointer_count = 3;
static const size_t cereal_Live100Data_struct_bytes_count = 128;
struct cereal_LiveEventData {
capn_text name;
int32_t value;
};
static const size_t cereal_LiveEventData_word_count = 1;
static const size_t cereal_LiveEventData_pointer_count = 1;
static const size_t cereal_LiveEventData_struct_bytes_count = 16;
struct cereal_ModelData {
uint32_t frameId;
cereal_ModelData_PathData_ptr path;
cereal_ModelData_PathData_ptr leftLane;
cereal_ModelData_PathData_ptr rightLane;
cereal_ModelData_LeadData_ptr lead;
cereal_ModelData_ModelSettings_ptr settings;
};
static const size_t cereal_ModelData_word_count = 1;
static const size_t cereal_ModelData_pointer_count = 5;
static const size_t cereal_ModelData_struct_bytes_count = 48;
struct cereal_ModelData_PathData {
capn_list32 points;
float prob;
float std;
};
static const size_t cereal_ModelData_PathData_word_count = 1;
static const size_t cereal_ModelData_PathData_pointer_count = 1;
static const size_t cereal_ModelData_PathData_struct_bytes_count = 16;
struct cereal_ModelData_LeadData {
float dist;
float prob;
float std;
};
static const size_t cereal_ModelData_LeadData_word_count = 2;
static const size_t cereal_ModelData_LeadData_pointer_count = 0;
static const size_t cereal_ModelData_LeadData_struct_bytes_count = 16;
struct cereal_ModelData_ModelSettings {
uint16_t bigBoxX;
uint16_t bigBoxY;
uint16_t bigBoxWidth;
uint16_t bigBoxHeight;
capn_list32 boxProjection;
capn_list32 yuvCorrection;
};
static const size_t cereal_ModelData_ModelSettings_word_count = 1;
static const size_t cereal_ModelData_ModelSettings_pointer_count = 2;
static const size_t cereal_ModelData_ModelSettings_struct_bytes_count = 24;
struct cereal_CalibrationFeatures {
uint32_t frameId;
capn_list32 p0;
capn_list32 p1;
capn_list8 status;
};
static const size_t cereal_CalibrationFeatures_word_count = 1;
static const size_t cereal_CalibrationFeatures_pointer_count = 3;
static const size_t cereal_CalibrationFeatures_struct_bytes_count = 32;
struct cereal_EncodeIndex {
uint32_t frameId;
enum cereal_EncodeIndex_Type type;
uint32_t encodeId;
int32_t segmentNum;
uint32_t segmentId;
};
static const size_t cereal_EncodeIndex_word_count = 3;
static const size_t cereal_EncodeIndex_pointer_count = 0;
static const size_t cereal_EncodeIndex_struct_bytes_count = 24;
struct cereal_AndroidLogEntry {
uint8_t id;
uint64_t ts;
uint8_t priority;
int32_t pid;
int32_t tid;
capn_text tag;
capn_text message;
};
static const size_t cereal_AndroidLogEntry_word_count = 3;
static const size_t cereal_AndroidLogEntry_pointer_count = 2;
static const size_t cereal_AndroidLogEntry_struct_bytes_count = 40;
struct cereal_LogRotate {
int32_t segmentNum;
capn_text path;
};
static const size_t cereal_LogRotate_word_count = 1;
static const size_t cereal_LogRotate_pointer_count = 1;
static const size_t cereal_LogRotate_struct_bytes_count = 16;
enum cereal_Event_which {
cereal_Event_initData = 0,
cereal_Event_frame = 1,
cereal_Event_gpsNMEA = 2,
cereal_Event_sensorEventDEPRECATED = 3,
cereal_Event_can = 4,
cereal_Event_thermal = 5,
cereal_Event_live100 = 6,
cereal_Event_liveEventDEPRECATED = 7,
cereal_Event_model = 8,
cereal_Event_features = 9,
cereal_Event_sensorEvents = 10,
cereal_Event_health = 11,
cereal_Event_live20 = 12,
cereal_Event_liveUIDEPRECATED = 13,
cereal_Event_encodeIdx = 14,
cereal_Event_liveTracks = 15,
cereal_Event_sendcan = 16,
cereal_Event_logMessage = 17,
cereal_Event_liveCalibration = 18,
cereal_Event_androidLogEntry = 19
};
struct cereal_Event {
uint64_t logMonoTime;
enum cereal_Event_which which;
union {
cereal_InitData_ptr initData;
cereal_FrameData_ptr frame;
cereal_GPSNMEAData_ptr gpsNMEA;
cereal_SensorEventData_ptr sensorEventDEPRECATED;
cereal_CanData_list can;
cereal_ThermalData_ptr thermal;
cereal_Live100Data_ptr live100;
cereal_LiveEventData_list liveEventDEPRECATED;
cereal_ModelData_ptr model;
cereal_CalibrationFeatures_ptr features;
cereal_SensorEventData_list sensorEvents;
cereal_HealthData_ptr health;
cereal_Live20Data_ptr live20;
cereal_LiveUI_ptr liveUIDEPRECATED;
cereal_EncodeIndex_ptr encodeIdx;
cereal_LiveTracks_list liveTracks;
cereal_CanData_list sendcan;
capn_text logMessage;
cereal_LiveCalibrationData_ptr liveCalibration;
cereal_AndroidLogEntry_ptr androidLogEntry;
};
};
static const size_t cereal_Event_word_count = 2;
static const size_t cereal_Event_pointer_count = 1;
static const size_t cereal_Event_struct_bytes_count = 24;
cereal_InitData_ptr cereal_new_InitData(struct capn_segment*);
cereal_FrameData_ptr cereal_new_FrameData(struct capn_segment*);
cereal_GPSNMEAData_ptr cereal_new_GPSNMEAData(struct capn_segment*);
cereal_SensorEventData_ptr cereal_new_SensorEventData(struct capn_segment*);
cereal_SensorEventData_SensorVec_ptr cereal_new_SensorEventData_SensorVec(struct capn_segment*);
cereal_CanData_ptr cereal_new_CanData(struct capn_segment*);
cereal_ThermalData_ptr cereal_new_ThermalData(struct capn_segment*);
cereal_HealthData_ptr cereal_new_HealthData(struct capn_segment*);
cereal_LiveUI_ptr cereal_new_LiveUI(struct capn_segment*);
cereal_Live20Data_ptr cereal_new_Live20Data(struct capn_segment*);
cereal_Live20Data_LeadData_ptr cereal_new_Live20Data_LeadData(struct capn_segment*);
cereal_LiveCalibrationData_ptr cereal_new_LiveCalibrationData(struct capn_segment*);
cereal_LiveTracks_ptr cereal_new_LiveTracks(struct capn_segment*);
cereal_Live100Data_ptr cereal_new_Live100Data(struct capn_segment*);
cereal_LiveEventData_ptr cereal_new_LiveEventData(struct capn_segment*);
cereal_ModelData_ptr cereal_new_ModelData(struct capn_segment*);
cereal_ModelData_PathData_ptr cereal_new_ModelData_PathData(struct capn_segment*);
cereal_ModelData_LeadData_ptr cereal_new_ModelData_LeadData(struct capn_segment*);
cereal_ModelData_ModelSettings_ptr cereal_new_ModelData_ModelSettings(struct capn_segment*);
cereal_CalibrationFeatures_ptr cereal_new_CalibrationFeatures(struct capn_segment*);
cereal_EncodeIndex_ptr cereal_new_EncodeIndex(struct capn_segment*);
cereal_AndroidLogEntry_ptr cereal_new_AndroidLogEntry(struct capn_segment*);
cereal_LogRotate_ptr cereal_new_LogRotate(struct capn_segment*);
cereal_Event_ptr cereal_new_Event(struct capn_segment*);
cereal_InitData_list cereal_new_InitData_list(struct capn_segment*, int len);
cereal_FrameData_list cereal_new_FrameData_list(struct capn_segment*, int len);
cereal_GPSNMEAData_list cereal_new_GPSNMEAData_list(struct capn_segment*, int len);
cereal_SensorEventData_list cereal_new_SensorEventData_list(struct capn_segment*, int len);
cereal_SensorEventData_SensorVec_list cereal_new_SensorEventData_SensorVec_list(struct capn_segment*, int len);
cereal_CanData_list cereal_new_CanData_list(struct capn_segment*, int len);
cereal_ThermalData_list cereal_new_ThermalData_list(struct capn_segment*, int len);
cereal_HealthData_list cereal_new_HealthData_list(struct capn_segment*, int len);
cereal_LiveUI_list cereal_new_LiveUI_list(struct capn_segment*, int len);
cereal_Live20Data_list cereal_new_Live20Data_list(struct capn_segment*, int len);
cereal_Live20Data_LeadData_list cereal_new_Live20Data_LeadData_list(struct capn_segment*, int len);
cereal_LiveCalibrationData_list cereal_new_LiveCalibrationData_list(struct capn_segment*, int len);
cereal_LiveTracks_list cereal_new_LiveTracks_list(struct capn_segment*, int len);
cereal_Live100Data_list cereal_new_Live100Data_list(struct capn_segment*, int len);
cereal_LiveEventData_list cereal_new_LiveEventData_list(struct capn_segment*, int len);
cereal_ModelData_list cereal_new_ModelData_list(struct capn_segment*, int len);
cereal_ModelData_PathData_list cereal_new_ModelData_PathData_list(struct capn_segment*, int len);
cereal_ModelData_LeadData_list cereal_new_ModelData_LeadData_list(struct capn_segment*, int len);
cereal_ModelData_ModelSettings_list cereal_new_ModelData_ModelSettings_list(struct capn_segment*, int len);
cereal_CalibrationFeatures_list cereal_new_CalibrationFeatures_list(struct capn_segment*, int len);
cereal_EncodeIndex_list cereal_new_EncodeIndex_list(struct capn_segment*, int len);
cereal_AndroidLogEntry_list cereal_new_AndroidLogEntry_list(struct capn_segment*, int len);
cereal_LogRotate_list cereal_new_LogRotate_list(struct capn_segment*, int len);
cereal_Event_list cereal_new_Event_list(struct capn_segment*, int len);
void cereal_read_InitData(struct cereal_InitData*, cereal_InitData_ptr);
void cereal_read_FrameData(struct cereal_FrameData*, cereal_FrameData_ptr);
void cereal_read_GPSNMEAData(struct cereal_GPSNMEAData*, cereal_GPSNMEAData_ptr);
void cereal_read_SensorEventData(struct cereal_SensorEventData*, cereal_SensorEventData_ptr);
void cereal_read_SensorEventData_SensorVec(struct cereal_SensorEventData_SensorVec*, cereal_SensorEventData_SensorVec_ptr);
void cereal_read_CanData(struct cereal_CanData*, cereal_CanData_ptr);
void cereal_read_ThermalData(struct cereal_ThermalData*, cereal_ThermalData_ptr);
void cereal_read_HealthData(struct cereal_HealthData*, cereal_HealthData_ptr);
void cereal_read_LiveUI(struct cereal_LiveUI*, cereal_LiveUI_ptr);
void cereal_read_Live20Data(struct cereal_Live20Data*, cereal_Live20Data_ptr);
void cereal_read_Live20Data_LeadData(struct cereal_Live20Data_LeadData*, cereal_Live20Data_LeadData_ptr);
void cereal_read_LiveCalibrationData(struct cereal_LiveCalibrationData*, cereal_LiveCalibrationData_ptr);
void cereal_read_LiveTracks(struct cereal_LiveTracks*, cereal_LiveTracks_ptr);
void cereal_read_Live100Data(struct cereal_Live100Data*, cereal_Live100Data_ptr);
void cereal_read_LiveEventData(struct cereal_LiveEventData*, cereal_LiveEventData_ptr);
void cereal_read_ModelData(struct cereal_ModelData*, cereal_ModelData_ptr);
void cereal_read_ModelData_PathData(struct cereal_ModelData_PathData*, cereal_ModelData_PathData_ptr);
void cereal_read_ModelData_LeadData(struct cereal_ModelData_LeadData*, cereal_ModelData_LeadData_ptr);
void cereal_read_ModelData_ModelSettings(struct cereal_ModelData_ModelSettings*, cereal_ModelData_ModelSettings_ptr);
void cereal_read_CalibrationFeatures(struct cereal_CalibrationFeatures*, cereal_CalibrationFeatures_ptr);
void cereal_read_EncodeIndex(struct cereal_EncodeIndex*, cereal_EncodeIndex_ptr);
void cereal_read_AndroidLogEntry(struct cereal_AndroidLogEntry*, cereal_AndroidLogEntry_ptr);
void cereal_read_LogRotate(struct cereal_LogRotate*, cereal_LogRotate_ptr);
void cereal_read_Event(struct cereal_Event*, cereal_Event_ptr);
void cereal_write_InitData(const struct cereal_InitData*, cereal_InitData_ptr);
void cereal_write_FrameData(const struct cereal_FrameData*, cereal_FrameData_ptr);
void cereal_write_GPSNMEAData(const struct cereal_GPSNMEAData*, cereal_GPSNMEAData_ptr);
void cereal_write_SensorEventData(const struct cereal_SensorEventData*, cereal_SensorEventData_ptr);
void cereal_write_SensorEventData_SensorVec(const struct cereal_SensorEventData_SensorVec*, cereal_SensorEventData_SensorVec_ptr);
void cereal_write_CanData(const struct cereal_CanData*, cereal_CanData_ptr);
void cereal_write_ThermalData(const struct cereal_ThermalData*, cereal_ThermalData_ptr);
void cereal_write_HealthData(const struct cereal_HealthData*, cereal_HealthData_ptr);
void cereal_write_LiveUI(const struct cereal_LiveUI*, cereal_LiveUI_ptr);
void cereal_write_Live20Data(const struct cereal_Live20Data*, cereal_Live20Data_ptr);
void cereal_write_Live20Data_LeadData(const struct cereal_Live20Data_LeadData*, cereal_Live20Data_LeadData_ptr);
void cereal_write_LiveCalibrationData(const struct cereal_LiveCalibrationData*, cereal_LiveCalibrationData_ptr);
void cereal_write_LiveTracks(const struct cereal_LiveTracks*, cereal_LiveTracks_ptr);
void cereal_write_Live100Data(const struct cereal_Live100Data*, cereal_Live100Data_ptr);
void cereal_write_LiveEventData(const struct cereal_LiveEventData*, cereal_LiveEventData_ptr);
void cereal_write_ModelData(const struct cereal_ModelData*, cereal_ModelData_ptr);
void cereal_write_ModelData_PathData(const struct cereal_ModelData_PathData*, cereal_ModelData_PathData_ptr);
void cereal_write_ModelData_LeadData(const struct cereal_ModelData_LeadData*, cereal_ModelData_LeadData_ptr);
void cereal_write_ModelData_ModelSettings(const struct cereal_ModelData_ModelSettings*, cereal_ModelData_ModelSettings_ptr);
void cereal_write_CalibrationFeatures(const struct cereal_CalibrationFeatures*, cereal_CalibrationFeatures_ptr);
void cereal_write_EncodeIndex(const struct cereal_EncodeIndex*, cereal_EncodeIndex_ptr);
void cereal_write_AndroidLogEntry(const struct cereal_AndroidLogEntry*, cereal_AndroidLogEntry_ptr);
void cereal_write_LogRotate(const struct cereal_LogRotate*, cereal_LogRotate_ptr);
void cereal_write_Event(const struct cereal_Event*, cereal_Event_ptr);
void cereal_get_InitData(struct cereal_InitData*, cereal_InitData_list, int i);
void cereal_get_FrameData(struct cereal_FrameData*, cereal_FrameData_list, int i);
void cereal_get_GPSNMEAData(struct cereal_GPSNMEAData*, cereal_GPSNMEAData_list, int i);
void cereal_get_SensorEventData(struct cereal_SensorEventData*, cereal_SensorEventData_list, int i);
void cereal_get_SensorEventData_SensorVec(struct cereal_SensorEventData_SensorVec*, cereal_SensorEventData_SensorVec_list, int i);
void cereal_get_CanData(struct cereal_CanData*, cereal_CanData_list, int i);
void cereal_get_ThermalData(struct cereal_ThermalData*, cereal_ThermalData_list, int i);
void cereal_get_HealthData(struct cereal_HealthData*, cereal_HealthData_list, int i);
void cereal_get_LiveUI(struct cereal_LiveUI*, cereal_LiveUI_list, int i);
void cereal_get_Live20Data(struct cereal_Live20Data*, cereal_Live20Data_list, int i);
void cereal_get_Live20Data_LeadData(struct cereal_Live20Data_LeadData*, cereal_Live20Data_LeadData_list, int i);
void cereal_get_LiveCalibrationData(struct cereal_LiveCalibrationData*, cereal_LiveCalibrationData_list, int i);
void cereal_get_LiveTracks(struct cereal_LiveTracks*, cereal_LiveTracks_list, int i);
void cereal_get_Live100Data(struct cereal_Live100Data*, cereal_Live100Data_list, int i);
void cereal_get_LiveEventData(struct cereal_LiveEventData*, cereal_LiveEventData_list, int i);
void cereal_get_ModelData(struct cereal_ModelData*, cereal_ModelData_list, int i);
void cereal_get_ModelData_PathData(struct cereal_ModelData_PathData*, cereal_ModelData_PathData_list, int i);
void cereal_get_ModelData_LeadData(struct cereal_ModelData_LeadData*, cereal_ModelData_LeadData_list, int i);
void cereal_get_ModelData_ModelSettings(struct cereal_ModelData_ModelSettings*, cereal_ModelData_ModelSettings_list, int i);
void cereal_get_CalibrationFeatures(struct cereal_CalibrationFeatures*, cereal_CalibrationFeatures_list, int i);
void cereal_get_EncodeIndex(struct cereal_EncodeIndex*, cereal_EncodeIndex_list, int i);
void cereal_get_AndroidLogEntry(struct cereal_AndroidLogEntry*, cereal_AndroidLogEntry_list, int i);
void cereal_get_LogRotate(struct cereal_LogRotate*, cereal_LogRotate_list, int i);
void cereal_get_Event(struct cereal_Event*, cereal_Event_list, int i);
void cereal_set_InitData(const struct cereal_InitData*, cereal_InitData_list, int i);
void cereal_set_FrameData(const struct cereal_FrameData*, cereal_FrameData_list, int i);
void cereal_set_GPSNMEAData(const struct cereal_GPSNMEAData*, cereal_GPSNMEAData_list, int i);
void cereal_set_SensorEventData(const struct cereal_SensorEventData*, cereal_SensorEventData_list, int i);
void cereal_set_SensorEventData_SensorVec(const struct cereal_SensorEventData_SensorVec*, cereal_SensorEventData_SensorVec_list, int i);
void cereal_set_CanData(const struct cereal_CanData*, cereal_CanData_list, int i);
void cereal_set_ThermalData(const struct cereal_ThermalData*, cereal_ThermalData_list, int i);
void cereal_set_HealthData(const struct cereal_HealthData*, cereal_HealthData_list, int i);
void cereal_set_LiveUI(const struct cereal_LiveUI*, cereal_LiveUI_list, int i);
void cereal_set_Live20Data(const struct cereal_Live20Data*, cereal_Live20Data_list, int i);
void cereal_set_Live20Data_LeadData(const struct cereal_Live20Data_LeadData*, cereal_Live20Data_LeadData_list, int i);
void cereal_set_LiveCalibrationData(const struct cereal_LiveCalibrationData*, cereal_LiveCalibrationData_list, int i);
void cereal_set_LiveTracks(const struct cereal_LiveTracks*, cereal_LiveTracks_list, int i);
void cereal_set_Live100Data(const struct cereal_Live100Data*, cereal_Live100Data_list, int i);
void cereal_set_LiveEventData(const struct cereal_LiveEventData*, cereal_LiveEventData_list, int i);
void cereal_set_ModelData(const struct cereal_ModelData*, cereal_ModelData_list, int i);
void cereal_set_ModelData_PathData(const struct cereal_ModelData_PathData*, cereal_ModelData_PathData_list, int i);
void cereal_set_ModelData_LeadData(const struct cereal_ModelData_LeadData*, cereal_ModelData_LeadData_list, int i);
void cereal_set_ModelData_ModelSettings(const struct cereal_ModelData_ModelSettings*, cereal_ModelData_ModelSettings_list, int i);
void cereal_set_CalibrationFeatures(const struct cereal_CalibrationFeatures*, cereal_CalibrationFeatures_list, int i);
void cereal_set_EncodeIndex(const struct cereal_EncodeIndex*, cereal_EncodeIndex_list, int i);
void cereal_set_AndroidLogEntry(const struct cereal_AndroidLogEntry*, cereal_AndroidLogEntry_list, int i);
void cereal_set_LogRotate(const struct cereal_LogRotate*, cereal_LogRotate_list, int i);
void cereal_set_Event(const struct cereal_Event*, cereal_Event_list, int i);
#ifdef __cplusplus
}
#endif
#endif

3732
cereal/gen/cpp/log.capnp.c++
View File

File diff suppressed because it is too large Load Diff

7251
cereal/gen/cpp/log.capnp.h
View File

File diff suppressed because it is too large Load Diff

272
cereal/log.capnp
Unescape View File

@ -0,0 +1,272 @@
using Cxx = import "c++.capnp";
$Cxx.namespace("cereal");
@0xf3b1f17e25a4285b;
const logVersion :Int32 = 1;
struct InitData {
kernelArgs @0 :List(Text);
gctx @1 :Text;
dongleId @2 :Text;
}
struct FrameData {
frameId @0 :UInt32;
encodeId @1 :UInt32; # DEPRECATED
timestampEof @2 :UInt64;
frameLength @3 :Int32;
integLines @4 :Int32;
globalGain @5 :Int32;
image @6 :Data;
}
struct GPSNMEAData {
timestamp @0 :Int64;
localWallTime @1 :UInt64;
nmea @2 :Text;
}
# android sensor_event_t
struct SensorEventData {
version @0 :Int32;
sensor @1 :Int32;
type @2 :Int32;
timestamp @3 :Int64;
union {
acceleration @4 :SensorVec;
magnetic @5 :SensorVec;
orientation @6 :SensorVec;
gyro @7 :SensorVec;
}
struct SensorVec {
v @0 :List(Float32);
status @1 :Int8;
}
}
struct CanData {
address @0 :UInt32;
busTime @1 :UInt16;
dat @2 :Data;
src @3 :Int8;
}
struct ThermalData {
cpu0 @0 :UInt16;
cpu1 @1 :UInt16;
cpu2 @2 :UInt16;
cpu3 @3 :UInt16;
mem @4 :UInt16;
gpu @5 :UInt16;
bat @6 :UInt32;
}
struct HealthData {
# from can health
voltage @0 :UInt32;
current @1 :UInt32;
started @2 :Bool;
controlsAllowed @3 :Bool;
gasInterceptorDetected @4 :Bool;
}
struct LiveUI {
rearViewCam @0 :Bool;
alertText1 @1 :Text;
alertText2 @2 :Text;
awarenessStatus @3 :Float32;
}
struct Live20Data {
canMonoTimes @10 :List(UInt64);
mdMonoTime @6 :UInt64;
ftMonoTime @7 :UInt64;
# all deprecated
warpMatrix @0 :List(Float32);
angleOffset @1 :Float32;
calStatus @2 :Int8;
calCycle @8 :Int32;
calPerc @9 :Int8;
leadOne @3 :LeadData;
leadTwo @4 :LeadData;
cumLagMs @5 :Float32;
struct LeadData {
dRel @0 :Float32;
yRel @1 :Float32;
vRel @2 :Float32;
aRel @3 :Float32;
vLead @4 :Float32;
aLead @5 :Float32;
dPath @6 :Float32;
vLat @7 :Float32;
vLeadK @8 :Float32;
aLeadK @9 :Float32;
fcw @10 :Bool;
status @11 :Bool;
}
}
struct LiveCalibrationData {
warpMatrix @0 :List(Float32);
calStatus @1 :Int8;
calCycle @2 :Int32;
calPerc @3 :Int8;
}
struct LiveTracks {
trackId @0 :Int32;
dRel @1 :Float32;
yRel @2 :Float32;
vRel @3 :Float32;
aRel @4 :Float32;
timeStamp @5 :Float32;
status @6 :Float32;
currentTime @7 :Float32;
stationary @8 :Bool;
oncoming @9 :Bool;
}
struct Live100Data {
canMonoTime @16 :UInt64;
canMonoTimes @21 :List(UInt64);
l20MonoTime @17 :UInt64;
mdMonoTime @18 :UInt64;
vEgo @0 :Float32;
aEgo @1 :Float32;
vPid @2 :Float32;
vTargetLead @3 :Float32;
upAccelCmd @4 :Float32;
uiAccelCmd @5 :Float32;
yActual @6 :Float32;
yDes @7 :Float32;
upSteer @8 :Float32;
uiSteer @9 :Float32;
aTargetMin @10 :Float32;
aTargetMax @11 :Float32;
jerkFactor @12 :Float32;
angleSteers @13 :Float32;
hudLead @14 :Int32;
cumLagMs @15 :Float32;
enabled @19: Bool;
steerOverride @20: Bool;
vCruise @22: Float32;
rearViewCam @23 :Bool;
alertText1 @24 :Text;
alertText2 @25 :Text;
awarenessStatus @26 :Float32;
}
struct LiveEventData {
name @0 :Text;
value @1 :Int32;
}
struct ModelData {
frameId @0 :UInt32;
path @1 :PathData;
leftLane @2 :PathData;
rightLane @3 :PathData;
lead @4 :LeadData;
settings @5 :ModelSettings;
struct PathData {
points @0 :List(Float32);
prob @1 :Float32;
std @2 :Float32;
}
struct LeadData {
dist @0 :Float32;
prob @1 :Float32;
std @2 :Float32;
}
struct ModelSettings {
bigBoxX @0 :UInt16;
bigBoxY @1 :UInt16;
bigBoxWidth @2 :UInt16;
bigBoxHeight @3 :UInt16;
boxProjection @4 :List(Float32);
yuvCorrection @5 :List(Float32);
}
}
struct CalibrationFeatures {
frameId @0 :UInt32;
p0 @1 :List(Float32);
p1 @2 :List(Float32);
status @3 :List(Int8);
}
struct EncodeIndex {
# picture from camera
frameId @0 :UInt32;
type @1 :Type;
# index of encoder from start of route
encodeId @2 :UInt32;
# minute long segment this frame is in
segmentNum @3 :Int32;
# index into camera file in segment from 0
segmentId @4 :UInt32;
enum Type {
bigBoxLossless @0; # rcamera.mkv
fullHEVC @1; # fcamera.hevc
bigBoxHEVC @2; # bcamera.hevc
}
}
struct AndroidLogEntry {
id @0 :UInt8;
ts @1 :UInt64;
priority @2 :UInt8;
pid @3 :Int32;
tid @4 :Int32;
tag @5 :Text;
message @6 :Text;
}
struct LogRotate {
segmentNum @0 :Int32;
path @1 :Text;
}
struct Event {
logMonoTime @0 :UInt64;
union {
initData @1 :InitData;
frame @2 :FrameData;
gpsNMEA @3 :GPSNMEAData;
sensorEventDEPRECATED @4 :SensorEventData;
can @5 :List(CanData);
thermal @6 :ThermalData;
live100 @7 :Live100Data;
liveEventDEPRECATED @8 :List(LiveEventData);
model @9 :ModelData;
features @10 :CalibrationFeatures;
sensorEvents @11 :List(SensorEventData);
health @12 : HealthData;
live20 @13 :Live20Data;
liveUIDEPRECATED @14 :LiveUI;
encodeIdx @15 :EncodeIndex;
liveTracks @16 :List(LiveTracks);
sendcan @17 :List(CanData);
logMessage @18 :Text;
liveCalibration @19 :LiveCalibrationData;
androidLogEntry @20 :AndroidLogEntry;
}
}

0
common/__init__.py
Unescape View File

8
common/api/__init__.py
Unescape View File

@ -0,0 +1,8 @@
import requests
def api_get(endpoint, method='GET', timeout=None, **params):
backend = "https://api.commadotai.com/"
params['_version'] = "OPENPILOTv0.0"
return requests.request(method, backend+endpoint, timeout=timeout, params=params)

26
common/crash.py
Unescape View File

@ -0,0 +1,26 @@
"""Install exception handler for process crash."""
import os
import sys
if os.getenv("NOLOG"):
def capture_exception(*exc_info):
pass
def install():
pass
else:
from raven import Client
from raven.transport.http import HTTPTransport
client = Client('https://1994756b5e6f41cf939a4c65de45f4f2:cefebaf3a8aa40d182609785f7189bd7@app.getsentry.com/77924',
install_sys_hook=False, transport=HTTPTransport)
capture_exception = client.captureException
def install():
# installs a sys.excepthook
__excepthook__ = sys.excepthook
def handle_exception(*exc_info):
if exc_info[0] not in (KeyboardInterrupt, SystemExit):
client.captureException(exc_info=exc_info)
__excepthook__(*exc_info)
sys.excepthook = handle_exception

182
common/dbc.py
Unescape View File

@ -0,0 +1,182 @@
import re
from collections import namedtuple
import bitstring
def int_or_float(s):
# return number, trying to maintain int format
try:
return int(s)
except ValueError:
return float(s)
DBCSignal = namedtuple(
"DBCSignal", ["name", "start_bit", "size", "is_little_endian", "is_signed",
"factor", "offset", "tmin", "tmax", "units"])
class dbc(object):
def __init__(self, fn):
self.txt = open(fn).read().split("\n")
self._warned_addresses = set()
# regexps from https://github.com/ebroecker/canmatrix/blob/master/canmatrix/importdbc.py
bo_regexp = re.compile("^BO\_ (\w+) (\w+) *: (\w+) (\w+)")
sg_regexp = re.compile("^SG\_ (\w+) : (\d+)\|(\d+)@(\d+)([\+|\-]) \(([0-9.+\-eE]+),([0-9.+\-eE]+)\) \[([0-9.+\-eE]+)\|([0-9.+\-eE]+)\] \"(.*)\" (.*)")
sgm_regexp = re.compile("^SG\_ (\w+) (\w+) *: (\d+)\|(\d+)@(\d+)([\+|\-]) \(([0-9.+\-eE]+),([0-9.+\-eE]+)\) \[([0-9.+\-eE]+)\|([0-9.+\-eE]+)\] \"(.*)\" (.*)")
# A dictionary which maps message ids to tuples ((name, size), signals).
# name is the ASCII name of the message.
# size is the size of the message in bytes.
# signals is a list signals contained in the message.
# signals is a list of DBCSignal in order of increasing start_bit.
self.msgs = {}
self.bits = []
for i in range(0, 64, 8):
for j in range(7, -1, -1):
self.bits.append(i+j)
for l in self.txt:
l = l.strip()
if l.startswith("BO_ "):
# new group
dat = bo_regexp.match(l)
if dat is None:
print "bad BO", l
name = dat.group(2)
size = int(dat.group(3))
ids = int(dat.group(1), 0) # could be hex
self.msgs[ids] = ((name, size), [])
if l.startswith("SG_ "):
# new signal
dat = sg_regexp.match(l)
go = 0
if dat is None:
dat = sgm_regexp.match(l)
go = 1
if dat is None:
print "bad SG", l
sgname = dat.group(1)
start_bit = int(dat.group(go+2))
signal_size = int(dat.group(go+3))
is_little_endian = int(dat.group(go+4))==1
is_signed = dat.group(go+5)=='-'
factor = int_or_float(dat.group(go+6))
offset = int_or_float(dat.group(go+7))
tmin = int_or_float(dat.group(go+8))
tmax = int_or_float(dat.group(go+9))
units = dat.group(go+10)
self.msgs[ids][1].append(
DBCSignal(sgname, start_bit, signal_size, is_little_endian,
is_signed, factor, offset, tmin, tmax, units))
for msg in self.msgs.viewvalues():
msg[1].sort(key=lambda x: x.start_bit)
def encode(self, msg_id, dd):
"""Encode a CAN message using the dbc.
Inputs:
msg_id: The message ID.
dd: A dictionary mapping signal name to signal data.
"""
# TODO: Stop using bitstring, which is super slow.
msg_def = self.msgs[msg_id]
size = msg_def[0][1]
bsf = bitstring.Bits(hex="00"*size)
for s in msg_def[1]:
ival = dd.get(s.name)
if ival is not None:
ival = (ival / s.factor) - s.offset
ival = int(round(ival))
# should pack this
if s.is_little_endian:
ss = s.start_bit
else:
ss = self.bits.index(s.start_bit)
if s.is_signed:
tbs = bitstring.Bits(int=ival, length=s.size)
else:
tbs = bitstring.Bits(uint=ival, length=s.size)
lpad = bitstring.Bits(bin="0b"+"0"*ss)
rpad = bitstring.Bits(bin="0b"+"0"*(8*size-(ss+s.size)))
tbs = lpad+tbs+rpad
bsf |= tbs
return bsf.tobytes()
def decode(self, x, arr=None, debug=False):
"""Decode a CAN message using the dbc.
Inputs:
x: A collection with elements (address, time, data), where address is
the CAN address, time is the bus time, and data is the CAN data as a
hex string.
arr: Optional list of signals which should be decoded and returned.
debug: True to print debugging statements.
Returns:
A tuple (name, data), where name is the name of the CAN message and data
is the decoded result. If arr is None, data is a dict of properties.
Otherwise data is a list of the same length as arr.
Returns (None, None) if the message could not be decoded.
"""
if arr is None:
out = {}
else:
out = [None]*len(arr)
msg = self.msgs.get(x[0])
if msg is None:
if x[0] not in self._warned_addresses:
print("WARNING: Unknown message address {}".format(x[0]))
self._warned_addresses.add(x[0])
return None, None
name = msg[0][0]
if debug:
print name
blen = (len(x[2])/2)*8
x2_int = int(x[2], 16)
for s in msg[1]:
if arr is not None and s[0] not in arr:
continue
# big or little endian?
# see http://vi-firmware.openxcplatform.com/en/master/config/bit-numbering.html
if s[3] is False:
ss = self.bits.index(s[1])
else:
ss = s[1]
data_bit_pos = (blen - (ss + s[2]))
if data_bit_pos < 0:
continue
ival = (x2_int >> data_bit_pos) & ((1 << (s[2])) - 1)
if s[4] and (ival & (1<<(s[2]-1))): # signed
ival -= (1<<s[2])
# control the offset
ival = (ival + s[6])*s[5]
if debug:
print "%40s %2d %2d %7.2f %s" % (s[0], s[1], s[2], ival, s[-1])
if arr is None:
out[s[0]] = ival
else:
out[arr.index(s[0])] = ival
return name, out

17
common/filters.py
Unescape View File

@ -0,0 +1,17 @@
"""Classes for filtering discrete time signals."""
import numpy as np
class FirstOrderLowpassFilter(object):
def __init__(self, fc, dt, x1=0):
self.kf = 2 * np.pi * fc * dt / (1 + 2 * np.pi * fc * dt)
self.x1 = x1
def __call__(self, x):
self.x1 = (1 - self.kf) * self.x1 + self.kf * x
# If previous or current is NaN, reset filter.
if np.isnan(self.x1):
self.x1 = x
return self.x1

0
common/kalman/__init__.py
Unescape View File

300
common/kalman/ekf.py
Unescape View File

@ -0,0 +1,300 @@
import abc
import numpy as np
import numpy.matlib
# The EKF class contains the framework for an Extended Kalman Filter, but must be subclassed to use.
# A subclass must implement:
# 1) calc_transfer_fun(); see bottom of file for more info.
# 2) __init__() to initialize self.state, self.covar, and self.process_noise appropriately
# Alternatively, the existing implementations of EKF can be used (e.g. EKF2D)
# Sensor classes are optionally used to pass measurement information into the EKF, to keep
# sensor parameters and processing methods for a each sensor together.
# Sensor classes have a read() method which takes raw sensor data and returns
# a SensorReading object, which can be passed to the EKF update() method.
# For usage, see run_ekf1d.py in selfdrive/new for a simple example.
# ekf.predict(dt) should be called between update cycles with the time since it was last called.
# Ideally, predict(dt) should be called at a relatively constant rate.
# update() should be called once per sensor, and can be called multiple times between predict steps.
# Access and set the state of the filter directly with ekf.state and ekf.covar.
class SensorReading:
# Given a perfect model and no noise, data = obs_model * state
def __init__(self, data, covar, obs_model):
self.data = data
self.obs_model = obs_model
self.covar = covar
def __repr__(self):
return "SensorReading(data={}, covar={}, obs_model={})".format(
repr(self.data), repr(self.covar), repr(self.obs_model))
# A generic sensor class that does no pre-processing of data
class SimpleSensor:
# obs_model can be
# a full obesrvation model matrix, or
# an integer or tuple of indices into ekf.state, indicating which variables are being directly observed
# covar can be
# a full covariance matrix
# a float or tuple of individual covars for each component of the sensor reading
# dims is the number of states in the EKF
def __init__(self, obs_model, covar, dims):
# Allow for integer covar/obs_model
if not hasattr(obs_model, "__len__"):
obs_model = (obs_model, )
if not hasattr(covar, "__len__"):
covar = (covar, )
# Full observation model passed
if dims in np.array(obs_model).shape:
self.obs_model = np.asmatrix(obs_model)
self.covar = np.asmatrix(covar)
# Indices of unit observations passed
else:
self.obs_model = np.matlib.zeros((len(obs_model), dims))
self.obs_model[:, list(obs_model)] = np.identity(len(obs_model))
if np.asarray(covar).ndim == 2:
self.covar = np.asmatrix(covar)
elif len(covar) == len(obs_model):
self.covar = np.matlib.diag(covar)
else:
self.covar = np.matlib.identity(len(obs_model)) * covar
def read(self, data, covar=None):
if covar:
self.covar = covar
return SensorReading(data, self.covar, self.obs_model)
class GPS:
earth_r = 6371e3 # m, average earth radius
def __init__(self, xy_idx=(0, 1), dims=2, var=1e4):
self.obs_model = np.matlib.zeros((2, dims))
self.obs_model[:, tuple(xy_idx)] = np.matlib.identity(2)
self.covar = np.matlib.identity(2) * var
# [lat, lon] in decimal degrees
def init_pos(self, latlon):
self.init_lat, self.init_lon = np.radians(np.asarray(latlon[:2]))
# Compute straight-line distance, in meters, between two lat/long coordinates
# Input in radians
def haversine(self, lat1, lon1, lat2, lon2):
lat_diff = lat2 - lat1
lon_diff = lon2 - lon1
d = np.sin(lat_diff * 0.5)**2 + np.cos(lat1) * np.cos(lat2) * np.sin(
lon_diff * 0.5)**2
h = 2 * GPS.earth_r * np.arcsin(np.sqrt(d))
return h
# Convert decimal degrees into meters
def convert_deg2m(self, lat, lon):
lat, lon = np.radians([lat, lon])
xs = (lon - self.init_lon) * np.cos(self.init_lat) * GPS.earth_r
ys = (lat - self.init_lat) * GPS.earth_r
return xs, ys
# Convert meters into decimal degrees
def convert_m2deg(self, xs, ys):
lat = ys / GPS.earth_r + self.init_lat
lon = xs / (GPS.earth_r * np.cos(self.init_lat)) + self.init_lon
return np.degrees(lat), np.degrees(lon)
# latlon is [lat, long,] as decimal degrees
# accuracy is as given by Android location service: radius of 68% confidence
def read(self, latlon, accuracy=None):
x_dist, y_dist = self.convert_deg2m(latlon[0], latlon[1])
if not accuracy:
covar = self.covar
else:
covar = np.matlib.identity(2) * accuracy**2
return SensorReading(
np.asmatrix([x_dist, y_dist]).T, covar, self.obs_model)
class EKF:
__metaclass__ = abc.ABCMeta
def __init__(self, debug=False):
self.DEBUG = debug
def __str__(self):
return "EKF(state={}, covar={})".format(self.state, self.covar)
# Measurement update
# Reading should be a SensorReading object with data, covar, and obs_model attributes
def update(self, reading):
# Potential improvements:
# deal with negative covars
# add noise to really low covars to ensure stability
# use mahalanobis distance to reject outliers
# wrap angles after state updates and innovation
# y = z - H*x
innovation = reading.data - reading.obs_model * self.state
if self.DEBUG:
print "reading:\n",reading.data
print "innovation:\n",innovation
# S = H*P*H' + R
innovation_covar = reading.obs_model * self.covar * reading.obs_model.T + reading.covar
# K = P*H'*S^-1
kalman_gain = self.covar * reading.obs_model.T * np.linalg.inv(
innovation_covar)
if self.DEBUG:
print "gain:\n", kalman_gain
print "innovation_covar:\n", innovation_covar
print "innovation: ", innovation
print "test: ", self.covar * reading.obs_model.T * (
reading.obs_model * self.covar * reading.obs_model.T + reading.covar *
0).I
# x = x + K*y
self.state += kalman_gain*innovation
# print "covar", np.diag(self.covar)
#self.state[(roll_vel, yaw_vel, pitch_vel),:] = reading.data
# Standard form: P = (I - K*H)*P
# self.covar = (self.identity - kalman_gain*reading.obs_model) * self.covar
# Use the Joseph form for numerical stability: P = (I-K*H)*P*(I - K*H)' + K*R*K'
aux_mtrx = (self.identity - kalman_gain * reading.obs_model)
self.covar = aux_mtrx * self.covar * aux_mtrx.T + kalman_gain * reading.covar * kalman_gain.T
if self.DEBUG:
print "After update"
print "state\n", self.state
print "covar:\n",self.covar
def update_scalar(self, reading):
# like update but knowing that measurment is a scalar
# this avoids matrix inversions and speeds up (surprisingly) drived.py a lot
# innovation = reading.data - np.matmul(reading.obs_model, self.state)
# innovation_covar = np.matmul(np.matmul(reading.obs_model, self.covar), reading.obs_model.T) + reading.covar
# kalman_gain = np.matmul(self.covar, reading.obs_model.T)/innovation_covar
# self.state += np.matmul(kalman_gain, innovation)
# aux_mtrx = self.identity - np.matmul(kalman_gain, reading.obs_model)
# self.covar = np.matmul(aux_mtrx, np.matmul(self.covar, aux_mtrx.T)) + np.matmul(kalman_gain, np.matmul(reading.covar, kalman_gain.T))
# written without np.matmul
es = np.einsum
ABC_T = "ij,jk,lk->il"
AB_T = "ij,kj->ik"
AB = "ij,jk->ik"
innovation = reading.data - es(AB, reading.obs_model, self.state)
innovation_covar = es(ABC_T, reading.obs_model, self.covar,
reading.obs_model) + reading.covar
kalman_gain = es(AB_T, self.covar, reading.obs_model) / innovation_covar
self.state += es(AB, kalman_gain, innovation)
aux_mtrx = self.identity - es(AB, kalman_gain, reading.obs_model)
self.covar = es(ABC_T, aux_mtrx, self.covar, aux_mtrx) + \
es(ABC_T, kalman_gain, reading.covar, kalman_gain)
# Prediction update
def predict(self, dt):
es = np.einsum
ABC_T = "ij,jk,lk->il"
AB = "ij,jk->ik"
# State update
transfer_fun, transfer_fun_jacobian = self.calc_transfer_fun(dt)
# self.state = np.matmul(transfer_fun, self.state)
# self.covar = np.matmul(np.matmul(transfer_fun_jacobian, self.covar), transfer_fun_jacobian.T) + self.process_noise * dt
# x = f(x, u), written in the form x = A(x, u)*x
self.state = es(AB, transfer_fun, self.state)
# P = J*P*J' + Q
self.covar = es(ABC_T, transfer_fun_jacobian, self.covar,
transfer_fun_jacobian) + self.process_noise * dt #!dt
#! Clip covariance to avoid explosions
self.covar = np.clip(self.covar,-1e10,1e10)
@abc.abstractmethod
def calc_transfer_fun(self, dt):
"""Return a tuple with the transfer function and transfer function jacobian
The transfer function and jacobian should both be a numpy matrix of size DIMSxDIMS
The transfer function matrix A should satisfy the state-update equation
x_(k+1) = A * x_k
The jacobian J is the direct jacobian A*x_k. For linear systems J=A.
Current implementations calculate A and J as functions of state. Control input
can be added trivially by adding a control parameter to predict() and calc_tranfer_update(),
and using it during calcualtion of A and J
"""
class FastEKF1D(EKF):
"""Fast version of EKF for 1D problems with scalar readings."""
def __init__(self, dt, var_init, Q):
super(FastEKF1D, self).__init__(False)
self.state = [0, 0]
self.covar = [var_init, var_init, 0]
# Process Noise
self.dtQ0 = dt * Q[0]
self.dtQ1 = dt * Q[1]
def update(self, reading):
raise NotImplementedError
def update_scalar(self, reading):
# TODO(mgraczyk): Delete this for speed.
# assert np.all(reading.obs_model == [1, 0])
rcov = reading.covar[0, 0]
x = self.state
S = self.covar
innovation = reading.data - x[0]
innovation_covar = S[0] + rcov
k0 = S[0] / innovation_covar
k1 = S[2] / innovation_covar
x[0] += k0 * innovation
x[1] += k1 * innovation
mk = 1 - k0
S[1] += k1 * (k1 * (S[0] + rcov) - 2 * S[2])
S[2] = mk * (S[2] - k1 * S[0]) + rcov * k0 * k1
S[0] = mk * mk * S[0] + rcov * k0 * k0
def predict(self, dt):
# State update
x = self.state
x[0] += dt * x[1]
# P = J*P*J' + Q
S = self.covar
S[0] += dt * (2 * S[2] + dt * S[1]) + self.dtQ0
S[2] += dt * S[1]
S[1] += self.dtQ1
# Clip covariance to avoid explosions
S = max(-1e10, min(S, 1e10))
def calc_transfer_fun(self, dt):
tf = np.identity(2)
tf[0, 1] = dt
tfj = tf
return tf, tfj

134
common/logging_extra.py
Unescape View File

@ -0,0 +1,134 @@
import os
import sys
import copy
import json
import socket
import logging
from threading import local
from collections import OrderedDict
from contextlib import contextmanager
class SwagFormatter(logging.Formatter):
def __init__(self, swaglogger):
logging.Formatter.__init__(self, None, '%a %b %d %H:%M:%S %Z %Y')
self.swaglogger = swaglogger
self.host = socket.gethostname()
def json_handler(self, obj):
# if isinstance(obj, (datetime.date, datetime.time)):
# return obj.isoformat()
return repr(obj)
def format(self, record):
record_dict = OrderedDict()
if isinstance(record.msg, dict):
record_dict['msg'] = record.msg
else:
try:
record_dict['msg'] = record.getMessage()
except (ValueError, TypeError):
record_dict['msg'] = [record.msg]+record.args
record_dict['ctx'] = self.swaglogger.get_ctx()
if record.exc_info:
record_dict['exc_info'] = self.formatException(record.exc_info)
record_dict['level'] = record.levelname
record_dict['levelnum'] = record.levelno
record_dict['name'] = record.name
record_dict['filename'] = record.filename
record_dict['lineno'] = record.lineno
record_dict['pathname'] = record.pathname
record_dict['module'] = record.module
record_dict['funcName'] = record.funcName
record_dict['host'] = self.host
record_dict['process'] = record.process
record_dict['thread'] = record.thread
record_dict['threadName'] = record.threadName
record_dict['created'] = record.created
# asctime = self.formatTime(record, self.datefmt)
return json.dumps(record_dict, default=self.json_handler)
_tmpfunc = lambda: 0
_srcfile = os.path.normcase(_tmpfunc.__code__.co_filename)
class SwagLogger(logging.Logger):
def __init__(self):
logging.Logger.__init__(self, "swaglog")
self.global_ctx = {}
self.log_local = local()
self.log_local.ctx = {}
def findCaller(self):
"""
Find the stack frame of the caller so that we can note the source
file name, line number and function name.
"""
# f = currentframe()
f = sys._getframe(3)
#On some versions of IronPython, currentframe() returns None if
#IronPython isn't run with -X:Frames.
if f is not None:
f = f.f_back
rv = "(unknown file)", 0, "(unknown function)"
while hasattr(f, "f_code"):
co = f.f_code
filename = os.path.normcase(co.co_filename)
if filename in (logging._srcfile, _srcfile):
f = f.f_back
continue
rv = (co.co_filename, f.f_lineno, co.co_name)
break
return rv
def local_ctx(self):
try:
return self.log_local.ctx
except AttributeError:
self.log_local.ctx = {}
return self.log_local.ctx
def get_ctx(self):
return dict(self.local_ctx(), **self.global_ctx)
@contextmanager
def ctx(self, **kwargs):
old_ctx = self.local_ctx()
self.log_local.ctx = copy.copy(old_ctx) or {}
self.log_local.ctx.update(kwargs)
try:
yield
finally:
self.log_local.ctx = old_ctx
def bind(self, **kwargs):
self.local_ctx().update(kwargs)
def bind_global(self, **kwargs):
self.global_ctx.update(kwargs)
def event(self, event_name, *args, **kwargs):
evt = OrderedDict()
evt['event'] = event_name
if args:
evt['args'] = args
evt.update(kwargs)
self.info(evt)
if __name__ == "__main__":
log = SwagLogger()
log.info("asdasd %s", "a")
log.info({'wut': 1})
with log.ctx():
log.bind(user="some user")
log.info("in req")
log.event("do_req")

2
common/numpy_fast.py
Unescape View File

@ -0,0 +1,2 @@
def clip(x, lo, hi):
return max(lo, min(hi, x))

94
common/realtime.py
Unescape View File

@ -0,0 +1,94 @@
"""Utilities for reading real time clocks and keeping soft real time constraints."""
import time
import ctypes
import platform
import subprocess
import multiprocessing
import os
CLOCK_MONOTONIC_RAW = 4 # see <linux/time.h>
CLOCK_BOOTTIME = 7
class timespec(ctypes.Structure):
_fields_ = [
('tv_sec', ctypes.c_long),
('tv_nsec', ctypes.c_long),
]
try:
libc = ctypes.CDLL('libc.so', use_errno=True)
except OSError:
try:
libc = ctypes.CDLL('libc.so.6', use_errno=True)
except OSError:
libc = None
if libc is not None:
libc.clock_gettime.argtypes = [ctypes.c_int, ctypes.POINTER(timespec)]
def clock_gettime(clk_id):
if platform.system() == "darwin":
# TODO: fix this
return time.time()
else:
t = timespec()
if libc.clock_gettime(clk_id, ctypes.pointer(t)) != 0:
errno_ = ctypes.get_errno()
raise OSError(errno_, os.strerror(errno_))
return t.tv_sec + t.tv_nsec * 1e-9
def monotonic_time():
return clock_gettime(CLOCK_MONOTONIC_RAW)
def sec_since_boot():
return clock_gettime(CLOCK_BOOTTIME)
def set_realtime_priority(level):
if os.getuid() != 0:
print "not setting priority, not root"
return
if platform.machine() == "x86_64":
NR_gettid = 186
elif platform.machine() == "aarch64":
NR_gettid = 178
else:
raise NotImplementedError
tid = libc.syscall(NR_gettid)
subprocess.check_call(['chrt', '-f', '-p', str(level), str(tid)])
class Ratekeeper(object):
def __init__(self, rate, print_delay_threshold=0.):
"""Rate in Hz for ratekeeping. print_delay_threshold must be nonnegative."""
self._interval = 1. / rate
self._next_frame_time = sec_since_boot() + self._interval
self._print_delay_threshold = print_delay_threshold
self._frame = 0
self._remaining = 0
self._process_name = multiprocessing.current_process().name
@property
def frame(self):
return self._frame
@property
def remaining(self):
return self._remaining
# Maintain loop rate by calling this at the end of each loop
def keep_time(self):
self.monitor_time()
if self._remaining > 0:
time.sleep(self._remaining)
# this only monitor the cumulative lag, but does not enforce a rate
def monitor_time(self):
remaining = self._next_frame_time - sec_since_boot()
self._next_frame_time += self._interval
if remaining < -self._print_delay_threshold:
print self._process_name, "lagging by", round(-remaining * 1000, 2), "ms"
self._frame += 1
self._remaining = remaining

82
common/services.py
Unescape View File

@ -0,0 +1,82 @@
# TODO: these port numbers are hardcoded in c, fix this
# LogRotate: 8001 is a PUSH PULL socket between loggerd and visiond
class Service(object):
def __init__(self, port, should_log):
self.port = port
self.should_log = should_log
# all ZMQ pub sub
service_list = {
# frame syncing packet
"frame": Service(8002, True),
# accel, gyro, and compass
"sensorEvents": Service(8003, True),
# GPS data, also global timestamp
"gpsNMEA": Service(8004, True),
# CPU+MEM+GPU+BAT temps
"thermal": Service(8005, True),
# List(CanData), list of can messages
"can": Service(8006, True),
"live100": Service(8007, True),
# random events we want to log
#"liveEvent": Service(8008, True),
"model": Service(8009, True),
"features": Service(8010, True),
"health": Service(8011, True),
"live20": Service(8012, True),
#"liveUI": Service(8014, True),
"encodeIdx": Service(8015, True),
"liveTracks": Service(8016, True),
"sendcan": Service(8017, True),
"logMessage": Service(8018, True),
"liveCalibration": Service(8019, True),
"androidLog": Service(8020, True),
}
# manager -- base process to manage starting and stopping of all others
# subscribes: health
# publishes: thermal
# boardd -- communicates with the car
# subscribes: sendcan
# publishes: can, health
# visiond -- talks to the cameras, runs the model, saves the videos
# subscribes: liveCalibration, sensorEvents
# publishes: frame, encodeIdx, model, features
# controlsd -- actually drives the car
# subscribes: can, thermal, model, live20
# publishes: sendcan, live100
# radard -- processes the radar data
# subscribes: can, live100, model
# publishes: live20, liveTracks
# sensord -- publishes the IMU and GPS
# publishes: sensorEvents, gpsNMEA
# calibrationd -- places the camera box
# subscribes: features, live100
# publishes: liveCalibration
# **** LOGGING SERVICE ****
# loggerd
# subscribes: EVERYTHING
# **** NON VITAL SERVICES ****
# ui
# subscribes: live100, live20, liveCalibration, model, (raw frames)
# uploader
# communicates through file system with loggerd
# logmessaged -- central logging service, can log to cloud
# publishes: logMessage
# logcatd -- fetches logcat info from android
# publishes: androidLog

2
dbcs/__init__.py
Unescape View File

@ -0,0 +1,2 @@
import os
DBC_PATH = os.path.dirname(os.path.abspath(__file__))

295
dbcs/acura_ilx_2016_can.dbc
Unescape View File

@ -0,0 +1,295 @@
VERSION ""
NS_ :
NS_DESC_
CM_
BA_DEF_
BA_
VAL_
CAT_DEF_
CAT_
FILTER
BA_DEF_DEF_
EV_DATA_
ENVVAR_DATA_
SGTYPE_
SGTYPE_VAL_
BA_DEF_SGTYPE_
BA_SGTYPE_
SIG_TYPE_REF_
VAL_TABLE_
SIG_GROUP_
SIG_VALTYPE_
SIGTYPE_VALTYPE_
BO_TX_BU_
BA_DEF_REL_
BA_REL_
BA_DEF_DEF_REL_
BU_SG_REL_
BU_EV_REL_
BU_BO_REL_
SG_MUL_VAL_
BS_:
BO_ 0x039 XXX: 3 XXX
BO_ 0x091 XXX: 8 XXX
SG_ LAT_ACCEL : 0|10@1+ (0.02,-512) [-20|20] "m/s2" Vector__XXX
BO_ 0x0E4 STEERING_CONTROL: 5 ADAS
SG_ STEER_TORQUE : 0|16@1- (1,0) [-3840|3840] "" Vector__XXX
SG_ STEER_TORQUE_REQUEST : 16|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ SET_ME_X00 : 17|7@1+ (1,0) [0|127] "" Vector__XXX
SG_ SET_ME_X00 : 24|8@1+ (1,0) [0|0] "" Vector__XXX
SG_ COUNTER : 34|2@1+ (1,0) [0|3] "" Vector__XXX
SG_ CHECKSUM : 36|4@1+ (1,0) [0|3] "" Vector__XXX
BO_ 0x130 GAS_PEDAL2: 8 PCM
SG_ ENGINE_TORQUE_ESTIMATE : 0|16@1- (1,0) [-1000|1000] "Nm" Vector__XXX
SG_ ENGINE_TORQUE_REQUEST : 16|16@1- (1,0) [-1000|1000] "Nm" Vector__XXX
SG_ CAR_GAS : 32|8@1+ (1,0) [0|255] "" Vector__XXX
BO_ 0x13C GAS_PEDAL: 8 PCM
SG_ CAR_GAS : 32|8@1+ (1,0) [0|255] "" Vector__XXX
BO_ 0x156 STEERING_SENSORS: 6 EPS
SG_ STEER_ANGLE : 0|16@1- (-0.1,0) [-500|500] "deg" Vector__XXX
SG_ STEER_ANGLE_RATE : 16|16@1- (1,0) [-3000|3000] "deg/s" Vector__XXX
SG_ COUNTER : 42|2@1+ (1,0) [0|3] "" Vector__XXX
SG_ CHECKSUM : 44|4@1+ (1,0) [0|3] "" Vector__XXX
BO_ 0x158 POWERTRAIN_DATA: 8 PCM
SG_ XMISSION_SPEED : 0|16@1+ (0.002759506,0) [0|70] "m/s" Vector__XXX
SG_ ENGINE_RPM : 16|16@1+ (1,0) [0|15000] "rpm" Vector__XXX
SG_ XMISSION_SPEED2 : 32|16@1+ (0.002759506,0) [0|70] "m/s" Vector__XXX
SG_ COUNTER : 58|2@1+ (1,0) [0|3] "" Vector__XXX
SG_ CHECKSUM : 60|4@1+ (1,0) [0|3] "" Vector__XXX
BO_ 0x17C POWERTRAIN_DATA2: 8 PCM
SG_ PEDAL_GAS : 0|8@1+ (1,0) [0|255] "" Vector__XXX
SG_ ENGINE_RPM : 16|16@1+ (1,0) [0|15000] "rpm" Vector__XXX
SG_ GAS_PRESSED : 32|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ ACC_STATUS : 33|1@1+ (1,0) [0|1] "rpm" Vector__XXX
SG_ BOH_17C : 34|5@1+ (1,0) [0|1] "rpm" Vector__XXX
SG_ BRAKE_LIGHTS_ON : 39|1@1+ (1,0) [0|1] "rpm" Vector__XXX
SG_ BOH2_17C : 40|10@1+ (1,0) [0|1] "rpm" Vector__XXX
SG_ BRAKE_PRESSED : 50|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ BOH3_17C : 51|5@1+ (1,0) [0|1] "rpm" Vector__XXX
SG_ COUNTER : 58|2@1+ (1,0) [0|3] "" Vector__XXX
SG_ CHECKSUM : 60|4@1+ (1,0) [0|3] "" Vector__XXX
BO_ 0x18E XXX: 3 XXX
BO_ 0x18F STEER_STATUS: 7 EPS
SG_ STEER_TORQUE_SENSOR : 0|16@1- (1,0) [-31000|31000] "tbd" Vector__XXX
SG_ STEER_TORQUE_MOTOR : 16|16@1- (1,0) [-31000|31000] "tbd" Vector__XXX
SG_ STEER_STATUS : 32|4@1+ (1,0) [0|15] "" Vector__XXX
SG_ STEER_CONTROL_ACTIVE : 36|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ COUNTER : 50|2@1+ (1,0) [0|3] "" Vector__XXX
SG_ CHECKSUM : 52|4@1+ (1,0) [0|3] "" Vector__XXX
BO_ 0x1A3 GEARBOX: 8 PCM
SG_ GEAR : 0|8@1+ (1,0) [0|256] "" Vector__XXX
SG_ GEAR_SHIFTER : 36|4@1+ (1,0) [0|15] "" Vector__XXX
SG_ COUNTER : 58|2@1+ (1,0) [0|3] "" Vector__XXX
SG_ CHECKSUM : 60|4@1+ (1,0) [0|3] "" Vector__XXX
BO_ 0x1A4 VSA_STATUS: 8 VSA
SG_ USER_BRAKE : 0|16@1+ (0.015625,-103) [0|1000] "" Vector__XXX
SG_ ESP_DISABLED : 27|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ COUNTER : 58|2@1+ (1,0) [0|3] "" Vector__XXX
SG_ CHECKSUM : 60|4@1+ (1,0) [0|3] "" Vector__XXX
BO_ 0x1A6 SCM_BUTTONS: 8 SCM
SG_ CRUISE_BUTTONS : 0|3@1+ (1,0) [0|7] "" Vector__XXX
SG_ LIGHTS_SETTING : 6|2@1+ (1,0) [0|3] "" Vector__XXX
SG_ MAIN_ON : 40|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ CRUISE_SETTING : 44|2@1+ (1,0) [0|3] "" Vector__XXX
SG_ COUNTER : 58|2@1+ (1,0) [0|3] "" Vector__XXX
SG_ CHECKSUM : 60|4@1+ (1,0) [0|3] "" Vector__XXX
BO_ 0x1AC XXX: 8 XXX
BO_ 0x1B0 STANDSTILL: 7 VSA
SG_ WHEELS_MOVING : 11|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ BRAKE_ERROR_1 : 12|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ BRAKE_ERROR_2 : 14|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ COUNTER : 50|2@1+ (1,0) [0|3] "" Vector__XXX
SG_ CHECKSUM : 52|4@1+ (1,0) [0|3] "" Vector__XXX
BO_ 0x1D0 WHEEL_SPEEDS: 8 VSA
SG_ WHEEL_SPEED_FL : 0|15@1+ (0.002759506,0) [0|70] "m/s" Vector__XXX
SG_ WHEEL_SPEED_FR : 15|15@1+ (0.002759506,0) [0|70] "m/s" Vector__XXX
SG_ WHEEL_SPEED_RL : 30|15@1+ (0.002759506,0) [0|70] "m/s" Vector__XXX
SG_ WHEEL_SPEED_RR : 45|15@1+ (0.002759506,0) [0|70] "m/s" Vector__XXX
SG_ CHECKSUM : 60|4@1+ (1,0) [0|3] "" Vector__XXX
BO_ 0x1DC XXX: 4 XXX
BO_ 0x1EA VEHICLE_DYNAMICS: 8 VSA
SG_ LONG_ACCEL : 16|16@1- (0.0015384,0) [-20|20] "m/s2" Vector__XXX
BO_ 0x1FA BRAKE_COMMAND: 8 ADAS
SG_ COMPUTER_BRAKE : 0|10@1+ (0.003906248,0) [0|1] "" Vector__XXX
SG_ ZEROS_BOH : 10|5@1+ (1,0) [0|1] "" Vector__XXX
SG_ COMPUTER_BRAKE_REQUEST : 15|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ CRUISE_BOH2 : 16|3@1+ (1,0) [0|1] "" Vector__XXX
SG_ CRUISE_OVERRIDE : 19|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ CRUISE_BOH3 : 20|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ CRUISE_FAULT_CMD : 21|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ CRUISE_CANCEL_CMD : 22|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ COMPUTER_BRAKE_REQUEST_2 : 23|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ CRUISE_BOH4 : 24|8@1+ (1,0) [0|1] "" Vector__XXX
SG_ BRAKE_LIGHTS : 32|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ CRUISE_BOH5 : 33|7@1+ (1,0) [0|1] "" Vector__XXX
SG_ CHIME : 40|3@1+ (1,0) [0|7] "" Vector__XXX
SG_ CRUISE_BOH6 : 43|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ FCW : 44|2@1+ (1,0) [0|3] "" Vector__XXX
SG_ CRUISE_BOH7 : 46|10@1+ (1,0) [0|0] "" Vector__XXX
SG_ COUNTER : 58|2@1+ (1,0) [0|3] "" Vector__XXX
SG_ CHECKSUM : 60|4@1+ (1,0) [0|3] "" Vector__XXX
BO_ 0x200 GAS_COMMAND: 3 ADAS
SG_ GAS_COMMAND : 0|16@1+ (0.253984064,-328) [0|1] "" Vector__XXX
SG_ COUNTER : 18|2@1+ (1,0) [0|3] "" Vector__XXX
SG_ CHECKSUM : 20|4@1+ (1,0) [0|3] "" Vector__XXX
BO_ 0x201 GAS_SENSOR: 5 ADAS
SG_ INTERCEPTOR_GAS : 0|16@1+ (0.253984064,-328) [0|1] "" Vector__XXX
SG_ INTERCEPTOR_GAS2 : 16|16@1+ (0.126992032,-656) [0|1] "" Vector__XXX
SG_ COUNTER : 34|2@1+ (1,0) [0|3] "" Vector__XXX
SG_ CHECKSUM : 36|4@1+ (1,0) [0|3] "" Vector__XXX
BO_ 0x21E XXX: 7 XXX
BO_ 0x221 XXX: 4 XXX
BO_ 0x255 ROUGH_WHEEL_SPEED: 8 VSA
SG_ WHEEL_SPEED_FL : 0|8@1+ (1,0) [0|255] "mph" Vector__XXX
SG_ WHEEL_SPEED_FR : 8|8@1+ (1,0) [0|255] "mph" Vector__XXX
SG_ WHEEL_SPEED_RL : 16|8@1+ (1,0) [0|255] "mph" Vector__XXX
SG_ WHEEL_SPEED_RR : 24|8@1+ (1,0) [0|255] "mph" Vector__XXX
SG_ SET_TO_X55 : 32|8@1+ (1,0) [0|255] "" Vector__XXX
SG_ SET_TO_X55 : 40|8@1+ (1,0) [0|255] "" Vector__XXX
BO_ 0x294 SCM_COMMANDS: 8 SCM
SG_ RIGHT_BLINKER : 1|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ LEFT_BLINKER : 2|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ WIPERS_SPEED : 3|2@1+ (1,0) [0|3] "" Vector__XXX
BO_ 0x305 SEATBELT_STATUS: 7 BDY
SG_ SEATBELT_DRIVER_LAMP : 0|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ SEATBELT_DRIVER_LATCHED : 10|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ COUNTER : 50|2@1+ (1,0) [0|3] "" Vector__XXX
SG_ CHECKSUM : 52|4@1+ (1,0) [0|3] "" Vector__XXX
BO_ 0x309 XXX: 8 XXX
BO_ 0x30C ACC_HUD: 8 ADAS
SG_ PCM_SPEED : 0|16@1+ (0.002763889,0) [0|100] "m/s" Vector__XXX
SG_ PCM_GAS : 16|7@1+ (1,0) [0|127] "" Vector__XXX
SG_ ZEROS_BOH : 23|1@1+ (1,0) [0|255] "" Vector__XXX
SG_ CRUISE_SPEED : 24|8@1+ (1,0) [0|255] "" Vector__XXX
SG_ DTC_MODE : 32|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ BOH : 33|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ ACC_PROBLEM : 34|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ FCM_OFF : 35|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ BOH_2 : 36|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ FCM_PROBLEM : 37|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ RADAR_OBSTRUCTED : 38|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ ENABLE_MINI_CAR : 39|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ SET_ME_X03 : 40|2@1+ (1,0) [0|3] "" Vector__XXX
SG_ HUD_LEAD : 42|2@1+ (1,0) [0|3] "" Vector__XXX
SG_ BOH_3 : 44|1@1+ (1,0) [0|3] "" Vector__XXX
SG_ BOH_4 : 45|1@1+ (1,0) [0|3] "" Vector__XXX
SG_ BOH_5 : 46|1@1+ (1,0) [0|3] "" Vector__XXX
SG_ CRUISE_CONTROL_LABEL : 47|1@1+ (1,0) [0|3] "" Vector__XXX
SG_ HUD_DISTANCE_3 : 51|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ COUNTER : 58|2@1+ (1,0) [0|3] "" Vector__XXX
SG_ CHECKSUM : 60|4@1+ (1,0) [0|3] "" Vector__XXX
BO_ 0x320 XXX: 8 XXX
BO_ 0x324 CRUISE: 8 PCM
SG_ ENGINE_TEMPERATURE : 0|8@1+ (1,0) [0|255] "" Vector__XXX
SG_ BOH : 8|8@1+ (1,0) [0|255] "" Vector__XXX
SG_ TRIP_FUEL_CONSUMED : 16|16@1+ (1,0) [0|255] "" Vector__XXX
SG_ CRUISE_SPEED_PCM : 32|8@1+ (1,0) [0|255] "" Vector__XXX
SG_ BOH2 : 40|8@1- (1,0) [0|255] "" Vector__XXX
SG_ BOH3 : 48|8@1+ (1,0) [0|255] "" Vector__XXX
SG_ COUNTER : 58|2@1+ (1,0) [0|3] "" Vector__XXX
SG_ CHECKSUM : 60|4@1+ (1,0) [0|3] "" Vector__XXX
BO_ 0x328 XXX: 8 XXX
BO_ 0x333 XXX: 7 XXX
BO_ 0x335 XXX: 5 XXX
BO_ 0x33D LKAS_HUD_2: 5 ADAS
SG_ CAM_TEMP_HIGH : 0|1@1+ (1,0) [0|255] "" Vector__XXX
SG_ BOH : 1|7@1+ (1,0) [0|127] "" Vector__XXX
SG_ DASHED_LANES : 9|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ DTC : 10|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ LKAS_PROBLEM : 11|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ LKAS_OFF : 12|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ SOLID_LANES : 13|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ LDW_RIGHT : 14|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ STEERING_REQUIRED : 15|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ BOH : 16|2@1+ (1,0) [0|4] "" Vector__XXX
SG_ LDW_PROBLEM : 18|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ BEEP : 22|2@1+ (1,0) [0|1] "" Vector__XXX
SG_ LDW_ON : 27|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ LDW_OFF : 28|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ CLEAN_WINDSHIELD : 29|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ SET_ME_X48 : 24|8@1+ (1,0) [0|255] "" Vector__XXX
SG_ COUNTER : 34|2@1+ (1,0) [0|3] "" Vector__XXX
SG_ CHECKSUM : 36|4@1+ (1,0) [0|3] "" Vector__XXX
BO_ 0x372 XXX: 2 XXX
BO_ 0x374 XXX: 7 XXX
BO_ 0x377 XXX: 8 XXX
BO_ 0x378 XXX: 8 XXX
BO_ 0x37C XXX: 8 XXX
BO_ 0x39B XXX: 2 XXX
BO_ 0x3A1 XXX: 4 XXX
BO_ 0x3D7 XXX: 8 XXX
BO_ 0x3D9 XXX: 3 XXX
BO_ 0x400 XXX: 5 XXX
BO_ 0x403 XXX: 5 XXX
BO_ 0x405 DOORS_STATUS: 8 BDY
SG_ DOOR_OPEN_FL : 34|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ DOOR_OPEN_FR : 33|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ DOOR_OPEN_RL : 32|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ DOOR_OPEN_RR : 47|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ COUNTER : 58|2@1+ (1,0) [0|3] "" Vector__XXX
SG_ CHECKSUM : 60|4@1+ (1,0) [0|3] "" Vector__XXX
BO_ 0x406 XXX: 5 VSA
BO_ 0x40A XXX: 5 XXX
BO_ 0x40C XXX: 8 XXX
BO_ 0x40F XXX: 8 XXX
BO_ 0x421 XXX: 5 EPS
BO_ 0x428 XXX: 7 XXX
BO_ 0x454 XXX: 8 XXX
BO_ 0x555 XXX: 5 XXX
BO_ 0x640 XXX: 5 XXX
BO_ 0x641 XXX: 8 XXX
VAL_ 0x1A6 CRUISE_BUTTONS 7 "tbd" 6 "tbd" 5 "tbd" 4 "accel_res" 3 "decel_set" 2 "cancel" 1 "main" 0 "none";
VAL_ 0x1A6 CRUISE_SETTING 3 "distance_adj" 2 "tbd" 1 "lkas_button" 0 "none";
VAL_ 0x30C HUD_LEAD 3 "no_car" 2 "solid_car" 1 "dashed_car" 0 "no_car";
VAL_ 0x1A6 LIGHTS_SETTING 3 "high_beam" 2 "low_beam" 1 "position" 0 "no_lights";
VAL_ 0x18F STEER_STATUS 5 "fault" 4 "no_torque_alert_2" 2 "no_torque_alert_1" 0 "normal";
VAL_ 0x1A3 GEAR_SHIFTER 10 "S" 4 "D" 3 "N" 2 "R" 1 "P";
VAL_ 0x33D BEEP 3 "single_beep" 2 "triple_beep" 1 "repeated_beep" 0 "no_beep";
VAL_ 0x1FA CHIME 4 "double_chime" 3 "single_chime" 2 "continuous_chime" 1 "repeating_chime" 0 "no_chime";
VAL_ 0x1FA FCW 3 "fcw" 2 "fcw" 1 "fcw" 0 "no_fcw";
CM_ SG_ 0x1A3 GEAR "10 = reverse, 11 = transition";
CM_ SG_ 0x324 CRUISE_SPEED_ECHO "255 = no speed";
CM_ SG_ 0x33D CRUISE_SPEED "255 = no speed";
CM_ SG_ 0x1EA LONG_ACCEL "wheel speed derivative, noisy and zero snapping";
CM_ SG_ 0x33D BEEP "beeps are pleasant, chimes are for warnngs etc...";

175
dbcs/acura_ilx_2016_nidec.dbc
Unescape View File

@ -0,0 +1,175 @@
VERSION ""
NS_ :
NS_DESC_
CM_
BA_DEF_
BA_
VAL_
CAT_DEF_
CAT_
FILTER
BA_DEF_DEF_
EV_DATA_
ENVVAR_DATA_
SGTYPE_
SGTYPE_VAL_
BA_DEF_SGTYPE_
BA_SGTYPE_
SIG_TYPE_REF_
VAL_TABLE_
SIG_GROUP_
SIG_VALTYPE_
SIGTYPE_VALTYPE_
BO_TX_BU_
BA_DEF_REL_
BA_REL_
BA_DEF_DEF_REL_
BU_SG_REL_
BU_EV_REL_
BU_BO_REL_
SG_MUL_VAL_
BS_:
BO_ 0x300 VEHICLE_STATE: 8 ADAS
SG_ SET_ME_XF9 : 0|8@1+ (1,0) [0|255] "" Vector__XXX
SG_ VEHICLE_SPEED : 8|8@1+ (1,0) [0|255] "kph" Vector__XXX
BO_ 0x301 VEHICLE_STATE2: 8 ADAS
SG_ SET_ME_0F18510 : 0|28@1+ (1,0) [0|268435455] "" Vector__XXX
SG_ SET_ME_25A0000 : 28|28@1+ (1,0) [0|268435455] "" Vector__XXX
BO_ 0x400 XXX: 8 RADAR
BO_ 0x410 XXX: 8 RADAR
BO_ 0x411 XXX: 8 RADAR
BO_ 0x412 XXX: 8 RADAR
BO_ 0x413 XXX: 8 RADAR
BO_ 0x414 XXX: 8 RADAR
BO_ 0x415 XXX: 8 RADAR
BO_ 0x416 XXX: 8 RADAR
BO_ 0x417 XXX: 8 RADAR
BO_ 0x420 XXX: 8 RADAR
BO_ 0x421 XXX: 8 RADAR
BO_ 0x422 XXX: 8 RADAR
BO_ 0x423 XXX: 8 RADAR
BO_ 0x424 XXX: 8 RADAR
BO_ 0x430 TRACK_0: 8 RADAR
SG_ LONG_DIST : 0|12@1+ (0.0625,0) [0|255.5] "m" Vector__XXX
SG_ NEW_TRACK : 12|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ LAT_DIST : 14|10@1- (0.0625,0) [0|63.5] "m" Vector__XXX
SG_ REL_SPEED : 24|12@1- (0.03125,0) [0|127.5] "m/s" Vector__XXX
BO_ 0x431 TRACK_1: 8 RADAR
SG_ LONG_DIST : 0|12@1+ (0.0625,0) [0|255.5] "m" Vector__XXX
SG_ NEW_TRACK : 12|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ LAT_DIST : 14|10@1- (0.0625,0) [0|63.5] "m" Vector__XXX
SG_ REL_SPEED : 24|12@1- (0.03125,0) [0|127.5] "m/s" Vector__XXX
BO_ 0x432 TRACK_2: 8 RADAR
SG_ LONG_DIST : 0|12@1+ (0.0625,0) [0|255.5] "m" Vector__XXX
SG_ NEW_TRACK : 12|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ LAT_DIST : 14|10@1- (0.0625,0) [0|63.5] "m" Vector__XXX
SG_ REL_SPEED : 24|12@1- (0.03125,0) [0|127.5] "m/s" Vector__XXX
BO_ 0x433 TRACK_3: 8 RADAR
SG_ LONG_DIST : 0|12@1+ (0.0625,0) [0|255.5] "m" Vector__XXX
SG_ NEW_TRACK : 12|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ LAT_DIST : 14|10@1- (0.0625,0) [0|63.5] "m" Vector__XXX
SG_ REL_SPEED : 24|12@1- (0.03125,0) [0|127.5] "m/s" Vector__XXX
BO_ 0x434 TRACK_4: 8 RADAR
SG_ LONG_DIST : 0|12@1+ (0.0625,0) [0|255.5] "m" Vector__XXX
SG_ NEW_TRACK : 12|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ LAT_DIST : 14|10@1- (0.0625,0) [0|63.5] "m" Vector__XXX
SG_ REL_SPEED : 24|12@1- (0.03125,0) [0|127.5] "m/s" Vector__XXX
BO_ 0x435 TRACK_5: 8 RADAR
SG_ LONG_DIST : 0|12@1+ (0.0625,0) [0|255.5] "m" Vector__XXX
SG_ NEW_TRACK : 12|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ LAT_DIST : 14|10@1- (0.0625,0) [0|63.5] "m" Vector__XXX
SG_ REL_SPEED : 24|12@1- (0.03125,0) [0|127.5] "m/s" Vector__XXX
BO_ 0x436 TRACK_6: 8 RADAR
SG_ LONG_DIST : 0|12@1+ (0.0625,0) [0|255.5] "m" Vector__XXX
SG_ NEW_TRACK : 12|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ LAT_DIST : 14|10@1- (0.0625,0) [0|63.5] "m" Vector__XXX
SG_ REL_SPEED : 24|12@1- (0.03125,0) [0|127.5] "m/s" Vector__XXX
BO_ 0x437 TRACK_7: 8 RADAR
SG_ LONG_DIST : 0|12@1+ (0.0625,0) [0|255.5] "m" Vector__XXX
SG_ NEW_TRACK : 12|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ LAT_DIST : 14|10@1- (0.0625,0) [0|63.5] "m" Vector__XXX
SG_ REL_SPEED : 24|12@1- (0.03125,0) [0|127.5] "m/s" Vector__XXX
BO_ 0x438 TRACK_8: 8 RADAR
SG_ LONG_DIST : 0|12@1+ (0.0625,0) [0|255.5] "m" Vector__XXX
SG_ NEW_TRACK : 12|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ LAT_DIST : 14|10@1- (0.0625,0) [0|63.5] "m" Vector__XXX
SG_ REL_SPEED : 24|12@1- (0.03125,0) [0|127.5] "m/s" Vector__XXX
BO_ 0x439 TRACK_9: 8 RADAR
SG_ LONG_DIST : 0|12@1+ (0.0625,0) [0|255.5] "m" Vector__XXX
SG_ NEW_TRACK : 12|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ LAT_DIST : 14|10@1- (0.0625,0) [0|63.5] "m" Vector__XXX
SG_ REL_SPEED : 24|12@1- (0.03125,0) [0|127.5] "m/s" Vector__XXX
BO_ 0x440 TRACK_10: 8 RADAR
SG_ LONG_DIST : 0|12@1+ (0.0625,0) [0|255.5] "m" Vector__XXX
SG_ NEW_TRACK : 12|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ LAT_DIST : 14|10@1- (0.0625,0) [0|63.5] "m" Vector__XXX
SG_ REL_SPEED : 24|12@1- (0.03125,0) [0|127.5] "m/s" Vector__XXX
BO_ 0x441 TRACK_11: 8 RADAR
SG_ LONG_DIST : 0|12@1+ (0.0625,0) [0|255.5] "m" Vector__XXX
SG_ NEW_TRACK : 12|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ LAT_DIST : 14|10@1- (0.0625,0) [0|63.5] "m" Vector__XXX
SG_ REL_SPEED : 24|12@1- (0.03125,0) [0|127.5] "m/s" Vector__XXX
BO_ 0x442 TRACK_12: 8 RADAR
SG_ LONG_DIST : 0|12@1+ (0.0625,0) [0|255.5] "m" Vector__XXX
SG_ NEW_TRACK : 12|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ LAT_DIST : 14|10@1- (0.0625,0) [0|63.5] "m" Vector__XXX
SG_ REL_SPEED : 24|12@1- (0.03125,0) [0|127.5] "m/s" Vector__XXX
BO_ 0x443 TRACK_13: 8 RADAR
SG_ LONG_DIST : 0|12@1+ (0.0625,0) [0|255.5] "m" Vector__XXX
SG_ NEW_TRACK : 12|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ LAT_DIST : 14|10@1- (0.0625,0) [0|63.5] "m" Vector__XXX
SG_ REL_SPEED : 24|12@1- (0.03125,0) [0|127.5] "m/s" Vector__XXX
BO_ 0x444 TRACK_14: 8 RADAR
SG_ LONG_DIST : 0|12@1+ (0.0625,0) [0|255.5] "m" Vector__XXX
SG_ NEW_TRACK : 12|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ LAT_DIST : 14|10@1- (0.0625,0) [0|63.5] "m" Vector__XXX
SG_ REL_SPEED : 24|12@1- (0.03125,0) [0|127.5] "m/s" Vector__XXX
BO_ 0x445 TRACK_15: 8 RADAR
SG_ LONG_DIST : 0|12@1+ (0.0625,0) [0|255.5] "m" Vector__XXX
SG_ NEW_TRACK : 12|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ LAT_DIST : 14|10@1- (0.0625,0) [0|63.5] "m" Vector__XXX
SG_ REL_SPEED : 24|12@1- (0.03125,0) [0|127.5] "m/s" Vector__XXX
BO_ 0x4FF XXX: 8 RADAR
BO_ 0x500 XXX: 8 RADAR
BO_ 0x510 XXX: 8 RADAR
BO_ 0x511 XXX: 8 RADAR

319
dbcs/honda_civic_touring_2016_can.dbc
Unescape View File

@ -0,0 +1,319 @@
VERSION ""
NS_ :
NS_DESC_
CM_
BA_DEF_
BA_
VAL_
CAT_DEF_
CAT_
FILTER
BA_DEF_DEF_
EV_DATA_
ENVVAR_DATA_
SGTYPE_
SGTYPE_VAL_
BA_DEF_SGTYPE_
BA_SGTYPE_
SIG_TYPE_REF_
VAL_TABLE_
SIG_GROUP_
SIG_VALTYPE_
SIGTYPE_VALTYPE_
BO_TX_BU_
BA_DEF_REL_
BA_REL_
BA_DEF_DEF_REL_
BU_SG_REL_
BU_EV_REL_
BU_BO_REL_
SG_MUL_VAL_
BS_:
BO_ 0x039 XXX: 3 XXX
BO_ 0x94 XXX: 8 XXX
SG_ LAT_ACCEL : 0|10@1+ (0.02,-512) [-20|20] "m/s2" Vector__XXX
SG_ LONG_ACCEL : 31|9@1- (-0.02,0) [-20|20] "m/s2" Vector__XXX
BO_ 0x0E4 STEERING_CONTROL: 5 ADAS
SG_ STEER_TORQUE : 0|16@1- (1,0) [-3840|3840] "" Vector__XXX
SG_ STEER_TORQUE_REQUEST : 16|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ SET_ME_X00 : 17|7@1+ (1,0) [0|127] "" Vector__XXX
SG_ SET_ME_X00_2 : 24|8@1+ (1,0) [0|0] "" Vector__XXX
SG_ CHECKSUM : 32|4@1+ (1,0) [0|15] "" Vector__XXX
SG_ COUNTER : 38|2@1+ (1,0) [0|3] "" Vector__XXX
BO_ 0x130 GAS_PEDAL2: 8 PCM
SG_ ENGINE_TORQUE_ESTIMATE : 0|16@1- (1,0) [-1000|1000] "Nm" Vector__XXX
SG_ ENGINE_TORQUE_REQUEST : 16|16@1- (1,0) [-1000|1000] "Nm" Vector__XXX
SG_ CAR_GAS : 32|8@1+ (1,0) [0|255] "" Vector__XXX
BO_ 0x14A STEERING_SENSORS: 8 EPS
SG_ STEER_ANGLE : 0|16@1- (-0.1,0) [-500|500] "deg" Vector__XXX
SG_ STEER_ANGLE_RATE : 16|16@1- (-1,0) [-3000|3000] "deg/s" Vector__XXX
SG_ STEER_ANGLE_OFFSET : 32|8@1- (-0.1,0) [-128|127] "deg" Vector__XXX
SG_ STEER_WHEEL_ANGLE : 40|16@1- (-0.1,0) [-500|500] "deg" Vector__XXX
SG_ COUNTER : 58|2@1+ (1,0) [0|3] "" Vector__XXX
SG_ CHECKSUM : 60|4@1+ (1,0) [0|3] "" Vector__XXX
BO_ 0x158 POWERTRAIN_DATA: 8 PCM
SG_ XMISSION_SPEED : 0|16@1+ (0.002759506,0) [0|70] "m/s" Vector__XXX
SG_ ENGINE_RPM : 16|16@1+ (1,0) [0|15000] "rpm" Vector__XXX
SG_ XMISSION_SPEED2 : 32|16@1+ (0.002759506,0) [0|70] "m/s" Vector__XXX
SG_ COUNTER : 58|2@1+ (1,0) [0|3] "" Vector__XXX
SG_ CHECKSUM : 60|4@1+ (1,0) [0|3] "" Vector__XXX
BO_ 0x17C POWERTRAIN_DATA2: 8 PCM
SG_ PEDAL_GAS : 0|8@1+ (1,0) [0|255] "" Vector__XXX
SG_ ENGINE_RPM : 16|16@1+ (1,0) [0|15000] "rpm" Vector__XXX
SG_ GAS_PRESSED : 32|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ ACC_STATUS : 33|1@1+ (1,0) [0|1] "rpm" Vector__XXX
SG_ BOH_17C : 34|5@1+ (1,0) [0|1] "rpm" Vector__XXX
SG_ BRAKE_LIGHTS_ON : 39|1@1+ (1,0) [0|1] "rpm" Vector__XXX
SG_ BOH2_17C : 40|10@1+ (1,0) [0|1] "rpm" Vector__XXX
SG_ BRAKE_PRESSED : 50|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ BOH3_17C : 51|5@1+ (1,0) [0|1] "rpm" Vector__XXX
SG_ COUNTER : 58|2@1+ (1,0) [0|3] "" Vector__XXX
SG_ CHECKSUM : 60|4@1+ (1,0) [0|3] "" Vector__XXX
BO_ 0x18F STEER_STATUS: 7 EPS
SG_ STEER_TORQUE_SENSOR : 0|16@1- (1,0) [-31000|31000] "tbd" Vector__XXX
SG_ STEER_TORQUE_MOTOR : 16|16@1- (1,0) [-31000|31000] "tbd" Vector__XXX
SG_ STEER_STATUS : 32|4@1+ (1,0) [0|15] "" Vector__XXX
SG_ STEER_CONTROL_ACTIVE : 36|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ COUNTER : 50|2@1+ (1,0) [0|3] "" Vector__XXX
SG_ CHECKSUM : 52|4@1+ (1,0) [0|3] "" Vector__XXX
BO_ 0x191 GEARBOX: 8 PCM
SG_ GEAR_SHIFTER : 2|6@1+ (1,0) [0|63] "" Vector__XXX
SG_ GEAR : 36|4@1+ (1,0) [0|15] "" Vector__XXX
SG_ COUNTER : 58|2@1+ (1,0) [0|3] "" Vector__XXX
SG_ CHECKSUM : 60|4@1+ (1,0) [0|3] "" Vector__XXX
BO_ 0x1A4 VSA_STATUS: 8 VSA
SG_ USER_BRAKE : 0|16@1+ (0.015625,-103) [0|1000] "" Vector__XXX
SG_ ESP_DISABLED : 27|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ COUNTER : 58|2@1+ (1,0) [0|3] "" Vector__XXX
SG_ CHECKSUM : 60|4@1+ (1,0) [0|3] "" Vector__XXX
BO_ 0x1AB XXX: 3 VSA
BO_ 0x1AC XXX: 8 XXX
BO_ 0x1B0 STANDSTILL: 7 VSA
SG_ WHEELS_MOVING : 11|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ BRAKE_ERROR_1 : 12|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ BRAKE_ERROR_2 : 14|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ COUNTER : 50|2@1+ (1,0) [0|3] "" Vector__XXX
SG_ CHECKSUM : 52|4@1+ (1,0) [0|3] "" Vector__XXX
BO_ 0x1C2 XXX: 8 EPB
SG_ EPB_ACTIVE : 4|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ EPB_STATE : 26|2@1+ (1,0) [0|3] "" Vector__XXX
BO_ 0x1D0 WHEEL_SPEEDS: 8 VSA
SG_ WHEEL_SPEED_FL : 0|15@1+ (0.002759506,0) [0|70] "m/s" Vector__XXX
SG_ WHEEL_SPEED_FR : 15|15@1+ (0.002759506,0) [0|70] "m/s" Vector__XXX
SG_ WHEEL_SPEED_RL : 30|15@1+ (0.002759506,0) [0|70] "m/s" Vector__XXX
SG_ WHEEL_SPEED_RR : 45|15@1+ (0.002759506,0) [0|70] "m/s" Vector__XXX
SG_ CHECKSUM : 60|4@1+ (1,0) [0|3] "" Vector__XXX
BO_ 0x1D6 XXX: 2 VSA
BO_ 0x1DC XXX: 7 XXX
BO_ 0x1E7 XXX: 4 VSA
SG_ BRAKE_PRESSURE1 : 0|10@1+ (0.015625,-103) [0|1000] "" Vector__XXX
SG_ BRAKE_PRESSURE2 : 14|10@1+ (0.015625,-103) [0|1000] "" Vector__XXX
BO_ 0x1EA VEHICLE_DYNAMICS: 8 VSA
SG_ LONG_ACCEL : 16|16@1- (0.0015384,0) [-20|20] "m/s2" Vector__XXX
BO_ 0x1ED XXX: 5 VSA
BO_ 0x1FA BRAKE_COMMAND: 8 ADAS
SG_ COMPUTER_BRAKE : 0|10@1+ (0.003906248,0) [0|1.0] "" Vector__XXX
SG_ ZEROS_BOH : 10|5@1+ (1,0) [0|1] "" Vector__XXX
SG_ COMPUTER_BRAKE_REQUEST : 15|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ CRUISE_BOH2 : 16|3@1+ (1,0) [0|1] "" Vector__XXX
SG_ CRUISE_OVERRIDE : 19|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ CRUISE_BOH3 : 20|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ CRUISE_FAULT_CMD : 21|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ CRUISE_CANCEL_CMD : 22|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ COMPUTER_BRAKE_REQUEST_2 : 23|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ SET_ME_0X80 : 24|8@1+ (1,0) [0|1] "" Vector__XXX
SG_ BRAKE_LIGHTS : 32|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ CRUISE_STATES : 33|7@1+ (1,0) [0|1] "" Vector__XXX
SG_ CHIME : 40|3@1+ (1,0) [0|7] "" Vector__XXX
SG_ ZEROS_BOH6 : 43|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ FCW : 44|1@1+ (1,0) [0|3] "" Vector__XXX
SG_ ZEROS_BOH3 : 45|2@1+ (1,0) [0|0] "" Vector__XXX
SG_ FCW2 : 47|1@1+ (1,0) [0|0] "" Vector__XXX
SG_ ZEROS_BOH4 : 48|8@1+ (1,0) [0|0] "" Vector__XXX
SG_ COUNTER : 58|2@1+ (1,0) [0|3] "" Vector__XXX
SG_ CHECKSUM : 60|4@1+ (1,0) [0|3] "" Vector__XXX
BO_ 0x200 GAS_COMMAND: 3 ADAS
SG_ GAS_COMMAND : 0|16@1+ (0.253984064,-328) [0|1] "" Vector__XXX
SG_ COUNTER : 18|2@1+ (1,0) [0|3] "" Vector__XXX
SG_ CHECKSUM : 20|4@1+ (1,0) [0|3] "" Vector__XXX
BO_ 0x201 GAS_SENSOR: 5 ADAS
SG_ INTERCEPTOR_GAS : 0|16@1+ (0.253984064,-328) [0|1] "" Vector__XXX
SG_ INTERCEPTOR_GAS2 : 16|16@1+ (0.126992032,-656) [0|1] "" Vector__XXX
SG_ COUNTER : 34|2@1+ (1,0) [0|3] "" Vector__XXX
SG_ CHECKSUM : 36|4@1+ (1,0) [0|3] "" Vector__XXX
BO_ 0x221 XXX: 6 XXX
BO_ 0x255 ROUGH_WHEEL_SPEED: 8 VSA
SG_ WHEEL_SPEED_FL : 0|8@1+ (1,0) [0|255] "mph" Vector__XXX
SG_ WHEEL_SPEED_FR : 8|8@1+ (1,0) [0|255] "mph" Vector__XXX
SG_ WHEEL_SPEED_RL : 16|8@1+ (1,0) [0|255] "mph" Vector__XXX
SG_ WHEEL_SPEED_RR : 24|8@1+ (1,0) [0|255] "mph" Vector__XXX
SG_ SET_TO_X55 : 32|8@1+ (1,0) [0|255] "" Vector__XXX
SG_ SET_TO_X55 : 40|8@1+ (1,0) [0|255] "" Vector__XXX
BO_ 0x296 CRUISE_BUTTONS: 4 SCM
SG_ CRUISE_BUTTONS : 0|3@1+ (1,0) [0|7] "" Vector__XXX
SG_ CRUISE_SETTING : 4|2@1+ (1,0) [0|3] "" Vector__XXX
BO_ 0x305 SEATBELT_STATUS: 7 BDY
SG_ SEATBELT_DRIVER_LAMP : 0|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ SEATBELT_DRIVER_LATCHED : 10|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ COUNTER : 50|2@1+ (1,0) [0|3] "" Vector__XXX
SG_ CHECKSUM : 52|4@1+ (1,0) [0|3] "" Vector__XXX
BO_ 0x309 XXX: 8 XXX
BO_ 0x30C ACC_HUD: 8 ADAS
SG_ PCM_SPEED : 0|16@1+ (0.002763889,0) [0|100] "m/s" Vector__XXX
SG_ PCM_GAS : 16|7@1+ (1,0) [0|127] "" Vector__XXX
SG_ ZEROS_BOH : 23|1@1+ (1,0) [0|255] "" Vector__XXX
SG_ CRUISE_SPEED : 24|8@1+ (1,0) [0|255] "" Vector__XXX
SG_ DTC_MODE : 32|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ BOH : 33|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ ACC_PROBLEM : 34|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ FCM_OFF : 35|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ BOH_2 : 36|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ FCM_PROBLEM : 37|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ RADAR_OBSTRUCTED : 38|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ ENABLE_MINI_CAR : 39|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ HUD_DISTANCE : 40|2@1+ (1,0) [0|3] "" Vector__XXX
SG_ HUD_LEAD : 42|2@1+ (1,0) [0|3] "" Vector__XXX
SG_ BOH_3 : 44|1@1+ (1,0) [0|3] "" Vector__XXX
SG_ BOH_4 : 45|1@1+ (1,0) [0|3] "" Vector__XXX
SG_ BOH_5 : 46|1@1+ (1,0) [0|3] "" Vector__XXX
SG_ CRUISE_CONTROL_LABEL : 47|1@1+ (1,0) [0|3] "" Vector__XXX
SG_ COUNTER : 58|2@1+ (1,0) [0|3] "" Vector__XXX
SG_ CHECKSUM : 60|4@1+ (1,0) [0|3] "" Vector__XXX
BO_ 0x31B XXX: 8 XXX
BO_ 0x320 XXX: 8 XXX
BO_ 0x324 CRUISE: 8 PCM
SG_ ENGINE_TEMPERATURE : 0|8@1+ (1,0) [0|255] "" Vector__XXX
SG_ BOH : 8|8@1+ (1,0) [0|255] "" Vector__XXX
SG_ TRIP_FUEL_CONSUMED : 16|16@1+ (1,0) [0|255] "" Vector__XXX
SG_ CRUISE_SPEED_PCM : 32|8@1+ (1,0) [0|255] "" Vector__XXX
SG_ BOH2 : 40|8@1- (1,0) [0|255] "" Vector__XXX
SG_ BOH3 : 48|8@1+ (1,0) [0|255] "" Vector__XXX
SG_ COUNTER : 58|2@1+ (1,0) [0|3] "" Vector__XXX
SG_ CHECKSUM : 60|4@1+ (1,0) [0|3] "" Vector__XXX
BO_ 0x326 SCM_FEEDBACK: 8 SCM
SG_ CMBS_BUTTON : 17|2@1+ (1,0) [0|3] "" Vector__XXX
SG_ MAIN_ON : 27|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ RIGHT_BLINKER : 28|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ LEFT_BLINKER : 29|1@1+ (1,0) [0|1] "" Vector__XXX
BO_ 0x328 XXX: 8 XXX
BO_ 0x33D LKAS_HUD_2: 5 ADAS
SG_ CAM_TEMP_HIGH : 0|1@1+ (1,0) [0|255] "" Vector__XXX
SG_ BOH : 1|7@1+ (1,0) [0|127] "" Vector__XXX
SG_ DASHED_LANES : 9|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ DTC : 10|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ LKAS_PROBLEM : 11|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ LKAS_OFF : 12|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ SOLID_LANES : 13|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ LDW_RIGHT : 14|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ STEERING_REQUIRED : 15|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ BOH : 16|2@1+ (1,0) [0|4] "" Vector__XXX
SG_ LDW_PROBLEM : 18|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ BEEP : 22|2@1+ (1,0) [0|1] "" Vector__XXX
SG_ LDW_ON : 27|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ LDW_OFF : 28|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ CLEAN_WINDSHIELD : 29|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ SET_ME_X48 : 24|8@1+ (1,0) [0|255] "" Vector__XXX
SG_ COUNTER : 34|2@1+ (1,0) [0|3] "" Vector__XXX
SG_ CHECKSUM : 36|4@1+ (1,0) [0|3] "" Vector__XXX
BO_ 0x35E XXX: 8 ADAS
SG_ UI_ALERTS : 0|56@1+ (1,0) [0|127] "" Vector__XXX
BO_ 0x374 XXX: 8 XXX
BO_ 0x37B XXX: 8 XXX
BO_ 0x37C XXX: 8 XXX
BO_ 0x39F XXX: 8 ADAS
SG_ ZEROS_BOH : 0|17@1+ (1,0) [0|127] "" Vector__XXX
SG_ APPLY_BRAKES_FOR_CANC : 16|1@1+ (1,0) [0|15] "" Vector__XXX
SG_ ZEROS_BOH2 : 17|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ RESUME_INSTRUCTION : 18|1@1+ (1,0) [0|15] "" Vector__XXX
SG_ ACC_ALERTS : 19|5@1+ (1,0) [0|15] "" Vector__XXX
SG_ ZEROS_BOH2 : 24|8@1+ (1,0) [0|127] "" Vector__XXX
SG_ LEAD_SPEED : 32|9@1+ (1,0) [0|127] "" Vector__XXX
SG_ LEAD_STATE : 41|3@1+ (1,0) [0|127] "" Vector__XXX
SG_ LEAD_DISTANCE : 44|5@1+ (1,0) [0|31] "" Vector__XXX
SG_ ZEROS_BOH3 : 49|7@1+ (1,0) [0|127] "" Vector__XXX
BO_ 0x3A1 XXX: 8 XXX
BO_ 0x3D9 XXX: 3 XXX
BO_ 0x400 XXX: 5 XXX
BO_ 0x403 XXX: 5 XXX
BO_ 0x405 DOORS_STATUS: 8 BDY
SG_ DOOR_OPEN_FL : 34|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ DOOR_OPEN_FR : 33|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ DOOR_OPEN_RL : 32|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ DOOR_OPEN_RR : 47|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ COUNTER : 58|2@1+ (1,0) [0|3] "" Vector__XXX
SG_ CHECKSUM : 60|4@1+ (1,0) [0|3] "" Vector__XXX
BO_ 0x40C XXX: 8 XXX
BO_ 0x40F XXX: 8 XXX
BO_ 0x454 XXX: 8 XXX
BO_ 0x516 XXX: 8 XXX
BO_ 0x52A XXX: 5 XXX
BO_ 0x551 XXX: 5 XXX
BO_ 0x555 XXX: 5 XXX
BO_ 0x590 XXX: 5 XXX
BO_ 0x640 XXX: 5 XXX
BO_ 0x641 XXX: 8 XXX
BO_ 0x661 XXX: 8 XXX
VAL_ 0x296 CRUISE_BUTTONS 7 "tbd" 6 "tbd" 5 "tbd" 4 "accel_res" 3 "decel_set" 2 "cancel" 1 "main" 0 "none";
VAL_ 0x296 CRUISE_SETTING 3 "distance_adj" 2 "tbd" 1 "lkas_button" 0 "none";
VAL_ 0x33D HUD_LEAD 3 "acc_off" 2 "solid_car" 1 "dashed_car" 0 "no_car";
VAL_ 0x1A6 LIGHTS_SETTING 3 "high_beam" 2 "low_beam" 1 "position" 0 "no_lights";
VAL_ 0x18F STEER_STATUS 5 "fault" 4 "no_torque_alert_2" 2 "no_torque_alert_1" 0 "normal";
VAL_ 0x191 GEAR_SHIFTER 32 "L" 16 "S" 8 "D" 4 "N" 2 "R" 1 "P";
VAL_ 0x33D BEEP 3 "single_beep" 2 "triple_beep" 1 "repeated_beep" 0 "no_beep";
VAL_ 0x1FA CHIME 4 "double_chime" 3 "single_chime" 2 "continuous_chime" 1 "repeating_chime" 0 "no_chime";
VAL_ 0x1C2 EPB_STATE 3 "engaged" 2 "disengaging" 1 "engaging" 0 "disengaged";
VAL_ 0x326 CMBS_BUTTON 3 "pressed" 0 "released";
VAL_ 0x39F ACC_ALERTS 29 "esp_active_acc_canceled" 10 "b_pedal_applied" 9 "speed_too_low" 8 "speed_too_high" 7 "p_brake_applied" 6 "gear_no_d" 5 "seatbelt" 4 "too_steep_downhill" 3 "too_steep_uphill" 2 "too_close" 1 "no_vehicle_ahead";
VAL_ 0x30C CRUISE_SPEED 255 "no_speed" 252 "stopped";
CM_ SG_ 0x1A3 GEAR "10 = reverse, 11 = transition";
CM_ SG_ 0x324 CRUISE_SPEED_PCM "255 = no speed";
CM_ SG_ 0x30C CRUISE_SPEED "255 = no speed";
CM_ SG_ 0x1EA LONG_ACCEL "wheel speed derivative, noisy and zero snapping";
CM_ SG_ 0x33D BEEP "beeps are pleasant, chimes are for warnngs etc...";

28
installation/files/continue.sh
Unescape View File

@ -0,0 +1,28 @@
#!/usr/bin/bash
# enable wifi access point for debugging only!
#service call wifi 37 i32 0 i32 1 # WifiService.setWifiApEnabled(null, true)
# use the openpilot ro key
export GIT_SSH_COMMAND="ssh -i /data/data/com.termux/files/id_rsa_openpilot_ro"
# check out the openpilot repo
# TODO: release branch only
if [ ! -d /data/openpilot ]; then
cd /tmp
git clone git@github.com:commaai/openpilot.git
mv /tmp/openpilot /data/openpilot
fi
# update the openpilot repo
cd /data/openpilot
git pull
# start manager
cd selfdrive
mkdir -p /sdcard/realdata
PYTHONPATH=/data/openpilot ./manager.py
# if broken, keep on screen error
while true; do sleep 1; done

27
installation/files/id_rsa_openpilot_ro
Unescape View File

@ -0,0 +1,27 @@
-----BEGIN RSA PRIVATE KEY-----
MIIEpAIBAAKCAQEAy/ZlYE/iHHjhbSvCnBm5Zsq5GPpVugLXFHai/doqyfRxErop
/g1TIRhzK3mkHRYRN7H0L9whogwoIVr5CldoxU49FDnNbVHNimScNrL4LprRWjq6
dRmCVoxMpLHZTyX1jIkcHsMr7klcUnssyeQO2pWvZv0ZC67wM7G20r7+ZLdEa0Ck
MBh8JYhDaZx2xvYtTnt6vKMmFVE5+zW/+wDVma3a4r9pG9s0+r0wCl8CUuJ+yDhR
mzNkPJ5mJCMx99AI6k4Gq9Vsng8/35b6Azh3TucPaXOLK7yPnL3YBKUa0PpR7IRH
+OKkVCH+LL7tcPFSqPPVy/pUTBdEUROjJdSHxwIDAQABAoIBAQCxBgUM56h3T89Q
AoghFg6dkdu/Ox8GmAp231UuAJncuMUfHObvcj8xXVgwZp4zBIEjFte6ZlPmoqh9
8sht2lm7zeEjWdvbQwGjWRlgPEs9n++OYaSNl/tRBOpMk3Ppxydst1/prznE0nVH
vVKtU7w0qXAYchm30zj1lQv5s/12CTGmnpQatbo5X488RfCfv2zFX1h+lEWF8ycL
eZRi8z6l8h2Y+JLyEwPCmR+gR6XtosZ/ECQcTknavqLqdr7NbYYfOo3JfHCUtpJa
8s7m0VFhMuxFFCl1sV0eMzAynJYNVz45DyaKpr2b/2YAGY8fn96FxaWv1xw1xTkK
c5+wStwJAoGBAOjQpLZ1qGa4GwXzeHoDsGFpGgY9ug6af0M23c8O42fJHAwYkk7r
Qeo4SSBddoSfo3jdchFLo59+m3qyTKpjkph7NBBCEwaCvX3heStDIMZEWX0IOV5y
iJD/D6EXSqFmXCUUaudX2OxlaHguA0yOEx9s/5uUJrvaIHbBAOpYyar1AoGBAOBG
MJp+EA3e1Zx/VszD2Tdxn8V0DAwvy9WIEqZuG689S/Sk5GnA4m2L8Txv0xAHFvLv
JpF7Zn9AoFXGpjf9P0FF53cpjEYn9f+uK84j1HOL/6R7Nj9rcS5yL2PCP1ZHymw6
xOXl3oZa1YtYE6jfvXUaOb8Z7y8gaStP763sXmpLAoGBAM1WSBANUcvXES58gIPN
ASHJGwTqKFF8/kV//L4EuZjuDWi1u0UTxX0Yy5ZaGI/8ZKfTWCnc9qFTfzoGTAvz
6nXGJDM6s6EIaqy90qrPd/amje7y8/ZTOhP4ggZojpAvwZGKoocMOey1vCBTJOG+
ZStQbVkAn/EK/5r9uxr12FiJAoGAH9UWlPcLpExamWnhkhLCRAJWoRoFk708+0Pj
EchTGZ5jp4e3++KqwM26Ic/lb0LyWOzk1oVjWPB9UW9urEe/sK4RWnKFPHfzjKTW
Bt5DC1t1n4z1eC7x05vVah1qC/8IljAJPnBQE1XVNX/82l1XcMWWKK+vqUq6YrFn
3ZHNHN0CgYA3uUVWqW37vfJuk0MJBkQSqMo5Y5TPlCt4b1ebkdhlM4v/N+iuiPiC
PBhjP1MLeudkJvzllt4YvNWLerCKpMWuw7Zvy5uzFEsqOrVlzfnyWqqqYbYjHe9f
Ef0/yXKuGJajs54Ts6Xrm0+elVUu//pEuf6NI96Ehctqz8/BqGqAtw==
-----END RSA PRIVATE KEY-----

11
installation/install.sh
Unescape View File

@ -0,0 +1,11 @@
#!/bin/bash
set -e
# move files into place
adb push files/id_rsa_openpilot_ro /tmp/id_rsa_openpilot_ro
adb shell mv /tmp/id_rsa_openpilot_ro /data/data/com.termux/files/
# moving continue into place runs the continue script
adb push files/continue.sh /tmp/continue.sh
adb shell mv /tmp/continue.sh /data/data/com.termux/files/

3
phonelibs/hierarchy/lib/_hierarchy.so
Unescape View File

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

3
phonelibs/nanovg/fontstash.h
Unescape View File

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

3
phonelibs/nanovg/nanovg.c
Unescape View File

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

3
phonelibs/nanovg/nanovg.h
Unescape View File

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

3
phonelibs/nanovg/nanovg_gl.h
Unescape View File

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

3
phonelibs/nanovg/nanovg_gl_utils.h
Unescape View File

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

3
phonelibs/nanovg/stb_image.h
Unescape View File

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

3
phonelibs/nanovg/stb_truetype.h
Unescape View File

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

0
selfdrive/__init__.py
Unescape View File

3
selfdrive/assets/Roboto-Bold.ttf
Unescape View File

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

3
selfdrive/assets/courbd.ttf
Unescape View File

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

73
selfdrive/boardd/Makefile
Unescape View File

@ -0,0 +1,73 @@
CC = clang
CXX = clang++
ARCH := $(shell uname -m)
PHONELIBS = ../../phonelibs
WARN_FLAGS = -Werror=implicit-function-declaration \
-Werror=incompatible-pointer-types \
-Werror=int-conversion \
-Werror=return-type \
-Werror=format-extra-args
CFLAGS = -std=gnu11 -g -fPIC -O2 $(WARN_FLAGS)
CXXFLAGS = -std=c++11 -g -fPIC -O2 $(WARN_FLAGS)
ZMQ_FLAGS = -I$(PHONELIBS)/zmq/aarch64/include
ZMQ_LIBS = -L$(PHONELIBS)/zmq/aarch64/lib \
-l:libczmq.a -l:libzmq.a \
-lgnustl_shared
CEREAL_FLAGS = -I$(PHONELIBS)/capnp-cpp/include
CEREAL_LIBS = -L$(PHONELIBS)/capnp-cpp/aarch64/lib/ \
-l:libcapnp.a -l:libkj.a
CEREAL_OBJS = ../../cereal/gen/c/log.capnp.o
EXTRA_LIBS = -lusb
ifeq ($(ARCH),x86_64)
ZMQ_LIBS = -L$(HOME)/drive/external/zmq/lib/ \
-l:libczmq.a -l:libzmq.a
CEREAL_LIBS = -L$(HOME)/drive/external/capnp/lib/ \
-l:libcapnp.a -l:libkj.a
EXTRA_LIBS = -lusb-1.0 -lpthread
endif
OBJS = boardd.o \
log.capnp.o
DEPS := $(OBJS:.o=.d)
all: boardd
boardd: $(OBJS)
@echo "[ LINK ] $@"
$(CXX) -fPIC -o '$@' $^ \
$(CEREAL_LIBS) \
$(ZMQ_LIBS) \
$(EXTRA_LIBS)
boardd.o: boardd.cc
@echo "[ CXX ] $@"
$(CXX) $(CXXFLAGS) \
-I$(PHONELIBS)/android_system_core/include \
$(CEREAL_FLAGS) \
$(ZMQ_FLAGS) \
-I../ \
-I../../ \
-c -o '$@' '$<'
log.capnp.o: ../../cereal/gen/cpp/log.capnp.c++
@echo "[ CXX ] $@"
$(CXX) $(CXXFLAGS) $(CEREAL_FLAGS) \
-c -o '$@' '$<'
.PHONY: clean
clean:
rm -f boardd $(OBJS) $(DEPS)
-include $(DEPS)

0
selfdrive/boardd/__init__.py
Unescape View File

322
selfdrive/boardd/boardd.cc
Unescape View File

@ -0,0 +1,322 @@
#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <signal.h>
#include <unistd.h>
#include <sys/time.h>
#include <sys/cdefs.h>
#include <sys/types.h>
#include <sys/time.h>
#include <sys/resource.h>
#include <assert.h>
#include <pthread.h>
#include <zmq.h>
#include <libusb.h>
#include <capnp/serialize.h>
#include "cereal/gen/cpp/log.capnp.h"
#include "common/timing.h"
int do_exit = 0;
libusb_context *ctx = NULL;
libusb_device_handle *dev_handle;
pthread_mutex_t usb_lock;
// double the FIFO size
#define RECV_SIZE (0x1000)
#define TIMEOUT 0
#define DEBUG_BOARDD
#ifdef DEBUG_BOARDD
#define DPRINTF(fmt, ...) printf("boardd: " fmt, ## __VA_ARGS__)
#else
#define DPRINTF(fmt, ...)
#endif
bool usb_connect() {
int err;
dev_handle = libusb_open_device_with_vid_pid(ctx, 0xbbaa, 0xddcc);
if (dev_handle == NULL) { return false; }
err = libusb_set_configuration(dev_handle, 1);
if (err != 0) { return false; }
err = libusb_claim_interface(dev_handle, 0);
if (err != 0) { return false; }
return true;
}
void handle_usb_issue(int err, const char func[]) {
DPRINTF("usb error %d \"%s\" in %s\n", err, libusb_strerror((enum libusb_error)err), func);
if (err == -4) {
while (!usb_connect()) { DPRINTF("attempting to connect\n"); usleep(100*1000); }
}
// TODO: check other errors, is simply retrying okay?
}
void can_recv(void *s) {
int err;
uint32_t data[RECV_SIZE/4];
int recv;
uint32_t f1, f2;
// do recv
pthread_mutex_lock(&usb_lock);
do {
err = libusb_bulk_transfer(dev_handle, 0x81, (uint8_t*)data, RECV_SIZE, &recv, TIMEOUT);
if (err != 0) { handle_usb_issue(err, __func__); }
if (err == -8) { DPRINTF("overflow got 0x%x\n", recv); };
// timeout is okay to exit, recv still happened
if (err == -7) { break; }
} while(err != 0);
pthread_mutex_unlock(&usb_lock);
// return if length is 0
if (recv <= 0) {
return;
}
// create message
capnp::MallocMessageBuilder msg;
cereal::Event::Builder event = msg.initRoot<cereal::Event>();
event.setLogMonoTime(nanos_since_boot());
auto canData = event.initCan(recv/0x10);
// populate message
for (int i = 0; i<(recv/0x10); i++) {
if (data[i*4] & 4) {
// extended
canData[i].setAddress(data[i*4] >> 3);
//printf("got extended: %x\n", data[i*4] >> 3);
} else {
// normal
canData[i].setAddress(data[i*4] >> 21);
}
canData[i].setBusTime(data[i*4+1] >> 16);
int len = data[i*4+1]&0xF;
canData[i].setDat(kj::arrayPtr((uint8_t*)&data[i*4+2], len));
canData[i].setSrc((data[i*4+1] >> 4) & 3);
}
// send to can
auto words = capnp::messageToFlatArray(msg);
auto bytes = words.asBytes();
zmq_send(s, bytes.begin(), bytes.size(), 0);
}
void can_health(void *s) {
int cnt;
// copied from board/main.c
struct health {
uint32_t voltage;
uint32_t current;
uint8_t started;
uint8_t controls_allowed;
uint8_t gas_interceptor_detected;
} health;
// recv from board
pthread_mutex_lock(&usb_lock);
do {
cnt = libusb_control_transfer(dev_handle, 0xc0, 0xd2, 0, 0, (unsigned char*)&health, sizeof(health), TIMEOUT);
if (cnt != sizeof(health)) { handle_usb_issue(cnt, __func__); }
} while(cnt != sizeof(health));
pthread_mutex_unlock(&usb_lock);
// create message
capnp::MallocMessageBuilder msg;
cereal::Event::Builder event = msg.initRoot<cereal::Event>();
event.setLogMonoTime(nanos_since_boot());
auto healthData = event.initHealth();
// set fields
healthData.setVoltage(health.voltage);
healthData.setCurrent(health.current);
healthData.setStarted(health.started);
healthData.setControlsAllowed(health.controls_allowed);
healthData.setGasInterceptorDetected(health.gas_interceptor_detected);
// send to health
auto words = capnp::messageToFlatArray(msg);
auto bytes = words.asBytes();
zmq_send(s, bytes.begin(), bytes.size(), 0);
}
void can_send(void *s) {
int err;
// recv from sendcan
zmq_msg_t msg;
zmq_msg_init(&msg);
err = zmq_msg_recv(&msg, s, 0);
assert(err >= 0);
// format for board
auto amsg = kj::arrayPtr((const capnp::word*)zmq_msg_data(&msg), zmq_msg_size(&msg));
capnp::FlatArrayMessageReader cmsg(amsg);
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
int msg_count = event.getCan().size();
uint32_t *send = (uint32_t*)malloc(msg_count*0x10);
memset(send, 0, msg_count*0x10);
for (int i = 0; i < msg_count; i++) {
auto cmsg = event.getCan()[i];
if (cmsg.getAddress() >= 0x800) {
// extended
send[i*4] = (cmsg.getAddress() << 3) | 5;
} else {
// normal
send[i*4] = (cmsg.getAddress() << 21) | 1;
}
assert(cmsg.getDat().size() <= 8);
send[i*4+1] = cmsg.getDat().size() | (cmsg.getSrc() << 4);
memcpy(&send[i*4+2], cmsg.getDat().begin(), cmsg.getDat().size());
}
//DPRINTF("got send message: %d\n", msg_count);
// release msg
zmq_msg_close(&msg);
// send to board
int sent;
pthread_mutex_lock(&usb_lock);
do {
err = libusb_bulk_transfer(dev_handle, 3, (uint8_t*)send, msg_count*0x10, &sent, TIMEOUT);
if (err != 0 || msg_count*0x10 != sent) { handle_usb_issue(err, __func__); }
} while(err != 0);
pthread_mutex_unlock(&usb_lock);
// done
free(send);
}
// **** threads ****
void *can_send_thread(void *crap) {
DPRINTF("start send thread\n");
// sendcan = 8017
void *context = zmq_ctx_new();
void *subscriber = zmq_socket(context, ZMQ_SUB);
zmq_setsockopt(subscriber, ZMQ_SUBSCRIBE, "", 0);
zmq_connect(subscriber, "tcp://127.0.0.1:8017");
// run as fast as messages come in
while (!do_exit) {
can_send(subscriber);
}
return NULL;
}
void *can_recv_thread(void *crap) {
DPRINTF("start recv thread\n");
// can = 8006
void *context = zmq_ctx_new();
void *publisher = zmq_socket(context, ZMQ_PUB);
zmq_bind(publisher, "tcp://*:8006");
// run at ~200hz
while (!do_exit) {
can_recv(publisher);
// 5ms
usleep(5*1000);
}
return NULL;
}
void *can_health_thread(void *crap) {
DPRINTF("start health thread\n");
// health = 8011
void *context = zmq_ctx_new();
void *publisher = zmq_socket(context, ZMQ_PUB);
zmq_bind(publisher, "tcp://*:8011");
// run at 1hz
while (!do_exit) {
can_health(publisher);
usleep(1000*1000);
}
return NULL;
}
int main() {
int err;
printf("boardd: starting boardd\n");
// set process priority
err = setpriority(PRIO_PROCESS, 0, -4);
printf("boardd: setpriority returns %d\n", err);
// connect to the board
err = libusb_init(&ctx);
assert(err == 0);
libusb_set_debug(ctx, 3);
// TODO: duplicate code from error handling
while (!usb_connect()) { DPRINTF("attempting to connect\n"); usleep(100*1000); }
/*int config;
err = libusb_get_configuration(dev_handle, &config);
assert(err == 0);
DPRINTF("configuration is %d\n", config);*/
/*err = libusb_set_interface_alt_setting(dev_handle, 0, 0);
assert(err == 0);*/
// create threads
pthread_t can_health_thread_handle;
err = pthread_create(&can_health_thread_handle, NULL,
can_health_thread, NULL);
assert(err == 0);
pthread_t can_send_thread_handle;
err = pthread_create(&can_send_thread_handle, NULL,
can_send_thread, NULL);
assert(err == 0);
pthread_t can_recv_thread_handle;
err = pthread_create(&can_recv_thread_handle, NULL,
can_recv_thread, NULL);
assert(err == 0);
// join threads
err = pthread_join(can_recv_thread_handle, NULL);
assert(err == 0);
err = pthread_join(can_send_thread_handle, NULL);
assert(err == 0);
err = pthread_join(can_health_thread_handle, NULL);
assert(err == 0);
// destruct libusb
libusb_close(dev_handle);
libusb_exit(ctx);
}

179
selfdrive/boardd/boardd.py
Unescape View File

@ -0,0 +1,179 @@
#!/usr/bin/env python
import os
import struct
import zmq
import selfdrive.messaging as messaging
from common.realtime import Ratekeeper
from common.services import service_list
from selfdrive.swaglog import cloudlog
# USB is optional
try:
import usb1
from usb1 import USBErrorIO, USBErrorOverflow
except Exception:
pass
# TODO: rewrite in C to save CPU
# *** serialization functions ***
def can_list_to_can_capnp(can_msgs):
dat = messaging.new_message()
dat.init('can', len(can_msgs))
for i, can_msg in enumerate(can_msgs):
dat.can[i].address = can_msg[0]
dat.can[i].busTime = can_msg[1]
dat.can[i].dat = can_msg[2]
dat.can[i].src = can_msg[3]
return dat
def can_capnp_to_can_list_old(dat, src_filter=[]):
ret = []
for msg in dat.can:
if msg.src in src_filter:
ret.append([msg.address, msg.busTime, msg.dat.encode("hex")])
return ret
def can_capnp_to_can_list(dat):
ret = []
for msg in dat.can:
ret.append([msg.address, msg.busTime, msg.dat, msg.src])
return ret
# *** can driver ***
def can_health():
while 1:
try:
dat = handle.controlRead(usb1.TYPE_VENDOR | usb1.RECIPIENT_DEVICE, 0xd2, 0, 0, 0x10)
break
except (USBErrorIO, USBErrorOverflow):
cloudlog.exception("CAN: BAD HEALTH, RETRYING")
v, i, started = struct.unpack("IIB", dat[0:9])
# TODO: units
return {"voltage": v, "current": i, "started": bool(started)}
def __parse_can_buffer(dat):
ret = []
for j in range(0, len(dat), 0x10):
ddat = dat[j:j+0x10]
f1, f2 = struct.unpack("II", ddat[0:8])
ret.append((f1 >> 21, f2>>16, ddat[8:8+(f2&0xF)], (f2>>4)&3))
return ret
def can_send_many(arr):
snds = []
for addr, _, dat, alt in arr:
snd = struct.pack("II", ((addr << 21) | 1), len(dat) | (alt << 4)) + dat
snd = snd.ljust(0x10, '\x00')
snds.append(snd)
while 1:
try:
handle.bulkWrite(3, ''.join(snds))
break
except (USBErrorIO, USBErrorOverflow):
cloudlog.exception("CAN: BAD SEND MANY, RETRYING")
def can_recv():
dat = ""
while 1:
try:
dat = handle.bulkRead(1, 0x10*256)
break
except (USBErrorIO, USBErrorOverflow):
cloudlog.exception("CAN: BAD RECV, RETRYING")
return __parse_can_buffer(dat)
def can_init():
global handle, context
cloudlog.info("attempting can init")
context = usb1.USBContext()
#context.setDebug(9)
for device in context.getDeviceList(skip_on_error=True):
if device.getVendorID() == 0xbbaa and device.getProductID() == 0xddcc:
handle = device.open()
handle.claimInterface(0)
if handle is None:
print "CAN NOT FOUND"
exit(-1)
print "got handle"
cloudlog.info("can init done")
def boardd_mock_loop():
context = zmq.Context()
can_init()
logcan = messaging.sub_sock(context, service_list['can'].port)
while 1:
tsc = messaging.drain_sock(logcan, wait_for_one=True)
snds = map(can_capnp_to_can_list, tsc)
snd = []
for s in snds:
snd += s
snd = filter(lambda x: x[-1] <= 1, snd)
can_send_many(snd)
# recv @ 100hz
can_msgs = can_recv()
print "sent %d got %d" % (len(snd), len(can_msgs))
#print can_msgs
# *** main loop ***
def boardd_loop(rate=200):
rk = Ratekeeper(rate)
context = zmq.Context()
can_init()
# *** publishes can and health
logcan = messaging.pub_sock(context, service_list['can'].port)
health_sock = messaging.pub_sock(context, service_list['health'].port)
# *** subscribes to can send
sendcan = messaging.sub_sock(context, service_list['sendcan'].port)
while 1:
# health packet @ 1hz
if (rk.frame%rate) == 0:
health = can_health()
msg = messaging.new_message()
msg.init('health')
# store the health to be logged
msg.health.voltage = health['voltage']
msg.health.current = health['current']
msg.health.started = health['started']
health_sock.send(msg.to_bytes())
# recv @ 100hz
can_msgs = can_recv()
# publish to logger
# TODO: refactor for speed
if len(can_msgs) > 0:
dat = can_list_to_can_capnp(can_msgs)
logcan.send(dat.to_bytes())
# send can if we have a packet
tsc = messaging.recv_sock(sendcan)
if tsc is not None:
can_send_many(can_capnp_to_can_list(tsc))
rk.keep_time()
def main(gctx=None):
if os.getenv("MOCK") is not None:
boardd_mock_loop()
else:
boardd_loop()
if __name__ == "__main__":
main()

0
selfdrive/calibrationd/__init__.py
Unescape View File

208
selfdrive/calibrationd/calibration.py
Unescape View File

@ -0,0 +1,208 @@
import numpy as np
import common.filters as filters
from selfdrive.controls.lib.latcontrol import calc_curvature
# Calibration Status
class CalibStatus(object):
INCOMPLETE = 0
VALID = 1
INVALID = 2
def line_intersection(line1, line2, no_int_sub = [0,0]):
xdiff = (line1[0][0] - line1[1][0], line2[0][0] - line2[1][0])
ydiff = (line1[0][1] - line1[1][1], line2[0][1] - line2[1][1])
def det(a, b):
return a[0] * b[1] - a[1] * b[0]
div = det(xdiff, ydiff)
if div == 0:
# since we are in float domain, this should really never happen
return no_int_sub
d = (det(*line1), det(*line2))
x = det(d, xdiff) / div
y = det(d, ydiff) / div
return [x, y]
def points_inside_hit_box(pts, box):
"""Determine which points lie inside a box.
Inputs:
pts: An nx2 array of points to hit test.
box: An array [[x_left, y_top], [x_right, y_bottom]] describing a box to
use for hit testing.
Returns:
A logical array with true for every member of pts inside box.
"""
hits = np.all(np.logical_and(pts > box[0, :], pts < box[1, :]), axis=1)
return hits
def warp_points(pt_s, warp_matrix):
# pt_s are the source points, nx2 array.
pt_d = np.dot(warp_matrix[:, :2], pt_s.T) + warp_matrix[:, 2][:, np.newaxis]
# divide by third dimension for representation in image space.
return (pt_d[:2, :] / pt_d[2, :]).T
class ViewCalibrator(object):
def __init__(self, box_size, big_box_size, vp_r, warp_matrix_start, vp_f=None, cal_cycle=0, cal_status=0):
self.calibration_threshold = 3000
self.box_size = box_size
self.big_box_size = big_box_size
self.warp_matrix_start = warp_matrix_start
self.vp_r = list(vp_r)
if vp_f is None:
self.vp_f = list(vp_r)
else:
self.vp_f = list(vp_f)
# slow filter fot the vanishing point
vp_fr = 0.005 # Hz, slow filter
self.dt = 0.05 # camera runs at 20Hz
self.update_warp_matrix()
self.vp_x_filter = filters.FirstOrderLowpassFilter(vp_fr, self.dt, self.vp_f[0])
self.vp_y_filter = filters.FirstOrderLowpassFilter(vp_fr, self.dt, self.vp_f[1])
self.cal_cycle = cal_cycle
self.cal_status = cal_status
self.cal_perc = int(np.minimum(self.cal_cycle*100./self.calibration_threshold, 100))
def vanishing_point_process(self, old_ps, new_ps, v_ego, steer_angle, VP):
# correct diffs by yaw rate
cam_fov = 23.06*np.pi/180. # deg
curvature = calc_curvature(v_ego, steer_angle, VP)
yaw_rate = curvature * v_ego
hor_angle_shift = yaw_rate * self.dt * self.box_size[0] / cam_fov
old_ps += [hor_angle_shift, 0] # old points have moved in the image due to yaw rate
pos_ps = [None]*len(new_ps)
for ii in range(len(old_ps)):
xo = old_ps[ii][0]
yo = old_ps[ii][1]
yn = new_ps[ii][1]
# don't consider points with low flow in y
if abs(yn - yo) > 1:
if xo > (self.vp_f[0] + 20):
pos_ps[ii] = 'r' # right lane point
elif xo < (self.vp_f[0] - 20):
pos_ps[ii] = 'l' # left lane point
# intersect all the right lines with the left lines
idxs_l = [i for i, x in enumerate(pos_ps) if x == 'l']
idxs_r = [i for i, x in enumerate(pos_ps) if x == 'r']
old_ps_l, new_ps_l = old_ps[idxs_l], new_ps[idxs_l]
old_ps_r, new_ps_r = old_ps[idxs_r], new_ps[idxs_r]
# return None if there is one side with no lines, the speed is low or the steer angle is high
if len(old_ps_l) == 0 or len(old_ps_r) == 0 or v_ego < 20 or abs(steer_angle) > 5:
return None
int_ps = [[None] * len(old_ps_r)] * len(old_ps_l)
for ll in range(len(old_ps_l)):
for rr in range(len(old_ps_r)):
old_p_l, old_p_r, new_p_l, new_p_r = old_ps_l[ll], old_ps_r[
rr], new_ps_l[ll], new_ps_r[rr]
line_l = [[old_p_l[0], old_p_l[1]], [new_p_l[0], new_p_l[1]]]
line_r = [[old_p_r[0], old_p_r[1]], [new_p_r[0], new_p_r[1]]]
int_ps[ll][rr] = line_intersection(
line_l, line_r, no_int_sub=self.vp_f)
# saturate outliers that are too far from the estimated vp
int_ps[ll][rr][0] = np.clip(int_ps[ll][rr][0], self.vp_f[0] - 20, self.vp_f[0] + 20)
int_ps[ll][rr][1] = np.clip(int_ps[ll][rr][1], self.vp_f[1] - 30, self.vp_f[1] + 30)
vp = np.mean(np.mean(np.array(int_ps), axis=0), axis=0)
return vp
def calibration_validity(self):
# this function sanity checks that the small box is contained in the big box.
# otherwise the warp function will generate black spots on the small box
cp = np.asarray([[0, 0],
[self.box_size[0], 0],
[self.box_size[0], self.box_size[1]],
[0, self.box_size[1]]])
cpw = warp_points(cp, self.warp_matrix)
# pixel margin for validity hysteresys:
# - if calibration is good, keep it good until small box is inside the big box
# - if calibration isn't good, then make it good again if small box is in big box with margin
margin_px = 0 if self.cal_status == CalibStatus.VALID else 5
big_hit_box = np.asarray(
[[margin_px, margin_px],
[self.big_box_size[0], self.big_box_size[1] - margin_px]])
cpw_outside_big_box = np.logical_not(points_inside_hit_box(cpw, big_hit_box))
return not np.any(cpw_outside_big_box)
def get_calibration_hit_box(self):
"""Returns an axis-aligned hit box in canonical image space.
Points which do not fall within this box should not be used for
calibration.
Returns:
An array [[x_left, y_top], [x_right, y_bottom]] describing a box inside
which all calibration points should lie.
"""
# We mainly care about feature from lanes, so removed points from sky.
y_filter = 50.
return np.asarray([[0, y_filter], [self.box_size[0], self.box_size[1]]])
def update_warp_matrix(self):
translation_matrix = np.asarray(
[[1, 0, self.vp_f[0] - self.vp_r[0]],
[0, 1, self.vp_f[1] - self.vp_r[1]],
[0, 0, 1]])
self.warp_matrix = np.dot(translation_matrix, self.warp_matrix_start)
self.warp_matrix_inv = np.linalg.inv(self.warp_matrix)
def calibration(self, p0, p1, st, v_ego, steer_angle, VP):
# convert to np array first thing
p0 = np.asarray(p0)
p1 = np.asarray(p1)
st = np.asarray(st)
p0 = p0.reshape((-1,2))
p1 = p1.reshape((-1,2))
# filter out pts with bad status
p0 = p0[st==1]
p1 = p1[st==1]
calib_hit_box = self.get_calibration_hit_box()
# remove all the points outside the small box and above the horizon line
good_idxs = points_inside_hit_box(
warp_points(p0, self.warp_matrix_inv), calib_hit_box)
p0 = p0[good_idxs]
p1 = p1[good_idxs]
# print("unwarped points: {}".format(warp_points(p0, self.warp_matrix_inv)))
# print("good_idxs {}:".format(good_idxs))
# get instantaneous vp
vp = self.vanishing_point_process(p0, p1, v_ego, steer_angle, VP)
if vp is not None:
# filter the vanishing point
self.vp_f = [self.vp_x_filter(vp[0]), self.vp_y_filter(vp[1])]
self.cal_cycle += 1
if not self.calibration_validity():
self.cal_status = CalibStatus.INVALID
else:
# 10 minutes @5Hz TODO: make this threshold function of convergency speed
self.cal_status = CalibStatus.VALID
#self.cal_status = CalibStatus.VALID if self.cal_cycle > self.calibration_threshold else CalibStatus.INCOMPLETE
self.cal_perc = int(np.minimum(self.cal_cycle*100./self.calibration_threshold, 100))
self.update_warp_matrix()

116
selfdrive/calibrationd/calibrationd.py
Unescape View File

@ -0,0 +1,116 @@
#!/usr/bin/env python
import os
import numpy as np
import zmq
from common.services import service_list
import selfdrive.messaging as messaging
from selfdrive.config import ImageParams, VehicleParams
from selfdrive.calibrationd.calibration import ViewCalibrator, CalibStatus
CALIBRATION_FILE = "/sdcard/calibration_param"
def load_calibration(gctx):
# calibration initialization
I = ImageParams()
vp_guess = None
if gctx is not None:
warp_matrix_start = np.array(
gctx['calibration']["initial_homography"]).reshape(3, 3)
big_box_size = [560, 304]
else:
warp_matrix_start = np.array([[1., 0., I.SX_R],
[0., 1., I.SY_R],
[0., 0., 1.]])
big_box_size = [640, 480]
# translate the vanishing point into phone image space
vp_box = (I.VPX_R-I.SX_R, I.VPY_R-I.SY_R)
vp_trans = np.dot(warp_matrix_start, vp_box+(1.,))
vp_img = (vp_trans[0]/vp_trans[2], vp_trans[1]/vp_trans[2])
# load calibration data
if os.path.isfile(CALIBRATION_FILE):
# if the calibration file exist, start from the last cal values
with open(CALIBRATION_FILE, "r") as cal_file:
data = [float(l.strip()) for l in cal_file.readlines()]
calib = ViewCalibrator((I.X, I.Y),
big_box_size,
vp_img,
warp_matrix_start,
vp_f=[data[2], data[3]],
cal_cycle=data[0],
cal_status=data[1])
if calib.cal_status == CalibStatus.INCOMPLETE:
print "CALIBRATION IN PROGRESS", calib.cal_cycle
else:
print "NO CALIBRATION FILE"
calib = ViewCalibrator((I.X, I.Y),
big_box_size,
vp_img,
warp_matrix_start,
vp_f=vp_guess)
return calib
def calibrationd_thread(gctx):
context = zmq.Context()
features = messaging.sub_sock(context, service_list['features'].port)
live100 = messaging.sub_sock(context, service_list['live100'].port)
livecalibration = messaging.pub_sock(context, service_list['liveCalibration'].port)
# subscribe to stats about the car
VP = VehicleParams(False)
v_ego = None
calib = load_calibration(gctx)
last_cal_cycle = calib.cal_cycle
while 1:
# calibration at the end so it does not delay radar processing above
ft = messaging.recv_sock(features, wait=True)
# get latest here
l100 = messaging.recv_sock(live100)
if l100 is not None:
v_ego = l100.live100.vEgo
steer_angle = l100.live100.angleSteers
if v_ego is None:
continue
p0 = ft.features.p0
p1 = ft.features.p1
st = ft.features.status
calib.calibration(p0, p1, st, v_ego, steer_angle, VP)
# write a new calibration every 100 cal cycle
if calib.cal_cycle - last_cal_cycle >= 100:
print "writing cal", calib.cal_cycle
with open(CALIBRATION_FILE, "w") as cal_file:
cal_file.write(str(calib.cal_cycle)+'\n')
cal_file.write(str(calib.cal_status)+'\n')
cal_file.write(str(calib.vp_f[0])+'\n')
cal_file.write(str(calib.vp_f[1])+'\n')
last_cal_cycle = calib.cal_cycle
warp_matrix = map(float, calib.warp_matrix.reshape(9).tolist())
dat = messaging.new_message()
dat.init('liveCalibration')
dat.liveCalibration.warpMatrix = warp_matrix
dat.liveCalibration.calStatus = calib.cal_status
dat.liveCalibration.calCycle = calib.cal_cycle
dat.liveCalibration.calPerc = calib.cal_perc
livecalibration.send(dat.to_bytes())
def main(gctx=None):
calibrationd_thread(gctx)
if __name__ == "__main__":
main()

135
selfdrive/common/framebuffer.cc
Unescape View File

@ -0,0 +1,135 @@
#include <cstdio>
#include <cstdlib>
#include <cassert>
#include <ui/DisplayInfo.h>
#include <gui/ISurfaceComposer.h>
#include <gui/Surface.h>
#include <gui/SurfaceComposerClient.h>
#include <GLES2/gl2.h>
#include <EGL/eglext.h>
#define BACKLIGHT_CONTROL "/sys/class/leds/lcd-backlight/brightness"
#define BACKLIGHT_LEVEL "205"
using namespace android;
struct FramebufferState {
sp<SurfaceComposerClient> session;
sp<IBinder> dtoken;
DisplayInfo dinfo;
sp<SurfaceControl> control;
sp<Surface> s;
EGLDisplay display;
EGLint egl_major, egl_minor;
EGLConfig config;
EGLSurface surface;
EGLContext context;
};
extern "C" FramebufferState* framebuffer_init(
const char* name, int32_t layer,
EGLDisplay *out_display, EGLSurface *out_surface,
int *out_w, int *out_h) {
status_t status;
int success;
FramebufferState *s = new FramebufferState;
s->session = new SurfaceComposerClient();
assert(s->session != NULL);
s->dtoken = SurfaceComposerClient::getBuiltInDisplay(
ISurfaceComposer::eDisplayIdMain);
assert(s->dtoken != NULL);
status = SurfaceComposerClient::getDisplayInfo(s->dtoken, &s->dinfo);
assert(status == 0);
int orientation = 3; // rotate framebuffer 270 degrees
if(orientation == 1 || orientation == 3) {
int temp = s->dinfo.h;
s->dinfo.h = s->dinfo.w;
s->dinfo.w = temp;
}
printf("dinfo %dx%d\n", s->dinfo.w, s->dinfo.h);
Rect destRect(s->dinfo.w, s->dinfo.h);
s->session->setDisplayProjection(s->dtoken, orientation, destRect, destRect);
s->control = s->session->createSurface(String8(name),
s->dinfo.w, s->dinfo.h, PIXEL_FORMAT_RGBX_8888);
assert(s->control != NULL);
SurfaceComposerClient::openGlobalTransaction();
status = s->control->setLayer(layer);
SurfaceComposerClient::closeGlobalTransaction();
assert(status == 0);
s->s = s->control->getSurface();
assert(s->s != NULL);
// init opengl and egl
const EGLint attribs[] = {
EGL_RED_SIZE, 8,
EGL_GREEN_SIZE, 8,
EGL_BLUE_SIZE, 8,
EGL_DEPTH_SIZE, 0,
EGL_STENCIL_SIZE, 8,
EGL_RENDERABLE_TYPE, EGL_OPENGL_ES3_BIT_KHR,
EGL_NONE,
};
s->display = eglGetDisplay(EGL_DEFAULT_DISPLAY);
assert(s->display != EGL_NO_DISPLAY);
success = eglInitialize(s->display, &s->egl_major, &s->egl_minor);
assert(success);
printf("egl version %d.%d\n", s->egl_major, s->egl_minor);
EGLint num_configs;
success = eglChooseConfig(s->display, attribs, &s->config, 1, &num_configs);
assert(success);
s->surface = eglCreateWindowSurface(s->display, s->config, s->s.get(), NULL);
assert(s->surface != EGL_NO_SURFACE);
const EGLint context_attribs[] = {
EGL_CONTEXT_CLIENT_VERSION, 3,
EGL_NONE,
};
s->context = eglCreateContext(s->display, s->config, NULL, context_attribs);
assert(s->context != EGL_NO_CONTEXT);
EGLint w, h;
eglQuerySurface(s->display, s->surface, EGL_WIDTH, &w);
eglQuerySurface(s->display, s->surface, EGL_HEIGHT, &h);
printf("egl w %d h %d\n", w, h);
success = eglMakeCurrent(s->display, s->surface, s->surface, s->context);
assert(success);
printf("gl version %s\n", glGetString(GL_VERSION));
// set brightness
int brightness_fd = open(BACKLIGHT_CONTROL, O_RDWR);
const char brightness_level[] = BACKLIGHT_LEVEL;
write(brightness_fd, brightness_level, strlen(brightness_level));
if (out_display) *out_display = s->display;
if (out_surface) *out_surface = s->surface;
if (out_w) *out_w = w;
if (out_h) *out_h = h;
return s;
}

21
selfdrive/common/framebuffer.h
Unescape View File

@ -0,0 +1,21 @@
#ifndef FRAMEBUFFER_H
#define FRAMEBUFFER_H
#include <EGL/eglext.h>
#ifdef __cplusplus
extern "C" {
#endif
typedef struct FramebufferState FramebufferState;
FramebufferState* framebuffer_init(
const char* name, int32_t layer,
EGLDisplay *out_display, EGLSurface *out_surface,
int *out_w, int *out_h);
#ifdef __cplusplus
}
#endif
#endif

68
selfdrive/common/mat.h
Unescape View File

@ -0,0 +1,68 @@
#ifndef COMMON_MAT_H
#define COMMON_MAT_H
typedef struct vec3 {
float v[3];
} vec3;
typedef struct vec4 {
float v[4];
} vec4;
typedef struct mat3 {
float v[3*3];
} mat3;
typedef struct mat4 {
float v[4*4];
} mat4;
static inline mat3 matmul3(const mat3 a, const mat3 b) {
mat3 ret = {{0.0}};
for (int r=0; r<3; r++) {
for (int c=0; c<3; c++) {
float v = 0.0;
for (int k=0; k<3; k++) {
v += a.v[r*3+k] * b.v[k*3+c];
}
ret.v[r*3+c] = v;
}
}
return ret;
}
static inline vec3 matvecmul3(const mat3 a, const vec3 b) {
vec3 ret = {{0.0}};
for (int r=0; r<3; r++) {
for (int c=0; c<3; c++) {
ret.v[r] += a.v[r*3+c] * b.v[c];
}
}
return ret;
}
static inline mat4 matmul(const mat4 a, const mat4 b) {
mat4 ret = {{0.0}};
for (int r=0; r<4; r++) {
for (int c=0; c<4; c++) {
float v = 0.0;
for (int k=0; k<4; k++) {
v += a.v[r*4+k] * b.v[k*4+c];
}
ret.v[r*4+c] = v;
}
}
return ret;
}
static inline vec4 matvecmul(const mat4 a, const vec4 b) {
vec4 ret = {{0.0}};
for (int r=0; r<4; r++) {
for (int c=0; c<4; c++) {
ret.v[r] += a.v[r*4+c] * b.v[c];
}
}
return ret;
}
#endif

23
selfdrive/common/modeldata.h
Unescape View File

@ -0,0 +1,23 @@
#ifndef MODELDATA_H
#define MODELDATA_H
typedef struct PathData {
float points[50];
float prob;
float std;
} PathData;
typedef struct LeadData {
float dist;
float prob;
float std;
} LeadData;
typedef struct ModelData {
PathData path;
PathData left_lane;
PathData right_lane;
LeadData lead;
} ModelData;
#endif

13
selfdrive/common/mutex.h
Unescape View File

@ -0,0 +1,13 @@
#ifndef COMMON_MUTEX_H
#define COMMON_MUTEX_H
#include <pthread.h>
static inline void mutex_init_reentrant(pthread_mutex_t *mutex) {
pthread_mutexattr_t attr;
pthread_mutexattr_init(&attr);
pthread_mutexattr_settype(&attr, PTHREAD_MUTEX_RECURSIVE);
pthread_mutex_init(mutex, &attr);
}
#endif

90
selfdrive/common/swaglog.c
Unescape View File

@ -0,0 +1,90 @@
#include <stdio.h>
#include <stdlib.h>
#include <stdarg.h>
#include <stdbool.h>
#include <string.h>
#include <assert.h>
#include <pthread.h>
#include <zmq.h>
#include <json.h>
#include "common/timing.h"
#include "swaglog.h"
typedef struct LogState {
pthread_mutex_t lock;
bool inited;
JsonNode *ctx_j;
void *zctx;
void *sock;
} LogState;
static LogState s = {
.lock = PTHREAD_MUTEX_INITIALIZER,
};
static void cloudlog_init() {
if (s.inited) return;
s.ctx_j = json_mkobject();
s.zctx = zmq_ctx_new();
s.sock = zmq_socket(s.zctx, ZMQ_PUSH);
zmq_connect(s.sock, "ipc:///tmp/logmessage");
s.inited = true;
}
void cloudlog_e(int levelnum, const char* filename, int lineno, const char* func, const char* srctime,
const char* fmt, ...) {
pthread_mutex_lock(&s.lock);
cloudlog_init();
char* msg_buf = NULL;
va_list args;
va_start(args, fmt);
vasprintf(&msg_buf, fmt, args);
va_end(args);
if (!msg_buf) {
pthread_mutex_unlock(&s.lock);
return;
}
if (levelnum >= CLOUDLOG_PRINT_LEVEL) {
printf("%s: %s\n", filename, msg_buf);
}
JsonNode *log_j = json_mkobject();
assert(log_j);
json_append_member(log_j, "msg", json_mkstring(msg_buf));
json_append_member(log_j, "ctx", s.ctx_j);
json_append_member(log_j, "levelnum", json_mknumber(levelnum));
json_append_member(log_j, "filename", json_mkstring(filename));
json_append_member(log_j, "lineno", json_mknumber(lineno));
json_append_member(log_j, "funcname", json_mkstring(func));
json_append_member(log_j, "srctime", json_mkstring(srctime));
json_append_member(log_j, "created", json_mknumber(seconds_since_epoch()));
char* log_s = json_encode(log_j);
assert(log_s);
json_remove_from_parent(s.ctx_j);
json_delete(log_j);
free(msg_buf);
char levelnum_c = levelnum;
zmq_send(s.sock, &levelnum_c, 1, ZMQ_NOBLOCK | ZMQ_SNDMORE);
zmq_send(s.sock, log_s, strlen(log_s), ZMQ_NOBLOCK);
free(log_s);
pthread_mutex_unlock(&s.lock);
}
void cloudlog_bind(const char* k, const char* v) {
pthread_mutex_lock(&s.lock);
cloudlog_init();
json_append_member(s.ctx_j, k, json_mkstring(v));
pthread_mutex_unlock(&s.lock);
}

26
selfdrive/common/swaglog.h
Unescape View File

@ -0,0 +1,26 @@
#ifndef SWAGLOG_H
#define SWAGLOG_H
#define CLOUDLOG_DEBUG 10
#define CLOUDLOG_INFO 20
#define CLOUDLOG_WARNING 30
#define CLOUDLOG_ERROR 40
#define CLOUDLOG_CRITICAL 50
#define CLOUDLOG_PRINT_LEVEL CLOUDLOG_WARNING
void cloudlog_e(int levelnum, const char* filename, int lineno, const char* func, const char* srctime,
const char* fmt, ...) /*__attribute__ ((format (printf, 6, 7)))*/;
void cloudlog_bind(const char* k, const char* v);
#define cloudlog(lvl, fmt, ...) cloudlog_e(lvl, __FILE__, __LINE__, \
__func__, __DATE__ " " __TIME__, \
fmt, ## __VA_ARGS__)
#define LOGD(fmt, ...) cloudlog(CLOUDLOG_DEBUG, fmt, ## __VA_ARGS__)
#define LOG(fmt, ...) cloudlog(CLOUDLOG_INFO, fmt, ## __VA_ARGS__)
#define LOGW(fmt, ...) cloudlog(CLOUDLOG_WARNING, fmt, ## __VA_ARGS__)
#define LOGE(fmt, ...) cloudlog(CLOUDLOG_ERROR, fmt, ## __VA_ARGS__)
#endif

31
selfdrive/common/timing.h
Unescape View File

@ -0,0 +1,31 @@
#ifndef COMMON_TIMING_H
#define COMMON_TIMING_H
#include <stdint.h>
#include <time.h>
static inline uint64_t nanos_since_boot() {
struct timespec t;
clock_gettime(CLOCK_BOOTTIME, &t);
return t.tv_sec * 1000000000ULL + t.tv_nsec;
}
static inline double millis_since_boot() {
struct timespec t;
clock_gettime(CLOCK_BOOTTIME, &t);
return t.tv_sec * 1000.0 + t.tv_nsec / 1000000.0;
}
static inline uint64_t nanos_since_epoch() {
struct timespec t;
clock_gettime(CLOCK_REALTIME, &t);
return t.tv_sec * 1000000000ULL + t.tv_nsec;
}
static inline double seconds_since_epoch() {
struct timespec t;
clock_gettime(CLOCK_REALTIME, &t);
return (double)t.tv_sec + t.tv_nsec / 1000000000.0;
}
#endif

22
selfdrive/common/util.h
Unescape View File

@ -0,0 +1,22 @@
#ifndef COMMON_UTIL_H
#define COMMON_UTIL_H
#define min(a,b) \
({ __typeof__ (a) _a = (a); \
__typeof__ (b) _b = (b); \
_a < _b ? _a : _b; })
#define max(a,b) \
({ __typeof__ (a) _a = (a); \
__typeof__ (b) _b = (b); \
_a > _b ? _a : _b; })
#define clamp(a,b,c) \
({ __typeof__ (a) _a = (a); \
__typeof__ (b) _b = (b); \
__typeof__ (c) _c = (c); \
_a < _b ? _b : (_a > _c ? _c : _a); })
#define ARRAYSIZE(x) (sizeof(x)/sizeof(x[0]))
#endif

127
selfdrive/common/visionipc.c
Unescape View File

@ -0,0 +1,127 @@
#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include <unistd.h>
#include <assert.h>
#include <errno.h>
#include <sys/socket.h>
#include <sys/un.h>
#include "visionipc.h"
typedef struct VisionPacketWire {
int type;
VisionPacketData d;
} VisionPacketWire;
int vipc_connect() {
int err;
int sock = socket(AF_UNIX, SOCK_SEQPACKET, 0);
assert(sock >= 0);
struct sockaddr_un addr = {
.sun_family = AF_UNIX,
.sun_path = VIPC_SOCKET_PATH,
};
err = connect(sock, (struct sockaddr*)&addr, sizeof(addr));
if (err != 0) {
close(sock);
return -1;
}
return sock;
}
static int sendrecv_with_fds(bool send, int fd, void *buf, size_t buf_size, int* fds, int num_fds,
int *out_num_fds) {
int err;
char control_buf[CMSG_SPACE(sizeof(int) * num_fds)];
memset(control_buf, 0, CMSG_SPACE(sizeof(int) * num_fds));
struct iovec iov = {
.iov_base = buf,
.iov_len = buf_size,
};
struct msghdr msg = {
.msg_iov = &iov,
.msg_iovlen = 1,
};
if (num_fds > 0) {
assert(fds);
msg.msg_control = control_buf;
msg.msg_controllen = CMSG_SPACE(sizeof(int) * num_fds);
}
if (send) {
if (num_fds) {
struct cmsghdr *cmsg = CMSG_FIRSTHDR(&msg);
assert(cmsg);
cmsg->cmsg_level = SOL_SOCKET;
cmsg->cmsg_type = SCM_RIGHTS;
cmsg->cmsg_len = CMSG_LEN(sizeof(int) * num_fds);
memcpy(CMSG_DATA(cmsg), fds, sizeof(int) * num_fds);
// printf("send clen %d -> %d\n", num_fds, cmsg->cmsg_len);
}
return sendmsg(fd, &msg, 0);
} else {
int r = recvmsg(fd, &msg, 0);
if (r < 0) return r;
int recv_fds = 0;
if (msg.msg_controllen > 0) {
struct cmsghdr *cmsg = CMSG_FIRSTHDR(&msg);
assert(cmsg);
assert(cmsg->cmsg_level == SOL_SOCKET && cmsg->cmsg_type == SCM_RIGHTS);
recv_fds = (cmsg->cmsg_len - CMSG_LEN(0));
assert(recv_fds > 0 && (recv_fds % sizeof(int)) == 0);
recv_fds /= sizeof(int);
// printf("recv clen %d -> %d\n", cmsg->cmsg_len, recv_fds);
// assert(cmsg->cmsg_len == CMSG_LEN(sizeof(int) * num_fds));
assert(fds && recv_fds <= num_fds);
memcpy(fds, CMSG_DATA(cmsg), sizeof(int) * recv_fds);
}
if (msg.msg_flags) {
for (int i=0; i<recv_fds; i++) {
close(fds[i]);
}
return -1;
}
if (fds) {
assert(out_num_fds);
*out_num_fds = recv_fds;
}
return r;
}
}
int vipc_recv(int fd, VisionPacket *out_p) {
VisionPacketWire p = {0};
VisionPacket p2 = {0};
int ret = sendrecv_with_fds(false, fd, &p, sizeof(p), (int*)p2.fds, VIPC_MAX_FDS, &p2.num_fds);
if (ret < 0) {
printf("vipc_recv err: %s\n", strerror(errno));
} else {
p2.type = p.type;
p2.d = p.d;
*out_p = p2;
}
return ret;
}
int vipc_send(int fd, const VisionPacket p2) {
assert(p2.num_fds <= VIPC_MAX_FDS);
VisionPacketWire p = {
.type = p2.type,
.d = p2.d,
};
return sendrecv_with_fds(true, fd, (void*)&p, sizeof(p), (int*)p2.fds, p2.num_fds, NULL);
}

50
selfdrive/common/visionipc.h
Unescape View File

@ -0,0 +1,50 @@
#ifndef VISIONIPC_H
#define VISIONIPC_H
#define VIPC_SOCKET_PATH "/tmp/vision_socket"
#define VIPC_MAX_FDS 64
#define VISION_INVALID 0
#define VISION_UI_SUBSCRIBE 1
#define VISION_UI_BUFS 2
#define VISION_UI_ACQUIRE 3
#define VISION_UI_RELEASE 4
typedef struct VisionUIBufs {
int width, height, stride;
int front_width, front_height, front_stride;
int big_box_x, big_box_y;
int big_box_width, big_box_height;
int transformed_width, transformed_height;
int front_box_x, front_box_y;
int front_box_width, front_box_height;
size_t buf_len;
int num_bufs;
size_t front_buf_len;
int num_front_bufs;
} VisionUIBufs;
typedef union VisionPacketData {
VisionUIBufs ui_bufs;
struct {
bool front;
int idx;
} ui_acq, ui_rel;
} VisionPacketData;
typedef struct VisionPacket {
int type;
VisionPacketData d;
int num_fds;
int fds[VIPC_MAX_FDS];
} VisionPacket;
int vipc_connect();
int vipc_recv(int fd, VisionPacket *out_p);
int vipc_send(int fd, const VisionPacket p);
#endif

68
selfdrive/config.py
Unescape View File

@ -0,0 +1,68 @@
import numpy as np
class Conversions:
MPH_TO_MS = 1.609/3.6
MS_TO_MPH = 3.6/1.609
KPH_TO_MS = 1./3.6
MS_TO_KPH = 3.6
MPH_TO_KPH = 1.609
KPH_TO_MPH = 1./1.609
KNOTS_TO_MS = 1/1.9438
MS_TO_KNOTS = 1.9438
# Car tecode decimal minutes into decimal degrees, can work with numpy arrays as input
@staticmethod
def dm2d(dm):
degs = np.round(dm/100.)
mins = dm - degs*100.
return degs + mins/60.
# Car button codes
class CruiseButtons:
RES_ACCEL = 4
DECEL_SET = 3
CANCEL = 2
MAIN = 1
# Image params for color cam on acura, calibrated on pre las vegas drive (2016-05-21)
class ImageParams:
def __init__(self):
self.SX_R = 160 # top left corner pixel shift of the visual region considered by the model
self.SY_R = 180 # top left corner pixel shift of the visual region considered by the model
self.VPX_R = 319 # vanishing point reference, as calibrated in Vegas drive
self.VPY_R = 201 # vanishing point reference, as calibrated in Vegas drive
self.X = 320 # pixel length of image for model
self.Y = 160 # pixel length of image for model
self.SX = self.SX_R # current visual region with shift
self.SY = self.SY_R # current visual region with shift
self.VPX = self.VPX_R # current vanishing point with shift
self.VPY = self.VPY_R # current vanishing point with shift
def shift(self, shift):
def to_int(fl):
return int(round(fl))
# shift comes from calibration and says how much to shift the viual region
self.SX = self.SX_R + to_int(shift[0]) # current visual region with shift
self.SY = self.SY_R + to_int(shift[1]) # current visual region with shift
self.VPX = self.VPX_R + to_int(shift[0]) # current vanishing point with shift
self.VPY = self.VPY_R + to_int(shift[1]) # current vanishing point with shift
class UIParams:
lidar_x, lidar_y, lidar_zoom = 384, 960, 8
lidar_car_x, lidar_car_y = lidar_x/2., lidar_y/1.1
car_hwidth = 1.7272/2 * lidar_zoom
car_front = 2.6924 * lidar_zoom
car_back = 1.8796 * lidar_zoom
car_color = 110
class VehicleParams:
def __init__(self, civic):
if civic:
self.wheelbase = 2.67
self.steer_ratio = 15.3
self.slip_factor = 0.0014
else:
self.wheelbase = 2.67 # from http://www.edmunds.com/acura/ilx/2016/sedan/features-specs/
self.steer_ratio = 15.3 # from http://www.edmunds.com/acura/ilx/2016/road-test-specs/
self.slip_factor = 0.0014

0
selfdrive/controls/__init__.py
Unescape View File

358
selfdrive/controls/controlsd.py
Unescape View File

@ -0,0 +1,358 @@
#!/usr/bin/env python
import zmq
import numpy as np
from common.services import service_list
from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper
from selfdrive.config import CruiseButtons
from selfdrive.config import Conversions as CV
from selfdrive.controls.lib.drive_helpers import learn_angle_offset
from selfdrive.controls.lib.alert_database import process_alert, AI
import selfdrive.messaging as messaging
from selfdrive.controls.lib.carstate import CarState
from selfdrive.controls.lib.carcontroller import CarController
from selfdrive.controls.lib.longcontrol import LongControl
from selfdrive.controls.lib.latcontrol import LatControl
from selfdrive.controls.lib.pathplanner import PathPlanner
from selfdrive.controls.lib.adaptivecruise import AdaptiveCruise
def controlsd_thread(gctx, rate=100): #rate in Hz
# *** log ***
context = zmq.Context()
live100 = messaging.pub_sock(context, service_list['live100'].port)
thermal = messaging.sub_sock(context, service_list['thermal'].port)
live20 = messaging.sub_sock(context, service_list['live20'].port)
model = messaging.sub_sock(context, service_list['model'].port)
logcan = messaging.sub_sock(context, service_list['can'].port)
sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
# *** init the major players ***
CS = CarState(logcan)
CC = CarController()
PP = PathPlanner(model)
AC = AdaptiveCruise(live20)
LoC = LongControl()
LaC = LatControl()
# *** control initial values ***
apply_brake = 0
enabled = False
# *** time values ***
last_enable_pressed = 0
# *** controls initial values ***
# *** display stuff
soft_disable_start = 0
sounding = False
no_mismatch_pcm_last, no_mismatch_ctrl_last = 0, 0
# car state
alert, sound_exp, hud_exp, text_exp, alert_p = None, 0, 0, 0, 0
rear_view_cam, rear_view_toggle = False, False
v_cruise = 255 # this means no display
v_cruise_max = 144
v_cruise_min = 8
v_cruise_delta = 8
# on activation target at least 25mph. With 5mph you need too much tapping
v_cruise_enable_min = 40
hud_v_cruise = 255
angle_offset = 0
max_enable_speed = 57. # ~91 mph
pcm_threshold = 25.*CV.MPH_TO_MS # below this speed pcm cancels
overtemp = True
# 0.0 - 1.0
awareness_status = 0.0
# start the loop
set_realtime_priority(2)
rk = Ratekeeper(rate)
while 1:
cur_time = sec_since_boot()
# read CAN
canMonoTimes = CS.update(logcan)
# **** rearview mirror management ***
if CS.cruise_setting == 1 and CS.prev_cruise_setting == 0:
rear_view_toggle = not rear_view_toggle
# show rear view camera on phone if in reverse gear or when lkas button is pressed
rear_view_cam = (CS.gear_shifter == 2) or rear_view_toggle or CS.blinker_on
# *** thermal checking logic ***
# thermal data, checked every second
td = messaging.recv_sock(thermal)
if td is not None:
cpu_temps = [td.thermal.cpu0, td.thermal.cpu1, td.thermal.cpu2,
td.thermal.cpu3, td.thermal.mem, td.thermal.gpu]
# check overtemp
overtemp = any(t > 950 for t in cpu_temps)
# *** getting model logic ***
PP.update(cur_time, CS.v_ego)
if rk.frame % 5 == 2:
# *** run this at 20hz again ***
angle_offset = learn_angle_offset(enabled, CS.v_ego, angle_offset, np.asarray(PP.d_poly), LaC.y_des, CS.steer_override)
# to avoid race conditions, check if control has been disabled for at least 0.2s
mismatch_ctrl = not CC.controls_allowed and enabled
mismatch_pcm = (not CS.pcm_acc_status and (not apply_brake or CS.v_ego < 0.1)) and enabled
# keep resetting start timer if mismatch isn't true
if not mismatch_ctrl:
no_mismatch_ctrl_last = cur_time
if not mismatch_pcm or not CS.brake_only:
no_mismatch_pcm_last = cur_time
#*** v_cruise logic ***
if CS.brake_only:
v_cruise = int(CS.v_cruise_pcm) # TODO: why sometimes v_cruise_pcm is long type?
else:
if CS.prev_cruise_buttons == 0 and CS.cruise_buttons == CruiseButtons.RES_ACCEL and enabled:
v_cruise = v_cruise - (v_cruise % v_cruise_delta) + v_cruise_delta
elif CS.prev_cruise_buttons == 0 and CS.cruise_buttons == CruiseButtons.DECEL_SET and enabled:
v_cruise = v_cruise + (v_cruise % v_cruise_delta) - v_cruise_delta
# *** enabling/disabling logic ***
enable_pressed = (CS.prev_cruise_buttons == CruiseButtons.DECEL_SET or CS.prev_cruise_buttons == CruiseButtons.RES_ACCEL) \
and CS.cruise_buttons == 0
if enable_pressed:
print "enabled pressed at", cur_time
last_enable_pressed = cur_time
# if pcm does speed control than we need to wait on pcm to enable
if CS.brake_only:
enable_condition = (cur_time - last_enable_pressed) < 0.2 and CS.pcm_acc_status
else:
enable_condition = enable_pressed
# always clear the alert at every cycle
alert_id = []
# check for PCM not enabling
if CS.brake_only and (cur_time - last_enable_pressed) < 0.2 and not CS.pcm_acc_status:
print "waiting for PCM to enable"
# check for denied enabling
if enable_pressed and not enabled:
deny_enable = \
[(AI.SEATBELT, not CS.seatbelt),
(AI.DOOR_OPEN, not CS.door_all_closed),
(AI.ESP_OFF, CS.esp_disabled),
(AI.STEER_ERROR, CS.steer_error),
(AI.BRAKE_ERROR, CS.brake_error),
(AI.GEAR_NOT_D, not CS.gear_shifter_valid),
(AI.MAIN_OFF, not CS.main_on),
(AI.PEDAL_PRESSED, CS.user_gas_pressed or CS.brake_pressed or (CS.pedal_gas > 0 and CS.brake_only)),
(AI.HIGH_SPEED, CS.v_ego > max_enable_speed),
(AI.OVERHEAT, overtemp),
(AI.COMM_ISSUE, PP.dead or AC.dead),
(AI.CONTROLSD_LAG, rk.remaining < -0.2)]
for alertn, cond in deny_enable:
if cond:
alert_id += [alertn]
# check for soft disables
if enabled:
soft_disable = \
[(AI.SEATBELT_SD, not CS.seatbelt),
(AI.DOOR_OPEN_SD, not CS.door_all_closed),
(AI.ESP_OFF_SD, CS.esp_disabled),
(AI.OVERHEAT_SD, overtemp),
(AI.COMM_ISSUE_SD, PP.dead or AC.dead),
(AI.CONTROLSD_LAG_SD, rk.remaining < -0.2)]
sounding = False
for alertn, cond in soft_disable:
if cond:
alert_id += [alertn]
sounding = True
# soft disengagement expired, user need to take control
if (cur_time - soft_disable_start) > 3.:
enabled = False
v_cruise = 255
if not sounding:
soft_disable_start = cur_time
# check for immediate disables
if enabled:
immediate_disable = \
[(AI.PCM_LOW_SPEED, (cur_time > no_mismatch_pcm_last > 0.2) and CS.v_ego < pcm_threshold),
(AI.STEER_ERROR_ID, CS.steer_error),
(AI.BRAKE_ERROR_ID, CS.brake_error),
(AI.CTRL_MISMATCH_ID, (cur_time - no_mismatch_ctrl_last) > 0.2),
(AI.PCM_MISMATCH_ID, (cur_time - no_mismatch_pcm_last) > 0.2)]
for alertn, cond in immediate_disable:
if cond:
alert_id += [alertn]
# immediate turn off control
enabled = False
v_cruise = 255
# user disabling
if enabled and (CS.user_gas_pressed or CS.brake_pressed or not CS.gear_shifter_valid or \
(CS.cruise_buttons == CruiseButtons.CANCEL and CS.prev_cruise_buttons == 0) or \
not CS.main_on or (CS.pedal_gas > 0 and CS.brake_only)):
enabled = False
v_cruise = 255
alert_id += [AI.DISABLE]
# enabling
if enable_condition and not enabled and len(alert_id) == 0:
print "*** enabling controls"
#enable both lateral and longitudinal controls
enabled = True
counter_pcm_enabled = CS.counter_pcm
# on activation, let's always set v_cruise from where we are, even if PCM ACC is active
# what we want to be displayed in mph
v_cruise_mph = round(CS.v_ego * CV.MS_TO_MPH * CS.ui_speed_fudge)
# what we need to send to have that displayed
v_cruise = int(round(np.maximum(v_cruise_mph * CV.MPH_TO_KPH, v_cruise_enable_min)))
# 6 minutes driver you're on
awareness_status = 1.0
# reset the PID loops
LaC.reset()
# start long control at actual speed
LoC.reset(v_pid = CS.v_ego)
alert_id += [AI.ENABLE]
if v_cruise != 255 and not CS.brake_only:
v_cruise = np.clip(v_cruise, v_cruise_min, v_cruise_max)
# **** awareness status manager ****
if enabled:
# gives the user 6 minutes
awareness_status -= 1.0/(100*60*6)
# reset on steering, blinker, or cruise buttons
if CS.steer_override or CS.blinker_on or CS.cruise_buttons or CS.cruise_setting:
awareness_status = 1.0
if awareness_status <= 0.:
alert_id += [AI.DRIVER_DISTRACTED]
# ****** initial actuators commands ***
# *** gas/brake PID loop ***
AC.update(cur_time, CS.v_ego, CS.angle_steers, LoC.v_pid, awareness_status, CS.VP)
final_gas, final_brake = LoC.update(enabled, CS, v_cruise, AC.v_target_lead, AC.a_target, AC.jerk_factor)
pcm_accel = int(np.clip(AC.a_pcm/1.4,0,1)*0xc6) # TODO: perc of max accel in ACC?
# *** steering PID loop ***
final_steer, sat_flag = LaC.update(enabled, CS, PP.d_poly, angle_offset)
# this needs to stay before hysteresis logic to avoid pcm staying on control during brake hysteresis
pcm_override = True # this is always True
pcm_cancel_cmd = False
if CS.brake_only and final_brake == 0.:
pcm_speed = LoC.v_pid - .3 # FIXME: just for exp
else:
pcm_speed = 0
# ***** handle alerts ****
# send a "steering required alert" if saturation count has reached the limit
if sat_flag:
alert_id += [AI.STEER_SATURATED]
# process the alert, based on id
alert, chime, beep, hud_alert, alert_text, sound_exp, hud_exp, text_exp, alert_p = \
process_alert(alert_id, alert, cur_time, sound_exp, hud_exp, text_exp, alert_p)
# alerts pub
if len(alert_id) != 0:
print alert_id, alert_text
# *** process for hud display ***
if not enabled or (hud_v_cruise == 255 and CS.counter_pcm == counter_pcm_enabled):
hud_v_cruise = 255
else:
hud_v_cruise = v_cruise
# *** actually do can sends ***
CC.update(sendcan, enabled, CS, rk.frame, \
final_gas, final_brake, final_steer, \
pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \
hud_v_cruise, hud_show_lanes = enabled, \
hud_show_car = AC.has_lead, \
hud_alert = hud_alert, \
snd_beep = beep, snd_chime = chime)
# ***** publish state to logger *****
# publish controls state at 100Hz
dat = messaging.new_message()
dat.init('live100')
# move liveUI into live100
dat.live100.rearViewCam = bool(rear_view_cam)
dat.live100.alertText1 = alert_text[0]
dat.live100.alertText2 = alert_text[1]
dat.live100.awarenessStatus = max(awareness_status, 0.0) if enabled else 0.0
# what packets were used to process
dat.live100.canMonoTimes = canMonoTimes
dat.live100.mdMonoTime = PP.logMonoTime
dat.live100.l20MonoTime = AC.logMonoTime
# if controls is enabled
dat.live100.enabled = enabled
# car state
dat.live100.vEgo = float(CS.v_ego)
dat.live100.aEgo = float(CS.a_ego)
dat.live100.angleSteers = float(CS.angle_steers)
dat.live100.hudLead = CS.hud_lead
dat.live100.steerOverride = CS.steer_override
# longitudinal control state
dat.live100.vPid = float(LoC.v_pid)
dat.live100.vCruise = float(v_cruise)
dat.live100.upAccelCmd = float(LoC.Up_accel_cmd)
dat.live100.uiAccelCmd = float(LoC.Ui_accel_cmd)
# lateral control state
dat.live100.yActual = float(LaC.y_actual)
dat.live100.yDes = float(LaC.y_des)
dat.live100.upSteer = float(LaC.Up_steer)
dat.live100.uiSteer = float(LaC.Ui_steer)
# processed radar state, should add a_pcm?
dat.live100.vTargetLead = float(AC.v_target_lead)
dat.live100.aTargetMin = float(AC.a_target[0])
dat.live100.aTargetMax = float(AC.a_target[1])
dat.live100.jerkFactor = float(AC.jerk_factor)
# lag
dat.live100.cumLagMs = -rk.remaining*1000.
live100.send(dat.to_bytes())
# *** run loop at fixed rate ***
rk.keep_time()
def main(gctx=None):
controlsd_thread(gctx, 100)
if __name__ == "__main__":
main()

0
selfdrive/controls/lib/__init__.py
Unescape View File

332
selfdrive/controls/lib/adaptivecruise.py
Unescape View File

@ -0,0 +1,332 @@
import selfdrive.messaging as messaging
import numpy as np
# lookup tables VS speed to determine min and max accels in cruise
_A_CRUISE_MIN_V = np.asarray([-1.0, -.8, -.67, -.5, -.30])
_A_CRUISE_MIN_BP = np.asarray([ 0., 5., 10., 20., 40.])
# need fast accel at very low speed for stop and go
_A_CRUISE_MAX_V = np.asarray([1., 1., .8, .5, .30])
_A_CRUISE_MAX_BP = np.asarray([0., 5., 10., 20., 40.])
def calc_cruise_accel_limits(v_ego):
a_cruise_min = np.interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V)
a_cruise_max = np.interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V)
a_pcm = 1. # always 1 for now
return np.vstack([a_cruise_min, a_cruise_max]), a_pcm
_A_TOTAL_MAX_V = np.asarray([1.5, 1.9, 3.2])
_A_TOTAL_MAX_BP = np.asarray([0., 20., 40.])
def limit_accel_in_turns(v_ego, angle_steers, a_target, a_pcm, VP):
#*** this function returns a limited long acceleration allowed, depending on the existing lateral acceleration
# this should avoid accelerating when losing the target in turns
deg_to_rad = np.pi / 180. # from can reading to rad
a_total_max = np.interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V)
a_y = v_ego**2 * angle_steers * deg_to_rad / (VP.steer_ratio * VP.wheelbase)
a_x_allowed = np.sqrt(np.maximum(a_total_max**2 - a_y**2, 0.))
a_target[1] = np.minimum(a_target[1], a_x_allowed)
a_pcm = np.minimum(a_pcm, a_x_allowed)
return a_target, a_pcm
def process_a_lead(a_lead):
# soft threshold of 0.5m/s^2 applied to a_lead to reject noise, also not considered positive a_lead
a_lead_threshold = 0.5
a_lead = np.minimum(a_lead + a_lead_threshold, 0)
return a_lead
def calc_desired_distance(v_lead):
#*** compute desired distance ***
t_gap = 1.7 # good to be far away
d_offset = 4 # distance when at zero speed
return d_offset + v_lead * t_gap
#linear slope
_L_SLOPE_V = np.asarray([0.40, 0.10])
_L_SLOPE_BP = np.asarray([0., 40])
# parabola slope
_P_SLOPE_V = np.asarray([1.0, 0.25])
_P_SLOPE_BP = np.asarray([0., 40])
def calc_desired_speed(d_lead, d_des, v_lead, a_lead):
#*** compute desired speed ***
# the desired speed curve is divided in 4 portions:
# 1-constant
# 2-linear to regain distance
# 3-linear to shorten distance
# 4-parabolic (constant decel)
max_runaway_speed = -2. # no slower than 2m/s over the lead
# interpolate the lookups to find the slopes for a give lead speed
l_slope = np.interp(v_lead, _L_SLOPE_BP, _L_SLOPE_V)
p_slope = np.interp(v_lead, _P_SLOPE_BP, _P_SLOPE_V)
# this is where parabola and linear curves are tangents
x_linear_to_parabola = p_slope / l_slope**2
# parabola offset to have the parabola being tangent to the linear curve
x_parabola_offset = p_slope / (2 * l_slope**2)
if d_lead < d_des:
# calculate v_rel_des on the line that connects 0m at max_runaway_speed to d_des
v_rel_des_1 = (- max_runaway_speed) / d_des * (d_lead - d_des)
# calculate v_rel_des on one third of the linear slope
v_rel_des_2 = (d_lead - d_des) * l_slope / 3.
# take the min of the 2 above
v_rel_des = np.minimum(v_rel_des_1, v_rel_des_2)
v_rel_des = np.maximum(v_rel_des, max_runaway_speed)
elif d_lead < d_des + x_linear_to_parabola:
v_rel_des = (d_lead - d_des) * l_slope
v_rel_des = np.maximum(v_rel_des, max_runaway_speed)
else:
v_rel_des = np.sqrt(2 * (d_lead - d_des - x_parabola_offset) * p_slope)
# compute desired speed
v_target = v_rel_des + v_lead
# compute v_coast: above this speed we want to coast
t_lookahead = 1. # how far in time we consider a_lead to anticipate the coast region
v_coast_shift = np.maximum(a_lead * t_lookahead, - v_lead) # don't consider projections that would make v_lead<0
v_coast = (v_lead + v_target)/2 + v_coast_shift # no accel allowed above this line
v_coast = np.minimum(v_coast, v_target)
return v_target, v_coast
def calc_critical_decel(d_lead, v_rel, d_offset, v_offset):
# this function computes the required decel to avoid crashing, given safety offsets
a_critical = - np.maximum(0., v_rel + v_offset)**2/np.maximum(2*(d_lead - d_offset), 0.5)
return a_critical
# maximum acceleration adjustment
_A_CORR_BY_SPEED_V = np.asarray([0.4, 0.4, 0])
# speeds
_A_CORR_BY_SPEED_BP = np.asarray([0., 5., 20.])
def calc_positive_accel_limit(d_lead, d_des, v_ego, v_rel, v_ref, v_rel_ref, v_coast, v_target, a_lead_contr, a_max):
a_coast_min = -1.0 # never coast faster then -1m/s^2
# coasting behavior above v_coast. Forcing a_max to be negative will force the pid_speed to decrease,
# regardless v_target
if v_ref > np.minimum(v_coast, v_target):
# for smooth coast we can be agrressive and target a point where car would actually crash
v_offset_coast = 0.
d_offset_coast = d_des/2. - 4.
# acceleration value to smoothly coast until we hit v_target
if d_lead > d_offset_coast + 0.1:
a_coast = calc_critical_decel(d_lead, v_rel_ref, d_offset_coast, v_offset_coast)
# if lead is decelerating, then offset the coast decel
a_coast += a_lead_contr
a_max = np.maximum(a_coast, a_coast_min)
else:
a_max = a_coast_min
else:
# same as cruise accel, but add a small correction based on lead acceleration at low speeds
# when lead car accelerates faster, we can do the same, and vice versa
a_max = a_max + np.interp(v_ego, _A_CORR_BY_SPEED_BP, _A_CORR_BY_SPEED_V) \
* np.clip(-v_rel / 4., -.5, 1)
return a_max
# arbitrary limits to avoid too high accel being computed
_A_SAT = np.asarray([-10., 5.])
# do not consider a_lead at 0m/s, fully consider it at 10m/s
_A_LEAD_LOW_SPEED_V = np.asarray([0., 1.])
# speed break points
_A_LEAD_LOW_SPEED_BP = np.asarray([0., 10.])
# add a small offset to the desired decel, just for safety margin
_DECEL_OFFSET_V = np.asarray([-0.3, -0.5, -0.5, -0.4, -0.3])
# speed bp: different offset based on the likelyhood that lead decels abruptly
_DECEL_OFFSET_BP = np.asarray([0., 4., 15., 30, 40.])
def calc_acc_accel_limits(d_lead, d_des, v_ego, v_pid, v_lead, v_rel, a_lead,
v_target, v_coast, a_target, a_pcm):
#*** compute max accel ***
# v_rel is now your velocity in lead car frame
v_rel = -v_rel # this simplifiess things when thinking in d_rel-v_rel diagram
v_rel_pid = v_pid - v_lead
# this is how much lead accel we consider in assigning the desired decel
a_lead_contr = a_lead * np.interp(v_lead, _A_LEAD_LOW_SPEED_BP,
_A_LEAD_LOW_SPEED_V) * 0.8
# first call of calc_positive_accel_limit is used to shape v_pid
a_target[1] = calc_positive_accel_limit(d_lead, d_des, v_ego, v_rel, v_pid,
v_rel_pid, v_coast, v_target,
a_lead_contr, a_target[1])
# second call of calc_positive_accel_limit is used to limit the pcm throttle
# control (only useful when we don't control throttle directly)
a_pcm = calc_positive_accel_limit(d_lead, d_des, v_ego, v_rel, v_ego, v_rel,
v_coast, v_target, a_lead_contr, a_pcm)
#*** compute max decel ***
v_offset = 1. # assume the car is 1m/s slower
d_offset = 1. # assume the distance is 1m lower
if v_target - v_ego > 0.5:
pass # acc target speed is above vehicle speed, so we can use the cruise limits
elif d_lead > d_offset + 0.01: # add small value to avoid by zero divisions
# compute needed accel to get to 1m distance with -1m/s rel speed
decel_offset = np.interp(v_lead, _DECEL_OFFSET_BP, _DECEL_OFFSET_V)
critical_decel = calc_critical_decel(d_lead, v_rel, d_offset, v_offset)
a_target[0] = np.minimum(decel_offset + critical_decel + a_lead_contr,
a_target[0])
else:
a_target[0] = _A_SAT[0]
# a_min can't be higher than a_max
a_target[0] = np.minimum(a_target[0], a_target[1])
# final check on limits
a_target = np.clip(a_target, _A_SAT[0], _A_SAT[1])
a_target = a_target.tolist()
return a_target, a_pcm
def calc_jerk_factor(d_lead, v_rel):
# we don't have an explicit jerk limit, so this function calculates a factor
# that is used by the PID controller to scale the gains. Not the cleanest solution
# but we need this for the demo.
# TODO: Calculate Kp and Ki directly in this function.
# the higher is the decel required to avoid a crash, the higher is the PI factor scaling
d_offset = 0.5
v_offset = 2.
a_offset = 1.
jerk_factor_max = 1.0 # can't increase Kp and Ki more than double.
if d_lead < d_offset + 0.1: # add small value to avoid by zero divisions
jerk_factor = jerk_factor_max
else:
a_critical = - calc_critical_decel(d_lead, -v_rel, d_offset, v_offset)
# increase Kp and Ki by 20% for every 1m/s2 of decel required above 1m/s2
jerk_factor = np.maximum(a_critical - a_offset, 0.)/5.
jerk_factor = np.minimum(jerk_factor, jerk_factor_max)
return jerk_factor
def calc_ttc(d_rel, v_rel, a_rel, v_lead):
# this function returns the time to collision (ttc), assuming that a_rel will stay constant
# TODO: Review these assumptions.
# change sign to rel quantities as it's going to be easier for calculations
v_rel = -v_rel
a_rel = -a_rel
# assuming that closing gap a_rel comes from lead vehicle decel, then limit a_rel so that v_lead will get to zero in no sooner than t_decel
# this helps overweighting a_rel when v_lead is close to zero.
t_decel = 2.
a_rel = np.minimum(a_rel, v_lead/t_decel)
delta = v_rel**2 + 2 * d_rel * a_rel
# assign an arbitrary high ttc value if there is no solution to ttc
if delta < 0.1:
ttc = 5.
elif np.sqrt(delta) + v_rel < 0.1:
ttc = 5.
else:
ttc = 2 * d_rel / (np.sqrt(delta) + v_rel)
return ttc
def limit_accel_driver_awareness(v_ego, a_target, a_pcm, awareness_status):
decel_bp = [0. , 40.]
decel_v = [-0.3, -0.2]
decel = np.interp(v_ego, decel_bp, decel_v)
# gives 18 seconds before decel begins (w 6 minute timeout)
if awareness_status < -0.05:
a_target[1] = np.minimum(a_target[1], decel)
a_target[0] = np.minimum(a_target[1], a_target[0])
a_pcm = 0.
return a_target, a_pcm
MAX_SPEED_POSSIBLE = 55.
def compute_speed_with_leads(v_ego, angle_steers, v_pid, l1, l2, awareness_status, VP):
# drive limits
# TODO: Make lims function of speed (more aggressive at low speed).
a_lim = [-3., 1.5]
#*** set target speed pretty high, as lead hasn't been considered yet
v_target_lead = MAX_SPEED_POSSIBLE
#*** set accel limits as cruise accel/decel limits ***
a_target, a_pcm = calc_cruise_accel_limits(v_ego)
#*** limit max accel in sharp turns
a_target, a_pcm = limit_accel_in_turns(v_ego, angle_steers, a_target, a_pcm, VP)
jerk_factor = 0.
if l1 is not None and l1.status:
#*** process noisy a_lead signal from radar processing ***
a_lead_p = process_a_lead(l1.aLeadK)
#*** compute desired distance ***
d_des = calc_desired_distance(l1.vLead)
#*** compute desired speed ***
v_target_lead, v_coast = calc_desired_speed(l1.dRel, d_des, l1.vLead, a_lead_p)
if l2 is not None and l2.status:
#*** process noisy a_lead signal from radar processing ***
a_lead_p2 = process_a_lead(l2.aLeadK)
#*** compute desired distance ***
d_des2 = calc_desired_distance(l2.vLead)
#*** compute desired speed ***
v_target_lead2, v_coast2 = calc_desired_speed(l2.dRel, d_des2, l2.vLead, a_lead_p2)
# listen to lead that makes you go slower
if v_target_lead2 < v_target_lead:
l1 = l2
d_des, a_lead_p, v_target_lead, v_coast = d_des2, a_lead_p2, v_target_lead2, v_coast2
# l1 is the main lead now
#*** compute accel limits ***
a_target1, a_pcm1 = calc_acc_accel_limits(l1.dRel, d_des, v_ego, v_pid, l1.vLead,
l1.vRel, a_lead_p, v_target_lead, v_coast, a_target, a_pcm)
# we can now limit a_target to a_lim
a_target = np.clip(a_target1, a_lim[0], a_lim[1])
a_pcm = np.clip(a_pcm1, a_lim[0], a_lim[1]).tolist()
#*** compute max factor ***
jerk_factor = calc_jerk_factor(l1.dRel, l1.vRel)
# force coasting decel if driver hasn't been controlling car in a while
a_target, a_pcm = limit_accel_driver_awareness(v_ego, a_target, a_pcm, awareness_status)
return v_target_lead, a_target, a_pcm, jerk_factor
class AdaptiveCruise(object):
def __init__(self, live20):
self.live20 = live20
self.last_cal = 0.
self.l1, self.l2 = None, None
self.logMonoTime = 0
self.dead = True
def update(self, cur_time, v_ego, angle_steers, v_pid, awareness_status, VP):
l20 = messaging.recv_sock(self.live20)
if l20 is not None:
self.l1 = l20.live20.leadOne
self.l2 = l20.live20.leadTwo
self.logMonoTime = l20.logMonoTime
# TODO: no longer has anything to do with calibration
self.last_cal = cur_time
self.dead = False
elif cur_time - self.last_cal > 0.5:
self.dead = True
self.v_target_lead, self.a_target, self.a_pcm, self.jerk_factor = \
compute_speed_with_leads(v_ego, angle_steers, v_pid, self.l1, self.l2, awareness_status, VP)
self.has_lead = self.v_target_lead != MAX_SPEED_POSSIBLE

178
selfdrive/controls/lib/alert_database.py
Unescape View File

@ -0,0 +1,178 @@
alerts = []
keys = ["id",
"chime",
"beep",
"hud_alert",
"screen_chime",
"priority",
"text_line_1",
"text_line_2",
"duration_sound",
"duration_hud_alert",
"duration_text"]
#car chimes: enumeration from dbc file. Chimes are for alerts and warnings
class CM:
MUTE = 0
SINGLE = 3
DOUBLE = 4
REPEATED = 1
CONTINUOUS = 2
#car beepss: enumeration from dbc file. Beeps are for activ and deactiv
class BP:
MUTE = 0
SINGLE = 3
TRIPLE = 2
REPEATED = 1
# lert ids
class AI:
ENABLE = 0
DISABLE = 1
SEATBELT = 2
DOOR_OPEN = 3
PEDAL_PRESSED = 4
COMM_ISSUE = 5
ESP_OFF = 6
FCW = 7
STEER_ERROR = 8
BRAKE_ERROR = 9
CALIB_INCOMPLETE = 10
CALIB_INVALID = 11
GEAR_NOT_D = 12
MAIN_OFF = 13
STEER_SATURATED = 14
PCM_LOW_SPEED = 15
THERMAL_DEAD = 16
OVERHEAT = 17
HIGH_SPEED = 18
CONTROLSD_LAG = 19
STEER_ERROR_ID = 100
BRAKE_ERROR_ID = 101
PCM_MISMATCH_ID = 102
CTRL_MISMATCH_ID = 103
SEATBELT_SD = 200
DOOR_OPEN_SD = 201
COMM_ISSUE_SD = 202
ESP_OFF_SD = 203
THERMAL_DEAD_SD = 204
OVERHEAT_SD = 205
CONTROLSD_LAG_SD = 206
CALIB_INCOMPLETE_SD = 207
CALIB_INVALID_SD = 208
DRIVER_DISTRACTED = 300
class AH:
#[alert_idx, value]
# See dbc files for info on values"
NONE = [0, 0]
FCW = [1, 0x8]
STEER = [2, 1]
BRAKE_PRESSED = [3, 10]
GEAR_NOT_D = [4, 6]
SEATBELT = [5, 5]
SPEED_TOO_HIGH = [6, 8]
class ET:
ENABLE = 0
NO_ENTRY = 1
WARNING = 2
SOFT_DISABLE = 3
IMMEDIATE_DISABLE = 4
USER_DISABLE = 5
def process_alert(alert_id, alert, cur_time, sound_exp, hud_exp, text_exp, alert_p):
# INPUTS:
# alert_id is mapped to the alert properties in alert_database
# cur_time is current time
# sound_exp is when the alert beep/chime is supposed to end
# hud_exp is when the hud visual is supposed to end
# text_exp is when the alert text is supposed to disappear
# alert_p is the priority of the current alert
# CM, BP, AH are classes defined in alert_database and they respresents chimes, beeps and hud_alerts
if len(alert_id) > 0:
# take the alert with higher priority
alerts_present = filter(lambda a_id: a_id['id'] in alert_id, alerts)
alert = sorted(alerts_present, key=lambda k: k['priority'])[-1]
# check if we have a more important alert
if alert['priority'] > alert_p:
alert_p = alert['priority']
sound_exp = cur_time + alert['duration_sound']
hud_exp = cur_time + alert['duration_hud_alert']
text_exp = cur_time + alert['duration_text']
chime = CM.MUTE
beep = BP.MUTE
if cur_time < sound_exp:
chime = alert['chime']
beep = alert['beep']
hud_alert = AH.NONE
if cur_time < hud_exp:
hud_alert = alert['hud_alert']
alert_text = ["", ""]
if cur_time < text_exp:
alert_text = [alert['text_line_1'], alert['text_line_2']]
if chime == CM.MUTE and beep == BP.MUTE and hud_alert == AH.NONE: #and alert_text[0] is None and alert_text[1] is None:
alert_p = 0
return alert, chime, beep, hud_alert, alert_text, sound_exp, hud_exp, text_exp, alert_p
def process_hud_alert(hud_alert):
# initialize to no alert
fcw_display = 0
steer_required = 0
acc_alert = 0
if hud_alert == AH.NONE: # no alert
pass
elif hud_alert == AH.FCW: # FCW
fcw_display = hud_alert[1]
elif hud_alert == AH.STEER: # STEER
steer_required = hud_alert[1]
else: # any other ACC alert
acc_alert = hud_alert[1]
return fcw_display, steer_required, acc_alert
def app_alert(alert_add):
alerts.append(dict(zip(keys, alert_add)))
app_alert([AI.ENABLE, CM.MUTE, BP.SINGLE, AH.NONE, ET.ENABLE, 2, "", "", .2, 0., 0.])
app_alert([AI.DISABLE, CM.MUTE, BP.SINGLE, AH.NONE, ET.USER_DISABLE, 2, "", "", .2, 0., 0.])
app_alert([AI.SEATBELT, CM.DOUBLE, BP.MUTE, AH.SEATBELT, ET.NO_ENTRY, 1, "Comma Unavailable", "Seatbelt Unlatched", .4, 2., 3.])
app_alert([AI.DOOR_OPEN, CM.DOUBLE, BP.MUTE, AH.NONE, ET.NO_ENTRY, 1, "Comma Unavailable", "Door Open", .4, 0., 3.])
app_alert([AI.PEDAL_PRESSED, CM.DOUBLE, BP.MUTE, AH.BRAKE_PRESSED, ET.NO_ENTRY, 1, "Comma Unavailable", "Pedal Pressed", .4, 2., 3.])
app_alert([AI.COMM_ISSUE, CM.DOUBLE, BP.MUTE, AH.NONE, ET.NO_ENTRY, 1, "Comma Unavailable", "Communcation Issues", .4, 0., 3.])
app_alert([AI.ESP_OFF, CM.DOUBLE, BP.MUTE, AH.NONE, ET.NO_ENTRY, 1, "Comma Unavailable", "ESP Off", .4, 0., 3.])
app_alert([AI.FCW, CM.REPEATED, BP.MUTE, AH.FCW, ET.WARNING, 3, "Risk of Collision", "", 1., 2., 3.])
app_alert([AI.STEER_ERROR, CM.DOUBLE, BP.MUTE, AH.NONE, ET.NO_ENTRY, 1, "Comma Unavailable", "Steer Error", .4, 0., 3.])
app_alert([AI.BRAKE_ERROR, CM.DOUBLE, BP.MUTE, AH.NONE, ET.NO_ENTRY, 1, "Comma Unavailable", "Brake Error", .4, 0., 3.])
app_alert([AI.CALIB_INCOMPLETE, CM.DOUBLE, BP.MUTE, AH.NONE, ET.NO_ENTRY, 1, "Comma Unavailable", "Calibration in Progress", .4, 0., 3.])
app_alert([AI.CALIB_INVALID, CM.DOUBLE, BP.MUTE, AH.NONE, ET.NO_ENTRY, 1, "Comma Unavailable", "Calibration Error", .4, 0., 3.])
app_alert([AI.GEAR_NOT_D, CM.DOUBLE, BP.MUTE, AH.GEAR_NOT_D, ET.NO_ENTRY, 1, "Comma Unavailable", "Gear not in D", .4, 2., 3.])
app_alert([AI.MAIN_OFF, CM.MUTE, BP.MUTE, AH.NONE, ET.NO_ENTRY, 1, "Comma Unavailable", "Main Switch Off", .4, 0., 3.])
app_alert([AI.STEER_SATURATED, CM.SINGLE, BP.MUTE, AH.STEER, ET.WARNING, 2, "Take Control", "Steer Control Saturated", 1., 2., 3.])
app_alert([AI.PCM_LOW_SPEED, CM.MUTE, BP.SINGLE, AH.STEER, ET.WARNING, 2, "Comma disengaged", "Speed too low", .2, 2., 3.])
app_alert([AI.THERMAL_DEAD, CM.DOUBLE, BP.MUTE, AH.NONE, ET.NO_ENTRY, 1, "Comma Unavailable", "Thermal Unavailable", .4, 0., 3.])
app_alert([AI.OVERHEAT, CM.DOUBLE, BP.MUTE, AH.NONE, ET.NO_ENTRY, 1, "Comma Unavailable", "System Overheated", .4, 0., 3.])
app_alert([AI.HIGH_SPEED, CM.DOUBLE, BP.MUTE, AH.SPEED_TOO_HIGH, ET.NO_ENTRY, 1, "Comma Unavailable", "Speed Too High", .4, 2., 3.])
app_alert([AI.CONTROLSD_LAG, CM.DOUBLE, BP.MUTE, AH.NONE, ET.NO_ENTRY, 1, "Comma Unavailable", "Controls Lagging", .4, 0., 3.])
app_alert([AI.STEER_ERROR_ID, CM.REPEATED, BP.MUTE, AH.STEER, ET.IMMEDIATE_DISABLE, 3, "Take Control Immediately", "Steer Error", 1., 3., 3.])
app_alert([AI.BRAKE_ERROR_ID, CM.REPEATED, BP.MUTE, AH.STEER, ET.IMMEDIATE_DISABLE, 3, "Take Control Immediately", "Brake Error", 1., 3., 3.])
app_alert([AI.PCM_MISMATCH_ID, CM.REPEATED, BP.MUTE, AH.STEER, ET.IMMEDIATE_DISABLE, 3, "Take Control Immediately", "Pcm Mismatch", 1., 3., 3.])
app_alert([AI.CTRL_MISMATCH_ID, CM.REPEATED, BP.MUTE, AH.STEER, ET.IMMEDIATE_DISABLE, 3, "Take Control Immediately", "Ctrl Mismatch", 1., 3., 3.])
app_alert([AI.SEATBELT_SD, CM.REPEATED, BP.MUTE, AH.STEER, ET.SOFT_DISABLE, 3, "Take Control Immediately", "Seatbelt Unlatched", 1., 3., 3.])
app_alert([AI.DOOR_OPEN_SD, CM.REPEATED, BP.MUTE, AH.STEER, ET.SOFT_DISABLE, 3, "Take Control Immediately", "Door Open", 1., 3., 3.])
app_alert([AI.COMM_ISSUE_SD, CM.REPEATED, BP.MUTE, AH.STEER, ET.SOFT_DISABLE, 3, "Take Control Immediately", "Technical Issues", 1., 3., 3.])
app_alert([AI.ESP_OFF_SD, CM.REPEATED, BP.MUTE, AH.STEER, ET.SOFT_DISABLE, 3, "Take Control Immediately", "ESP Off", 1., 3., 3.])
app_alert([AI.THERMAL_DEAD_SD, CM.REPEATED, BP.MUTE, AH.STEER, ET.SOFT_DISABLE, 3, "Take Control Immediately", "Thermal Unavailable", 1., 3., 3.])
app_alert([AI.OVERHEAT_SD, CM.REPEATED, BP.MUTE, AH.STEER, ET.SOFT_DISABLE, 3, "Take Control Immediately", "System Overheated", 1., 3., 3.])
app_alert([AI.CONTROLSD_LAG_SD, CM.REPEATED, BP.MUTE, AH.STEER, ET.SOFT_DISABLE, 3, "Take Control Immediately", "Controls Lagging", 1., 3., 3.])
app_alert([AI.CALIB_INCOMPLETE_SD, CM.REPEATED, BP.MUTE, AH.STEER, ET.SOFT_DISABLE, 3, "Take Control Immediately", "Calibration in Progress", 1., 3., 3.])
app_alert([AI.CALIB_INVALID_SD, CM.REPEATED, BP.MUTE, AH.STEER, ET.SOFT_DISABLE, 3, "Take Control Immediately", "Calibration Error", 1., 3., 3.])
app_alert([AI.DRIVER_DISTRACTED, CM.REPEATED, BP.MUTE, AH.STEER, ET.SOFT_DISABLE, 2, "Take Control to Regain Speed", "User Distracted", 1., 1., 1.])

123
selfdrive/controls/lib/can_parser.py
Unescape View File

@ -0,0 +1,123 @@
import os
import dbcs
from collections import defaultdict
from selfdrive.controls.lib.hondacan import fix
from common.realtime import sec_since_boot
from common.dbc import dbc
class CANParser(object):
def __init__(self, dbc_f, signals, checks=[]):
### input:
# dbc_f : dbc file
# signals : List of tuples (name, address, ival) where
# - name is the signal name.
# - address is the corresponding message address.
# - ival is the initial value.
# checks : List of pairs (address, frequency) where
# - address is the message address of a message for which health should be
# monitored.
# - frequency is the frequency at which health should be monitored.
self.msgs_ck = [check[0] for check in checks]
self.frqs = [check[1] for check in checks]
self.can_valid = False # start with False CAN assumption
self.msgs_upd = [] # list of updated messages
# list of received msg we want to monitor counter and checksum for
# read dbc file
self.can_dbc = dbc(os.path.join(dbcs.DBC_PATH, dbc_f))
# initialize variables to initial values
self.vl = {} # signal values
self.ts = {} # time stamp recorded in log
self.ct = {} # current time stamp
self.ok = {} # valid message?
self.cn = {} # message counter
self.cn_vl = {} # message counter mismatch value
self.ck = {} # message checksum status
for _, addr, _ in signals:
self.vl[addr] = {}
self.ts[addr] = 0
self.ct[addr] = sec_since_boot()
self.ok[addr] = False
self.cn[addr] = 0
self.cn_vl[addr] = 0
self.ck[addr] = False
for name, addr, ival in signals:
self.vl[addr][name] = ival
self._msgs = [s[1] for s in signals]
self._sgs = [s[0] for s in signals]
self._message_indices = defaultdict(list)
for i, x in enumerate(self._msgs):
self._message_indices[x].append(i)
def update_can(self, can_recv):
self.msgs_upd = []
cn_vl_max = 5 # no more than 5 wrong counter checks
# we are subscribing to PID_XXX, else data from USB
for msg, ts, cdat in can_recv:
idxs = self._message_indices[msg]
if idxs:
self.msgs_upd += [msg]
# read the entire message
out = self.can_dbc.decode([msg, 0, cdat])[1]
# checksum check
self.ck[msg] = True
if "CHECKSUM" in out.keys() and msg in self.msgs_ck:
# remove checksum (half byte)
ck_portion = (''.join((cdat[:-1], '0'))).decode('hex')
# recalculate checksum
msg_vl = fix(ck_portion, msg)
# compare recalculated vs received checksum
if msg_vl != cdat.decode('hex'):
print hex(msg), "CHECKSUM FAIL"
self.ck[msg] = False
self.ok[msg] = False
# counter check
cn = 0
if "COUNTER" in out.keys():
cn = out["COUNTER"]
# check counter validity if it's a relevant message
if cn != ((self.cn[msg] + 1) % 4) and msg in self.msgs_ck and "COUNTER" in out.keys():
#print hex(msg), "FAILED COUNTER!"
self.cn_vl[msg] += 1 # counter check failed
else:
self.cn_vl[msg] -= 1 # counter check passed
# message status is invalid if we received too many wrong counter values
if self.cn_vl[msg] >= cn_vl_max:
self.ok[msg] = False
# update msg time stamps and counter value
self.ts[msg] = ts
self.ct[msg] = sec_since_boot()
self.cn[msg] = cn
self.cn_vl[msg] = min(max(self.cn_vl[msg], 0), cn_vl_max)
# set msg valid status if checksum is good and wrong counter counter is zero
if self.ck[msg] and self.cn_vl[msg] == 0:
self.ok[msg] = True
# update value of signals in the
for ii in idxs:
sg = self._sgs[ii]
self.vl[msg][sg] = out[sg]
# for each message, check if it's too long since last time we received it
self._check_dead_msgs()
# assess overall can validity: if there is one relevant message invalid, then set can validity flag to False
self.can_valid = True
if False in self.ok.values():
print "CAN INVALID!"
self.can_valid = False
def _check_dead_msgs(self):
### input:
## simple stuff for now: msg is not valid if a message isn't received for 10 consecutive steps
for msg in set(self._msgs):
if msg in self.msgs_ck and sec_since_boot() - self.ct[msg] > 10./self.frqs[self.msgs_ck.index(msg)]:
self.ok[msg] = False

185
selfdrive/controls/lib/carcontroller.py
Unescape View File

@ -0,0 +1,185 @@
from collections import namedtuple
import common.numpy_fast as np
import selfdrive.controls.lib.hondacan as hondacan
from common.realtime import sec_since_boot
from selfdrive.config import CruiseButtons
from selfdrive.boardd.boardd import can_list_to_can_capnp
from selfdrive.controls.lib.alert_database import process_hud_alert
from selfdrive.controls.lib.drive_helpers import actuator_hystereses, rate_limit
HUDData = namedtuple("HUDData",
["pcm_accel", "v_cruise", "X2", "car", "X4", "X5",
"lanes", "beep", "X8", "chime", "acc_alert"])
class CarController(object):
def __init__(self):
self.controls_allowed = False
self.mismatch_start, self.pcm_mismatch_start = 0, 0
self.braking = False
self.brake_steady = 0.
self.final_brake_last = 0.
def update(self, sendcan, enabled, CS, frame, final_gas, final_brake, final_steer, \
pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \
hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert, \
snd_beep, snd_chime):
""" Controls thread """
# *** apply brake hysteresis ***
final_brake, self.braking, self.brake_steady = actuator_hystereses(final_brake, self.braking, self.brake_steady, CS.v_ego, CS.civic)
# *** no output if not enabled ***
if not enabled:
final_gas = 0.
final_brake = 0.
final_steer = 0.
# send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
if CS.pcm_acc_status:
pcm_cancel_cmd = True
# *** rate limit after the enable check ***
final_brake = rate_limit(final_brake, self.final_brake_last, -2., 1./100)
self.final_brake_last = final_brake
# vehicle hud display, wait for one update from 10Hz 0x304 msg
#TODO: use enum!!
if hud_show_lanes:
hud_lanes = 0x04
else:
hud_lanes = 0x00
# TODO: factor this out better
if enabled:
if hud_show_car:
hud_car = 0xe0
else:
hud_car = 0xd0
else:
hud_car = 0xc0
#print chime, alert_id, hud_alert
fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert)
hud = HUDData(pcm_accel, hud_v_cruise, 0x41, hud_car,
0xc1, 0x41, hud_lanes + steer_required,
snd_beep, 0x48, (snd_chime << 5) + fcw_display, acc_alert)
if not all(isinstance(x, int) and 0 <= x < 256 for x in hud):
print "INVALID HUD", hud
hud = HUDData(0xc6, 255, 64, 0xc0, 209, 0x41, 0x40, 0, 0x48, 0, 0)
# **** process the car messages ****
user_brake_ctrl = CS.user_brake/0.015625 # FIXME: factor needed to convert to old scale
# *** compute control surfaces ***
tt = sec_since_boot()
GAS_MAX = 1004
BRAKE_MAX = 1024/4
#STEER_MAX = 0xF00 if not CS.torque_mod else 0xF00/4 # ilx has 8x steering torque limit, used as a 2x
STEER_MAX = 0xF00 # ilx has 8x steering torque limit, used as a 2x
GAS_OFFSET = 328
# steer torque is converted back to CAN reference (positive when steering right)
apply_gas = int(np.clip(final_gas*GAS_MAX, 0, GAS_MAX-1))
apply_brake = int(np.clip(final_brake*BRAKE_MAX, 0, BRAKE_MAX-1))
apply_steer = int(np.clip(-final_steer*STEER_MAX, -STEER_MAX, STEER_MAX))
# no gas if you are hitting the brake or the user is
if apply_gas > 0 and (apply_brake != 0 or user_brake_ctrl > 10):
print "CANCELLING GAS", apply_brake, user_brake_ctrl
apply_gas = 0
# no computer brake if the user is hitting the gas
# if the computer is trying to brake, it can't be hitting the gas
# TODO: car_gas can override brakes without canceling... this is bad
if CS.car_gas > 0 and apply_brake != 0:
apply_brake = 0
if (CS.prev_cruise_buttons == CruiseButtons.DECEL_SET or CS.prev_cruise_buttons == CruiseButtons.RES_ACCEL) and \
CS.cruise_buttons == 0 and not self.controls_allowed:
print "CONTROLS ARE LIVE"
self.controls_allowed = True
# to avoid race conditions, check if control has been disabled for at least 0.2s
# keep resetting start timer if mismatch isn't true
if not (self.controls_allowed and not enabled):
self.mismatch_start = tt
# to avoid race conditions, check if control is disabled but pcm control is on for at least 0.2s
if not (not self.controls_allowed and CS.pcm_acc_status):
self.pcm_mismatch_start = tt
# something is very wrong, since pcm control is active but controls should not be allowed; TODO: send pcm fault cmd?
if (tt - self.pcm_mismatch_start) > 0.2:
pcm_cancel_cmd = True
# TODO: clean up gear condition, ideally only D (and P for debug) shall be valid gears
if (CS.cruise_buttons == CruiseButtons.CANCEL or CS.brake_pressed or
CS.user_gas_pressed or (tt - self.mismatch_start) > 0.2 or
not CS.main_on or not CS.gear_shifter_valid or
(CS.pedal_gas > 0 and CS.brake_only)) and self.controls_allowed:
self.controls_allowed = False
# 5 is a permanent fault, no torque request will be fullfilled
if CS.steer_error:
print "STEER ERROR"
self.controls_allowed = False
# any other cp.vl[0x18F]['STEER_STATUS'] is common and can happen during user override. sending 0 torque to avoid EPS sending error 5
elif CS.steer_not_allowed:
print "STEER ALERT, TORQUE INHIBITED"
apply_steer = 0
if CS.brake_error:
print "BRAKE ERROR"
self.controls_allowed = False
if not CS.can_valid and self.controls_allowed: # 200 ms
print "CAN INVALID"
self.controls_allowed = False
if not self.controls_allowed:
apply_steer = 0
apply_gas = 0
apply_brake = 0
pcm_speed = 0 # make sure you send 0 target speed to pcm
#pcm_cancel_cmd = 1 # prevent pcm control from turning on. FIXME: we can't just do this
# Send CAN commands.
can_sends = []
# Send steering command.
idx = frame % 4
can_sends.append(hondacan.create_steering_control(apply_steer, idx))
# Send gas and brake commands.
if (frame % 2) == 0:
idx = (frame / 2) % 4
can_sends.append(
hondacan.create_brake_command(apply_brake, pcm_override,
pcm_cancel_cmd, hud.chime, idx))
if not CS.brake_only:
# send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
# This prevents unexpected pedal range rescaling
gas_amount = (apply_gas + GAS_OFFSET) * (apply_gas > 0)
can_sends.append(hondacan.create_gas_command(gas_amount, idx))
# Send dashboard UI commands.
if (frame % 10) == 0:
idx = (frame/10) % 4
can_sends.extend(hondacan.create_ui_commands(pcm_speed, hud, CS.civic, idx))
# radar at 20Hz, but these msgs need to be sent at 50Hz on ilx (seems like an Acura bug)
if CS.civic:
radar_send_step = 5
else:
radar_send_step = 2
if (frame % radar_send_step) == 0:
idx = (frame/radar_send_step) % 4
can_sends.extend(hondacan.create_radar_commands(CS.v_ego, CS.civic, idx))
sendcan.send(can_list_to_can_capnp(can_sends).to_bytes())

275
selfdrive/controls/lib/carstate.py
Unescape View File

@ -0,0 +1,275 @@
import numpy as np
import selfdrive.messaging as messaging
from selfdrive.boardd.boardd import can_capnp_to_can_list_old, can_capnp_to_can_list
from selfdrive.controls.lib.can_parser import CANParser
from selfdrive.controls.lib.fingerprints import fingerprints
from selfdrive.config import VehicleParams
from common.realtime import sec_since_boot
def get_can_parser(civic, brake_only):
# this function generates lists for signal, messages and initial values
if civic:
dbc_f = 'honda_civic_touring_2016_can.dbc'
signals = [
("XMISSION_SPEED", 0x158, 0),
("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),
]
checks = [
(0x14a, 100),
(0x158, 100),
(0x17c, 100),
(0x191, 100),
(0x1a4, 50),
(0x326, 10),
(0x1b0, 50),
(0x1d0, 50),
(0x305, 10),
(0x324, 10),
(0x405, 3),
]
else:
dbc_f = 'acura_ilx_2016_can.dbc'
signals = [
("XMISSION_SPEED", 0x158, 0),
("WHEEL_SPEED_FL", 0x1d0, 0),
("WHEEL_SPEED_FR", 0x1d0, 0),
("WHEEL_SPEED_RL", 0x1d0, 0),
("STEER_ANGLE", 0x156, 0),
("STEER_TORQUE_SENSOR", 0x18f, 0),
("GEAR", 0x1a3, 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", 0x1a6, 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", 0x1a3, 0),
("MAIN_ON", 0x1a6, 0),
("ACC_STATUS", 0x17c, 0),
("PEDAL_GAS", 0x17c, 0),
("CRUISE_SETTING", 0x1a6, 0),
("LEFT_BLINKER", 0x294, 0),
("RIGHT_BLINKER", 0x294, 0),
("COUNTER", 0x324, 0),
]
checks = [
(0x156, 100),
(0x158, 100),
(0x17c, 100),
(0x1a3, 50),
(0x1a4, 50),
(0x1a6, 50),
(0x1b0, 50),
(0x1d0, 50),
(0x305, 10),
(0x324, 10),
(0x405, 3),
]
# add gas interceptor reading if we are using it
if not brake_only:
signals.append(("INTERCEPTOR_GAS", 0x201, 0))
checks.append((0x201, 50))
return CANParser(dbc_f, signals, checks)
def fingerprint(logcan):
print "waiting for fingerprint..."
brake_only = True
finger = {}
st = None
while 1:
possible_cars = []
for a in messaging.drain_sock(logcan, wait_for_one=True):
if st is None:
st = sec_since_boot()
for adr, _, msg, idx in can_capnp_to_can_list(a):
# pedal
if adr == 0x201 and idx == 0:
brake_only = False
if idx == 0:
finger[adr] = len(msg)
# check for a single match
for f in fingerprints:
is_possible = True
for adr in finger:
# confirm all messages we have seen match
if adr not in fingerprints[f] or fingerprints[f][adr] != finger[adr]:
#print "mismatch", f, adr
is_possible = False
break
if is_possible:
possible_cars.append(f)
# if we only have one car choice and it's been 100ms since we got our first message, exit
if len(possible_cars) == 1 and st is not None and (sec_since_boot()-st) > 0.1:
break
elif len(possible_cars) == 0:
raise Exception("car doesn't match any fingerprints")
print "fingerprinted", possible_cars[0]
return brake_only, possible_cars[0]
class CarState(object):
def __init__(self, logcan):
self.torque_mod = False
self.brake_only, self.car_type = fingerprint(logcan)
# assuming if you have a pedal interceptor you also have a torque mod
if not self.brake_only:
self.torque_mod = True
if self.car_type == "HONDA CIVIC 2016 TOURING":
self.civic = True
elif self.car_type == "ACURA ILX 2016 ACURAWATCH PLUS":
self.civic = False
else:
raise ValueError("unsupported car %s" % self.car_type)
# initialize can parser
self.cp = get_can_parser(self.civic, self.brake_only)
self.user_gas, self.user_gas_pressed = 0., 0
self.cruise_buttons = 0
self.cruise_setting = 0
self.blinker_on = 0
# TODO: actually make this work
self.a_ego = 0.
# speed in UI is shown as few % higher
self.ui_speed_fudge = 1.01 if self.civic else 1.025
# load vehicle params
self.VP = VehicleParams(self.civic)
def update(self, logcan):
# ******************* do can recv *******************
can_pub_main = []
canMonoTimes = []
for a in messaging.drain_sock(logcan):
canMonoTimes.append(a.logMonoTime)
can_pub_main.extend(can_capnp_to_can_list_old(a, [0,2]))
cp = self.cp
cp.update_can(can_pub_main)
# copy can_valid
self.can_valid = cp.can_valid
# car params
v_weight_v = [0., 1. ] # don't trust smooth speed at low values to avoid premature zero snapping
v_weight_bp = [1., 6.] # smooth blending, below ~0.6m/s the smooth speed snaps to zero
# update prevs, update must run once per loop
self.prev_cruise_buttons = self.cruise_buttons
self.prev_cruise_setting = self.cruise_setting
self.prev_blinker_on = self.blinker_on
# ******************* parse out can *******************
self.door_all_closed = not any([cp.vl[0x405]['DOOR_OPEN_FL'], cp.vl[0x405]['DOOR_OPEN_FR'],
cp.vl[0x405]['DOOR_OPEN_RL'], cp.vl[0x405]['DOOR_OPEN_RR']])
self.seatbelt = not cp.vl[0x305]['SEATBELT_DRIVER_LAMP'] and cp.vl[0x305]['SEATBELT_DRIVER_LATCHED']
# error 2 = temporary
# error 4 = temporary, hit a bump
# error 5 (permanent)
# error 6 = temporary
# error 7 (permanent)
#self.steer_error = cp.vl[0x18F]['STEER_STATUS'] in [5,7]
# whitelist instead of blacklist, safer at the expense of disengages
self.steer_error = cp.vl[0x18F]['STEER_STATUS'] not in [0,2,4,6]
self.steer_not_allowed = cp.vl[0x18F]['STEER_STATUS'] != 0
if cp.vl[0x18F]['STEER_STATUS'] != 0:
print cp.vl[0x18F]['STEER_STATUS']
self.brake_error = cp.vl[0x1B0]['BRAKE_ERROR_1'] or cp.vl[0x1B0]['BRAKE_ERROR_2']
self.esp_disabled = cp.vl[0x1A4]['ESP_DISABLED']
# calc best v_ego estimate, by averaging two opposite corners
self.v_wheel = (
cp.vl[0x1D0]['WHEEL_SPEED_FL'] + cp.vl[0x1D0]['WHEEL_SPEED_FR'] +
cp.vl[0x1D0]['WHEEL_SPEED_RL'] + cp.vl[0x1D0]['WHEEL_SPEED_RR']) / 4.
# blend in transmission speed at low speed, since it has more low speed accuracy
self.v_weight = np.interp(self.v_wheel, v_weight_bp, v_weight_v)
self.v_ego = (1. - self.v_weight) * cp.vl[0x158]['XMISSION_SPEED'] + self.v_weight * self.v_wheel
if not self.brake_only:
self.user_gas = cp.vl[0x201]['INTERCEPTOR_GAS']
self.user_gas_pressed = self.user_gas > 0 # this works because interceptor read < 0 when pedal position is 0. Once calibrated, this will change
#print user_gas, user_gas_pressed
if self.civic:
self.gear_shifter = cp.vl[0x191]['GEAR_SHIFTER']
self.angle_steers = cp.vl[0x14A]['STEER_ANGLE']
self.gear = 0 # TODO: civic has CVT... needs rev engineering
self.cruise_setting = cp.vl[0x296]['CRUISE_SETTING']
self.cruise_buttons = cp.vl[0x296]['CRUISE_BUTTONS']
self.main_on = cp.vl[0x326]['MAIN_ON']
self.gear_shifter_valid = self.gear_shifter in [1,8] # TODO: 1/P allowed for debug
self.blinker_on = cp.vl[0x326]['LEFT_BLINKER'] or cp.vl[0x326]['RIGHT_BLINKER']
else:
self.gear_shifter = cp.vl[0x1A3]['GEAR_SHIFTER']
self.angle_steers = cp.vl[0x156]['STEER_ANGLE']
self.gear = cp.vl[0x1A3]['GEAR']
self.cruise_setting = cp.vl[0x1A6]['CRUISE_SETTING']
self.cruise_buttons = cp.vl[0x1A6]['CRUISE_BUTTONS']
self.main_on = cp.vl[0x1A6]['MAIN_ON']
self.gear_shifter_valid = self.gear_shifter in [1,4] # TODO: 1/P allowed for debug
self.blinker_on = cp.vl[0x294]['LEFT_BLINKER'] or cp.vl[0x294]['RIGHT_BLINKER']
self.car_gas = cp.vl[0x130]['CAR_GAS']
self.brake_pressed = cp.vl[0x17C]['BRAKE_PRESSED']
self.user_brake = cp.vl[0x1A4]['USER_BRAKE']
self.standstill = not cp.vl[0x1B0]['WHEELS_MOVING']
self.steer_override = abs(cp.vl[0x18F]['STEER_TORQUE_SENSOR']) > 1200
self.v_cruise_pcm = cp.vl[0x324]['CRUISE_SPEED_PCM']
self.pcm_acc_status = cp.vl[0x17C]['ACC_STATUS']
self.pedal_gas = cp.vl[0x17C]['PEDAL_GAS']
self.hud_lead = cp.vl[0x30C]['HUD_LEAD']
self.counter_pcm = cp.vl[0x324]['COUNTER']
return canMonoTimes

52
selfdrive/controls/lib/drive_helpers.py
Unescape View File

@ -0,0 +1,52 @@
import numpy as np
def rate_limit(new_value, last_value, dw_step, up_step):
return np.clip(new_value, last_value + dw_step, last_value + up_step)
def learn_angle_offset(lateral_control, v_ego, angle_offset, d_poly, y_des, steer_override):
# simple integral controller that learns how much steering offset to put to have the car going straight
min_offset = -1. # deg
max_offset = 1. # deg
alpha = 1./36000. # correct by 1 deg in 2 mins, at 30m/s, with 50cm of error, at 20Hz
min_learn_speed = 1.
# learn less at low speed or when turning
alpha_v = alpha*(np.maximum(v_ego - min_learn_speed, 0.))/(1. + 0.5*abs(y_des))
# only learn if lateral control is active and if driver is not overriding:
if lateral_control and not steer_override:
angle_offset += d_poly[3] * alpha_v
angle_offset = np.clip(angle_offset, min_offset, max_offset)
return angle_offset
def actuator_hystereses(final_brake, braking, brake_steady, v_ego, civic):
# hyst params... TODO: move these to VehicleParams
brake_hyst_on = 0.055 if civic else 0.1 # to activate brakes exceed this value
brake_hyst_off = 0.005 # to deactivate brakes below this value
brake_hyst_gap = 0.01 # don't change brake command for small ocilalitons within this value
#*** histeresys logic to avoid brake blinking. go above 0.1 to trigger
if (final_brake < brake_hyst_on and not braking) or final_brake < brake_hyst_off:
final_brake = 0.
braking = final_brake > 0.
# for small brake oscillations within brake_hyst_gap, don't change the brake command
if final_brake == 0.:
brake_steady = 0.
elif final_brake > brake_steady + brake_hyst_gap:
brake_steady = final_brake - brake_hyst_gap
elif final_brake < brake_steady - brake_hyst_gap:
brake_steady = final_brake + brake_hyst_gap
final_brake = brake_steady
if not civic:
brake_on_offset_v = [.25, .15] # min brake command on brake activation. below this no decel is perceived
brake_on_offset_bp = [15., 30.] # offset changes VS speed to not have too abrupt decels at high speeds
# offset the brake command for threshold in the brake system. no brake torque perceived below it
brake_on_offset = np.interp(v_ego, brake_on_offset_bp, brake_on_offset_v)
brake_offset = brake_on_offset - brake_hyst_on
if final_brake > 0.0:
final_brake += brake_offset
return final_brake, braking, brake_steady

8
selfdrive/controls/lib/fingerprints.py
Unescape View File

@ -0,0 +1,8 @@
fingerprints = {
"ACURA ILX 2016 ACURAWATCH PLUS": {
1024L: 5, 513L: 5, 1027L: 5, 1029L: 8, 929L: 4, 1057L: 5, 777L: 8, 1034L: 5, 1036L: 8, 398L: 3, 399L: 7, 145L: 8, 660L: 8, 985L: 3, 923L: 2, 542L: 7, 773L: 7, 800L: 8, 432L: 7, 419L: 8, 420L: 8, 1030L: 5, 422L: 8, 808L: 8, 428L: 8, 304L: 8, 819L: 7, 821L: 5, 57L: 3, 316L: 8, 545L: 4, 464L: 8, 1108L: 8, 597L: 8, 342L: 6, 983L: 8, 344L: 8, 804L: 8, 1039L: 8, 476L: 4, 892L: 8, 490L: 8, 1064L: 7, 882L: 2, 884L: 7, 887L: 8, 888L: 8, 380L: 8, 1365L: 5
},
"HONDA CIVIC 2016 TOURING": {
1024L: 5, 513L: 5, 1027L: 5, 1029L: 8, 777L: 8, 1036L: 8, 1039L: 8, 1424L: 5, 401L: 8, 148L: 8, 662L: 4, 985L: 3, 795L: 8, 773L: 7, 800L: 8, 545L: 6, 420L: 8, 806L: 8, 808L: 8, 1322L: 5, 427L: 3, 428L: 8, 304L: 8, 432L: 7, 57L: 3, 450L: 8, 929L: 8, 330L: 8, 1302L: 8, 464L: 8, 1361L: 5, 1108L: 8, 597L: 8, 470L: 2, 344L: 8, 804L: 8, 399L: 7, 476L: 7, 1633L: 8, 487L: 4, 892L: 8, 490L: 8, 493L: 5, 884L: 8, 891L: 8, 380L: 8, 1365L: 5
}
}

88
selfdrive/controls/lib/hondacan.py
Unescape View File

@ -0,0 +1,88 @@
import struct
import common.numpy_fast as np
from selfdrive.config import Conversions as CV
# *** Honda specific ***
def can_cksum(mm):
s = 0
for c in mm:
c = ord(c)
s += (c>>4)
s += c & 0xF
s = 8-s
s %= 0x10
return s
def fix(msg, addr):
msg2 = msg[0:-1] + chr(ord(msg[-1]) | can_cksum(struct.pack("I", addr)+msg))
return msg2
def make_can_msg(addr, dat, idx, alt):
if idx is not None:
dat += chr(idx << 4)
dat = fix(dat, addr)
return [addr, 0, dat, alt]
def create_brake_command(apply_brake, pcm_override, pcm_cancel_cmd, chime, idx):
"""Creates a CAN message for the Honda DBC BRAKE_COMMAND."""
pump_on = apply_brake > 0
brakelights = apply_brake > 0
brake_rq = apply_brake > 0
pcm_fault_cmd = False
amount = struct.pack("!H", (apply_brake << 6) + pump_on)
msg = amount + struct.pack("BBB", (pcm_override << 4) |
(pcm_fault_cmd << 2) |
(pcm_cancel_cmd << 1) | brake_rq, 0x80,
brakelights << 7) + chr(chime) + "\x00"
return make_can_msg(0x1fa, msg, idx, 0)
def create_gas_command(gas_amount, idx):
"""Creates a CAN message for the Honda DBC GAS_COMMAND."""
msg = struct.pack("!H", gas_amount)
return make_can_msg(0x200, msg, idx, 0)
def create_steering_control(apply_steer, idx):
"""Creates a CAN message for the Honda DBC STEERING_CONTROL."""
msg = struct.pack("!h", apply_steer) + ("\x80\x00" if apply_steer != 0 else "\x00\x00")
return make_can_msg(0xe4, msg, idx, 0)
def create_ui_commands(pcm_speed, hud, civic, idx):
"""Creates an iterable of CAN messages for the UIs."""
commands = []
pcm_speed_real = np.clip(int(round(pcm_speed / 0.002763889)), 0,
64000) # conversion factor from dbc file
msg_0x30c = struct.pack("!HBBBBB", pcm_speed_real, hud.pcm_accel,
hud.v_cruise, hud.X2, hud.car, hud.X4)
commands.append(make_can_msg(0x30c, msg_0x30c, idx, 0))
msg_0x33d = chr(hud.X5) + chr(hud.lanes) + chr(hud.beep) + chr(hud.X8)
commands.append(make_can_msg(0x33d, msg_0x33d, idx, 0))
if civic: # 2 more msgs
msg_0x35e = chr(0) * 7
commands.append(make_can_msg(0x35e, msg_0x35e, idx, 0))
msg_0x39f = (
chr(0) * 2 + chr(hud.acc_alert) + chr(0) + chr(0xff) + chr(0x7f) + chr(0)
)
commands.append(make_can_msg(0x39f, msg_0x39f, idx, 0))
return commands
def create_radar_commands(v_ego, civic, idx):
"""Creates an iterable of CAN messages for the radar system."""
commands = []
v_ego_kph = np.clip(int(round(v_ego * CV.MS_TO_KPH)), 0, 255)
speed = struct.pack('!B', v_ego_kph)
msg_0x300 = ("\xf9" + speed + "\x8a\xd0" +\
("\x20" if idx == 0 or idx == 3 else "\x00") +\
"\x00\x00")
if civic:
msg_0x301 = "\x02\x38\x44\x32\x4f\x00\x00"
# add 8 on idx.
commands.append(make_can_msg(0x300, msg_0x300, idx + 8, 1))
else:
msg_0x301 = "\x0f\x18\x51\x02\x5a\x00\x00"
commands.append(make_can_msg(0x300, msg_0x300, idx, 1))
commands.append(make_can_msg(0x301, msg_0x301, idx, 1))
return commands

120
selfdrive/controls/lib/latcontrol.py
Unescape View File

@ -0,0 +1,120 @@
import numpy as np
def calc_curvature(v_ego, angle_steers, VP, angle_offset=0):
deg_to_rad = np.pi/180.
angle_steers_rad = (angle_steers - angle_offset) * deg_to_rad
curvature = angle_steers_rad/(VP.steer_ratio * VP.wheelbase * (1. + VP.slip_factor * v_ego**2))
return curvature
def calc_d_lookahead(v_ego):
#*** this function computes how far too look for lateral control
# howfar we look ahead is function of speed
offset_lookahead = 1.
coeff_lookahead = 4.4
# sqrt on speed is needed to keep, for a given curvature, the y_offset
# proportional to speed. Indeed, y_offset is prop to d_lookahead^2
# 26m at 25m/s
d_lookahead = offset_lookahead + np.sqrt(np.maximum(v_ego, 0)) * coeff_lookahead
return d_lookahead
def calc_lookahead_offset(v_ego, angle_steers, d_lookahead, VP, angle_offset):
#*** this function return teh lateral offset given the steering angle, speed and the lookahead distance
curvature = calc_curvature(v_ego, angle_steers, VP, angle_offset)
# clip is to avoid arcsin NaNs due to too sharp turns
y_actual = d_lookahead * np.tan(np.arcsin(np.clip(d_lookahead * curvature, -0.999, 0.999))/2.)
return y_actual, curvature
def pid_lateral_control(v_ego, y_actual, y_des, Ui_steer, steer_max,
steer_override, sat_count, enabled, half_pid, rate):
sat_count_rate = 1./rate
sat_count_limit = 0.8 # after 0.8s of continuous saturation, an alert will be sent
error_steer = y_des - y_actual
Ui_unwind_speed = 0.3/rate #.3 per second
if not half_pid:
Kp, Ki = 12.0, 1.0
else:
Kp, Ki = 6.0, .5 # 2x limit in ILX
Up_steer = error_steer*Kp
Ui_steer_new = Ui_steer + error_steer*Ki * 1./rate
output_steer_new = Ui_steer_new + Up_steer
# Anti-wind up for integrator: do not integrate if we are against the steer limits
if (
(error_steer >= 0. and (output_steer_new < steer_max or Ui_steer < 0)) or
(error_steer <= 0. and
(output_steer_new > -steer_max or Ui_steer > 0))) and not steer_override:
#update integrator
Ui_steer = Ui_steer_new
# unwind integrator if driver is maneuvering the steering wheel
elif steer_override:
Ui_steer -= Ui_unwind_speed * np.sign(Ui_steer)
# still, intergral term should not be bigger then limits
Ui_steer = np.clip(Ui_steer, -steer_max, steer_max)
output_steer = Up_steer + Ui_steer
# don't run steer control if at very low speed
if v_ego < 0.3 or not enabled:
output_steer = 0.
Ui_steer = 0.
# useful to know if control is against the limit
lateral_control_sat = False
if abs(output_steer) > steer_max:
lateral_control_sat = True
output_steer = np.clip(output_steer, -steer_max, steer_max)
# if lateral control is saturated for a certain period of time, send an alert for taking control of the car
# wind
if lateral_control_sat and not steer_override and v_ego > 10 and abs(error_steer) > 0.1:
sat_count += sat_count_rate
# unwind
else:
sat_count -= sat_count_rate
sat_flag = False
if sat_count >= sat_count_limit:
sat_flag = True
sat_count = np.clip(sat_count, 0, 1)
return output_steer, Up_steer, Ui_steer, lateral_control_sat, sat_count, sat_flag
class LatControl(object):
def __init__(self):
self.Up_steer = 0.
self.sat_count = 0
self.y_des = 0.0
self.lateral_control_sat = False
self.Ui_steer = 0.
self.reset()
def reset(self):
self.Ui_steer = 0.
def update(self, enabled, CS, d_poly, angle_offset):
rate = 100
steer_max = 1.0
# how far we look ahead is function of speed
d_lookahead = calc_d_lookahead(CS.v_ego)
# calculate actual offset at the lookahead point
self.y_actual, _ = calc_lookahead_offset(CS.v_ego, CS.angle_steers,
d_lookahead, CS.VP, angle_offset)
# desired lookahead offset
self.y_des = np.polyval(d_poly, d_lookahead)
output_steer, self.Up_steer, self.Ui_steer, self.lateral_control_sat, self.sat_count, sat_flag = pid_lateral_control(
CS.v_ego, self.y_actual, self.y_des, self.Ui_steer, steer_max,
CS.steer_override, self.sat_count, enabled, CS.torque_mod, rate)
final_steer = np.clip(output_steer, -steer_max, steer_max)
return final_steer, sat_flag

232
selfdrive/controls/lib/longcontrol.py
Unescape View File

@ -0,0 +1,232 @@
import numpy as np
from selfdrive.config import Conversions as CV
class LongCtrlState:
#*** this function handles the long control state transitions
# long_control_state labels:
off = 0 # Off
pid = 1 # moving and tracking targets, with PID control running
stopping = 2 # stopping and changing controls to almost open loop as PID does not fit well at such a low speed
starting = 3 # starting and releasing brakes in open loop before giving back to PID
def long_control_state_trans(enabled, long_control_state, v_ego, v_target, v_pid, output_gb):
stopping_speed = 0.5
stopping_target_speed = 0.3
starting_target_speed = 0.5
brake_threshold_to_pid = 0.2
stopping_condition = ((v_ego < stopping_speed) and (v_pid < stopping_target_speed) and (v_target < stopping_target_speed))
if not enabled:
long_control_state = LongCtrlState.off
else:
if long_control_state == LongCtrlState.off:
if enabled:
long_control_state = LongCtrlState.pid
elif long_control_state == LongCtrlState.pid:
if stopping_condition:
long_control_state = LongCtrlState.stopping
elif long_control_state == LongCtrlState.stopping:
if (v_target > starting_target_speed):
long_control_state = LongCtrlState.starting
elif long_control_state == LongCtrlState.starting:
if stopping_condition:
long_control_state = LongCtrlState.stopping
elif output_gb >= -brake_threshold_to_pid:
long_control_state = LongCtrlState.pid
return long_control_state
def get_compute_gb():
# see debug/dump_accel_from_fiber.py
w0 = np.array([[ 1.22056961, -0.39625418, 0.67952657],
[ 1.03691769, 0.78210306, -0.41343188]])
b0 = np.array([ 0.01536703, -0.14335321, -0.26932889])
w2 = np.array([[-0.59124422, 0.42899439, 0.38660881],
[ 0.79973811, 0.13178682, 0.08550351],
[-0.15651935, -0.44360259, 0.76910877]])
b2 = np.array([ 0.15624429, 0.02294923, -0.0341086 ])
w4 = np.array([[-0.31521443],
[-0.38626176],
[ 0.52667892]])
b4 = np.array([-0.02922216])
def compute_output(dat, w0, b0, w2, b2, w4, b4):
m0 = np.dot(dat, w0) + b0
m0 = leakyrelu(m0, 0.1)
m2 = np.dot(m0, w2) + b2
m2 = leakyrelu(m2, 0.1)
m4 = np.dot(m2, w4) + b4
return m4
def leakyrelu(x, alpha):
return np.maximum(x, alpha * x)
def _compute_gb(dat):
#linearly extrap below v1 using v1 and v2 data
v1 = 5.
v2 = 10.
vx = dat[1]
if vx > 5.:
m4 = compute_output(dat, w0, b0, w2, b2, w4, b4)
else:
dat[1] = v1
m4v1 = compute_output(dat, w0, b0, w2, b2, w4, b4)
dat[1] = v2
m4v2 = compute_output(dat, w0, b0, w2, b2, w4, b4)
m4 = (vx - v1) * (m4v2 - m4v1) / (v2 - v1) + m4v1
return m4
return _compute_gb
# takes in [desired_accel, current_speed] -> [-1.0, 1.0] where -1.0 is max brake and 1.0 is max gas
compute_gb = get_compute_gb()
def pid_long_control(v_ego, v_pid, Ui_accel_cmd, gas_max, brake_max, jerk_factor, gear, rate):
#*** This function compute the gb pedal positions in order to track the desired speed
# proportional and integral terms. More precision at low speed
Kp_v = [1.2, 0.8, 0.5]
Kp_bp = [0., 5., 35.]
Kp = np.interp(v_ego, Kp_bp, Kp_v)
Ki_v = [0.18, 0.12]
Ki_bp = [0., 35.]
Ki = np.interp(v_ego, Ki_bp, Ki_v)
# scle Kp and Ki by jerk factor drom drive_thread
Kp = (1. + jerk_factor)*Kp
Ki = (1. + jerk_factor)*Ki
# this is ugly but can speed reports 0 when speed<0.3m/s and we can't have that jump
v_ego_min = 0.3
v_ego = np.maximum(v_ego, v_ego_min)
v_error = v_pid - v_ego
Up_accel_cmd = v_error*Kp
Ui_accel_cmd_new = Ui_accel_cmd + v_error*Ki*1.0/rate
accel_cmd_new = Ui_accel_cmd_new + Up_accel_cmd
output_gb_new = compute_gb([accel_cmd_new, v_ego])
# Anti-wind up for integrator: only update integrator if we not against the thottle and brake limits
# do not wind up if we are changing gear and we are on the gas pedal
if (((v_error >= 0. and (output_gb_new < gas_max or Ui_accel_cmd < 0)) or
(v_error <= 0. and (output_gb_new > - brake_max or Ui_accel_cmd > 0))) and
not (v_error >= 0. and gear == 11 and output_gb_new > 0)):
#update integrator
Ui_accel_cmd = Ui_accel_cmd_new
accel_cmd = Ui_accel_cmd + Up_accel_cmd
# go from accel to pedals
output_gb = compute_gb([accel_cmd, v_ego])
output_gb = output_gb[0]
# useful to know if control is against the limit
long_control_sat = False
if output_gb > gas_max or output_gb < -brake_max:
long_control_sat = True
output_gb = np.clip(output_gb, -brake_max, gas_max)
return output_gb, Up_accel_cmd, Ui_accel_cmd, long_control_sat
stopping_brake_rate = 0.2 # brake_travel/s while trying to stop
starting_brake_rate = 0.6 # brake_travel/s while releasing on restart
starting_Ui = 0.5 # Since we don't have much info about acceleration at this point, be conservative
brake_stopping_target = 0.5 # apply at least this amount of brake to maintain the vehicle stationary
max_speed_error_v = [1.5, .8] # max positive v_pid error VS actual speed; this avoids controls windup due to slow pedal resp
max_speed_error_bp = [0., 30.] # speed breakpoints
class LongControl(object):
def __init__(self):
self.long_control_state = LongCtrlState.off # initialized to off
self.long_control_sat = False
self.Up_accel_cmd = 0.
self.last_output_gb = 0.
self.reset(0.)
def reset(self, v_pid):
self.Ui_accel_cmd = 0.
self.v_pid = v_pid
def update(self, enabled, CS, v_cruise, v_target_lead, a_target, jerk_factor):
# TODO: not every time
if CS.brake_only:
gas_max_v = [0, 0] # values
else:
gas_max_v = [0.6, 0.6] # values
gas_max_bp = [0., 100.] # speeds
brake_max_v = [1.0, 1.0, 0.8, 0.8] # values
brake_max_bp = [0., 5., 20., 100.] # speeds
# brake and gas limits
brake_max = np.interp(CS.v_ego, brake_max_bp, brake_max_v)
gas_max = np.interp(CS.v_ego, gas_max_bp, gas_max_v)
overshoot_allowance = 2.0 # overshoot allowed when changing accel sign
output_gb = self.last_output_gb
rate = 100
# limit max target speed based on cruise setting:
v_cruise_mph = round(v_cruise * CV.KPH_TO_MPH) # what's displayed in mph on the IC
v_target = np.minimum(v_target_lead, v_cruise_mph * CV.MPH_TO_MS / CS.ui_speed_fudge)
max_speed_delta_up = a_target[1]*1.0/rate
max_speed_delta_down = a_target[0]*1.0/rate
# *** long control substate transitions
self.long_control_state = long_control_state_trans(enabled, self.long_control_state, CS.v_ego, v_target, self.v_pid, output_gb)
# *** long control behavior based on state
# TODO: move this to drive_helpers
# disabled
if self.long_control_state == LongCtrlState.off:
self.v_pid = CS.v_ego # do nothing
output_gb = 0.
self.Ui_accel_cmd = 0.
# tracking objects and driving
elif self.long_control_state == LongCtrlState.pid:
#reset v_pid close to v_ego if it was too far and new v_target is closer to v_ego
if ((self.v_pid > CS.v_ego + overshoot_allowance) and
(v_target < self.v_pid)):
self.v_pid = np.maximum(v_target, CS.v_ego + overshoot_allowance)
elif ((self.v_pid < CS.v_ego - overshoot_allowance) and
(v_target > self.v_pid)):
self.v_pid = np.minimum(v_target, CS.v_ego - overshoot_allowance)
# move v_pid no faster than allowed accel limits
if (v_target > self.v_pid + max_speed_delta_up):
self.v_pid += max_speed_delta_up
elif (v_target < self.v_pid + max_speed_delta_down):
self.v_pid += max_speed_delta_down
else:
self.v_pid = v_target
# to avoid too much wind up on acceleration, limit positive speed error
if not CS.brake_only:
max_speed_error = np.interp(CS.v_ego, max_speed_error_bp, max_speed_error_v)
self.v_pid = np.minimum(self.v_pid, CS.v_ego + max_speed_error)
output_gb, self.Up_accel_cmd, self.Ui_accel_cmd, self.long_control_sat = pid_long_control(CS.v_ego, self.v_pid, \
self.Ui_accel_cmd, gas_max, brake_max, jerk_factor, CS.gear, rate)
# intention is to stop, switch to a different brake control until we stop
elif self.long_control_state == LongCtrlState.stopping:
if CS.v_ego > 0. or output_gb > -brake_stopping_target or not CS.standstill:
output_gb -= stopping_brake_rate/rate
output_gb = np.clip(output_gb, -brake_max, gas_max)
self.v_pid = CS.v_ego
self.Ui_accel_cmd = 0.
# intention is to move again, release brake fast before handling control to PID
elif self.long_control_state == LongCtrlState.starting:
if output_gb < -0.2:
output_gb += starting_brake_rate/rate
self.v_pid = CS.v_ego
self.Ui_accel_cmd = starting_Ui
self.last_output_gb = output_gb
final_gas = np.clip(output_gb, 0., gas_max)
final_brake = -np.clip(output_gb, -brake_max, 0.)
return final_gas, final_brake

63
selfdrive/controls/lib/pathplanner.py
Unescape View File

@ -0,0 +1,63 @@
import selfdrive.messaging as messaging
import numpy as np
X_PATH = np.arange(0.0, 50.0)
def model_polyfit(points):
return np.polyfit(X_PATH, map(float, points), 3)
# lane width http://safety.fhwa.dot.gov/geometric/pubs/mitigationstrategies/chapter3/3_lanewidth.cfm
_LANE_WIDTH_V = np.asarray([3., 3.8])
# break points of speed
_LANE_WIDTH_BP = np.asarray([0., 31.])
def calc_desired_path(l_poly, r_poly, p_poly, l_prob, r_prob, p_prob, speed):
#*** this function computes the poly for the center of the lane, averaging left and right polys
lane_width = np.interp(speed, _LANE_WIDTH_BP, _LANE_WIDTH_V)
# lanes in US are ~3.6m wide
half_lane_poly = np.array([0., 0., 0., lane_width / 2.])
if l_prob + r_prob > 0.01:
c_poly = ((l_poly - half_lane_poly) * l_prob +
(r_poly + half_lane_poly) * r_prob) / (l_prob + r_prob)
c_prob = np.sqrt((l_prob**2 + r_prob**2) / 2.)
else:
c_poly = np.zeros(4)
c_prob = 0.
p_weight = 1. # predicted path weight relatively to the center of the lane
d_poly = list((c_poly*c_prob + p_poly*p_prob*p_weight ) / (c_prob + p_prob*p_weight))
return d_poly, c_poly, c_prob
class PathPlanner(object):
def __init__(self, model):
self.model = model
self.dead = True
self.d_poly = [0., 0., 0., 0.]
self.last_model = 0.
self.logMonoTime = 0
self.lead_dist, self.lead_prob, self.lead_var = 0, 0, 1
def update(self, cur_time, v_ego):
md = messaging.recv_sock(self.model)
if md is not None:
self.logMonoTime = md.logMonoTime
p_poly = model_polyfit(md.model.path.points) # predicted path
p_prob = 1. # model does not tell this probability yet, so set to 1 for now
l_poly = model_polyfit(md.model.leftLane.points) # left line
l_prob = md.model.leftLane.prob # left line prob
r_poly = model_polyfit(md.model.rightLane.points) # right line
r_prob = md.model.rightLane.prob # right line prob
self.lead_dist = md.model.lead.dist
self.lead_prob = md.model.lead.prob
self.lead_var = md.model.lead.std**2
#*** compute target path ***
self.d_poly, _, _ = calc_desired_path(l_poly, r_poly, p_poly, l_prob, r_prob, p_prob, v_ego)
self.last_model = cur_time
self.dead = False
elif cur_time - self.last_model > 0.5:
self.dead = True

256
selfdrive/controls/lib/radar_helpers.py
Unescape View File

@ -0,0 +1,256 @@
import numpy as np
import platform
import os
import sys
from common.kalman.ekf import FastEKF1D, SimpleSensor
# radar tracks
SPEED, ACCEL = 0, 1 # Kalman filter states enum
rate, ratev = 20., 20. # model and radar are both at 20Hz
ts = 1./rate
freq_v_lat = 0.2 # Hz
k_v_lat = 2*np.pi*freq_v_lat*ts / (1 + 2*np.pi*freq_v_lat*ts)
freq_a_lead = .5 # Hz
k_a_lead = 2*np.pi*freq_a_lead*ts / (1 + 2*np.pi*freq_a_lead*ts)
# stationary qualification parameters
v_stationary_thr = 4. # objects moving below this speed are classified as stationary
v_oncoming_thr = -3.9 # needs to be a bit lower in abs value than v_stationary_thr to not leave "holes"
v_ego_stationary = 4. # no stationary object flag below this speed
class Track(object):
def __init__(self):
self.ekf = None
self.stationary = True
self.initted = False
def update(self, d_rel, y_rel, v_rel, d_path, v_ego_t_aligned):
if self.initted:
self.dPathPrev = self.dPath
self.vLeadPrev = self.vLead
self.vRelPrev = self.vRel
# relative values, copy
self.dRel = d_rel # LONG_DIST
self.yRel = y_rel # -LAT_DIST
self.vRel = v_rel # REL_SPEED
# compute distance to path
self.dPath = d_path
# computed velocity and accelerations
self.vLead = self.vRel + v_ego_t_aligned
if not self.initted:
self.aRel = 0. # nidec gives no information about this
self.vLat = 0.
self.aLead = 0.
else:
# estimate acceleration
a_rel_unfilt = (self.vRel - self.vRelPrev) / ts
a_rel_unfilt = np.clip(a_rel_unfilt, -10., 10.)
self.aRel = k_a_lead * a_rel_unfilt + (1 - k_a_lead) * self.aRel
v_lat_unfilt = (self.dPath - self.dPathPrev) / ts
self.vLat = k_v_lat * v_lat_unfilt + (1 - k_v_lat) * self.vLat
a_lead_unfilt = (self.vLead - self.vLeadPrev) / ts
a_lead_unfilt = np.clip(a_lead_unfilt, -10., 10.)
self.aLead = k_a_lead * a_lead_unfilt + (1 - k_a_lead) * self.aLead
if self.stationary:
# stationary objects can become non stationary, but not the other way around
self.stationary = v_ego_t_aligned > v_ego_stationary and abs(self.vLead) < v_stationary_thr
self.oncoming = self.vLead < v_oncoming_thr
if self.ekf is None:
self.ekf = FastEKF1D(ts, 1e3, [0.1, 1])
self.ekf.state[SPEED] = self.vLead
self.ekf.state[ACCEL] = 0
self.lead_sensor = SimpleSensor(SPEED, 1, 2)
self.vLeadK = self.vLead
self.aLeadK = self.aLead
else:
self.ekf.update_scalar(self.lead_sensor.read(self.vLead))
self.ekf.predict(ts)
self.vLeadK = float(self.ekf.state[SPEED])
self.aLeadK = float(self.ekf.state[ACCEL])
if not self.initted:
self.cnt = 1
self.vision_cnt = 0
else:
self.cnt += 1
self.initted = True
self.vision = False
def mix_vision(self, dist_to_vision, rel_speed_diff):
# rel speed is very hard to estimate from vision
if dist_to_vision < 4.0 and rel_speed_diff < 10.:
# vision point is never stationary
self.stationary = False
self.vision = True
self.vision_cnt += 1
def get_key_for_cluster(self):
# Weigh y higher since radar is inaccurate in this dimension
return [self.dRel, self.dPath*2, self.vRel]
# ******************* Cluster *******************
if platform.machine() == 'aarch64':
for x in sys.path:
pp = os.path.join(x, "phonelibs/hierarchy/lib")
if os.path.isfile(os.path.join(pp, "_hierarchy.so")):
sys.path.append(pp)
break
import _hierarchy
else:
from scipy.cluster import _hierarchy
def fcluster(Z, t, criterion='inconsistent', depth=2, R=None, monocrit=None):
# supersimplified function to get fast clustering. Got it from scipy
Z = np.asarray(Z, order='c')
n = Z.shape[0] + 1
T = np.zeros((n,), dtype='i')
_hierarchy.cluster_dist(Z, T, float(t), int(n))
return T
RDR_TO_LDR = 2.7
def mean(l):
return sum(l)/len(l)
class Cluster(object):
def __init__(self):
self.tracks = set()
def add(self, t):
# add the first track
self.tracks.add(t)
# TODO: make generic
@property
def dRel(self):
return mean([t.dRel for t in self.tracks])
@property
def yRel(self):
return mean([t.yRel for t in self.tracks])
@property
def vRel(self):
return mean([t.vRel for t in self.tracks])
@property
def aRel(self):
return mean([t.aRel for t in self.tracks])
@property
def vLead(self):
return mean([t.vLead for t in self.tracks])
@property
def aLead(self):
return mean([t.aLead for t in self.tracks])
@property
def dPath(self):
return mean([t.dPath for t in self.tracks])
@property
def vLat(self):
return mean([t.vLat for t in self.tracks])
@property
def vLeadK(self):
return mean([t.vLeadK for t in self.tracks])
@property
def aLeadK(self):
return mean([t.aLeadK for t in self.tracks])
@property
def vision(self):
return any([t.vision for t in self.tracks])
@property
def vision_cnt(self):
return max([t.vision_cnt for t in self.tracks])
@property
def stationary(self):
return all([t.stationary for t in self.tracks])
@property
def oncoming(self):
return all([t.oncoming for t in self.tracks])
def toLive20(self, lead):
lead.dRel = float(self.dRel) - RDR_TO_LDR
lead.yRel = float(self.yRel)
lead.vRel = float(self.vRel)
lead.aRel = float(self.aRel)
lead.vLead = float(self.vLead)
lead.aLead = float(self.aLead)
lead.dPath = float(self.dPath)
lead.vLat = float(self.vLat)
lead.vLeadK = float(self.vLeadK)
lead.aLeadK = float(self.aLeadK)
lead.status = True
lead.fcw = False
def __str__(self):
ret = "x: %7.2f y: %7.2f v: %7.2f a: %7.2f" % (self.dRel, self.yRel, self.vRel, self.aRel)
if self.stationary:
ret += " stationary"
if self.vision:
ret += " vision"
if self.oncoming:
ret += " oncoming"
if self.vision_cnt > 0:
ret += " vision_cnt: %6.0f" % self.vision_cnt
return ret
def is_potential_lead(self, v_ego, enabled):
# predict cut-ins by extrapolating lateral speed by a lookahead time
# lookahead time depends on cut-in distance. more attentive for close cut-ins
# also, above 50 meters the predicted path isn't very reliable
# the distance at which v_lat matters is higher at higher speed
lookahead_dist = 40. + v_ego/1.2 #40m at 0mph, ~70m at 80mph
t_lookahead_v = [1., 0.]
t_lookahead_bp = [10., lookahead_dist]
# average dist
d_path = self.dPath
if enabled:
t_lookahead = np.interp(self.dRel, t_lookahead_bp, t_lookahead_v)
# correct d_path for lookahead time, considering only cut-ins and no more than 1m impact
lat_corr = np.clip(t_lookahead * self.vLat, -1, 0)
else:
lat_corr = 0.
d_path = np.maximum(d_path + lat_corr, 0)
if d_path < 1.5 and not self.stationary and not self.oncoming:
return True
else:
return False
def is_potential_lead2(self, lead_clusters):
if len(lead_clusters) > 0:
lead_cluster = lead_clusters[0]
# check if the new lead is too close and roughly at the same speed of the first lead: it might just be the second axle of the same vehicle
if (self.dRel - lead_cluster.dRel) < 8. and abs(self.vRel - lead_cluster.vRel) < 1.:
return False
else:
return True
else:
return False

268
selfdrive/controls/radard.py
Unescape View File

@ -0,0 +1,268 @@
#!/usr/bin/env python
import zmq
import numpy as np
import numpy.matlib
from collections import defaultdict
from fastcluster import linkage_vector
import selfdrive.messaging as messaging
from selfdrive.boardd.boardd import can_capnp_to_can_list_old
from selfdrive.controls.lib.latcontrol import calc_lookahead_offset
from selfdrive.controls.lib.can_parser import CANParser
from selfdrive.controls.lib.pathplanner import PathPlanner
from selfdrive.config import VehicleParams
from selfdrive.controls.lib.radar_helpers import Track, Cluster, fcluster, RDR_TO_LDR
from common.services import service_list
from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper
from common.kalman.ekf import EKF, SimpleSensor
#vision point
DIMSV = 2
XV, SPEEDV = 0, 1
VISION_POINT = 1
class EKFV1D(EKF):
def __init__(self):
super(EKFV1D, self).__init__(False)
self.identity = np.matlib.identity(DIMSV)
self.state = np.matlib.zeros((DIMSV, 1))
self.var_init = 1e2 # ~ model variance when probability is 70%, so good starting point
self.covar = self.identity * self.var_init
# self.process_noise = np.asmatrix(np.diag([100, 10]))
self.process_noise = np.matlib.diag([0.5, 1])
def calc_transfer_fun(self, dt):
tf = np.matlib.identity(DIMSV)
tf[XV, SPEEDV] = dt
tfj = tf
return tf, tfj
# nidec radar decoding
def nidec_decode(cp, ar_pts):
for ii in cp.msgs_upd:
# filter points with very big distance, as fff (~255) is invalid. FIXME: use VAL tables from dbc
if cp.vl[ii]['LONG_DIST'] < 255:
ar_pts[ii] = [cp.vl[ii]['LONG_DIST'] + RDR_TO_LDR,
-cp.vl[ii]['LAT_DIST'], cp.vl[ii]['REL_SPEED'], np.nan,
cp.ts[ii], cp.vl[ii]['NEW_TRACK'], cp.ct[ii]]
elif ii in ar_pts:
del ar_pts[ii]
return ar_pts
def _create_radard_can_parser():
dbc_f = 'acura_ilx_2016_nidec.dbc'
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)
return CANParser(dbc_f, signals, checks)
# fuses camera and radar data for best lead detection
def radard_thread(gctx=None):
set_realtime_priority(1)
context = zmq.Context()
# *** subscribe to features and model from visiond
model = messaging.sub_sock(context, service_list['model'].port)
logcan = messaging.sub_sock(context, service_list['can'].port)
live100 = messaging.sub_sock(context, service_list['live100'].port)
PP = PathPlanner(model)
# *** publish live20 and liveTracks
live20 = messaging.pub_sock(context, service_list['live20'].port)
liveTracks = messaging.pub_sock(context, service_list['liveTracks'].port)
# subscribe to stats about the car
# TODO: move this to new style packet
VP = VehicleParams(False) # same for ILX and civic
ar_pts = {}
path_x = np.arange(0.0, 140.0, 0.1) # 140 meters is max
# Time-alignment
rate = 20. # model and radar are both at 20Hz
tsv = 1./rate
rdr_delay = 0.10 # radar data delay in s
v_len = 20 # how many speed data points to remember for t alignment with rdr data
enabled = 0
steer_angle = 0.
tracks = defaultdict(dict)
# Nidec
cp = _create_radard_can_parser()
# Kalman filter stuff:
ekfv = EKFV1D()
speedSensorV = SimpleSensor(XV, 1, 2)
# v_ego
v_ego = None
v_ego_array = np.zeros([2, v_len])
v_ego_t_aligned = 0.
rk = Ratekeeper(rate, print_delay_threshold=np.inf)
while 1:
canMonoTimes = []
can_pub_radar = []
for a in messaging.drain_sock(logcan, wait_for_one=True):
canMonoTimes.append(a.logMonoTime)
can_pub_radar.extend(can_capnp_to_can_list_old(a, [1, 3]))
# only run on the 0x445 packets, used for timing
if not any(x[0] == 0x445 for x in can_pub_radar):
continue
cp.update_can(can_pub_radar)
if not cp.can_valid:
# TODO: handle this
pass
ar_pts = nidec_decode(cp, ar_pts)
# receive the live100s
l100 = messaging.recv_sock(live100)
if l100 is not None:
enabled = l100.live100.enabled
v_ego = l100.live100.vEgo
steer_angle = l100.live100.angleSteers
v_ego_array = np.append(v_ego_array, [[v_ego], [float(rk.frame)/rate]], 1)
v_ego_array = v_ego_array[:, 1:]
if v_ego is None:
continue
# *** get path prediction from the model ***
PP.update(sec_since_boot(), v_ego)
# run kalman filter only if prob is high enough
if PP.lead_prob > 0.7:
ekfv.update(speedSensorV.read(PP.lead_dist, covar=PP.lead_var))
ekfv.predict(tsv)
ar_pts[VISION_POINT] = (float(ekfv.state[XV]), np.polyval(PP.d_poly, float(ekfv.state[XV])),
float(ekfv.state[SPEEDV]), np.nan, PP.logMonoTime, np.nan, sec_since_boot())
else:
ekfv.state[XV] = PP.lead_dist
ekfv.covar = (np.diag([PP.lead_var, ekfv.var_init]))
ekfv.state[SPEEDV] = 0.
if VISION_POINT in ar_pts:
del ar_pts[VISION_POINT]
# *** compute the likely path_y ***
if enabled: # use path from model path_poly
path_y = np.polyval(PP.d_poly, path_x)
else: # use path from steer, set angle_offset to 0 since calibration does not exactly report the physical offset
path_y = calc_lookahead_offset(v_ego, steer_angle, path_x, VP, angle_offset=0)[0]
# *** remove missing points from meta data ***
for ids in tracks.keys():
if ids not in ar_pts:
tracks.pop(ids, None)
# *** compute the tracks ***
for ids in ar_pts:
# ignore the vision point for now
if ids == VISION_POINT:
continue
rpt = ar_pts[ids]
# align v_ego by a fixed time to align it with the radar measurement
cur_time = float(rk.frame)/rate
v_ego_t_aligned = np.interp(cur_time - rdr_delay, v_ego_array[1], v_ego_array[0])
d_path = np.sqrt(np.amin((path_x - rpt[0]) ** 2 + (path_y - rpt[1]) ** 2))
# create the track
if ids not in tracks or rpt[5] == 1:
tracks[ids] = Track()
tracks[ids].update(rpt[0], rpt[1], rpt[2], d_path, v_ego_t_aligned)
# allow the vision model to remove the stationary flag if distance and rel speed roughly match
if VISION_POINT in ar_pts:
dist_to_vision = np.sqrt((0.5*(ar_pts[VISION_POINT][0] - rpt[0])) ** 2 + (2*(ar_pts[VISION_POINT][1] - rpt[1])) ** 2)
rel_speed_diff = abs(ar_pts[VISION_POINT][2] - rpt[2])
tracks[ids].mix_vision(dist_to_vision, rel_speed_diff)
# publish tracks (debugging)
dat = messaging.new_message()
dat.init('liveTracks', len(tracks))
for cnt, ids in enumerate(tracks.keys()):
dat.liveTracks[cnt].trackId = ids
dat.liveTracks[cnt].dRel = float(tracks[ids].dRel)
dat.liveTracks[cnt].yRel = float(tracks[ids].yRel)
dat.liveTracks[cnt].vRel = float(tracks[ids].vRel)
dat.liveTracks[cnt].aRel = float(tracks[ids].aRel)
dat.liveTracks[cnt].stationary = tracks[ids].stationary
dat.liveTracks[cnt].oncoming = tracks[ids].oncoming
liveTracks.send(dat.to_bytes())
idens = tracks.keys()
track_pts = np.array([tracks[iden].get_key_for_cluster() for iden in idens])
# If we have multiple points, cluster them
if len(track_pts) > 1:
link = linkage_vector(track_pts, method='centroid')
cluster_idxs = fcluster(link, 2.5, criterion='distance')
clusters = [None]*max(cluster_idxs)
for idx in xrange(len(track_pts)):
cluster_i = cluster_idxs[idx]-1
if clusters[cluster_i] == None:
clusters[cluster_i] = Cluster()
clusters[cluster_i].add(tracks[idens[idx]])
elif len(track_pts) == 1:
# TODO: why do we need this?
clusters = [Cluster()]
clusters[0].add(tracks[idens[0]])
else:
clusters = []
# *** extract the lead car ***
lead_clusters = [c for c in clusters
if c.is_potential_lead(v_ego, enabled)]
lead_clusters.sort(key=lambda x: x.dRel)
lead_len = len(lead_clusters)
# *** extract the second lead from the whole set of leads ***
lead2_clusters = [c for c in lead_clusters
if c.is_potential_lead2(lead_clusters)]
lead2_clusters.sort(key=lambda x: x.dRel)
lead2_len = len(lead2_clusters)
# *** publish live20 ***
dat = messaging.new_message()
dat.init('live20')
dat.live20.mdMonoTime = PP.logMonoTime
dat.live20.canMonoTimes = canMonoTimes
if lead_len > 0:
lead_clusters[0].toLive20(dat.live20.leadOne)
if lead2_len > 0:
lead2_clusters[0].toLive20(dat.live20.leadTwo)
else:
dat.live20.leadTwo.status = False
else:
dat.live20.leadOne.status = False
dat.live20.cumLagMs = -rk.remaining*1000.
live20.send(dat.to_bytes())
rk.monitor_time()
def main(gctx=None):
radard_thread(gctx)
if __name__ == "__main__":
main()

58
selfdrive/logcatd/Makefile
Unescape View File

@ -0,0 +1,58 @@
CC = clang
CXX = clang++
PHONELIBS = ../../phonelibs
WARN_FLAGS = -Werror=implicit-function-declaration \
-Werror=incompatible-pointer-types \
-Werror=int-conversion \
-Werror=return-type \
-Werror=format-extra-args
CFLAGS = -std=gnu11 -g -fPIC -O2 $(WARN_FLAGS)
CXXFLAGS = -std=c++11 -g -fPIC -O2 $(WARN_FLAGS)
ZMQ_FLAGS = -I$(PHONELIBS)/zmq/aarch64/include
ZMQ_LIBS = -L$(PHONELIBS)/zmq/aarch64/lib \
-l:libczmq.a -l:libzmq.a \
-lgnustl_shared
CEREAL_FLAGS = -I$(PHONELIBS)/capnp-cpp/include
CEREAL_LIBS = -L$(PHONELIBS)/capnp-cpp/aarch64/lib/ \
-l:libcapnp.a -l:libkj.a
CEREAL_OBJS = ../../cereal/gen/c/log.capnp.o
OBJS = logcatd.o \
log.capnp.o
DEPS := $(OBJS:.o=.d)
all: logcatd
logcatd: $(OBJS)
@echo "[ LINK ] $@"
$(CXX) -fPIC -o '$@' $^ \
$(CEREAL_LIBS) \
$(ZMQ_LIBS) \
-llog
%.o: %.cc
@echo "[ CXX ] $@"
$(CXX) $(CXXFLAGS) \
-I$(PHONELIBS)/android_system_core/include \
$(CEREAL_FLAGS) \
$(ZMQ_FLAGS) \
-I../ \
-I../../ \
-c -o '$@' '$<'
log.capnp.o: ../../cereal/gen/cpp/log.capnp.c++
@echo "[ CXX ] $@"
$(CXX) $(CXXFLAGS) $(CEREAL_FLAGS) \
-c -o '$@' '$<'
.PHONY: clean
clean:
rm -f logcatd $(OBJS) $(DEPS)
-include $(DEPS)

68
selfdrive/logcatd/logcatd.cc
Unescape View File

@ -0,0 +1,68 @@
#include <cstdio>
#include <cstdlib>
#include <cassert>
#include <log/log.h>
#include <log/logger.h>
#include <log/logprint.h>
#include <zmq.h>
#include <capnp/serialize.h>
#include "common/timing.h"
#include "cereal/gen/cpp/log.capnp.h"
int main() {
int err;
struct logger_list *logger_list = android_logger_list_alloc(ANDROID_LOG_RDONLY, 0, 0);
assert(logger_list);
struct logger *main_logger = android_logger_open(logger_list, LOG_ID_MAIN);
assert(main_logger);
struct logger *radio_logger = android_logger_open(logger_list, LOG_ID_RADIO);
assert(radio_logger);
struct logger *system_logger = android_logger_open(logger_list, LOG_ID_SYSTEM);
assert(system_logger);
struct logger *crash_logger = android_logger_open(logger_list, LOG_ID_CRASH);
assert(crash_logger);
struct logger *kernel_logger = android_logger_open(logger_list, LOG_ID_KERNEL);
assert(kernel_logger);
void *context = zmq_ctx_new();
void *publisher = zmq_socket(context, ZMQ_PUB);
err = zmq_bind(publisher, "tcp://*:8020");
assert(err == 0);
while (1) {
log_msg log_msg;
err = android_logger_list_read(logger_list, &log_msg);
if (err <= 0) {
break;
}
AndroidLogEntry entry;
err = android_log_processLogBuffer(&log_msg.entry_v1, &entry);
if (err < 0) {
continue;
}
capnp::MallocMessageBuilder msg;
cereal::Event::Builder event = msg.initRoot<cereal::Event>();
event.setLogMonoTime(nanos_since_boot());
auto androidEntry = event.initAndroidLogEntry();
androidEntry.setId(log_msg.id());
androidEntry.setTs(entry.tv_sec * 1000000000ULL + entry.tv_nsec);
androidEntry.setPriority(entry.priority);
androidEntry.setPid(entry.pid);
androidEntry.setTid(entry.tid);
androidEntry.setTag(entry.tag);
androidEntry.setMessage(entry.message);
auto words = capnp::messageToFlatArray(msg);
auto bytes = words.asBytes();
zmq_send(publisher, bytes.begin(), bytes.size(), 0);
}
android_logger_list_close(logger_list);
return 0;
}

0
selfdrive/loggerd/__init__.py
Unescape View File

9
selfdrive/loggerd/config.py
Unescape View File

@ -0,0 +1,9 @@
import os
# fetch from environment
DONGLE_ID = os.getenv("DONGLE_ID")
DONGLE_SECRET = os.getenv("DONGLE_SECRET")
ROOT = '/sdcard/realdata/'
SEGMENT_LENGTH = 60

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

Write Preview
Loading…
Cancel
Save