openpilot release
Vehicle Researcher
commit
e94a30bec0
|
|
@ -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.
|
||||
|
|
@ -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.**
|
||||
|
|
@ -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.
|
||||
|
|
@ -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.
|
||||
|
|
@ -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/*
|
||||
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
|
|
@ -0,0 +1,609 @@
|
|||
/**************************************************************************//**
|
||||
* @file core_cmFunc.h
|
||||
* @brief CMSIS Cortex-M Core Function Access Header File
|
||||
* @version V2.10
|
||||
* @date 26. July 2011
|
||||
*
|
||||
* @note
|
||||
* Copyright (C) 2009-2011 ARM Limited. All rights reserved.
|
||||
*
|
||||
* @par
|
||||
* ARM Limited (ARM) is supplying this software for use with Cortex-M
|
||||
* processor based microcontrollers. This file can be freely distributed
|
||||
* within development tools that are supporting such ARM based processors.
|
||||
*
|
||||
* @par
|
||||
* THIS SOFTWARE IS PROVIDED "AS IS". NO WARRANTIES, WHETHER EXPRESS, IMPLIED
|
||||
* OR STATUTORY, INCLUDING, BUT NOT LIMITED TO, IMPLIED WARRANTIES OF
|
||||
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE APPLY TO THIS SOFTWARE.
|
||||
* ARM SHALL NOT, IN ANY CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL, OR
|
||||
* CONSEQUENTIAL DAMAGES, FOR ANY REASON WHATSOEVER.
|
||||
*
|
||||
******************************************************************************/
|
||||
|
||||
#ifndef __CORE_CMFUNC_H
|
||||
#define __CORE_CMFUNC_H
|
||||
|
||||
|
||||
/* ########################### Core Function Access ########################### */
|
||||
/** \ingroup CMSIS_Core_FunctionInterface
|
||||
\defgroup CMSIS_Core_RegAccFunctions CMSIS Core Register Access Functions
|
||||
@{
|
||||
*/
|
||||
|
||||
#if defined ( __CC_ARM ) /*------------------RealView Compiler -----------------*/
|
||||
/* ARM armcc specific functions */
|
||||
|
||||
#if (__ARMCC_VERSION < 400677)
|
||||
#error "Please use ARM Compiler Toolchain V4.0.677 or later!"
|
||||
#endif
|
||||
|
||||
/* intrinsic void __enable_irq(); */
|
||||
/* intrinsic void __disable_irq(); */
|
||||
|
||||
/** \brief Get Control Register
|
||||
|
||||
This function returns the content of the Control Register.
|
||||
|
||||
\return Control Register value
|
||||
*/
|
||||
static __INLINE uint32_t __get_CONTROL(void)
|
||||
{
|
||||
register uint32_t __regControl __ASM("control");
|
||||
return(__regControl);
|
||||
}
|
||||
|
||||
|
||||
/** \brief Set Control Register
|
||||
|
||||
This function writes the given value to the Control Register.
|
||||
|
||||
\param [in] control Control Register value to set
|
||||
*/
|
||||
static __INLINE void __set_CONTROL(uint32_t control)
|
||||
{
|
||||
register uint32_t __regControl __ASM("control");
|
||||
__regControl = control;
|
||||
}
|
||||
|
||||
|
||||
/** \brief Get ISPR Register
|
||||
|
||||
This function returns the content of the ISPR Register.
|
||||
|
||||
\return ISPR Register value
|
||||
*/
|
||||
static __INLINE uint32_t __get_IPSR(void)
|
||||
{
|
||||
register uint32_t __regIPSR __ASM("ipsr");
|
||||
return(__regIPSR);
|
||||
}
|
||||
|
||||
|
||||
/** \brief Get APSR Register
|
||||
|
||||
This function returns the content of the APSR Register.
|
||||
|
||||
\return APSR Register value
|
||||
*/
|
||||
static __INLINE uint32_t __get_APSR(void)
|
||||
{
|
||||
register uint32_t __regAPSR __ASM("apsr");
|
||||
return(__regAPSR);
|
||||
}
|
||||
|
||||
|
||||
/** \brief Get xPSR Register
|
||||
|
||||
This function returns the content of the xPSR Register.
|
||||
|
||||
\return xPSR Register value
|
||||
*/
|
||||
static __INLINE uint32_t __get_xPSR(void)
|
||||
{
|
||||
register uint32_t __regXPSR __ASM("xpsr");
|
||||
return(__regXPSR);
|
||||
}
|
||||
|
||||
|
||||
/** \brief Get Process Stack Pointer
|
||||
|
||||
This function returns the current value of the Process Stack Pointer (PSP).
|
||||
|
||||
\return PSP Register value
|
||||
*/
|
||||
static __INLINE uint32_t __get_PSP(void)
|
||||
{
|
||||
register uint32_t __regProcessStackPointer __ASM("psp");
|
||||
return(__regProcessStackPointer);
|
||||
}
|
||||
|
||||
|
||||
/** \brief Set Process Stack Pointer
|
||||
|
||||
This function assigns the given value to the Process Stack Pointer (PSP).
|
||||
|
||||
\param [in] topOfProcStack Process Stack Pointer value to set
|
||||
*/
|
||||
static __INLINE void __set_PSP(uint32_t topOfProcStack)
|
||||
{
|
||||
register uint32_t __regProcessStackPointer __ASM("psp");
|
||||
__regProcessStackPointer = topOfProcStack;
|
||||
}
|
||||
|
||||
|
||||
/** \brief Get Main Stack Pointer
|
||||
|
||||
This function returns the current value of the Main Stack Pointer (MSP).
|
||||
|
||||
\return MSP Register value
|
||||
*/
|
||||
static __INLINE uint32_t __get_MSP(void)
|
||||
{
|
||||
register uint32_t __regMainStackPointer __ASM("msp");
|
||||
return(__regMainStackPointer);
|
||||
}
|
||||
|
||||
|
||||
/** \brief Set Main Stack Pointer
|
||||
|
||||
This function assigns the given value to the Main Stack Pointer (MSP).
|
||||
|
||||
\param [in] topOfMainStack Main Stack Pointer value to set
|
||||
*/
|
||||
static __INLINE void __set_MSP(uint32_t topOfMainStack)
|
||||
{
|
||||
register uint32_t __regMainStackPointer __ASM("msp");
|
||||
__regMainStackPointer = topOfMainStack;
|
||||
}
|
||||
|
||||
|
||||
/** \brief Get Priority Mask
|
||||
|
||||
This function returns the current state of the priority mask bit from the Priority Mask Register.
|
||||
|
||||
\return Priority Mask value
|
||||
*/
|
||||
static __INLINE uint32_t __get_PRIMASK(void)
|
||||
{
|
||||
register uint32_t __regPriMask __ASM("primask");
|
||||
return(__regPriMask);
|
||||
}
|
||||
|
||||
|
||||
/** \brief Set Priority Mask
|
||||
|
||||
This function assigns the given value to the Priority Mask Register.
|
||||
|
||||
\param [in] priMask Priority Mask
|
||||
*/
|
||||
static __INLINE void __set_PRIMASK(uint32_t priMask)
|
||||
{
|
||||
register uint32_t __regPriMask __ASM("primask");
|
||||
__regPriMask = (priMask);
|
||||
}
|
||||
|
||||
|
||||
#if (__CORTEX_M >= 0x03)
|
||||
|
||||
/** \brief Enable FIQ
|
||||
|
||||
This function enables FIQ interrupts by clearing the F-bit in the CPSR.
|
||||
Can only be executed in Privileged modes.
|
||||
*/
|
||||
#define __enable_fault_irq __enable_fiq
|
||||
|
||||
|
||||
/** \brief Disable FIQ
|
||||
|
||||
This function disables FIQ interrupts by setting the F-bit in the CPSR.
|
||||
Can only be executed in Privileged modes.
|
||||
*/
|
||||
#define __disable_fault_irq __disable_fiq
|
||||
|
||||
|
||||
/** \brief Get Base Priority
|
||||
|
||||
This function returns the current value of the Base Priority register.
|
||||
|
||||
\return Base Priority register value
|
||||
*/
|
||||
static __INLINE uint32_t __get_BASEPRI(void)
|
||||
{
|
||||
register uint32_t __regBasePri __ASM("basepri");
|
||||
return(__regBasePri);
|
||||
}
|
||||
|
||||
|
||||
/** \brief Set Base Priority
|
||||
|
||||
This function assigns the given value to the Base Priority register.
|
||||
|
||||
\param [in] basePri Base Priority value to set
|
||||
*/
|
||||
static __INLINE void __set_BASEPRI(uint32_t basePri)
|
||||
{
|
||||
register uint32_t __regBasePri __ASM("basepri");
|
||||
__regBasePri = (basePri & 0xff);
|
||||
}
|
||||
|
||||
|
||||
/** \brief Get Fault Mask
|
||||
|
||||
This function returns the current value of the Fault Mask register.
|
||||
|
||||
\return Fault Mask register value
|
||||
*/
|
||||
static __INLINE uint32_t __get_FAULTMASK(void)
|
||||
{
|
||||
register uint32_t __regFaultMask __ASM("faultmask");
|
||||
return(__regFaultMask);
|
||||
}
|
||||
|
||||
|
||||
/** \brief Set Fault Mask
|
||||
|
||||
This function assigns the given value to the Fault Mask register.
|
||||
|
||||
\param [in] faultMask Fault Mask value to set
|
||||
*/
|
||||
static __INLINE void __set_FAULTMASK(uint32_t faultMask)
|
||||
{
|
||||
register uint32_t __regFaultMask __ASM("faultmask");
|
||||
__regFaultMask = (faultMask & (uint32_t)1);
|
||||
}
|
||||
|
||||
#endif /* (__CORTEX_M >= 0x03) */
|
||||
|
||||
|
||||
#if (__CORTEX_M == 0x04)
|
||||
|
||||
/** \brief Get FPSCR
|
||||
|
||||
This function returns the current value of the Floating Point Status/Control register.
|
||||
|
||||
\return Floating Point Status/Control register value
|
||||
*/
|
||||
static __INLINE uint32_t __get_FPSCR(void)
|
||||
{
|
||||
#if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
|
||||
register uint32_t __regfpscr __ASM("fpscr");
|
||||
return(__regfpscr);
|
||||
#else
|
||||
return(0);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
/** \brief Set FPSCR
|
||||
|
||||
This function assigns the given value to the Floating Point Status/Control register.
|
||||
|
||||
\param [in] fpscr Floating Point Status/Control value to set
|
||||
*/
|
||||
static __INLINE void __set_FPSCR(uint32_t fpscr)
|
||||
{
|
||||
#if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
|
||||
register uint32_t __regfpscr __ASM("fpscr");
|
||||
__regfpscr = (fpscr);
|
||||
#endif
|
||||
}
|
||||
|
||||
#endif /* (__CORTEX_M == 0x04) */
|
||||
|
||||
|
||||
#elif defined ( __ICCARM__ ) /*------------------ ICC Compiler -------------------*/
|
||||
/* IAR iccarm specific functions */
|
||||
|
||||
#include <cmsis_iar.h>
|
||||
|
||||
#elif defined ( __GNUC__ ) /*------------------ GNU Compiler ---------------------*/
|
||||
/* GNU gcc specific functions */
|
||||
|
||||
/** \brief Enable IRQ Interrupts
|
||||
|
||||
This function enables IRQ interrupts by clearing the I-bit in the CPSR.
|
||||
Can only be executed in Privileged modes.
|
||||
*/
|
||||
__attribute__( ( always_inline ) ) static __INLINE void __enable_irq(void)
|
||||
{
|
||||
__ASM volatile ("cpsie i");
|
||||
}
|
||||
|
||||
|
||||
/** \brief Disable IRQ Interrupts
|
||||
|
||||
This function disables IRQ interrupts by setting the I-bit in the CPSR.
|
||||
Can only be executed in Privileged modes.
|
||||
*/
|
||||
__attribute__( ( always_inline ) ) static __INLINE void __disable_irq(void)
|
||||
{
|
||||
__ASM volatile ("cpsid i");
|
||||
}
|
||||
|
||||
|
||||
/** \brief Get Control Register
|
||||
|
||||
This function returns the content of the Control Register.
|
||||
|
||||
\return Control Register value
|
||||
*/
|
||||
__attribute__( ( always_inline ) ) static __INLINE uint32_t __get_CONTROL(void)
|
||||
{
|
||||
uint32_t result;
|
||||
|
||||
__ASM volatile ("MRS %0, control" : "=r" (result) );
|
||||
return(result);
|
||||
}
|
||||
|
||||
|
||||
/** \brief Set Control Register
|
||||
|
||||
This function writes the given value to the Control Register.
|
||||
|
||||
\param [in] control Control Register value to set
|
||||
*/
|
||||
__attribute__( ( always_inline ) ) static __INLINE void __set_CONTROL(uint32_t control)
|
||||
{
|
||||
__ASM volatile ("MSR control, %0" : : "r" (control) );
|
||||
}
|
||||
|
||||
|
||||
/** \brief Get ISPR Register
|
||||
|
||||
This function returns the content of the ISPR Register.
|
||||
|
||||
\return ISPR Register value
|
||||
*/
|
||||
__attribute__( ( always_inline ) ) static __INLINE uint32_t __get_IPSR(void)
|
||||
{
|
||||
uint32_t result;
|
||||
|
||||
__ASM volatile ("MRS %0, ipsr" : "=r" (result) );
|
||||
return(result);
|
||||
}
|
||||
|
||||
|
||||
/** \brief Get APSR Register
|
||||
|
||||
This function returns the content of the APSR Register.
|
||||
|
||||
\return APSR Register value
|
||||
*/
|
||||
__attribute__( ( always_inline ) ) static __INLINE uint32_t __get_APSR(void)
|
||||
{
|
||||
uint32_t result;
|
||||
|
||||
__ASM volatile ("MRS %0, apsr" : "=r" (result) );
|
||||
return(result);
|
||||
}
|
||||
|
||||
|
||||
/** \brief Get xPSR Register
|
||||
|
||||
This function returns the content of the xPSR Register.
|
||||
|
||||
\return xPSR Register value
|
||||
*/
|
||||
__attribute__( ( always_inline ) ) static __INLINE uint32_t __get_xPSR(void)
|
||||
{
|
||||
uint32_t result;
|
||||
|
||||
__ASM volatile ("MRS %0, xpsr" : "=r" (result) );
|
||||
return(result);
|
||||
}
|
||||
|
||||
|
||||
/** \brief Get Process Stack Pointer
|
||||
|
||||
This function returns the current value of the Process Stack Pointer (PSP).
|
||||
|
||||
\return PSP Register value
|
||||
*/
|
||||
__attribute__( ( always_inline ) ) static __INLINE uint32_t __get_PSP(void)
|
||||
{
|
||||
register uint32_t result;
|
||||
|
||||
__ASM volatile ("MRS %0, psp\n" : "=r" (result) );
|
||||
return(result);
|
||||
}
|
||||
|
||||
|
||||
/** \brief Set Process Stack Pointer
|
||||
|
||||
This function assigns the given value to the Process Stack Pointer (PSP).
|
||||
|
||||
\param [in] topOfProcStack Process Stack Pointer value to set
|
||||
*/
|
||||
__attribute__( ( always_inline ) ) static __INLINE void __set_PSP(uint32_t topOfProcStack)
|
||||
{
|
||||
__ASM volatile ("MSR psp, %0\n" : : "r" (topOfProcStack) );
|
||||
}
|
||||
|
||||
|
||||
/** \brief Get Main Stack Pointer
|
||||
|
||||
This function returns the current value of the Main Stack Pointer (MSP).
|
||||
|
||||
\return MSP Register value
|
||||
*/
|
||||
__attribute__( ( always_inline ) ) static __INLINE uint32_t __get_MSP(void)
|
||||
{
|
||||
register uint32_t result;
|
||||
|
||||
__ASM volatile ("MRS %0, msp\n" : "=r" (result) );
|
||||
return(result);
|
||||
}
|
||||
|
||||
|
||||
/** \brief Set Main Stack Pointer
|
||||
|
||||
This function assigns the given value to the Main Stack Pointer (MSP).
|
||||
|
||||
\param [in] topOfMainStack Main Stack Pointer value to set
|
||||
*/
|
||||
__attribute__( ( always_inline ) ) static __INLINE void __set_MSP(uint32_t topOfMainStack)
|
||||
{
|
||||
__ASM volatile ("MSR msp, %0\n" : : "r" (topOfMainStack) );
|
||||
}
|
||||
|
||||
|
||||
/** \brief Get Priority Mask
|
||||
|
||||
This function returns the current state of the priority mask bit from the Priority Mask Register.
|
||||
|
||||
\return Priority Mask value
|
||||
*/
|
||||
__attribute__( ( always_inline ) ) static __INLINE uint32_t __get_PRIMASK(void)
|
||||
{
|
||||
uint32_t result;
|
||||
|
||||
__ASM volatile ("MRS %0, primask" : "=r" (result) );
|
||||
return(result);
|
||||
}
|
||||
|
||||
|
||||
/** \brief Set Priority Mask
|
||||
|
||||
This function assigns the given value to the Priority Mask Register.
|
||||
|
||||
\param [in] priMask Priority Mask
|
||||
*/
|
||||
__attribute__( ( always_inline ) ) static __INLINE void __set_PRIMASK(uint32_t priMask)
|
||||
{
|
||||
__ASM volatile ("MSR primask, %0" : : "r" (priMask) );
|
||||
}
|
||||
|
||||
|
||||
#if (__CORTEX_M >= 0x03)
|
||||
|
||||
/** \brief Enable FIQ
|
||||
|
||||
This function enables FIQ interrupts by clearing the F-bit in the CPSR.
|
||||
Can only be executed in Privileged modes.
|
||||
*/
|
||||
__attribute__( ( always_inline ) ) static __INLINE void __enable_fault_irq(void)
|
||||
{
|
||||
__ASM volatile ("cpsie f");
|
||||
}
|
||||
|
||||
|
||||
/** \brief Disable FIQ
|
||||
|
||||
This function disables FIQ interrupts by setting the F-bit in the CPSR.
|
||||
Can only be executed in Privileged modes.
|
||||
*/
|
||||
__attribute__( ( always_inline ) ) static __INLINE void __disable_fault_irq(void)
|
||||
{
|
||||
__ASM volatile ("cpsid f");
|
||||
}
|
||||
|
||||
|
||||
/** \brief Get Base Priority
|
||||
|
||||
This function returns the current value of the Base Priority register.
|
||||
|
||||
\return Base Priority register value
|
||||
*/
|
||||
__attribute__( ( always_inline ) ) static __INLINE uint32_t __get_BASEPRI(void)
|
||||
{
|
||||
uint32_t result;
|
||||
|
||||
__ASM volatile ("MRS %0, basepri_max" : "=r" (result) );
|
||||
return(result);
|
||||
}
|
||||
|
||||
|
||||
/** \brief Set Base Priority
|
||||
|
||||
This function assigns the given value to the Base Priority register.
|
||||
|
||||
\param [in] basePri Base Priority value to set
|
||||
*/
|
||||
__attribute__( ( always_inline ) ) static __INLINE void __set_BASEPRI(uint32_t value)
|
||||
{
|
||||
__ASM volatile ("MSR basepri, %0" : : "r" (value) );
|
||||
}
|
||||
|
||||
|
||||
/** \brief Get Fault Mask
|
||||
|
||||
This function returns the current value of the Fault Mask register.
|
||||
|
||||
\return Fault Mask register value
|
||||
*/
|
||||
__attribute__( ( always_inline ) ) static __INLINE uint32_t __get_FAULTMASK(void)
|
||||
{
|
||||
uint32_t result;
|
||||
|
||||
__ASM volatile ("MRS %0, faultmask" : "=r" (result) );
|
||||
return(result);
|
||||
}
|
||||
|
||||
|
||||
/** \brief Set Fault Mask
|
||||
|
||||
This function assigns the given value to the Fault Mask register.
|
||||
|
||||
\param [in] faultMask Fault Mask value to set
|
||||
*/
|
||||
__attribute__( ( always_inline ) ) static __INLINE void __set_FAULTMASK(uint32_t faultMask)
|
||||
{
|
||||
__ASM volatile ("MSR faultmask, %0" : : "r" (faultMask) );
|
||||
}
|
||||
|
||||
#endif /* (__CORTEX_M >= 0x03) */
|
||||
|
||||
|
||||
#if (__CORTEX_M == 0x04)
|
||||
|
||||
/** \brief Get FPSCR
|
||||
|
||||
This function returns the current value of the Floating Point Status/Control register.
|
||||
|
||||
\return Floating Point Status/Control register value
|
||||
*/
|
||||
__attribute__( ( always_inline ) ) static __INLINE uint32_t __get_FPSCR(void)
|
||||
{
|
||||
#if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
|
||||
uint32_t result;
|
||||
|
||||
__ASM volatile ("VMRS %0, fpscr" : "=r" (result) );
|
||||
return(result);
|
||||
#else
|
||||
return(0);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
/** \brief Set FPSCR
|
||||
|
||||
This function assigns the given value to the Floating Point Status/Control register.
|
||||
|
||||
\param [in] fpscr Floating Point Status/Control value to set
|
||||
*/
|
||||
__attribute__( ( always_inline ) ) static __INLINE void __set_FPSCR(uint32_t fpscr)
|
||||
{
|
||||
#if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
|
||||
__ASM volatile ("VMSR fpscr, %0" : : "r" (fpscr) );
|
||||
#endif
|
||||
}
|
||||
|
||||
#endif /* (__CORTEX_M == 0x04) */
|
||||
|
||||
|
||||
#elif defined ( __TASKING__ ) /*------------------ TASKING Compiler --------------*/
|
||||
/* TASKING carm specific functions */
|
||||
|
||||
/*
|
||||
* The CMSIS functions have been implemented as intrinsics in the compiler.
|
||||
* Please use "carm -?i" to get an up to date list of all instrinsics,
|
||||
* Including the CMSIS ones.
|
||||
*/
|
||||
|
||||
#endif
|
||||
|
||||
/*@} end of CMSIS_Core_RegAccFunctions */
|
||||
|
||||
|
||||
#endif /* __CORE_CMFUNC_H */
|
||||
|
|
@ -0,0 +1,585 @@
|
|||
/**************************************************************************//**
|
||||
* @file core_cmInstr.h
|
||||
* @brief CMSIS Cortex-M Core Instruction Access Header File
|
||||
* @version V2.10
|
||||
* @date 19. July 2011
|
||||
*
|
||||
* @note
|
||||
* Copyright (C) 2009-2011 ARM Limited. All rights reserved.
|
||||
*
|
||||
* @par
|
||||
* ARM Limited (ARM) is supplying this software for use with Cortex-M
|
||||
* processor based microcontrollers. This file can be freely distributed
|
||||
* within development tools that are supporting such ARM based processors.
|
||||
*
|
||||
* @par
|
||||
* THIS SOFTWARE IS PROVIDED "AS IS". NO WARRANTIES, WHETHER EXPRESS, IMPLIED
|
||||
* OR STATUTORY, INCLUDING, BUT NOT LIMITED TO, IMPLIED WARRANTIES OF
|
||||
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE APPLY TO THIS SOFTWARE.
|
||||
* ARM SHALL NOT, IN ANY CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL, OR
|
||||
* CONSEQUENTIAL DAMAGES, FOR ANY REASON WHATSOEVER.
|
||||
*
|
||||
******************************************************************************/
|
||||
|
||||
#ifndef __CORE_CMINSTR_H
|
||||
#define __CORE_CMINSTR_H
|
||||
|
||||
|
||||
/* ########################## Core Instruction Access ######################### */
|
||||
/** \defgroup CMSIS_Core_InstructionInterface CMSIS Core Instruction Interface
|
||||
Access to dedicated instructions
|
||||
@{
|
||||
*/
|
||||
|
||||
#if defined ( __CC_ARM ) /*------------------RealView Compiler -----------------*/
|
||||
/* ARM armcc specific functions */
|
||||
|
||||
#if (__ARMCC_VERSION < 400677)
|
||||
#error "Please use ARM Compiler Toolchain V4.0.677 or later!"
|
||||
#endif
|
||||
|
||||
|
||||
/** \brief No Operation
|
||||
|
||||
No Operation does nothing. This instruction can be used for code alignment purposes.
|
||||
*/
|
||||
#define __NOP __nop
|
||||
|
||||
|
||||
/** \brief Wait For Interrupt
|
||||
|
||||
Wait For Interrupt is a hint instruction that suspends execution
|
||||
until one of a number of events occurs.
|
||||
*/
|
||||
#define __WFI __wfi
|
||||
|
||||
|
||||
/** \brief Wait For Event
|
||||
|
||||
Wait For Event is a hint instruction that permits the processor to enter
|
||||
a low-power state until one of a number of events occurs.
|
||||
*/
|
||||
#define __WFE __wfe
|
||||
|
||||
|
||||
/** \brief Send Event
|
||||
|
||||
Send Event is a hint instruction. It causes an event to be signaled to the CPU.
|
||||
*/
|
||||
#define __SEV __sev
|
||||
|
||||
|
||||
/** \brief Instruction Synchronization Barrier
|
||||
|
||||
Instruction Synchronization Barrier flushes the pipeline in the processor,
|
||||
so that all instructions following the ISB are fetched from cache or
|
||||
memory, after the instruction has been completed.
|
||||
*/
|
||||
#define __ISB() __isb(0xF)
|
||||
|
||||
|
||||
/** \brief Data Synchronization Barrier
|
||||
|
||||
This function acts as a special kind of Data Memory Barrier.
|
||||
It completes when all explicit memory accesses before this instruction complete.
|
||||
*/
|
||||
#define __DSB() __dsb(0xF)
|
||||
|
||||
|
||||
/** \brief Data Memory Barrier
|
||||
|
||||
This function ensures the apparent order of the explicit memory operations before
|
||||
and after the instruction, without ensuring their completion.
|
||||
*/
|
||||
#define __DMB() __dmb(0xF)
|
||||
|
||||
|
||||
/** \brief Reverse byte order (32 bit)
|
||||
|
||||
This function reverses the byte order in integer value.
|
||||
|
||||
\param [in] value Value to reverse
|
||||
\return Reversed value
|
||||
*/
|
||||
#define __REV __rev
|
||||
|
||||
|
||||
/** \brief Reverse byte order (16 bit)
|
||||
|
||||
This function reverses the byte order in two unsigned short values.
|
||||
|
||||
\param [in] value Value to reverse
|
||||
\return Reversed value
|
||||
*/
|
||||
static __INLINE __ASM uint32_t __REV16(uint32_t value)
|
||||
{
|
||||
rev16 r0, r0
|
||||
bx lr
|
||||
}
|
||||
|
||||
|
||||
/** \brief Reverse byte order in signed short value
|
||||
|
||||
This function reverses the byte order in a signed short value with sign extension to integer.
|
||||
|
||||
\param [in] value Value to reverse
|
||||
\return Reversed value
|
||||
*/
|
||||
static __INLINE __ASM int32_t __REVSH(int32_t value)
|
||||
{
|
||||
revsh r0, r0
|
||||
bx lr
|
||||
}
|
||||
|
||||
|
||||
#if (__CORTEX_M >= 0x03)
|
||||
|
||||
/** \brief Reverse bit order of value
|
||||
|
||||
This function reverses the bit order of the given value.
|
||||
|
||||
\param [in] value Value to reverse
|
||||
\return Reversed value
|
||||
*/
|
||||
#define __RBIT __rbit
|
||||
|
||||
|
||||
/** \brief LDR Exclusive (8 bit)
|
||||
|
||||
This function performs a exclusive LDR command for 8 bit value.
|
||||
|
||||
\param [in] ptr Pointer to data
|
||||
\return value of type uint8_t at (*ptr)
|
||||
*/
|
||||
#define __LDREXB(ptr) ((uint8_t ) __ldrex(ptr))
|
||||
|
||||
|
||||
/** \brief LDR Exclusive (16 bit)
|
||||
|
||||
This function performs a exclusive LDR command for 16 bit values.
|
||||
|
||||
\param [in] ptr Pointer to data
|
||||
\return value of type uint16_t at (*ptr)
|
||||
*/
|
||||
#define __LDREXH(ptr) ((uint16_t) __ldrex(ptr))
|
||||
|
||||
|
||||
/** \brief LDR Exclusive (32 bit)
|
||||
|
||||
This function performs a exclusive LDR command for 32 bit values.
|
||||
|
||||
\param [in] ptr Pointer to data
|
||||
\return value of type uint32_t at (*ptr)
|
||||
*/
|
||||
#define __LDREXW(ptr) ((uint32_t ) __ldrex(ptr))
|
||||
|
||||
|
||||
/** \brief STR Exclusive (8 bit)
|
||||
|
||||
This function performs a exclusive STR command for 8 bit values.
|
||||
|
||||
\param [in] value Value to store
|
||||
\param [in] ptr Pointer to location
|
||||
\return 0 Function succeeded
|
||||
\return 1 Function failed
|
||||
*/
|
||||
#define __STREXB(value, ptr) __strex(value, ptr)
|
||||
|
||||
|
||||
/** \brief STR Exclusive (16 bit)
|
||||
|
||||
This function performs a exclusive STR command for 16 bit values.
|
||||
|
||||
\param [in] value Value to store
|
||||
\param [in] ptr Pointer to location
|
||||
\return 0 Function succeeded
|
||||
\return 1 Function failed
|
||||
*/
|
||||
#define __STREXH(value, ptr) __strex(value, ptr)
|
||||
|
||||
|
||||
/** \brief STR Exclusive (32 bit)
|
||||
|
||||
This function performs a exclusive STR command for 32 bit values.
|
||||
|
||||
\param [in] value Value to store
|
||||
\param [in] ptr Pointer to location
|
||||
\return 0 Function succeeded
|
||||
\return 1 Function failed
|
||||
*/
|
||||
#define __STREXW(value, ptr) __strex(value, ptr)
|
||||
|
||||
|
||||
/** \brief Remove the exclusive lock
|
||||
|
||||
This function removes the exclusive lock which is created by LDREX.
|
||||
|
||||
*/
|
||||
#define __CLREX __clrex
|
||||
|
||||
|
||||
/** \brief Signed Saturate
|
||||
|
||||
This function saturates a signed value.
|
||||
|
||||
\param [in] value Value to be saturated
|
||||
\param [in] sat Bit position to saturate to (1..32)
|
||||
\return Saturated value
|
||||
*/
|
||||
#define __SSAT __ssat
|
||||
|
||||
|
||||
/** \brief Unsigned Saturate
|
||||
|
||||
This function saturates an unsigned value.
|
||||
|
||||
\param [in] value Value to be saturated
|
||||
\param [in] sat Bit position to saturate to (0..31)
|
||||
\return Saturated value
|
||||
*/
|
||||
#define __USAT __usat
|
||||
|
||||
|
||||
/** \brief Count leading zeros
|
||||
|
||||
This function counts the number of leading zeros of a data value.
|
||||
|
||||
\param [in] value Value to count the leading zeros
|
||||
\return number of leading zeros in value
|
||||
*/
|
||||
#define __CLZ __clz
|
||||
|
||||
#endif /* (__CORTEX_M >= 0x03) */
|
||||
|
||||
|
||||
|
||||
#elif defined ( __ICCARM__ ) /*------------------ ICC Compiler -------------------*/
|
||||
/* IAR iccarm specific functions */
|
||||
|
||||
#include <cmsis_iar.h>
|
||||
|
||||
|
||||
#elif defined ( __GNUC__ ) /*------------------ GNU Compiler ---------------------*/
|
||||
/* GNU gcc specific functions */
|
||||
|
||||
/** \brief No Operation
|
||||
|
||||
No Operation does nothing. This instruction can be used for code alignment purposes.
|
||||
*/
|
||||
__attribute__( ( always_inline ) ) static __INLINE void __NOP(void)
|
||||
{
|
||||
__ASM volatile ("nop");
|
||||
}
|
||||
|
||||
|
||||
/** \brief Wait For Interrupt
|
||||
|
||||
Wait For Interrupt is a hint instruction that suspends execution
|
||||
until one of a number of events occurs.
|
||||
*/
|
||||
__attribute__( ( always_inline ) ) static __INLINE void __WFI(void)
|
||||
{
|
||||
__ASM volatile ("wfi");
|
||||
}
|
||||
|
||||
|
||||
/** \brief Wait For Event
|
||||
|
||||
Wait For Event is a hint instruction that permits the processor to enter
|
||||
a low-power state until one of a number of events occurs.
|
||||
*/
|
||||
__attribute__( ( always_inline ) ) static __INLINE void __WFE(void)
|
||||
{
|
||||
__ASM volatile ("wfe");
|
||||
}
|
||||
|
||||
|
||||
/** \brief Send Event
|
||||
|
||||
Send Event is a hint instruction. It causes an event to be signaled to the CPU.
|
||||
*/
|
||||
__attribute__( ( always_inline ) ) static __INLINE void __SEV(void)
|
||||
{
|
||||
__ASM volatile ("sev");
|
||||
}
|
||||
|
||||
|
||||
/** \brief Instruction Synchronization Barrier
|
||||
|
||||
Instruction Synchronization Barrier flushes the pipeline in the processor,
|
||||
so that all instructions following the ISB are fetched from cache or
|
||||
memory, after the instruction has been completed.
|
||||
*/
|
||||
__attribute__( ( always_inline ) ) static __INLINE void __ISB(void)
|
||||
{
|
||||
__ASM volatile ("isb");
|
||||
}
|
||||
|
||||
|
||||
/** \brief Data Synchronization Barrier
|
||||
|
||||
This function acts as a special kind of Data Memory Barrier.
|
||||
It completes when all explicit memory accesses before this instruction complete.
|
||||
*/
|
||||
__attribute__( ( always_inline ) ) static __INLINE void __DSB(void)
|
||||
{
|
||||
__ASM volatile ("dsb");
|
||||
}
|
||||
|
||||
|
||||
/** \brief Data Memory Barrier
|
||||
|
||||
This function ensures the apparent order of the explicit memory operations before
|
||||
and after the instruction, without ensuring their completion.
|
||||
*/
|
||||
__attribute__( ( always_inline ) ) static __INLINE void __DMB(void)
|
||||
{
|
||||
__ASM volatile ("dmb");
|
||||
}
|
||||
|
||||
|
||||
/** \brief Reverse byte order (32 bit)
|
||||
|
||||
This function reverses the byte order in integer value.
|
||||
|
||||
\param [in] value Value to reverse
|
||||
\return Reversed value
|
||||
*/
|
||||
__attribute__( ( always_inline ) ) static __INLINE uint32_t __REV(uint32_t value)
|
||||
{
|
||||
uint32_t result;
|
||||
|
||||
__ASM volatile ("rev %0, %1" : "=r" (result) : "r" (value) );
|
||||
return(result);
|
||||
}
|
||||
|
||||
|
||||
/** \brief Reverse byte order (16 bit)
|
||||
|
||||
This function reverses the byte order in two unsigned short values.
|
||||
|
||||
\param [in] value Value to reverse
|
||||
\return Reversed value
|
||||
*/
|
||||
__attribute__( ( always_inline ) ) static __INLINE uint32_t __REV16(uint32_t value)
|
||||
{
|
||||
uint32_t result;
|
||||
|
||||
__ASM volatile ("rev16 %0, %1" : "=r" (result) : "r" (value) );
|
||||
return(result);
|
||||
}
|
||||
|
||||
|
||||
/** \brief Reverse byte order in signed short value
|
||||
|
||||
This function reverses the byte order in a signed short value with sign extension to integer.
|
||||
|
||||
\param [in] value Value to reverse
|
||||
\return Reversed value
|
||||
*/
|
||||
__attribute__( ( always_inline ) ) static __INLINE int32_t __REVSH(int32_t value)
|
||||
{
|
||||
uint32_t result;
|
||||
|
||||
__ASM volatile ("revsh %0, %1" : "=r" (result) : "r" (value) );
|
||||
return((int32_t)result);
|
||||
}
|
||||
|
||||
|
||||
#if (__CORTEX_M >= 0x03)
|
||||
|
||||
/** \brief Reverse bit order of value
|
||||
|
||||
This function reverses the bit order of the given value.
|
||||
|
||||
\param [in] value Value to reverse
|
||||
\return Reversed value
|
||||
*/
|
||||
__attribute__( ( always_inline ) ) static __INLINE uint32_t __RBIT(uint32_t value)
|
||||
{
|
||||
uint32_t result;
|
||||
|
||||
__ASM volatile ("rbit %0, %1" : "=r" (result) : "r" (value) );
|
||||
return(result);
|
||||
}
|
||||
|
||||
|
||||
/** \brief LDR Exclusive (8 bit)
|
||||
|
||||
This function performs a exclusive LDR command for 8 bit value.
|
||||
|
||||
\param [in] ptr Pointer to data
|
||||
\return value of type uint8_t at (*ptr)
|
||||
*/
|
||||
__attribute__( ( always_inline ) ) static __INLINE uint8_t __LDREXB(volatile uint8_t *addr)
|
||||
{
|
||||
uint8_t result;
|
||||
|
||||
__ASM volatile ("ldrexb %0, [%1]" : "=r" (result) : "r" (addr) );
|
||||
return(result);
|
||||
}
|
||||
|
||||
|
||||
/** \brief LDR Exclusive (16 bit)
|
||||
|
||||
This function performs a exclusive LDR command for 16 bit values.
|
||||
|
||||
\param [in] ptr Pointer to data
|
||||
\return value of type uint16_t at (*ptr)
|
||||
*/
|
||||
__attribute__( ( always_inline ) ) static __INLINE uint16_t __LDREXH(volatile uint16_t *addr)
|
||||
{
|
||||
uint16_t result;
|
||||
|
||||
__ASM volatile ("ldrexh %0, [%1]" : "=r" (result) : "r" (addr) );
|
||||
return(result);
|
||||
}
|
||||
|
||||
|
||||
/** \brief LDR Exclusive (32 bit)
|
||||
|
||||
This function performs a exclusive LDR command for 32 bit values.
|
||||
|
||||
\param [in] ptr Pointer to data
|
||||
\return value of type uint32_t at (*ptr)
|
||||
*/
|
||||
__attribute__( ( always_inline ) ) static __INLINE uint32_t __LDREXW(volatile uint32_t *addr)
|
||||
{
|
||||
uint32_t result;
|
||||
|
||||
__ASM volatile ("ldrex %0, [%1]" : "=r" (result) : "r" (addr) );
|
||||
return(result);
|
||||
}
|
||||
|
||||
|
||||
/** \brief STR Exclusive (8 bit)
|
||||
|
||||
This function performs a exclusive STR command for 8 bit values.
|
||||
|
||||
\param [in] value Value to store
|
||||
\param [in] ptr Pointer to location
|
||||
\return 0 Function succeeded
|
||||
\return 1 Function failed
|
||||
*/
|
||||
__attribute__( ( always_inline ) ) static __INLINE uint32_t __STREXB(uint8_t value, volatile uint8_t *addr)
|
||||
{
|
||||
uint32_t result;
|
||||
|
||||
__ASM volatile ("strexb %0, %2, [%1]" : "=r" (result) : "r" (addr), "r" (value) );
|
||||
return(result);
|
||||
}
|
||||
|
||||
|
||||
/** \brief STR Exclusive (16 bit)
|
||||
|
||||
This function performs a exclusive STR command for 16 bit values.
|
||||
|
||||
\param [in] value Value to store
|
||||
\param [in] ptr Pointer to location
|
||||
\return 0 Function succeeded
|
||||
\return 1 Function failed
|
||||
*/
|
||||
__attribute__( ( always_inline ) ) static __INLINE uint32_t __STREXH(uint16_t value, volatile uint16_t *addr)
|
||||
{
|
||||
uint32_t result;
|
||||
|
||||
__ASM volatile ("strexh %0, %2, [%1]" : "=r" (result) : "r" (addr), "r" (value) );
|
||||
return(result);
|
||||
}
|
||||
|
||||
|
||||
/** \brief STR Exclusive (32 bit)
|
||||
|
||||
This function performs a exclusive STR command for 32 bit values.
|
||||
|
||||
\param [in] value Value to store
|
||||
\param [in] ptr Pointer to location
|
||||
\return 0 Function succeeded
|
||||
\return 1 Function failed
|
||||
*/
|
||||
__attribute__( ( always_inline ) ) static __INLINE uint32_t __STREXW(uint32_t value, volatile uint32_t *addr)
|
||||
{
|
||||
uint32_t result;
|
||||
|
||||
__ASM volatile ("strex %0, %2, [%1]" : "=r" (result) : "r" (addr), "r" (value) );
|
||||
return(result);
|
||||
}
|
||||
|
||||
|
||||
/** \brief Remove the exclusive lock
|
||||
|
||||
This function removes the exclusive lock which is created by LDREX.
|
||||
|
||||
*/
|
||||
__attribute__( ( always_inline ) ) static __INLINE void __CLREX(void)
|
||||
{
|
||||
__ASM volatile ("clrex");
|
||||
}
|
||||
|
||||
|
||||
/** \brief Signed Saturate
|
||||
|
||||
This function saturates a signed value.
|
||||
|
||||
\param [in] value Value to be saturated
|
||||
\param [in] sat Bit position to saturate to (1..32)
|
||||
\return Saturated value
|
||||
*/
|
||||
#define __SSAT(ARG1,ARG2) \
|
||||
({ \
|
||||
uint32_t __RES, __ARG1 = (ARG1); \
|
||||
__ASM ("ssat %0, %1, %2" : "=r" (__RES) : "I" (ARG2), "r" (__ARG1) ); \
|
||||
__RES; \
|
||||
})
|
||||
|
||||
|
||||
/** \brief Unsigned Saturate
|
||||
|
||||
This function saturates an unsigned value.
|
||||
|
||||
\param [in] value Value to be saturated
|
||||
\param [in] sat Bit position to saturate to (0..31)
|
||||
\return Saturated value
|
||||
*/
|
||||
#define __USAT(ARG1,ARG2) \
|
||||
({ \
|
||||
uint32_t __RES, __ARG1 = (ARG1); \
|
||||
__ASM ("usat %0, %1, %2" : "=r" (__RES) : "I" (ARG2), "r" (__ARG1) ); \
|
||||
__RES; \
|
||||
})
|
||||
|
||||
|
||||
/** \brief Count leading zeros
|
||||
|
||||
This function counts the number of leading zeros of a data value.
|
||||
|
||||
\param [in] value Value to count the leading zeros
|
||||
\return number of leading zeros in value
|
||||
*/
|
||||
__attribute__( ( always_inline ) ) static __INLINE uint8_t __CLZ(uint32_t value)
|
||||
{
|
||||
uint8_t result;
|
||||
|
||||
__ASM volatile ("clz %0, %1" : "=r" (result) : "r" (value) );
|
||||
return(result);
|
||||
}
|
||||
|
||||
#endif /* (__CORTEX_M >= 0x03) */
|
||||
|
||||
|
||||
|
||||
|
||||
#elif defined ( __TASKING__ ) /*------------------ TASKING Compiler --------------*/
|
||||
/* TASKING carm specific functions */
|
||||
|
||||
/*
|
||||
* The CMSIS functions have been implemented as intrinsics in the compiler.
|
||||
* Please use "carm -?i" to get an up to date list of all intrinsics,
|
||||
* Including the CMSIS ones.
|
||||
*/
|
||||
|
||||
#endif
|
||||
|
||||
/*@}*/ /* end of group CMSIS_Core_InstructionInterface */
|
||||
|
||||
#endif /* __CORE_CMINSTR_H */
|
||||
File diff suppressed because it is too large
Load Diff
|
|
@ -0,0 +1,197 @@
|
|||
/**
|
||||
******************************************************************************
|
||||
* @file stm32f2xx.h
|
||||
* @author MCD Application Team
|
||||
* @version V2.0.1
|
||||
* @date 25-March-2014
|
||||
* @brief CMSIS STM32F2xx Device Peripheral Access Layer Header File.
|
||||
*
|
||||
* The file is the unique include file that the application programmer
|
||||
* is using in the C source code, usually in main.c. This file contains:
|
||||
* - Configuration section that allows to select:
|
||||
* - The STM32F2xx device used in the target application
|
||||
* - To use or not the peripheral's drivers in application code(i.e.
|
||||
* code will be based on direct access to peripheral's registers
|
||||
* rather than drivers API), this option is controlled by
|
||||
* "#define USE_HAL_DRIVER"
|
||||
*
|
||||
******************************************************************************
|
||||
* @attention
|
||||
*
|
||||
* <h2><center>© COPYRIGHT(c) 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.
|
||||
*
|
||||
******************************************************************************
|
||||
*/
|
||||
|
||||
/** @addtogroup CMSIS
|
||||
* @{
|
||||
*/
|
||||
|
||||
/** @addtogroup stm32f2xx
|
||||
* @{
|
||||
*/
|
||||
|
||||
#ifndef __STM32F2xx_H
|
||||
#define __STM32F2xx_H
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif /* __cplusplus */
|
||||
|
||||
/** @addtogroup Library_configuration_section
|
||||
* @{
|
||||
*/
|
||||
|
||||
/* Uncomment the line below according to the target STM32 device used in your
|
||||
application
|
||||
*/
|
||||
|
||||
#if !defined (STM32F205xx) && !defined (STM32F215xx) && !defined (STM32F207xx) && !defined (STM32F217xx)
|
||||
|
||||
/* #define STM32F205xx */ /*!< STM32Fxx Devices */
|
||||
/* #define STM32F215xx */ /*!< STM32Fxx Devices */
|
||||
/* #define STM32F207xx */ /*!< STM32Fxx Devices */
|
||||
/* #define STM32F217xx */ /*!< STM32Fxx Devices */
|
||||
|
||||
#endif
|
||||
|
||||
/* Tip: To avoid modifying this file each time you need to switch between these
|
||||
devices, you can define the device in your toolchain compiler preprocessor.
|
||||
*/
|
||||
#if !defined (USE_HAL_DRIVER)
|
||||
/**
|
||||
* @brief Comment the line below if you will not use the peripherals drivers.
|
||||
In this case, these drivers will not be included and the application code will
|
||||
be based on direct access to peripherals registers
|
||||
*/
|
||||
/*#define USE_HAL_DRIVER */
|
||||
#endif /* USE_HAL_DRIVER */
|
||||
|
||||
/**
|
||||
* @brief CMSIS Device version number V2.0.1
|
||||
*/
|
||||
#define __STM32F2xx_CMSIS_DEVICE_VERSION_MAIN (0x02) /*!< [31:24] main version */
|
||||
#define __STM32F2xx_CMSIS_DEVICE_VERSION_SUB1 (0x00) /*!< [23:16] sub1 version */
|
||||
#define __STM32F2xx_CMSIS_DEVICE_VERSION_SUB2 (0x00) /*!< [15:8] sub2 version */
|
||||
#define __STM32F2xx_CMSIS_DEVICE_VERSION_RC (0x00) /*!< [7:0] release candidate */
|
||||
#define __STM32F2xx_CMSIS_DEVICE_VERSION ((__CMSIS_DEVICE_VERSION_MAIN << 24)\
|
||||
|(__CMSIS_DEVICE_HAL_VERSION_SUB1 << 16)\
|
||||
|(__CMSIS_DEVICE_HAL_VERSION_SUB2 << 8 )\
|
||||
|(__CMSIS_DEVICE_HAL_VERSION_RC))
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
/** @addtogroup Device_Included
|
||||
* @{
|
||||
*/
|
||||
|
||||
#if defined(STM32F205xx)
|
||||
#include "stm32f205xx.h"
|
||||
#elif defined(STM32F215xx)
|
||||
#include "stm32f215xx.h"
|
||||
#elif defined(STM32F207xx)
|
||||
#include "stm32f207xx.h"
|
||||
#elif defined(STM32F217xx)
|
||||
#include "stm32f217xx.h"
|
||||
#else
|
||||
#error "Please select first the target STM32F2xx device used in your application (in stm32f2xx.h file)"
|
||||
#endif
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
/** @addtogroup Exported_types
|
||||
* @{
|
||||
*/
|
||||
typedef enum
|
||||
{
|
||||
RESET = 0,
|
||||
SET = !RESET
|
||||
} FlagStatus, ITStatus;
|
||||
|
||||
typedef enum
|
||||
{
|
||||
DISABLE = 0,
|
||||
ENABLE = !DISABLE
|
||||
} FunctionalState;
|
||||
#define IS_FUNCTIONAL_STATE(STATE) (((STATE) == DISABLE) || ((STATE) == ENABLE))
|
||||
|
||||
typedef enum
|
||||
{
|
||||
ERROR = 0,
|
||||
SUCCESS = !ERROR
|
||||
} ErrorStatus;
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
|
||||
/** @addtogroup Exported_macro
|
||||
* @{
|
||||
*/
|
||||
#define SET_BIT(REG, BIT) ((REG) |= (BIT))
|
||||
|
||||
#define CLEAR_BIT(REG, BIT) ((REG) &= ~(BIT))
|
||||
|
||||
#define READ_BIT(REG, BIT) ((REG) & (BIT))
|
||||
|
||||
#define CLEAR_REG(REG) ((REG) = (0x0))
|
||||
|
||||
#define WRITE_REG(REG, VAL) ((REG) = (VAL))
|
||||
|
||||
#define READ_REG(REG) ((REG))
|
||||
|
||||
#define MODIFY_REG(REG, CLEARMASK, SETMASK) WRITE_REG((REG), (((READ_REG(REG)) & (~(CLEARMASK))) | (SETMASK)))
|
||||
|
||||
#define POSITION_VAL(VAL) (__CLZ(__RBIT(VAL)))
|
||||
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif /* __cplusplus */
|
||||
|
||||
#endif /* __STM32F2xx_H */
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
|
||||
|
||||
|
||||
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
|
||||
|
|
@ -0,0 +1,99 @@
|
|||
/**
|
||||
******************************************************************************
|
||||
* @file system_stm32f2xx.h
|
||||
* @author MCD Application Team
|
||||
* @version V1.0.0
|
||||
* @date 18-April-2011
|
||||
* @brief CMSIS Cortex-M3 Device Peripheral Access Layer System Header File.
|
||||
******************************************************************************
|
||||
* @attention
|
||||
*
|
||||
* THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
|
||||
* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE
|
||||
* TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY
|
||||
* DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING
|
||||
* FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE
|
||||
* CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
|
||||
*
|
||||
* <h2><center>© COPYRIGHT 2011 STMicroelectronics</center></h2>
|
||||
******************************************************************************
|
||||
*/
|
||||
|
||||
/** @addtogroup CMSIS
|
||||
* @{
|
||||
*/
|
||||
|
||||
/** @addtogroup stm32f2xx_system
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* @brief Define to prevent recursive inclusion
|
||||
*/
|
||||
#ifndef __SYSTEM_STM32F2XX_H
|
||||
#define __SYSTEM_STM32F2XX_H
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/** @addtogroup STM32F2xx_System_Includes
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
|
||||
/** @addtogroup STM32F2xx_System_Exported_types
|
||||
* @{
|
||||
*/
|
||||
|
||||
extern uint32_t SystemCoreClock; /*!< System Clock Frequency (Core Clock) */
|
||||
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
/** @addtogroup STM32F2xx_System_Exported_Constants
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
/** @addtogroup STM32F2xx_System_Exported_Macros
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
/** @addtogroup STM32F2xx_System_Exported_Functions
|
||||
* @{
|
||||
*/
|
||||
|
||||
extern void SystemInit(void);
|
||||
extern void SystemCoreClockUpdate(void);
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /*__SYSTEM_STM32F2XX_H */
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
/******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF 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;
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
@ -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>© 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****/
|
||||
|
|
@ -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) }
|
||||
}
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
||||
Binary file not shown.
Binary file not shown.
|
|
@ -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)
|
||||
|
|
@ -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);
|
||||
}
|
||||
|
||||
|
|
@ -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"))
|
||||
|
||||
|
|
@ -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;
|
||||
File diff suppressed because it is too large
Load Diff
|
|
@ -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
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
|
|
@ -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,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)
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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,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
|
||||
|
|
@ -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")
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
def clip(x, lo, hi):
|
||||
return max(lo, min(hi, x))
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
import os
|
||||
DBC_PATH = os.path.dirname(os.path.abspath(__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...";
|
||||
|
|
@ -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
|
||||
|
|
@ -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...";
|
||||
|
|
@ -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
|
||||
|
||||
|
|
@ -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-----
|
||||
|
|
@ -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/
|
||||
|
||||
Binary file not shown.
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
|
|
@ -0,0 +1,681 @@
|
|||
//
|
||||
// Copyright (c) 2013 Mikko Mononen memon@inside.org
|
||||
//
|
||||
// This software is provided 'as-is', without any express or implied
|
||||
// warranty. In no event will the authors be held liable for any damages
|
||||
// arising from the use of this software.
|
||||
// Permission is granted to anyone to use this software for any purpose,
|
||||
// including commercial applications, and to alter it and redistribute it
|
||||
// freely, subject to the following restrictions:
|
||||
// 1. The origin of this software must not be misrepresented; you must not
|
||||
// claim that you wrote the original software. If you use this software
|
||||
// in a product, an acknowledgment in the product documentation would be
|
||||
// appreciated but is not required.
|
||||
// 2. Altered source versions must be plainly marked as such, and must not be
|
||||
// misrepresented as being the original software.
|
||||
// 3. This notice may not be removed or altered from any source distribution.
|
||||
//
|
||||
|
||||
#ifndef NANOVG_H
|
||||
#define NANOVG_H
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#define NVG_PI 3.14159265358979323846264338327f
|
||||
|
||||
#ifdef _MSC_VER
|
||||
#pragma warning(push)
|
||||
#pragma warning(disable: 4201) // nonstandard extension used : nameless struct/union
|
||||
#endif
|
||||
|
||||
typedef struct NVGcontext NVGcontext;
|
||||
|
||||
struct NVGcolor {
|
||||
union {
|
||||
float rgba[4];
|
||||
struct {
|
||||
float r,g,b,a;
|
||||
};
|
||||
};
|
||||
};
|
||||
typedef struct NVGcolor NVGcolor;
|
||||
|
||||
struct NVGpaint {
|
||||
float xform[6];
|
||||
float extent[2];
|
||||
float radius;
|
||||
float feather;
|
||||
NVGcolor innerColor;
|
||||
NVGcolor outerColor;
|
||||
int image;
|
||||
};
|
||||
typedef struct NVGpaint NVGpaint;
|
||||
|
||||
enum NVGwinding {
|
||||
NVG_CCW = 1, // Winding for solid shapes
|
||||
NVG_CW = 2, // Winding for holes
|
||||
};
|
||||
|
||||
enum NVGsolidity {
|
||||
NVG_SOLID = 1, // CCW
|
||||
NVG_HOLE = 2, // CW
|
||||
};
|
||||
|
||||
enum NVGlineCap {
|
||||
NVG_BUTT,
|
||||
NVG_ROUND,
|
||||
NVG_SQUARE,
|
||||
NVG_BEVEL,
|
||||
NVG_MITER,
|
||||
};
|
||||
|
||||
enum NVGalign {
|
||||
// Horizontal align
|
||||
NVG_ALIGN_LEFT = 1<<0, // Default, align text horizontally to left.
|
||||
NVG_ALIGN_CENTER = 1<<1, // Align text horizontally to center.
|
||||
NVG_ALIGN_RIGHT = 1<<2, // Align text horizontally to right.
|
||||
// Vertical align
|
||||
NVG_ALIGN_TOP = 1<<3, // Align text vertically to top.
|
||||
NVG_ALIGN_MIDDLE = 1<<4, // Align text vertically to middle.
|
||||
NVG_ALIGN_BOTTOM = 1<<5, // Align text vertically to bottom.
|
||||
NVG_ALIGN_BASELINE = 1<<6, // Default, align text vertically to baseline.
|
||||
};
|
||||
|
||||
enum NVGblendFactor {
|
||||
NVG_ZERO = 1<<0,
|
||||
NVG_ONE = 1<<1,
|
||||
NVG_SRC_COLOR = 1<<2,
|
||||
NVG_ONE_MINUS_SRC_COLOR = 1<<3,
|
||||
NVG_DST_COLOR = 1<<4,
|
||||
NVG_ONE_MINUS_DST_COLOR = 1<<5,
|
||||
NVG_SRC_ALPHA = 1<<6,
|
||||
NVG_ONE_MINUS_SRC_ALPHA = 1<<7,
|
||||
NVG_DST_ALPHA = 1<<8,
|
||||
NVG_ONE_MINUS_DST_ALPHA = 1<<9,
|
||||
NVG_SRC_ALPHA_SATURATE = 1<<10,
|
||||
};
|
||||
|
||||
enum NVGcompositeOperation {
|
||||
NVG_SOURCE_OVER,
|
||||
NVG_SOURCE_IN,
|
||||
NVG_SOURCE_OUT,
|
||||
NVG_ATOP,
|
||||
NVG_DESTINATION_OVER,
|
||||
NVG_DESTINATION_IN,
|
||||
NVG_DESTINATION_OUT,
|
||||
NVG_DESTINATION_ATOP,
|
||||
NVG_LIGHTER,
|
||||
NVG_COPY,
|
||||
NVG_XOR,
|
||||
};
|
||||
|
||||
struct NVGcompositeOperationState {
|
||||
int srcRGB;
|
||||
int dstRGB;
|
||||
int srcAlpha;
|
||||
int dstAlpha;
|
||||
};
|
||||
typedef struct NVGcompositeOperationState NVGcompositeOperationState;
|
||||
|
||||
struct NVGglyphPosition {
|
||||
const char* str; // Position of the glyph in the input string.
|
||||
float x; // The x-coordinate of the logical glyph position.
|
||||
float minx, maxx; // The bounds of the glyph shape.
|
||||
};
|
||||
typedef struct NVGglyphPosition NVGglyphPosition;
|
||||
|
||||
struct NVGtextRow {
|
||||
const char* start; // Pointer to the input text where the row starts.
|
||||
const char* end; // Pointer to the input text where the row ends (one past the last character).
|
||||
const char* next; // Pointer to the beginning of the next row.
|
||||
float width; // Logical width of the row.
|
||||
float minx, maxx; // Actual bounds of the row. Logical with and bounds can differ because of kerning and some parts over extending.
|
||||
};
|
||||
typedef struct NVGtextRow NVGtextRow;
|
||||
|
||||
enum NVGimageFlags {
|
||||
NVG_IMAGE_GENERATE_MIPMAPS = 1<<0, // Generate mipmaps during creation of the image.
|
||||
NVG_IMAGE_REPEATX = 1<<1, // Repeat image in X direction.
|
||||
NVG_IMAGE_REPEATY = 1<<2, // Repeat image in Y direction.
|
||||
NVG_IMAGE_FLIPY = 1<<3, // Flips (inverses) image in Y direction when rendered.
|
||||
NVG_IMAGE_PREMULTIPLIED = 1<<4, // Image data has premultiplied alpha.
|
||||
};
|
||||
|
||||
// Begin drawing a new frame
|
||||
// Calls to nanovg drawing API should be wrapped in nvgBeginFrame() & nvgEndFrame()
|
||||
// nvgBeginFrame() defines the size of the window to render to in relation currently
|
||||
// set viewport (i.e. glViewport on GL backends). Device pixel ration allows to
|
||||
// control the rendering on Hi-DPI devices.
|
||||
// For example, GLFW returns two dimension for an opened window: window size and
|
||||
// frame buffer size. In that case you would set windowWidth/Height to the window size
|
||||
// devicePixelRatio to: frameBufferWidth / windowWidth.
|
||||
void nvgBeginFrame(NVGcontext* ctx, int windowWidth, int windowHeight, float devicePixelRatio);
|
||||
|
||||
// Cancels drawing the current frame.
|
||||
void nvgCancelFrame(NVGcontext* ctx);
|
||||
|
||||
// Ends drawing flushing remaining render state.
|
||||
void nvgEndFrame(NVGcontext* ctx);
|
||||
|
||||
//
|
||||
// Composite operation
|
||||
//
|
||||
// The composite operations in NanoVG are modeled after HTML Canvas API, and
|
||||
// the blend func is based on OpenGL (see corresponding manuals for more info).
|
||||
// The colors in the blending state have premultiplied alpha.
|
||||
|
||||
// Sets the composite operation. The op parameter should be one of NVGcompositeOperation.
|
||||
void nvgGlobalCompositeOperation(NVGcontext* ctx, int op);
|
||||
|
||||
// Sets the composite operation with custom pixel arithmetic. The parameters should be one of NVGblendFactor.
|
||||
void nvgGlobalCompositeBlendFunc(NVGcontext* ctx, int sfactor, int dfactor);
|
||||
|
||||
// Sets the composite operation with custom pixel arithmetic for RGB and alpha components separately. The parameters should be one of NVGblendFactor.
|
||||
void nvgGlobalCompositeBlendFuncSeparate(NVGcontext* ctx, int srcRGB, int dstRGB, int srcAlpha, int dstAlpha);
|
||||
|
||||
//
|
||||
// Color utils
|
||||
//
|
||||
// Colors in NanoVG are stored as unsigned ints in ABGR format.
|
||||
|
||||
// Returns a color value from red, green, blue values. Alpha will be set to 255 (1.0f).
|
||||
NVGcolor nvgRGB(unsigned char r, unsigned char g, unsigned char b);
|
||||
|
||||
// Returns a color value from red, green, blue values. Alpha will be set to 1.0f.
|
||||
NVGcolor nvgRGBf(float r, float g, float b);
|
||||
|
||||
|
||||
// Returns a color value from red, green, blue and alpha values.
|
||||
NVGcolor nvgRGBA(unsigned char r, unsigned char g, unsigned char b, unsigned char a);
|
||||
|
||||
// Returns a color value from red, green, blue and alpha values.
|
||||
NVGcolor nvgRGBAf(float r, float g, float b, float a);
|
||||
|
||||
|
||||
// Linearly interpolates from color c0 to c1, and returns resulting color value.
|
||||
NVGcolor nvgLerpRGBA(NVGcolor c0, NVGcolor c1, float u);
|
||||
|
||||
// Sets transparency of a color value.
|
||||
NVGcolor nvgTransRGBA(NVGcolor c0, unsigned char a);
|
||||
|
||||
// Sets transparency of a color value.
|
||||
NVGcolor nvgTransRGBAf(NVGcolor c0, float a);
|
||||
|
||||
// Returns color value specified by hue, saturation and lightness.
|
||||
// HSL values are all in range [0..1], alpha will be set to 255.
|
||||
NVGcolor nvgHSL(float h, float s, float l);
|
||||
|
||||
// Returns color value specified by hue, saturation and lightness and alpha.
|
||||
// HSL values are all in range [0..1], alpha in range [0..255]
|
||||
NVGcolor nvgHSLA(float h, float s, float l, unsigned char a);
|
||||
|
||||
//
|
||||
// State Handling
|
||||
//
|
||||
// NanoVG contains state which represents how paths will be rendered.
|
||||
// The state contains transform, fill and stroke styles, text and font styles,
|
||||
// and scissor clipping.
|
||||
|
||||
// Pushes and saves the current render state into a state stack.
|
||||
// A matching nvgRestore() must be used to restore the state.
|
||||
void nvgSave(NVGcontext* ctx);
|
||||
|
||||
// Pops and restores current render state.
|
||||
void nvgRestore(NVGcontext* ctx);
|
||||
|
||||
// Resets current render state to default values. Does not affect the render state stack.
|
||||
void nvgReset(NVGcontext* ctx);
|
||||
|
||||
//
|
||||
// Render styles
|
||||
//
|
||||
// Fill and stroke render style can be either a solid color or a paint which is a gradient or a pattern.
|
||||
// Solid color is simply defined as a color value, different kinds of paints can be created
|
||||
// using nvgLinearGradient(), nvgBoxGradient(), nvgRadialGradient() and nvgImagePattern().
|
||||
//
|
||||
// Current render style can be saved and restored using nvgSave() and nvgRestore().
|
||||
|
||||
// Sets current stroke style to a solid color.
|
||||
void nvgStrokeColor(NVGcontext* ctx, NVGcolor color);
|
||||
|
||||
// Sets current stroke style to a paint, which can be a one of the gradients or a pattern.
|
||||
void nvgStrokePaint(NVGcontext* ctx, NVGpaint paint);
|
||||
|
||||
// Sets current fill style to a solid color.
|
||||
void nvgFillColor(NVGcontext* ctx, NVGcolor color);
|
||||
|
||||
// Sets current fill style to a paint, which can be a one of the gradients or a pattern.
|
||||
void nvgFillPaint(NVGcontext* ctx, NVGpaint paint);
|
||||
|
||||
// Sets the miter limit of the stroke style.
|
||||
// Miter limit controls when a sharp corner is beveled.
|
||||
void nvgMiterLimit(NVGcontext* ctx, float limit);
|
||||
|
||||
// Sets the stroke width of the stroke style.
|
||||
void nvgStrokeWidth(NVGcontext* ctx, float size);
|
||||
|
||||
// Sets how the end of the line (cap) is drawn,
|
||||
// Can be one of: NVG_BUTT (default), NVG_ROUND, NVG_SQUARE.
|
||||
void nvgLineCap(NVGcontext* ctx, int cap);
|
||||
|
||||
// Sets how sharp path corners are drawn.
|
||||
// Can be one of NVG_MITER (default), NVG_ROUND, NVG_BEVEL.
|
||||
void nvgLineJoin(NVGcontext* ctx, int join);
|
||||
|
||||
// Sets the transparency applied to all rendered shapes.
|
||||
// Already transparent paths will get proportionally more transparent as well.
|
||||
void nvgGlobalAlpha(NVGcontext* ctx, float alpha);
|
||||
|
||||
//
|
||||
// Transforms
|
||||
//
|
||||
// The paths, gradients, patterns and scissor region are transformed by an transformation
|
||||
// matrix at the time when they are passed to the API.
|
||||
// The current transformation matrix is a affine matrix:
|
||||
// [sx kx tx]
|
||||
// [ky sy ty]
|
||||
// [ 0 0 1]
|
||||
// Where: sx,sy define scaling, kx,ky skewing, and tx,ty translation.
|
||||
// The last row is assumed to be 0,0,1 and is not stored.
|
||||
//
|
||||
// Apart from nvgResetTransform(), each transformation function first creates
|
||||
// specific transformation matrix and pre-multiplies the current transformation by it.
|
||||
//
|
||||
// Current coordinate system (transformation) can be saved and restored using nvgSave() and nvgRestore().
|
||||
|
||||
// Resets current transform to a identity matrix.
|
||||
void nvgResetTransform(NVGcontext* ctx);
|
||||
|
||||
// Premultiplies current coordinate system by specified matrix.
|
||||
// The parameters are interpreted as matrix as follows:
|
||||
// [a c e]
|
||||
// [b d f]
|
||||
// [0 0 1]
|
||||
void nvgTransform(NVGcontext* ctx, float a, float b, float c, float d, float e, float f);
|
||||
|
||||
// Translates current coordinate system.
|
||||
void nvgTranslate(NVGcontext* ctx, float x, float y);
|
||||
|
||||
// Rotates current coordinate system. Angle is specified in radians.
|
||||
void nvgRotate(NVGcontext* ctx, float angle);
|
||||
|
||||
// Skews the current coordinate system along X axis. Angle is specified in radians.
|
||||
void nvgSkewX(NVGcontext* ctx, float angle);
|
||||
|
||||
// Skews the current coordinate system along Y axis. Angle is specified in radians.
|
||||
void nvgSkewY(NVGcontext* ctx, float angle);
|
||||
|
||||
// Scales the current coordinate system.
|
||||
void nvgScale(NVGcontext* ctx, float x, float y);
|
||||
|
||||
// Stores the top part (a-f) of the current transformation matrix in to the specified buffer.
|
||||
// [a c e]
|
||||
// [b d f]
|
||||
// [0 0 1]
|
||||
// There should be space for 6 floats in the return buffer for the values a-f.
|
||||
void nvgCurrentTransform(NVGcontext* ctx, float* xform);
|
||||
|
||||
|
||||
// The following functions can be used to make calculations on 2x3 transformation matrices.
|
||||
// A 2x3 matrix is represented as float[6].
|
||||
|
||||
// Sets the transform to identity matrix.
|
||||
void nvgTransformIdentity(float* dst);
|
||||
|
||||
// Sets the transform to translation matrix matrix.
|
||||
void nvgTransformTranslate(float* dst, float tx, float ty);
|
||||
|
||||
// Sets the transform to scale matrix.
|
||||
void nvgTransformScale(float* dst, float sx, float sy);
|
||||
|
||||
// Sets the transform to rotate matrix. Angle is specified in radians.
|
||||
void nvgTransformRotate(float* dst, float a);
|
||||
|
||||
// Sets the transform to skew-x matrix. Angle is specified in radians.
|
||||
void nvgTransformSkewX(float* dst, float a);
|
||||
|
||||
// Sets the transform to skew-y matrix. Angle is specified in radians.
|
||||
void nvgTransformSkewY(float* dst, float a);
|
||||
|
||||
// Sets the transform to the result of multiplication of two transforms, of A = A*B.
|
||||
void nvgTransformMultiply(float* dst, const float* src);
|
||||
|
||||
// Sets the transform to the result of multiplication of two transforms, of A = B*A.
|
||||
void nvgTransformPremultiply(float* dst, const float* src);
|
||||
|
||||
// Sets the destination to inverse of specified transform.
|
||||
// Returns 1 if the inverse could be calculated, else 0.
|
||||
int nvgTransformInverse(float* dst, const float* src);
|
||||
|
||||
// Transform a point by given transform.
|
||||
void nvgTransformPoint(float* dstx, float* dsty, const float* xform, float srcx, float srcy);
|
||||
|
||||
// Converts degrees to radians and vice versa.
|
||||
float nvgDegToRad(float deg);
|
||||
float nvgRadToDeg(float rad);
|
||||
|
||||
//
|
||||
// Images
|
||||
//
|
||||
// NanoVG allows you to load jpg, png, psd, tga, pic and gif files to be used for rendering.
|
||||
// In addition you can upload your own image. The image loading is provided by stb_image.
|
||||
// The parameter imageFlags is combination of flags defined in NVGimageFlags.
|
||||
|
||||
// Creates image by loading it from the disk from specified file name.
|
||||
// Returns handle to the image.
|
||||
int nvgCreateImage(NVGcontext* ctx, const char* filename, int imageFlags);
|
||||
|
||||
// Creates image by loading it from the specified chunk of memory.
|
||||
// Returns handle to the image.
|
||||
int nvgCreateImageMem(NVGcontext* ctx, int imageFlags, unsigned char* data, int ndata);
|
||||
|
||||
// Creates image from specified image data.
|
||||
// Returns handle to the image.
|
||||
int nvgCreateImageRGBA(NVGcontext* ctx, int w, int h, int imageFlags, const unsigned char* data);
|
||||
|
||||
// Updates image data specified by image handle.
|
||||
void nvgUpdateImage(NVGcontext* ctx, int image, const unsigned char* data);
|
||||
|
||||
// Returns the dimensions of a created image.
|
||||
void nvgImageSize(NVGcontext* ctx, int image, int* w, int* h);
|
||||
|
||||
// Deletes created image.
|
||||
void nvgDeleteImage(NVGcontext* ctx, int image);
|
||||
|
||||
//
|
||||
// Paints
|
||||
//
|
||||
// NanoVG supports four types of paints: linear gradient, box gradient, radial gradient and image pattern.
|
||||
// These can be used as paints for strokes and fills.
|
||||
|
||||
// Creates and returns a linear gradient. Parameters (sx,sy)-(ex,ey) specify the start and end coordinates
|
||||
// of the linear gradient, icol specifies the start color and ocol the end color.
|
||||
// The gradient is transformed by the current transform when it is passed to nvgFillPaint() or nvgStrokePaint().
|
||||
NVGpaint nvgLinearGradient(NVGcontext* ctx, float sx, float sy, float ex, float ey,
|
||||
NVGcolor icol, NVGcolor ocol);
|
||||
|
||||
// Creates and returns a box gradient. Box gradient is a feathered rounded rectangle, it is useful for rendering
|
||||
// drop shadows or highlights for boxes. Parameters (x,y) define the top-left corner of the rectangle,
|
||||
// (w,h) define the size of the rectangle, r defines the corner radius, and f feather. Feather defines how blurry
|
||||
// the border of the rectangle is. Parameter icol specifies the inner color and ocol the outer color of the gradient.
|
||||
// The gradient is transformed by the current transform when it is passed to nvgFillPaint() or nvgStrokePaint().
|
||||
NVGpaint nvgBoxGradient(NVGcontext* ctx, float x, float y, float w, float h,
|
||||
float r, float f, NVGcolor icol, NVGcolor ocol);
|
||||
|
||||
// Creates and returns a radial gradient. Parameters (cx,cy) specify the center, inr and outr specify
|
||||
// the inner and outer radius of the gradient, icol specifies the start color and ocol the end color.
|
||||
// The gradient is transformed by the current transform when it is passed to nvgFillPaint() or nvgStrokePaint().
|
||||
NVGpaint nvgRadialGradient(NVGcontext* ctx, float cx, float cy, float inr, float outr,
|
||||
NVGcolor icol, NVGcolor ocol);
|
||||
|
||||
// Creates and returns an image patter. Parameters (ox,oy) specify the left-top location of the image pattern,
|
||||
// (ex,ey) the size of one image, angle rotation around the top-left corner, image is handle to the image to render.
|
||||
// The gradient is transformed by the current transform when it is passed to nvgFillPaint() or nvgStrokePaint().
|
||||
NVGpaint nvgImagePattern(NVGcontext* ctx, float ox, float oy, float ex, float ey,
|
||||
float angle, int image, float alpha);
|
||||
|
||||
//
|
||||
// Scissoring
|
||||
//
|
||||
// Scissoring allows you to clip the rendering into a rectangle. This is useful for various
|
||||
// user interface cases like rendering a text edit or a timeline.
|
||||
|
||||
// Sets the current scissor rectangle.
|
||||
// The scissor rectangle is transformed by the current transform.
|
||||
void nvgScissor(NVGcontext* ctx, float x, float y, float w, float h);
|
||||
|
||||
// Intersects current scissor rectangle with the specified rectangle.
|
||||
// The scissor rectangle is transformed by the current transform.
|
||||
// Note: in case the rotation of previous scissor rect differs from
|
||||
// the current one, the intersection will be done between the specified
|
||||
// rectangle and the previous scissor rectangle transformed in the current
|
||||
// transform space. The resulting shape is always rectangle.
|
||||
void nvgIntersectScissor(NVGcontext* ctx, float x, float y, float w, float h);
|
||||
|
||||
// Reset and disables scissoring.
|
||||
void nvgResetScissor(NVGcontext* ctx);
|
||||
|
||||
//
|
||||
// Paths
|
||||
//
|
||||
// Drawing a new shape starts with nvgBeginPath(), it clears all the currently defined paths.
|
||||
// Then you define one or more paths and sub-paths which describe the shape. The are functions
|
||||
// to draw common shapes like rectangles and circles, and lower level step-by-step functions,
|
||||
// which allow to define a path curve by curve.
|
||||
//
|
||||
// NanoVG uses even-odd fill rule to draw the shapes. Solid shapes should have counter clockwise
|
||||
// winding and holes should have counter clockwise order. To specify winding of a path you can
|
||||
// call nvgPathWinding(). This is useful especially for the common shapes, which are drawn CCW.
|
||||
//
|
||||
// Finally you can fill the path using current fill style by calling nvgFill(), and stroke it
|
||||
// with current stroke style by calling nvgStroke().
|
||||
//
|
||||
// The curve segments and sub-paths are transformed by the current transform.
|
||||
|
||||
// Clears the current path and sub-paths.
|
||||
void nvgBeginPath(NVGcontext* ctx);
|
||||
|
||||
// Starts new sub-path with specified point as first point.
|
||||
void nvgMoveTo(NVGcontext* ctx, float x, float y);
|
||||
|
||||
// Adds line segment from the last point in the path to the specified point.
|
||||
void nvgLineTo(NVGcontext* ctx, float x, float y);
|
||||
|
||||
// Adds cubic bezier segment from last point in the path via two control points to the specified point.
|
||||
void nvgBezierTo(NVGcontext* ctx, float c1x, float c1y, float c2x, float c2y, float x, float y);
|
||||
|
||||
// Adds quadratic bezier segment from last point in the path via a control point to the specified point.
|
||||
void nvgQuadTo(NVGcontext* ctx, float cx, float cy, float x, float y);
|
||||
|
||||
// Adds an arc segment at the corner defined by the last path point, and two specified points.
|
||||
void nvgArcTo(NVGcontext* ctx, float x1, float y1, float x2, float y2, float radius);
|
||||
|
||||
// Closes current sub-path with a line segment.
|
||||
void nvgClosePath(NVGcontext* ctx);
|
||||
|
||||
// Sets the current sub-path winding, see NVGwinding and NVGsolidity.
|
||||
void nvgPathWinding(NVGcontext* ctx, int dir);
|
||||
|
||||
// Creates new circle arc shaped sub-path. The arc center is at cx,cy, the arc radius is r,
|
||||
// and the arc is drawn from angle a0 to a1, and swept in direction dir (NVG_CCW, or NVG_CW).
|
||||
// Angles are specified in radians.
|
||||
void nvgArc(NVGcontext* ctx, float cx, float cy, float r, float a0, float a1, int dir);
|
||||
|
||||
// Creates new rectangle shaped sub-path.
|
||||
void nvgRect(NVGcontext* ctx, float x, float y, float w, float h);
|
||||
|
||||
// Creates new rounded rectangle shaped sub-path.
|
||||
void nvgRoundedRect(NVGcontext* ctx, float x, float y, float w, float h, float r);
|
||||
|
||||
// Creates new rounded rectangle shaped sub-path with varying radii for each corner.
|
||||
void nvgRoundedRectVarying(NVGcontext* ctx, float x, float y, float w, float h, float radTopLeft, float radTopRight, float radBottomRight, float radBottomLeft);
|
||||
|
||||
// Creates new ellipse shaped sub-path.
|
||||
void nvgEllipse(NVGcontext* ctx, float cx, float cy, float rx, float ry);
|
||||
|
||||
// Creates new circle shaped sub-path.
|
||||
void nvgCircle(NVGcontext* ctx, float cx, float cy, float r);
|
||||
|
||||
// Fills the current path with current fill style.
|
||||
void nvgFill(NVGcontext* ctx);
|
||||
|
||||
// Fills the current path with current stroke style.
|
||||
void nvgStroke(NVGcontext* ctx);
|
||||
|
||||
|
||||
//
|
||||
// Text
|
||||
//
|
||||
// NanoVG allows you to load .ttf files and use the font to render text.
|
||||
//
|
||||
// The appearance of the text can be defined by setting the current text style
|
||||
// and by specifying the fill color. Common text and font settings such as
|
||||
// font size, letter spacing and text align are supported. Font blur allows you
|
||||
// to create simple text effects such as drop shadows.
|
||||
//
|
||||
// At render time the font face can be set based on the font handles or name.
|
||||
//
|
||||
// Font measure functions return values in local space, the calculations are
|
||||
// carried in the same resolution as the final rendering. This is done because
|
||||
// the text glyph positions are snapped to the nearest pixels sharp rendering.
|
||||
//
|
||||
// The local space means that values are not rotated or scale as per the current
|
||||
// transformation. For example if you set font size to 12, which would mean that
|
||||
// line height is 16, then regardless of the current scaling and rotation, the
|
||||
// returned line height is always 16. Some measures may vary because of the scaling
|
||||
// since aforementioned pixel snapping.
|
||||
//
|
||||
// While this may sound a little odd, the setup allows you to always render the
|
||||
// same way regardless of scaling. I.e. following works regardless of scaling:
|
||||
//
|
||||
// const char* txt = "Text me up.";
|
||||
// nvgTextBounds(vg, x,y, txt, NULL, bounds);
|
||||
// nvgBeginPath(vg);
|
||||
// nvgRoundedRect(vg, bounds[0],bounds[1], bounds[2]-bounds[0], bounds[3]-bounds[1]);
|
||||
// nvgFill(vg);
|
||||
//
|
||||
// Note: currently only solid color fill is supported for text.
|
||||
|
||||
// Creates font by loading it from the disk from specified file name.
|
||||
// Returns handle to the font.
|
||||
int nvgCreateFont(NVGcontext* ctx, const char* name, const char* filename);
|
||||
|
||||
// Creates font by loading it from the specified memory chunk.
|
||||
// Returns handle to the font.
|
||||
int nvgCreateFontMem(NVGcontext* ctx, const char* name, unsigned char* data, int ndata, int freeData);
|
||||
|
||||
// Finds a loaded font of specified name, and returns handle to it, or -1 if the font is not found.
|
||||
int nvgFindFont(NVGcontext* ctx, const char* name);
|
||||
|
||||
// Adds a fallback font by handle.
|
||||
int nvgAddFallbackFontId(NVGcontext* ctx, int baseFont, int fallbackFont);
|
||||
|
||||
// Adds a fallback font by name.
|
||||
int nvgAddFallbackFont(NVGcontext* ctx, const char* baseFont, const char* fallbackFont);
|
||||
|
||||
// Sets the font size of current text style.
|
||||
void nvgFontSize(NVGcontext* ctx, float size);
|
||||
|
||||
// Sets the blur of current text style.
|
||||
void nvgFontBlur(NVGcontext* ctx, float blur);
|
||||
|
||||
// Sets the letter spacing of current text style.
|
||||
void nvgTextLetterSpacing(NVGcontext* ctx, float spacing);
|
||||
|
||||
// Sets the proportional line height of current text style. The line height is specified as multiple of font size.
|
||||
void nvgTextLineHeight(NVGcontext* ctx, float lineHeight);
|
||||
|
||||
// Sets the text align of current text style, see NVGalign for options.
|
||||
void nvgTextAlign(NVGcontext* ctx, int align);
|
||||
|
||||
// Sets the font face based on specified id of current text style.
|
||||
void nvgFontFaceId(NVGcontext* ctx, int font);
|
||||
|
||||
// Sets the font face based on specified name of current text style.
|
||||
void nvgFontFace(NVGcontext* ctx, const char* font);
|
||||
|
||||
// Draws text string at specified location. If end is specified only the sub-string up to the end is drawn.
|
||||
float nvgText(NVGcontext* ctx, float x, float y, const char* string, const char* end);
|
||||
|
||||
// Draws multi-line text string at specified location wrapped at the specified width. If end is specified only the sub-string up to the end is drawn.
|
||||
// White space is stripped at the beginning of the rows, the text is split at word boundaries or when new-line characters are encountered.
|
||||
// Words longer than the max width are slit at nearest character (i.e. no hyphenation).
|
||||
void nvgTextBox(NVGcontext* ctx, float x, float y, float breakRowWidth, const char* string, const char* end);
|
||||
|
||||
// Measures the specified text string. Parameter bounds should be a pointer to float[4],
|
||||
// if the bounding box of the text should be returned. The bounds value are [xmin,ymin, xmax,ymax]
|
||||
// Returns the horizontal advance of the measured text (i.e. where the next character should drawn).
|
||||
// Measured values are returned in local coordinate space.
|
||||
float nvgTextBounds(NVGcontext* ctx, float x, float y, const char* string, const char* end, float* bounds);
|
||||
|
||||
// Measures the specified multi-text string. Parameter bounds should be a pointer to float[4],
|
||||
// if the bounding box of the text should be returned. The bounds value are [xmin,ymin, xmax,ymax]
|
||||
// Measured values are returned in local coordinate space.
|
||||
void nvgTextBoxBounds(NVGcontext* ctx, float x, float y, float breakRowWidth, const char* string, const char* end, float* bounds);
|
||||
|
||||
// Calculates the glyph x positions of the specified text. If end is specified only the sub-string will be used.
|
||||
// Measured values are returned in local coordinate space.
|
||||
int nvgTextGlyphPositions(NVGcontext* ctx, float x, float y, const char* string, const char* end, NVGglyphPosition* positions, int maxPositions);
|
||||
|
||||
// Returns the vertical metrics based on the current text style.
|
||||
// Measured values are returned in local coordinate space.
|
||||
void nvgTextMetrics(NVGcontext* ctx, float* ascender, float* descender, float* lineh);
|
||||
|
||||
// Breaks the specified text into lines. If end is specified only the sub-string will be used.
|
||||
// White space is stripped at the beginning of the rows, the text is split at word boundaries or when new-line characters are encountered.
|
||||
// Words longer than the max width are slit at nearest character (i.e. no hyphenation).
|
||||
int nvgTextBreakLines(NVGcontext* ctx, const char* string, const char* end, float breakRowWidth, NVGtextRow* rows, int maxRows);
|
||||
|
||||
//
|
||||
// Internal Render API
|
||||
//
|
||||
enum NVGtexture {
|
||||
NVG_TEXTURE_ALPHA = 0x01,
|
||||
NVG_TEXTURE_RGBA = 0x02,
|
||||
};
|
||||
|
||||
struct NVGscissor {
|
||||
float xform[6];
|
||||
float extent[2];
|
||||
};
|
||||
typedef struct NVGscissor NVGscissor;
|
||||
|
||||
struct NVGvertex {
|
||||
float x,y,u,v;
|
||||
};
|
||||
typedef struct NVGvertex NVGvertex;
|
||||
|
||||
struct NVGpath {
|
||||
int first;
|
||||
int count;
|
||||
unsigned char closed;
|
||||
int nbevel;
|
||||
NVGvertex* fill;
|
||||
int nfill;
|
||||
NVGvertex* stroke;
|
||||
int nstroke;
|
||||
int winding;
|
||||
int convex;
|
||||
};
|
||||
typedef struct NVGpath NVGpath;
|
||||
|
||||
struct NVGparams {
|
||||
void* userPtr;
|
||||
int edgeAntiAlias;
|
||||
int (*renderCreate)(void* uptr);
|
||||
int (*renderCreateTexture)(void* uptr, int type, int w, int h, int imageFlags, const unsigned char* data);
|
||||
int (*renderDeleteTexture)(void* uptr, int image);
|
||||
int (*renderUpdateTexture)(void* uptr, int image, int x, int y, int w, int h, const unsigned char* data);
|
||||
int (*renderGetTextureSize)(void* uptr, int image, int* w, int* h);
|
||||
void (*renderViewport)(void* uptr, int width, int height, float devicePixelRatio);
|
||||
void (*renderCancel)(void* uptr);
|
||||
void (*renderFlush)(void* uptr, NVGcompositeOperationState compositeOperation);
|
||||
void (*renderFill)(void* uptr, NVGpaint* paint, NVGscissor* scissor, float fringe, const float* bounds, const NVGpath* paths, int npaths);
|
||||
void (*renderStroke)(void* uptr, NVGpaint* paint, NVGscissor* scissor, float fringe, float strokeWidth, const NVGpath* paths, int npaths);
|
||||
void (*renderTriangles)(void* uptr, NVGpaint* paint, NVGscissor* scissor, const NVGvertex* verts, int nverts);
|
||||
void (*renderDelete)(void* uptr);
|
||||
};
|
||||
typedef struct NVGparams NVGparams;
|
||||
|
||||
// Constructor and destructor, called by the render back-end.
|
||||
NVGcontext* nvgCreateInternal(NVGparams* params);
|
||||
void nvgDeleteInternal(NVGcontext* ctx);
|
||||
|
||||
NVGparams* nvgInternalParams(NVGcontext* ctx);
|
||||
|
||||
// Debug function to dump cached path data.
|
||||
void nvgDebugDumpPathCache(NVGcontext* ctx);
|
||||
|
||||
#ifdef _MSC_VER
|
||||
#pragma warning(pop)
|
||||
#endif
|
||||
|
||||
#define NVG_NOTUSED(v) for (;;) { (void)(1 ? (void)0 : ( (void)(v) ) ); break; }
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // NANOVG_H
|
||||
File diff suppressed because it is too large
Load Diff
|
|
@ -0,0 +1,143 @@
|
|||
//
|
||||
// Copyright (c) 2009-2013 Mikko Mononen memon@inside.org
|
||||
//
|
||||
// This software is provided 'as-is', without any express or implied
|
||||
// warranty. In no event will the authors be held liable for any damages
|
||||
// arising from the use of this software.
|
||||
// Permission is granted to anyone to use this software for any purpose,
|
||||
// including commercial applications, and to alter it and redistribute it
|
||||
// freely, subject to the following restrictions:
|
||||
// 1. The origin of this software must not be misrepresented; you must not
|
||||
// claim that you wrote the original software. If you use this software
|
||||
// in a product, an acknowledgment in the product documentation would be
|
||||
// appreciated but is not required.
|
||||
// 2. Altered source versions must be plainly marked as such, and must not be
|
||||
// misrepresented as being the original software.
|
||||
// 3. This notice may not be removed or altered from any source distribution.
|
||||
//
|
||||
#ifndef NANOVG_GL_UTILS_H
|
||||
#define NANOVG_GL_UTILS_H
|
||||
|
||||
struct NVGLUframebuffer {
|
||||
NVGcontext* ctx;
|
||||
GLuint fbo;
|
||||
GLuint rbo;
|
||||
GLuint texture;
|
||||
int image;
|
||||
};
|
||||
typedef struct NVGLUframebuffer NVGLUframebuffer;
|
||||
|
||||
// Helper function to create GL frame buffer to render to.
|
||||
void nvgluBindFramebuffer(NVGLUframebuffer* fb);
|
||||
NVGLUframebuffer* nvgluCreateFramebuffer(NVGcontext* ctx, int w, int h, int imageFlags);
|
||||
void nvgluDeleteFramebuffer(NVGLUframebuffer* fb);
|
||||
|
||||
#endif // NANOVG_GL_UTILS_H
|
||||
|
||||
#ifdef NANOVG_GL_IMPLEMENTATION
|
||||
|
||||
#if defined(NANOVG_GL3) || defined(NANOVG_GLES2) || defined(NANOVG_GLES3)
|
||||
// FBO is core in OpenGL 3>.
|
||||
# define NANOVG_FBO_VALID 1
|
||||
#elif defined(NANOVG_GL2)
|
||||
// On OS X including glext defines FBO on GL2 too.
|
||||
# ifdef __APPLE__
|
||||
# include <OpenGL/glext.h>
|
||||
# define NANOVG_FBO_VALID 1
|
||||
# endif
|
||||
#endif
|
||||
|
||||
static GLint defaultFBO = -1;
|
||||
|
||||
NVGLUframebuffer* nvgluCreateFramebuffer(NVGcontext* ctx, int w, int h, int imageFlags)
|
||||
{
|
||||
#ifdef NANOVG_FBO_VALID
|
||||
GLint defaultFBO;
|
||||
GLint defaultRBO;
|
||||
NVGLUframebuffer* fb = NULL;
|
||||
|
||||
glGetIntegerv(GL_FRAMEBUFFER_BINDING, &defaultFBO);
|
||||
glGetIntegerv(GL_RENDERBUFFER_BINDING, &defaultRBO);
|
||||
|
||||
fb = (NVGLUframebuffer*)malloc(sizeof(NVGLUframebuffer));
|
||||
if (fb == NULL) goto error;
|
||||
memset(fb, 0, sizeof(NVGLUframebuffer));
|
||||
|
||||
fb->image = nvgCreateImageRGBA(ctx, w, h, imageFlags | NVG_IMAGE_FLIPY | NVG_IMAGE_PREMULTIPLIED, NULL);
|
||||
|
||||
#if defined NANOVG_GL2
|
||||
fb->texture = nvglImageHandleGL2(ctx, fb->image);
|
||||
#elif defined NANOVG_GL3
|
||||
fb->texture = nvglImageHandleGL3(ctx, fb->image);
|
||||
#elif defined NANOVG_GLES2
|
||||
fb->texture = nvglImageHandleGLES2(ctx, fb->image);
|
||||
#elif defined NANOVG_GLES3
|
||||
fb->texture = nvglImageHandleGLES3(ctx, fb->image);
|
||||
#endif
|
||||
|
||||
fb->ctx = ctx;
|
||||
|
||||
// frame buffer object
|
||||
glGenFramebuffers(1, &fb->fbo);
|
||||
glBindFramebuffer(GL_FRAMEBUFFER, fb->fbo);
|
||||
|
||||
// render buffer object
|
||||
glGenRenderbuffers(1, &fb->rbo);
|
||||
glBindRenderbuffer(GL_RENDERBUFFER, fb->rbo);
|
||||
glRenderbufferStorage(GL_RENDERBUFFER, GL_STENCIL_INDEX8, w, h);
|
||||
|
||||
// combine all
|
||||
glFramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, fb->texture, 0);
|
||||
glFramebufferRenderbuffer(GL_FRAMEBUFFER, GL_STENCIL_ATTACHMENT, GL_RENDERBUFFER, fb->rbo);
|
||||
|
||||
if (glCheckFramebufferStatus(GL_FRAMEBUFFER) != GL_FRAMEBUFFER_COMPLETE) goto error;
|
||||
|
||||
glBindFramebuffer(GL_FRAMEBUFFER, defaultFBO);
|
||||
glBindRenderbuffer(GL_RENDERBUFFER, defaultRBO);
|
||||
return fb;
|
||||
error:
|
||||
glBindFramebuffer(GL_FRAMEBUFFER, defaultFBO);
|
||||
glBindRenderbuffer(GL_RENDERBUFFER, defaultRBO);
|
||||
nvgluDeleteFramebuffer(fb);
|
||||
return NULL;
|
||||
#else
|
||||
NVG_NOTUSED(ctx);
|
||||
NVG_NOTUSED(w);
|
||||
NVG_NOTUSED(h);
|
||||
NVG_NOTUSED(imageFlags);
|
||||
return NULL;
|
||||
#endif
|
||||
}
|
||||
|
||||
void nvgluBindFramebuffer(NVGLUframebuffer* fb)
|
||||
{
|
||||
#ifdef NANOVG_FBO_VALID
|
||||
if (defaultFBO == -1) glGetIntegerv(GL_FRAMEBUFFER_BINDING, &defaultFBO);
|
||||
glBindFramebuffer(GL_FRAMEBUFFER, fb != NULL ? fb->fbo : defaultFBO);
|
||||
#else
|
||||
NVG_NOTUSED(fb);
|
||||
#endif
|
||||
}
|
||||
|
||||
void nvgluDeleteFramebuffer(NVGLUframebuffer* fb)
|
||||
{
|
||||
#ifdef NANOVG_FBO_VALID
|
||||
if (fb == NULL) return;
|
||||
if (fb->fbo != 0)
|
||||
glDeleteFramebuffers(1, &fb->fbo);
|
||||
if (fb->rbo != 0)
|
||||
glDeleteRenderbuffers(1, &fb->rbo);
|
||||
if (fb->image >= 0)
|
||||
nvgDeleteImage(fb->ctx, fb->image);
|
||||
fb->ctx = NULL;
|
||||
fb->fbo = 0;
|
||||
fb->rbo = 0;
|
||||
fb->texture = 0;
|
||||
fb->image = -1;
|
||||
free(fb);
|
||||
#else
|
||||
NVG_NOTUSED(fb);
|
||||
#endif
|
||||
}
|
||||
|
||||
#endif // NANOVG_GL_IMPLEMENTATION
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
Binary file not shown.
Binary file not shown.
|
|
@ -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,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);
|
||||
}
|
||||
|
||||
|
|
@ -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,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()
|
||||
|
|
@ -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()
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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);
|
||||
}
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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);
|
||||
}
|
||||
|
||||
|
|
@ -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
|
||||
|
|
@ -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,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,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
|
||||
|
|
@ -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.])
|
||||
|
|
@ -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
|
||||
|
|
@ -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())
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
}
|
||||
}
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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()
|
||||
|
|
@ -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)
|
||||
|
|
@ -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,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
|
||||
|
|
@ -0,0 +1,65 @@
|
|||
import os
|
||||
import time
|
||||
|
||||
|
||||
class Logger(object):
|
||||
def __init__(self, root, init_data):
|
||||
self.root = root
|
||||
self.init_data = init_data
|
||||
|
||||
self.part = None
|
||||
self.data_dir = None
|
||||
self.cur_dir = None
|
||||
self.log_file = None
|
||||
self.started = False
|
||||
self.log_path = None
|
||||
self.lock_path = None
|
||||
self.log_file = None
|
||||
|
||||
def open(self):
|
||||
self.data_dir = self.cur_dir + "--" + str(self.part)
|
||||
|
||||
try:
|
||||
os.makedirs(self.data_dir)
|
||||
except OSError:
|
||||
pass
|
||||
|
||||
self.log_path = os.path.join(self.data_dir, "rlog")
|
||||
self.lock_path = self.log_path + ".lock"
|
||||
|
||||
open(self.lock_path, "wb").close()
|
||||
self.log_file = open(self.log_path, "wb")
|
||||
self.log_file.write(self.init_data)
|
||||
|
||||
def start(self):
|
||||
self.part = 0
|
||||
self.cur_dir = self.root + time.strftime("%Y-%m-%d--%H-%M-%S")
|
||||
|
||||
self.open()
|
||||
|
||||
self.started = True
|
||||
|
||||
return self.data_dir, self.part
|
||||
|
||||
def stop(self):
|
||||
if not self.started:
|
||||
return
|
||||
self.log_file.close()
|
||||
os.unlink(self.lock_path)
|
||||
self.started = False
|
||||
|
||||
def rotate(self):
|
||||
old_lock_path = self.lock_path
|
||||
old_log_file = self.log_file
|
||||
self.part += 1
|
||||
self.open()
|
||||
|
||||
old_log_file.close()
|
||||
os.unlink(old_lock_path)
|
||||
|
||||
return self.data_dir, self.part
|
||||
|
||||
def log_data(self, d):
|
||||
if not self.started:
|
||||
return
|
||||
self.log_file.write(d)
|
||||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue