@@ -32,8 +32,8 @@
* 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
* @{
@@ -41,8 +41,8 @@
/** @addtogroup stm32f4xx_system
* @{
- */
-
+ */
+
/**
* @brief Define to prevent recursive inclusion
*/
@@ -51,7 +51,7 @@
#ifdef __cplusplus
extern "C" {
-#endif
+#endif
/** @addtogroup STM32F4xx_System_Includes
* @{
@@ -68,7 +68,7 @@
/* This variable is updated in three ways:
1) by calling CMSIS function SystemCoreClockUpdate()
2) by calling HAL API function HAL_RCC_GetSysClockFreq()
- 3) each time HAL_RCC_ClockConfig() is called to configure the system clock frequency
+ 3) each time HAL_RCC_ClockConfig() is called to configure the system clock frequency
Note: If you use this function to configure the system clock; then there
is no need to call the 2 first functions listed above, since SystemCoreClock
variable is updated automatically.
@@ -101,7 +101,7 @@ extern const uint8_t APBPrescTable[8]; /*!< APB prescalers table values */
/** @addtogroup STM32F4xx_System_Exported_Functions
* @{
*/
-
+
extern void SystemInit(void);
extern void SystemCoreClockUpdate(void);
/**
@@ -117,8 +117,8 @@ extern void SystemCoreClockUpdate(void);
/**
* @}
*/
-
+
/**
* @}
- */
+ */
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
diff --git a/panda/board/libc.h b/panda/board/libc.h
index 564d29cd8..83adb9c09 100644
--- a/panda/board/libc.h
+++ b/panda/board/libc.h
@@ -6,32 +6,36 @@ void delay(int a) {
}
void *memset(void *str, int c, unsigned int n) {
- unsigned int i;
- for (i = 0; i < n; i++) {
- *((uint8_t*)str) = c;
- ++str;
+ uint8_t *s = str;
+ for (unsigned int i = 0; i < n; i++) {
+ *s = c;
+ s++;
}
return str;
}
void *memcpy(void *dest, const void *src, unsigned int n) {
- unsigned int i;
- // TODO: make not slow
- for (i = 0; i < n; i++) {
- ((uint8_t*)dest)[i] = *(uint8_t*)src;
- ++src;
+ uint8_t *d = dest;
+ const uint8_t *s = src;
+ for (unsigned int i = 0; i < n; i++) {
+ *d = *s;
+ d++;
+ s++;
}
return dest;
}
int memcmp(const void * ptr1, const void * ptr2, unsigned int num) {
- unsigned int i;
int ret = 0;
- for (i = 0; i < num; i++) {
- if ( ((uint8_t*)ptr1)[i] != ((uint8_t*)ptr2)[i] ) {
+ const uint8_t *p1 = ptr1;
+ const uint8_t *p2 = ptr2;
+ for (unsigned int i = 0; i < num; i++) {
+ if (*p1 != *p2) {
ret = -1;
break;
}
+ p1++;
+ p2++;
}
return ret;
}
diff --git a/panda/board/main.c b/panda/board/main.c
index e24df3818..7473b0775 100644
--- a/panda/board/main.c
+++ b/panda/board/main.c
@@ -1,39 +1,44 @@
-//#define EON
+//#define EON
+//#define PANDA
+// ********************* Includes *********************
#include "config.h"
#include "obj/gitversion.h"
-// ********************* includes *********************
-
-
#include "libc.h"
#include "provision.h"
+#include "main_declarations.h"
+
#include "drivers/llcan.h"
#include "drivers/llgpio.h"
-#include "gpio.h"
+#include "drivers/adc.h"
+
+#include "board.h"
#include "drivers/uart.h"
-#include "drivers/adc.h"
#include "drivers/usb.h"
#include "drivers/gmlan_alt.h"
#include "drivers/timer.h"
#include "drivers/clock.h"
+#include "gpio.h"
+
#ifndef EON
#include "drivers/spi.h"
#endif
#include "power_saving.h"
#include "safety.h"
+
#include "drivers/can.h"
-// ********************* serial debugging *********************
+// ********************* Serial debugging *********************
void debug_ring_callback(uart_ring *ring) {
char rcv;
while (getc(ring, &rcv)) {
- putc(ring, rcv);
+ (void)putc(ring, rcv); // misra-c2012-17.7: cast to void is ok: debug function
// jump to DFU flash
if (rcv == 'z') {
@@ -49,29 +54,23 @@ void debug_ring_callback(uart_ring *ring) {
// enable CDP mode
if (rcv == 'C') {
puts("switching USB to CDP mode\n");
- set_usb_power_mode(USB_POWER_CDP);
+ current_board->set_usb_power_mode(USB_POWER_CDP);
}
if (rcv == 'c') {
puts("switching USB to client mode\n");
- set_usb_power_mode(USB_POWER_CLIENT);
+ current_board->set_usb_power_mode(USB_POWER_CLIENT);
}
if (rcv == 'D') {
puts("switching USB to DCP mode\n");
- set_usb_power_mode(USB_POWER_DCP);
+ current_board->set_usb_power_mode(USB_POWER_DCP);
}
}
}
// ***************************** started logic *****************************
-
-bool is_gpio_started(void) {
- // ignition is on PA1
- return (GPIOA->IDR & (1U << 1)) == 0;
-}
-
-void EXTI1_IRQHandler(void) {
- volatile int pr = EXTI->PR & (1U << 1);
- if ((pr & (1U << 1)) != 0) {
+void started_interrupt_handler(uint8_t interrupt_line) {
+ volatile unsigned int pr = EXTI->PR & (1U << interrupt_line);
+ if ((pr & (1U << interrupt_line)) != 0U) {
#ifdef DEBUG
puts("got started interrupt\n");
#endif
@@ -80,10 +79,25 @@ void EXTI1_IRQHandler(void) {
delay(100000);
// set power savings mode here
- int power_save_state = is_gpio_started() ? POWER_SAVE_STATUS_DISABLED : POWER_SAVE_STATUS_ENABLED;
+ int power_save_state = current_board->check_ignition() ? POWER_SAVE_STATUS_DISABLED : POWER_SAVE_STATUS_ENABLED;
set_power_save_state(power_save_state);
- EXTI->PR = (1U << 1);
}
+ EXTI->PR = (1U << interrupt_line);
+}
+
+// cppcheck-suppress unusedFunction ; used in headers not included in cppcheck
+void EXTI0_IRQHandler(void) {
+ started_interrupt_handler(0);
+}
+
+// cppcheck-suppress unusedFunction ; used in headers not included in cppcheck
+void EXTI1_IRQHandler(void) {
+ started_interrupt_handler(1);
+}
+
+// cppcheck-suppress unusedFunction ; used in headers not included in cppcheck
+void EXTI3_IRQHandler(void) {
+ started_interrupt_handler(3);
}
void started_interrupt_init(void) {
@@ -94,17 +108,64 @@ void started_interrupt_init(void) {
NVIC_EnableIRQ(EXTI1_IRQn);
}
+// ****************************** safety mode ******************************
+
+// this is the only way to leave silent mode
+void set_safety_mode(uint16_t mode, int16_t param) {
+ int err = safety_set_mode(mode, param);
+ if (err == -1) {
+ puts("Error: safety set mode failed\n");
+ } else {
+ if (mode == SAFETY_NOOUTPUT) {
+ can_silent = ALL_CAN_SILENT;
+ } else {
+ can_silent = ALL_CAN_LIVE;
+ }
+
+ switch (mode) {
+ case SAFETY_NOOUTPUT:
+ set_intercept_relay(false);
+ if(hw_type == HW_TYPE_BLACK_PANDA){
+ current_board->set_can_mode(CAN_MODE_NORMAL);
+ }
+ break;
+ case SAFETY_ELM327:
+ set_intercept_relay(false);
+ if(hw_type == HW_TYPE_BLACK_PANDA){
+ current_board->set_can_mode(CAN_MODE_OBD_CAN2);
+ }
+ break;
+ default:
+ set_intercept_relay(true);
+ if(hw_type == HW_TYPE_BLACK_PANDA){
+ current_board->set_can_mode(CAN_MODE_NORMAL);
+ }
+ break;
+ }
+ if (safety_ignition_hook() != -1) {
+ // if the ignition hook depends on something other than the started GPIO
+ // we have to disable power savings (fix for GM and Tesla)
+ set_power_save_state(POWER_SAVE_STATUS_DISABLED);
+ } else {
+ // power mode is already POWER_SAVE_STATUS_DISABLED and CAN TXs are active
+ }
+ can_init_all();
+ }
+}
+
// ***************************** USB port *****************************
int get_health_pkt(void *dat) {
struct __attribute__((packed)) {
- uint32_t voltage;
- uint32_t current;
- uint8_t started;
- uint8_t controls_allowed;
- uint8_t gas_interceptor_detected;
- uint8_t started_signal_detected;
- uint8_t started_alt;
+ uint32_t voltage_pkt;
+ uint32_t current_pkt;
+ uint32_t can_send_errs_pkt;
+ uint32_t can_fwd_errs_pkt;
+ uint32_t gmlan_send_errs_pkt;
+ uint8_t started_pkt;
+ uint8_t controls_allowed_pkt;
+ uint8_t gas_interceptor_detected_pkt;
+ uint8_t car_harness_status_pkt;
} *health = dat;
//Voltage will be measured in mv. 5000 = 5V
@@ -117,29 +178,35 @@ int get_health_pkt(void *dat) {
// s = 1000/((4095/3.3)*(1/11)) = 8.8623046875
// Avoid needing floating point math
- health->voltage = (voltage * 8862) / 1000;
+ health->voltage_pkt = (voltage * 8862U) / 1000U;
+
+ // No current sense on panda black
+ if(hw_type != HW_TYPE_BLACK_PANDA){
+ health->current_pkt = adc_get(ADCCHAN_CURRENT);
+ } else {
+ health->current_pkt = 0;
+ }
- health->current = adc_get(ADCCHAN_CURRENT);
int safety_ignition = safety_ignition_hook();
if (safety_ignition < 0) {
//Use the GPIO pin to determine ignition
- health->started = is_gpio_started();
+ health->started_pkt = (uint8_t)(current_board->check_ignition());
} else {
//Current safety hooks want to determine ignition (ex: GM)
- health->started = safety_ignition;
+ health->started_pkt = safety_ignition;
}
- health->controls_allowed = controls_allowed;
- health->gas_interceptor_detected = gas_interceptor_detected;
-
- // DEPRECATED
- health->started_alt = 0;
- health->started_signal_detected = 0;
-
+ health->controls_allowed_pkt = controls_allowed;
+ health->gas_interceptor_detected_pkt = gas_interceptor_detected;
+ health->can_send_errs_pkt = can_send_errs;
+ health->can_fwd_errs_pkt = can_fwd_errs;
+ health->gmlan_send_errs_pkt = gmlan_send_errs;
+ health->car_harness_status_pkt = car_harness_status;
+
return sizeof(*health);
}
-int usb_cb_ep1_in(uint8_t *usbdata, int len, bool hardwired) {
+int usb_cb_ep1_in(void *usbdata, int len, bool hardwired) {
UNUSED(hardwired);
CAN_FIFOMailBox_TypeDef *reply = (CAN_FIFOMailBox_TypeDef *)usbdata;
int ilen = 0;
@@ -150,13 +217,14 @@ int usb_cb_ep1_in(uint8_t *usbdata, int len, bool hardwired) {
}
// send on serial, first byte to select the ring
-void usb_cb_ep2_out(uint8_t *usbdata, int len, bool hardwired) {
+void usb_cb_ep2_out(void *usbdata, int len, bool hardwired) {
UNUSED(hardwired);
- uart_ring *ur = get_ring_by_number(usbdata[0]);
+ uint8_t *usbdata8 = (uint8_t *)usbdata;
+ uart_ring *ur = get_ring_by_number(usbdata8[0]);
if ((len != 0) && (ur != NULL)) {
- if ((usbdata[0] < 2) || safety_tx_lin_hook(usbdata[0]-2, usbdata+1, len-1)) {
+ if ((usbdata8[0] < 2U) || safety_tx_lin_hook(usbdata8[0] - 2U, usbdata8 + 1, len - 1)) {
for (int i = 1; i < len; i++) {
- while (!putc(ur, usbdata[i])) {
+ while (!putc(ur, usbdata8[i])) {
// wait
}
}
@@ -165,33 +233,29 @@ void usb_cb_ep2_out(uint8_t *usbdata, int len, bool hardwired) {
}
// send on CAN
-void usb_cb_ep3_out(uint8_t *usbdata, int len, bool hardwired) {
+void usb_cb_ep3_out(void *usbdata, int len, bool hardwired) {
UNUSED(hardwired);
int dpkt = 0;
- for (dpkt = 0; dpkt < len; dpkt += 0x10) {
- uint32_t *tf = (uint32_t*)(&usbdata[dpkt]);
-
- // make a copy
+ uint32_t *d32 = (uint32_t *)usbdata;
+ for (dpkt = 0; dpkt < (len / 4); dpkt += 4) {
CAN_FIFOMailBox_TypeDef to_push;
- to_push.RDHR = tf[3];
- to_push.RDLR = tf[2];
- to_push.RDTR = tf[1];
- to_push.RIR = tf[0];
+ to_push.RDHR = d32[dpkt + 3];
+ to_push.RDLR = d32[dpkt + 2];
+ to_push.RDTR = d32[dpkt + 1];
+ to_push.RIR = d32[dpkt];
uint8_t bus_number = (to_push.RDTR >> 4) & CAN_BUS_NUM_MASK;
can_send(&to_push, bus_number);
}
}
-bool is_enumerated = 0;
-
void usb_cb_enumeration_complete() {
puts("USB enumeration complete\n");
is_enumerated = 1;
}
int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired) {
- int resp_len = 0;
+ unsigned int resp_len = 0;
uart_ring *ur = NULL;
int i;
switch (setup->b.bRequest) {
@@ -203,16 +267,16 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired)
puts(" err: "); puth(can_err_cnt);
puts("\n");
break;
- // **** 0xc1: is grey panda
+ // **** 0xc1: get hardware type
case 0xc1:
- resp[0] = is_grey_panda;
+ resp[0] = hw_type;
resp_len = 1;
break;
// **** 0xd0: fetch serial number
case 0xd0:
// addresses are OTP
- if (setup->b.wValue.w == 1) {
- memcpy(resp, (void *)0x1fff79c0, 0x10);
+ if (setup->b.wValue.w == 1U) {
+ (void)memcpy(resp, (uint8_t *)0x1fff79c0, 0x10);
resp_len = 0x10;
} else {
get_provision_chunk(resp);
@@ -248,8 +312,8 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired)
// **** 0xd6: get version
case 0xd6:
COMPILE_TIME_ASSERT(sizeof(gitversion) <= MAX_RESP_LEN);
- memcpy(resp, gitversion, sizeof(gitversion));
- resp_len = sizeof(gitversion)-1;
+ (void)memcpy(resp, gitversion, sizeof(gitversion));
+ resp_len = sizeof(gitversion) - 1U;
break;
// **** 0xd8: reset ST
case 0xd8:
@@ -257,66 +321,58 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired)
break;
// **** 0xd9: set ESP power
case 0xd9:
- if (setup->b.wValue.w == 1) {
- set_esp_mode(ESP_ENABLED);
- } else if (setup->b.wValue.w == 2) {
- set_esp_mode(ESP_BOOTMODE);
+ if (setup->b.wValue.w == 1U) {
+ current_board->set_esp_gps_mode(ESP_GPS_ENABLED);
+ } else if (setup->b.wValue.w == 2U) {
+ current_board->set_esp_gps_mode(ESP_GPS_BOOTMODE);
} else {
- set_esp_mode(ESP_DISABLED);
+ current_board->set_esp_gps_mode(ESP_GPS_DISABLED);
}
break;
// **** 0xda: reset ESP, with optional boot mode
case 0xda:
- set_esp_mode(ESP_DISABLED);
+ current_board->set_esp_gps_mode(ESP_GPS_DISABLED);
delay(1000000);
- if (setup->b.wValue.w == 1) {
- set_esp_mode(ESP_BOOTMODE);
+ if (setup->b.wValue.w == 1U) {
+ current_board->set_esp_gps_mode(ESP_GPS_BOOTMODE);
} else {
- set_esp_mode(ESP_ENABLED);
+ current_board->set_esp_gps_mode(ESP_GPS_ENABLED);
}
delay(1000000);
- set_esp_mode(ESP_ENABLED);
+ current_board->set_esp_gps_mode(ESP_GPS_ENABLED);
break;
- // **** 0xdb: set GMLAN multiplexing mode
+ // **** 0xdb: set GMLAN (white/grey) or OBD CAN (black) multiplexing mode
case 0xdb:
- if (setup->b.wValue.w == 1) {
- // GMLAN ON
- if (setup->b.wIndex.w == 1) {
- can_set_gmlan(1);
- } else if (setup->b.wIndex.w == 2) {
- can_set_gmlan(2);
- }
+ if(hw_type == HW_TYPE_BLACK_PANDA){
+ if (setup->b.wValue.w == 1U) {
+ // Enable OBD CAN
+ current_board->set_can_mode(CAN_MODE_OBD_CAN2);
+ } else {
+ // Disable OBD CAN
+ current_board->set_can_mode(CAN_MODE_NORMAL);
+ }
} else {
- can_set_gmlan(-1);
+ if (setup->b.wValue.w == 1U) {
+ // GMLAN ON
+ if (setup->b.wIndex.w == 1U) {
+ can_set_gmlan(1);
+ } else if (setup->b.wIndex.w == 2U) {
+ can_set_gmlan(2);
+ } else {
+ puts("Invalid bus num for GMLAN CAN set\n");
+ }
+ } else {
+ can_set_gmlan(-1);
+ }
}
break;
+
// **** 0xdc: set safety mode
case 0xdc:
- // this is the only way to leave silent mode
- // and it's blocked over WiFi
- // Allow ELM security mode to be set over wifi.
+ // Blocked over WiFi.
+ // Allow NOOUTPUT and ELM security mode to be set over wifi.
if (hardwired || (setup->b.wValue.w == SAFETY_NOOUTPUT) || (setup->b.wValue.w == SAFETY_ELM327)) {
- safety_set_mode(setup->b.wValue.w, (int16_t)setup->b.wIndex.w);
- if (safety_ignition_hook() != -1) {
- // if the ignition hook depends on something other than the started GPIO
- // we have to disable power savings (fix for GM and Tesla)
- set_power_save_state(POWER_SAVE_STATUS_DISABLED);
- }
- #ifndef EON
- // always LIVE on EON
- switch (setup->b.wValue.w) {
- case SAFETY_NOOUTPUT:
- can_silent = ALL_CAN_SILENT;
- break;
- case SAFETY_ELM327:
- can_silent = ALL_CAN_BUT_MAIN_SILENT;
- break;
- default:
- can_silent = ALL_CAN_LIVE;
- break;
- }
- #endif
- can_init_all();
+ set_safety_mode(setup->b.wValue.w, (uint16_t) setup->b.wIndex.w);
}
break;
// **** 0xdd: enable can forwarding
@@ -326,8 +382,10 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired)
if ((setup->b.wValue.w < BUS_MAX) && (setup->b.wIndex.w < BUS_MAX) &&
(setup->b.wValue.w != setup->b.wIndex.w)) { // set forwarding
can_set_forwarding(setup->b.wValue.w, setup->b.wIndex.w & CAN_BUS_NUM_MASK);
- } else if((setup->b.wValue.w < BUS_MAX) && (setup->b.wIndex.w == 0xFF)){ //Clear Forwarding
+ } else if((setup->b.wValue.w < BUS_MAX) && (setup->b.wIndex.w == 0xFFU)){ //Clear Forwarding
can_set_forwarding(setup->b.wValue.w, -1);
+ } else {
+ puts("Invalid CAN bus forwarding\n");
}
break;
// **** 0xde: set can bitrate
@@ -340,7 +398,7 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired)
// **** 0xdf: set long controls allowed
case 0xdf:
if (hardwired) {
- long_controls_allowed = setup->b.wValue.w & 1;
+ long_controls_allowed = setup->b.wValue.w & 1U;
}
break;
// **** 0xe0: uart read
@@ -401,25 +459,25 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired)
break;
// **** 0xe5: set CAN loopback (for testing)
case 0xe5:
- can_loopback = (setup->b.wValue.w > 0);
+ can_loopback = (setup->b.wValue.w > 0U);
can_init_all();
break;
// **** 0xe6: set USB power
case 0xe6:
- if (setup->b.wValue.w == 1) {
+ if (setup->b.wValue.w == 1U) {
puts("user setting CDP mode\n");
- set_usb_power_mode(USB_POWER_CDP);
- } else if (setup->b.wValue.w == 2) {
+ current_board->set_usb_power_mode(USB_POWER_CDP);
+ } else if (setup->b.wValue.w == 2U) {
puts("user setting DCP mode\n");
- set_usb_power_mode(USB_POWER_DCP);
+ current_board->set_usb_power_mode(USB_POWER_DCP);
} else {
puts("user setting CLIENT mode\n");
- set_usb_power_mode(USB_POWER_CLIENT);
+ current_board->set_usb_power_mode(USB_POWER_CLIENT);
}
break;
// **** 0xf0: do k-line wValue pulse on uart2 for Acura
case 0xf0:
- if (setup->b.wValue.w == 1) {
+ if (setup->b.wValue.w == 1U) {
GPIOC->ODR &= ~(1U << 10);
GPIOC->MODER &= ~GPIO_MODER_MODER10_1;
GPIOC->MODER |= GPIO_MODER_MODER10_0;
@@ -431,7 +489,7 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired)
for (i = 0; i < 80; i++) {
delay(8000);
- if (setup->b.wValue.w == 1) {
+ if (setup->b.wValue.w == 1U) {
GPIOC->ODR |= (1U << 10);
GPIOC->ODR &= ~(1U << 10);
} else {
@@ -440,7 +498,7 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired)
}
}
- if (setup->b.wValue.w == 1) {
+ if (setup->b.wValue.w == 1U) {
GPIOC->MODER &= ~GPIO_MODER_MODER10_0;
GPIOC->MODER |= GPIO_MODER_MODER10_1;
} else {
@@ -452,12 +510,14 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired)
break;
// **** 0xf1: Clear CAN ring buffer.
case 0xf1:
- if (setup->b.wValue.w == 0xFFFF) {
+ if (setup->b.wValue.w == 0xFFFFU) {
puts("Clearing CAN Rx queue\n");
can_clear(&can_rx_q);
} else if (setup->b.wValue.w < BUS_MAX) {
puts("Clearing CAN Tx queue\n");
can_clear(can_queues[setup->b.wValue.w]);
+ } else {
+ puts("Clearing CAN CAN ring buffer failed: wrong bus number\n");
}
break;
// **** 0xf2: Clear UART ring buffer.
@@ -470,6 +530,12 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired)
}
break;
}
+ // **** 0xf3: Heartbeat. Resets heartbeat counter.
+ case 0xf3:
+ {
+ heartbeat_counter = 0U;
+ break;
+ }
default:
puts("NO HANDLER ");
puth(setup->b.bRequest);
@@ -479,6 +545,7 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired)
return resp_len;
}
+#ifndef EON
int spi_cb_rx(uint8_t *data, int len, uint8_t *data_out) {
// data[0] = endpoint
// data[2] = length
@@ -508,111 +575,70 @@ int spi_cb_rx(uint8_t *data, int len, uint8_t *data_out) {
}
return resp_len;
}
-
+#endif
// ***************************** main code *****************************
+// cppcheck-suppress unusedFunction ; used in headers not included in cppcheck
void __initialize_hardware_early(void) {
early();
}
void __attribute__ ((noinline)) enable_fpu(void) {
// enable the FPU
- SCB->CPACR |= ((3UL << (10U * 2)) | (3UL << (11U * 2)));
+ SCB->CPACR |= ((3UL << (10U * 2U)) | (3UL << (11U * 2U)));
}
uint64_t tcnt = 0;
-uint64_t marker = 0;
+
+// go into NOOUTPUT when the EON does not send a heartbeat for this amount of seconds.
+#define EON_HEARTBEAT_THRESHOLD_IGNITION_ON 5U
+#define EON_HEARTBEAT_THRESHOLD_IGNITION_OFF 2U
// called once per second
+// cppcheck-suppress unusedFunction ; used in headers not included in cppcheck
void TIM3_IRQHandler(void) {
- #define CURRENT_THRESHOLD 0xF00
- #define CLICKS 5 // 5 seconds to switch modes
-
if (TIM3->SR != 0) {
can_live = pending_can_live;
+ current_board->usb_power_mode_tick(tcnt);
+
//puth(usart1_dma); puts(" "); puth(DMA2_Stream5->M0AR); puts(" "); puth(DMA2_Stream5->NDTR); puts("\n");
- uint32_t current = adc_get(ADCCHAN_CURRENT);
-
- switch (usb_power_mode) {
- case USB_POWER_CLIENT:
- if ((tcnt-marker) >= CLICKS) {
- if (!is_enumerated) {
- puts("USBP: didn't enumerate, switching to CDP mode\n");
- // switch to CDP
- set_usb_power_mode(USB_POWER_CDP);
- marker = tcnt;
- }
- }
- // keep resetting the timer if it's enumerated
- if (is_enumerated) {
- marker = tcnt;
- }
- break;
- case USB_POWER_CDP:
- // On the EON, if we get into CDP mode we stay here. No need to go to DCP.
- #ifndef EON
- // been CLICKS clicks since we switched to CDP
- if ((tcnt-marker) >= CLICKS) {
- // measure current draw, if positive and no enumeration, switch to DCP
- if (!is_enumerated && (current < CURRENT_THRESHOLD)) {
- puts("USBP: no enumeration with current draw, switching to DCP mode\n");
- set_usb_power_mode(USB_POWER_DCP);
- marker = tcnt;
- }
- }
- // keep resetting the timer if there's no current draw in CDP
- if (current >= CURRENT_THRESHOLD) {
- marker = tcnt;
- }
- #endif
- break;
- case USB_POWER_DCP:
- // been at least CLICKS clicks since we switched to DCP
- if ((tcnt-marker) >= CLICKS) {
- // if no current draw, switch back to CDP
- if (current >= CURRENT_THRESHOLD) {
- puts("USBP: no current draw, switching back to CDP mode\n");
- set_usb_power_mode(USB_POWER_CDP);
- marker = tcnt;
- }
- }
- // keep resetting the timer if there's current draw in DCP
- if (current < CURRENT_THRESHOLD) {
- marker = tcnt;
- }
- break;
- default:
- puts("USB power mode invalid\n"); // set_usb_power_mode prevents assigning invalid values
- break;
- }
-
- // ~0x9a = 500 ma
- /*puth(current);
- puts("\n");*/
-
// reset this every 16th pass
- if ((tcnt&0xF) == 0) {
+ if ((tcnt & 0xFU) == 0U) {
pending_can_live = 0;
}
#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");
+ //TODO: re-enable
+ //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
// set green LED to be controls allowed
- set_led(LED_GREEN, controls_allowed);
+ current_board->set_led(LED_GREEN, controls_allowed);
// turn off the blue LED, turned on by CAN
// unless we are in power saving mode
- set_led(LED_BLUE, (tcnt & 1) && (power_save_status == POWER_SAVE_STATUS_ENABLED));
+ current_board->set_led(LED_BLUE, (tcnt & 1U) && (power_save_status == POWER_SAVE_STATUS_ENABLED));
+
+ // increase heartbeat counter and cap it at the uint32 limit
+ if (heartbeat_counter < __UINT32_MAX__) {
+ heartbeat_counter += 1U;
+ }
+
+ // check heartbeat counter if we are running EON code. If the heartbeat has been gone for a while, go to NOOUTPUT safety mode.
+ #ifdef EON
+ if (heartbeat_counter >= (current_board->check_ignition() ? EON_HEARTBEAT_THRESHOLD_IGNITION_ON : EON_HEARTBEAT_THRESHOLD_IGNITION_OFF)) {
+ puts("EON hasn't sent a heartbeat for 0x"); puth(heartbeat_counter); puts(" seconds. Safety is set to NOOUTPUT mode.\n");
+ set_safety_mode(SAFETY_NOOUTPUT, 0U);
+ }
+ #endif
// on to the next one
- tcnt += 1;
+ tcnt += 1U;
}
TIM3->SR = 0;
}
@@ -623,26 +649,27 @@ int main(void) {
// init early devices
clock_init();
- periph_init();
- detect();
-
+ peripherals_init();
+ detect_configuration();
+ detect_board_type();
+ adc_init();
+
// print hello
puts("\n\n\n************************ MAIN START ************************\n");
- // detect the revision and init the GPIOs
- puts("config:\n");
- puts((revision == PANDA_REV_C) ? " panda rev c\n" : " panda rev a or b\n");
- puts(has_external_debug_serial ? " real serial\n" : " USB serial\n");
- puts(is_giant_panda ? " GIANTpanda detected\n" : " not GIANTpanda\n");
- puts(is_grey_panda ? " gray panda detected!\n" : " white panda\n");
- puts(is_entering_bootmode ? " ESP wants bootmode\n" : " no bootmode\n");
-
- // non rev c panda are no longer supported
- while (revision != PANDA_REV_C) {
- // hang
+ // check for non-supported board types
+ if(hw_type == HW_TYPE_UNKNOWN){
+ puts("Unsupported board type\n");
+ while (1) { /* hang */ }
}
- gpio_init();
+ puts("Config:\n");
+ puts(" Board type: "); puts(current_board->board_type); puts("\n");
+ puts(has_external_debug_serial ? " Real serial\n" : " USB serial\n");
+ puts(is_entering_bootmode ? " ESP wants bootmode\n" : " No bootmode\n");
+
+ // init board
+ current_board->init();
// panda has an FPU, let's use it!
enable_fpu();
@@ -654,18 +681,21 @@ int main(void) {
uart_init(USART2, 115200);
}
- if (is_grey_panda) {
+ if (board_has_gps()) {
uart_init(USART1, 9600);
} else {
// enable ESP uart
uart_init(USART1, 115200);
}
- // enable LIN
- uart_init(UART5, 10400);
- UART5->CR2 |= USART_CR2_LINEN;
- uart_init(USART3, 10400);
- USART3->CR2 |= USART_CR2_LINEN;
+ // there is no LIN on panda black
+ if(hw_type != HW_TYPE_BLACK_PANDA){
+ // enable LIN
+ uart_init(UART5, 10400);
+ UART5->CR2 |= USART_CR2_LINEN;
+ uart_init(USART3, 10400);
+ USART3->CR2 |= USART_CR2_LINEN;
+ }
// init microsecond system timer
// increments 1000000 times per second
@@ -675,37 +705,36 @@ int main(void) {
TIM2->EGR = TIM_EGR_UG;
// use TIM2->CNT to read
- // enable USB
- usb_init();
-
// default to silent mode to prevent issues with Ford
// hardcode a specific safety mode if you want to force the panda to be in a specific mode
- safety_set_mode(SAFETY_NOOUTPUT, 0);
-#ifdef EON
- // if we're on an EON, it's fine for CAN to be live for fingerprinting
- can_silent = ALL_CAN_LIVE;
-#else
+ int err = safety_set_mode(SAFETY_NOOUTPUT, 0);
+ if (err == -1) {
+ puts("Failed to set safety mode\n");
+ while (true) {
+ // if SAFETY_NOOUTPUT isn't succesfully set, we can't continue
+ }
+ }
can_silent = ALL_CAN_SILENT;
-#endif
can_init_all();
- adc_init();
-
#ifndef EON
spi_init();
#endif
#ifdef EON
// have to save power
- if (!is_grey_panda) {
- set_esp_mode(ESP_DISABLED);
+ if (hw_type == HW_TYPE_WHITE_PANDA) {
+ current_board->set_esp_gps_mode(ESP_GPS_DISABLED);
}
// only enter power save after the first cycle
- /*if (is_gpio_started()) {
+ /*if (current_board->check_ignition()) {
set_power_save_state(POWER_SAVE_STATUS_ENABLED);
}*/
- // interrupt on started line
- started_interrupt_init();
+
+ if (hw_type != HW_TYPE_BLACK_PANDA) {
+ // interrupt on started line
+ started_interrupt_init();
+ }
#endif
// 48mhz / 65536 ~= 732 / 732 = 1
@@ -715,6 +744,8 @@ int main(void) {
#ifdef DEBUG
puts("DEBUG ENABLED\n");
#endif
+ // enable USB (right before interrupts or enum can fail!)
+ usb_init();
puts("**** INTERRUPTS ON ****\n");
enable_interrupts();
@@ -730,9 +761,9 @@ int main(void) {
for (int div_mode_loop = 0; div_mode_loop < div_mode; div_mode_loop++) {
for (int fade = 0; fade < 1024; fade += 8) {
for (int i = 0; i < (128/div_mode); i++) {
- set_led(LED_RED, 1);
+ current_board->set_led(LED_RED, 1);
if (fade < 512) { delay(fade); } else { delay(1024-fade); }
- set_led(LED_RED, 0);
+ current_board->set_led(LED_RED, 0);
if (fade < 512) { delay(512-fade); } else { delay(fade-512); }
}
}
@@ -744,4 +775,3 @@ int main(void) {
return 0;
}
-
diff --git a/panda/board/main_declarations.h b/panda/board/main_declarations.h
new file mode 100644
index 000000000..8929e9ac0
--- /dev/null
+++ b/panda/board/main_declarations.h
@@ -0,0 +1,14 @@
+// ******************** Prototypes ********************
+void puts(const char *a);
+void puth(unsigned int i);
+void puth2(unsigned int i);
+typedef struct board board;
+typedef struct harness_configuration harness_configuration;
+void can_flip_buses(uint8_t bus1, uint8_t bus2);
+void can_set_obd(uint8_t harness_orientation, bool obd);
+
+// ********************* Globals **********************
+uint8_t hw_type = 0;
+const board *current_board;
+bool is_enumerated = 0;
+uint32_t heartbeat_counter = 0;
\ No newline at end of file
diff --git a/panda/board/pedal/Makefile b/panda/board/pedal/Makefile
index 37b95f90f..7ce6dd076 100644
--- a/panda/board/pedal/Makefile
+++ b/panda/board/pedal/Makefile
@@ -1,10 +1,10 @@
# :set noet
PROJ_NAME = comma
-CFLAGS = -O2 -Wall -std=gnu11 -DPEDAL
+CFLAGS = -O2 -Wall -Wextra -Wstrict-prototypes -Werror -std=gnu11 -DPEDAL
CFLAGS += -mlittle-endian -mthumb -mcpu=cortex-m3
CFLAGS += -msoft-float -DSTM32F2 -DSTM32F205xx
-CFLAGS += -I ../inc -I ../ -I ../../ -nostdlib
+CFLAGS += -I ../inc -I ../ -I ../../ -nostdlib -fno-builtin
CFLAGS += -T../stm32_flash.ld
STARTUP_FILE = startup_stm32f205xx
diff --git a/panda/board/pedal/main.c b/panda/board/pedal/main.c
index 4656db283..194370fa3 100644
--- a/panda/board/pedal/main.c
+++ b/panda/board/pedal/main.c
@@ -1,14 +1,20 @@
+// ********************* Includes *********************
#include "../config.h"
+#include "libc.h"
+
+#include "main_declarations.h"
#include "drivers/llcan.h"
#include "drivers/llgpio.h"
-#include "drivers/clock.h"
#include "drivers/adc.h"
+
+#include "board.h"
+
+#include "drivers/clock.h"
#include "drivers/dac.h"
#include "drivers/timer.h"
#include "gpio.h"
-#include "libc.h"
#define CAN CAN1
@@ -19,14 +25,22 @@
#include "drivers/usb.h"
#else
// no serial either
- void puts(const char *a) {}
- void puth(unsigned int i) {}
+ void puts(const char *a) {
+ UNUSED(a);
+ }
+ void puth(unsigned int i) {
+ UNUSED(i);
+ }
+ void puth2(unsigned int i) {
+ UNUSED(i);
+ }
#endif
#define ENTER_BOOTLOADER_MAGIC 0xdeadbeef
uint32_t enter_bootloader_mode;
-void __initialize_hardware_early() {
+// cppcheck-suppress unusedFunction ; used in headers not included in cppcheck
+void __initialize_hardware_early(void) {
early();
}
@@ -36,31 +50,54 @@ void __initialize_hardware_early() {
void debug_ring_callback(uart_ring *ring) {
char rcv;
- while (getc(ring, &rcv)) {
- putc(ring, rcv);
+ while (getc(ring, &rcv) != 0) {
+ (void)putc(ring, rcv);
}
}
-int usb_cb_ep1_in(uint8_t *usbdata, int len, bool hardwired) { return 0; }
-void usb_cb_ep2_out(uint8_t *usbdata, int len, bool hardwired) {}
-void usb_cb_ep3_out(uint8_t *usbdata, int len, bool hardwired) {}
-void usb_cb_enumeration_complete() {}
+int usb_cb_ep1_in(uint8_t *usbdata, int len, bool hardwired) {
+ UNUSED(usbdata);
+ UNUSED(len);
+ UNUSED(hardwired);
+ return 0;
+}
+void usb_cb_ep2_out(uint8_t *usbdata, int len, bool hardwired) {
+ UNUSED(usbdata);
+ UNUSED(len);
+ UNUSED(hardwired);
+}
+void usb_cb_ep3_out(uint8_t *usbdata, int len, bool hardwired) {
+ UNUSED(usbdata);
+ UNUSED(len);
+ UNUSED(hardwired);
+}
+void usb_cb_enumeration_complete(void) {}
int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired) {
- int resp_len = 0;
+ UNUSED(hardwired);
+ unsigned int resp_len = 0;
uart_ring *ur = NULL;
switch (setup->b.bRequest) {
// **** 0xe0: uart read
case 0xe0:
ur = get_ring_by_number(setup->b.wValue.w);
- if (!ur) break;
- if (ur == &esp_ring) uart_dma_drain();
+ if (!ur) {
+ break;
+ }
+ if (ur == &esp_ring) {
+ uart_dma_drain();
+ }
// read
while ((resp_len < MIN(setup->b.wLength.w, MAX_RESP_LEN)) &&
getc(ur, (char*)&resp[resp_len])) {
++resp_len;
}
break;
+ default:
+ puts("NO HANDLER ");
+ puth(setup->b.bRequest);
+ puts("\n");
+ break;
}
return resp_len;
}
@@ -76,7 +113,7 @@ uint8_t pedal_checksum(uint8_t *dat, int len) {
for (i = len - 1; i >= 0; i--) {
crc ^= dat[i];
for (j = 0; j < 8; j++) {
- if ((crc & 0x80) != 0) {
+ if ((crc & 0x80U) != 0U) {
crc = (uint8_t)((crc << 1) ^ poly);
}
else {
@@ -91,11 +128,12 @@ uint8_t pedal_checksum(uint8_t *dat, int len) {
// addresses to be used on CAN
#define CAN_GAS_INPUT 0x200
-#define CAN_GAS_OUTPUT 0x201
+#define CAN_GAS_OUTPUT 0x201U
#define CAN_GAS_SIZE 6
-#define COUNTER_CYCLE 0xF
+#define COUNTER_CYCLE 0xFU
-void CAN1_TX_IRQHandler() {
+// cppcheck-suppress unusedFunction ; used in headers not included in cppcheck
+void CAN1_TX_IRQHandler(void) {
// clear interrupt
CAN->TSR |= CAN_TSR_RQCP0;
}
@@ -104,54 +142,54 @@ void CAN1_TX_IRQHandler() {
uint16_t gas_set_0 = 0;
uint16_t gas_set_1 = 0;
-#define MAX_TIMEOUT 10
+#define MAX_TIMEOUT 10U
uint32_t timeout = 0;
uint32_t current_index = 0;
-#define NO_FAULT 0
-#define FAULT_BAD_CHECKSUM 1
-#define FAULT_SEND 2
-#define FAULT_SCE 3
-#define FAULT_STARTUP 4
-#define FAULT_TIMEOUT 5
-#define FAULT_INVALID 6
+#define NO_FAULT 0U
+#define FAULT_BAD_CHECKSUM 1U
+#define FAULT_SEND 2U
+#define FAULT_SCE 3U
+#define FAULT_STARTUP 4U
+#define FAULT_TIMEOUT 5U
+#define FAULT_INVALID 6U
uint8_t state = FAULT_STARTUP;
-void CAN1_RX0_IRQHandler() {
- while (CAN->RF0R & CAN_RF0R_FMP0) {
+// cppcheck-suppress unusedFunction ; used in headers not included in cppcheck
+void CAN1_RX0_IRQHandler(void) {
+ while ((CAN->RF0R & CAN_RF0R_FMP0) != 0) {
#ifdef DEBUG
puts("CAN RX\n");
#endif
- uint32_t address = CAN->sFIFOMailBox[0].RIR>>21;
+ int address = CAN->sFIFOMailBox[0].RIR >> 21;
if (address == CAN_GAS_INPUT) {
// softloader entry
- if (CAN->sFIFOMailBox[0].RDLR == 0xdeadface) {
- if (CAN->sFIFOMailBox[0].RDHR == 0x0ab00b1e) {
+ if (GET_BYTES_04(&CAN->sFIFOMailBox[0]) == 0xdeadface) {
+ if (GET_BYTES_48(&CAN->sFIFOMailBox[0]) == 0x0ab00b1e) {
enter_bootloader_mode = ENTER_SOFTLOADER_MAGIC;
NVIC_SystemReset();
- } else if (CAN->sFIFOMailBox[0].RDHR == 0x02b00b1e) {
+ } else if (GET_BYTES_48(&CAN->sFIFOMailBox[0]) == 0x02b00b1e) {
enter_bootloader_mode = ENTER_BOOTLOADER_MAGIC;
NVIC_SystemReset();
+ } else {
+ puts("Failed entering Softloader or Bootloader\n");
}
}
// normal packet
uint8_t dat[8];
- uint8_t *rdlr = (uint8_t *)&CAN->sFIFOMailBox[0].RDLR;
- uint8_t *rdhr = (uint8_t *)&CAN->sFIFOMailBox[0].RDHR;
- for (int i=0; i<4; i++) {
- dat[i] = rdlr[i];
- dat[i+4] = rdhr[i];
+ for (int i=0; i<8; i++) {
+ dat[i] = GET_BYTE(&CAN->sFIFOMailBox[0], i);
}
uint16_t value_0 = (dat[0] << 8) | dat[1];
uint16_t value_1 = (dat[2] << 8) | dat[3];
- uint8_t enable = (dat[4] >> 7) & 1;
+ bool enable = ((dat[4] >> 7) & 1U) != 0U;
uint8_t index = dat[4] & COUNTER_CYCLE;
if (pedal_checksum(dat, CAN_GAS_SIZE - 1) == dat[5]) {
- if (((current_index + 1) & COUNTER_CYCLE) == index) {
+ if (((current_index + 1U) & COUNTER_CYCLE) == index) {
#ifdef DEBUG
puts("setting gas ");
- puth(value);
+ puth(value_0);
puts("\n");
#endif
if (enable) {
@@ -159,12 +197,13 @@ void CAN1_RX0_IRQHandler() {
gas_set_1 = value_1;
} else {
// clear the fault state if values are 0
- if (value_0 == 0 && value_1 == 0) {
+ if ((value_0 == 0U) && (value_1 == 0U)) {
state = NO_FAULT;
} else {
state = FAULT_INVALID;
}
- gas_set_0 = gas_set_1 = 0;
+ gas_set_0 = 0;
+ gas_set_1 = 0;
}
// clear the timeout
timeout = 0;
@@ -180,17 +219,20 @@ void CAN1_RX0_IRQHandler() {
}
}
-void CAN1_SCE_IRQHandler() {
+// cppcheck-suppress unusedFunction ; used in headers not included in cppcheck
+void CAN1_SCE_IRQHandler(void) {
state = FAULT_SCE;
llcan_clear_send(CAN);
}
-int pdl0 = 0, pdl1 = 0;
-int pkt_idx = 0;
+uint32_t pdl0 = 0;
+uint32_t pdl1 = 0;
+unsigned int pkt_idx = 0;
int led_value = 0;
-void TIM3_IRQHandler() {
+// cppcheck-suppress unusedFunction ; used in headers not included in cppcheck
+void TIM3_IRQHandler(void) {
#ifdef DEBUG
puth(TIM3->CNT);
puts(" ");
@@ -203,16 +245,16 @@ void TIM3_IRQHandler() {
// check timer for sending the user pedal and clearing the CAN
if ((CAN->TSR & CAN_TSR_TME0) == CAN_TSR_TME0) {
uint8_t dat[8];
- dat[0] = (pdl0>>8) & 0xFF;
- dat[1] = (pdl0>>0) & 0xFF;
- dat[2] = (pdl1>>8) & 0xFF;
- dat[3] = (pdl1>>0) & 0xFF;
- dat[4] = (state & 0xF) << 4 | pkt_idx;
+ dat[0] = (pdl0 >> 8) & 0xFFU;
+ dat[1] = (pdl0 >> 0) & 0xFFU;
+ dat[2] = (pdl1 >> 8) & 0xFFU;
+ dat[3] = (pdl1 >> 0) & 0xFFU;
+ dat[4] = ((state & 0xFU) << 4) | pkt_idx;
dat[5] = pedal_checksum(dat, CAN_GAS_SIZE - 1);
- CAN->sTxMailBox[0].TDLR = dat[0] | (dat[1]<<8) | (dat[2]<<16) | (dat[3]<<24);
- CAN->sTxMailBox[0].TDHR = dat[4] | (dat[5]<<8);
+ CAN->sTxMailBox[0].TDLR = dat[0] | (dat[1] << 8) | (dat[2] << 16) | (dat[3] << 24);
+ CAN->sTxMailBox[0].TDHR = dat[4] | (dat[5] << 8);
CAN->sTxMailBox[0].TDTR = 6; // len of packet is 5
- CAN->sTxMailBox[0].TIR = (CAN_GAS_OUTPUT << 21) | 1;
+ CAN->sTxMailBox[0].TIR = (CAN_GAS_OUTPUT << 21) | 1U;
++pkt_idx;
pkt_idx &= COUNTER_CYCLE;
} else {
@@ -224,7 +266,7 @@ void TIM3_IRQHandler() {
}
// blink the LED
- set_led(LED_GREEN, led_value);
+ current_board->set_led(LED_GREEN, led_value);
led_value = !led_value;
TIM3->SR = 0;
@@ -233,13 +275,13 @@ void TIM3_IRQHandler() {
if (timeout == MAX_TIMEOUT) {
state = FAULT_TIMEOUT;
} else {
- timeout += 1;
+ timeout += 1U;
}
}
// ***************************** main code *****************************
-void pedal() {
+void pedal(void) {
// read/write
pdl0 = adc_get(ADCCHAN_ACCEL0);
pdl1 = adc_get(ADCCHAN_ACCEL1);
@@ -256,13 +298,14 @@ void pedal() {
watchdog_feed();
}
-int main() {
+int main(void) {
__disable_irq();
// init devices
clock_init();
- periph_init();
- gpio_init();
+ peripherals_init();
+ detect_configuration();
+ detect_board_type();
#ifdef PEDAL_USB
// enable USB
@@ -274,7 +317,11 @@ int main() {
adc_init();
// init can
- llcan_set_speed(CAN1, 5000, false, false);
+ bool llcan_speed_set = llcan_set_speed(CAN1, 5000, false, false);
+ if (!llcan_speed_set) {
+ puts("Failed to set llcan speed");
+ }
+
llcan_init(CAN1);
// 48mhz / 65536 ~= 732
diff --git a/panda/board/pedal/main_declarations.h b/panda/board/pedal/main_declarations.h
new file mode 100644
index 000000000..9a40f8ae3
--- /dev/null
+++ b/panda/board/pedal/main_declarations.h
@@ -0,0 +1,11 @@
+// ******************** Prototypes ********************
+void puts(const char *a);
+void puth(unsigned int i);
+void puth2(unsigned int i);
+typedef struct board board;
+typedef struct harness_configuration harness_configuration;
+
+// ********************* Globals **********************
+uint8_t hw_type = 0;
+const board *current_board;
+bool is_enumerated = 0;
\ No newline at end of file
diff --git a/panda/board/power_saving.h b/panda/board/power_saving.h
index 986adf3ce..94ebbb53c 100644
--- a/panda/board/power_saving.h
+++ b/panda/board/power_saving.h
@@ -10,33 +10,33 @@ void set_power_save_state(int state) {
bool enable = false;
if (state == POWER_SAVE_STATUS_ENABLED) {
puts("enable power savings\n");
- if (is_grey_panda) {
+ if (board_has_gps()) {
char UBLOX_SLEEP_MSG[] = "\xb5\x62\x06\x04\x04\x00\x01\x00\x08\x00\x17\x78";
uart_ring *ur = get_ring_by_number(1);
- for (unsigned int i = 0; i < sizeof(UBLOX_SLEEP_MSG) - 1; i++) while (!putc(ur, UBLOX_SLEEP_MSG[i]));
+ for (unsigned int i = 0; i < sizeof(UBLOX_SLEEP_MSG) - 1U; i++) while (!putc(ur, UBLOX_SLEEP_MSG[i]));
}
} else {
puts("disable power savings\n");
- if (is_grey_panda) {
+ if (board_has_gps()) {
char UBLOX_WAKE_MSG[] = "\xb5\x62\x06\x04\x04\x00\x01\x00\x09\x00\x18\x7a";
uart_ring *ur = get_ring_by_number(1);
- for (unsigned int i = 0; i < sizeof(UBLOX_WAKE_MSG) - 1; i++) while (!putc(ur, UBLOX_WAKE_MSG[i]));
+ for (unsigned int i = 0; i < sizeof(UBLOX_WAKE_MSG) - 1U; i++) while (!putc(ur, UBLOX_WAKE_MSG[i]));
}
enable = true;
}
- // turn on can
- set_can_enable(CAN1, enable);
- set_can_enable(CAN2, enable);
- set_can_enable(CAN3, enable);
+ // Switch CAN transcievers
+ current_board->enable_can_transcievers(enable);
- // turn on GMLAN
- set_gpio_output(GPIOB, 14, enable);
- set_gpio_output(GPIOB, 15, enable);
+ if(hw_type != HW_TYPE_BLACK_PANDA){
+ // turn on GMLAN
+ set_gpio_output(GPIOB, 14, enable);
+ set_gpio_output(GPIOB, 15, enable);
- // turn on LIN
- set_gpio_output(GPIOB, 7, enable);
- set_gpio_output(GPIOA, 14, enable);
+ // turn on LIN
+ set_gpio_output(GPIOB, 7, enable);
+ set_gpio_output(GPIOA, 14, enable);
+ }
power_save_status = state;
}
diff --git a/panda/board/provision.h b/panda/board/provision.h
index 2fad51350..9091322f1 100644
--- a/panda/board/provision.h
+++ b/panda/board/provision.h
@@ -5,9 +5,9 @@
// SHA1 checksum = 0x1C - 0x20
void get_provision_chunk(uint8_t *resp) {
- memcpy(resp, (void *)0x1fff79e0, PROVISION_CHUNK_LEN);
+ (void)memcpy(resp, (uint8_t *)0x1fff79e0, PROVISION_CHUNK_LEN);
if (memcmp(resp, "\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff", 0x20) == 0) {
- memcpy(resp, "unprovisioned\x00\x00\x00testing123\x00\x00\xa3\xa6\x99\xec", 0x20);
+ (void)memcpy(resp, "unprovisioned\x00\x00\x00testing123\x00\x00\xa3\xa6\x99\xec", 0x20);
}
}
diff --git a/panda/board/safety.h b/panda/board/safety.h
index dd99e6db2..6b2b5a045 100644
--- a/panda/board/safety.h
+++ b/panda/board/safety.h
@@ -44,21 +44,21 @@ typedef struct {
const safety_hooks *hooks;
} safety_hook_config;
-#define SAFETY_NOOUTPUT 0
-#define SAFETY_HONDA 1
-#define SAFETY_TOYOTA 2
-#define SAFETY_GM 3
-#define SAFETY_HONDA_BOSCH 4
-#define SAFETY_FORD 5
-#define SAFETY_CADILLAC 6
-#define SAFETY_HYUNDAI 7
-#define SAFETY_TESLA 8
-#define SAFETY_CHRYSLER 9
-#define SAFETY_SUBARU 10
-#define SAFETY_GM_ASCM 0x1334
-#define SAFETY_TOYOTA_IPAS 0x1335
-#define SAFETY_ALLOUTPUT 0x1337
-#define SAFETY_ELM327 0xE327
+#define SAFETY_NOOUTPUT 0U
+#define SAFETY_HONDA 1U
+#define SAFETY_TOYOTA 2U
+#define SAFETY_GM 3U
+#define SAFETY_HONDA_BOSCH 4U
+#define SAFETY_FORD 5U
+#define SAFETY_CADILLAC 6U
+#define SAFETY_HYUNDAI 7U
+#define SAFETY_TESLA 8U
+#define SAFETY_CHRYSLER 9U
+#define SAFETY_SUBARU 10U
+#define SAFETY_GM_ASCM 0x1334U
+#define SAFETY_TOYOTA_IPAS 0x1335U
+#define SAFETY_ALLOUTPUT 0x1337U
+#define SAFETY_ELM327 0xE327U
const safety_hook_config safety_hook_registry[] = {
{SAFETY_NOOUTPUT, &nooutput_hooks},
diff --git a/panda/board/safety/safety_cadillac.h b/panda/board/safety/safety_cadillac.h
index ef114abfe..ef6336095 100644
--- a/panda/board/safety/safety_cadillac.h
+++ b/panda/board/safety/safety_cadillac.h
@@ -10,13 +10,13 @@ const int CADILLAC_MAX_RATE_DOWN = 5;
const int CADILLAC_DRIVER_TORQUE_ALLOWANCE = 50;
const int CADILLAC_DRIVER_TORQUE_FACTOR = 4;
-int cadillac_ign = 0;
+bool cadillac_ign = 0;
int cadillac_cruise_engaged_last = 0;
int cadillac_rt_torque_last = 0;
const int cadillac_torque_msgs_n = 4;
int cadillac_desired_torque_last[CADILLAC_TORQUE_MSG_N] = {0};
uint32_t cadillac_ts_last = 0;
-int cadillac_supercruise_on = 0;
+bool cadillac_supercruise_on = 0;
struct sample_t cadillac_torque_driver; // last few driver torques measured
int cadillac_get_torque_idx(int addr, int array_size) {
@@ -28,7 +28,8 @@ static void cadillac_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
int addr = GET_ADDR(to_push);
if (addr == 356) {
- int torque_driver_new = ((to_push->RDLR & 0x7) << 8) | ((to_push->RDLR >> 8) & 0xFF);
+ int torque_driver_new = ((GET_BYTE(to_push, 0) & 0x7U) << 8) | (GET_BYTE(to_push, 1));
+
torque_driver_new = to_signed(torque_driver_new, 11);
// update array of samples
update_sample(&cadillac_torque_driver, torque_driver_new);
@@ -36,12 +37,12 @@ static void cadillac_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// this message isn't all zeros when ignition is on
if ((addr == 0x160) && (bus == 0)) {
- cadillac_ign = to_push->RDLR > 0;
+ cadillac_ign = GET_BYTES_04(to_push) != 0;
}
// enter controls on rising edge of ACC, exit controls on ACC off
if ((addr == 0x370) && (bus == 0)) {
- int cruise_engaged = to_push->RDLR & 0x800000; // bit 23
+ int cruise_engaged = GET_BYTE(to_push, 2) & 0x80; // bit 23
if (cruise_engaged && !cadillac_cruise_engaged_last) {
controls_allowed = 1;
}
@@ -53,7 +54,7 @@ static void cadillac_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// know supercruise mode and block openpilot msgs if on
if ((addr == 0x152) || (addr == 0x154)) {
- cadillac_supercruise_on = (to_push->RDHR>>4) & 0x1;
+ cadillac_supercruise_on = (GET_BYTE(to_push, 4) & 0x10) != 0;
}
}
@@ -63,7 +64,7 @@ static int cadillac_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
// steer cmd checks
if ((addr == 0x151) || (addr == 0x152) || (addr == 0x153) || (addr == 0x154)) {
- int desired_torque = ((to_send->RDLR & 0x3f) << 8) + ((to_send->RDLR & 0xff00) >> 8);
+ int desired_torque = ((GET_BYTE(to_send, 0) & 0x3f) << 8) | GET_BYTE(to_send, 1);
int violation = 0;
uint32_t ts = TIM2->CNT;
int idx = cadillac_get_torque_idx(addr, CADILLAC_TORQUE_MSG_N);
diff --git a/panda/board/safety/safety_chrysler.h b/panda/board/safety/safety_chrysler.h
index 19149b6b7..e60878573 100644
--- a/panda/board/safety/safety_chrysler.h
+++ b/panda/board/safety/safety_chrysler.h
@@ -18,8 +18,7 @@ static void chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// Measured eps torque
if (addr == 544) {
- uint32_t rdhr = to_push->RDHR;
- int torque_meas_new = ((rdhr & 0x7U) << 8) + ((rdhr & 0xFF00U) >> 8) - 1024U;
+ int torque_meas_new = ((GET_BYTE(to_push, 4) & 0x7U) << 8) + GET_BYTE(to_push, 5) - 1024U;
// update array of samples
update_sample(&chrysler_torque_meas, torque_meas_new);
@@ -27,7 +26,7 @@ static void chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// enter controls on rising edge of ACC, exit controls on ACC off
if (addr == 0x1F4) {
- int cruise_engaged = ((to_push->RDLR & 0x380000) >> 19) == 7;
+ int cruise_engaged = ((GET_BYTE(to_push, 2) & 0x38) >> 3) == 7;
if (cruise_engaged && !chrysler_cruise_engaged_last) {
controls_allowed = 1;
}
@@ -57,8 +56,7 @@ static int chrysler_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
// LKA STEER
if (addr == 0x292) {
- uint32_t rdlr = to_send->RDLR;
- int desired_torque = ((rdlr & 0x7U) << 8) + ((rdlr & 0xFF00U) >> 8) - 1024U;
+ int desired_torque = ((GET_BYTE(to_send, 0) & 0x7U) << 8) + GET_BYTE(to_send, 1) - 1024U;
uint32_t ts = TIM2->CNT;
bool violation = 0;
diff --git a/panda/board/safety/safety_elm327.h b/panda/board/safety/safety_elm327.h
index 1f44e992a..bbad909f2 100644
--- a/panda/board/safety/safety_elm327.h
+++ b/panda/board/safety/safety_elm327.h
@@ -1,15 +1,9 @@
static int elm327_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
int tx = 1;
- int bus = GET_BUS(to_send);
int addr = GET_ADDR(to_send);
int len = GET_LEN(to_send);
- //All ELM traffic must appear on CAN0
- if (bus != 0) {
- tx = 0;
- }
-
//All ISO 15765-4 messages must be 8 bytes long
if (len != 8) {
tx = 0;
diff --git a/panda/board/safety/safety_ford.h b/panda/board/safety/safety_ford.h
index 21c9c54db..0bb839f2f 100644
--- a/panda/board/safety/safety_ford.h
+++ b/panda/board/safety/safety_ford.h
@@ -9,7 +9,7 @@
int ford_brake_prev = 0;
int ford_gas_prev = 0;
-int ford_is_moving = 0;
+bool ford_moving = false;
static void ford_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
@@ -17,14 +17,16 @@ static void ford_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
if (addr == 0x217) {
// wheel speeds are 14 bits every 16
- ford_is_moving = 0xFCFF & (to_push->RDLR | (to_push->RDLR >> 16) |
- to_push->RDHR | (to_push->RDHR >> 16));
+ ford_moving = false;
+ for (int i = 0; i < 8; i += 2) {
+ ford_moving |= GET_BYTE(to_push, i) | (GET_BYTE(to_push, (int)(i + 1)) & 0xFCU);
+ }
}
// state machine to enter and exit controls
if (addr == 0x83) {
- bool cancel = (to_push->RDLR >> 8) & 0x1;
- bool set_or_resume = (to_push->RDLR >> 28) & 0x3;
+ bool cancel = GET_BYTE(to_push, 1) & 0x1;
+ bool set_or_resume = GET_BYTE(to_push, 3) & 0x30;
if (cancel) {
controls_allowed = 0;
}
@@ -36,8 +38,8 @@ static void ford_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// exit controls on rising edge of brake press or on brake press when
// speed > 0
if (addr == 0x165) {
- int brake = to_push->RDLR & 0x20;
- if (brake && (!(ford_brake_prev) || ford_is_moving)) {
+ int brake = GET_BYTE(to_push, 0) & 0x20;
+ if (brake && (!(ford_brake_prev) || ford_moving)) {
controls_allowed = 0;
}
ford_brake_prev = brake;
@@ -45,7 +47,7 @@ static void ford_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// exit controls on rising edge of gas press
if (addr == 0x204) {
- int gas = to_push->RDLR & 0xFF03;
+ int gas = (GET_BYTE(to_push, 0) & 0x03) | GET_BYTE(to_push, 1);
if (gas && !(ford_gas_prev)) {
controls_allowed = 0;
}
@@ -64,7 +66,7 @@ static int ford_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
int tx = 1;
// disallow actuator commands if gas or brake (with vehicle moving) are pressed
// and the the latching controls_allowed flag is True
- int pedal_pressed = ford_gas_prev || (ford_brake_prev && ford_is_moving);
+ int pedal_pressed = ford_gas_prev || (ford_brake_prev && ford_moving);
bool current_controls_allowed = controls_allowed && !(pedal_pressed);
int addr = GET_ADDR(to_send);
@@ -72,7 +74,7 @@ static int ford_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
if (addr == 0x3CA) {
if (!current_controls_allowed) {
// bits 7-4 need to be 0xF to disallow lkas commands
- if (((to_send->RDLR >> 4) & 0xF) != 0xF) {
+ if ((GET_BYTE(to_send, 0) & 0xF0) != 0xF0) {
tx = 0;
}
}
@@ -81,7 +83,7 @@ static int ford_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
// FORCE CANCEL: safety check only relevant when spamming the cancel button
// ensuring that set and resume aren't sent
if (addr == 0x83) {
- if (((to_send->RDLR >> 28) & 0x3) != 0) {
+ if ((GET_BYTE(to_send, 3) & 0x30) != 0) {
tx = 0;
}
}
diff --git a/panda/board/safety/safety_gm.h b/panda/board/safety/safety_gm.h
index 949f5c7e7..9ca5ca323 100644
--- a/panda/board/safety/safety_gm.h
+++ b/panda/board/safety/safety_gm.h
@@ -21,7 +21,7 @@ const int GM_MAX_BRAKE = 350;
int gm_brake_prev = 0;
int gm_gas_prev = 0;
-int gm_speed = 0;
+bool gm_moving = false;
// silence everything if stock car control ECUs are still online
bool gm_ascm_detected = 0;
bool gm_ignition_started = 0;
@@ -35,7 +35,7 @@ static void gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
int addr = GET_ADDR(to_push);
if (addr == 388) {
- int torque_driver_new = (((to_push->RDHR >> 16) & 0x7) << 8) | ((to_push->RDHR >> 24) & 0xFF);
+ int torque_driver_new = ((GET_BYTE(to_push, 6) & 0x7) << 8) | GET_BYTE(to_push, 7);
torque_driver_new = to_signed(torque_driver_new, 11);
// update array of samples
update_sample(&gm_torque_driver, torque_driver_new);
@@ -44,14 +44,14 @@ static void gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
if ((addr == 0x1F1) && (bus_number == 0)) {
//Bit 5 should be ignition "on"
//Backup plan is Bit 2 (accessory power)
- bool ign = ((to_push->RDLR) & 0x20) != 0;
+ bool ign = (GET_BYTE(to_push, 0) & 0x20) != 0;
gm_ignition_started = ign;
}
// sample speed, really only care if car is moving or not
// rear left wheel speed
if (addr == 842) {
- gm_speed = to_push->RDLR & 0xFFFF;
+ gm_moving = GET_BYTE(to_push, 0) | GET_BYTE(to_push, 1);
}
// Check if ASCM or LKA camera are online
@@ -65,7 +65,7 @@ static void gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// ACC steering wheel buttons
if (addr == 481) {
- int button = (to_push->RDHR >> 12) & 0x7;
+ int button = (GET_BYTE(to_push, 5) & 0x70) >> 4;
switch (button) {
case 2: // resume
case 3: // set
@@ -82,13 +82,13 @@ static void gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// exit controls on rising edge of brake press or on brake press when
// speed > 0
if (addr == 241) {
- int brake = (to_push->RDLR & 0xFF00) >> 8;
+ int brake = GET_BYTE(to_push, 1);
// Brake pedal's potentiometer returns near-zero reading
// even when pedal is not pressed
if (brake < 10) {
brake = 0;
}
- if (brake && (!gm_brake_prev || gm_speed)) {
+ if (brake && (!gm_brake_prev || gm_moving)) {
controls_allowed = 0;
}
gm_brake_prev = brake;
@@ -96,7 +96,7 @@ static void gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// exit controls on rising edge of gas press
if (addr == 417) {
- int gas = to_push->RDHR & 0xFF0000;
+ int gas = GET_BYTE(to_push, 6);
if (gas && !gm_gas_prev && long_controls_allowed) {
controls_allowed = 0;
}
@@ -105,7 +105,7 @@ static void gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// exit controls on regen paddle
if (addr == 189) {
- bool regen = to_push->RDLR & 0x20;
+ bool regen = GET_BYTE(to_push, 0) & 0x20;
if (regen) {
controls_allowed = 0;
}
@@ -129,15 +129,14 @@ static int gm_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
// disallow actuator commands if gas or brake (with vehicle moving) are pressed
// and the the latching controls_allowed flag is True
- int pedal_pressed = gm_gas_prev || (gm_brake_prev && gm_speed);
+ int pedal_pressed = gm_gas_prev || (gm_brake_prev && gm_moving);
bool current_controls_allowed = controls_allowed && !pedal_pressed;
int addr = GET_ADDR(to_send);
// BRAKE: safety check
if (addr == 789) {
- uint32_t rdlr = to_send->RDLR;
- int brake = ((rdlr & 0xFU) << 8) + ((rdlr & 0xFF00U) >> 8);
+ int brake = ((GET_BYTE(to_send, 0) & 0xFU) << 8) + GET_BYTE(to_send, 1);
brake = (0x1000 - brake) & 0xFFF;
if (!current_controls_allowed || !long_controls_allowed) {
if (brake != 0) {
@@ -151,8 +150,7 @@ static int gm_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
// LKA STEER: safety check
if (addr == 384) {
- uint32_t rdlr = to_send->RDLR;
- int desired_torque = ((rdlr & 0x7U) << 8) + ((rdlr & 0xFF00U) >> 8);
+ int desired_torque = ((GET_BYTE(to_send, 0) & 0x7U) << 8) + GET_BYTE(to_send, 1);
uint32_t ts = TIM2->CNT;
bool violation = 0;
desired_torque = to_signed(desired_torque, 11);
@@ -205,12 +203,11 @@ static int gm_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
// GAS/REGEN: safety check
if (addr == 715) {
- uint32_t rdlr = to_send->RDLR;
- int gas_regen = ((rdlr & 0x7F0000U) >> 11) + ((rdlr & 0xF8000000U) >> 27);
+ int gas_regen = ((GET_BYTE(to_send, 2) & 0x7FU) << 5) + ((GET_BYTE(to_send, 3) & 0xF8U) >> 3);
// Disabled message is !engaed with gas
// value that corresponds to max regen.
if (!current_controls_allowed || !long_controls_allowed) {
- bool apply = (rdlr & 1U) != 0U;
+ bool apply = GET_BYTE(to_send, 0) & 1U;
if (apply || (gas_regen != GM_MAX_REGEN)) {
tx = 0;
}
diff --git a/panda/board/safety/safety_gm_ascm.h b/panda/board/safety/safety_gm_ascm.h
index d452818d6..82f1db6ae 100644
--- a/panda/board/safety/safety_gm_ascm.h
+++ b/panda/board/safety/safety_gm_ascm.h
@@ -13,7 +13,7 @@ static int gm_ascm_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
// block 0x315 and 0x2cb, which are the brake and accel commands from ASCM1
//if ((addr == 0x152) || (addr == 0x154) || (addr == 0x315) || (addr == 0x2cb)) {
if ((addr == 0x152) || (addr == 0x154)) {
- int supercruise_on = (to_fwd->RDHR >> 4) & 0x1; // bit 36
+ bool supercruise_on = (GET_BYTE(to_fwd, 4) & 0x10) != 0; // bit 36
if (!supercruise_on) {
bus_fwd = -1;
}
diff --git a/panda/board/safety/safety_honda.h b/panda/board/safety/safety_honda.h
index 44a57ec97..80237dccb 100644
--- a/panda/board/safety/safety_honda.h
+++ b/panda/board/safety/safety_honda.h
@@ -10,7 +10,7 @@
const int HONDA_GAS_INTERCEPTOR_THRESHOLD = 328; // ratio between offset and gain from dbc file
int honda_brake_prev = 0;
int honda_gas_prev = 0;
-int honda_ego_speed = 0;
+bool honda_moving = false;
bool honda_bosch_hardware = false;
bool honda_alt_brake_msg = false;
@@ -22,13 +22,13 @@ static void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// sample speed
if (addr == 0x158) {
// first 2 bytes
- honda_ego_speed = to_push->RDLR & 0xFFFF;
+ honda_moving = GET_BYTE(to_push, 0) | GET_BYTE(to_push, 1);
}
// state machine to enter and exit controls
// 0x1A6 for the ILX, 0x296 for the Civic Touring
if ((addr == 0x1A6) || (addr == 0x296)) {
- int button = (to_push->RDLR & 0xE0) >> 5;
+ int button = (GET_BYTE(to_push, 0) & 0xE0) >> 5;
switch (button) {
case 2: // cancel
controls_allowed = 0;
@@ -48,14 +48,11 @@ static void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// in these cases, this is used instead.
// most hondas: 0x17C bit 53
// accord, crv: 0x1BE bit 4
- #define IS_USER_BRAKE_MSG(addr) (!honda_alt_brake_msg ? ((addr) == 0x17C) : ((addr) == 0x1BE))
- #define USER_BRAKE_VALUE(to_push) (!honda_alt_brake_msg ? ((to_push)->RDHR & 0x200000) : ((to_push)->RDLR & 0x10))
- // exit controls on rising edge of brake press or on brake press when
- // speed > 0
- bool is_user_brake_msg = IS_USER_BRAKE_MSG(addr); // needed to enforce type
+ // exit controls on rising edge of brake press or on brake press when speed > 0
+ bool is_user_brake_msg = honda_alt_brake_msg ? ((addr) == 0x1BE) : ((addr) == 0x17C);
if (is_user_brake_msg) {
- int brake = USER_BRAKE_VALUE(to_push);
- if (brake && (!(honda_brake_prev) || honda_ego_speed)) {
+ int brake = honda_alt_brake_msg ? (GET_BYTE((to_push), 0) & 0x10) : (GET_BYTE((to_push), 6) & 0x20);
+ if (brake && (!(honda_brake_prev) || honda_moving)) {
controls_allowed = 0;
}
honda_brake_prev = brake;
@@ -65,7 +62,7 @@ static void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// length check because bosch hardware also uses this id (0x201 w/ len = 8)
if ((addr == 0x201) && (len == 6)) {
gas_interceptor_detected = 1;
- int gas_interceptor = ((to_push->RDLR & 0xFF) << 8) | ((to_push->RDLR & 0xFF00) >> 8);
+ int gas_interceptor = GET_INTERCEPTOR(to_push);
if ((gas_interceptor > HONDA_GAS_INTERCEPTOR_THRESHOLD) &&
(gas_interceptor_prev <= HONDA_GAS_INTERCEPTOR_THRESHOLD) &&
long_controls_allowed) {
@@ -77,7 +74,7 @@ static void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// exit controls on rising edge of gas press if no interceptor
if (!gas_interceptor_detected) {
if (addr == 0x17C) {
- int gas = to_push->RDLR & 0xFF;
+ int gas = GET_BYTE(to_push, 0);
if (gas && !(honda_gas_prev) && long_controls_allowed) {
controls_allowed = 0;
}
@@ -101,17 +98,18 @@ static int honda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
// disallow actuator commands if gas or brake (with vehicle moving) are pressed
// and the the latching controls_allowed flag is True
int pedal_pressed = honda_gas_prev || (gas_interceptor_prev > HONDA_GAS_INTERCEPTOR_THRESHOLD) ||
- (honda_brake_prev && honda_ego_speed);
+ (honda_brake_prev && honda_moving);
bool current_controls_allowed = controls_allowed && !(pedal_pressed);
// BRAKE: safety check
if (addr == 0x1FA) {
+ int brake = (GET_BYTE(to_send, 0) << 2) + (GET_BYTE(to_send, 1) & 0x3);
if (!current_controls_allowed || !long_controls_allowed) {
- if ((to_send->RDLR & 0xFFFF0000) != to_send->RDLR) {
+ if (brake != 0) {
tx = 0;
}
}
- if ((to_send->RDLR & 0xFFFFFF3F) != to_send->RDLR) {
+ if (brake > 255) {
tx = 0;
}
}
@@ -119,7 +117,8 @@ static int honda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
// STEER: safety check
if ((addr == 0xE4) || (addr == 0x194)) {
if (!current_controls_allowed) {
- if ((to_send->RDLR & 0xFFFF0000) != to_send->RDLR) {
+ bool steer_applied = GET_BYTE(to_send, 0) | GET_BYTE(to_send, 1);
+ if (steer_applied) {
tx = 0;
}
}
@@ -128,7 +127,7 @@ static int honda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
// GAS: safety check
if (addr == 0x200) {
if (!current_controls_allowed || !long_controls_allowed) {
- if ((to_send->RDLR & 0xFFFF0000) != to_send->RDLR) {
+ if (GET_BYTE(to_send, 0) || GET_BYTE(to_send, 1)) {
tx = 0;
}
}
@@ -137,9 +136,10 @@ static int honda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
// FORCE CANCEL: safety check only relevant when spamming the cancel button in Bosch HW
// ensuring that only the cancel button press is sent (VAL 2) when controls are off.
// This avoids unintended engagements while still allowing resume spam
+ int bus_pt = ((hw_type == HW_TYPE_BLACK_PANDA) && honda_bosch_hardware)? 1 : 0;
if ((addr == 0x296) && honda_bosch_hardware &&
- !current_controls_allowed && (bus == 0)) {
- if (((to_send->RDLR >> 5) & 0x7) != 2) {
+ !current_controls_allowed && (bus == bus_pt)) {
+ if (((GET_BYTE(to_send, 0) >> 5) & 0x7) != 2) {
tx = 0;
}
}
@@ -187,15 +187,17 @@ static int honda_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
static int honda_bosch_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
int bus_fwd = -1;
+ int bus_rdr_cam = (hw_type == HW_TYPE_BLACK_PANDA) ? 2 : 1; // radar bus, camera side
+ int bus_rdr_car = (hw_type == HW_TYPE_BLACK_PANDA) ? 0 : 2; // radar bus, car side
- if (bus_num == 2) {
- bus_fwd = 1;
+ if (bus_num == bus_rdr_car) {
+ bus_fwd = bus_rdr_cam;
}
- if (bus_num == 1) {
+ if (bus_num == bus_rdr_cam) {
int addr = GET_ADDR(to_fwd);
int is_lkas_msg = (addr == 0xE4) || (addr == 0x33D);
if (!is_lkas_msg) {
- bus_fwd = 2;
+ bus_fwd = bus_rdr_car;
}
}
return bus_fwd;
diff --git a/panda/board/safety/safety_hyundai.h b/panda/board/safety/safety_hyundai.h
index c1b55359b..aed30621f 100644
--- a/panda/board/safety/safety_hyundai.h
+++ b/panda/board/safety/safety_hyundai.h
@@ -20,7 +20,7 @@ static void hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
int addr = GET_ADDR(to_push);
if (addr == 897) {
- int torque_driver_new = ((to_push->RDLR >> 11) & 0xfff) - 2048;
+ int torque_driver_new = ((GET_BYTES_04(to_push) >> 11) & 0xfff) - 2048;
// update array of samples
update_sample(&hyundai_torque_driver, torque_driver_new);
}
@@ -39,7 +39,7 @@ static void hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// enter controls on rising edge of ACC, exit controls on ACC off
if (addr == 1057) {
// 2 bits: 13-14
- int cruise_engaged = (to_push->RDLR >> 13) & 0x3;
+ int cruise_engaged = (GET_BYTES_04(to_push) >> 13) & 0x3;
if (cruise_engaged && !hyundai_cruise_engaged_last) {
controls_allowed = 1;
}
@@ -67,7 +67,7 @@ static int hyundai_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
// LKA STEER: safety check
if (addr == 832) {
- int desired_torque = ((to_send->RDLR >> 16) & 0x7ff) - 1024;
+ int desired_torque = ((GET_BYTES_04(to_send) >> 16) & 0x7ff) - 1024;
uint32_t ts = TIM2->CNT;
bool violation = 0;
@@ -117,7 +117,7 @@ static int hyundai_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
// This avoids unintended engagements while still allowing resume spam
// TODO: fix bug preventing the button msg to be fwd'd on bus 2
//if ((addr == 1265) && !controls_allowed && (bus == 0) {
- // if ((to_send->RDLR & 0x7) != 4) {
+ // if ((GET_BYTES_04(to_send) & 0x7) != 4) {
// tx = 0;
// }
//}
diff --git a/panda/board/safety/safety_subaru.h b/panda/board/safety/safety_subaru.h
index c7a8c20e5..3eda8369b 100644
--- a/panda/board/safety/safety_subaru.h
+++ b/panda/board/safety/safety_subaru.h
@@ -20,7 +20,7 @@ static void subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
int addr = GET_ADDR(to_push);
if ((addr == 0x119) && (bus == 0)){
- int torque_driver_new = ((to_push->RDLR >> 16) & 0x7FF);
+ int torque_driver_new = ((GET_BYTES_04(to_push) >> 16) & 0x7FF);
torque_driver_new = to_signed(torque_driver_new, 11);
// update array of samples
update_sample(&subaru_torque_driver, torque_driver_new);
@@ -28,7 +28,7 @@ static void subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// enter controls on rising edge of ACC, exit controls on ACC off
if ((addr == 0x240) && (bus == 0)) {
- int cruise_engaged = (to_push->RDHR >> 9) & 1;
+ int cruise_engaged = GET_BYTE(to_push, 5) & 2;
if (cruise_engaged && !subaru_cruise_engaged_last) {
controls_allowed = 1;
}
@@ -45,7 +45,7 @@ static int subaru_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
// steer cmd checks
if (addr == 0x122) {
- int desired_torque = ((to_send->RDLR >> 16) & 0x1FFF);
+ int desired_torque = ((GET_BYTES_04(to_send) >> 16) & 0x1FFF);
bool violation = 0;
uint32_t ts = TIM2->CNT;
desired_torque = to_signed(desired_torque, 13);
diff --git a/panda/board/safety/safety_tesla.h b/panda/board/safety/safety_tesla.h
index b58e6b2bb..188b12ac4 100644
--- a/panda/board/safety/safety_tesla.h
+++ b/panda/board/safety/safety_tesla.h
@@ -55,7 +55,7 @@ static void tesla_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
if (addr == 0x45) {
// 6 bits starting at position 0
- int lever_position = (to_push->RDLR & 0x3F);
+ int lever_position = GET_BYTE(to_push, 0) & 0x3F;
if (lever_position == 2) { // pull forward
// activate openpilot
controls_allowed = 1;
@@ -69,7 +69,7 @@ static void tesla_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// Detect drive rail on (ignition) (start recording)
if (addr == 0x348) {
// GTW_status
- int drive_rail_on = (to_push->RDLR & 0x0001);
+ int drive_rail_on = GET_BYTE(to_push, 0) & 0x1;
tesla_ignition_started = drive_rail_on == 1;
}
@@ -77,12 +77,12 @@ static void tesla_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// DI_torque2::DI_brakePedal 0x118
if (addr == 0x118) {
// 1 bit at position 16
- if ((((to_push->RDLR & 0x8000)) >> 15) == 1) {
+ if ((GET_BYTE(to_push, 1) & 0x80) != 0) {
// disable break cancel by commenting line below
controls_allowed = 0;
}
//get vehicle speed in m/s. Tesla gives MPH
- tesla_speed = ((((((to_push->RDLR >> 24) & 0xF) << 8) + ((to_push->RDLR >> 16) & 0xFF)) * 0.05) - 25) * 1.609 / 3.6;
+ tesla_speed = (((((GET_BYTE(to_push, 3) & 0xF) << 8) + GET_BYTE(to_push, 2)) * 0.05) - 25) * 1.609 / 3.6;
if (tesla_speed < 0) {
tesla_speed = 0;
}
@@ -92,7 +92,7 @@ static void tesla_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// EPAS_sysStatus::EPAS_eacStatus 0x370
if (addr == 0x370) {
// if EPAS_eacStatus is not 1 or 2, disable control
- eac_status = ((to_push->RDHR >> 21)) & 0x7;
+ eac_status = (GET_BYTE(to_push, 6) >> 5) & 0x7;
// For human steering override we must not disable controls when eac_status == 0
// Additional safety: we could only allow eac_status == 0 when we have human steering allowed
if (controls_allowed && (eac_status != 0) && (eac_status != 1) && (eac_status != 2)) {
@@ -102,7 +102,7 @@ static void tesla_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
}
//get latest steering wheel angle
if (addr == 0x00E) {
- float angle_meas_now = (int)(((((to_push->RDLR & 0x3F) << 8) + ((to_push->RDLR >> 8) & 0xFF)) * 0.1) - 819.2);
+ float angle_meas_now = (int)(((((GET_BYTE(to_push, 0) & 0x3F) << 8) + GET_BYTE(to_push, 1)) * 0.1) - 819.2);
uint32_t ts = TIM2->CNT;
uint32_t ts_elapsed = get_ts_elapsed(ts, tesla_ts_angle_last);
@@ -146,10 +146,10 @@ static int tesla_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
// do not transmit CAN message if steering angle too high
// DAS_steeringControl::DAS_steeringAngleRequest
if (addr == 0x488) {
- float angle_raw = ((to_send->RDLR & 0x7F) << 8) + ((to_send->RDLR & 0xFF00) >> 8);
+ float angle_raw = ((GET_BYTE(to_send, 0) & 0x7F) << 8) + GET_BYTE(to_send, 1);
float desired_angle = (angle_raw * 0.1) - 1638.35;
bool violation = 0;
- int st_enabled = (to_send->RDLR & 0x400000) >> 22;
+ int st_enabled = GET_BYTE(to_send, 2) & 0x40;
if (st_enabled == 0) {
//steering is not enabled, do not check angles and do send
@@ -204,10 +204,10 @@ static int tesla_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
bus_fwd = 2; // Custom EPAS bus
}
if (addr == 0x101) {
- to_fwd->RDLR = to_fwd->RDLR | 0x4000; // 0x4000: WITH_ANGLE, 0xC000: WITH_BOTH (angle and torque)
- uint32_t checksum = (((to_fwd->RDLR & 0xFF00) >> 8) + (to_fwd->RDLR & 0xFF) + 2) & 0xFF;
- to_fwd->RDLR = to_fwd->RDLR & 0xFFFF;
- to_fwd->RDLR = to_fwd->RDLR + (checksum << 16);
+ to_fwd->RDLR = GET_BYTES_04(to_fwd) | 0x4000; // 0x4000: WITH_ANGLE, 0xC000: WITH_BOTH (angle and torque)
+ uint32_t checksum = (GET_BYTE(to_fwd, 1) + GET_BYTE(to_fwd, 0) + 2) & 0xFF;
+ to_fwd->RDLR = GET_BYTES_04(to_fwd) & 0xFFFF;
+ to_fwd->RDLR = GET_BYTES_04(to_fwd) + (checksum << 16);
}
}
if (bus_num == 2) {
diff --git a/panda/board/safety/safety_toyota.h b/panda/board/safety/safety_toyota.h
index c4d579563..c1ce99605 100644
--- a/panda/board/safety/safety_toyota.h
+++ b/panda/board/safety/safety_toyota.h
@@ -39,7 +39,7 @@ static void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// get eps motor torque (0.66 factor in dbc)
if (addr == 0x260) {
- int torque_meas_new = (((to_push->RDHR) & 0xFF00) | ((to_push->RDHR >> 16) & 0xFF));
+ int torque_meas_new = (GET_BYTE(to_push, 5) << 8) | GET_BYTE(to_push, 6);
torque_meas_new = to_signed(torque_meas_new, 16);
// scale by dbc_factor
@@ -56,7 +56,7 @@ static void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// enter controls on rising edge of ACC, exit controls on ACC off
if (addr == 0x1D2) {
// 5th bit is CRUISE_ACTIVE
- int cruise_engaged = to_push->RDLR & 0x20;
+ int cruise_engaged = GET_BYTE(to_push, 0) & 0x20;
if (!cruise_engaged) {
controls_allowed = 0;
}
@@ -69,7 +69,7 @@ static void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// exit controls on rising edge of interceptor gas press
if (addr == 0x201) {
gas_interceptor_detected = 1;
- int gas_interceptor = ((to_push->RDLR & 0xFF) << 8) | ((to_push->RDLR & 0xFF00) >> 8);
+ int gas_interceptor = GET_INTERCEPTOR(to_push);
if ((gas_interceptor > TOYOTA_GAS_INTERCEPTOR_THRESHOLD) &&
(gas_interceptor_prev <= TOYOTA_GAS_INTERCEPTOR_THRESHOLD) &&
long_controls_allowed) {
@@ -80,7 +80,7 @@ static void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// exit controls on rising edge of gas press
if (addr == 0x2C1) {
- int gas = (to_push->RDHR >> 16) & 0xFF;
+ int gas = GET_BYTE(to_push, 6) & 0xFF;
if ((gas > 0) && (toyota_gas_prev == 0) && !gas_interceptor_detected && long_controls_allowed) {
controls_allowed = 0;
}
@@ -115,7 +115,7 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
// GAS PEDAL: safety check
if (addr == 0x200) {
if (!controls_allowed || !long_controls_allowed) {
- if ((to_send->RDLR & 0xFFFF0000) != to_send->RDLR) {
+ if (GET_BYTE(to_send, 0) || GET_BYTE(to_send, 1)) {
tx = 0;
}
}
@@ -123,7 +123,7 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
// ACCEL: safety check on byte 1-2
if (addr == 0x343) {
- int desired_accel = ((to_send->RDLR & 0xFF) << 8) | ((to_send->RDLR >> 8) & 0xFF);
+ int desired_accel = (GET_BYTE(to_send, 0) << 8) | GET_BYTE(to_send, 1);
desired_accel = to_signed(desired_accel, 16);
if (!controls_allowed || !long_controls_allowed) {
if (desired_accel != 0) {
@@ -138,7 +138,7 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
// STEER: safety check on bytes 2-3
if (addr == 0x2E4) {
- int desired_torque = (to_send->RDLR & 0xFF00) | ((to_send->RDLR >> 16) & 0xFF);
+ int desired_torque = (GET_BYTE(to_send, 1) << 8) | GET_BYTE(to_send, 2);
desired_torque = to_signed(desired_torque, 16);
bool violation = 0;
diff --git a/panda/board/safety/safety_toyota_ipas.h b/panda/board/safety/safety_toyota_ipas.h
index 99e6dae05..3e3a3b3a2 100644
--- a/panda/board/safety/safety_toyota_ipas.h
+++ b/panda/board/safety/safety_toyota_ipas.h
@@ -39,7 +39,7 @@ static void toyota_ipas_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
if (addr == 0x260) {
// get driver steering torque
- int16_t torque_driver_new = (((to_push->RDLR) & 0xFF00) | ((to_push->RDLR >> 16) & 0xFF));
+ int16_t torque_driver_new = (GET_BYTE(to_push, 1) << 8) | GET_BYTE(to_push, 2);
// update array of samples
update_sample(&torque_driver, torque_driver_new);
@@ -47,7 +47,7 @@ static void toyota_ipas_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// get steer angle
if (addr == 0x25) {
- int angle_meas_new = ((to_push->RDLR & 0xf) << 8) + ((to_push->RDLR & 0xff00) >> 8);
+ int angle_meas_new = ((GET_BYTE(to_push, 0) & 0xF) << 8) | GET_BYTE(to_push, 1);
uint32_t ts = TIM2->CNT;
angle_meas_new = to_signed(angle_meas_new, 12);
@@ -81,12 +81,12 @@ static void toyota_ipas_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// get speed
if (addr == 0xb4) {
- speed = ((float) (((to_push->RDHR) & 0xFF00) | ((to_push->RDHR >> 16) & 0xFF))) * 0.01 / 3.6;
+ speed = ((float)((GET_BYTE(to_push, 5) << 8) | GET_BYTE(to_push, 6))) * 0.01 / 3.6;
}
// get ipas state
if (addr == 0x262) {
- ipas_state = (to_push->RDLR & 0xf);
+ ipas_state = GET_BYTE(to_push, 0) & 0xf;
}
// exit controls on high steering override
@@ -111,8 +111,8 @@ static int toyota_ipas_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
if ((addr == 0x266) || (addr == 0x167)) {
angle_control = 1; // we are in angle control mode
- int desired_angle = ((to_send->RDLR & 0xf) << 8) + ((to_send->RDLR & 0xff00) >> 8);
- int ipas_state_cmd = ((to_send->RDLR & 0xff) >> 4);
+ int desired_angle = ((GET_BYTE(to_send, 0) & 0xF) << 8) | GET_BYTE(to_send, 1);
+ int ipas_state_cmd = GET_BYTE(to_send, 0) >> 4;
bool violation = 0;
desired_angle = to_signed(desired_angle, 12);
diff --git a/panda/board/safety_declarations.h b/panda/board/safety_declarations.h
index 21e863869..7e0a54d73 100644
--- a/panda/board/safety_declarations.h
+++ b/panda/board/safety_declarations.h
@@ -1,7 +1,3 @@
-#define GET_BUS(msg) (((msg)->RDTR >> 4) & 0xFF)
-#define GET_LEN(msg) ((msg)->RDTR & 0xf)
-#define GET_ADDR(msg) ((((msg)->RIR & 4) != 0) ? ((msg)->RIR >> 3) : ((msg)->RIR >> 21))
-
// sample struct that keeps 3 samples in memory
struct sample_t {
int values[6];
@@ -54,3 +50,6 @@ int gas_interceptor_prev = 0;
// This is set by USB command 0xdf
bool long_controls_allowed = 1;
+
+// avg between 2 tracks
+#define GET_INTERCEPTOR(msg) (((GET_BYTE((msg), 0) << 8) + GET_BYTE((msg), 1) + ((GET_BYTE((msg), 2) << 8) + GET_BYTE((msg), 3)) / 2 ) / 2)
diff --git a/panda/board/spi_flasher.h b/panda/board/spi_flasher.h
index 94d41c7e3..4eab35671 100644
--- a/panda/board/spi_flasher.h
+++ b/panda/board/spi_flasher.h
@@ -31,7 +31,7 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired)
FLASH->KEYR = 0xCDEF89AB;
resp[1] = 0xff;
}
- set_led(LED_GREEN, 1);
+ current_board->set_led(LED_GREEN, 1);
unlocked = 1;
prog_ptr = (uint32_t *)0x8004000;
break;
@@ -92,17 +92,27 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired)
return resp_len;
}
-int usb_cb_ep1_in(uint8_t *usbdata, int len, bool hardwired) { return 0; }
-void usb_cb_ep3_out(uint8_t *usbdata, int len, bool hardwired) { }
+int usb_cb_ep1_in(void *usbdata, int len, bool hardwired) {
+ UNUSED(usbdata);
+ UNUSED(len);
+ UNUSED(hardwired);
+ return 0;
+}
+void usb_cb_ep3_out(void *usbdata, int len, bool hardwired) {
+ UNUSED(usbdata);
+ UNUSED(len);
+ UNUSED(hardwired);
+}
int is_enumerated = 0;
-void usb_cb_enumeration_complete() {
+void usb_cb_enumeration_complete(void) {
puts("USB enumeration complete\n");
is_enumerated = 1;
}
-void usb_cb_ep2_out(uint8_t *usbdata, int len, bool hardwired) {
- set_led(LED_RED, 0);
+void usb_cb_ep2_out(void *usbdata, int len, bool hardwired) {
+ UNUSED(hardwired);
+ current_board->set_led(LED_RED, 0);
for (int i = 0; i < len/4; i++) {
// program byte 1
FLASH->CR = FLASH_CR_PSIZE_1 | FLASH_CR_PG;
@@ -113,11 +123,12 @@ void usb_cb_ep2_out(uint8_t *usbdata, int len, bool hardwired) {
//*(uint64_t*)(&spi_tx_buf[0x30+(i*4)]) = *prog_ptr;
prog_ptr++;
}
- set_led(LED_RED, 1);
+ current_board->set_led(LED_RED, 1);
}
int spi_cb_rx(uint8_t *data, int len, uint8_t *data_out) {
+ UNUSED(len);
int resp_len = 0;
switch (data[0]) {
case 0:
@@ -140,7 +151,7 @@ int spi_cb_rx(uint8_t *data, int len, uint8_t *data_out) {
#define CAN_BL_INPUT 0x1
#define CAN_BL_OUTPUT 0x2
-void CAN1_TX_IRQHandler() {
+void CAN1_TX_IRQHandler(void) {
// clear interrupt
CAN->TSR |= CAN_TSR_RQCP0;
}
@@ -167,12 +178,13 @@ void bl_can_send(uint8_t *odat) {
CAN->sTxMailBox[0].TIR = (CAN_BL_OUTPUT << 21) | 1;
}
-void CAN1_RX0_IRQHandler() {
+void CAN1_RX0_IRQHandler(void) {
while (CAN->RF0R & CAN_RF0R_FMP0) {
if ((CAN->sFIFOMailBox[0].RIR>>21) == CAN_BL_INPUT) {
uint8_t dat[8];
- ((uint32_t*)dat)[0] = CAN->sFIFOMailBox[0].RDLR;
- ((uint32_t*)dat)[1] = CAN->sFIFOMailBox[0].RDHR;
+ for (int i = 0; i < 8; i++) {
+ dat[0] = GET_BYTE(&CAN->sFIFOMailBox[0], i);
+ }
uint8_t odat[8];
uint8_t type = dat[0] & 0xF0;
if (type == 0x30) {
@@ -241,13 +253,13 @@ void CAN1_RX0_IRQHandler() {
}
}
-void CAN1_SCE_IRQHandler() {
+void CAN1_SCE_IRQHandler(void) {
llcan_clear_send(CAN);
}
#endif
-void soft_flasher_start() {
+void soft_flasher_start(void) {
puts("\n\n\n************************ FLASHER START ************************\n");
enter_bootloader_mode = 0;
@@ -264,7 +276,7 @@ void soft_flasher_start() {
// B8,B9: CAN 1
set_gpio_alternate(GPIOB, 8, GPIO_AF9_CAN1);
set_gpio_alternate(GPIOB, 9, GPIO_AF9_CAN1);
- set_can_enable(CAN1, 1);
+ current_board->enable_can_transciever(1, true);
// init can
llcan_set_speed(CAN1, 5000, false, false);
@@ -293,7 +305,7 @@ void soft_flasher_start() {
usb_init();
// green LED on for flashing
- set_led(LED_GREEN, 1);
+ current_board->set_led(LED_GREEN, 1);
__enable_irq();
@@ -304,13 +316,13 @@ void soft_flasher_start() {
// if you are connected through a hub to the phone
// you need power to be able to see the device
puts("USBP: didn't enumerate, switching to CDP mode\n");
- set_usb_power_mode(USB_POWER_CDP);
- set_led(LED_BLUE, 1);
+ current_board->set_usb_power_mode(USB_POWER_CDP);
+ current_board->set_led(LED_BLUE, 1);
}
// blink the green LED fast
- set_led(LED_GREEN, 0);
+ current_board->set_led(LED_GREEN, 0);
delay(500000);
- set_led(LED_GREEN, 1);
+ current_board->set_led(LED_GREEN, 1);
delay(500000);
}
}
diff --git a/panda/board/startup_stm32f205xx.s b/panda/board/startup_stm32f205xx.s
index f4b6c6cb7..7554efc4c 100644
--- a/panda/board/startup_stm32f205xx.s
+++ b/panda/board/startup_stm32f205xx.s
@@ -4,7 +4,7 @@
* @author MCD Application Team
* @version V2.1.2
* @date 29-June-2016
- * @brief STM32F205xx Devices vector table for Atollic TrueSTUDIO toolchain.
+ * @brief STM32F205xx Devices vector table for Atollic TrueSTUDIO toolchain.
* This module performs:
* - Set the initial SP
* - Set the initial PC == Reset_Handler,
@@ -42,7 +42,7 @@
*
******************************************************************************
*/
-
+
.syntax unified
.cpu cortex-m3
.thumb
@@ -50,10 +50,10 @@
.global g_pfnVectors
.global Default_Handler
-/* start address for the initialization values of the .data section.
+/* 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 */
+/* start address for the .data section. defined in linker script */
.word _sdata
/* end address for the .data section. defined in linker script */
.word _edata
@@ -67,7 +67,7 @@ defined in linker script */
* @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.
+ * supplied main() routine is called.
* @param None
* @retval : None
*/
@@ -75,7 +75,7 @@ defined in linker script */
.section .text.Reset_Handler
.weak Reset_Handler
.type Reset_Handler, %function
-Reset_Handler:
+Reset_Handler:
ldr sp, =_estack /* set stack pointer */
bl __initialize_hardware_early
@@ -88,7 +88,7 @@ CopyDataInit:
ldr r3, [r3, r1]
str r3, [r0, r1]
adds r1, r1, #4
-
+
LoopCopyDataInit:
ldr r0, =_sdata
ldr r3, =_edata
@@ -101,7 +101,7 @@ LoopCopyDataInit:
FillZerobss:
movs r3, #0
str r3, [r2], #4
-
+
LoopFillZerobss:
ldr r3, = _ebss
cmp r2, r3
@@ -113,15 +113,15 @@ LoopFillZerobss:
/*bl __libc_init_array*/
/* Call the application's entry point.*/
bl main
- bx lr
+ bx lr
.size Reset_Handler, .-Reset_Handler
/**
- * @brief This is the code that gets called when the processor receives an
+ * @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
+ * @param None
+ * @retval None
*/
.section .text.Default_Handler,"ax",%progbits
Default_Handler:
@@ -133,14 +133,14 @@ Infinite_Loop:
* 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
@@ -159,7 +159,7 @@ g_pfnVectors:
.word 0
.word PendSV_Handler
.word SysTick_Handler
-
+
/* External Interrupts */
.word WWDG_IRQHandler /* Window WatchDog */
.word PVD_IRQHandler /* PVD through EXTI Line detection */
@@ -248,7 +248,7 @@ g_pfnVectors:
* 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
@@ -302,7 +302,7 @@ g_pfnVectors:
.thumb_set EXTI1_IRQHandler,Default_Handler
.weak EXTI2_IRQHandler
- .thumb_set EXTI2_IRQHandler,Default_Handler
+ .thumb_set EXTI2_IRQHandler,Default_Handler
.weak EXTI3_IRQHandler
.thumb_set EXTI3_IRQHandler,Default_Handler
@@ -320,7 +320,7 @@ g_pfnVectors:
.thumb_set DMA1_Stream2_IRQHandler,Default_Handler
.weak DMA1_Stream3_IRQHandler
- .thumb_set DMA1_Stream3_IRQHandler,Default_Handler
+ .thumb_set DMA1_Stream3_IRQHandler,Default_Handler
.weak DMA1_Stream4_IRQHandler
.thumb_set DMA1_Stream4_IRQHandler,Default_Handler
@@ -432,7 +432,7 @@ g_pfnVectors:
.weak SPI3_IRQHandler
.thumb_set SPI3_IRQHandler,Default_Handler
-
+
.weak UART4_IRQHandler
.thumb_set UART4_IRQHandler,Default_Handler
diff --git a/panda/board/startup_stm32f413xx.s b/panda/board/startup_stm32f413xx.s
index 00b645d11..6e6fb5ffa 100644
--- a/panda/board/startup_stm32f413xx.s
+++ b/panda/board/startup_stm32f413xx.s
@@ -4,7 +4,7 @@
* @author MCD Application Team
* @version V2.6.0
* @date 04-November-2016
- * @brief STM32F413xx Devices vector table for GCC based toolchains.
+ * @brief STM32F413xx Devices vector table for GCC based toolchains.
* This module performs:
* - Set the initial SP
* - Set the initial PC == Reset_Handler,
@@ -42,7 +42,7 @@
*
******************************************************************************
*/
-
+
.syntax unified
.cpu cortex-m4
.fpu softvfp
@@ -51,7 +51,7 @@
.global g_pfnVectors
.global Default_Handler
-/* start address for the initialization values of the .data section.
+/* 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 */
@@ -68,7 +68,7 @@ defined in linker script */
* @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.
+ * supplied main() routine is called.
* @param None
* @retval : None
*/
@@ -76,7 +76,7 @@ defined in linker script */
.section .text.Reset_Handler
.weak Reset_Handler
.type Reset_Handler, %function
-Reset_Handler:
+Reset_Handler:
ldr sp, =_estack /* set stack pointer */
bl __initialize_hardware_early
@@ -89,7 +89,7 @@ CopyDataInit:
ldr r3, [r3, r1]
str r3, [r0, r1]
adds r1, r1, #4
-
+
LoopCopyDataInit:
ldr r0, =_sdata
ldr r3, =_edata
@@ -102,7 +102,7 @@ LoopCopyDataInit:
FillZerobss:
movs r3, #0
str r3, [r2], #4
-
+
LoopFillZerobss:
ldr r3, = _ebss
cmp r2, r3
@@ -114,15 +114,15 @@ LoopFillZerobss:
/* bl __libc_init_array */
/* Call the application's entry point.*/
bl main
- bx lr
+ bx lr
.size Reset_Handler, .-Reset_Handler
/**
- * @brief This is the code that gets called when the processor receives an
+ * @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
+ * @param None
+ * @retval None
*/
.section .text.Default_Handler,"ax",%progbits
Default_Handler:
@@ -134,12 +134,12 @@ Infinite_Loop:
* 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
@@ -261,11 +261,11 @@ g_pfnVectors:
.word DFSDM2_FLT1_IRQHandler /* DFSDM2 Filter1 */
.word DFSDM2_FLT2_IRQHandler /* DFSDM2 Filter2 */
.word DFSDM2_FLT3_IRQHandler /* DFSDM2 Filter3 */
-
+
/*******************************************************************************
*
-* Provide weak aliases for each Exception handler to the Default_Handler.
-* As they are weak aliases, any function with the same name will override
+* 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.
*
*******************************************************************************/
@@ -277,7 +277,7 @@ g_pfnVectors:
.weak MemManage_Handler
.thumb_set MemManage_Handler,Default_Handler
-
+
.weak BusFault_Handler
.thumb_set BusFault_Handler,Default_Handler
@@ -321,7 +321,7 @@ g_pfnVectors:
.thumb_set EXTI1_IRQHandler,Default_Handler
.weak EXTI2_IRQHandler
- .thumb_set EXTI2_IRQHandler,Default_Handler
+ .thumb_set EXTI2_IRQHandler,Default_Handler
.weak EXTI3_IRQHandler
.thumb_set EXTI3_IRQHandler,Default_Handler
@@ -439,9 +439,9 @@ g_pfnVectors:
.weak DMA1_Stream7_IRQHandler
.thumb_set DMA1_Stream7_IRQHandler,Default_Handler
-
+
.weak FSMC_IRQHandler
- .thumb_set FSMC_IRQHandler,Default_Handler
+ .thumb_set FSMC_IRQHandler,Default_Handler
.weak SDIO_IRQHandler
.thumb_set SDIO_IRQHandler,Default_Handler
@@ -451,12 +451,12 @@ g_pfnVectors:
.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
+ .thumb_set UART5_IRQHandler,Default_Handler
.weak TIM6_DAC_IRQHandler
.thumb_set TIM6_DAC_IRQHandler,Default_Handler
@@ -517,7 +517,7 @@ g_pfnVectors:
.weak I2C3_ER_IRQHandler
.thumb_set I2C3_ER_IRQHandler,Default_Handler
-
+
.weak CAN3_TX_IRQHandler
.thumb_set CAN3_TX_IRQHandler,Default_Handler
@@ -528,26 +528,26 @@ g_pfnVectors:
.thumb_set CAN3_RX1_IRQHandler,Default_Handler
.weak CAN3_SCE_IRQHandler
- .thumb_set CAN3_SCE_IRQHandler,Default_Handler
+ .thumb_set CAN3_SCE_IRQHandler,Default_Handler
.weak RNG_IRQHandler
.thumb_set RNG_IRQHandler,Default_Handler
.weak FPU_IRQHandler
.thumb_set FPU_IRQHandler,Default_Handler
-
+
.weak UART7_IRQHandler
.thumb_set UART7_IRQHandler,Default_Handler
.weak UART8_IRQHandler
- .thumb_set UART8_IRQHandler,Default_Handler
+ .thumb_set UART8_IRQHandler,Default_Handler
.weak SPI4_IRQHandler
.thumb_set SPI4_IRQHandler,Default_Handler
.weak SPI5_IRQHandler
.thumb_set SPI5_IRQHandler,Default_Handler
-
+
.weak SAI1_IRQHandler
.thumb_set SAI1_IRQHandler,Default_Handler
@@ -555,7 +555,7 @@ g_pfnVectors:
.thumb_set UART9_IRQHandler,Default_Handler
.weak UART10_IRQHandler
- .thumb_set UART10_IRQHandler,Default_Handler
+ .thumb_set UART10_IRQHandler,Default_Handler
.weak QUADSPI_IRQHandler
.thumb_set QUADSPI_IRQHandler,Default_Handler
@@ -565,7 +565,7 @@ g_pfnVectors:
.weak FMPI2C1_ER_IRQHandler
.thumb_set FMPI2C1_ER_IRQHandler,Default_Handler
-
+
.weak LPTIM1_IRQHandler
.thumb_set LPTIM1_IRQHandler,Default_Handler
diff --git a/panda/boardesp/webserver.c b/panda/boardesp/webserver.c
index f855f88c9..b1a514626 100644
--- a/panda/boardesp/webserver.c
+++ b/panda/boardesp/webserver.c
@@ -28,8 +28,7 @@ char pageheader[] = "HTTP/1.0 200 OK\nContent-Type: text/html\n\n"
"\n"
"\n"
"
This is your comma.ai panda\n\n"
-"It's open source. Find the code here\n"
-"Designed to work with our dashcam, chffr\n";
+"It's open source. Find the code here\n";
char pagefooter[] = "
\n"
"\n"
@@ -84,7 +83,7 @@ int ICACHE_FLASH_ATTR usb_cmd(int ep, int len, int request,
return recv[0];
}
-
+
void ICACHE_FLASH_ATTR st_flash() {
if (st_firmware != NULL) {
@@ -212,14 +211,14 @@ static void ICACHE_FLASH_ATTR web_rx_cb(void *arg, char *data, uint16_t len) {
} else {
ets_strcat(resp, "\nin INSECURE mode...secure it");
}
-
+
ets_strcat(resp,"\nSet USB Mode:"
""
""
"\n");
ets_strcat(resp, pagefooter);
-
+
espconn_send_string(&web_conn, resp);
espconn_disconnect(conn);
} else if (memcmp(data, "GET /secure", 11) == 0 && !wifi_secure_mode) {
@@ -235,7 +234,7 @@ static void ICACHE_FLASH_ATTR web_rx_cb(void *arg, char *data, uint16_t len) {
os_sprintf(resp, "%sUSB Mode set to %02x\n\n", OK_header, mode_value);
espconn_send_string(&web_conn, resp);
espconn_disconnect(conn);
- }
+ }
} else if (memcmp(data, "PUT /stupdate ", 14) == 0 && wifi_secure_mode) {
os_printf("init st firmware\n");
char *cl = strstr(data, "Content-Length: ");
@@ -251,7 +250,7 @@ static void ICACHE_FLASH_ATTR web_rx_cb(void *arg, char *data, uint16_t len) {
memset(st_firmware, 0, real_content_length);
state = RECEIVING_ST_FIRMWARE;
}
-
+
} else if (((memcmp(data, "PUT /espupdate1 ", 16) == 0) ||
(memcmp(data, "PUT /espupdate2 ", 16) == 0)) && wifi_secure_mode) {
// 0x1000 = user1.bin
diff --git a/panda/crypto/sha.c b/panda/crypto/sha.c
index 8e1715525..a13162c5f 100644
--- a/panda/crypto/sha.c
+++ b/panda/crypto/sha.c
@@ -130,7 +130,7 @@ const uint8_t* SHA_final(SHA_CTX* ctx) {
/* Hack - right shift operator with non const argument requires
* libgcc.a which is missing in EON
- * thus expanding for loop from
+ * thus expanding for loop from
for (i = 0; i < 8; ++i) {
uint8_t tmp = (uint8_t) (cnt >> ((7 - i) * 8));
diff --git a/panda/drivers/windows/pandaJ2534DLL/PandaJ2534Device.cpp b/panda/drivers/windows/pandaJ2534DLL/PandaJ2534Device.cpp
index 4cda1fa2e..19ae43b0d 100644
--- a/panda/drivers/windows/pandaJ2534DLL/PandaJ2534Device.cpp
+++ b/panda/drivers/windows/pandaJ2534DLL/PandaJ2534Device.cpp
@@ -93,7 +93,7 @@ DWORD PandaJ2534Device::can_process_thread() {
if (count == 0) {
continue;
}
-
+
for (int i = 0; i < count; i++) {
auto msg_in = msg_recv[i];
J2534Frame msg_out(msg_in);
diff --git a/panda/drivers/windows/pandaJ2534DLL/resource.h b/panda/drivers/windows/pandaJ2534DLL/resource.h
index 771e7b80b..af0e13cc0 100644
--- a/panda/drivers/windows/pandaJ2534DLL/resource.h
+++ b/panda/drivers/windows/pandaJ2534DLL/resource.h
@@ -3,7 +3,7 @@
// Used by pandaJ2534DLL.rc
// Next default values for new objects
-//
+//
#ifdef APSTUDIO_INVOKED
#ifndef APSTUDIO_READONLY_SYMBOLS
#define _APS_NEXT_RESOURCE_VALUE 101
diff --git a/panda/examples/can_unique.md b/panda/examples/can_unique.md
index bf316940d..4d8ac460e 100644
--- a/panda/examples/can_unique.md
+++ b/panda/examples/can_unique.md
@@ -10,8 +10,8 @@ First record a few minutes of background CAN messages with all the doors closed
./can_logger.py
mv output.csv background.csv
```
-Then run can_logger.py for a few seconds while performing the action you're interested, such as opening and then closing the
-front-left door and save it as door-fl-1.csv
+Then run can_logger.py for a few seconds while performing the action you're interested, such as opening and then closing the
+front-left door and save it as door-fl-1.csv
Repeat the process and save it as door-f1-2.csv to have an easy way to confirm any suspicions.
Now we'll use can_unique.py to look for unique bits:
diff --git a/panda/examples/get_panda_password.py b/panda/examples/get_panda_password.py
index 11071d035..575cbb079 100644
--- a/panda/examples/get_panda_password.py
+++ b/panda/examples/get_panda_password.py
@@ -2,11 +2,11 @@
from panda import Panda
def get_panda_password():
-
+
try:
print("Trying to connect to Panda over USB...")
p = Panda()
-
+
except AssertionError:
print("USB connection failed")
sys.exit(0)
@@ -15,6 +15,6 @@ def get_panda_password():
#print('[%s]' % ', '.join(map(str, wifi)))
print("SSID: " + wifi[0])
print("Password: " + wifi[1])
-
+
if __name__ == "__main__":
get_panda_password()
\ No newline at end of file
diff --git a/panda/examples/tesla_tester.py b/panda/examples/tesla_tester.py
index 99d8d9285..4365e424b 100644
--- a/panda/examples/tesla_tester.py
+++ b/panda/examples/tesla_tester.py
@@ -4,14 +4,14 @@ import binascii
from panda import Panda
def tesla_tester():
-
+
try:
print("Trying to connect to Panda over USB...")
p = Panda()
-
+
except AssertionError:
print("USB connection failed. Trying WiFi...")
-
+
try:
p = Panda("WIFI")
except:
@@ -21,12 +21,12 @@ def tesla_tester():
body_bus_speed = 125 # Tesla Body busses (B, BF) are 125kbps, rest are 500kbps
body_bus_num = 1 # My TDC to OBD adapter has PT on bus0 BDY on bus1 and CH on bus2
p.set_can_speed_kbps(body_bus_num, body_bus_speed)
-
+
# Now set the panda from its default of SAFETY_NOOUTPUT (read only) to SAFETY_ALLOUTPUT
# Careful, as this will let us send any CAN messages we want (which could be very bad!)
print("Setting Panda to output mode...")
p.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
-
+
# BDY 0x248 is the MCU_commands message, which includes folding mirrors, opening the trunk, frunk, setting the cars lock state and more. For our test, we will edit the 3rd byte, which is MCU_lockRequest. 0x01 will lock, 0x02 will unlock:
print("Unlocking Tesla...")
p.can_send(0x248, "\x00\x00\x02\x00\x00\x00\x00\x00", body_bus_num)
@@ -34,13 +34,13 @@ def tesla_tester():
#Or, we can set the first byte, MCU_frontHoodCommand + MCU_liftgateSwitch, to 0x01 to pop the frunk, or 0x04 to open/close the trunk (0x05 should open both)
print("Opening Frunk...")
p.can_send(0x248, "\x01\x00\x00\x00\x00\x00\x00\x00", body_bus_num)
-
+
#Back to safety...
print("Disabling output on Panda...")
p.set_safety_mode(Panda.SAFETY_NOOUTPUT)
-
+
print("Reading VIN from 0x568. This is painfully slow and can take up to 3 minutes (1 minute per message; 3 messages needed for full VIN)...")
-
+
vin = {}
while True:
#Read the VIN
diff --git a/panda/python/__init__.py b/panda/python/__init__.py
index bfca642e8..e83a4a169 100644
--- a/panda/python/__init__.py
+++ b/panda/python/__init__.py
@@ -16,7 +16,7 @@ from update import ensure_st_up_to_date
from serial import PandaSerial
from isotp import isotp_send, isotp_recv
-__version__ = '0.0.8'
+__version__ = '0.0.9'
BASEDIR = os.path.join(os.path.dirname(os.path.realpath(__file__)), "../")
@@ -232,10 +232,10 @@ class Panda(object):
print("flash: unlocking")
handle.controlWrite(Panda.REQUEST_IN, 0xb1, 0, 0, b'')
- # erase sectors 1 and 2
+ # erase sectors 1 through 3
print("flash: erasing")
- handle.controlWrite(Panda.REQUEST_IN, 0xb2, 1, 0, b'')
- handle.controlWrite(Panda.REQUEST_IN, 0xb2, 2, 0, b'')
+ for i in range(1, 4):
+ handle.controlWrite(Panda.REQUEST_IN, 0xb2, i, 0, b'')
# flash over EP2
STEP = 0x10
@@ -334,13 +334,19 @@ class Panda(object):
# ******************* health *******************
def health(self):
- dat = self._handle.controlRead(Panda.REQUEST_IN, 0xd2, 0, 0, 13)
- a = struct.unpack("IIBBBBB", dat)
- return {"voltage": a[0], "current": a[1],
- "started": a[2], "controls_allowed": a[3],
- "gas_interceptor_detected": a[4],
- "started_signal_detected": a[5],
- "started_alt": a[6]}
+ dat = self._handle.controlRead(Panda.REQUEST_IN, 0xd2, 0, 0, 24)
+ a = struct.unpack("IIIIIBBBB", dat)
+ return {
+ "voltage": a[0],
+ "current": a[1],
+ "can_send_errs": a[2],
+ "can_fwd_errs": a[3],
+ "gmlan_send_errs": a[4],
+ "started": a[5],
+ "controls_allowed": a[6],
+ "gas_interceptor_detected": a[7],
+ "car_harness_status": a[8]
+ }
# ******************* control *******************
@@ -354,9 +360,14 @@ class Panda(object):
def get_version(self):
return self._handle.controlRead(Panda.REQUEST_IN, 0xd6, 0, 0, 0x40)
+ def get_type(self):
+ return self._handle.controlRead(Panda.REQUEST_IN, 0xc1, 0, 0, 0x40)
+
def is_grey(self):
- ret = self._handle.controlRead(Panda.REQUEST_IN, 0xc1, 0, 0, 0x40)
- return ret == "\x01"
+ return self.get_type() == "\x02"
+
+ def is_black(self):
+ return self.get_type() == "\x03"
def get_serial(self):
dat = self._handle.controlRead(Panda.REQUEST_IN, 0xd0, 0, 0, 0x20)
@@ -387,11 +398,16 @@ class Panda(object):
self._handle.controlWrite(Panda.REQUEST_OUT, 0xdd, from_bus, to_bus, b'')
def set_gmlan(self, bus=2):
+ # TODO: check panda type
if bus is None:
self._handle.controlWrite(Panda.REQUEST_OUT, 0xdb, 0, 0, b'')
elif bus in [Panda.GMLAN_CAN2, Panda.GMLAN_CAN3]:
self._handle.controlWrite(Panda.REQUEST_OUT, 0xdb, 1, bus, b'')
+ def set_obd(self, obd):
+ # TODO: check panda type
+ self._handle.controlWrite(Panda.REQUEST_OUT, 0xdb, int(obd), 0, b'')
+
def set_can_loopback(self, enable):
# set can loopback mode for all buses
self._handle.controlWrite(Panda.REQUEST_OUT, 0xe5, int(enable), 0, b'')
@@ -559,3 +575,6 @@ class Panda(object):
msg = self.kline_ll_recv(2, bus=bus)
msg += self.kline_ll_recv(ord(msg[1])-2, bus=bus)
return msg
+
+ def send_heartbeat(self):
+ self._handle.controlWrite(Panda.REQUEST_OUT, 0xf3, 0, 0, b'')
diff --git a/panda/python/esptool.py b/panda/python/esptool.py
index e68e6cd6e..970aa3d4d 100755
--- a/panda/python/esptool.py
+++ b/panda/python/esptool.py
@@ -1216,7 +1216,7 @@ def main():
operation_func = globals()[args.operation]
operation_args,_,_,_ = inspect.getargspec(operation_func)
if operation_args[0] == 'esp': # operation function takes an ESPROM connection object
- initial_baud = MIN(ESPROM.ESP_ROM_BAUD, args.baud) # don't sync faster than the default baud rate
+ initial_baud = min(ESPROM.ESP_ROM_BAUD, args.baud) # don't sync faster than the default baud rate
esp = ESPROM(args.port, initial_baud)
esp.connect()
operation_func(esp, args)
diff --git a/panda/python/flash_release.py b/panda/python/flash_release.py
index 51f6a72e7..0f407ff22 100755
--- a/panda/python/flash_release.py
+++ b/panda/python/flash_release.py
@@ -89,7 +89,7 @@ def flash_release(path=None, st_serial=None):
# done!
status("6. Success!")
-
+
if __name__ == "__main__":
flash_release(*sys.argv[1:])
diff --git a/panda/python/isotp.py b/panda/python/isotp.py
index d68aa4d70..971827007 100644
--- a/panda/python/isotp.py
+++ b/panda/python/isotp.py
@@ -29,7 +29,7 @@ def recv(panda, cnt, addr, nbus):
def isotp_recv_subaddr(panda, addr, bus, sendaddr, subaddr):
msg = recv(panda, 1, addr, bus)[0]
- # TODO: handle other subaddr also communicating
+ # TODO: handle other subaddr also communicating
assert ord(msg[0]) == subaddr
if ord(msg[1])&0xf0 == 0x10:
diff --git a/panda/run_automated_tests.sh b/panda/run_automated_tests.sh
index 4e07d329c..583d6c1ed 100755
--- a/panda/run_automated_tests.sh
+++ b/panda/run_automated_tests.sh
@@ -1,14 +1,21 @@
-#!/bin/bash
+#!/bin/bash -e
TEST_FILENAME=${TEST_FILENAME:-nosetests.xml}
-if [ ! -f "/EON" ]; then
+if [ -f "/EON" ]; then
TESTSUITE_NAME="Panda_Test-EON"
else
TESTSUITE_NAME="Panda_Test-DEV"
fi
-cd boardesp
-make flashall
-cd ..
+if [ ! -z "${SKIPWIFI}" ] || [ -f "/EON" ]; then
+ TEST_SCRIPTS=$(ls tests/automated/$1*.py | grep -v wifi)
+else
+ TEST_SCRIPTS=$(ls tests/automated/$1*.py)
+fi
+IFS=$'\n'
+for NAME in $(nmcli --fields NAME con show | grep panda | awk '{$1=$1};1')
+do
+ nmcli connection delete "$NAME"
+done
-PYTHONPATH="." python $(which nosetests) -v --with-xunit --xunit-file=./$TEST_FILENAME --xunit-testsuite-name=$TESTSUITE_NAME -s tests/automated/$1*.py
+PYTHONPATH="." python $(which nosetests) -v --with-xunit --xunit-file=./$TEST_FILENAME --xunit-testsuite-name=$TESTSUITE_NAME -s $TEST_SCRIPTS
diff --git a/panda/tests/automated/2_usb_to_can.py b/panda/tests/automated/2_usb_to_can.py
index 7860d3290..9e3e07aa4 100644
--- a/panda/tests/automated/2_usb_to_can.py
+++ b/panda/tests/automated/2_usb_to_can.py
@@ -26,6 +26,9 @@ def test_can_loopback(serial=None):
busses = [0,1,2]
for bus in busses:
+ # send heartbeat
+ p.send_heartbeat()
+
# set bus 0 speed to 250
p.set_can_speed_kbps(bus, 250)
@@ -52,6 +55,9 @@ def test_safety_nooutput(serial=None):
# enable output mode
p.set_safety_mode(Panda.SAFETY_NOOUTPUT)
+ # send heartbeat
+ p.send_heartbeat()
+
# enable CAN loopback mode
p.set_can_loopback(True)
@@ -76,11 +82,17 @@ def test_reliability(serial=None):
p.set_can_loopback(True)
p.set_can_speed_kbps(0, 1000)
+ # send heartbeat
+ p.send_heartbeat()
+
addrs = range(100, 100+MSG_COUNT)
ts = [(j, 0, "\xaa"*8, 0) for j in addrs]
# 100 loops
for i in range(LOOP_COUNT):
+ # send heartbeat
+ p.send_heartbeat()
+
st = time.time()
p.can_send_many(ts)
@@ -111,6 +123,9 @@ def test_throughput(serial=None):
# enable output mode
p.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
+ # send heartbeat
+ p.send_heartbeat()
+
# enable CAN loopback mode
p.set_can_loopback(True)
@@ -119,6 +134,9 @@ def test_throughput(serial=None):
p.set_can_speed_kbps(0, speed)
time.sleep(0.05)
+ # send heartbeat
+ p.send_heartbeat()
+
comp_kbps = time_many_sends(p, 0)
# bit count from https://en.wikipedia.org/wiki/CAN_bus
@@ -139,6 +157,9 @@ def test_gmlan(serial=None):
# enable output mode
p.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
+ # send heartbeat
+ p.send_heartbeat()
+
# enable CAN loopback mode
p.set_can_loopback(True)
@@ -148,6 +169,9 @@ def test_gmlan(serial=None):
# set gmlan on CAN2
for bus in [Panda.GMLAN_CAN2, Panda.GMLAN_CAN3, Panda.GMLAN_CAN2, Panda.GMLAN_CAN3]:
+ # send heartbeat
+ p.send_heartbeat()
+
p.set_gmlan(bus)
comp_kbps_gmlan = time_many_sends(p, 3)
assert_greater(comp_kbps_gmlan, 0.8 * SPEED_GMLAN)
@@ -171,11 +195,17 @@ def test_gmlan_bad_toggle(serial=None):
# enable output mode
p.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
+ # send heartbeat
+ p.send_heartbeat()
+
# enable CAN loopback mode
p.set_can_loopback(True)
# GMLAN_CAN2
for bus in [Panda.GMLAN_CAN2, Panda.GMLAN_CAN3]:
+ # send heartbeat
+ p.send_heartbeat()
+
p.set_gmlan(bus)
comp_kbps_gmlan = time_many_sends(p, 3)
assert_greater(comp_kbps_gmlan, 0.6 * SPEED_GMLAN)
@@ -183,6 +213,9 @@ def test_gmlan_bad_toggle(serial=None):
# normal
for bus in [Panda.GMLAN_CAN2, Panda.GMLAN_CAN3]:
+ # send heartbeat
+ p.send_heartbeat()
+
p.set_gmlan(None)
comp_kbps_normal = time_many_sends(p, bus)
assert_greater(comp_kbps_normal, 0.6 * SPEED_NORMAL)
diff --git a/panda/tests/automated/4_wifi_functionality.py b/panda/tests/automated/4_wifi_functionality.py
index 0cf42d1f3..ab9bed700 100644
--- a/panda/tests/automated/4_wifi_functionality.py
+++ b/panda/tests/automated/4_wifi_functionality.py
@@ -21,12 +21,18 @@ def test_throughput(serial=None):
# enable output mode
p.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
+ # send heartbeat
+ p.send_heartbeat()
+
# enable CAN loopback mode
p.set_can_loopback(True)
p = Panda("WIFI")
for speed in [100,250,500,750,1000]:
+ # send heartbeat
+ p.send_heartbeat()
+
# set bus 0 speed to speed
p.set_can_speed_kbps(0, speed)
time.sleep(0.1)
@@ -46,11 +52,18 @@ def test_recv_only(serial=None):
connect_wifi(serial)
p = Panda(serial)
p.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
+
+ # send heartbeat
+ p.send_heartbeat()
+
p.set_can_loopback(True)
pwifi = Panda("WIFI")
# TODO: msg_count=1000 drops packets, is this fixable?
for msg_count in [10,100,200]:
+ # send heartbeat
+ p.send_heartbeat()
+
speed = 500
p.set_can_speed_kbps(0, speed)
comp_kbps = time_many_sends(p, 0, pwifi, msg_count)
diff --git a/panda/tests/automated/5_wifi_udp.py b/panda/tests/automated/5_wifi_udp.py
index 873f78bdb..d55baa659 100644
--- a/panda/tests/automated/5_wifi_udp.py
+++ b/panda/tests/automated/5_wifi_udp.py
@@ -34,7 +34,7 @@ def test_udp_doesnt_drop(serial=None):
sys.stdout.flush()
else:
print("UDP WIFI loopback %d messages at speed %d, comp speed is %.2f, percent %.2f" % (msg_count, speed, comp_kbps, saturation_pct))
- assert_greater(saturation_pct, 15) #sometimes the wifi can be slow...
+ assert_greater(saturation_pct, 20) #sometimes the wifi can be slow...
assert_less(saturation_pct, 100)
saturation_pcts.append(saturation_pct)
if len(saturation_pcts) > 0:
diff --git a/panda/tests/automated/6_two_panda.py b/panda/tests/automated/6_two_panda.py
index 3c29a0e7a..09cf1861f 100644
--- a/panda/tests/automated/6_two_panda.py
+++ b/panda/tests/automated/6_two_panda.py
@@ -13,6 +13,9 @@ def test_send_recv(serial_sender=None, serial_reciever=None):
p_send.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
p_send.set_can_loopback(False)
+ # send heartbeat
+ p_send.send_heartbeat()
+
p_recv.set_can_loopback(False)
assert not p_send.legacy
@@ -27,6 +30,9 @@ def test_send_recv(serial_sender=None, serial_reciever=None):
for bus in busses:
for speed in [100, 250, 500, 750, 1000]:
+ # send heartbeat
+ p_send.send_heartbeat()
+
p_send.set_can_speed_kbps(bus, speed)
p_recv.set_can_speed_kbps(bus, speed)
time.sleep(0.05)
@@ -45,6 +51,10 @@ def test_latency(serial_sender=None, serial_reciever=None):
p_send = Panda(serial_sender)
p_recv = Panda(serial_reciever)
+ # send heartbeat
+ p_send.send_heartbeat()
+ p_recv.send_heartbeat()
+
p_send.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
p_send.set_can_loopback(False)
@@ -62,10 +72,18 @@ def test_latency(serial_sender=None, serial_reciever=None):
p_recv.can_recv()
p_send.can_recv()
+ # send heartbeat
+ p_send.send_heartbeat()
+ p_recv.send_heartbeat()
+
busses = [0,1,2]
for bus in busses:
for speed in [100, 250, 500, 750, 1000]:
+ # send heartbeat
+ p_send.send_heartbeat()
+ p_recv.send_heartbeat()
+
p_send.set_can_speed_kbps(bus, speed)
p_recv.set_can_speed_kbps(bus, speed)
time.sleep(0.1)
diff --git a/panda/tests/black_loopback_test.py b/panda/tests/black_loopback_test.py
new file mode 100755
index 000000000..8683561a4
--- /dev/null
+++ b/panda/tests/black_loopback_test.py
@@ -0,0 +1,139 @@
+#!/usr/bin/env python
+
+# Loopback test between black panda (+ harness and power) and white/grey panda
+# Tests all buses, including OBD CAN, which is on the same bus as CAN0 in this test.
+# To be sure, the test should be run with both harness orientations
+
+from __future__ import print_function
+import os
+import sys
+import time
+import random
+import argparse
+
+from hexdump import hexdump
+from itertools import permutations
+
+sys.path.append(os.path.join(os.path.dirname(os.path.realpath(__file__)), ".."))
+from panda import Panda
+
+def get_test_string():
+ return b"test"+os.urandom(10)
+
+def run_test(sleep_duration):
+ pandas = Panda.list()
+ print(pandas)
+
+ # make sure two pandas are connected
+ if len(pandas) != 2:
+ print("Connect white/grey and black panda to run this test!")
+ assert False
+
+ # connect
+ pandas[0] = Panda(pandas[0])
+ pandas[1] = Panda(pandas[1])
+
+ # find out which one is black
+ type0 = pandas[0].get_type()
+ type1 = pandas[1].get_type()
+
+ black_panda = None
+ other_panda = None
+
+ if type0 == "\x03" and type1 != "\x03":
+ black_panda = pandas[0]
+ other_panda = pandas[1]
+ elif type0 != "\x03" and type1 == "\x03":
+ black_panda = pandas[1]
+ other_panda = pandas[0]
+ else:
+ print("Connect white/grey and black panda to run this test!")
+ assert False
+
+ # disable safety modes
+ black_panda.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
+ other_panda.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
+
+ # test health packet
+ print("black panda health", black_panda.health())
+ print("other panda health", other_panda.health())
+
+ # test black -> other
+ test_buses(black_panda, other_panda, True, [(0, False, [0]), (1, False, [1]), (2, False, [2]), (1, True, [0])], sleep_duration)
+ test_buses(black_panda, other_panda, False, [(0, False, [0]), (1, False, [1]), (2, False, [2]), (0, True, [0, 1])], sleep_duration)
+
+
+def test_buses(black_panda, other_panda, direction, test_array, sleep_duration):
+ if direction:
+ print("***************** TESTING (BLACK --> OTHER) *****************")
+ else:
+ print("***************** TESTING (OTHER --> BLACK) *****************")
+
+ for send_bus, obd, recv_buses in test_array:
+ black_panda.send_heartbeat()
+ other_panda.send_heartbeat()
+ print("\ntest can: ", send_bus, " OBD: ", obd)
+
+ # set OBD on black panda
+ black_panda.set_gmlan(True if obd else None)
+
+ # clear and flush
+ if direction:
+ black_panda.can_clear(send_bus)
+ else:
+ other_panda.can_clear(send_bus)
+
+ for recv_bus in recv_buses:
+ if direction:
+ other_panda.can_clear(recv_bus)
+ else:
+ black_panda.can_clear(recv_bus)
+
+ black_panda.can_recv()
+ other_panda.can_recv()
+
+ # send the characters
+ at = random.randint(1, 2000)
+ st = get_test_string()[0:8]
+ if direction:
+ black_panda.can_send(at, st, send_bus)
+ else:
+ other_panda.can_send(at, st, send_bus)
+ time.sleep(0.1)
+
+ # check for receive
+ if direction:
+ cans_echo = black_panda.can_recv()
+ cans_loop = other_panda.can_recv()
+ else:
+ cans_echo = other_panda.can_recv()
+ cans_loop = black_panda.can_recv()
+
+ loop_buses = []
+ for loop in cans_loop:
+ print(" Loop on bus", str(loop[3]))
+ loop_buses.append(loop[3])
+ if len(cans_loop) == 0:
+ print(" No loop")
+
+ # test loop buses
+ recv_buses.sort()
+ loop_buses.sort()
+ assert recv_buses == loop_buses
+ print(" TEST PASSED")
+
+ time.sleep(sleep_duration)
+ print("\n")
+
+if __name__ == "__main__":
+ parser = argparse.ArgumentParser()
+ parser.add_argument("-n", type=int, help="Number of test iterations to run")
+ parser.add_argument("-sleep", type=int, help="Sleep time between tests", default=0)
+ args = parser.parse_args()
+
+ if args.n is None:
+ while True:
+ run_test(sleep_duration=args.sleep)
+ else:
+ for i in range(args.n):
+ run_test(sleep_duration=args.sleep)
diff --git a/panda/tests/build_strict/Dockerfile b/panda/tests/build_strict/Dockerfile
deleted file mode 100644
index b1c75c025..000000000
--- a/panda/tests/build_strict/Dockerfile
+++ /dev/null
@@ -1,9 +0,0 @@
-FROM ubuntu:16.04
-
-RUN apt-get update && apt-get install -y gcc-arm-none-eabi libnewlib-arm-none-eabi python python-pip gcc g++
-
-RUN pip install pycrypto==2.6.1
-
-COPY . /panda
-
-WORKDIR /panda
diff --git a/panda/tests/build_strict/test_build_strict.sh b/panda/tests/build_strict/test_build_strict.sh
deleted file mode 100755
index ee57ba8ad..000000000
--- a/panda/tests/build_strict/test_build_strict.sh
+++ /dev/null
@@ -1,15 +0,0 @@
-#!/bin/bash -e
-
-cd ../../board/
-
-make -f Makefile.strict clean
-make -f Makefile.strict bin 2> compiler_output.txt
-
-
-if [[ -s "compiler_output.txt" ]]
-then
- echo "Found alerts from the compiler:"
- cat compiler_output.txt
- exit 1
-fi
-
diff --git a/panda/tests/elm_car_simulator.py b/panda/tests/elm_car_simulator.py
index bcee821cd..f931e66ff 100755
--- a/panda/tests/elm_car_simulator.py
+++ b/panda/tests/elm_car_simulator.py
@@ -152,7 +152,7 @@ class ELMCarSimulator():
if len(outmsg) <= 5:
self._lin_send(0x10, obd_header + outmsg)
else:
- first_msg_len = MIN(4, len(outmsg)%4) or 4
+ first_msg_len = min(4, len(outmsg)%4) or 4
self._lin_send(0x10, obd_header + b'\x01' +
b'\x00'*(4-first_msg_len) +
outmsg[:first_msg_len])
@@ -229,7 +229,7 @@ class ELMCarSimulator():
outaddr = 0x7E8 if address == 0x7DF or address == 0x7E0 else 0x18DAF110
msgnum = 1
while(self.__can_multipart_data):
- datalen = MIN(7, len(self.__can_multipart_data))
+ datalen = min(7, len(self.__can_multipart_data))
msgpiece = struct.pack("B", 0x20 | msgnum) + self.__can_multipart_data[:datalen]
self._can_send(outaddr, msgpiece)
self.__can_multipart_data = self.__can_multipart_data[7:]
@@ -246,7 +246,7 @@ class ELMCarSimulator():
self._can_send(outaddr,
struct.pack("BBB", len(outmsg)+2, 0x40|data[1], pid) + outmsg)
else:
- first_msg_len = MIN(3, len(outmsg)%7)
+ first_msg_len = min(3, len(outmsg)%7)
payload_len = len(outmsg)+3
msgpiece = struct.pack("BBBBB", 0x10 | ((payload_len>>8)&0xF),
payload_len&0xFF,
diff --git a/panda/tests/gmbitbang/test_one.py b/panda/tests/gmbitbang/test_one.py
index a398e2780..d7d430437 100755
--- a/panda/tests/gmbitbang/test_one.py
+++ b/panda/tests/gmbitbang/test_one.py
@@ -5,7 +5,7 @@ from panda import Panda
p = Panda()
p.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
-# ack any crap on bus
+# hack anything on bus
p.set_gmlan(bus=2)
time.sleep(0.1)
while len(p.can_recv()) > 0:
diff --git a/panda/tests/language/Dockerfile b/panda/tests/language/Dockerfile
new file mode 100644
index 000000000..068847145
--- /dev/null
+++ b/panda/tests/language/Dockerfile
@@ -0,0 +1,6 @@
+FROM ubuntu:16.04
+
+RUN apt-get update && apt-get install -y make python python-pip
+COPY tests/safety/requirements.txt /panda/tests/safety/requirements.txt
+RUN pip install -r /panda/tests/safety/requirements.txt
+COPY . /panda
diff --git a/panda/tests/language/LICENSE b/panda/tests/language/LICENSE
new file mode 100644
index 000000000..8dada3eda
--- /dev/null
+++ b/panda/tests/language/LICENSE
@@ -0,0 +1,201 @@
+ Apache License
+ Version 2.0, January 2004
+ http://www.apache.org/licenses/
+
+ TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+ 1. Definitions.
+
+ "License" shall mean the terms and conditions for use, reproduction,
+ and distribution as defined by Sections 1 through 9 of this document.
+
+ "Licensor" shall mean the copyright owner or entity authorized by
+ the copyright owner that is granting the License.
+
+ "Legal Entity" shall mean the union of the acting entity and all
+ other entities that control, are controlled by, or are under common
+ control with that entity. For the purposes of this definition,
+ "control" means (i) the power, direct or indirect, to cause the
+ direction or management of such entity, whether by contract or
+ otherwise, or (ii) ownership of fifty percent (50%) or more of the
+ outstanding shares, or (iii) beneficial ownership of such entity.
+
+ "You" (or "Your") shall mean an individual or Legal Entity
+ exercising permissions granted by this License.
+
+ "Source" form shall mean the preferred form for making modifications,
+ including but not limited to software source code, documentation
+ source, and configuration files.
+
+ "Object" form shall mean any form resulting from mechanical
+ transformation or translation of a Source form, including but
+ not limited to compiled object code, generated documentation,
+ and conversions to other media types.
+
+ "Work" shall mean the work of authorship, whether in Source or
+ Object form, made available under the License, as indicated by a
+ copyright notice that is included in or attached to the work
+ (an example is provided in the Appendix below).
+
+ "Derivative Works" shall mean any work, whether in Source or Object
+ form, that is based on (or derived from) the Work and for which the
+ editorial revisions, annotations, elaborations, or other modifications
+ represent, as a whole, an original work of authorship. For the purposes
+ of this License, Derivative Works shall not include works that remain
+ separable from, or merely link (or bind by name) to the interfaces of,
+ the Work and Derivative Works thereof.
+
+ "Contribution" shall mean any work of authorship, including
+ the original version of the Work and any modifications or additions
+ to that Work or Derivative Works thereof, that is intentionally
+ submitted to Licensor for inclusion in the Work by the copyright owner
+ or by an individual or Legal Entity authorized to submit on behalf of
+ the copyright owner. For the purposes of this definition, "submitted"
+ means any form of electronic, verbal, or written communication sent
+ to the Licensor or its representatives, including but not limited to
+ communication on electronic mailing lists, source code control systems,
+ and issue tracking systems that are managed by, or on behalf of, the
+ Licensor for the purpose of discussing and improving the Work, but
+ excluding communication that is conspicuously marked or otherwise
+ designated in writing by the copyright owner as "Not a Contribution."
+
+ "Contributor" shall mean Licensor and any individual or Legal Entity
+ on behalf of whom a Contribution has been received by Licensor and
+ subsequently incorporated within the Work.
+
+ 2. Grant of Copyright License. Subject to the terms and conditions of
+ this License, each Contributor hereby grants to You a perpetual,
+ worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+ copyright license to reproduce, prepare Derivative Works of,
+ publicly display, publicly perform, sublicense, and distribute the
+ Work and such Derivative Works in Source or Object form.
+
+ 3. Grant of Patent License. Subject to the terms and conditions of
+ this License, each Contributor hereby grants to You a perpetual,
+ worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+ (except as stated in this section) patent license to make, have made,
+ use, offer to sell, sell, import, and otherwise transfer the Work,
+ where such license applies only to those patent claims licensable
+ by such Contributor that are necessarily infringed by their
+ Contribution(s) alone or by combination of their Contribution(s)
+ with the Work to which such Contribution(s) was submitted. If You
+ institute patent litigation against any entity (including a
+ cross-claim or counterclaim in a lawsuit) alleging that the Work
+ or a Contribution incorporated within the Work constitutes direct
+ or contributory patent infringement, then any patent licenses
+ granted to You under this License for that Work shall terminate
+ as of the date such litigation is filed.
+
+ 4. Redistribution. You may reproduce and distribute copies of the
+ Work or Derivative Works thereof in any medium, with or without
+ modifications, and in Source or Object form, provided that You
+ meet the following conditions:
+
+ (a) You must give any other recipients of the Work or
+ Derivative Works a copy of this License; and
+
+ (b) You must cause any modified files to carry prominent notices
+ stating that You changed the files; and
+
+ (c) You must retain, in the Source form of any Derivative Works
+ that You distribute, all copyright, patent, trademark, and
+ attribution notices from the Source form of the Work,
+ excluding those notices that do not pertain to any part of
+ the Derivative Works; and
+
+ (d) If the Work includes a "NOTICE" text file as part of its
+ distribution, then any Derivative Works that You distribute must
+ include a readable copy of the attribution notices contained
+ within such NOTICE file, excluding those notices that do not
+ pertain to any part of the Derivative Works, in at least one
+ of the following places: within a NOTICE text file distributed
+ as part of the Derivative Works; within the Source form or
+ documentation, if provided along with the Derivative Works; or,
+ within a display generated by the Derivative Works, if and
+ wherever such third-party notices normally appear. The contents
+ of the NOTICE file are for informational purposes only and
+ do not modify the License. You may add Your own attribution
+ notices within Derivative Works that You distribute, alongside
+ or as an addendum to the NOTICE text from the Work, provided
+ that such additional attribution notices cannot be construed
+ as modifying the License.
+
+ You may add Your own copyright statement to Your modifications and
+ may provide additional or different license terms and conditions
+ for use, reproduction, or distribution of Your modifications, or
+ for any such Derivative Works as a whole, provided Your use,
+ reproduction, and distribution of the Work otherwise complies with
+ the conditions stated in this License.
+
+ 5. Submission of Contributions. Unless You explicitly state otherwise,
+ any Contribution intentionally submitted for inclusion in the Work
+ by You to the Licensor shall be under the terms and conditions of
+ this License, without any additional terms or conditions.
+ Notwithstanding the above, nothing herein shall supersede or modify
+ the terms of any separate license agreement you may have executed
+ with Licensor regarding such Contributions.
+
+ 6. Trademarks. This License does not grant permission to use the trade
+ names, trademarks, service marks, or product names of the Licensor,
+ except as required for reasonable and customary use in describing the
+ origin of the Work and reproducing the content of the NOTICE file.
+
+ 7. Disclaimer of Warranty. Unless required by applicable law or
+ agreed to in writing, Licensor provides the Work (and each
+ Contributor provides its Contributions) on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+ implied, including, without limitation, any warranties or conditions
+ of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+ PARTICULAR PURPOSE. You are solely responsible for determining the
+ appropriateness of using or redistributing the Work and assume any
+ risks associated with Your exercise of permissions under this License.
+
+ 8. Limitation of Liability. In no event and under no legal theory,
+ whether in tort (including negligence), contract, or otherwise,
+ unless required by applicable law (such as deliberate and grossly
+ negligent acts) or agreed to in writing, shall any Contributor be
+ liable to You for damages, including any direct, indirect, special,
+ incidental, or consequential damages of any character arising as a
+ result of this License or out of the use or inability to use the
+ Work (including but not limited to damages for loss of goodwill,
+ work stoppage, computer failure or malfunction, or any and all
+ other commercial damages or losses), even if such Contributor
+ has been advised of the possibility of such damages.
+
+ 9. Accepting Warranty or Additional Liability. While redistributing
+ the Work or Derivative Works thereof, You may choose to offer,
+ and charge a fee for, acceptance of support, warranty, indemnity,
+ or other liability obligations and/or rights consistent with this
+ License. However, in accepting such obligations, You may act only
+ on Your own behalf and on Your sole responsibility, not on behalf
+ of any other Contributor, and only if You agree to indemnify,
+ defend, and hold each Contributor harmless for any liability
+ incurred by, or claims asserted against, such Contributor by reason
+ of your accepting any such warranty or additional liability.
+
+ END OF TERMS AND CONDITIONS
+
+ APPENDIX: How to apply the Apache License to your work.
+
+ To apply the Apache License to your work, attach the following
+ boilerplate notice, with the fields enclosed by brackets "{}"
+ replaced with your own identifying information. (Don't include
+ the brackets!) The text should be enclosed in the appropriate
+ comment syntax for the file format. We also recommend that a
+ file or class name and description of purpose be included on the
+ same "printed page" as the copyright notice for easier
+ identification within third-party archives.
+
+ Copyright {yyyy} {name of copyright owner}
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
diff --git a/panda/tests/language/list.txt b/panda/tests/language/list.txt
new file mode 100644
index 000000000..cfd25897d
--- /dev/null
+++ b/panda/tests/language/list.txt
@@ -0,0 +1,451 @@
+4r5e
+5h1t
+5hit
+a55
+anal
+anus
+ar5e
+arrse
+arse
+ass
+ass-fucker
+asses
+assfucker
+assfukka
+asshole
+assholes
+asswhole
+a_s_s
+b!tch
+b00bs
+b17ch
+b1tch
+ballbag
+balls
+ballsack
+bastard
+beastial
+beastiality
+bellend
+bestial
+bestiality
+bi+ch
+biatch
+bitch
+bitcher
+bitchers
+bitches
+bitchin
+bitching
+bloody
+blow job
+blowjob
+blowjobs
+boiolas
+bollock
+bollok
+boner
+boob
+boobs
+booobs
+boooobs
+booooobs
+booooooobs
+breasts
+buceta
+bugger
+bum
+bunny fucker
+bullshit
+butt
+butthole
+buttmuch
+buttplug
+c0ck
+c0cksucker
+carpet muncher
+cawk
+chink
+cipa
+cl1t
+clit
+clitoris
+clits
+cnut
+cock
+cock-sucker
+cockface
+cockhead
+cockmunch
+cockmuncher
+cocks
+cocksuck
+cocksucked
+cocksucker
+cocksucking
+cocksucks
+cocksuka
+cocksukka
+cok
+cokmuncher
+coksucka
+coon
+cox
+crap
+cum
+cummer
+cumming
+cums
+cumshot
+cunilingus
+cunillingus
+cunnilingus
+cunt
+cuntlick
+cuntlicker
+cuntlicking
+cunts
+cyalis
+cyberfuc
+cyberfuck
+cyberfucked
+cyberfucker
+cyberfuckers
+cyberfucking
+d1ck
+damn
+dick
+dickhead
+dildo
+dildos
+dink
+dinks
+dirsa
+dlck
+dog-fucker
+doggin
+dogging
+donkeyribber
+doosh
+duche
+dyke
+ejaculate
+ejaculated
+ejaculates
+ejaculating
+ejaculatings
+ejaculation
+ejakulate
+f u c k
+f u c k e r
+f4nny
+fag
+fagging
+faggitt
+faggot
+faggs
+fagot
+fagots
+fags
+fanny
+fannyflaps
+fannyfucker
+fanyy
+fatass
+fcuk
+fcuker
+fcuking
+feck
+fecker
+felching
+fellate
+fellatio
+fingerfuck
+fingerfucked
+fingerfucker
+fingerfuckers
+fingerfucking
+fingerfucks
+fistfuck
+fistfucked
+fistfucker
+fistfuckers
+fistfucking
+fistfuckings
+fistfucks
+flange
+fook
+fooker
+fuck
+fucka
+fucked
+fucker
+fuckers
+fuckhead
+fuckheads
+fuckin
+fucking
+fuckings
+fuckingshitmotherfucker
+fuckme
+fucks
+fuckwhit
+fuckwit
+fudge packer
+fudgepacker
+fuk
+fuker
+fukker
+fukkin
+fuks
+fukwhit
+fukwit
+fux
+fux0r
+f_u_c_k
+gangbang
+gangbanged
+gangbangs
+gaylord
+gaysex
+goatse
+God
+god-dam
+god-damned
+goddamn
+goddamned
+hardcoresex
+hell
+heshe
+hoar
+hoare
+hoer
+homo
+hore
+horniest
+horny
+hotsex
+jack-off
+jackoff
+jap
+jerk-off
+jism
+jiz
+jizm
+jizz
+kawk
+knob
+knobead
+knobed
+knobend
+knobhead
+knobjocky
+knobjokey
+kock
+kondum
+kondums
+kum
+kummer
+kumming
+kums
+kunilingus
+l3i+ch
+l3itch
+labia
+lmfao
+lust
+lusting
+m0f0
+m0fo
+m45terbate
+ma5terb8
+ma5terbate
+masochist
+master-bate
+masterb8
+masterbat*
+masterbat3
+masterbate
+masterbation
+masterbations
+masturbate
+mo-fo
+mof0
+mofo
+mothafuck
+mothafucka
+mothafuckas
+mothafuckaz
+mothafucked
+mothafucker
+mothafuckers
+mothafuckin
+mothafucking
+mothafuckings
+mothafucks
+mother fucker
+motherfuck
+motherfucked
+motherfucker
+motherfuckers
+motherfuckin
+motherfucking
+motherfuckings
+motherfuckka
+motherfucks
+muff
+mutha
+muthafecker
+muthafuckker
+muther
+mutherfucker
+n1gga
+n1gger
+nazi
+nigg3r
+nigg4h
+nigga
+niggah
+niggas
+niggaz
+nigger
+niggers
+nob
+nob jokey
+nobhead
+nobjocky
+nobjokey
+numbnuts
+nutsack
+orgasim
+orgasims
+orgasm
+orgasms
+p0rn
+pawn
+pecker
+penis
+penisfucker
+phonesex
+phuck
+phuk
+phuked
+phuking
+phukked
+phukking
+phuks
+phuq
+pigfucker
+pimpis
+piss
+pissed
+pisser
+pissers
+pisses
+pissflaps
+pissin
+pissing
+pissoff
+poop
+porn
+porno
+pornography
+pornos
+prick
+pricks
+pron
+pube
+pusse
+pussi
+pussies
+pussy
+pussys
+rectum
+retard
+rimjaw
+rimming
+s hit
+s.o.b.
+sadist
+schlong
+screwing
+scroat
+scrote
+scrotum
+semen
+sex
+sh!+
+sh!t
+sh1t
+shag
+shagger
+shaggin
+shagging
+shemale
+shi+
+shit
+shitdick
+shite
+shited
+shitey
+shitfuck
+shitfull
+shithead
+shiting
+shitings
+shits
+shitted
+shitter
+shitters
+shitting
+shittings
+shitty
+skank
+slut
+sluts
+smegma
+smut
+snatch
+son-of-a-bitch
+spac
+spunk
+s_h_i_t
+t1tt1e5
+t1tties
+teets
+teez
+testical
+testicle
+tit
+titfuck
+tits
+titt
+tittie5
+tittiefucker
+titties
+tittyfuck
+tittywank
+titwank
+tosser
+turd
+tw4t
+twat
+twathead
+twatty
+twunt
+twunter
+v14gra
+v1gra
+vagina
+viagra
+vulva
+w00se
+wang
+wank
+wanker
+wanky
+whoar
+whore
+willies
+willy
+xrated
diff --git a/panda/tests/language/test_language.py b/panda/tests/language/test_language.py
new file mode 100755
index 000000000..3afb34619
--- /dev/null
+++ b/panda/tests/language/test_language.py
@@ -0,0 +1,28 @@
+#!/usr/bin/env python
+
+import subprocess
+import sys
+
+checked_ext = ["h", "c", "py", "pyx", "cpp", "hpp", "md", "mk"]
+
+if __name__ == "__main__":
+ with open("list.txt", 'r') as handle:
+
+ suffix_cmd = " "
+ for i in checked_ext:
+ suffix_cmd += "--include \*." + i + " "
+
+ found_bad_language = False
+ for line in handle:
+ line = line.rstrip('\n').rstrip(" ")
+ try:
+ cmd = "cd ../../; grep -R -i -w " + suffix_cmd + " '" + line + "'"
+ res = subprocess.check_output(cmd, shell=True, stderr=subprocess.STDOUT)
+ print res
+ found_bad_language = True
+ except subprocess.CalledProcessError as e:
+ pass
+ if found_bad_language:
+ sys.exit("Failed: found bad language")
+ else:
+ print "Success"
diff --git a/panda/tests/misra/suppressions.txt b/panda/tests/misra/suppressions.txt
new file mode 100644
index 000000000..8e58b6e34
--- /dev/null
+++ b/panda/tests/misra/suppressions.txt
@@ -0,0 +1,8 @@
+# Advisory: union types can be used
+misra.19.2
+# FIXME: add it back when fixed in cppcheck. Macro identifiers are unique but it false triggers on defines in #ifdef..#else conditions
+misra.5.4
+# Advisory: casting from void pointer to type pointer is ok. Done by STM libraries as well
+misra.11.4
+# Advisory: casting from void pointer to type pointer is ok. Done by STM libraries as well
+misra.11.5
diff --git a/panda/tests/misra/test_misra.sh b/panda/tests/misra/test_misra.sh
index 835f4ebcf..633800912 100755
--- a/panda/tests/misra/test_misra.sh
+++ b/panda/tests/misra/test_misra.sh
@@ -1,24 +1,53 @@
#!/bin/bash -e
+mkdir /tmp/misra || true
git clone https://github.com/danmar/cppcheck.git || true
cd cppcheck
git fetch
-git checkout 44d6066c6fad32e2b0332b3f2b24bd340febaef8
+git checkout 862c4ef87b109ae86c2d5f12769b7c8d199f35c5
make -j4
cd ../../../
-# whole panda code
-tests/misra/cppcheck/cppcheck --dump --enable=all --inline-suppr board/main.c 2>/tmp/misra/cppcheck_output.txt || true
-python tests/misra/cppcheck/addons/misra.py board/main.c.dump 2>/tmp/misra/misra_output.txt || true
-# violations in safety files
-(cat /tmp/misra/misra_output.txt | grep safety) > /tmp/misra/misra_safety_output.txt || true
-(cat /tmp/misra/cppcheck_output.txt | grep safety) > /tmp/misra/cppcheck_safety_output.txt || true
+printf "\nPANDA CODE\n"
+tests/misra/cppcheck/cppcheck -DPANDA -UPEDAL -DCAN3 -DUID_BASE -DEON \
+ --suppressions-list=tests/misra/suppressions.txt \
+ --dump --enable=all --inline-suppr --force \
+ board/main.c 2>/tmp/misra/cppcheck_output.txt
-if [[ -s "/tmp/misra/misra_safety_output.txt" ]] || [[ -s "/tmp/misra/cppcheck_safety_output.txt" ]]
+python tests/misra/cppcheck/addons/misra.py board/main.c.dump 2> /tmp/misra/misra_output.txt || true
+
+# strip (information) lines
+cppcheck_output=$( cat /tmp/misra/cppcheck_output.txt | grep -v "(information) " ) || true
+misra_output=$( cat /tmp/misra/misra_output.txt | grep -v "(information) " ) || true
+
+
+printf "\nPEDAL CODE\n"
+tests/misra/cppcheck/cppcheck -UPANDA -DPEDAL -UCAN3 \
+ --suppressions-list=tests/misra/suppressions.txt \
+ -I board/ --dump --enable=all --inline-suppr --force \
+ board/pedal/main.c 2>/tmp/misra/cppcheck_pedal_output.txt
+
+python tests/misra/cppcheck/addons/misra.py board/pedal/main.c.dump 2> /tmp/misra/misra_pedal_output.txt || true
+
+# strip (information) lines
+cppcheck_pedal_output=$( cat /tmp/misra/cppcheck_pedal_output.txt | grep -v "(information) " ) || true
+misra_pedal_output=$( cat /tmp/misra/misra_pedal_output.txt | grep -v "(information) " ) || true
+
+if [[ -n "$misra_output" ]] || [[ -n "$cppcheck_output" ]]
then
- echo "Found Misra violations in the safety code:"
- cat /tmp/misra/misra_safety_output.txt
- cat /tmp/misra/cppcheck_safety_output.txt
+ echo "Failed! found Misra violations in panda code:"
+ echo "$misra_output"
+ echo "$cppcheck_output"
exit 1
fi
+
+if [[ -n "$misra_pedal_output" ]] || [[ -n "$cppcheck_pedal_output" ]]
+then
+ echo "Failed! found Misra violations in pedal code:"
+ echo "$misra_pedal_output"
+ echo "$cppcheck_pedal_output"
+ exit 1
+fi
+
+echo "Success"
diff --git a/panda/tests/safety/libpandasafety_py.py b/panda/tests/safety/libpandasafety_py.py
index dc5e5be5a..888bd36e9 100644
--- a/panda/tests/safety/libpandasafety_py.py
+++ b/panda/tests/safety/libpandasafety_py.py
@@ -37,6 +37,7 @@ bool get_long_controls_allowed(void);
void set_gas_interceptor_detected(bool c);
bool get_gas_interceptor_detetcted(void);
int get_gas_interceptor_prev(void);
+int get_hw_type(void);
void set_timer(uint32_t t);
void reset_angle_control(void);
@@ -55,11 +56,12 @@ void set_toyota_camera_forwarded(int t);
void set_toyota_rt_torque_last(int t);
void init_tests_honda(void);
-int get_honda_ego_speed(void);
+bool get_honda_moving(void);
int get_honda_brake_prev(void);
int get_honda_gas_prev(void);
void set_honda_alt_brake_msg(bool);
void set_honda_bosch_hardware(bool);
+int get_honda_bosch_hardware(void);
void init_tests_cadillac(void);
void set_cadillac_desired_torque_last(int t);
diff --git a/panda/tests/safety/test.c b/panda/tests/safety/test.c
index be13d346a..7cd9b86d8 100644
--- a/panda/tests/safety/test.c
+++ b/panda/tests/safety/test.c
@@ -1,5 +1,6 @@
#include
#include
+#include
typedef struct
{
@@ -32,6 +33,7 @@ struct sample_t subaru_torque_driver;
TIM_TypeDef timer;
TIM_TypeDef *TIM2 = &timer;
+// from config.h
#define MIN(a,b) \
({ __typeof__ (a) _a = (a); \
__typeof__ (b) _b = (b); \
@@ -42,6 +44,24 @@ TIM_TypeDef *TIM2 = &timer;
__typeof__ (b) _b = (b); \
_a > _b ? _a : _b; })
+// from llcan.h
+#define GET_BUS(msg) (((msg)->RDTR >> 4) & 0xFF)
+#define GET_LEN(msg) ((msg)->RDTR & 0xf)
+#define GET_ADDR(msg) ((((msg)->RIR & 4) != 0) ? ((msg)->RIR >> 3) : ((msg)->RIR >> 21))
+#define GET_BYTE(msg, b) (((int)(b) > 3) ? (((msg)->RDHR >> (8U * ((unsigned int)(b) % 4U))) & 0XFFU) : (((msg)->RDLR >> (8U * (unsigned int)(b))) & 0xFFU))
+#define GET_BYTES_04(msg) ((msg)->RDLR)
+#define GET_BYTES_48(msg) ((msg)->RDHR)
+
+// from board_declarations.h
+#define HW_TYPE_UNKNOWN 0U
+#define HW_TYPE_WHITE_PANDA 1U
+#define HW_TYPE_GREY_PANDA 2U
+#define HW_TYPE_BLACK_PANDA 3U
+#define HW_TYPE_PEDAL 4U
+
+// from main_declarations.h
+uint8_t hw_type = 0U;
+
#define UNUSED(x) (void)(x)
#define PANDA
@@ -81,6 +101,10 @@ int get_gas_interceptor_prev(void){
return gas_interceptor_prev;
}
+int get_hw_type(void){
+ return hw_type;
+}
+
void set_timer(uint32_t t){
timer.CNT = t;
}
@@ -199,8 +223,8 @@ void set_subaru_desired_torque_last(int t){
subaru_desired_torque_last = t;
}
-int get_honda_ego_speed(void){
- return honda_ego_speed;
+bool get_honda_moving(void){
+ return honda_moving;
}
int get_honda_brake_prev(void){
@@ -219,7 +243,17 @@ void set_honda_bosch_hardware(bool c){
honda_bosch_hardware = c;
}
+int get_honda_bosch_hardware(void) {
+ return honda_bosch_hardware;
+}
+
+void init_tests(void){
+ // get HW_TYPE from env variable set in test.sh
+ hw_type = atoi(getenv("HW_TYPE"));
+}
+
void init_tests_toyota(void){
+ init_tests();
toyota_torque_meas.min = 0;
toyota_torque_meas.max = 0;
toyota_desired_torque_last = 0;
@@ -229,6 +263,7 @@ void init_tests_toyota(void){
}
void init_tests_cadillac(void){
+ init_tests();
cadillac_torque_driver.min = 0;
cadillac_torque_driver.max = 0;
for (int i = 0; i < 4; i++) cadillac_desired_torque_last[i] = 0;
@@ -238,6 +273,7 @@ void init_tests_cadillac(void){
}
void init_tests_gm(void){
+ init_tests();
gm_torque_driver.min = 0;
gm_torque_driver.max = 0;
gm_desired_torque_last = 0;
@@ -247,6 +283,7 @@ void init_tests_gm(void){
}
void init_tests_hyundai(void){
+ init_tests();
hyundai_torque_driver.min = 0;
hyundai_torque_driver.max = 0;
hyundai_desired_torque_last = 0;
@@ -256,6 +293,7 @@ void init_tests_hyundai(void){
}
void init_tests_chrysler(void){
+ init_tests();
chrysler_torque_meas.min = 0;
chrysler_torque_meas.max = 0;
chrysler_desired_torque_last = 0;
@@ -265,6 +303,7 @@ void init_tests_chrysler(void){
}
void init_tests_subaru(void){
+ init_tests();
subaru_torque_driver.min = 0;
subaru_torque_driver.max = 0;
subaru_desired_torque_last = 0;
@@ -274,7 +313,8 @@ void init_tests_subaru(void){
}
void init_tests_honda(void){
- honda_ego_speed = 0;
+ init_tests();
+ honda_moving = false;
honda_brake_prev = 0;
honda_gas_prev = 0;
}
diff --git a/panda/tests/safety/test.sh b/panda/tests/safety/test.sh
index 83d8f5b31..2674281ad 100755
--- a/panda/tests/safety/test.sh
+++ b/panda/tests/safety/test.sh
@@ -1,2 +1,17 @@
#!/usr/bin/env sh
-python -m unittest discover .
+
+# Loop over all hardware types:
+# HW_TYPE_UNKNOWN 0U
+# HW_TYPE_WHITE_PANDA 1U
+# HW_TYPE_GREY_PANDA 2U
+# HW_TYPE_BLACK_PANDA 3U
+# HW_TYPE_PEDAL 4U
+
+# Make sure test fails if one HW_TYPE fails
+set -e
+
+for hw_type in 0 1 2 3 4
+do
+ echo "Testing HW_TYPE: $hw_type"
+ HW_TYPE=$hw_type python -m unittest discover .
+done
diff --git a/panda/tests/safety/test_honda.py b/panda/tests/safety/test_honda.py
index bc5e8d192..f16030843 100755
--- a/panda/tests/safety/test_honda.py
+++ b/panda/tests/safety/test_honda.py
@@ -5,6 +5,8 @@ import libpandasafety_py
MAX_BRAKE = 255
+INTERCEPTOR_THRESHOLD = 328
+
class TestHondaSafety(unittest.TestCase):
@classmethod
def setUp(cls):
@@ -31,6 +33,10 @@ class TestHondaSafety(unittest.TestCase):
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
to_send[0].RIR = msg << 21
to_send[0].RDLR = buttons << 5
+ is_panda_black = self.safety.get_hw_type() == 3 # black_panda
+ honda_bosch_hardware = self.safety.get_honda_bosch_hardware()
+ bus = 1 if is_panda_black and honda_bosch_hardware else 0
+ to_send[0].RDTR = bus << 4
return to_send
@@ -66,7 +72,9 @@ class TestHondaSafety(unittest.TestCase):
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
to_send[0].RIR = addr << 21
to_send[0].RDTR = 6
- to_send[0].RDLR = ((gas & 0xff) << 8) | ((gas & 0xff00) >> 8)
+ gas2 = gas * 2
+ to_send[0].RDLR = ((gas & 0xff) << 8) | ((gas & 0xff00) >> 8) | \
+ ((gas2 & 0xff) << 24) | ((gas2 & 0xff00) << 8)
return to_send
@@ -99,9 +107,9 @@ class TestHondaSafety(unittest.TestCase):
self.assertFalse(self.safety.get_controls_allowed())
def test_sample_speed(self):
- self.assertEqual(0, self.safety.get_honda_ego_speed())
+ self.assertEqual(0, self.safety.get_honda_moving())
self.safety.safety_rx_hook(self._speed_msg(100))
- self.assertEqual(100, self.safety.get_honda_ego_speed())
+ self.assertEqual(1, self.safety.get_honda_moving())
def test_prev_brake(self):
self.assertFalse(self.safety.get_honda_brake_prev())
@@ -176,16 +184,15 @@ class TestHondaSafety(unittest.TestCase):
def test_disengage_on_gas_interceptor(self):
for long_controls_allowed in [0, 1]:
- self.safety.set_long_controls_allowed(long_controls_allowed)
- self.safety.safety_rx_hook(self._send_interceptor_msg(0, 0x201))
- self.safety.set_controls_allowed(1)
- self.safety.safety_rx_hook(self._send_interceptor_msg(0x1000, 0x201))
- if long_controls_allowed:
- self.assertFalse(self.safety.get_controls_allowed())
- else:
- self.assertTrue(self.safety.get_controls_allowed())
- self.safety.safety_rx_hook(self._send_interceptor_msg(0, 0x201))
- self.safety.set_gas_interceptor_detected(False)
+ for g in range(0, 0x1000):
+ self.safety.set_long_controls_allowed(long_controls_allowed)
+ self.safety.safety_rx_hook(self._send_interceptor_msg(0, 0x201))
+ self.safety.set_controls_allowed(True)
+ self.safety.safety_rx_hook(self._send_interceptor_msg(g, 0x201))
+ remain_enabled = (not long_controls_allowed or g <= INTERCEPTOR_THRESHOLD)
+ self.assertEqual(remain_enabled, self.safety.get_controls_allowed())
+ self.safety.safety_rx_hook(self._send_interceptor_msg(0, 0x201))
+ self.safety.set_gas_interceptor_detected(False)
self.safety.set_long_controls_allowed(True)
def test_allow_engage_with_gas_interceptor_pressed(self):
diff --git a/panda/tests/safety/test_honda_bosch.py b/panda/tests/safety/test_honda_bosch.py
index 11c939140..0d37cbe80 100755
--- a/panda/tests/safety/test_honda_bosch.py
+++ b/panda/tests/safety/test_honda_bosch.py
@@ -23,16 +23,20 @@ class TestHondaSafety(unittest.TestCase):
def test_fwd_hook(self):
buss = range(0x0, 0x3)
msgs = range(0x1, 0x800)
+ is_panda_black = self.safety.get_hw_type() == 3 # black panda
+ bus_rdr_cam = 2 if is_panda_black else 1
+ bus_rdr_car = 0 if is_panda_black else 2
+ bus_pt = 1 if is_panda_black else 0
blocked_msgs = [0xE4, 0x33D]
for b in buss:
for m in msgs:
- if b == 0:
+ if b == bus_pt:
fwd_bus = -1
- elif b == 1:
- fwd_bus = -1 if m in blocked_msgs else 2
- elif b == 2:
- fwd_bus = 1
+ elif b == bus_rdr_cam:
+ fwd_bus = -1 if m in blocked_msgs else bus_rdr_car
+ elif b == bus_rdr_car:
+ fwd_bus = bus_rdr_cam
# assume len 8
self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, self._send_msg(b, m, 8)))
diff --git a/panda/tests/safety/test_toyota.py b/panda/tests/safety/test_toyota.py
index 7dd1601d7..dc5b21ac8 100644
--- a/panda/tests/safety/test_toyota.py
+++ b/panda/tests/safety/test_toyota.py
@@ -15,6 +15,8 @@ RT_INTERVAL = 250000
MAX_TORQUE_ERROR = 350
+INTERCEPTOR_THRESHOLD = 475
+
def twos_comp(val, bits):
if val >= 0:
return val
@@ -81,7 +83,9 @@ class TestToyotaSafety(unittest.TestCase):
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
to_send[0].RIR = addr << 21
to_send[0].RDTR = 6
- to_send[0].RDLR = ((gas & 0xff) << 8) | ((gas & 0xff00) >> 8)
+ gas2 = gas * 2
+ to_send[0].RDLR = ((gas & 0xff) << 8) | ((gas & 0xff00) >> 8) | \
+ ((gas2 & 0xff) << 24) | ((gas2 & 0xff00) << 8)
return to_send
@@ -145,16 +149,15 @@ class TestToyotaSafety(unittest.TestCase):
def test_disengage_on_gas_interceptor(self):
for long_controls_allowed in [0, 1]:
- self.safety.set_long_controls_allowed(long_controls_allowed)
- self.safety.safety_rx_hook(self._send_interceptor_msg(0, 0x201))
- self.safety.set_controls_allowed(True)
- self.safety.safety_rx_hook(self._send_interceptor_msg(0x1000, 0x201))
- if long_controls_allowed:
- self.assertFalse(self.safety.get_controls_allowed())
- else:
- self.assertTrue(self.safety.get_controls_allowed())
- self.safety.safety_rx_hook(self._send_interceptor_msg(0, 0x201))
- self.safety.set_gas_interceptor_detected(False)
+ for g in range(0, 0x1000):
+ self.safety.set_long_controls_allowed(long_controls_allowed)
+ self.safety.safety_rx_hook(self._send_interceptor_msg(0, 0x201))
+ self.safety.set_controls_allowed(True)
+ self.safety.safety_rx_hook(self._send_interceptor_msg(g, 0x201))
+ remain_enabled = (not long_controls_allowed or g <= INTERCEPTOR_THRESHOLD)
+ self.assertEqual(remain_enabled, self.safety.get_controls_allowed())
+ self.safety.safety_rx_hook(self._send_interceptor_msg(0, 0x201))
+ self.safety.set_gas_interceptor_detected(False)
self.safety.set_long_controls_allowed(True)
def test_allow_engage_with_gas_interceptor_pressed(self):
diff --git a/panda/tests/safety_replay/Dockerfile b/panda/tests/safety_replay/Dockerfile
index bf2b7f2c1..5d59ca38d 100644
--- a/panda/tests/safety_replay/Dockerfile
+++ b/panda/tests/safety_replay/Dockerfile
@@ -17,4 +17,4 @@ COPY . /openpilot/panda
WORKDIR /openpilot/panda/tests/safety_replay
RUN git clone https://github.com/commaai/openpilot-tools.git tools || true
WORKDIR tools
-RUN git checkout b6461274d684915f39dc45efc5292ea890698da9
+RUN git checkout feb724a14f0f5223c700c94317efaf46923fd48a
diff --git a/panda/tests/safety_replay/install_capnp.sh b/panda/tests/safety_replay/install_capnp.sh
index e13ab48c2..51559d399 100755
--- a/panda/tests/safety_replay/install_capnp.sh
+++ b/panda/tests/safety_replay/install_capnp.sh
@@ -8,13 +8,3 @@ cd capnproto-c++-0.6.1
make -j4
make install
-cd ..
-git clone https://github.com/commaai/c-capnproto.git
-cd c-capnproto
-git checkout 2e625acacf58a5f5c8828d8453d1f8dacc700a96
-git submodule update --init --recursive
-autoreconf -f -i -s
-CFLAGS="-fPIC" ./configure --prefix=/usr/local
-make -j4
-make install
-
diff --git a/run_docker_tests.sh b/run_docker_tests.sh
index 10d1bbc83..dfee5f866 100755
--- a/run_docker_tests.sh
+++ b/run_docker_tests.sh
@@ -3,7 +3,7 @@ set -e
docker build -t tmppilot -f Dockerfile.openpilot .
-docker run --rm tmppilot /bin/sh -c 'cd /tmp/openpilot/selfdrive/test/ && ./test_fingerprints.py'
+docker run --rm tmppilot /bin/sh -c 'cd /tmp/openpilot/ && make -C cereal && cd /tmp/openpilot/selfdrive/test/ && ./test_fingerprints.py'
docker run --rm tmppilot /bin/sh -c 'cd /tmp/openpilot/ && pyflakes $(find . -iname "*.py" | grep -vi "^\./pyextra.*" | grep -vi "^\./panda" | grep -vi "^\./tools")'
docker run --rm tmppilot /bin/sh -c 'cd /tmp/openpilot/ && pylint $(find . -iname "*.py" | grep -vi "^\./pyextra.*" | grep -vi "^\./panda" | grep -vi "^\./tools"); exit $(($? & 3))'
docker run --rm tmppilot /bin/sh -c 'cd /tmp/openpilot/ && make -C cereal && python -m unittest discover common'
@@ -12,3 +12,5 @@ docker run --rm tmppilot /bin/sh -c 'cd /tmp/openpilot/ && make -C cereal && pyt
docker run --rm tmppilot /bin/sh -c 'cd /tmp/openpilot/ && make -C cereal && python -m unittest discover selfdrive/controls'
docker run --rm tmppilot /bin/sh -c 'cd /tmp/openpilot/ && python -m unittest discover selfdrive/loggerd'
docker run --rm -v "$(pwd)"/selfdrive/test/tests/plant/out:/tmp/openpilot/selfdrive/test/tests/plant/out tmppilot /bin/sh -c 'cd /tmp/openpilot/selfdrive/test/tests/plant && OPTEST=1 ./test_longitudinal.py'
+docker run --rm tmppilot /bin/sh -c 'cd /tmp/openpilot/ && make -C cereal && cd /tmp/openpilot/selfdrive/test/tests/process_replay/ && ./test_processes.py'
+docker run --rm tmppilot /bin/sh -c 'mkdir -p /data/params && cd /tmp/openpilot/ && make -C cereal && cd /tmp/openpilot/selfdrive/test/ && ./test_car_models_openpilot.py'
diff --git a/selfdrive/assets/Roboto-Bold.ttf b/selfdrive/assets/Roboto-Bold.ttf
deleted file mode 100644
index a355c27cd..000000000
Binary files a/selfdrive/assets/Roboto-Bold.ttf and /dev/null differ
diff --git a/selfdrive/assets/courbd.ttf b/selfdrive/assets/fonts/courbd.ttf
similarity index 100%
rename from selfdrive/assets/courbd.ttf
rename to selfdrive/assets/fonts/courbd.ttf
diff --git a/selfdrive/assets/OpenSans-Bold.ttf b/selfdrive/assets/fonts/opensans_bold.ttf
similarity index 100%
rename from selfdrive/assets/OpenSans-Bold.ttf
rename to selfdrive/assets/fonts/opensans_bold.ttf
diff --git a/selfdrive/assets/OpenSans-Regular.ttf b/selfdrive/assets/fonts/opensans_regular.ttf
similarity index 100%
rename from selfdrive/assets/OpenSans-Regular.ttf
rename to selfdrive/assets/fonts/opensans_regular.ttf
diff --git a/selfdrive/assets/OpenSans-SemiBold.ttf b/selfdrive/assets/fonts/opensans_semibold.ttf
similarity index 100%
rename from selfdrive/assets/OpenSans-SemiBold.ttf
rename to selfdrive/assets/fonts/opensans_semibold.ttf
diff --git a/selfdrive/assets/sounds/disengaged.wav b/selfdrive/assets/sounds/disengaged.wav
index 958e08fd8..655aa3126 100644
Binary files a/selfdrive/assets/sounds/disengaged.wav and b/selfdrive/assets/sounds/disengaged.wav differ
diff --git a/selfdrive/assets/sounds/engaged.wav b/selfdrive/assets/sounds/engaged.wav
index c6c088e01..b33e8181f 100644
Binary files a/selfdrive/assets/sounds/engaged.wav and b/selfdrive/assets/sounds/engaged.wav differ
diff --git a/selfdrive/assets/sounds/error.wav b/selfdrive/assets/sounds/error.wav
index 1ff0c540d..309eaef8a 100644
Binary files a/selfdrive/assets/sounds/error.wav and b/selfdrive/assets/sounds/error.wav differ
diff --git a/selfdrive/assets/sounds/warning_1.wav b/selfdrive/assets/sounds/warning_1.wav
index 67b8d76fe..920c11846 100644
Binary files a/selfdrive/assets/sounds/warning_1.wav and b/selfdrive/assets/sounds/warning_1.wav differ
diff --git a/selfdrive/assets/sounds/warning_2.wav b/selfdrive/assets/sounds/warning_2.wav
index 8e1b1d7d9..f5ed8521d 100644
Binary files a/selfdrive/assets/sounds/warning_2.wav and b/selfdrive/assets/sounds/warning_2.wav differ
diff --git a/selfdrive/athena/athenad.py b/selfdrive/athena/athenad.py
index 58918de81..6aa84d106 100755
--- a/selfdrive/athena/athenad.py
+++ b/selfdrive/athena/athenad.py
@@ -1,19 +1,27 @@
#!/usr/bin/env python2.7
import json
+import jwt
import os
import random
+import re
+import select
+import subprocess
+import socket
import time
import threading
import traceback
import zmq
import requests
import six.moves.queue
+from datetime import datetime, timedelta
+from functools import partial
from jsonrpc import JSONRPCResponseManager, dispatcher
-from websocket import create_connection, WebSocketTimeoutException
+from websocket import create_connection, WebSocketTimeoutException, ABNF
from selfdrive.loggerd.config import ROOT
import selfdrive.crash as crash
import selfdrive.messaging as messaging
+from common.api import Api
from common.params import Params
from selfdrive.services import service_list
from selfdrive.swaglog import cloudlog
@@ -21,6 +29,7 @@ from selfdrive.version import version, dirty
ATHENA_HOST = os.getenv('ATHENA_HOST', 'wss://athena.comma.ai')
HANDLER_THREADS = os.getenv('HANDLER_THREADS', 4)
+LOCAL_PORT_WHITELIST = set([8022])
dispatcher["echo"] = lambda s: s
payload_queue = six.moves.queue.Queue()
@@ -49,6 +58,7 @@ def handle_long_poll(ws):
thread.join()
def jsonrpc_handler(end_event):
+ dispatcher["startLocalProxy"] = partial(startLocalProxy, end_event)
while not end_event.is_set():
try:
data = payload_queue.get(timeout=1)
@@ -85,6 +95,109 @@ def uploadFileToUrl(fn, url, headers):
ret = requests.put(url, data=f, headers=headers, timeout=10)
return ret.status_code
+def startLocalProxy(global_end_event, remote_ws_uri, local_port):
+ try:
+ cloudlog.event("athena startLocalProxy", remote_ws_uri=remote_ws_uri, local_port=local_port)
+
+ if local_port not in LOCAL_PORT_WHITELIST:
+ raise Exception("Requested local port not whitelisted")
+
+ params = Params()
+ dongle_id = params.get("DongleId")
+ private_key = open("/persist/comma/id_rsa").read()
+ identity_token = jwt.encode({'identity':dongle_id, 'exp': datetime.utcnow() + timedelta(hours=1)}, private_key, algorithm='RS256')
+
+ ws = create_connection(remote_ws_uri,
+ cookie="jwt=" + identity_token,
+ enable_multithread=True)
+
+ ssock, csock = socket.socketpair()
+ local_sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+ local_sock.connect(('127.0.0.1', local_port))
+ local_sock.setblocking(0)
+
+ proxy_end_event = threading.Event()
+ threads = [
+ threading.Thread(target=ws_proxy_recv, args=(ws, local_sock, ssock, proxy_end_event, global_end_event)),
+ threading.Thread(target=ws_proxy_send, args=(ws, local_sock, csock, proxy_end_event))
+ ]
+
+ map(lambda thread: thread.start(), threads)
+
+ return {"success": 1}
+ except Exception as e:
+ traceback.print_exc()
+ raise e
+
+@dispatcher.add_method
+def getPublicKey():
+ if not os.path.isfile('/persist/comma/id_rsa.pub'):
+ return None
+
+ with open('/persist/comma/id_rsa.pub', 'r') as f:
+ return f.read()
+
+@dispatcher.add_method
+def getSshAuthorizedKeys():
+ with open('/system/comma/home/.ssh/authorized_keys', 'r') as f:
+ return f.read()
+
+@dispatcher.add_method
+def getSimInfo():
+ sim_state = subprocess.check_output(['getprop', 'gsm.sim.state']).strip().split(',')
+ network_type = subprocess.check_output(['getprop', 'gsm.network.type']).strip().split(',')
+ mcc_mnc = subprocess.check_output(['getprop', 'gsm.sim.operator.numeric']).strip() or None
+
+ sim_id_aidl_out = subprocess.check_output(['service', 'call', 'iphonesubinfo', '11'])
+ sim_id_aidl_lines = sim_id_aidl_out.split('\n')
+ if len(sim_id_aidl_lines) > 3:
+ sim_id_lines = sim_id_aidl_lines[1:4]
+ sim_id_fragments = [re.search(r"'([0-9\.]+)'", line).group(1) for line in sim_id_lines]
+ sim_id = reduce(lambda frag1, frag2: frag1.replace('.', '') + frag2.replace('.', ''), sim_id_fragments)
+ else:
+ sim_id = None
+
+ return {
+ 'sim_id': sim_id,
+ 'mcc_mnc': mcc_mnc,
+ 'network_type': network_type,
+ 'sim_state': sim_state
+ }
+
+def ws_proxy_recv(ws, local_sock, ssock, end_event, global_end_event):
+ while not (end_event.is_set() or global_end_event.is_set()):
+ try:
+ data = ws.recv()
+ local_sock.sendall(data)
+ except WebSocketTimeoutException:
+ pass
+ except Exception:
+ traceback.print_exc()
+ break
+
+ ssock.close()
+ end_event.set()
+
+def ws_proxy_send(ws, local_sock, signal_sock, end_event):
+ while not end_event.is_set():
+ try:
+ r, _, _ = select.select((local_sock, signal_sock), (), ())
+ if r:
+ if r[0].fileno() == signal_sock.fileno():
+ # got end signal from ws_proxy_recv
+ end_event.set()
+ break
+ data = local_sock.recv(4096)
+ if not data:
+ # local_sock is dead
+ end_event.set()
+ break
+
+ ws.send(data, ABNF.OPCODE_BINARY)
+ except Exception:
+ traceback.print_exc()
+ end_event.set()
+
def ws_recv(ws, end_event):
while not end_event.is_set():
try:
@@ -113,19 +226,21 @@ def backoff(retries):
def main(gctx=None):
params = Params()
dongle_id = params.get("DongleId")
- access_token = params.get("AccessToken")
- ws_uri = ATHENA_HOST + "/ws/" + dongle_id
+ ws_uri = ATHENA_HOST + "/ws/v2/" + dongle_id
crash.bind_user(id=dongle_id)
crash.bind_extra(version=version, dirty=dirty, is_eon=True)
crash.install()
+ private_key = open("/persist/comma/id_rsa").read()
+ api = Api(dongle_id, private_key)
+
conn_retries = 0
while 1:
try:
print("connecting to %s" % ws_uri)
ws = create_connection(ws_uri,
- cookie="jwt=" + access_token,
+ cookie="jwt=" + api.get_token(),
enable_multithread=True)
ws.settimeout(1)
conn_retries = 0
@@ -138,5 +253,7 @@ def main(gctx=None):
time.sleep(backoff(conn_retries))
+ params.delete("AthenadPid")
+
if __name__ == "__main__":
main()
diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc
index 1a64416e9..f4ca033f5 100644
--- a/selfdrive/boardd/boardd.cc
+++ b/selfdrive/boardd/boardd.cc
@@ -59,7 +59,8 @@ pthread_mutex_t usb_lock;
bool spoofing_started = false;
bool fake_send = false;
bool loopback_can = false;
-bool is_grey_panda = false;
+cereal::HealthData::HwType hw_type = cereal::HealthData::HwType::UNKNOWN;
+bool is_pigeon = false;
pthread_t safety_setter_thread_handle = -1;
pthread_t pigeon_thread_handle = -1;
@@ -69,6 +70,29 @@ void pigeon_init();
void *pigeon_thread(void *crap);
void *safety_setter_thread(void *s) {
+ char *value_vin;
+ size_t value_vin_sz = 0;
+
+ // switch to no_output when CarVin param is read
+ while (1) {
+ if (do_exit) return NULL;
+ const int result = read_db_value(NULL, "CarVin", &value_vin, &value_vin_sz);
+ if (value_vin_sz > 0) {
+ // sanity check VIN format
+ assert(value_vin_sz == 17);
+ break;
+ }
+ usleep(100*1000);
+ }
+ LOGW("got CarVin %s", value_vin);
+
+ pthread_mutex_lock(&usb_lock);
+
+ // VIN qury done, stop listening to OBDII
+ libusb_control_transfer(dev_handle, 0x40, 0xdc, SAFETY_NOOUTPUT, 0, NULL, 0, TIMEOUT);
+
+ pthread_mutex_unlock(&usb_lock);
+
char *value;
size_t value_sz = 0;
@@ -151,7 +175,7 @@ void *safety_setter_thread(void *s) {
// must be called before threads or with mutex
bool usb_connect() {
int err;
- unsigned char is_pigeon[1] = {0};
+ unsigned char hw_query[1] = {0};
dev_handle = libusb_open_device_with_vid_pid(ctx, 0xbbaa, 0xddcc);
if (dev_handle == NULL) { goto fail; }
@@ -184,11 +208,12 @@ bool usb_connect() {
assert(err == 0);
}
- libusb_control_transfer(dev_handle, 0xc0, 0xc1, 0, 0, is_pigeon, 1, TIMEOUT);
+ libusb_control_transfer(dev_handle, 0xc0, 0xc1, 0, 0, hw_query, 1, TIMEOUT);
- if (is_pigeon[0]) {
- LOGW("grey panda detected");
- is_grey_panda = true;
+ hw_type = (cereal::HealthData::HwType)(hw_query[0]);
+ is_pigeon = (hw_type == cereal::HealthData::HwType::GREY_PANDA) || (hw_type == cereal::HealthData::HwType::BLACK_PANDA);
+ if (is_pigeon) {
+ LOGW("panda with gps detected");
pigeon_needs_init = true;
if (pigeon_thread_handle == -1) {
err = pthread_create(&pigeon_thread_handle, NULL, pigeon_thread, NULL);
@@ -280,11 +305,13 @@ void can_health(void *s) {
struct __attribute__((packed)) health {
uint32_t voltage;
uint32_t current;
+ uint32_t can_send_errs;
+ uint32_t can_fwd_errs;
+ uint32_t gmlan_send_errs;
uint8_t started;
uint8_t controls_allowed;
uint8_t gas_interceptor_detected;
- uint8_t started_signal_detected;
- uint8_t started_alt;
+ uint8_t car_harness_status_pkt;
} health;
// recv from board
@@ -292,7 +319,9 @@ void can_health(void *s) {
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__); }
+ if (cnt != sizeof(health)) {
+ handle_usb_issue(cnt, __func__);
+ }
} while(cnt != sizeof(health));
pthread_mutex_unlock(&usb_lock);
@@ -313,13 +342,23 @@ void can_health(void *s) {
}
healthData.setControlsAllowed(health.controls_allowed);
healthData.setGasInterceptorDetected(health.gas_interceptor_detected);
- healthData.setStartedSignalDetected(health.started_signal_detected);
- healthData.setIsGreyPanda(is_grey_panda);
+ healthData.setHasGps(is_pigeon);
+ healthData.setCanSendErrs(health.can_send_errs);
+ healthData.setCanFwdErrs(health.can_fwd_errs);
+ healthData.setGmlanSendErrs(health.gmlan_send_errs);
+ healthData.setHwType(hw_type);
// send to health
auto words = capnp::messageToFlatArray(msg);
auto bytes = words.asBytes();
zmq_send(s, bytes.begin(), bytes.size(), 0);
+
+ pthread_mutex_lock(&usb_lock);
+
+ // send heartbeat back to panda
+ libusb_control_transfer(dev_handle, 0x40, 0xf3, 1, 0, NULL, 0, TIMEOUT);
+
+ pthread_mutex_unlock(&usb_lock);
}
@@ -444,10 +483,10 @@ void *can_health_thread(void *crap) {
void *publisher = zmq_socket(context, ZMQ_PUB);
zmq_bind(publisher, "tcp://*:8011");
- // run at 1hz
+ // run at 2hz
while (!do_exit) {
can_health(publisher);
- usleep(1000*1000);
+ usleep(500*1000);
}
return NULL;
}
@@ -499,7 +538,7 @@ void pigeon_set_baud(int baud) {
void pigeon_init() {
usleep(1000*1000);
- LOGW("grey panda start");
+ LOGW("panda GPS start");
// power off pigeon
pigeon_set_power(0);
@@ -540,7 +579,7 @@ void pigeon_init() {
pigeon_send("\xB5\x62\x06\x01\x03\x00\x02\x15\x01\x22\x70");
pigeon_send("\xB5\x62\x06\x01\x03\x00\x02\x13\x01\x20\x6C");
- LOGW("grey panda is ready to fly");
+ LOGW("panda GPS on");
}
static void pigeon_publish_raw(void *publisher, unsigned char *dat, int alen) {
diff --git a/selfdrive/can/parser.cc b/selfdrive/can/parser.cc
index e3225181b..69b30fb51 100644
--- a/selfdrive/can/parser.cc
+++ b/selfdrive/can/parser.cc
@@ -194,32 +194,36 @@ class CANParser {
: bus(abus) {
// connect to can on 8006
context = zmq_ctx_new();
- subscriber = zmq_socket(context, ZMQ_SUB);
- zmq_setsockopt(subscriber, ZMQ_SUBSCRIBE, "", 0);
- zmq_setsockopt(subscriber, ZMQ_RCVTIMEO, &timeout, sizeof(int));
- std::string tcp_addr_str;
+ if (tcp_addr.length() > 0) {
+ subscriber = zmq_socket(context, ZMQ_SUB);
+ zmq_setsockopt(subscriber, ZMQ_SUBSCRIBE, "", 0);
+ zmq_setsockopt(subscriber, ZMQ_RCVTIMEO, &timeout, sizeof(int));
- if (sendcan) {
- tcp_addr_str = "tcp://" + tcp_addr + ":8017";
+ std::string tcp_addr_str;
+
+ if (sendcan) {
+ tcp_addr_str = "tcp://" + tcp_addr + ":8017";
+ } else {
+ tcp_addr_str = "tcp://" + tcp_addr + ":8006";
+ }
+ const char *tcp_addr_char = tcp_addr_str.c_str();
+
+ zmq_connect(subscriber, tcp_addr_char);
+
+ // drain sendcan to delete any stale messages from previous runs
+ zmq_msg_t msgDrain;
+ zmq_msg_init(&msgDrain);
+ int err = 0;
+ while(err >= 0) {
+ err = zmq_msg_recv(&msgDrain, subscriber, ZMQ_DONTWAIT);
+ }
} else {
- tcp_addr_str = "tcp://" + tcp_addr + ":8006";
- }
- const char *tcp_addr_char = tcp_addr_str.c_str();
-
- zmq_connect(subscriber, tcp_addr_char);
-
- // drain sendcan to delete any stale messages from previous runs
- zmq_msg_t msgDrain;
- zmq_msg_init(&msgDrain);
- int err = 0;
- while(err >= 0) {
- err = zmq_msg_recv(&msgDrain, subscriber, ZMQ_DONTWAIT);
+ subscriber = NULL;
}
dbc = dbc_lookup(dbc_name);
- assert(dbc);
-
+ assert(dbc);
for (const auto& op : options) {
MessageState state = {
.address = op.address,
@@ -326,6 +330,21 @@ class CANParser {
}
}
+ void update_string(uint64_t sec, std::string data) {
+ // format for board, make copy due to alignment issues, will be freed on out of scope
+ auto amsg = kj::heapArray((data.length() / sizeof(capnp::word)) + 1);
+ memcpy(amsg.begin(), data.data(), data.length());
+
+ // extract the messages
+ capnp::FlatArrayMessageReader cmsg(amsg);
+ cereal::Event::Reader event = cmsg.getRoot();
+
+ auto cans = event.getCan();
+ UpdateCans(sec, cans);
+
+ UpdateValid(sec);
+ }
+
int update(uint64_t sec, bool wait) {
int err;
int result = 0;
@@ -336,7 +355,7 @@ class CANParser {
// multiple recv is fine
bool first = wait;
- while (1) {
+ while (subscriber != NULL) {
if (first) {
err = zmq_msg_recv(&msg, subscriber, 0);
first = false;
@@ -432,6 +451,11 @@ int can_update(void* can, uint64_t sec, bool wait) {
return cp->update(sec, wait);
}
+void can_update_string(void *can, uint64_t sec, const char* dat, int len) {
+ CANParser* cp = (CANParser*)can;
+ cp->update_string(sec, std::string(dat, len));
+}
+
size_t can_query(void* can, uint64_t sec, bool *out_can_valid, size_t out_values_size, SignalValue* out_values) {
CANParser* cp = (CANParser*)can;
diff --git a/selfdrive/can/parser_pyx.pxd b/selfdrive/can/parser_pyx.pxd
index 9d8efa318..ac619707a 100644
--- a/selfdrive/can/parser_pyx.pxd
+++ b/selfdrive/can/parser_pyx.pxd
@@ -67,6 +67,7 @@ ctypedef void* (*can_init_with_vectors_func)(int bus, const char* dbc_name,
const char* tcp_addr,
int timeout)
ctypedef int (*can_update_func)(void* can, uint64_t sec, bool wait);
+ctypedef void (*can_update_string_func)(void* can, uint64_t sec, const char* dat, int len);
ctypedef size_t (*can_query_func)(void* can, uint64_t sec, bool *out_can_valid, size_t out_values_size, SignalValue* out_values);
ctypedef void (*can_query_vector_func)(void* can, uint64_t sec, bool *out_can_valid, vector[SignalValue] &values)
@@ -77,6 +78,7 @@ cdef class CANParser:
dbc_lookup_func dbc_lookup
can_init_with_vectors_func can_init_with_vectors
can_update_func can_update
+ can_update_string_func can_update_string
can_query_vector_func can_query_vector
map[string, uint32_t] msg_name_to_address
map[uint32_t, string] address_to_msg_name
diff --git a/selfdrive/can/parser_pyx.pyx b/selfdrive/can/parser_pyx.pyx
index 65c6f5ab2..c6f1f58e0 100644
--- a/selfdrive/can/parser_pyx.pyx
+++ b/selfdrive/can/parser_pyx.pyx
@@ -8,7 +8,7 @@ import numbers
cdef int CAN_INVALID_CNT = 5
cdef class CANParser:
- def __init__(self, dbc_name, signals, checks=None, bus=0, sendcan=False, tcp_addr="127.0.0.1", timeout=-1):
+ def __init__(self, dbc_name, signals, checks=None, bus=0, sendcan=False, tcp_addr="", timeout=-1):
self.test_mode_enabled = False
can_dir = os.path.dirname(os.path.abspath(__file__))
libdbc_fn = os.path.join(can_dir, "libdbc.so")
@@ -17,6 +17,7 @@ cdef class CANParser:
self.can_init_with_vectors = dlsym(libdbc, 'can_init_with_vectors')
self.dbc_lookup = dlsym(libdbc, 'dbc_lookup')
self.can_update = dlsym(libdbc, 'can_update')
+ self.can_update_string = dlsym(libdbc, 'can_update_string')
self.can_query_vector = dlsym(libdbc, 'can_query_vector')
if checks is None:
checks = []
@@ -99,6 +100,19 @@ cdef class CANParser:
return updated_val
+ def update_string(self, uint64_t sec, dat):
+ self.can_update_string(self.can, sec, dat, len(dat))
+ return self.update_vl(sec)
+
+ def update_strings(self, uint64_t sec, strings):
+ updated_vals = set()
+
+ for s in strings:
+ updated_val = self.update_string(sec, s)
+ updated_vals.update(updated_val)
+
+ return updated_vals
+
def update(self, uint64_t sec, bool wait):
r = (self.can_update(self.can, sec, wait) >= 0)
updated_val = self.update_vl(sec)
diff --git a/selfdrive/can/tests/test_packer_honda.py b/selfdrive/can/tests/test_packer_honda.py
index 87d62244e..b6e779e40 100644
--- a/selfdrive/can/tests/test_packer_honda.py
+++ b/selfdrive/can/tests/test_packer_honda.py
@@ -16,40 +16,39 @@ class TestPackerMethods(unittest.TestCase):
def test_correctness(self):
# Test all commands, randomize the params.
for _ in xrange(1000):
+ is_panda_black = False
+ car_fingerprint = HONDA_BOSCH[0]
+
apply_brake = (random.randint(0, 2) % 2 == 0)
pump_on = (random.randint(0, 2) % 2 == 0)
pcm_override = (random.randint(0, 2) % 2 == 0)
pcm_cancel_cmd = (random.randint(0, 2) % 2 == 0)
- chime = random.randint(0, 65536)
fcw = random.randint(0, 65536)
idx = random.randint(0, 65536)
- m_old = hondacan.create_brake_command(self.honda_cp_old, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, chime, fcw, idx)
- m = hondacan.create_brake_command(self.honda_cp, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, chime, fcw, idx)
+ m_old = hondacan.create_brake_command(self.honda_cp_old, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, fcw, idx, car_fingerprint, is_panda_black)
+ m = hondacan.create_brake_command(self.honda_cp, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, fcw, idx, car_fingerprint, is_panda_black)
self.assertEqual(m_old, m)
apply_steer = (random.randint(0, 2) % 2 == 0)
lkas_active = (random.randint(0, 2) % 2 == 0)
- car_fingerprint = HONDA_BOSCH[0]
idx = random.randint(0, 65536)
- m_old = hondacan.create_steering_control(self.honda_cp_old, apply_steer, lkas_active, car_fingerprint, idx)
- m = hondacan.create_steering_control(self.honda_cp, apply_steer, lkas_active, car_fingerprint, idx)
+ m_old = hondacan.create_steering_control(self.honda_cp_old, apply_steer, lkas_active, car_fingerprint, idx, is_panda_black)
+ m = hondacan.create_steering_control(self.honda_cp, apply_steer, lkas_active, car_fingerprint, idx, is_panda_black)
self.assertEqual(m_old, m)
pcm_speed = random.randint(0, 65536)
hud = HUDData(random.randint(0, 65536), random.randint(0, 65536), 1, random.randint(0, 65536),
- 0xc1, random.randint(0, 65536), random.randint(0, 65536), random.randint(0, 65536),
- random.randint(0, 65536), random.randint(0, 65536), random.randint(0, 65536))
- car_fingerprint = HONDA_BOSCH[0]
+ 0xc1, random.randint(0, 65536), random.randint(0, 65536), random.randint(0, 65536), random.randint(0, 65536))
idx = random.randint(0, 65536)
is_metric = (random.randint(0, 2) % 2 == 0)
- m_old = hondacan.create_ui_commands(self.honda_cp_old, pcm_speed, hud, car_fingerprint, is_metric, idx)
- m = hondacan.create_ui_commands(self.honda_cp, pcm_speed, hud, car_fingerprint, is_metric, idx)
+ m_old = hondacan.create_ui_commands(self.honda_cp_old, pcm_speed, hud, car_fingerprint, is_metric, idx, is_panda_black)
+ m = hondacan.create_ui_commands(self.honda_cp, pcm_speed, hud, car_fingerprint, is_metric, idx, is_panda_black)
self.assertEqual(m_old, m)
button_val = random.randint(0, 65536)
idx = random.randint(0, 65536)
- m_old = hondacan.spam_buttons_command(self.honda_cp_old, button_val, idx)
- m = hondacan.spam_buttons_command(self.honda_cp, button_val, idx)
+ m_old = hondacan.spam_buttons_command(self.honda_cp_old, button_val, idx, car_fingerprint, is_panda_black)
+ m = hondacan.spam_buttons_command(self.honda_cp, button_val, idx, car_fingerprint, is_panda_black)
self.assertEqual(m_old, m)
diff --git a/selfdrive/can/tests/test_packer_toyota.py b/selfdrive/can/tests/test_packer_toyota.py
index 42e36f317..f5f0e8a7f 100644
--- a/selfdrive/can/tests/test_packer_toyota.py
+++ b/selfdrive/can/tests/test_packer_toyota.py
@@ -47,36 +47,32 @@ class TestPackerMethods(unittest.TestCase):
self.assertEqual(m_old, m)
steer = (random.randint(0, 2) % 2 == 0)
- sound1 = random.randint(1, 65536)
- sound2 = random.randint(1, 65536)
left_line = (random.randint(0, 2) % 2 == 0)
right_line = (random.randint(0, 2) % 2 == 0)
left_lane_depart = (random.randint(0, 2) % 2 == 0)
right_lane_depart = (random.randint(0, 2) % 2 == 0)
- m_old = create_ui_command(self.cp_old, steer, sound1, sound2, left_line, right_line, left_lane_depart, right_lane_depart)
- m = create_ui_command(self.cp, steer, sound1, sound2, left_line, right_line, left_lane_depart, right_lane_depart)
+ m_old = create_ui_command(self.cp_old, steer, left_line, right_line, left_lane_depart, right_lane_depart)
+ m = create_ui_command(self.cp, steer, left_line, right_line, left_lane_depart, right_lane_depart)
self.assertEqual(m_old, m)
def test_performance(self):
n1 = sec_since_boot()
recursions = 100000
steer = (random.randint(0, 2) % 2 == 0)
- sound1 = random.randint(1, 65536)
- sound2 = random.randint(1, 65536)
left_line = (random.randint(0, 2) % 2 == 0)
right_line = (random.randint(0, 2) % 2 == 0)
left_lane_depart = (random.randint(0, 2) % 2 == 0)
right_lane_depart = (random.randint(0, 2) % 2 == 0)
for _ in xrange(recursions):
- create_ui_command(self.cp_old, steer, sound1, sound2, left_line, right_line, left_lane_depart, right_lane_depart)
+ create_ui_command(self.cp_old, steer, left_line, right_line, left_lane_depart, right_lane_depart)
n2 = sec_since_boot()
elapsed_old = n2 - n1
# print('Old API, elapsed time: {} secs'.format(elapsed_old))
n1 = sec_since_boot()
for _ in xrange(recursions):
- create_ui_command(self.cp, steer, sound1, sound2, left_line, right_line, left_lane_depart, right_lane_depart)
+ create_ui_command(self.cp, steer, left_line, right_line, left_lane_depart, right_lane_depart)
n2 = sec_since_boot()
elapsed_new = n2 - n1
# print('New API, elapsed time: {} secs'.format(elapsed_new))
diff --git a/selfdrive/can/tests/test_parser.py b/selfdrive/can/tests/test_parser.py
index bb00d042f..53c95ce91 100755
--- a/selfdrive/can/tests/test_parser.py
+++ b/selfdrive/can/tests/test_parser.py
@@ -47,8 +47,9 @@ def run_route(route):
CP = CarInterface.get_params(CAR.CIVIC, {})
signals, checks = get_can_signals(CP)
- parser_old = CANParserOld(DBC[CP.carFingerprint]['pt'], signals, checks, 0, timeout=-1)
- parser_new = CANParserNew(DBC[CP.carFingerprint]['pt'], signals, checks, 0, timeout=-1)
+ parser_old = CANParserOld(DBC[CP.carFingerprint]['pt'], signals, checks, 0, timeout=-1, tcp_addr="127.0.0.1")
+ parser_new = CANParserNew(DBC[CP.carFingerprint]['pt'], signals, checks, 0, timeout=-1, tcp_addr="127.0.0.1")
+ parser_string = CANParserNew(DBC[CP.carFingerprint]['pt'], signals, checks, 0, timeout=-1)
if dict_keys_differ(parser_old.vl, parser_new.vl):
return False
@@ -61,19 +62,29 @@ def run_route(route):
for msg in lr:
if msg.which() == 'can':
t += DT
- can.send(msg.as_builder().to_bytes())
+ msg_bytes = msg.as_builder().to_bytes()
+ can.send(msg_bytes)
_, updated_old = parser_old.update(t, True)
_, updated_new = parser_new.update(t, True)
+ updated_string = parser_string.update_string(t, msg_bytes)
if updated_old != updated_new:
route_ok = False
print(t, "Diff in seen")
+ if updated_new != updated_string:
+ route_ok = False
+ print(t, "Diff in seen string")
+
if dicts_vals_differ(parser_old.vl, parser_new.vl):
print(t, "Diff in dict")
route_ok = False
+ if dicts_vals_differ(parser_new.vl, parser_string.vl):
+ print(t, "Diff in dict string")
+ route_ok = False
+
return route_ok
class TestCanParser(unittest.TestCase):
diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py
index f7e8c5375..45b9f94e1 100644
--- a/selfdrive/car/car_helpers.py
+++ b/selfdrive/car/car_helpers.py
@@ -1,8 +1,9 @@
import os
-from common.vin import is_vin_response_valid
+from cereal import car
+from common.params import Params
+from common.vin import get_vin, VIN_UNKNOWN
from common.basedir import BASEDIR
from common.fingerprints import eliminate_incompatible_cars, all_known_cars
-from selfdrive.boardd.boardd import can_list_to_can_capnp
from selfdrive.swaglog import cloudlog
import selfdrive.messaging as messaging
@@ -50,102 +51,86 @@ def _get_interface_names():
# imports from directory selfdrive/car//
interfaces = load_interfaces(_get_interface_names())
+def only_toyota_left(candidate_cars):
+ return all(("TOYOTA" in c or "LEXUS" in c) for c in candidate_cars) and len(candidate_cars) > 0
# BOUNTY: every added fingerprint in selfdrive/car/*/values.py is a $100 coupon code on shop.comma.ai
# **** for use live only ****
-def fingerprint(logcan, sendcan):
+def fingerprint(logcan, sendcan, is_panda_black):
if os.getenv("SIMULATOR2") is not None:
return ("simulator2", None, "")
elif os.getenv("SIMULATOR") is not None:
return ("simulator", None, "")
- finger = {}
- cloudlog.warning("waiting for fingerprint...")
- candidate_cars = all_known_cars()
- can_seen_frame = None
- can_seen = False
+ params = Params()
+ car_params = params.get("CarParams")
- # works on standard 11-bit addresses for diagnostic. Tested on Toyota and Subaru;
- # Honda uses the extended 29-bit addresses, and unfortunately only works from OBDII
- vin_query_msg = [[0x7df, 0, '\x02\x09\x02'.ljust(8, "\x00"), 0],
- [0x7e0, 0, '\x30'.ljust(8, "\x00"), 0]]
+ if car_params is not None:
+ # use already stored VIN: a new VIN query cannot be done, since panda isn't in ELM327 mode
+ car_params = car.CarParams.from_bytes(car_params)
+ vin = VIN_UNKNOWN if car_params.carVin == "" else car_params.carVin
+ elif is_panda_black:
+ # Vin query only reliably works thorugh OBDII
+ vin = get_vin(logcan, sendcan, 1)
+ else:
+ vin = VIN_UNKNOWN
- vin_cnts = [1, 2] # number of messages to wait for at each iteration
- vin_step = 0
- vin_cnt = 0
- vin_responded = False
- vin_never_responded = True
- vin_dat = []
- vin = ""
+ cloudlog.warning("VIN %s", vin)
+ Params().put("CarVin", vin)
+ finger = {i: {} for i in range(0, 4)} # collect on all buses
+ candidate_cars = {i: all_known_cars() for i in [0, 1]} # attempt fingerprint on both bus 0 and 1
frame = 0
- while True:
+ frame_fingerprint = 10 # 0.1s
+ car_fingerprint = None
+ done = False
+
+ while not done:
a = messaging.recv_one(logcan)
for can in a.can:
- can_seen = True
+ # need to independently try to fingerprint both bus 0 and 1 to work
+ # for the combo black_panda and honda_bosch. Ignore extended messages
+ # and VIN query response.
+ # Include bus 2 for toyotas to disambiguate cars using camera messages
+ # (ideally should be done for all cars but we can't for Honda Bosch)
+ for b in candidate_cars:
+ if (can.src == b or (only_toyota_left(candidate_cars[b]) and can.src == 2)) and \
+ can.address < 0x800 and can.address not in [0x7df, 0x7e0, 0x7e8]:
+ finger[can.src][can.address] = len(can.dat)
+ candidate_cars[b] = eliminate_incompatible_cars(can, candidate_cars[b])
- # have we got a VIN query response?
- if can.src == 0 and can.address == 0x7e8:
- vin_never_responded = False
- # basic sanity checks on ISO-TP response
- if is_vin_response_valid(can.dat, vin_step, vin_cnt):
- vin_dat += can.dat[2:] if vin_step == 0 else can.dat[1:]
- vin_cnt += 1
- if vin_cnt == vin_cnts[vin_step]:
- vin_responded = True
- vin_step += 1
+ # if we only have one car choice and the time since we got our first
+ # message has elapsed, exit
+ for b in candidate_cars:
+ # Toyota needs higher time to fingerprint, since DSU does not broadcast immediately
+ if only_toyota_left(candidate_cars[b]):
+ frame_fingerprint = 100 # 1s
+ if len(candidate_cars[b]) == 1:
+ if frame > frame_fingerprint:
+ # fingerprint done
+ car_fingerprint = candidate_cars[b][0]
- # ignore everything not on bus 0 and with more than 11 bits,
- # which are ussually sporadic and hard to include in fingerprints.
- # also exclude VIN query response on 0x7e8
- if can.src == 0 and can.address < 0x800 and can.address != 0x7e8:
- finger[can.address] = len(can.dat)
- candidate_cars = eliminate_incompatible_cars(can, candidate_cars)
-
- if can_seen_frame is None and can_seen:
- can_seen_frame = frame
-
- # if we only have one car choice and the time_fingerprint since we got our first
- # message has elapsed, exit. Toyota needs higher time_fingerprint, since DSU does not
- # broadcast immediately
- if len(candidate_cars) == 1 and can_seen_frame is not None:
- time_fingerprint = 1.0 if ("TOYOTA" in candidate_cars[0] or "LEXUS" in candidate_cars[0]) else 0.1
- if (frame - can_seen_frame) > (time_fingerprint * 100):
- break
-
- # bail if no cars left or we've been waiting for more than 2s since can_seen
- elif len(candidate_cars) == 0 or (can_seen_frame is not None and (frame - can_seen_frame) > 200):
- return None, finger, ""
-
- # keep sending VIN qury if ECU isn't responsing.
- # sendcan is probably not ready due to the zmq slow joiner syndrome
- # TODO: VIN query temporarily disabled until we have the harness
- if False and can_seen and (vin_never_responded or (vin_responded and vin_step < len(vin_cnts))):
- sendcan.send(can_list_to_can_capnp([vin_query_msg[vin_step]], msgtype='sendcan'))
- vin_responded = False
- vin_cnt = 0
+ # bail if no cars left or we've been waiting for more than 2s
+ failed = all(len(cc) == 0 for cc in candidate_cars.itervalues()) or frame > 200
+ succeeded = car_fingerprint is not None
+ done = failed or succeeded
frame += 1
- # only report vin if procedure is finished
- if vin_step == len(vin_cnts) and vin_cnt == vin_cnts[-1]:
- vin = "".join(vin_dat[3:])
-
- cloudlog.warning("fingerprinted %s", candidate_cars[0])
- cloudlog.warning("VIN %s", vin)
- return candidate_cars[0], finger, vin
+ cloudlog.warning("fingerprinted %s", car_fingerprint)
+ return car_fingerprint, finger, vin
-def get_car(logcan, sendcan):
+def get_car(logcan, sendcan, is_panda_black=False):
- candidate, fingerprints, vin = fingerprint(logcan, sendcan)
+ candidate, fingerprints, vin = fingerprint(logcan, sendcan, is_panda_black)
if candidate is None:
cloudlog.warning("car doesn't match any fingerprints: %r", fingerprints)
candidate = "mock"
CarInterface, CarController = interfaces[candidate]
- params = CarInterface.get_params(candidate, fingerprints, vin)
+ car_params = CarInterface.get_params(candidate, fingerprints[0], vin, is_panda_black)
- return CarInterface(params, CarController), params
+ return CarInterface(car_params, CarController), car_params
diff --git a/selfdrive/car/chrysler/carcontroller.py b/selfdrive/car/chrysler/carcontroller.py
index eea51f8f6..dac43ca95 100644
--- a/selfdrive/car/chrysler/carcontroller.py
+++ b/selfdrive/car/chrysler/carcontroller.py
@@ -1,14 +1,9 @@
-from cereal import car
from selfdrive.car import apply_toyota_steer_torque_limits
from selfdrive.car.chrysler.chryslercan import create_lkas_hud, create_lkas_command, \
- create_wheel_buttons, \
- create_chimes
+ create_wheel_buttons
from selfdrive.car.chrysler.values import ECU, CAR
from selfdrive.can.packer import CANPacker
-AudibleAlert = car.CarControl.HUDControl.AudibleAlert
-LOUD_ALERTS = [AudibleAlert.chimeWarning1, AudibleAlert.chimeWarning2, AudibleAlert.chimeWarningRepeat]
-
class SteerLimitParams:
STEER_MAX = 261 # 262 faults
STEER_DELTA_UP = 3 # 3 is stock. 100 is fine. 200 is too much it seems
@@ -27,7 +22,6 @@ class CarController(object):
self.hud_count = 0
self.car_fingerprint = car_fingerprint
self.alert_active = False
- self.send_chime = False
self.gone_fast_yet = False
self.fake_ecus = set()
@@ -37,7 +31,7 @@ class CarController(object):
self.packer = CANPacker(dbc_name)
- def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert, audible_alert):
+ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert):
# this seems needed to avoid steering faults and to force the sync with the EPS counter
frame = CS.lkas_counter
if self.prev_frame == frame:
@@ -62,19 +56,10 @@ class CarController(object):
self.apply_steer_last = apply_steer
- if audible_alert in LOUD_ALERTS:
- self.send_chime = True
-
can_sends = []
#*** control msgs ***
- if self.send_chime:
- new_msg = create_chimes(AudibleAlert)
- can_sends.append(new_msg)
- if audible_alert not in LOUD_ALERTS:
- self.send_chime = False
-
if pcm_cancel_cmd:
# TODO: would be better to start from frame_2b3
new_msg = create_wheel_buttons(self.ccframe)
diff --git a/selfdrive/car/chrysler/carstate.py b/selfdrive/car/chrysler/carstate.py
index 1d38b8cda..6368847c8 100644
--- a/selfdrive/car/chrysler/carstate.py
+++ b/selfdrive/car/chrysler/carstate.py
@@ -27,7 +27,7 @@ def get_can_parser(CP):
("DOOR_OPEN_RL", "DOORS", 0),
("DOOR_OPEN_RR", "DOORS", 0),
("BRAKE_PRESSED_2", "BRAKE_2", 0),
- ("ACCEL_PEDAL", "ACCEL_PEDAL_MSG", 0),
+ ("ACCEL_134", "ACCEL_GAS_134", 0),
("SPEED_LEFT", "SPEED_1", 0),
("SPEED_RIGHT", "SPEED_1", 0),
("WHEEL_SPEED_FL", "WHEEL_SPEEDS", 0),
@@ -60,7 +60,7 @@ def get_can_parser(CP):
("ACC_2", 50),
]
- return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0, timeout=100)
+ return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
def get_camera_parser(CP):
signals = [
@@ -72,7 +72,7 @@ def get_camera_parser(CP):
]
checks = []
- return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2, timeout=100)
+ return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)
class CarState(object):
@@ -112,7 +112,7 @@ class CarState(object):
self.seatbelt = (cp.vl["SEATBELT_STATUS"]['SEATBELT_DRIVER_UNLATCHED'] == 0)
self.brake_pressed = cp.vl["BRAKE_2"]['BRAKE_PRESSED_2'] == 5 # human-only
- self.pedal_gas = cp.vl["ACCEL_PEDAL_MSG"]['ACCEL_PEDAL']
+ self.pedal_gas = cp.vl["ACCEL_GAS_134"]['ACCEL_134']
self.car_gas = self.pedal_gas
self.esp_disabled = (cp.vl["TRACTION_BUTTON"]['TRACTION_OFF'] == 1)
diff --git a/selfdrive/car/chrysler/chryslercan.py b/selfdrive/car/chrysler/chryslercan.py
index f803e6094..d710262e9 100644
--- a/selfdrive/car/chrysler/chryslercan.py
+++ b/selfdrive/car/chrysler/chryslercan.py
@@ -2,7 +2,6 @@ from cereal import car
VisualAlert = car.CarControl.HUDControl.VisualAlert
-AudibleAlert = car.CarControl.HUDControl.AudibleAlert
def calc_checksum(data):
"""This function does not want the checksum byte in the input data.
@@ -94,15 +93,6 @@ def create_lkas_command(packer, apply_steer, moving_fast, frame):
return packer.make_can_msg("LKAS_COMMAND", 0, values)
-def create_chimes(audible_alert):
- # '0050' nothing, chime '4f55'
- if audible_alert == AudibleAlert.none:
- msg = '0050'.decode('hex')
- else:
- msg = '4f55'.decode('hex')
- return make_can_msg(0x339, msg)
-
-
def create_wheel_buttons(frame):
# WHEEL_BUTTONS (571) Message sent to cancel ACC.
start = [0x01] # acc cancel set
diff --git a/selfdrive/car/chrysler/chryslercan_test.py b/selfdrive/car/chrysler/chryslercan_test.py
index 4bcb346ba..54b845342 100644
--- a/selfdrive/car/chrysler/chryslercan_test.py
+++ b/selfdrive/car/chrysler/chryslercan_test.py
@@ -3,7 +3,6 @@ from selfdrive.can.packer import CANPacker
from cereal import car
VisualAlert = car.CarControl.HUDControl.VisualAlert
-AudibleAlert = car.CarControl.HUDControl.AudibleAlert
import unittest
diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py
index 793f73208..c43c6c08a 100755
--- a/selfdrive/car/chrysler/interface.py
+++ b/selfdrive/car/chrysler/interface.py
@@ -38,13 +38,14 @@ class CarInterface(object):
return 1.0
@staticmethod
- def get_params(candidate, fingerprint, vin=""):
+ def get_params(candidate, fingerprint, vin="", is_panda_black=False):
ret = car.CarParams.new_message()
ret.carName = "chrysler"
ret.carFingerprint = candidate
ret.carVin = vin
+ ret.isPandaBlack = is_panda_black
ret.safetyModel = car.CarParams.SafetyModel.chrysler
@@ -94,7 +95,7 @@ class CarInterface(object):
ret.brakeMaxBP = [5., 20.]
ret.brakeMaxV = [1., 0.8]
- ret.enableCamera = not check_ecu_msgs(fingerprint, ECU.CAM)
+ ret.enableCamera = not check_ecu_msgs(fingerprint, ECU.CAM) or is_panda_black
print("ECU Camera Simulated: {0}".format(ret.enableCamera))
ret.openpilotLongitudinalControl = False
@@ -112,18 +113,17 @@ class CarInterface(object):
return ret
# returns a car.CarState
- def update(self, c):
+ def update(self, c, can_strings):
# ******************* do can recv *******************
- canMonoTimes = []
- can_rcv_valid, _ = self.cp.update(int(sec_since_boot() * 1e9), True)
- cam_rcv_valid, _ = self.cp_cam.update(int(sec_since_boot() * 1e9), False)
+ self.cp.update_strings(int(sec_since_boot() * 1e9), can_strings)
+ self.cp_cam.update_strings(int(sec_since_boot() * 1e9), can_strings)
self.CS.update(self.cp, self.cp_cam)
# create message
ret = car.CarState.new_message()
- ret.canValid = can_rcv_valid and cam_rcv_valid and self.cp.can_valid and self.cp_cam.can_valid
+ ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
# speeds
ret.vEgo = self.CS.v_ego
@@ -222,7 +222,6 @@ class CarInterface(object):
events.append(create_event('belowSteerSpeed', [ET.WARNING]))
ret.events = events
- ret.canMonoTimes = canMonoTimes
self.gas_pressed_prev = ret.gasPressed
self.brake_pressed_prev = ret.brakePressed
@@ -239,7 +238,6 @@ class CarInterface(object):
self.frame = self.CS.frame
can_sends = self.CC.update(c.enabled, self.CS, self.frame,
- c.actuators, c.cruiseControl.cancel, c.hudControl.visualAlert,
- c.hudControl.audibleAlert)
+ c.actuators, c.cruiseControl.cancel, c.hudControl.visualAlert)
return can_sends
diff --git a/selfdrive/car/chrysler/radar_interface.py b/selfdrive/car/chrysler/radar_interface.py
index b4970b222..43f5c6105 100755
--- a/selfdrive/car/chrysler/radar_interface.py
+++ b/selfdrive/car/chrysler/radar_interface.py
@@ -51,27 +51,24 @@ class RadarInterface(object):
self.pts = {}
self.delay = 0.0 # Delay of radar #TUNE
self.rcp = _create_radar_can_parser()
+ self.updated_messages = set()
+ self.trigger_msg = LAST_MSG
- def update(self):
- canMonoTimes = []
+ def update(self, can_strings):
+ tm = int(sec_since_boot() * 1e9)
+ vls = self.rcp.update_strings(tm, can_strings)
+ self.updated_messages.update(vls)
- updated_messages = set() # set of message IDs (sig_addresses) we've seen
-
- while 1:
- tm = int(sec_since_boot() * 1e9)
- _, vls = self.rcp.update(tm, True)
- updated_messages.update(vls)
- if LAST_MSG in updated_messages:
- break
+ if self.trigger_msg not in self.updated_messages:
+ return None
ret = car.RadarData.new_message()
errors = []
if not self.rcp.can_valid:
errors.append("canError")
ret.errors = errors
- ret.canMonoTimes = canMonoTimes
- for ii in updated_messages: # ii should be the message ID as a number
+ for ii in self.updated_messages: # ii should be the message ID as a number
cpt = self.rcp.vl[ii]
trackId = _address_to_track(ii)
@@ -92,11 +89,6 @@ class RadarInterface(object):
# We want a list, not a dictionary. Filter out LONG_DIST==0 because that means it's not valid.
ret.points = [x for x in self.pts.values() if x.dRel != 0]
- return ret
-if __name__ == "__main__":
- RI = RadarInterface(None)
- while 1:
- ret = RI.update()
- print(chr(27) + "[2J") # clear screen
- print(ret)
+ self.updated_messages.clear()
+ return ret
diff --git a/selfdrive/car/chrysler/values.py b/selfdrive/car/chrysler/values.py
index 188931a06..2e74d7023 100644
--- a/selfdrive/car/chrysler/values.py
+++ b/selfdrive/car/chrysler/values.py
@@ -24,7 +24,7 @@ FINGERPRINTS = {
{168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 515: 7, 516: 7, 517: 7, 518: 7, 520: 8, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 908: 8, 924: 3, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 956: 8, 958: 8, 959: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1225: 8, 1235: 8, 1242: 8, 1246: 8, 1250: 8, 1284: 8, 1537: 8, 1538: 8, 1562: 8, 1568: 8, 1856: 8, 1858: 8, 1860: 8, 1865: 8, 1875: 8, 1882: 8, 1886: 8, 1890: 8, 1892: 8, 2016: 8, 2024: 8},
],
CAR.PACIFICA_2018: [
- {55: 8, 257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 746: 5, 752: 2, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 924: 8, 926: 3, 937: 8, 947: 8, 948: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1098: 8, 1100: 8},
+ {55: 8, 257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 516: 7, 517: 7, 520: 8, 524: 8, 526: 6, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 656: 4, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 746: 5, 752: 2, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 924: 8, 926: 3, 937: 8, 947: 8, 948: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1098: 8, 1100: 8, 1537: 8, 1538: 8, 1562: 8},
],
CAR.PACIFICA_2018_HYBRID: [
{68: 8, 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 660: 8, 669: 3, 671: 8, 672: 8, 680: 8, 701: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 736: 8, 737: 8, 746: 5, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8},
@@ -44,7 +44,7 @@ FINGERPRINTS = {
],
CAR.JEEP_CHEROKEE: [
# JEEP GRAND CHEROKEE V6 2018
- {55: 8, 168: 8, 181: 8, 256: 4, 257: 5, 258: 8, 264: 8, 268: 8, 272: 6, 273: 6, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 656: 4, 658: 6, 660: 8, 671: 8, 672: 8, 676: 8, 678: 8, 680: 8, 683: 8, 684: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 761: 8, 764: 8, 766: 8, 773: 8, 776: 8, 779: 8, 782: 8, 783: 8, 784: 8, 785: 8, 788: 3, 792: 8, 799: 8, 800: 8, 804: 8, 806: 2, 808: 8, 810: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 831: 6, 832: 8, 838: 2, 844: 5, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 906: 8, 924: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 968: 8, 969: 4, 970: 8, 973: 8, 974: 5, 976: 8, 977: 4, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1062: 8, 1098: 8, 1100: 8},
+ {55: 8, 168: 8, 181: 8, 256: 4, 257: 5, 258: 8, 264: 8, 268: 8, 272: 6, 273: 6, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 579: 8, 584: 8, 608: 8, 618: 8, 624: 8, 625: 8, 632: 8, 639: 8, 656: 4, 658: 6, 660: 8, 671: 8, 672: 8, 676: 8, 678: 8, 680: 8, 683: 8, 684: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 761: 8, 764: 8, 766: 8, 773: 8, 776: 8, 779: 8, 782: 8, 783: 8, 784: 8, 785: 8, 788: 3, 792: 8, 799: 8, 800: 8, 804: 8, 806: 2, 808: 8, 810: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 831: 6, 832: 8, 838: 2, 844: 5, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 906: 8, 924: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 956: 8, 968: 8, 969: 4, 970: 8, 973: 8, 974: 5, 976: 8, 977: 4, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1062: 8, 1098: 8, 1100: 8},
# Jeep Grand Cherokee 2017 Trailhawk
{257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 584: 8, 608: 8, 618: 8, 624: 8, 625: 8, 632: 8, 639: 8, 660: 8, 671: 8, 672: 8, 680: 8, 684: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 736: 8, 737: 8, 746: 5, 752: 2, 760: 8, 761: 8, 764: 8, 766: 8, 773: 8, 776: 8, 779: 8, 783: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 806: 2, 808: 8, 810: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 831: 6, 832: 8, 838: 2, 844: 5, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 924: 3, 937: 8, 947: 8, 948: 8, 969: 4, 974: 5, 977: 4, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1062: 8, 1098: 8, 1100: 8},
],
diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py
index dc6d824ff..5e87a2c87 100644
--- a/selfdrive/car/ford/carstate.py
+++ b/selfdrive/car/ford/carstate.py
@@ -29,7 +29,7 @@ def get_can_parser(CP):
checks = [
]
- return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0, timeout=100)
+ return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
class CarState(object):
diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py
index 0f3bbc33c..59acc8432 100755
--- a/selfdrive/car/ford/interface.py
+++ b/selfdrive/car/ford/interface.py
@@ -38,13 +38,14 @@ class CarInterface(object):
return 1.0
@staticmethod
- def get_params(candidate, fingerprint, vin=""):
+ def get_params(candidate, fingerprint, vin="", is_panda_black=False):
ret = car.CarParams.new_message()
ret.carName = "ford"
ret.carFingerprint = candidate
ret.carVin = vin
+ ret.isPandaBlack = is_panda_black
ret.safetyModel = car.CarParams.SafetyModel.ford
@@ -87,7 +88,7 @@ class CarInterface(object):
ret.brakeMaxBP = [5., 20.]
ret.brakeMaxV = [1., 0.8]
- ret.enableCamera = not any(x for x in [970, 973, 984] if x in fingerprint)
+ ret.enableCamera = not any(x for x in [970, 973, 984] if x in fingerprint) or is_panda_black
ret.openpilotLongitudinalControl = False
cloudlog.warn("ECU Camera Simulated: %r", ret.enableCamera)
@@ -105,18 +106,16 @@ class CarInterface(object):
return ret
# returns a car.CarState
- def update(self, c):
+ def update(self, c, can_strings):
# ******************* do can recv *******************
- canMonoTimes = []
-
- can_rcv_valid, _ = self.cp.update(int(sec_since_boot() * 1e9), True)
+ self.cp.update_strings(int(sec_since_boot() * 1e9), can_strings)
self.CS.update(self.cp)
# create message
ret = car.CarState.new_message()
- ret.canValid = can_rcv_valid and self.cp.can_valid
+ ret.canValid = self.cp.can_valid
# speeds
ret.vEgo = self.CS.v_ego
@@ -167,7 +166,6 @@ class CarInterface(object):
events.append(create_event('steerTempUnavailableMute', [ET.WARNING]))
ret.events = events
- ret.canMonoTimes = canMonoTimes
self.gas_pressed_prev = ret.gasPressed
self.brake_pressed_prev = ret.brakePressed
diff --git a/selfdrive/car/ford/radar_interface.py b/selfdrive/car/ford/radar_interface.py
index 08b54723d..04cab9c66 100755
--- a/selfdrive/car/ford/radar_interface.py
+++ b/selfdrive/car/ford/radar_interface.py
@@ -28,28 +28,25 @@ class RadarInterface(object):
# Nidec
self.rcp = _create_radar_can_parser()
+ self.trigger_msg = 0x53f
+ self.updated_messages = set()
- def update(self):
- canMonoTimes = []
+ def update(self, can_strings):
+ tm = int(sec_since_boot() * 1e9)
+ vls = self.rcp.update_strings(tm, can_strings)
+ self.updated_messages.update(vls)
- updated_messages = set()
- while 1:
- tm = int(sec_since_boot() * 1e9)
- _, vls = self.rcp.update(tm, True)
- updated_messages.update(vls)
+ if self.trigger_msg not in self.updated_messages:
+ return None
- # TODO: do not hardcode last msg
- if 0x53f in updated_messages:
- break
ret = car.RadarData.new_message()
errors = []
if not self.rcp.can_valid:
errors.append("canError")
ret.errors = errors
- ret.canMonoTimes = canMonoTimes
- for ii in updated_messages:
+ for ii in self.updated_messages:
cpt = self.rcp.vl[ii]
if cpt['X_Rel'] > 0.00001:
@@ -78,11 +75,5 @@ class RadarInterface(object):
del self.pts[ii]
ret.points = self.pts.values()
+ self.updated_messages.clear()
return ret
-
-if __name__ == "__main__":
- RI = RadarInterface(None)
- while 1:
- ret = RI.update()
- print(chr(27) + "[2J")
- print(ret)
diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py
index a2ffac61f..da820864f 100644
--- a/selfdrive/car/gm/carcontroller.py
+++ b/selfdrive/car/gm/carcontroller.py
@@ -72,7 +72,6 @@ class CarController(object):
def __init__(self, canbus, car_fingerprint):
self.pedal_steady = 0.
self.start_time = 0.
- self.chime = 0
self.steer_idx = 0
self.apply_steer_last = 0
self.car_fingerprint = car_fingerprint
@@ -87,7 +86,7 @@ class CarController(object):
self.packer_ch = CANPacker(DBC[car_fingerprint]['chassis'])
def update(self, enabled, CS, frame, actuators, \
- hud_v_cruise, hud_show_lanes, hud_show_car, chime, chime_cnt, hud_alert):
+ hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert):
P = self.params
@@ -183,21 +182,4 @@ class CarController(object):
can_sends.append(gmcan.create_lka_icon_command(canbus.sw_gmlan, lka_active, lka_critical, steer))
self.lka_icon_status_last = lka_icon_status
- # Send chimes
- if self.chime != chime:
- duration = 0x3c
-
- # There is no 'repeat forever' chime command
- # TODO: Manage periodic re-issuing of chime command
- # and chime cancellation
- if chime_cnt == -1:
- chime_cnt = 10
-
- if chime != 0:
- can_sends.append(gmcan.create_chime_command(canbus.sw_gmlan, chime, duration, chime_cnt))
-
- # If canceling a repeated chime, cancel command must be
- # issued for the same chime type and duration
- self.chime = chime
-
return can_sends
diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py
index 174d11b46..2501598dd 100644
--- a/selfdrive/car/gm/carstate.py
+++ b/selfdrive/car/gm/carstate.py
@@ -47,7 +47,7 @@ def get_powertrain_can_parser(CP, canbus):
("CruiseState", "AcceleratorPedal2", 0),
]
- return CANParser(DBC[CP.carFingerprint]['pt'], signals, [], canbus.powertrain, timeout=100)
+ return CANParser(DBC[CP.carFingerprint]['pt'], signals, [], canbus.powertrain)
class CarState(object):
diff --git a/selfdrive/car/gm/gmcan.py b/selfdrive/car/gm/gmcan.py
index 64fd84f4a..6919de3bd 100644
--- a/selfdrive/car/gm/gmcan.py
+++ b/selfdrive/car/gm/gmcan.py
@@ -130,10 +130,6 @@ def create_adas_accelerometer_speed_status(bus, speed_ms, idx):
def create_adas_headlights_status(bus):
return [0x310, 0, "\x42\x04", bus]
-def create_chime_command(bus, chime_type, duration, repeat_cnt):
- dat = [chime_type, duration, repeat_cnt, 0xff, 0]
- return [0x10400060, 0, "".join(map(chr, dat)), bus]
-
def create_lka_icon_command(bus, active, critical, steer):
if active and steer == 1:
if critical:
diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py
index c066a6523..f5f641836 100755
--- a/selfdrive/car/gm/interface.py
+++ b/selfdrive/car/gm/interface.py
@@ -4,7 +4,7 @@ from common.realtime import sec_since_boot
from selfdrive.config import Conversions as CV
from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET
from selfdrive.controls.lib.vehicle_model import VehicleModel
-from selfdrive.car.gm.values import DBC, CAR, STOCK_CONTROL_MSGS, AUDIO_HUD, \
+from selfdrive.car.gm.values import DBC, CAR, STOCK_CONTROL_MSGS, \
SUPERCRUISE_CARS, AccState
from selfdrive.car.gm.carstate import CarState, CruiseButtons, get_powertrain_can_parser
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness
@@ -46,19 +46,20 @@ class CarInterface(object):
return 1.0
@staticmethod
- def get_params(candidate, fingerprint, vin=""):
+ def get_params(candidate, fingerprint, vin="", is_panda_black=False):
ret = car.CarParams.new_message()
ret.carName = "gm"
ret.carFingerprint = candidate
ret.carVin = vin
+ ret.isPandaBlack = is_panda_black
ret.enableCruise = False
# Presence of a camera on the object bus is ok.
# Have to go to read_only if ASCM is online (ACC-enabled cars),
# or camera is on powertrain bus (LKA cars without ACC).
- ret.enableCamera = not any(x for x in STOCK_CONTROL_MSGS[candidate] if x in fingerprint)
+ ret.enableCamera = not any(x for x in STOCK_CONTROL_MSGS[candidate] if x in fingerprint) or is_panda_black
ret.openpilotLongitudinalControl = ret.enableCamera
tire_stiffness_factor = 0.444 # not optimized yet
@@ -170,15 +171,15 @@ class CarInterface(object):
return ret
# returns a car.CarState
- def update(self, c):
- can_rcv_valid, _ = self.pt_cp.update(int(sec_since_boot() * 1e9), True)
+ def update(self, c, can_strings):
+ self.pt_cp.update_strings(int(sec_since_boot() * 1e9), can_strings)
self.CS.update(self.pt_cp)
# create message
ret = car.CarState.new_message()
- ret.canValid = can_rcv_valid and self.pt_cp.can_valid
+ ret.canValid = self.pt_cp.can_valid
# speeds
ret.vEgo = self.CS.v_ego
@@ -324,8 +325,6 @@ class CarInterface(object):
if hud_v_cruise > 70:
hud_v_cruise = 0
- chime, chime_count = AUDIO_HUD[c.hudControl.audibleAlert.raw]
-
# For Openpilot, "enabled" includes pre-enable.
# In GM, PCM faults out if ACC command overlaps user gas.
enabled = c.enabled and not self.CS.user_gas_pressed
@@ -333,8 +332,7 @@ class CarInterface(object):
can_sends = self.CC.update(enabled, self.CS, self.frame, \
c.actuators,
hud_v_cruise, c.hudControl.lanesVisible, \
- c.hudControl.leadVisible, \
- chime, chime_count, c.hudControl.visualAlert)
+ c.hudControl.leadVisible, c.hudControl.visualAlert)
self.frame += 1
return can_sends
diff --git a/selfdrive/car/gm/radar_interface.py b/selfdrive/car/gm/radar_interface.py
index 12fb7c234..6788e1ce7 100755
--- a/selfdrive/car/gm/radar_interface.py
+++ b/selfdrive/car/gm/radar_interface.py
@@ -52,21 +52,23 @@ class RadarInterface(object):
print "Using %d as obstacle CAN bus ID" % canbus.obstacle
self.rcp = create_radar_can_parser(canbus, CP.carFingerprint)
- def update(self):
- updated_messages = set()
+ self.trigger_msg = LAST_RADAR_MSG
+ self.updated_messages = set()
+
+ def update(self, can_strings):
+ if self.rcp is None:
+ time.sleep(0.05) # nothing to do
+ return car.RadarData.new_message()
+
+ tm = int(sec_since_boot() * 1e9)
+ vls = self.rcp.update_strings(tm, can_strings)
+ self.updated_messages.update(vls)
+
+ if self.trigger_msg not in self.updated_messages:
+ return None
+
+
ret = car.RadarData.new_message()
- while 1:
-
- if self.rcp is None:
- time.sleep(0.05) # nothing to do
- return ret
-
- tm = int(sec_since_boot() * 1e9)
- _, vls = self.rcp.update(tm, True)
- updated_messages.update(vls)
- if LAST_RADAR_MSG in updated_messages:
- break
-
header = self.rcp.vl[RADAR_HEADER_MSG]
fault = header['FLRRSnsrBlckd'] or header['FLRRSnstvFltPrsntInt'] or \
header['FLRRYawRtPlsblityFlt'] or header['FLRRHWFltPrsntInt'] or \
@@ -83,7 +85,7 @@ class RadarInterface(object):
# Not all radar messages describe targets,
# no need to monitor all of the self.rcp.msgs_upd
- for ii in updated_messages:
+ for ii in self.updated_messages:
if ii == RADAR_HEADER_MSG:
continue
@@ -112,11 +114,5 @@ class RadarInterface(object):
del self.pts[oldTarget]
ret.points = self.pts.values()
+ self.updated_messages.clear()
return ret
-
-if __name__ == "__main__":
- RI = RadarInterface(None)
- while 1:
- ret = RI.update()
- print(chr(27) + "[2J")
- print(ret)
diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py
index b41919acb..1aa5a64b1 100644
--- a/selfdrive/car/gm/values.py
+++ b/selfdrive/car/gm/values.py
@@ -1,8 +1,6 @@
from cereal import car
from selfdrive.car import dbc_dict
-AudibleAlert = car.CarControl.HUDControl.AudibleAlert
-
class CAR:
HOLDEN_ASTRA = "HOLDEN ASTRA RS-V BK 2017"
VOLT = "CHEVROLET VOLT PREMIER 2017"
@@ -21,31 +19,12 @@ class CruiseButtons:
MAIN = 5
CANCEL = 6
-# Car chimes, beeps, blinker sounds etc
-class CM:
- TOCK = 0x81
- TICK = 0x82
- LOW_BEEP = 0x84
- HIGH_BEEP = 0x85
- LOW_CHIME = 0x86
- HIGH_CHIME = 0x87
-
class AccState:
OFF = 0
ACTIVE = 1
FAULTED = 3
STANDSTILL = 4
-AUDIO_HUD = {
- AudibleAlert.none: (0, 0),
- AudibleAlert.chimeEngage: (CM.HIGH_CHIME, 1),
- AudibleAlert.chimeDisengage: (CM.HIGH_CHIME, 1),
- AudibleAlert.chimeError: (CM.LOW_CHIME, 2),
- AudibleAlert.chimePrompt: (CM.LOW_CHIME, 1),
- AudibleAlert.chimeWarning1: (CM.LOW_CHIME, 2),
- AudibleAlert.chimeWarning2: (CM.LOW_CHIME, -1),
- AudibleAlert.chimeWarningRepeat: (CM.LOW_CHIME, -1)}
-
def is_eps_status_ok(eps_status, car_fingerprint):
valid_eps_status = []
if car_fingerprint in SUPERCRUISE_CARS:
diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py
index 3a5703e11..8b96e511c 100644
--- a/selfdrive/car/honda/carcontroller.py
+++ b/selfdrive/car/honda/carcontroller.py
@@ -70,7 +70,7 @@ def process_hud_alert(hud_alert):
HUDData = namedtuple("HUDData",
["pcm_accel", "v_cruise", "mini_car", "car", "X4",
- "lanes", "beep", "chime", "fcw", "acc_alert", "steer_required"])
+ "lanes", "fcw", "acc_alert", "steer_required"])
class CarController(object):
@@ -85,8 +85,7 @@ class CarController(object):
def update(self, enabled, CS, frame, actuators, \
pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \
- hud_v_cruise, hud_show_lanes, hud_show_car, \
- hud_alert, snd_beep, snd_chime):
+ hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert):
# *** apply brake hysteresis ***
brake, self.braking, self.brake_steady = actuator_hystereses(actuators.brake, self.braking, self.brake_steady, CS.v_ego, CS.CP.carFingerprint)
@@ -113,15 +112,10 @@ class CarController(object):
else:
hud_car = 0
- # For lateral control-only, send chimes as a beep since we don't send 0x1fa
- if CS.CP.radarOffCan:
- snd_beep = snd_beep if snd_beep != 0 else snd_chime
-
- #print("{0} {1} {2}".format(chime, alert_id, hud_alert))
fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert)
hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), 1, hud_car,
- 0xc1, hud_lanes, int(snd_beep), snd_chime, fcw_display, acc_alert, steer_required)
+ 0xc1, hud_lanes, fcw_display, acc_alert, steer_required)
# **** process the car messages ****
@@ -149,19 +143,19 @@ class CarController(object):
# Send steering command.
idx = frame % 4
can_sends.append(hondacan.create_steering_control(self.packer, apply_steer,
- lkas_active, CS.CP.carFingerprint, idx))
+ lkas_active, CS.CP.carFingerprint, idx, CS.CP.isPandaBlack))
# Send dashboard UI commands.
if (frame % 10) == 0:
idx = (frame//10) % 4
- can_sends.extend(hondacan.create_ui_commands(self.packer, pcm_speed, hud, CS.CP.carFingerprint, CS.is_metric, idx))
+ can_sends.extend(hondacan.create_ui_commands(self.packer, pcm_speed, hud, CS.CP.carFingerprint, CS.is_metric, idx, CS.CP.isPandaBlack))
if CS.CP.radarOffCan:
# If using stock ACC, spam cancel command to kill gas when OP disengages.
if pcm_cancel_cmd:
- can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.CANCEL, idx))
+ can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.CANCEL, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack))
elif CS.stopped:
- can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx))
+ can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack))
else:
# Send gas and brake commands.
@@ -170,7 +164,7 @@ class CarController(object):
ts = frame * DT_CTRL
pump_on, self.last_pump_ts = brake_pump_hysteresis(apply_brake, self.apply_brake_last, self.last_pump_ts, ts)
can_sends.append(hondacan.create_brake_command(self.packer, apply_brake, pump_on,
- pcm_override, pcm_cancel_cmd, hud.chime, hud.fcw, idx))
+ pcm_override, pcm_cancel_cmd, hud.fcw, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack))
self.apply_brake_last = apply_brake
if CS.CP.enableGasInterceptor:
diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py
index 6c0aec52a..abd622d15 100644
--- a/selfdrive/car/honda/carstate.py
+++ b/selfdrive/car/honda/carstate.py
@@ -38,6 +38,7 @@ def get_can_signals(CP):
("STEER_ANGLE", "STEERING_SENSORS", 0),
("STEER_ANGLE_RATE", "STEERING_SENSORS", 0),
("STEER_TORQUE_SENSOR", "STEER_STATUS", 0),
+ ("STEER_TORQUE_MOTOR", "STEER_STATUS", 0),
("LEFT_BLINKER", "SCM_FEEDBACK", 0),
("RIGHT_BLINKER", "SCM_FEEDBACK", 0),
("GEAR", "GEARBOX", 0),
@@ -93,6 +94,7 @@ def get_can_signals(CP):
checks += [("BRAKE_MODULE", 50)]
signals += [("CAR_GAS", "GAS_PEDAL_2", 0),
("MAIN_ON", "SCM_FEEDBACK", 0),
+ ("CRUISE_CONTROL_LABEL", "ACC_HUD", 0),
("EPB_STATE", "EPB_STATUS", 0),
("CRUISE_SPEED", "ACC_HUD", 0)]
checks += [("GAS_PEDAL_2", 100)]
@@ -146,6 +148,7 @@ def get_can_signals(CP):
# add gas interceptor reading if we are using it
if CP.enableGasInterceptor:
signals.append(("INTERCEPTOR_GAS", "GAS_SENSOR", 0))
+ signals.append(("INTERCEPTOR_GAS2", "GAS_SENSOR", 0))
checks.append(("GAS_SENSOR", 50))
return signals, checks
@@ -153,7 +156,8 @@ def get_can_signals(CP):
def get_can_parser(CP):
signals, checks = get_can_signals(CP)
- return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0, timeout=100)
+ bus_pt = 1 if CP.isPandaBlack and CP.carFingerprint in HONDA_BOSCH else 0
+ return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, bus_pt)
def get_cam_can_parser(CP):
@@ -164,9 +168,8 @@ def get_cam_can_parser(CP):
if CP.carFingerprint in [CAR.CRV, CAR.ACURA_RDX, CAR.ODYSSEY_CHN]:
checks = [(0x194, 100)]
- cam_bus = 1 if CP.carFingerprint in HONDA_BOSCH else 2
-
- return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, cam_bus, timeout=100)
+ bus_cam = 1 if CP.carFingerprint in HONDA_BOSCH and not CP.isPandaBlack else 2
+ return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, bus_cam)
class CarState(object):
def __init__(self, CP):
@@ -186,6 +189,7 @@ class CarState(object):
self.left_blinker_on = 0
self.right_blinker_on = 0
+ self.cruise_mode = 0
self.stopped = 0
# vEgo kalman filter
@@ -261,7 +265,7 @@ class CarState(object):
# this is a hack for the interceptor. This is now only used in the simulation
# TODO: Replace tests by toyota so this can go away
if self.CP.enableGasInterceptor:
- self.user_gas = cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS']
+ self.user_gas = (cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS'] + cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS2']) / 2.
self.user_gas_pressed = self.user_gas > 0 # this works because interceptor read < 0 when pedal position is 0. Once calibrated, this will change
self.gear = 0 if self.CP.carFingerprint == CAR.CIVIC else cp.vl["GEARBOX"]['GEAR']
@@ -297,11 +301,13 @@ class CarState(object):
self.car_gas = cp.vl["GAS_PEDAL_2"]['CAR_GAS']
self.steer_torque_driver = cp.vl["STEER_STATUS"]['STEER_TORQUE_SENSOR']
+ self.steer_torque_motor = cp.vl["STEER_STATUS"]['STEER_TORQUE_MOTOR']
self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD[self.CP.carFingerprint]
self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH']
if self.CP.radarOffCan:
+ self.cruise_mode = cp.vl["ACC_HUD"]['CRUISE_CONTROL_LABEL']
self.stopped = cp.vl["ACC_HUD"]['CRUISE_SPEED'] == 252.
self.cruise_speed_offset = calc_cruise_offset(0, self.v_ego)
if self.CP.carFingerprint in (CAR.CIVIC_BOSCH, CAR.ACCORDH, CAR.CRV_HYBRID):
diff --git a/selfdrive/car/honda/hondacan.py b/selfdrive/car/honda/hondacan.py
index 3955bfdcd..723368a92 100644
--- a/selfdrive/car/honda/hondacan.py
+++ b/selfdrive/car/honda/hondacan.py
@@ -19,7 +19,15 @@ def fix(msg, addr):
return msg2
-def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, chime, fcw, idx):
+def get_pt_bus(car_fingerprint, is_panda_black):
+ return 1 if car_fingerprint in HONDA_BOSCH and is_panda_black else 0
+
+
+def get_lkas_cmd_bus(car_fingerprint, is_panda_black):
+ return 2 if car_fingerprint in HONDA_BOSCH and not is_panda_black else 0
+
+
+def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, fcw, idx, car_fingerprint, is_panda_black):
# TODO: do we loose pressure if we keep pump off for long?
brakelights = apply_brake > 0
brake_rq = apply_brake > 0
@@ -32,33 +40,34 @@ def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_
"CRUISE_FAULT_CMD": pcm_fault_cmd,
"CRUISE_CANCEL_CMD": pcm_cancel_cmd,
"COMPUTER_BRAKE_REQUEST": brake_rq,
- "SET_ME_0X80": 0x80,
+ "SET_ME_1": 1,
"BRAKE_LIGHTS": brakelights,
- "CHIME": chime,
+ "CHIME": 0,
# TODO: Why are there two bits for fcw? According to dbc file the first bit should also work
"FCW": fcw << 1,
+ "AEB_REQ_1": 0,
+ "AEB_REQ_2": 0,
+ "AEB": 0,
}
- return packer.make_can_msg("BRAKE_COMMAND", 0, values, idx)
+ bus = get_pt_bus(car_fingerprint, is_panda_black)
+ return packer.make_can_msg("BRAKE_COMMAND", bus, values, idx)
-def create_steering_control(packer, apply_steer, lkas_active, car_fingerprint, idx):
+def create_steering_control(packer, apply_steer, lkas_active, car_fingerprint, idx, is_panda_black):
values = {
"STEER_TORQUE": apply_steer if lkas_active else 0,
"STEER_TORQUE_REQUEST": lkas_active,
}
- # Set bus 2 for accord and new crv.
- bus = 2 if car_fingerprint in HONDA_BOSCH else 0
+ bus = get_lkas_cmd_bus(car_fingerprint, is_panda_black)
return packer.make_can_msg("STEERING_CONTROL", bus, values, idx)
-def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, is_metric, idx):
+def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, is_metric, idx, is_panda_black):
commands = []
- bus = 0
+ bus_pt = get_pt_bus(car_fingerprint, is_panda_black)
+ bus_lkas = get_lkas_cmd_bus(car_fingerprint, is_panda_black)
- # Bosch sends commands to bus 2.
- if car_fingerprint in HONDA_BOSCH:
- bus = 2
- else:
+ if car_fingerprint not in HONDA_BOSCH:
acc_hud_values = {
'PCM_SPEED': pcm_speed * CV.MS_TO_KPH,
'PCM_GAS': hud.pcm_accel,
@@ -70,32 +79,32 @@ def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, is_metric, idx):
'SET_ME_X01_2': 1,
'SET_ME_X01': 1,
}
- commands.append(packer.make_can_msg("ACC_HUD", 0, acc_hud_values, idx))
+ commands.append(packer.make_can_msg("ACC_HUD", bus_pt, acc_hud_values, idx))
lkas_hud_values = {
'SET_ME_X41': 0x41,
'SET_ME_X48': 0x48,
'STEERING_REQUIRED': hud.steer_required,
'SOLID_LANES': hud.lanes,
- 'BEEP': hud.beep,
+ 'BEEP': 0,
}
- commands.append(packer.make_can_msg('LKAS_HUD', bus, lkas_hud_values, idx))
+ commands.append(packer.make_can_msg('LKAS_HUD', bus_lkas, lkas_hud_values, idx))
if car_fingerprint in (CAR.CIVIC, CAR.ODYSSEY):
-
radar_hud_values = {
'ACC_ALERTS': hud.acc_alert,
'LEAD_SPEED': 0x1fe, # What are these magic values
'LEAD_STATE': 0x7,
'LEAD_DISTANCE': 0x1e,
}
- commands.append(packer.make_can_msg('RADAR_HUD', 0, radar_hud_values, idx))
+ commands.append(packer.make_can_msg('RADAR_HUD', bus_pt, radar_hud_values, idx))
return commands
-def spam_buttons_command(packer, button_val, idx):
+def spam_buttons_command(packer, button_val, idx, car_fingerprint, is_panda_black):
values = {
'CRUISE_BUTTONS': button_val,
'CRUISE_SETTING': 0,
}
- return packer.make_can_msg("SCM_BUTTONS", 0, values, idx)
+ bus = get_pt_bus(car_fingerprint, is_panda_black)
+ return packer.make_can_msg("SCM_BUTTONS", bus, values, idx)
diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py
index 93aaf0182..d192f3a34 100755
--- a/selfdrive/car/honda/interface.py
+++ b/selfdrive/car/honda/interface.py
@@ -9,7 +9,7 @@ from selfdrive.config import Conversions as CV
from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET, get_events
from selfdrive.controls.lib.vehicle_model import VehicleModel
from selfdrive.car.honda.carstate import CarState, get_can_parser, get_cam_can_parser
-from selfdrive.car.honda.values import CruiseButtons, CAR, HONDA_BOSCH, AUDIO_HUD, VISUAL_HUD, CAMERA_MSGS
+from selfdrive.car.honda.values import CruiseButtons, CAR, HONDA_BOSCH, VISUAL_HUD, CAMERA_MSGS
from selfdrive.car import STD_CARGO_KG, CivicParams, scale_rot_inertia, scale_tire_stiffness
from selfdrive.controls.lib.planner import _A_CRUISE_MAX_V_FOLLOWING
@@ -129,12 +129,13 @@ class CarInterface(object):
return float(max(max_accel, a_target / A_ACC_MAX)) * min(speedLimiter, accelLimiter)
@staticmethod
- def get_params(candidate, fingerprint, vin=""):
+ def get_params(candidate, fingerprint, vin="", is_panda_black=False):
ret = car.CarParams.new_message()
ret.carName = "honda"
ret.carFingerprint = candidate
ret.carVin = vin
+ ret.isPandaBlack = is_panda_black
if candidate in HONDA_BOSCH:
ret.safetyModel = car.CarParams.SafetyModel.hondaBosch
@@ -143,7 +144,7 @@ class CarInterface(object):
ret.openpilotLongitudinalControl = False
else:
ret.safetyModel = car.CarParams.SafetyModel.honda
- ret.enableCamera = not any(x for x in CAMERA_MSGS if x in fingerprint)
+ ret.enableCamera = not any(x for x in CAMERA_MSGS if x in fingerprint) or is_panda_black
ret.enableGasInterceptor = 0x201 in fingerprint
ret.openpilotLongitudinalControl = ret.enableCamera
@@ -167,10 +168,10 @@ class CarInterface(object):
ret.mass = CivicParams.MASS
ret.wheelbase = CivicParams.WHEELBASE
ret.centerToFront = CivicParams.CENTER_TO_FRONT
- ret.steerRatio = 14.63 # 10.93 is end-to-end spec
+ ret.steerRatio = 15.38 # 10.93 is end-to-end spec
tire_stiffness_factor = 1.
# Civic at comma has modified steering FW, so different tuning for the Neo in that car
- is_fw_modified = os.getenv("DONGLE_ID") in ['99c94dc769b5d96e']
+ is_fw_modified = os.getenv("DONGLE_ID") in ['5b7c365c50084530']
if is_fw_modified:
ret.lateralTuning.pid.kf = 0.00004
@@ -187,7 +188,7 @@ class CarInterface(object):
ret.mass = 3279. * CV.LB_TO_KG + STD_CARGO_KG
ret.wheelbase = 2.83
ret.centerToFront = ret.wheelbase * 0.39
- ret.steerRatio = 15.96 # 11.82 is spec end-to-end
+ ret.steerRatio = 16.33 # 11.82 is spec end-to-end
tire_stiffness_factor = 0.8467
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]]
ret.longitudinalTuning.kpBP = [0., 5., 35.]
@@ -213,7 +214,7 @@ class CarInterface(object):
ret.mass = 3572. * CV.LB_TO_KG + STD_CARGO_KG
ret.wheelbase = 2.62
ret.centerToFront = ret.wheelbase * 0.41
- ret.steerRatio = 15.3 # as spec
+ ret.steerRatio = 16.89 # as spec
tire_stiffness_factor = 0.444 # not optimized yet
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]]
ret.longitudinalTuning.kpBP = [0., 5., 35.]
@@ -293,7 +294,7 @@ class CarInterface(object):
ret.mass = 4204. * CV.LB_TO_KG + STD_CARGO_KG # average weight
ret.wheelbase = 2.82
ret.centerToFront = ret.wheelbase * 0.428 # average weight distribution
- ret.steerRatio = 16.0 # as spec
+ ret.steerRatio = 17.25 # as spec
tire_stiffness_factor = 0.444 # not optimized yet
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.38], [0.11]]
ret.longitudinalTuning.kpBP = [0., 5., 35.]
@@ -358,18 +359,17 @@ class CarInterface(object):
return ret
# returns a car.CarState
- def update(self, c):
+ def update(self, c, can_strings):
# ******************* do can recv *******************
- canMonoTimes = []
- can_rcv_valid, _ = self.cp.update(int(sec_since_boot() * 1e9), True)
- cam_rcv_valid, _ = self.cp_cam.update(int(sec_since_boot() * 1e9), False)
+ self.cp.update_strings(int(sec_since_boot() * 1e9), can_strings)
+ self.cp_cam.update_strings(int(sec_since_boot() * 1e9), can_strings)
self.CS.update(self.cp, self.cp_cam)
# create message
ret = car.CarState.new_message()
- ret.canValid = can_rcv_valid and cam_rcv_valid and self.cp.can_valid
+ ret.canValid = self.cp.can_valid
# speeds
ret.vEgo = self.CS.v_ego
@@ -405,12 +405,13 @@ class CarInterface(object):
ret.gearShifter = self.CS.gear_shifter
ret.steeringTorque = self.CS.steer_torque_driver
+ ret.steeringTorqueEps = self.CS.steer_torque_motor
ret.steeringPressed = self.CS.steer_override
# cruise state
ret.cruiseState.enabled = self.CS.pcm_acc_status != 0
ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS
- ret.cruiseState.available = bool(self.CS.main_on)
+ ret.cruiseState.available = bool(self.CS.main_on) and not bool(self.CS.cruise_mode)
ret.cruiseState.speedOffset = self.CS.cruise_speed_offset
ret.cruiseState.standstill = False
@@ -487,7 +488,7 @@ class CarInterface(object):
events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if self.CS.esp_disabled:
events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
- if not self.CS.main_on:
+ if not self.CS.main_on or self.CS.cruise_mode:
events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.gearShifter == 'reverse':
events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
@@ -547,7 +548,6 @@ class CarInterface(object):
events.append(create_event('buttonEnable', [ET.ENABLE]))
ret.events = events
- ret.canMonoTimes = canMonoTimes
# update previous brake/gas pressed
self.gas_pressed_prev = ret.gasPressed
@@ -565,7 +565,6 @@ class CarInterface(object):
hud_v_cruise = 255
hud_alert = VISUAL_HUD[c.hudControl.visualAlert.raw]
- snd_beep, snd_chime = AUDIO_HUD[c.hudControl.audibleAlert.raw]
pcm_accel = int(clip(c.cruiseControl.accelOverride, 0, 1) * 0xc6)
@@ -578,9 +577,7 @@ class CarInterface(object):
hud_v_cruise,
c.hudControl.lanesVisible,
hud_show_car=c.hudControl.leadVisible,
- hud_alert=hud_alert,
- snd_beep=snd_beep,
- snd_chime=snd_chime)
+ hud_alert=hud_alert)
self.frame += 1
return can_sends
diff --git a/selfdrive/car/honda/radar_interface.py b/selfdrive/car/honda/radar_interface.py
index 94e98cf2c..f8cecd6de 100755
--- a/selfdrive/car/honda/radar_interface.py
+++ b/selfdrive/car/honda/radar_interface.py
@@ -31,25 +31,30 @@ class RadarInterface(object):
# Nidec
self.rcp = _create_nidec_can_parser()
+ self.trigger_msg = 0x445
+ self.updated_messages = set()
- def update(self):
- canMonoTimes = []
-
- updated_messages = set()
- ret = car.RadarData.new_message()
-
+ def update(self, can_strings):
# in Bosch radar and we are only steering for now, so sleep 0.05s to keep
# radard at 20Hz and return no points
if self.radar_off_can:
time.sleep(0.05)
- return ret
+ return car.RadarData.new_message()
- while 1:
- tm = int(sec_since_boot() * 1e9)
- _, vls = self.rcp.update(tm, True)
- updated_messages.update(vls)
- if 0x445 in updated_messages:
- break
+ tm = int(sec_since_boot() * 1e9)
+ vls = self.rcp.update_strings(tm, can_strings)
+ self.updated_messages.update(vls)
+
+ if self.trigger_msg not in self.updated_messages:
+ return None
+
+ rr = self._update(self.updated_messages)
+ self.updated_messages.clear()
+ return rr
+
+
+ def _update(self, updated_messages):
+ ret = car.RadarData.new_message()
for ii in updated_messages:
cpt = self.rcp.vl[ii]
@@ -80,19 +85,7 @@ class RadarInterface(object):
if self.radar_wrong_config:
errors.append("wrongConfig")
ret.errors = errors
- ret.canMonoTimes = canMonoTimes
ret.points = self.pts.values()
return ret
-
-
-if __name__ == "__main__":
- class CarParams:
- radarOffCan = False
-
- RI = RadarInterface(CarParams)
- while 1:
- ret = RI.update()
- print(chr(27) + "[2J")
- print(ret)
diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py
index 8b4d13bf7..c7acaffc8 100644
--- a/selfdrive/car/honda/values.py
+++ b/selfdrive/car/honda/values.py
@@ -1,7 +1,6 @@
from cereal import car
from selfdrive.car import dbc_dict
-AudibleAlert = car.CarControl.HUDControl.AudibleAlert
VisualAlert = car.CarControl.HUDControl.VisualAlert
# Car button codes
@@ -11,31 +10,6 @@ class CruiseButtons:
CANCEL = 2
MAIN = 1
-#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 beeps: enumeration from dbc file. Beeps are for engage and disengage
-class BP:
- MUTE = 0
- SINGLE = 3
- TRIPLE = 2
- REPEATED = 1
-
-AUDIO_HUD = {
- AudibleAlert.none: (BP.MUTE, CM.MUTE),
- AudibleAlert.chimeEngage: (BP.SINGLE, CM.MUTE),
- AudibleAlert.chimeDisengage: (BP.SINGLE, CM.MUTE),
- AudibleAlert.chimeError: (BP.MUTE, CM.DOUBLE),
- AudibleAlert.chimePrompt: (BP.MUTE, CM.SINGLE),
- AudibleAlert.chimeWarning1: (BP.MUTE, CM.DOUBLE),
- AudibleAlert.chimeWarning2: (BP.MUTE, CM.REPEATED),
- AudibleAlert.chimeWarningRepeat: (BP.MUTE, CM.REPEATED)}
-
class AH:
#[alert_idx, value]
# See dbc files for info on values"
@@ -73,6 +47,9 @@ class CAR:
PILOT_2019 = "HONDA PILOT 2019 ELITE"
RIDGELINE = "HONDA RIDGELINE 2017 BLACK EDITION"
+# diag message that in some Nidec cars only appear with 1s freq if VIN query is performed
+DIAG_MSGS = {1600: 5, 1601: 8}
+
FINGERPRINTS = {
CAR.ACCORD: [{
148: 8, 228: 5, 304: 8, 330: 8, 344: 8, 380: 8, 399: 7, 419: 8, 420: 8, 427: 3, 432: 7, 441: 5, 446: 3, 450: 8, 464: 8, 477: 8, 479: 8, 495: 8, 545: 6, 662: 4, 773: 7, 777: 8, 780: 8, 804: 8, 806: 8, 808: 8, 829: 5, 862: 8, 884: 8, 891: 8, 927: 8, 929: 8, 1302: 8, 1600: 5, 1601: 8, 1652: 8
@@ -139,6 +116,12 @@ FINGERPRINTS = {
}]
}
+# add DIAG_MSGS to fingerprints
+for c in FINGERPRINTS:
+ for f, _ in enumerate(FINGERPRINTS[c]):
+ for d in DIAG_MSGS:
+ FINGERPRINTS[c][f][d] = DIAG_MSGS[d]
+
DBC = {
CAR.ACCORD: dbc_dict('honda_accord_s2t_2018_can_generated', None),
CAR.ACCORD_15: dbc_dict('honda_accord_lx15t_2018_can_generated', None),
diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py
index 8c900d73f..f993bd2a0 100644
--- a/selfdrive/car/hyundai/carstate.py
+++ b/selfdrive/car/hyundai/carstate.py
@@ -93,7 +93,7 @@ def get_can_parser(CP):
("SAS11", 100)
]
- return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0, timeout=100)
+ return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
def get_camera_parser(CP):
@@ -119,7 +119,7 @@ def get_camera_parser(CP):
checks = []
- return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2, timeout=100)
+ return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)
class CarState(object):
diff --git a/selfdrive/car/hyundai/hyundaican.py b/selfdrive/car/hyundai/hyundaican.py
index 5a895497a..d9e7cf3a9 100644
--- a/selfdrive/car/hyundai/hyundaican.py
+++ b/selfdrive/car/hyundai/hyundaican.py
@@ -8,7 +8,7 @@ def make_can_msg(addr, dat, alt):
def create_lkas11(packer, car_fingerprint, apply_steer, steer_req, cnt, enabled, lkas11, hud_alert, keep_stock=False):
values = {
- "CF_Lkas_Icon": 3 if enabled else 0,
+ "CF_Lkas_Bca_R": 3 if enabled else 0,
"CF_Lkas_LdwsSysState": 3 if steer_req else 1,
"CF_Lkas_SysWarning": hud_alert,
"CF_Lkas_LdwsLHWarning": lkas11["CF_Lkas_LdwsLHWarning"] if keep_stock else 0,
diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py
index 67ca2261f..be92371f1 100644
--- a/selfdrive/car/hyundai/interface.py
+++ b/selfdrive/car/hyundai/interface.py
@@ -40,13 +40,14 @@ class CarInterface(object):
return 1.0
@staticmethod
- def get_params(candidate, fingerprint, vin=""):
+ def get_params(candidate, fingerprint, vin="", is_panda_black=False):
ret = car.CarParams.new_message()
ret.carName = "hyundai"
ret.carFingerprint = candidate
ret.carVin = vin
+ ret.isPandaBlack = is_panda_black
ret.radarOffCan = True
ret.safetyModel = car.CarParams.SafetyModel.hyundai
ret.enableCruise = True # stock acc
@@ -141,7 +142,7 @@ class CarInterface(object):
ret.brakeMaxBP = [0.]
ret.brakeMaxV = [1.]
- ret.enableCamera = not any(x for x in CAMERA_MSGS if x in fingerprint)
+ ret.enableCamera = not any(x for x in CAMERA_MSGS if x in fingerprint) or is_panda_black
ret.openpilotLongitudinalControl = False
ret.steerLimitAlert = False
@@ -151,17 +152,16 @@ class CarInterface(object):
return ret
# returns a car.CarState
- def update(self, c):
+ def update(self, c, can_strings):
# ******************* do can recv *******************
- canMonoTimes = []
- can_rcv_valid, _ = self.cp.update(int(sec_since_boot() * 1e9), True)
- cam_rcv_valid, _ = self.cp_cam.update(int(sec_since_boot() * 1e9), False)
+ self.cp.update_strings(int(sec_since_boot() * 1e9), can_strings)
+ self.cp_cam.update_strings(int(sec_since_boot() * 1e9), can_strings)
self.CS.update(self.cp, self.cp_cam)
# create message
ret = car.CarState.new_message()
- ret.canValid = can_rcv_valid and cam_rcv_valid and self.cp.can_valid # TODO: check cp_cam validity
+ ret.canValid = self.cp.can_valid # TODO: check cp_cam validity
# speeds
ret.vEgo = self.CS.v_ego
@@ -269,7 +269,6 @@ class CarInterface(object):
events.append(create_event('belowSteerSpeed', [ET.WARNING]))
ret.events = events
- ret.canMonoTimes = canMonoTimes
self.gas_pressed_prev = ret.gasPressed
self.brake_pressed_prev = ret.brakePressed
@@ -279,7 +278,7 @@ class CarInterface(object):
def apply(self, c):
- hud_alert = get_hud_alerts(c.hudControl.visualAlert, c.hudControl.audibleAlert)
+ hud_alert = get_hud_alerts(c.hudControl.visualAlert)
can_sends = self.CC.update(c.enabled, self.CS, c.actuators,
c.cruiseControl.cancel, hud_alert)
diff --git a/selfdrive/car/hyundai/radar_interface.py b/selfdrive/car/hyundai/radar_interface.py
index 75256683d..1d7772fd3 100644
--- a/selfdrive/car/hyundai/radar_interface.py
+++ b/selfdrive/car/hyundai/radar_interface.py
@@ -9,16 +9,8 @@ class RadarInterface(object):
self.pts = {}
self.delay = 0.1
- def update(self):
-
+ def update(self, can_strings):
ret = car.RadarData.new_message()
time.sleep(0.05) # radard runs on RI updates
return ret
-
-if __name__ == "__main__":
- RI = RadarInterface(None)
- while 1:
- ret = RI.update()
- print(chr(27) + "[2J")
- print(ret)
diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py
index 1756976e9..6380ca446 100644
--- a/selfdrive/car/hyundai/values.py
+++ b/selfdrive/car/hyundai/values.py
@@ -2,11 +2,10 @@ from cereal import car
from selfdrive.car import dbc_dict
VisualAlert = car.CarControl.HUDControl.VisualAlert
-AudibleAlert = car.CarControl.HUDControl.AudibleAlert
-def get_hud_alerts(visual_alert, audible_alert):
+def get_hud_alerts(visual_alert):
if visual_alert == VisualAlert.steerRequired:
- return 4 if audible_alert != AudibleAlert.none else 5
+ return 5
else:
return 0
diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py
index a18d2bf24..b703776c0 100755
--- a/selfdrive/car/mock/interface.py
+++ b/selfdrive/car/mock/interface.py
@@ -4,7 +4,6 @@ from selfdrive.config import Conversions as CV
from selfdrive.services import service_list
from selfdrive.swaglog import cloudlog
import selfdrive.messaging as messaging
-from common.realtime import Ratekeeper
# mocked car interface to work with chffrplus
TS = 0.01 # 100Hz
@@ -30,8 +29,6 @@ class CarInterface(object):
self.yaw_rate = 0.
self.yaw_rate_meas = 0.
- self.rk = Ratekeeper(100, print_delay_threshold=2. / 1000)
-
@staticmethod
def compute_gb(accel, speed):
return accel
@@ -41,7 +38,7 @@ class CarInterface(object):
return 1.0
@staticmethod
- def get_params(candidate, fingerprint, vin=""):
+ def get_params(candidate, fingerprint, vin="", is_panda_black=False):
ret = car.CarParams.new_message()
@@ -80,9 +77,7 @@ class CarInterface(object):
return ret
# returns a car.CarState
- def update(self, c):
- self.rk.keep_time()
-
+ def update(self, c, can_strings):
# get basic data from phone and gps since CAN isn't connected
sensors = messaging.recv_sock(self.sensor)
if sensors is not None:
diff --git a/selfdrive/car/mock/radar_interface.py b/selfdrive/car/mock/radar_interface.py
index 437bb0538..8e5f7b7fc 100755
--- a/selfdrive/car/mock/radar_interface.py
+++ b/selfdrive/car/mock/radar_interface.py
@@ -9,15 +9,7 @@ class RadarInterface(object):
self.pts = {}
self.delay = 0.1
- def update(self):
-
+ def update(self, can_strings):
ret = car.RadarData.new_message()
time.sleep(0.05) # radard runs on RI updates
return ret
-
-if __name__ == "__main__":
- RI = RadarInterface(None)
- while 1:
- ret = RI.update()
- print(chr(27) + "[2J")
- print(ret)
diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py
index 9a9277754..6c5f67826 100644
--- a/selfdrive/car/subaru/carstate.py
+++ b/selfdrive/car/subaru/carstate.py
@@ -37,7 +37,7 @@ def get_powertrain_can_parser(CP):
("BodyInfo", 10),
]
- return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0, timeout=100)
+ return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
def get_camera_can_parser(CP):
@@ -79,7 +79,7 @@ def get_camera_can_parser(CP):
("ES_DashStatus", 10),
]
- return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2, timeout=100)
+ return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)
class CarState(object):
diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py
index e9d9c117f..c5fe0062a 100644
--- a/selfdrive/car/subaru/interface.py
+++ b/selfdrive/car/subaru/interface.py
@@ -38,12 +38,13 @@ class CarInterface(object):
return 1.0
@staticmethod
- def get_params(candidate, fingerprint, vin=""):
+ def get_params(candidate, fingerprint, vin="", is_panda_black=False):
ret = car.CarParams.new_message()
ret.carName = "subaru"
ret.carFingerprint = candidate
ret.carVin = vin
+ ret.isPandaBlack = is_panda_black
ret.safetyModel = car.CarParams.SafetyModel.subaru
ret.enableCruise = True
@@ -94,16 +95,16 @@ class CarInterface(object):
return ret
# returns a car.CarState
- def update(self, c):
- can_rcv_valid, _ = self.pt_cp.update(int(sec_since_boot() * 1e9), True)
- cam_rcv_valid, _ = self.cam_cp.update(int(sec_since_boot() * 1e9), False)
+ def update(self, c, can_strings):
+ self.pt_cp.update_strings(int(sec_since_boot() * 1e9), can_strings)
+ self.cam_cp.update_strings(int(sec_since_boot() * 1e9), can_strings)
self.CS.update(self.pt_cp, self.cam_cp)
# create message
ret = car.CarState.new_message()
- ret.canValid = can_rcv_valid and cam_rcv_valid and self.pt_cp.can_valid and self.cam_cp.can_valid
+ ret.canValid = self.pt_cp.can_valid and self.cam_cp.can_valid
# speeds
ret.vEgo = self.CS.v_ego
diff --git a/selfdrive/car/subaru/radar_interface.py b/selfdrive/car/subaru/radar_interface.py
index 75256683d..0f8108771 100644
--- a/selfdrive/car/subaru/radar_interface.py
+++ b/selfdrive/car/subaru/radar_interface.py
@@ -9,16 +9,9 @@ class RadarInterface(object):
self.pts = {}
self.delay = 0.1
- def update(self):
+ def update(self, can_strings):
ret = car.RadarData.new_message()
time.sleep(0.05) # radard runs on RI updates
return ret
-
-if __name__ == "__main__":
- RI = RadarInterface(None)
- while 1:
- ret = RI.update()
- print(chr(27) + "[2J")
- print(ret)
diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index c6ffe0ea1..28ea8cf6e 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -10,7 +10,6 @@ from selfdrive.car.toyota.values import ECU, STATIC_MSGS, TSS2_CAR
from selfdrive.can.packer import CANPacker
VisualAlert = car.CarControl.HUDControl.VisualAlert
-AudibleAlert = car.CarControl.HUDControl.AudibleAlert
# Accel limits
ACCEL_HYST_GAP = 0.02 # don't change accel command for small oscilalitons within this value
@@ -53,25 +52,17 @@ def accel_hysteresis(accel, accel_steady, enabled):
return accel, accel_steady
-def process_hud_alert(hud_alert, audible_alert):
+def process_hud_alert(hud_alert):
# initialize to no alert
steer = 0
fcw = 0
- sound1 = 0
- sound2 = 0
if hud_alert == VisualAlert.fcw:
fcw = 1
elif hud_alert == VisualAlert.steerRequired:
steer = 1
- if audible_alert == AudibleAlert.chimeWarningRepeat:
- sound1 = 1
- elif audible_alert != AudibleAlert.none:
- # TODO: find a way to send single chimes
- sound2 = 1
-
- return steer, fcw, sound1, sound2
+ return steer, fcw
def ipas_state_transition(steer_angle_enabled, enabled, ipas_active, ipas_reset_counter):
@@ -124,8 +115,8 @@ class CarController(object):
self.packer = CANPacker(dbc_name)
def update(self, enabled, CS, frame, actuators,
- pcm_cancel_cmd, hud_alert, audible_alert, forwarding_camera,
- left_line, right_line, lead, left_lane_depart, right_lane_depart):
+ pcm_cancel_cmd, hud_alert, forwarding_camera, left_line,
+ right_line, lead, left_lane_depart, right_lane_depart):
# *** compute control surfaces ***
@@ -235,8 +226,8 @@ class CarController(object):
# ui mesg is at 100Hz but we send asap if:
# - there is something to display
# - there is something to stop displaying
- alert_out = process_hud_alert(hud_alert, audible_alert)
- steer, fcw, sound1, sound2 = alert_out
+ alert_out = process_hud_alert(hud_alert)
+ steer, fcw = alert_out
if (any(alert_out) and not self.alert_active) or \
(not any(alert_out) and self.alert_active):
@@ -246,7 +237,7 @@ class CarController(object):
send_ui = False
if (frame % 100 == 0 or send_ui) and ECU.CAM in self.fake_ecus:
- can_sends.append(create_ui_command(self.packer, steer, sound1, sound2, left_line, right_line, left_lane_depart, right_lane_depart))
+ can_sends.append(create_ui_command(self.packer, steer, left_line, right_line, left_lane_depart, right_lane_depart))
if frame % 100 == 0 and ECU.DSU in self.fake_ecus and self.car_fingerprint not in TSS2_CAR:
can_sends.append(create_fcw_command(self.packer, fcw))
diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
index 8ed5bccf5..330102ea9 100644
--- a/selfdrive/car/toyota/carstate.py
+++ b/selfdrive/car/toyota/carstate.py
@@ -1,9 +1,9 @@
import numpy as np
from common.kalman.simple_kalman import KF1D
-from selfdrive.can.parser import CANParser
from selfdrive.can.can_define import CANDefine
+from selfdrive.can.parser import CANParser
from selfdrive.config import Conversions as CV
-from selfdrive.car.toyota.values import CAR, DBC, STEER_THRESHOLD, NO_DSU_CAR
+from selfdrive.car.toyota.values import CAR, DBC, STEER_THRESHOLD, TSS2_CAR, NO_DSU_CAR
def parse_gear_shifter(gear, vals):
@@ -19,6 +19,7 @@ def get_can_parser(CP):
signals = [
# sig_name, sig_address, default
+ ("STEER_ANGLE", "STEER_ANGLE_SENSOR", 0),
("GEAR", "GEAR_PACKET", 0),
("BRAKE_PRESSED", "BRAKE_MODULE", 0),
("GAS_PEDAL", "GAS_PEDAL", 0),
@@ -53,8 +54,6 @@ def get_can_parser(CP):
if CP.carFingerprint in NO_DSU_CAR:
signals += [("STEER_ANGLE", "STEER_TORQUE_SENSOR", 0)]
- else:
- signals += [("STEER_ANGLE", "STEER_ANGLE_SENSOR", 0)]
if CP.carFingerprint == CAR.LEXUS_ISH:
checks += [
@@ -88,9 +87,10 @@ def get_can_parser(CP):
# add gas interceptor reading if we are using it
if CP.enableGasInterceptor:
signals.append(("INTERCEPTOR_GAS", "GAS_SENSOR", 0))
+ signals.append(("INTERCEPTOR_GAS2", "GAS_SENSOR", 0))
checks.append(("GAS_SENSOR", 50))
- return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0, timeout=100)
+ return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
def get_cam_can_parser(CP):
@@ -100,7 +100,7 @@ def get_cam_can_parser(CP):
# use steering message to check if panda is connected to frc
checks = [("STEERING_LKA", 42)]
- return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2, timeout=100)
+ return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)
class CarState(object):
@@ -111,6 +111,8 @@ class CarState(object):
self.shifter_values = self.can_define.dv["GEAR_PACKET"]['GEAR']
self.left_blinker_on = 0
self.right_blinker_on = 0
+ self.angle_offset = 0.
+ self.init_angle_offset = False
# initialize can parser
self.car_fingerprint = CP.carFingerprint
@@ -136,7 +138,7 @@ class CarState(object):
self.brake_pressed = cp.vl["BRAKE_MODULE"]['BRAKE_PRESSED']
if self.CP.enableGasInterceptor:
- self.pedal_gas = cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS']
+ self.pedal_gas = (cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS'] + cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS2']) / 2.
else:
self.pedal_gas = cp.vl["GAS_PEDAL"]['GAS_PEDAL']
self.car_gas = self.pedal_gas
@@ -159,8 +161,16 @@ class CarState(object):
self.a_ego = float(v_ego_x[1])
self.standstill = not v_wheel > 0.001
- if self.CP.carFingerprint in NO_DSU_CAR:
+ if self.CP.carFingerprint in TSS2_CAR:
self.angle_steers = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE']
+ elif self.CP.carFingerprint in NO_DSU_CAR:
+ # cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] is zeroed to where the steering angle is at start.
+ # need to apply an offset as soon as the steering angle measurements are both received
+ self.angle_steers = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] - self.angle_offset
+ angle_wheel = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
+ if abs(angle_wheel) > 1e-3 and abs(self.angle_steers) > 1e-3 and not self.init_angle_offset:
+ self.init_angle_offset = True
+ self.angle_offset = self.angle_steers - angle_wheel
else:
self.angle_steers = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
self.angle_steers_rate = cp.vl["STEER_ANGLE_SENSOR"]['STEER_RATE']
diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index 5f303e6c1..624f773f2 100755
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -9,7 +9,6 @@ from selfdrive.car.toyota.values import ECU, check_ecu_msgs, CAR, NO_STOP_TIMER_
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness
from selfdrive.swaglog import cloudlog
-
class CarInterface(object):
def __init__(self, CP, CarController):
self.CP = CP
@@ -41,13 +40,14 @@ class CarInterface(object):
return 1.0
@staticmethod
- def get_params(candidate, fingerprint, vin=""):
+ def get_params(candidate, fingerprint, vin="", is_panda_black=False):
ret = car.CarParams.new_message()
ret.carName = "toyota"
ret.carFingerprint = candidate
ret.carVin = vin
+ ret.isPandaBlack = is_panda_black
ret.safetyModel = car.CarParams.SafetyModel.toyota
@@ -55,7 +55,8 @@ class CarInterface(object):
ret.enableCruise = not ret.enableGasInterceptor
ret.steerActuatorDelay = 0.12 # Default delay, Prius has larger delay
- if candidate != CAR.PRIUS:
+
+ if candidate not in [CAR.PRIUS, CAR.RAV4, CAR.RAV4H]: # These cars use LQR/INDI
ret.lateralTuning.init('pid')
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
@@ -63,7 +64,7 @@ class CarInterface(object):
stop_and_go = True
ret.safetyParam = 66 # see conversion factor for STEER_TORQUE_EPS in dbc file
ret.wheelbase = 2.70
- ret.steerRatio = 15.00 # unknown end-to-end spec
+ ret.steerRatio = 15.74 # unknown end-to-end spec
tire_stiffness_factor = 0.6371 # hand-tune
ret.mass = 3045. * CV.LB_TO_KG + STD_CARGO_KG
@@ -73,23 +74,44 @@ class CarInterface(object):
ret.lateralTuning.indi.timeConstant = 1.0
ret.lateralTuning.indi.actuatorEffectiveness = 1.0
+ # TODO: Determine if this is better than INDI
+ # ret.lateralTuning.init('lqr')
+ # ret.lateralTuning.lqr.scale = 1500.0
+ # ret.lateralTuning.lqr.ki = 0.01
+
+ # ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268]
+ # ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05]
+ # ret.lateralTuning.lqr.c = [1., 0.]
+ # ret.lateralTuning.lqr.k = [-110.73572306, 451.22718255]
+ # ret.lateralTuning.lqr.l = [0.03233671, 0.03185757]
+ # ret.lateralTuning.lqr.dcGain = 0.002237852961363602
+
ret.steerActuatorDelay = 0.5
elif candidate in [CAR.RAV4, CAR.RAV4H]:
stop_and_go = True if (candidate in CAR.RAV4H) else False
ret.safetyParam = 73
ret.wheelbase = 2.65
- ret.steerRatio = 16.30 # 14.5 is spec end-to-end
+ ret.steerRatio = 16.88 # 14.5 is spec end-to-end
tire_stiffness_factor = 0.5533
ret.mass = 3650. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid
- ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.05]]
- ret.lateralTuning.pid.kf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594
+ ret.lateralTuning.init('lqr')
+
+ ret.lateralTuning.lqr.scale = 1500.0
+ ret.lateralTuning.lqr.ki = 0.05
+
+ ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268]
+ ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05]
+ ret.lateralTuning.lqr.c = [1., 0.]
+ ret.lateralTuning.lqr.k = [-110.73572306, 451.22718255]
+ ret.lateralTuning.lqr.l = [0.3233671, 0.3185757]
+ ret.lateralTuning.lqr.dcGain = 0.002237852961363602
elif candidate == CAR.COROLLA:
stop_and_go = False
ret.safetyParam = 100
ret.wheelbase = 2.70
- ret.steerRatio = 17.8
+ ret.steerRatio = 18.27
tire_stiffness_factor = 0.444 # not optimized yet
ret.mass = 2860. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.05]]
@@ -130,10 +152,10 @@ class CarInterface(object):
ret.safetyParam = 73
ret.wheelbase = 2.78
ret.steerRatio = 16.0
- tire_stiffness_factor = 0.444 # not optimized yet
+ tire_stiffness_factor = 0.8
ret.mass = 4607. * CV.LB_TO_KG + STD_CARGO_KG #mean between normal and hybrid limited
- ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.05]]
- ret.lateralTuning.pid.kf = 0.00006
+ ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.18], [0.015]] # community tuning
+ ret.lateralTuning.pid.kf = 0.00012 # community tuning
elif candidate == CAR.AVALON:
stop_and_go = False
@@ -175,6 +197,16 @@ class CarInterface(object):
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]]
ret.lateralTuning.pid.kf = 0.00007818594
+ elif candidate == CAR.SIENNA:
+ stop_and_go = True
+ ret.safetyParam = 73
+ ret.wheelbase = 3.03
+ ret.steerRatio = 16.0
+ tire_stiffness_factor = 0.444
+ ret.mass = 4590. * CV.LB_TO_KG + STD_CARGO_KG
+ ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.05]]
+ ret.lateralTuning.pid.kf = 0.00007818594
+
elif candidate == CAR.LEXUS_ISH:
stop_and_go = True
ret.safetyParam = 66
@@ -211,10 +243,10 @@ class CarInterface(object):
# steer, gas, brake limitations VS speed
ret.steerMaxBP = [16. * CV.KPH_TO_MS, 45. * CV.KPH_TO_MS] # breakpoints at 1 and 40 kph
ret.steerMaxV = [1., 1.] # 2/3rd torque allowed above 45 kph
- ret.brakeMaxBP = [5., 20.]
- ret.brakeMaxV = [1., 0.8]
+ ret.brakeMaxBP = [0.]
+ ret.brakeMaxV = [1.]
- ret.enableCamera = not check_ecu_msgs(fingerprint, ECU.CAM)
+ ret.enableCamera = not check_ecu_msgs(fingerprint, ECU.CAM) or is_panda_black
ret.enableDsu = not check_ecu_msgs(fingerprint, ECU.DSU)
ret.enableApgs = False #not check_ecu_msgs(fingerprint, ECU.APGS)
ret.openpilotLongitudinalControl = ret.enableCamera and ret.enableDsu
@@ -246,22 +278,20 @@ class CarInterface(object):
return ret
# returns a car.CarState
- def update(self, c):
+ def update(self, c, can_strings):
# ******************* do can recv *******************
- canMonoTimes = []
-
- can_rcv_valid, _ = self.cp.update(int(sec_since_boot() * 1e9), True)
+ self.cp.update_strings(int(sec_since_boot() * 1e9), can_strings)
# run the cam can update for 10s as we just need to know if the camera is alive
if self.frame < 1000:
- self.cp_cam.update(int(sec_since_boot() * 1e9), False)
+ self.cp_cam.update_strings(int(sec_since_boot() * 1e9), can_strings)
self.CS.update(self.cp)
# create message
ret = car.CarState.new_message()
- ret.canValid = can_rcv_valid and self.cp.can_valid
+ ret.canValid = self.cp.can_valid
# speeds
ret.vEgo = self.CS.v_ego
@@ -295,6 +325,7 @@ class CarInterface(object):
ret.steeringRate = self.CS.angle_steers_rate
ret.steeringTorque = self.CS.steer_torque_driver
+ ret.steeringTorqueEps = self.CS.steer_torque_motor
ret.steeringPressed = self.CS.steer_override
# cruise state
@@ -378,7 +409,6 @@ class CarInterface(object):
events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
ret.events = events
- ret.canMonoTimes = canMonoTimes
self.gas_pressed_prev = ret.gasPressed
self.brake_pressed_prev = ret.brakePressed
@@ -392,8 +422,8 @@ class CarInterface(object):
can_sends = self.CC.update(c.enabled, self.CS, self.frame,
c.actuators, c.cruiseControl.cancel, c.hudControl.visualAlert,
- c.hudControl.audibleAlert, self.forwarding_camera,
- c.hudControl.leftLaneVisible, c.hudControl.rightLaneVisible, c.hudControl.leadVisible,
+ self.forwarding_camera, c.hudControl.leftLaneVisible,
+ c.hudControl.rightLaneVisible, c.hudControl.leadVisible,
c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart)
self.frame += 1
diff --git a/selfdrive/car/toyota/radar_interface.py b/selfdrive/car/toyota/radar_interface.py
index c160e751a..4e0a0e809 100755
--- a/selfdrive/car/toyota/radar_interface.py
+++ b/selfdrive/car/toyota/radar_interface.py
@@ -46,32 +46,36 @@ class RadarInterface(object):
self.valid_cnt = {key: 0 for key in self.RADAR_A_MSGS}
self.rcp = _create_radar_can_parser(CP.carFingerprint)
+ self.trigger_msg = self.RADAR_B_MSGS[-1]
+ self.updated_messages = set()
+
# No radar dbc for cars without DSU which are not TSS 2.0
# TODO: make a adas dbc file for dsu-less models
self.no_radar = CP.carFingerprint in NO_DSU_CAR and CP.carFingerprint not in TSS2_CAR
- def update(self):
-
- ret = car.RadarData.new_message()
-
+ def update(self, can_strings):
if self.no_radar:
time.sleep(0.05)
- return ret
+ return car.RadarData.new_message()
- canMonoTimes = []
- updated_messages = set()
- while 1:
- tm = int(sec_since_boot() * 1e9)
- _, vls = self.rcp.update(tm, True)
- updated_messages.update(vls)
- if self.RADAR_B_MSGS[-1] in updated_messages:
- break
+ tm = int(sec_since_boot() * 1e9)
+ vls = self.rcp.update_strings(tm, can_strings)
+ self.updated_messages.update(vls)
+ if self.trigger_msg not in self.updated_messages:
+ return None
+
+ rr = self._update(self.updated_messages)
+ self.updated_messages.clear()
+
+ return rr
+
+ def _update(self, updated_messages):
+ ret = car.RadarData.new_message()
errors = []
if not self.rcp.can_valid:
errors.append("canError")
ret.errors = errors
- ret.canMonoTimes = canMonoTimes
for ii in updated_messages:
if ii in self.RADAR_A_MSGS:
@@ -105,10 +109,3 @@ class RadarInterface(object):
ret.points = self.pts.values()
return ret
-
-if __name__ == "__main__":
- RI = RadarInterface(None)
- while 1:
- ret = RI.update()
- print(chr(27) + "[2J")
- print(ret)
diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py
index 7de38c891..35ba67452 100644
--- a/selfdrive/car/toyota/toyotacan.py
+++ b/selfdrive/car/toyota/toyotacan.py
@@ -105,7 +105,7 @@ def create_fcw_command(packer, fcw):
return packer.make_can_msg("ACC_HUD", 0, values)
-def create_ui_command(packer, steer, sound1, sound2, left_line, right_line, left_lane_depart, right_lane_depart):
+def create_ui_command(packer, steer, left_line, right_line, left_lane_depart, right_lane_depart):
values = {
"RIGHT_LINE": 3 if right_lane_depart else 1 if right_line else 2,
"LEFT_LINE": 3 if left_lane_depart else 1 if left_line else 2,
@@ -116,8 +116,8 @@ def create_ui_command(packer, steer, sound1, sound2, left_line, right_line, left
"SET_ME_X02": 0x02,
"SET_ME_X01": 1,
"SET_ME_X01_2": 1,
- "REPEATED_BEEPS": sound1,
- "TWO_BEEPS": sound2,
+ "REPEATED_BEEPS": 0,
+ "TWO_BEEPS": 0,
"LDA_ALERT": steer,
}
return packer.make_can_msg("LKAS_HUD", 0, values)
diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py
index 67d780927..3087867b6 100644
--- a/selfdrive/car/toyota/values.py
+++ b/selfdrive/car/toyota/values.py
@@ -16,6 +16,8 @@ class CAR:
RAV4_TSS2 = "TOYOTA RAV4 2019"
COROLLA_TSS2 = "TOYOTA COROLLA TSS2 2019"
LEXUS_ESH_TSS2 = "LEXUS ES 300H 2019"
+ SIENNA = "TOYOTA SIENNA XLE 2018"
+
LEXUS_ISH = "LEXUS IS HYBRID 2017"
class ECU:
@@ -48,23 +50,23 @@ STATIC_MSGS = [
(0x4d3, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.AVALON, CAR.LEXUS_ISH), 0, 100, '\x1C\x00\x00\x01\x00\x00\x00\x00'),
(0x128, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.AVALON), 1, 3, '\xf4\x01\x90\x83\x00\x37'),
- (0x128, ECU.DSU, (CAR.HIGHLANDER, CAR.HIGHLANDERH), 1, 3, '\x03\x00\x20\x00\x00\x52'),
- (0x141, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 1, 2, '\x00\x00\x00\x46'),
- (0x160, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 1, 7, '\x00\x00\x08\x12\x01\x31\x9c\x51'),
+ (0x128, ECU.DSU, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.SIENNA), 1, 3, '\x03\x00\x20\x00\x00\x52'),
+ (0x141, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA), 1, 2, '\x00\x00\x00\x46'),
+ (0x160, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA), 1, 7, '\x00\x00\x08\x12\x01\x31\x9c\x51'),
(0x161, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.AVALON), 1, 7, '\x00\x1e\x00\x00\x00\x80\x07'),
- (0X161, ECU.DSU, (CAR.HIGHLANDERH, CAR.HIGHLANDER), 1, 7, '\x00\x1e\x00\xd4\x00\x00\x5b'),
- (0x283, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 0, 3, '\x00\x00\x00\x00\x00\x00\x8c'),
+ (0X161, ECU.DSU, (CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.SIENNA), 1, 7, '\x00\x1e\x00\xd4\x00\x00\x5b'),
+ (0x283, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA), 0, 3, '\x00\x00\x00\x00\x00\x00\x8c'),
(0x2E6, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 3, '\xff\xf8\x00\x08\x7f\xe0\x00\x4e'),
(0x2E7, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 3, '\xa8\x9c\x31\x9c\x00\x00\x00\x02'),
(0x33E, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 20, '\x0f\xff\x26\x40\x00\x1f\x00'),
- (0x344, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 0, 5, '\x00\x00\x01\x00\x00\x00\x00\x50'),
+ (0x344, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA), 0, 5, '\x00\x00\x01\x00\x00\x00\x00\x50'),
(0x365, ECU.DSU, (CAR.PRIUS, CAR.LEXUS_RXH, CAR.HIGHLANDERH), 0, 20, '\x00\x00\x00\x80\x03\x00\x08'),
- (0x365, ECU.DSU, (CAR.RAV4, CAR.RAV4H, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON), 0, 20, '\x00\x00\x00\x80\xfc\x00\x08'),
+ (0x365, ECU.DSU, (CAR.RAV4, CAR.RAV4H, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA), 0, 20, '\x00\x00\x00\x80\xfc\x00\x08'),
(0x366, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.HIGHLANDERH), 0, 20, '\x00\x00\x4d\x82\x40\x02\x00'),
- (0x366, ECU.DSU, (CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON), 0, 20, '\x00\x72\x07\xff\x09\xfe\x00'),
+ (0x366, ECU.DSU, (CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA), 0, 20, '\x00\x72\x07\xff\x09\xfe\x00'),
(0x470, ECU.DSU, (CAR.PRIUS, CAR.LEXUS_RXH), 1, 100, '\x00\x00\x02\x7a'),
- (0x470, ECU.DSU, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.RAV4H), 1, 100, '\x00\x00\x01\x79'),
- (0x4CB, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.AVALON), 0, 100, '\x0c\x00\x00\x00\x00\x00\x00\x00'),
+ (0x470, ECU.DSU, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.RAV4H, CAR.SIENNA), 1, 100, '\x00\x00\x01\x79'),
+ (0x4CB, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA), 0, 100, '\x0c\x00\x00\x00\x00\x00\x00\x00'),
(0x292, ECU.APGS, (CAR.PRIUS), 0, 3, '\x00\x00\x00\x00\x00\x00\x00\x9e'),
(0x32E, ECU.APGS, (CAR.PRIUS), 0, 20, '\x00\x00\x00\x00\x00\x00\x00\x00'),
@@ -187,6 +189,9 @@ FINGERPRINTS = {
{
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 401: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 658: 8, 713: 8, 728: 8, 740: 5, 742: 8, 743: 8, 744: 8, 761: 8, 764: 8, 765: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 863: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 882: 8, 885: 8, 889: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 987: 8, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1228: 8, 1235: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1696: 8, 1775: 8, 1777: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
}],
+ CAR.SIENNA: [{
+ 36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 545: 5, 548: 8, 550: 8, 552: 4, 562: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 764: 8, 800: 8, 824: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 888: 8, 896: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 1, 918: 7, 921: 8, 933: 8, 944: 6, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1008: 2, 1014: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1114: 8, 1160: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1200: 8, 1201: 8, 1202: 8, 1203: 8, 1212: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1279: 8, 1552: 8, 1553: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1656: 8, 1664: 8, 1666: 8, 1667: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
+ }],
}
STEER_THRESHOLD = 100
@@ -207,9 +212,10 @@ DBC = {
CAR.RAV4_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.COROLLA_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.LEXUS_ESH_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
+ CAR.SIENNA: dbc_dict('toyota_sienna_xle_2018_pt_generated', 'toyota_adas'),
CAR.LEXUS_ISH: dbc_dict('lexus_is_hybrid_2017_pt_generated', 'toyota_adas'),
}
NO_DSU_CAR = [CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.LEXUS_ESH_TSS2]
TSS2_CAR = [CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.LEXUS_ESH_TSS2]
-NO_STOP_TIMER_CAR = [CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.LEXUS_ESH_TSS2, CAR.LEXUS_ISH] # no resume button press required
+NO_STOP_TIMER_CAR = [CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.LEXUS_ESH_TSS2, CAR.SIENNA, CAR.LEXUS_ISH] # no resume button press required
diff --git a/selfdrive/common/modeldata.h b/selfdrive/common/modeldata.h
index b5b38bafd..6c0cd006f 100644
--- a/selfdrive/common/modeldata.h
+++ b/selfdrive/common/modeldata.h
@@ -1,8 +1,9 @@
#ifndef MODELDATA_H
#define MODELDATA_H
-#define MODEL_PATH_DISTANCE 100
+#define MODEL_PATH_DISTANCE 192
#define POLYFIT_DEGREE 4
+#define SPEED_PERCENTILES 10
typedef struct PathData {
float points[MODEL_PATH_DISTANCE];
@@ -16,8 +17,12 @@ typedef struct LeadData {
float dist;
float prob;
float std;
+ float rel_y;
+ float rel_y_std;
float rel_v;
float rel_v_std;
+ float rel_a;
+ float rel_a_std;
} LeadData;
typedef struct ModelData {
@@ -25,6 +30,8 @@ typedef struct ModelData {
PathData left_lane;
PathData right_lane;
LeadData lead;
+ LeadData lead_future;
+ float speed[SPEED_PERCENTILES];
} ModelData;
#endif
diff --git a/selfdrive/common/params.cc b/selfdrive/common/params.cc
index 7bbcf5fad..723b06d92 100644
--- a/selfdrive/common/params.cc
+++ b/selfdrive/common/params.cc
@@ -184,6 +184,9 @@ int read_db_all(const char* params_path, std::map *par
while ((de = readdir(d))) {
if (!isalnum(de->d_name[0])) continue;
std::string key = std::string(de->d_name);
+
+ if (key == "AccessToken") continue;
+
std::string value = util::read_file(util::string_format("%s/%s", key_path.c_str(), key.c_str()));
(*params)[key] = value;
diff --git a/selfdrive/common/version.h b/selfdrive/common/version.h
index ad486deed..d17343827 100644
--- a/selfdrive/common/version.h
+++ b/selfdrive/common/version.h
@@ -1 +1 @@
-#define COMMA_VERSION "0.6-release"
+#define COMMA_VERSION "0.6.3-release"
diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index f1c556ba1..9a463baca 100755
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -1,4 +1,5 @@
#!/usr/bin/env python
+import os
import gc
import capnp
from cereal import car, log
@@ -11,7 +12,7 @@ from selfdrive.config import Conversions as CV
from selfdrive.services import service_list
from selfdrive.boardd.boardd import can_list_to_can_capnp
from selfdrive.car.car_helpers import get_car, get_startup_alert
-from selfdrive.controls.lib.model_parser import CAMERA_OFFSET
+from selfdrive.controls.lib.lane_planner import CAMERA_OFFSET
from selfdrive.controls.lib.drive_helpers import get_events, \
create_event, \
EventTypes as ET, \
@@ -20,9 +21,10 @@ from selfdrive.controls.lib.drive_helpers import get_events, \
from selfdrive.controls.lib.longcontrol import LongControl, STARTING_TARGET_SPEED
from selfdrive.controls.lib.latcontrol_pid import LatControlPID
from selfdrive.controls.lib.latcontrol_indi import LatControlINDI
+from selfdrive.controls.lib.latcontrol_lqr import LatControlLQR
from selfdrive.controls.lib.alertmanager import AlertManager
from selfdrive.controls.lib.vehicle_model import VehicleModel
-from selfdrive.controls.lib.driver_monitor import DriverStatus
+from selfdrive.controls.lib.driver_monitor import DriverStatus, MAX_TERMINAL_ALERTS
from selfdrive.controls.lib.planner import LON_MPC_STEP
from selfdrive.locationd.calibration_helpers import Calibration, Filter
@@ -48,17 +50,27 @@ def events_to_bytes(events):
ret.append(e.to_bytes())
return ret
+def wait_for_can(logcan):
+ print("Waiting for CAN messages...")
+ while len(messaging.recv_one(logcan).can) == 0:
+ pass
-def data_sample(CI, CC, sm, cal_status, cal_perc, overtemp, free_space, low_battery,
+def data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp, free_space, low_battery,
driver_status, state, mismatch_counter, params):
"""Receive data from sockets and create events for battery, temperature and disk space"""
# Update carstate from CAN and create events
- CS = CI.update(CC)
+ can_strs = messaging.drain_sock_raw(can_sock, wait_for_one=True)
+ CS = CI.update(CC, can_strs)
+
+ sm.update(0)
+
events = list(CS.events)
enabled = isEnabled(state)
- sm.update(0)
+ # Check for CAN timeout
+ if not can_strs:
+ events.append(create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if sm.updated['thermal']:
overtemp = sm['thermal'].thermalStatus >= ThermalStatus.red
@@ -73,16 +85,22 @@ def data_sample(CI, CC, sm, cal_status, cal_perc, overtemp, free_space, low_batt
if free_space:
events.append(create_event('outOfSpace', [ET.NO_ENTRY]))
+
# Handle calibration
if sm.updated['liveCalibration']:
cal_status = sm['liveCalibration'].calStatus
cal_perc = sm['liveCalibration'].calPerc
+ cal_rpy = [0,0,0]
if cal_status != Calibration.CALIBRATED:
if cal_status == Calibration.UNCALIBRATED:
events.append(create_event('calibrationIncomplete', [ET.NO_ENTRY, ET.SOFT_DISABLE, ET.PERMANENT]))
else:
events.append(create_event('calibrationInvalid', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
+ else:
+ rpy = sm['liveCalibration'].rpyCalib
+ if len(rpy) == 3:
+ cal_rpy = rpy
# When the panda and controlsd do not agree on controls_allowed
# we want to disengage openpilot. However the status from the panda goes through
@@ -100,7 +118,10 @@ def data_sample(CI, CC, sm, cal_status, cal_perc, overtemp, free_space, low_batt
# Driver monitoring
if sm.updated['driverMonitoring']:
- driver_status.get_pose(sm['driverMonitoring'], params)
+ driver_status.get_pose(sm['driverMonitoring'], params, cal_rpy)
+
+ if driver_status.terminal_alert_cnt >= MAX_TERMINAL_ALERTS:
+ events.append(create_event("tooDistracted", [ET.NO_ENTRY]))
return CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter
@@ -240,8 +261,7 @@ def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cr
actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.brakePressed, CS.standstill, CS.cruiseState.standstill,
v_cruise_kph, v_acc_sol, plan.vTargetFuture, a_acc_sol, CP)
# Steering PID loop and lateral MPC
- actuators.steer, actuators.steerAngle, lac_log = LaC.update(active, CS.vEgo, CS.steeringAngle, CS.steeringRate,
- CS.steeringPressed, CP, VM, path_plan)
+ actuators.steer, actuators.steerAngle, lac_log = LaC.update(active, CS.vEgo, CS.steeringAngle, CS.steeringRate, CS.steeringTorqueEps, CS.steeringPressed, CP, VM, path_plan)
# Send a "steering required alert" if saturation count has reached the limit
if LaC.sat_flag and CP.steerLimitAlert:
@@ -295,12 +315,11 @@ def data_send(sm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, ca
ldw_allowed = CS.vEgo > 12.5 and not blinker
if len(list(sm['pathPlan'].rPoly)) == 4:
- CC.hudControl.rightLaneDepart = bool(ldw_allowed and sm['pathPlan'].rPoly[3] > -(1 + CAMERA_OFFSET) and right_lane_visible)
+ CC.hudControl.rightLaneDepart = bool(ldw_allowed and sm['pathPlan'].rPoly[3] > -(1.08 + CAMERA_OFFSET) and right_lane_visible)
if len(list(sm['pathPlan'].lPoly)) == 4:
- CC.hudControl.leftLaneDepart = bool(ldw_allowed and sm['pathPlan'].lPoly[3] < (1 - CAMERA_OFFSET) and left_lane_visible)
+ CC.hudControl.leftLaneDepart = bool(ldw_allowed and sm['pathPlan'].lPoly[3] < (1.08 - CAMERA_OFFSET) and left_lane_visible)
CC.hudControl.visualAlert = AM.visual_alert
- CC.hudControl.audibleAlert = AM.audible_alert
if not read_only:
# send car controls over can
@@ -320,7 +339,7 @@ def data_send(sm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, ca
"alertStatus": AM.alert_status,
"alertBlinkingRate": AM.alert_rate,
"alertType": AM.alert_type,
- "alertSound": "", # no EON sounds yet
+ "alertSound": AM.audible_alert,
"awarenessStatus": max(driver_status.awareness, 0.0) if isEnabled(state) else 0.0,
"driverMonitoringOn": bool(driver_status.monitor_on and driver_status.face_detected),
"canMonoTimes": list(CS.canMonoTimes),
@@ -331,7 +350,7 @@ def data_send(sm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, ca
"vEgo": CS.vEgo,
"vEgoRaw": CS.vEgoRaw,
"angleSteers": CS.steeringAngle,
- "curvature": VM.calc_curvature(CS.steeringAngle * CV.DEG_TO_RAD, CS.vEgo),
+ "curvature": VM.calc_curvature((CS.steeringAngle - sm['pathPlan'].angleOffset) * CV.DEG_TO_RAD, CS.vEgo),
"steerOverride": CS.steeringPressed,
"state": state,
"engageable": not bool(get_events(events, [ET.NO_ENTRY])),
@@ -348,7 +367,7 @@ def data_send(sm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, ca
"angleModelBias": 0.,
"gpsPlannerActive": sm['plan'].gpsPlannerActive,
"vCurvature": sm['plan'].vCurvature,
- "decelForTurn": sm['plan'].decelForTurn,
+ "decelForModel": sm['plan'].longitudinalPlanSource == log.Plan.LongitudinalPlanSource.model,
"cumLagMs": -rk.remaining * 1000.,
"startMonoTime": int(start_time * 1e9),
"mapValid": sm['plan'].mapValid,
@@ -357,7 +376,9 @@ def data_send(sm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, ca
if CP.lateralTuning.which() == 'pid':
dat.controlsState.lateralControlState.pidState = lac_log
- else:
+ elif CP.lateralTuning.which() == 'lqr':
+ dat.controlsState.lateralControlState.lqrState = lac_log
+ elif CP.lateralTuning.which() == 'indi':
dat.controlsState.lateralControlState.indiState = lac_log
controlsstate.send(dat.to_bytes())
@@ -414,11 +435,20 @@ def controlsd_thread(gctx=None):
passive = params.get("Passive") != "0"
sm = messaging.SubMaster(['thermal', 'health', 'liveCalibration', 'driverMonitoring', 'plan', 'pathPlan'])
+
logcan = messaging.sub_sock(service_list['can'].port)
- CC = car.CarControl.new_message()
- CI, CP = get_car(logcan, sendcan)
- AM = AlertManager()
+ # wait for health and CAN packets
+ hw_type = messaging.recv_one(sm.sock['health']).health.hwType
+ is_panda_black = hw_type == log.HealthData.HwType.blackPanda
+ wait_for_can(logcan)
+
+ CI, CP = get_car(logcan, sendcan, is_panda_black)
+ logcan.close()
+
+ # TODO: Use the logcan socket from above, but that will currenly break the tests
+ can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100
+ can_sock = messaging.sub_sock(service_list['can'].port, timeout=can_timeout)
car_recognized = CP.carName != 'mock'
# If stock camera is disconnected, we loaded car controls and it's not chffrplus
@@ -427,6 +457,13 @@ def controlsd_thread(gctx=None):
if read_only:
CP.safetyModel = car.CarParams.SafetyModel.elm327 # diagnostic only
+ # Write CarParams for radard and boardd safety mode
+ params.put("CarParams", CP.to_bytes())
+ params.put("LongitudinalControl", "1" if CP.openpilotLongitudinalControl else "0")
+
+ CC = car.CarControl.new_message()
+ AM = AlertManager()
+
startup_alert = get_startup_alert(car_recognized, controller_available)
AM.add(sm.frame, startup_alert, False)
@@ -435,15 +472,13 @@ def controlsd_thread(gctx=None):
if CP.lateralTuning.which() == 'pid':
LaC = LatControlPID(CP)
- else:
+ elif CP.lateralTuning.which() == 'indi':
LaC = LatControlINDI(CP)
+ elif CP.lateralTuning.which() == 'lqr':
+ LaC = LatControlLQR(CP)
driver_status = DriverStatus()
- # Write CarParams for radard and boardd safety mode
- params.put("CarParams", CP.to_bytes())
- params.put("LongitudinalControl", "1" if CP.openpilotLongitudinalControl else "0")
-
state = State.disabled
soft_disable_timer = 0
v_cruise_kph = 255
@@ -457,6 +492,10 @@ def controlsd_thread(gctx=None):
events_prev = []
sm['pathPlan'].sensorValid = True
+ sm['pathPlan'].posenetValid = True
+
+ # detect sound card presence
+ sounds_available = not os.path.isfile('/EON') or (os.path.isdir('/proc/asound/card0') and open('/proc/asound/card0/state').read().strip() == 'ONLINE')
# controlsd is driven by can recv, expected at 100Hz
rk = Ratekeeper(100, print_delay_threshold=None)
@@ -469,7 +508,7 @@ def controlsd_thread(gctx=None):
# Sample data and compute car events
CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter =\
- data_sample(CI, CC, sm, cal_status, cal_perc, overtemp, free_space, low_battery,
+ data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp, free_space, low_battery,
driver_status, state, mismatch_counter, params)
prof.checkpoint("Sample")
@@ -482,12 +521,16 @@ def controlsd_thread(gctx=None):
events.append(create_event('sensorDataInvalid', [ET.NO_ENTRY, ET.PERMANENT]))
if not sm['pathPlan'].paramsValid:
events.append(create_event('vehicleModelInvalid', [ET.WARNING]))
+ if not sm['pathPlan'].posenetValid:
+ events.append(create_event('posenetInvalid', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not sm['plan'].radarValid:
events.append(create_event('radarFault', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if sm['plan'].radarCanError:
events.append(create_event('radarCanError', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not CS.canValid:
events.append(create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
+ if not sounds_available:
+ events.append(create_event('soundsUnavailable', [ET.NO_ENTRY, ET.PERMANENT]))
# Only allow engagement with brake pressed when stopped behind another stopped car
if CS.brakePressed and sm['plan'].vTargetFuture >= STARTING_TARGET_SPEED and not CP.radarOffCan and CS.vEgo < 0.3:
diff --git a/selfdrive/controls/lib/alertmanager.py b/selfdrive/controls/lib/alertmanager.py
index df2727e6e..77196273b 100644
--- a/selfdrive/controls/lib/alertmanager.py
+++ b/selfdrive/controls/lib/alertmanager.py
@@ -1,4 +1,4 @@
-from cereal import log
+from cereal import car, log
from common.realtime import DT_CTRL
from selfdrive.swaglog import cloudlog
from selfdrive.controls.lib.alerts import ALERTS
@@ -7,7 +7,8 @@ import copy
AlertSize = log.ControlsState.AlertSize
AlertStatus = log.ControlsState.AlertStatus
-
+VisualAlert = car.CarControl.HUDControl.VisualAlert
+AudibleAlert = car.CarControl.HUDControl.AudibleAlert
class AlertManager(object):
@@ -49,8 +50,8 @@ class AlertManager(object):
self.alert_text_2 = ""
self.alert_status = AlertStatus.normal
self.alert_size = AlertSize.none
- self.visual_alert = "none"
- self.audible_alert = "none"
+ self.visual_alert = VisualAlert.none
+ self.audible_alert = AudibleAlert.none
self.alert_rate = 0.
if current_alert:
diff --git a/selfdrive/controls/lib/alerts.py b/selfdrive/controls/lib/alerts.py
index da7e88acb..fbef59fe4 100644
--- a/selfdrive/controls/lib/alerts.py
+++ b/selfdrive/controls/lib/alerts.py
@@ -298,83 +298,104 @@ ALERTS = [
AlertStatus.normal, AlertSize.mid,
Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
+ Alert(
+ "soundsUnavailableNoEntry",
+ "openpilot Unavailable",
+ "Speaker not found",
+ AlertStatus.normal, AlertSize.mid,
+ Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
+
+ Alert(
+ "tooDistractedNoEntry",
+ "openpilot Unavailable",
+ "Distraction Level Too High",
+ AlertStatus.normal, AlertSize.mid,
+ Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
+
# Cancellation alerts causing soft disabling
Alert(
"overheat",
"TAKE CONTROL IMMEDIATELY",
"System Overheated",
AlertStatus.critical, AlertSize.full,
- Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
+ Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.),
Alert(
"wrongGear",
"TAKE CONTROL IMMEDIATELY",
"Gear not D",
AlertStatus.critical, AlertSize.full,
- Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
+ Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.),
Alert(
"calibrationInvalid",
"TAKE CONTROL IMMEDIATELY",
"Calibration Invalid: Reposition EON and Recalibrate",
AlertStatus.critical, AlertSize.full,
- Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
+ Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.),
Alert(
"calibrationIncomplete",
"TAKE CONTROL IMMEDIATELY",
"Calibration in Progress",
AlertStatus.critical, AlertSize.full,
- Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
+ Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.),
Alert(
"doorOpen",
"TAKE CONTROL IMMEDIATELY",
"Door Open",
AlertStatus.critical, AlertSize.full,
- Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
+ Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.),
Alert(
"seatbeltNotLatched",
"TAKE CONTROL IMMEDIATELY",
"Seatbelt Unlatched",
AlertStatus.critical, AlertSize.full,
- Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
+ Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.),
Alert(
"espDisabled",
"TAKE CONTROL IMMEDIATELY",
"ESP Off",
AlertStatus.critical, AlertSize.full,
- Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
+ Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.),
Alert(
"lowBattery",
"TAKE CONTROL IMMEDIATELY",
"Low Battery",
AlertStatus.critical, AlertSize.full,
- Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
+ Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.),
Alert(
"commIssue",
"TAKE CONTROL IMMEDIATELY",
"Communication Issue between Processes",
AlertStatus.critical, AlertSize.full,
- Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
+ Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.),
Alert(
"radarCanError",
"TAKE CONTROL IMMEDIATELY",
"Radar Error: Restart the Car",
AlertStatus.critical, AlertSize.full,
- Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
+ Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.),
Alert(
"radarFault",
"TAKE CONTROL IMMEDIATELY",
"Radar Error: Restart the Car",
AlertStatus.critical, AlertSize.full,
- Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
+ Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.),
+
+ Alert(
+ "posenetInvalid",
+ "TAKE CONTROL IMMEDIATELY",
+ "Vision Failure: Check Camera View",
+ AlertStatus.critical, AlertSize.full,
+ Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.),
# Cancellation alerts causing immediate disabling
Alert(
@@ -533,6 +554,13 @@ ALERTS = [
AlertStatus.normal, AlertSize.mid,
Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
+ Alert(
+ "posenetInvalidNoEntry",
+ "openpilot Unavailable",
+ "Vision Failure: Check Camera View",
+ AlertStatus.normal, AlertSize.mid,
+ Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
+
Alert(
"controlsFailedNoEntry",
"openpilot Unavailable",
@@ -653,6 +681,13 @@ ALERTS = [
AlertStatus.normal, AlertSize.mid,
Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2),
+ Alert(
+ "soundsUnavailablePermanent",
+ "Speaker not found",
+ "Reboot your EON",
+ AlertStatus.normal, AlertSize.mid,
+ Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2),
+
Alert(
"vehicleModelInvalid",
"Vehicle Parameter Identification Failed",
diff --git a/selfdrive/controls/lib/driver_monitor.py b/selfdrive/controls/lib/driver_monitor.py
index e813d0b17..3b45e5a06 100644
--- a/selfdrive/controls/lib/driver_monitor.py
+++ b/selfdrive/controls/lib/driver_monitor.py
@@ -3,41 +3,56 @@ from common.realtime import sec_since_boot, DT_CTRL, DT_DMON
from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET
from common.filter_simple import FirstOrderFilter
-_AWARENESS_TIME = 180 # 3 minutes limit without user touching steering wheels make the car enter a terminal status
-_AWARENESS_PRE_TIME = 20. # a first alert is issued 20s before expiration
-_AWARENESS_PROMPT_TIME = 5. # a second alert is issued 5s before start decelerating the car
-_DISTRACTED_TIME = 7.
-_DISTRACTED_PRE_TIME = 4.
-_DISTRACTED_PROMPT_TIME = 2.
-# model output refers to center of cropped image, so need to apply the x displacement offset
-_PITCH_WEIGHT = 1.5 # pitch matters a lot more
+_AWARENESS_TIME = 90. # 1.5 minutes limit without user touching steering wheels make the car enter a terminal status
+_AWARENESS_PRE_TIME_TILL_TERMINAL = 20. # a first alert is issued 20s before expiration
+_AWARENESS_PROMPT_TIME_TILL_TERMINAL = 5. # a second alert is issued 5s before start decelerating the car
+_DISTRACTED_TIME = 10.
+_DISTRACTED_PRE_TIME_TILL_TERMINAL = 7.
+_DISTRACTED_PROMPT_TIME_TILL_TERMINAL = 5.
+
+_FACE_THRESHOLD = 0.4
+_EYE_THRESHOLD = 0.4
+_BLINK_THRESHOLD = 0.2 # 0.225
+_PITCH_WEIGHT = 1.35 # 1.5 # pitch matters a lot more
_METRIC_THRESHOLD = 0.4
-_PITCH_POS_ALLOWANCE = 0.08 # rad, to not be too sensitive on positive pitch
-_PITCH_NATURAL_OFFSET = 0.1 # people don't seem to look straight when they drive relaxed, rather a bit up
-_YAW_NATURAL_OFFSET = 0.08 # people don't seem to look straight when they drive relaxed, rather a bit to the right (center of car)
-_STD_THRESHOLD = 0.1 # above this standard deviation consider the measurement invalid
+_PITCH_POS_ALLOWANCE = 0.04 # 0.08 # rad, to not be too sensitive on positive pitch
+_PITCH_NATURAL_OFFSET = 0.12 # 0.1 # people don't seem to look straight when they drive relaxed, rather a bit up
+_YAW_NATURAL_OFFSET = 0.08 # people don't seem to look straight when they drive relaxed, rather a bit to the right (center of car)
_DISTRACTED_FILTER_TS = 0.25 # 0.6Hz
_VARIANCE_FILTER_TS = 20. # 0.008Hz
+MAX_TERMINAL_ALERTS = 3 # not allowed to engage after 3 terminal alerts
+
+# model output refers to center of cropped image, so need to apply the x displacement offset
RESIZED_FOCAL = 320.0
H, W, FULL_W = 320, 160, 426
+class DistractedType(object):
+ NOT_DISTRACTED = 0
+ BAD_POSE = 1
+ BAD_BLINK = 2
-def head_orientation_from_descriptor(desc):
+def head_orientation_from_descriptor(angles_desc, pos_desc, rpy_calib):
# the output of these angles are in device frame
# so from driver's perspective, pitch is up and yaw is right
- # TODO this should be calibrated
- pitch_prnet = desc[0]
- yaw_prnet = desc[1]
- roll_prnet = desc[2]
- face_pixel_position = ((desc[3] + .5)*W - W + FULL_W, (desc[4]+.5)*H)
+ # TODO: calibrate based on position
+ pitch_prnet = angles_desc[0]
+ yaw_prnet = angles_desc[1]
+ roll_prnet = angles_desc[2]
+
+ face_pixel_position = ((pos_desc[0] + .5)*W - W + FULL_W, (pos_desc[1]+.5)*H)
yaw_focal_angle = np.arctan2(face_pixel_position[0] - FULL_W/2, RESIZED_FOCAL)
pitch_focal_angle = np.arctan2(face_pixel_position[1] - H/2, RESIZED_FOCAL)
roll = roll_prnet
pitch = pitch_prnet + pitch_focal_angle
yaw = -yaw_prnet + yaw_focal_angle
+
+ # no calib for roll
+ pitch -= rpy_calib[1]
+ yaw -= rpy_calib[2]
+
return np.array([roll, pitch, yaw])
@@ -49,15 +64,21 @@ class _DriverPose():
self.yaw_offset = 0.
self.pitch_offset = 0.
+class _DriverBlink():
+ def __init__(self):
+ self.left_blink = 0.
+ self.right_blink = 0.
+
+
def _monitor_hysteresis(variance_level, monitor_valid_prev):
var_thr = 0.63 if monitor_valid_prev else 0.37
return variance_level < var_thr
-
class DriverStatus():
def __init__(self, monitor_on=False):
self.pose = _DriverPose()
+ self.blink = _DriverBlink()
self.monitor_on = monitor_on
self.monitor_param_on = monitor_on
self.monitor_valid = True # variance needs to be low
@@ -68,54 +89,60 @@ class DriverStatus():
self.variance_filter = FirstOrderFilter(0., _VARIANCE_FILTER_TS, DT_DMON)
self.ts_last_check = 0.
self.face_detected = False
- self._set_timers()
+ self.terminal_alert_cnt = 0
+ self.step_change = 0.
+ self._set_timers(self.monitor_on)
def _reset_filters(self):
self.driver_distraction_filter.x = 0.
self.variance_filter.x = 0.
self.monitor_valid = True
- def _set_timers(self):
- if self.monitor_on:
- self.threshold_pre = _DISTRACTED_PRE_TIME / _DISTRACTED_TIME
- self.threshold_prompt = _DISTRACTED_PROMPT_TIME / _DISTRACTED_TIME
+ def _set_timers(self, active_monitoring):
+ if active_monitoring:
+ # when falling back from passive mode to active mode, reset awareness to avoid false alert
+ if self.step_change == DT_CTRL / _AWARENESS_TIME:
+ self.awareness = 1.
+ self.threshold_pre = _DISTRACTED_PRE_TIME_TILL_TERMINAL / _DISTRACTED_TIME
+ self.threshold_prompt = _DISTRACTED_PROMPT_TIME_TILL_TERMINAL / _DISTRACTED_TIME
self.step_change = DT_CTRL / _DISTRACTED_TIME
else:
- self.threshold_pre = _AWARENESS_PRE_TIME / _AWARENESS_TIME
- self.threshold_prompt = _AWARENESS_PROMPT_TIME / _AWARENESS_TIME
+ self.threshold_pre = _AWARENESS_PRE_TIME_TILL_TERMINAL / _AWARENESS_TIME
+ self.threshold_prompt = _AWARENESS_PROMPT_TIME_TILL_TERMINAL / _AWARENESS_TIME
self.step_change = DT_CTRL / _AWARENESS_TIME
- def _is_driver_distracted(self, pose):
- # to be tuned and to learn the driver's normal pose
+ def _is_driver_distracted(self, pose, blink):
+ # TODO: natural pose calib of each driver
pitch_error = pose.pitch - _PITCH_NATURAL_OFFSET
yaw_error = pose.yaw - _YAW_NATURAL_OFFSET
# add positive pitch allowance
if pitch_error > 0.:
pitch_error = max(pitch_error - _PITCH_POS_ALLOWANCE, 0.)
pitch_error *= _PITCH_WEIGHT
- metric = np.sqrt(yaw_error**2 + pitch_error**2)
- #print "%02.4f" % np.degrees(pose.pitch), "%02.4f" % np.degrees(pitch_error), "%03.4f" % np.degrees(pose.pitch_offset), metric
- return 1 if metric > _METRIC_THRESHOLD else 0
+ pose_metric = np.sqrt(yaw_error**2 + pitch_error**2)
-
- def get_pose(self, driver_monitoring, params):
-
- self.pose.roll, self.pose.pitch, self.pose.yaw = head_orientation_from_descriptor(driver_monitoring.descriptor)
-
- # TODO: DM data should not be in a list if they are not homogeneous
- if len(driver_monitoring.descriptor) > 6:
- self.face_detected = driver_monitoring.descriptor[6] > 0.
+ if pose_metric > _METRIC_THRESHOLD:
+ return DistractedType.BAD_POSE
+ elif blink.left_blink>_BLINK_THRESHOLD and blink.right_blink>_BLINK_THRESHOLD:
+ return DistractedType.BAD_BLINK
else:
- self.face_detected = True
+ return DistractedType.NOT_DISTRACTED
- self.driver_distracted = self._is_driver_distracted(self.pose)
+
+ def get_pose(self, driver_monitoring, params, cal_rpy):
+ if len(driver_monitoring.faceOrientation) == 0 or len(driver_monitoring.facePosition) == 0:
+ return
+
+ self.pose.roll, self.pose.pitch, self.pose.yaw = head_orientation_from_descriptor(driver_monitoring.faceOrientation, driver_monitoring.facePosition, cal_rpy)
+ self.blink.left_blink = driver_monitoring.leftBlinkProb * (driver_monitoring.leftEyeProb>_EYE_THRESHOLD)
+ self.blink.right_blink = driver_monitoring.rightBlinkProb * (driver_monitoring.rightEyeProb>_EYE_THRESHOLD)
+ self.face_detected = driver_monitoring.faceProb > _FACE_THRESHOLD
+
+ self.driver_distracted = self._is_driver_distracted(self.pose, self.blink)>0
# first order filters
self.driver_distraction_filter.update(self.driver_distracted)
- self.variance_high = driver_monitoring.std > _STD_THRESHOLD
- self.variance_filter.update(self.variance_high)
monitor_param_on_prev = self.monitor_param_on
- monitor_valid_prev = self.monitor_valid
# don't check for param too often as it's a kernel call
ts = sec_since_boot()
@@ -123,36 +150,42 @@ class DriverStatus():
self.monitor_param_on = params.get("IsDriverMonitoringEnabled") == "1"
self.ts_last_check = ts
- self.monitor_valid = _monitor_hysteresis(self.variance_filter.x, monitor_valid_prev)
self.monitor_on = self.monitor_valid and self.monitor_param_on
if monitor_param_on_prev != self.monitor_param_on:
self._reset_filters()
- self._set_timers()
+ self._set_timers(self.monitor_on and self.face_detected)
def update(self, events, driver_engaged, ctrl_active, standstill):
+ if driver_engaged:
+ self.awareness = 1.
+ return events
driver_engaged |= (self.driver_distraction_filter.x < 0.37 and self.monitor_on)
+ awareness_prev = self.awareness
- if (driver_engaged and self.awareness > 0.) or not ctrl_active:
+ if (driver_engaged and self.awareness > 0) or not ctrl_active:
# always reset if driver is in control (unless we are in red alert state) or op isn't active
- self.awareness = 1.
+ self.awareness = min(self.awareness + (2.75*(1.-self.awareness)+1.25)*self.step_change, 1.)
- # only update if face is detected, driver is distracted and distraction filter is high
- if (not self.monitor_on or (self.driver_distraction_filter.x > 0.63 and self.driver_distracted and self.face_detected)) and \
+ # should always be counting if distracted unless at standstill and reaching orange
+ if ((not self.monitor_on or (self.monitor_on and not self.face_detected)) or (self.driver_distraction_filter.x > 0.63 and self.driver_distracted and self.face_detected)) and \
not (standstill and self.awareness - self.step_change <= self.threshold_prompt):
self.awareness = max(self.awareness - self.step_change, -0.1)
alert = None
- if self.awareness <= 0.:
+ if self.awareness < 0.:
# terminal red alert: disengagement required
alert = 'driverDistracted' if self.monitor_on else 'driverUnresponsive'
+ if awareness_prev >= 0.:
+ self.terminal_alert_cnt += 1
elif self.awareness <= self.threshold_prompt:
# prompt orange alert
alert = 'promptDriverDistracted' if self.monitor_on else 'promptDriverUnresponsive'
elif self.awareness <= self.threshold_pre:
# pre green alert
alert = 'preDriverDistracted' if self.monitor_on else 'preDriverUnresponsive'
+
if alert is not None:
events.append(create_event(alert, [ET.WARNING]))
diff --git a/selfdrive/controls/lib/fcw.py b/selfdrive/controls/lib/fcw.py
index f93a72cfc..8180fadeb 100644
--- a/selfdrive/controls/lib/fcw.py
+++ b/selfdrive/controls/lib/fcw.py
@@ -43,8 +43,10 @@ class FCWChecker(object):
ttc = np.minimum(2 * x_lead / (np.sqrt(delta) + v_rel), max_ttc)
return ttc
- def update(self, mpc_solution, cur_time, v_ego, a_ego, x_lead, v_lead, a_lead, y_lead, vlat_lead, fcw_lead, blinkers):
+ def update(self, mpc_solution, cur_time, active, v_ego, a_ego, x_lead, v_lead, a_lead, y_lead, vlat_lead, fcw_lead, blinkers):
mpc_solution_a = list(mpc_solution[0].a_ego)
+ a_target = mpc_solution_a[1]
+
self.last_min_a = min(mpc_solution_a)
self.v_lead_max = max(self.v_lead_max, v_lead)
@@ -62,8 +64,11 @@ class FCWChecker(object):
a_thr = interp(v_lead, _FCW_A_ACT_BP, _FCW_A_ACT_V)
a_delta = min(mpc_solution_a[:15]) - min(0.0, a_ego)
- fcw_allowed = all(c >= 10 for c in self.counters.values())
- if (self.last_min_a < -3.0 or a_delta < a_thr) and fcw_allowed and self.last_fcw_time + 5.0 < cur_time:
+ future_fcw_allowed = all(c >= 10 for c in self.counters.values())
+ future_fcw = (self.last_min_a < -3.0 or a_delta < a_thr) and future_fcw_allowed
+ current_fcw = a_target < -3.0 and active
+
+ if (future_fcw or current_fcw) and (self.last_fcw_time + 5.0 < cur_time):
self.last_fcw_time = cur_time
self.last_fcw_a = self.last_min_a
return True
diff --git a/selfdrive/controls/lib/lane_planner.py b/selfdrive/controls/lib/lane_planner.py
new file mode 100644
index 000000000..60a24b6d9
--- /dev/null
+++ b/selfdrive/controls/lib/lane_planner.py
@@ -0,0 +1,74 @@
+from common.numpy_fast import interp
+import numpy as np
+from selfdrive.controls.lib.latcontrol_helpers import model_polyfit, compute_path_pinv
+
+CAMERA_OFFSET = 0.06 # m from center car to camera
+
+
+def calc_d_poly(l_poly, r_poly, p_poly, l_prob, r_prob, lane_width):
+ # This will improve behaviour when lanes suddenly widen
+ lane_width = min(4.0, lane_width)
+ l_prob = l_prob * interp(abs(l_poly[3]), [2, 2.5], [1.0, 0.0])
+ r_prob = r_prob * interp(abs(r_poly[3]), [2, 2.5], [1.0, 0.0])
+
+ path_from_left_lane = l_poly.copy()
+ path_from_left_lane[3] -= lane_width / 2.0
+ path_from_right_lane = r_poly.copy()
+ path_from_right_lane[3] += lane_width / 2.0
+
+ lr_prob = l_prob + r_prob - l_prob * r_prob
+
+ d_poly_lane = (l_prob * path_from_left_lane + r_prob * path_from_right_lane) / (l_prob + r_prob + 0.0001)
+ return lr_prob * d_poly_lane + (1.0 - lr_prob) * p_poly
+
+
+class LanePlanner(object):
+ def __init__(self):
+ self.l_poly = [0., 0., 0., 0.]
+ self.r_poly = [0., 0., 0., 0.]
+ self.p_poly = [0., 0., 0., 0.]
+ self.d_poly = [0., 0., 0., 0.]
+
+ self.lane_width_estimate = 3.7
+ self.lane_width_certainty = 1.0
+ self.lane_width = 3.7
+
+ self.l_prob = 0.
+ self.r_prob = 0.
+ self.lr_prob = 0.
+
+ self._path_pinv = compute_path_pinv()
+ self.x_points = np.arange(50)
+
+ def parse_model(self, md):
+ if len(md.leftLane.poly):
+ self.l_poly = np.array(md.leftLane.poly)
+ self.r_poly = np.array(md.rightLane.poly)
+ self.p_poly = np.array(md.path.poly)
+ else:
+ self.l_poly = model_polyfit(md.leftLane.points, self._path_pinv) # left line
+ self.r_poly = model_polyfit(md.rightLane.points, self._path_pinv) # right line
+ self.p_poly = model_polyfit(md.path.points, self._path_pinv) # predicted path
+ self.l_prob = md.leftLane.prob # left line prob
+ self.r_prob = md.rightLane.prob # right line prob
+
+ def update_lane(self, v_ego):
+ # only offset left and right lane lines; offsetting p_poly does not make sense
+ self.l_poly[3] += CAMERA_OFFSET
+ self.r_poly[3] += CAMERA_OFFSET
+
+ self.lr_prob = self.l_prob + self.r_prob - self.l_prob * self.r_prob
+
+ # Find current lanewidth
+ self.lane_width_certainty += 0.05 * (self.l_prob * self.r_prob - self.lane_width_certainty)
+ current_lane_width = abs(self.l_poly[3] - self.r_poly[3])
+ self.lane_width_estimate += 0.005 * (current_lane_width - self.lane_width_estimate)
+ speed_lane_width = interp(v_ego, [0., 31.], [2.8, 3.5])
+ self.lane_width = self.lane_width_certainty * self.lane_width_estimate + \
+ (1 - self.lane_width_certainty) * speed_lane_width
+
+ self.d_poly = calc_d_poly(self.l_poly, self.r_poly, self.p_poly, self.l_prob, self.r_prob, self.lane_width)
+
+ def update(self, v_ego, md):
+ self.parse_model(md)
+ self.update_lane(v_ego)
diff --git a/selfdrive/controls/lib/latcontrol_helpers.py b/selfdrive/controls/lib/latcontrol_helpers.py
index c04d820bb..ff5fa478a 100644
--- a/selfdrive/controls/lib/latcontrol_helpers.py
+++ b/selfdrive/controls/lib/latcontrol_helpers.py
@@ -60,30 +60,3 @@ def compute_path_pinv(l=50):
def model_polyfit(points, path_pinv):
return np.dot(path_pinv, [float(x) for x in points])
-
-
-def calc_desired_path(l_poly,
- r_poly,
- p_poly,
- l_prob,
- r_prob,
- p_prob,
- speed,
- lane_width=None):
- # this function computes the poly for the center of the lane, averaging left and right polys
- if lane_width is None:
- lane_width = 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 = l_prob + r_prob - l_prob * r_prob
- 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
diff --git a/selfdrive/controls/lib/latcontrol_indi.py b/selfdrive/controls/lib/latcontrol_indi.py
index 31ff6dd3d..38ba8883b 100644
--- a/selfdrive/controls/lib/latcontrol_indi.py
+++ b/selfdrive/controls/lib/latcontrol_indi.py
@@ -47,7 +47,7 @@ class LatControlINDI(object):
self.output_steer = 0.
self.counter = 0
- def update(self, active, v_ego, angle_steers, angle_steers_rate, steer_override, CP, VM, path_plan):
+ def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, steer_override, CP, VM, path_plan):
# Update Kalman filter
y = np.matrix([[math.radians(angle_steers)], [math.radians(angle_steers_rate)]])
self.x = np.dot(self.A_K, self.x) + np.dot(self.K, y)
diff --git a/selfdrive/controls/lib/latcontrol_lqr.py b/selfdrive/controls/lib/latcontrol_lqr.py
new file mode 100644
index 000000000..8c0e3ec31
--- /dev/null
+++ b/selfdrive/controls/lib/latcontrol_lqr.py
@@ -0,0 +1,72 @@
+import numpy as np
+from selfdrive.controls.lib.drive_helpers import get_steer_max
+from common.numpy_fast import clip
+from cereal import log
+
+
+class LatControlLQR(object):
+ def __init__(self, CP, rate=100):
+ self.sat_flag = False
+ self.scale = CP.lateralTuning.lqr.scale
+ self.ki = CP.lateralTuning.lqr.ki
+
+
+ self.A = np.array(CP.lateralTuning.lqr.a).reshape((2,2))
+ self.B = np.array(CP.lateralTuning.lqr.b).reshape((2,1))
+ self.C = np.array(CP.lateralTuning.lqr.c).reshape((1,2))
+ self.K = np.array(CP.lateralTuning.lqr.k).reshape((1,2))
+ self.L = np.array(CP.lateralTuning.lqr.l).reshape((2,1))
+ self.dc_gain = CP.lateralTuning.lqr.dcGain
+
+ self.x_hat = np.array([[0], [0]])
+ self.i_unwind_rate = 0.3 / rate
+ self.i_rate = 1.0 / rate
+
+ self.reset()
+
+ def reset(self):
+ self.i_lqr = 0.0
+ self.output_steer = 0.0
+
+ def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, steer_override, CP, VM, path_plan):
+ lqr_log = log.ControlsState.LateralLQRState.new_message()
+
+ torque_scale = (0.45 + v_ego / 60.0)**2 # Scale actuator model with speed
+
+ # Subtract offset. Zero angle should correspond to zero torque
+ self.angle_steers_des = path_plan.angleSteers - path_plan.angleOffset
+ angle_steers -= path_plan.angleOffset
+
+ # Update Kalman filter
+ angle_steers_k = float(self.C.dot(self.x_hat))
+ e = angle_steers - angle_steers_k
+ self.x_hat = self.A.dot(self.x_hat) + self.B.dot(eps_torque / torque_scale) + self.L.dot(e)
+
+ if v_ego < 0.3 or not active:
+ lqr_log.active = False
+ self.reset()
+ else:
+ lqr_log.active = True
+
+ # LQR
+ u_lqr = float(self.angle_steers_des / self.dc_gain - self.K.dot(self.x_hat))
+
+ # Integrator
+ if steer_override:
+ self.i_lqr -= self.i_unwind_rate * float(np.sign(self.i_lqr))
+ else:
+ self.i_lqr += self.ki * self.i_rate * (self.angle_steers_des - angle_steers_k)
+
+ lqr_output = torque_scale * u_lqr / self.scale
+ self.i_lqr = clip(self.i_lqr, -1.0 - lqr_output, 1.0 - lqr_output) # (LQR + I) has to be between -1 and 1
+
+ self.output_steer = lqr_output + self.i_lqr
+
+ # Clip output
+ steers_max = get_steer_max(CP, v_ego)
+ self.output_steer = clip(self.output_steer, -steers_max, steers_max)
+
+ lqr_log.steerAngle = angle_steers_k + path_plan.angleOffset
+ lqr_log.i = self.i_lqr
+ lqr_log.output = self.output_steer
+ return self.output_steer, float(self.angle_steers_des), lqr_log
diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py
index 65a5a8b6d..5fe456d88 100644
--- a/selfdrive/controls/lib/latcontrol_pid.py
+++ b/selfdrive/controls/lib/latcontrol_pid.py
@@ -14,7 +14,7 @@ class LatControlPID(object):
def reset(self):
self.pid.reset()
- def update(self, active, v_ego, angle_steers, angle_steers_rate, steer_override, CP, VM, path_plan):
+ def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, steer_override, CP, VM, path_plan):
pid_log = log.ControlsState.LateralPIDState.new_message()
pid_log.steerAngle = float(angle_steers)
pid_log.steerRate = float(angle_steers_rate)
diff --git a/selfdrive/controls/lib/lateral_mpc/generator.cpp b/selfdrive/controls/lib/lateral_mpc/generator.cpp
index 523ed8ac8..5f4a9a28d 100644
--- a/selfdrive/controls/lib/lateral_mpc/generator.cpp
+++ b/selfdrive/controls/lib/lateral_mpc/generator.cpp
@@ -23,8 +23,8 @@ int main( )
OnlineData v_ref; // m/s
OnlineData l_poly_r0, l_poly_r1, l_poly_r2, l_poly_r3;
OnlineData r_poly_r0, r_poly_r1, r_poly_r2, r_poly_r3;
- OnlineData p_poly_r0, p_poly_r1, p_poly_r2, p_poly_r3;
- OnlineData l_prob, r_prob, p_prob;
+ OnlineData d_poly_r0, d_poly_r1, d_poly_r2, d_poly_r3;
+ OnlineData l_prob, r_prob;
OnlineData lane_width;
Control t;
@@ -39,26 +39,13 @@ int main( )
auto poly_l = l_poly_r0*(xx*xx*xx) + l_poly_r1*(xx*xx) + l_poly_r2*xx + l_poly_r3;
auto poly_r = r_poly_r0*(xx*xx*xx) + r_poly_r1*(xx*xx) + r_poly_r2*xx + r_poly_r3;
- auto poly_p = p_poly_r0*(xx*xx*xx) + p_poly_r1*(xx*xx) + p_poly_r2*xx + p_poly_r3;
+ auto poly_d = d_poly_r0*(xx*xx*xx) + d_poly_r1*(xx*xx) + d_poly_r2*xx + d_poly_r3;
- auto angle_l = atan(3*l_poly_r0*xx*xx + 2*l_poly_r1*xx + l_poly_r2);
- auto angle_r = atan(3*r_poly_r0*xx*xx + 2*r_poly_r1*xx + r_poly_r2);
- auto angle_p = atan(3*p_poly_r0*xx*xx + 2*p_poly_r1*xx + p_poly_r2);
-
- // given the lane width estimate, this is where we estimate the path given lane lines
- auto path_from_left_lane = poly_l - lane_width/2.0;
- auto path_from_right_lane = poly_r + lane_width/2.0;
-
- // if the lanes are visible, drive in the center, otherwise follow the path
- auto path = lr_prob * (l_prob * path_from_left_lane + r_prob * path_from_right_lane) / (l_prob + r_prob + 0.0001)
- + (1-lr_prob) * poly_p;
-
- auto angle = lr_prob * (l_prob * angle_l + r_prob * angle_r) / (l_prob + r_prob + 0.0001)
- + (1-lr_prob) * angle_p;
+ auto angle_d = atan(3*d_poly_r0*xx*xx + 2*d_poly_r1*xx + d_poly_r2);
// When the lane is not visible, use an estimate of its position
- auto weighted_left_lane = l_prob * poly_l + (1 - l_prob) * (path + lane_width/2.0);
- auto weighted_right_lane = r_prob * poly_r + (1 - r_prob) * (path - lane_width/2.0);
+ auto weighted_left_lane = l_prob * poly_l + (1 - l_prob) * (poly_d + lane_width/2.0);
+ auto weighted_right_lane = r_prob * poly_r + (1 - r_prob) * (poly_d - lane_width/2.0);
auto c_left_lane = exp(-(weighted_left_lane - yy));
auto c_right_lane = exp(weighted_right_lane - yy);
@@ -67,12 +54,12 @@ int main( )
Function h;
// Distance errors
- h << path - yy;
+ h << poly_d - yy;
h << lr_prob * c_left_lane;
h << lr_prob * c_right_lane;
// Heading error
- h << (v_ref + 1.0 ) * (angle - psi);
+ h << (v_ref + 1.0 ) * (angle_d - psi);
// Angular rate error
h << (v_ref + 1.0 ) * t;
@@ -88,12 +75,12 @@ int main( )
Function hN;
// Distance errors
- hN << path - yy;
+ hN << poly_d - yy;
hN << l_prob * c_left_lane;
hN << r_prob * c_right_lane;
// Heading errors
- hN << (2.0 * v_ref + 1.0 ) * (angle - psi);
+ hN << (2.0 * v_ref + 1.0 ) * (angle_d - psi);
BMatrix QN(4,4); QN.setAll(true);
// QN(0,0) = 1.0;
@@ -125,7 +112,7 @@ int main( )
ocp.subjectTo( deg2rad(-90) <= psi <= deg2rad(90));
// more than absolute max steer angle
ocp.subjectTo( deg2rad(-50) <= delta <= deg2rad(50));
- ocp.setNOD(18);
+ ocp.setNOD(17);
OCPexport mpc(ocp);
mpc.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON );
diff --git a/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c b/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c
index ea6f85b30..d8d29cc61 100644
--- a/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c
+++ b/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c
@@ -65,8 +65,8 @@ void init(double pathCost, double laneCost, double headingCost, double steerRate
}
int run_mpc(state_t * x0, log_t * solution,
- double l_poly[4], double r_poly[4], double p_poly[4],
- double l_prob, double r_prob, double p_prob, double curvature_factor, double v_ref, double lane_width){
+ double l_poly[4], double r_poly[4], double d_poly[4],
+ double l_prob, double r_prob, double curvature_factor, double v_ref, double lane_width){
int i;
@@ -84,16 +84,15 @@ int run_mpc(state_t * x0, log_t * solution,
acadoVariables.od[i+8] = r_poly[2];
acadoVariables.od[i+9] = r_poly[3];
- acadoVariables.od[i+10] = p_poly[0];
- acadoVariables.od[i+11] = p_poly[1];
- acadoVariables.od[i+12] = p_poly[2];
- acadoVariables.od[i+13] = p_poly[3];
+ acadoVariables.od[i+10] = d_poly[0];
+ acadoVariables.od[i+11] = d_poly[1];
+ acadoVariables.od[i+12] = d_poly[2];
+ acadoVariables.od[i+13] = d_poly[3];
acadoVariables.od[i+14] = l_prob;
acadoVariables.od[i+15] = r_prob;
- acadoVariables.od[i+16] = p_prob;
- acadoVariables.od[i+17] = lane_width;
+ acadoVariables.od[i+16] = lane_width;
}
diff --git a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_common.h b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_common.h
index d5b567277..f069b62c0 100644
--- a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_common.h
+++ b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_common.h
@@ -64,7 +64,7 @@ extern "C"
/** Number of control/estimation intervals. */
#define ACADO_N 20
/** Number of online data values. */
-#define ACADO_NOD 18
+#define ACADO_NOD 17
/** Number of path constraints. */
#define ACADO_NPAC 0
/** Number of control variables. */
@@ -114,11 +114,11 @@ real_t x[ 84 ];
*/
real_t u[ 20 ];
-/** Matrix of size: 21 x 18 (row major format)
+/** Matrix of size: 21 x 17 (row major format)
*
* Matrix containing 21 online data vectors.
*/
-real_t od[ 378 ];
+real_t od[ 357 ];
/** Column vector of size: 100
*
@@ -160,14 +160,14 @@ real_t rhs_aux[ 14 ];
real_t rk_ttt;
-/** Row vector of size: 43 */
-real_t rk_xxx[ 43 ];
+/** Row vector of size: 42 */
+real_t rk_xxx[ 42 ];
/** Matrix of size: 4 x 24 (row major format) */
real_t rk_kkk[ 96 ];
-/** Row vector of size: 43 */
-real_t state[ 43 ];
+/** Row vector of size: 42 */
+real_t state[ 42 ];
/** Column vector of size: 80 */
real_t d[ 80 ];
@@ -184,11 +184,11 @@ real_t evGx[ 320 ];
/** Column vector of size: 80 */
real_t evGu[ 80 ];
-/** Column vector of size: 21 */
-real_t objAuxVar[ 21 ];
+/** Column vector of size: 11 */
+real_t objAuxVar[ 11 ];
-/** Row vector of size: 23 */
-real_t objValueIn[ 23 ];
+/** Row vector of size: 22 */
+real_t objValueIn[ 22 ];
/** Row vector of size: 30 */
real_t objValueOut[ 30 ];
diff --git a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.c b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.c
index b93108899..5df7cb47f 100644
--- a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.c
+++ b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.c
@@ -118,7 +118,6 @@ acadoWorkspace.rk_xxx[38] = rk_eta[38];
acadoWorkspace.rk_xxx[39] = rk_eta[39];
acadoWorkspace.rk_xxx[40] = rk_eta[40];
acadoWorkspace.rk_xxx[41] = rk_eta[41];
-acadoWorkspace.rk_xxx[42] = rk_eta[42];
for (run1 = 0; run1 < 1; ++run1)
{
diff --git a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.c b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.c
index 014c19a01..347e77fa7 100644
--- a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.c
+++ b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.c
@@ -43,24 +43,23 @@ acadoWorkspace.state[2] = acadoVariables.x[lRun1 * 4 + 2];
acadoWorkspace.state[3] = acadoVariables.x[lRun1 * 4 + 3];
acadoWorkspace.state[24] = acadoVariables.u[lRun1];
-acadoWorkspace.state[25] = acadoVariables.od[lRun1 * 18];
-acadoWorkspace.state[26] = acadoVariables.od[lRun1 * 18 + 1];
-acadoWorkspace.state[27] = acadoVariables.od[lRun1 * 18 + 2];
-acadoWorkspace.state[28] = acadoVariables.od[lRun1 * 18 + 3];
-acadoWorkspace.state[29] = acadoVariables.od[lRun1 * 18 + 4];
-acadoWorkspace.state[30] = acadoVariables.od[lRun1 * 18 + 5];
-acadoWorkspace.state[31] = acadoVariables.od[lRun1 * 18 + 6];
-acadoWorkspace.state[32] = acadoVariables.od[lRun1 * 18 + 7];
-acadoWorkspace.state[33] = acadoVariables.od[lRun1 * 18 + 8];
-acadoWorkspace.state[34] = acadoVariables.od[lRun1 * 18 + 9];
-acadoWorkspace.state[35] = acadoVariables.od[lRun1 * 18 + 10];
-acadoWorkspace.state[36] = acadoVariables.od[lRun1 * 18 + 11];
-acadoWorkspace.state[37] = acadoVariables.od[lRun1 * 18 + 12];
-acadoWorkspace.state[38] = acadoVariables.od[lRun1 * 18 + 13];
-acadoWorkspace.state[39] = acadoVariables.od[lRun1 * 18 + 14];
-acadoWorkspace.state[40] = acadoVariables.od[lRun1 * 18 + 15];
-acadoWorkspace.state[41] = acadoVariables.od[lRun1 * 18 + 16];
-acadoWorkspace.state[42] = acadoVariables.od[lRun1 * 18 + 17];
+acadoWorkspace.state[25] = acadoVariables.od[lRun1 * 17];
+acadoWorkspace.state[26] = acadoVariables.od[lRun1 * 17 + 1];
+acadoWorkspace.state[27] = acadoVariables.od[lRun1 * 17 + 2];
+acadoWorkspace.state[28] = acadoVariables.od[lRun1 * 17 + 3];
+acadoWorkspace.state[29] = acadoVariables.od[lRun1 * 17 + 4];
+acadoWorkspace.state[30] = acadoVariables.od[lRun1 * 17 + 5];
+acadoWorkspace.state[31] = acadoVariables.od[lRun1 * 17 + 6];
+acadoWorkspace.state[32] = acadoVariables.od[lRun1 * 17 + 7];
+acadoWorkspace.state[33] = acadoVariables.od[lRun1 * 17 + 8];
+acadoWorkspace.state[34] = acadoVariables.od[lRun1 * 17 + 9];
+acadoWorkspace.state[35] = acadoVariables.od[lRun1 * 17 + 10];
+acadoWorkspace.state[36] = acadoVariables.od[lRun1 * 17 + 11];
+acadoWorkspace.state[37] = acadoVariables.od[lRun1 * 17 + 12];
+acadoWorkspace.state[38] = acadoVariables.od[lRun1 * 17 + 13];
+acadoWorkspace.state[39] = acadoVariables.od[lRun1 * 17 + 14];
+acadoWorkspace.state[40] = acadoVariables.od[lRun1 * 17 + 15];
+acadoWorkspace.state[41] = acadoVariables.od[lRun1 * 17 + 16];
ret = acado_integrate(acadoWorkspace.state, 1, lRun1);
@@ -99,51 +98,41 @@ void acado_evaluateLSQ(const real_t* in, real_t* out)
const real_t* xd = in;
const real_t* u = in + 4;
const real_t* od = in + 5;
-/* Vector of auxiliary variables; number of elements: 21. */
+/* Vector of auxiliary variables; number of elements: 11. */
real_t* a = acadoWorkspace.objAuxVar;
/* Compute intermediate quantities: */
-a[0] = (exp(((real_t)(0.0000000000000000e+00)-(((od[14]*((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5]))+(((real_t)(1.0000000000000000e+00)-od[14])*((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))+(od[17]/(real_t)(2.0000000000000000e+00)))))-xd[1]))));
-a[1] = (exp((((od[15]*((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9]))+(((real_t)(1.0000000000000000e+00)-od[15])*((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))-(od[17]/(real_t)(2.0000000000000000e+00)))))-xd[1])));
-a[2] = (atan(((((((real_t)(3.0000000000000000e+00)*od[2])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[3])*xd[0]))+od[4])));
-a[3] = (atan(((((((real_t)(3.0000000000000000e+00)*od[6])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[7])*xd[0]))+od[8])));
-a[4] = (atan(((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[11])*xd[0]))+od[12])));
-a[5] = ((real_t)(1.0000000000000000e+00)/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)));
-a[6] = ((real_t)(1.0000000000000000e+00)/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)));
-a[7] = (exp(((real_t)(0.0000000000000000e+00)-(((od[14]*((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5]))+(((real_t)(1.0000000000000000e+00)-od[14])*((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))+(od[17]/(real_t)(2.0000000000000000e+00)))))-xd[1]))));
-a[8] = (((real_t)(0.0000000000000000e+00)-((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(((real_t)(1.0000000000000000e+00)-od[14])*(((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))))*a[6])+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12]))))))*a[7]);
-a[9] = (((real_t)(0.0000000000000000e+00)-((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)))*a[7]);
-a[10] = ((real_t)(1.0000000000000000e+00)/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)));
-a[11] = (exp((((od[15]*((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9]))+(((real_t)(1.0000000000000000e+00)-od[15])*((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))-(od[17]/(real_t)(2.0000000000000000e+00)))))-xd[1])));
-a[12] = (((od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))+(((real_t)(1.0000000000000000e+00)-od[15])*(((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))))*a[10])+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12])))))*a[11]);
-a[13] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[11]);
-a[14] = ((real_t)(1.0000000000000000e+00)/((real_t)(1.0000000000000000e+00)+(pow(((((((real_t)(3.0000000000000000e+00)*od[2])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[3])*xd[0]))+od[4]),2))));
-a[15] = ((((((real_t)(3.0000000000000000e+00)*od[2])*xd[0])+(((real_t)(3.0000000000000000e+00)*od[2])*xd[0]))+((real_t)(2.0000000000000000e+00)*od[3]))*a[14]);
-a[16] = ((real_t)(1.0000000000000000e+00)/((real_t)(1.0000000000000000e+00)+(pow(((((((real_t)(3.0000000000000000e+00)*od[6])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[7])*xd[0]))+od[8]),2))));
-a[17] = ((((((real_t)(3.0000000000000000e+00)*od[6])*xd[0])+(((real_t)(3.0000000000000000e+00)*od[6])*xd[0]))+((real_t)(2.0000000000000000e+00)*od[7]))*a[16]);
-a[18] = ((real_t)(1.0000000000000000e+00)/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)));
-a[19] = ((real_t)(1.0000000000000000e+00)/((real_t)(1.0000000000000000e+00)+(pow(((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[11])*xd[0]))+od[12]),2))));
-a[20] = ((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])+(((real_t)(3.0000000000000000e+00)*od[10])*xd[0]))+((real_t)(2.0000000000000000e+00)*od[11]))*a[19]);
+a[0] = (exp(((real_t)(0.0000000000000000e+00)-(((od[14]*((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5]))+(((real_t)(1.0000000000000000e+00)-od[14])*(((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])+(od[16]/(real_t)(2.0000000000000000e+00)))))-xd[1]))));
+a[1] = (exp((((od[15]*((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9]))+(((real_t)(1.0000000000000000e+00)-od[15])*(((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])-(od[16]/(real_t)(2.0000000000000000e+00)))))-xd[1])));
+a[2] = (atan(((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[11])*xd[0]))+od[12])));
+a[3] = (exp(((real_t)(0.0000000000000000e+00)-(((od[14]*((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5]))+(((real_t)(1.0000000000000000e+00)-od[14])*(((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])+(od[16]/(real_t)(2.0000000000000000e+00)))))-xd[1]))));
+a[4] = (((real_t)(0.0000000000000000e+00)-((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(((real_t)(1.0000000000000000e+00)-od[14])*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12]))))*a[3]);
+a[5] = (((real_t)(0.0000000000000000e+00)-((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)))*a[3]);
+a[6] = (exp((((od[15]*((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9]))+(((real_t)(1.0000000000000000e+00)-od[15])*(((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])-(od[16]/(real_t)(2.0000000000000000e+00)))))-xd[1])));
+a[7] = (((od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))+(((real_t)(1.0000000000000000e+00)-od[15])*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12])))*a[6]);
+a[8] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[6]);
+a[9] = ((real_t)(1.0000000000000000e+00)/((real_t)(1.0000000000000000e+00)+(pow(((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[11])*xd[0]))+od[12]),2))));
+a[10] = ((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])+(((real_t)(3.0000000000000000e+00)*od[10])*xd[0]))+((real_t)(2.0000000000000000e+00)*od[11]))*a[9]);
/* Compute outputs: */
-out[0] = ((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))-xd[1]);
+out[0] = (((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])-xd[1]);
out[1] = (((od[14]+od[15])-(od[14]*od[15]))*a[0]);
out[2] = (((od[14]+od[15])-(od[14]*od[15]))*a[1]);
-out[3] = ((od[1]+(real_t)(1.0000000000000000e+00))*((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*a[2])+(od[15]*a[3])))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*a[4]))-xd[2]));
+out[3] = ((od[1]+(real_t)(1.0000000000000000e+00))*(a[2]-xd[2]));
out[4] = ((od[1]+(real_t)(1.0000000000000000e+00))*u[0]);
-out[5] = (((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))))*a[5])+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12])));
+out[5] = (((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12]);
out[6] = ((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00));
out[7] = (real_t)(0.0000000000000000e+00);
out[8] = (real_t)(0.0000000000000000e+00);
-out[9] = (((od[14]+od[15])-(od[14]*od[15]))*a[8]);
-out[10] = (((od[14]+od[15])-(od[14]*od[15]))*a[9]);
+out[9] = (((od[14]+od[15])-(od[14]*od[15]))*a[4]);
+out[10] = (((od[14]+od[15])-(od[14]*od[15]))*a[5]);
out[11] = (real_t)(0.0000000000000000e+00);
out[12] = (real_t)(0.0000000000000000e+00);
-out[13] = (((od[14]+od[15])-(od[14]*od[15]))*a[12]);
-out[14] = (((od[14]+od[15])-(od[14]*od[15]))*a[13]);
+out[13] = (((od[14]+od[15])-(od[14]*od[15]))*a[7]);
+out[14] = (((od[14]+od[15])-(od[14]*od[15]))*a[8]);
out[15] = (real_t)(0.0000000000000000e+00);
out[16] = (real_t)(0.0000000000000000e+00);
-out[17] = ((od[1]+(real_t)(1.0000000000000000e+00))*(((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*a[15])+(od[15]*a[17])))*a[18])+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*a[20])));
+out[17] = ((od[1]+(real_t)(1.0000000000000000e+00))*a[10]);
out[18] = (real_t)(0.0000000000000000e+00);
out[19] = ((od[1]+(real_t)(1.0000000000000000e+00))*((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)));
out[20] = (real_t)(0.0000000000000000e+00);
@@ -162,50 +151,40 @@ void acado_evaluateLSQEndTerm(const real_t* in, real_t* out)
{
const real_t* xd = in;
const real_t* od = in + 4;
-/* Vector of auxiliary variables; number of elements: 21. */
+/* Vector of auxiliary variables; number of elements: 11. */
real_t* a = acadoWorkspace.objAuxVar;
/* Compute intermediate quantities: */
-a[0] = (exp(((real_t)(0.0000000000000000e+00)-(((od[14]*((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5]))+(((real_t)(1.0000000000000000e+00)-od[14])*((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))+(od[17]/(real_t)(2.0000000000000000e+00)))))-xd[1]))));
-a[1] = (exp((((od[15]*((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9]))+(((real_t)(1.0000000000000000e+00)-od[15])*((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))-(od[17]/(real_t)(2.0000000000000000e+00)))))-xd[1])));
-a[2] = (atan(((((((real_t)(3.0000000000000000e+00)*od[2])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[3])*xd[0]))+od[4])));
-a[3] = (atan(((((((real_t)(3.0000000000000000e+00)*od[6])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[7])*xd[0]))+od[8])));
-a[4] = (atan(((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[11])*xd[0]))+od[12])));
-a[5] = ((real_t)(1.0000000000000000e+00)/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)));
-a[6] = ((real_t)(1.0000000000000000e+00)/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)));
-a[7] = (exp(((real_t)(0.0000000000000000e+00)-(((od[14]*((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5]))+(((real_t)(1.0000000000000000e+00)-od[14])*((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))+(od[17]/(real_t)(2.0000000000000000e+00)))))-xd[1]))));
-a[8] = (((real_t)(0.0000000000000000e+00)-((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(((real_t)(1.0000000000000000e+00)-od[14])*(((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))))*a[6])+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12]))))))*a[7]);
-a[9] = (((real_t)(0.0000000000000000e+00)-((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)))*a[7]);
-a[10] = ((real_t)(1.0000000000000000e+00)/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)));
-a[11] = (exp((((od[15]*((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9]))+(((real_t)(1.0000000000000000e+00)-od[15])*((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))-(od[17]/(real_t)(2.0000000000000000e+00)))))-xd[1])));
-a[12] = (((od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))+(((real_t)(1.0000000000000000e+00)-od[15])*(((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))))*a[10])+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12])))))*a[11]);
-a[13] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[11]);
-a[14] = ((real_t)(1.0000000000000000e+00)/((real_t)(1.0000000000000000e+00)+(pow(((((((real_t)(3.0000000000000000e+00)*od[2])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[3])*xd[0]))+od[4]),2))));
-a[15] = ((((((real_t)(3.0000000000000000e+00)*od[2])*xd[0])+(((real_t)(3.0000000000000000e+00)*od[2])*xd[0]))+((real_t)(2.0000000000000000e+00)*od[3]))*a[14]);
-a[16] = ((real_t)(1.0000000000000000e+00)/((real_t)(1.0000000000000000e+00)+(pow(((((((real_t)(3.0000000000000000e+00)*od[6])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[7])*xd[0]))+od[8]),2))));
-a[17] = ((((((real_t)(3.0000000000000000e+00)*od[6])*xd[0])+(((real_t)(3.0000000000000000e+00)*od[6])*xd[0]))+((real_t)(2.0000000000000000e+00)*od[7]))*a[16]);
-a[18] = ((real_t)(1.0000000000000000e+00)/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)));
-a[19] = ((real_t)(1.0000000000000000e+00)/((real_t)(1.0000000000000000e+00)+(pow(((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[11])*xd[0]))+od[12]),2))));
-a[20] = ((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])+(((real_t)(3.0000000000000000e+00)*od[10])*xd[0]))+((real_t)(2.0000000000000000e+00)*od[11]))*a[19]);
+a[0] = (exp(((real_t)(0.0000000000000000e+00)-(((od[14]*((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5]))+(((real_t)(1.0000000000000000e+00)-od[14])*(((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])+(od[16]/(real_t)(2.0000000000000000e+00)))))-xd[1]))));
+a[1] = (exp((((od[15]*((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9]))+(((real_t)(1.0000000000000000e+00)-od[15])*(((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])-(od[16]/(real_t)(2.0000000000000000e+00)))))-xd[1])));
+a[2] = (atan(((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[11])*xd[0]))+od[12])));
+a[3] = (exp(((real_t)(0.0000000000000000e+00)-(((od[14]*((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5]))+(((real_t)(1.0000000000000000e+00)-od[14])*(((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])+(od[16]/(real_t)(2.0000000000000000e+00)))))-xd[1]))));
+a[4] = (((real_t)(0.0000000000000000e+00)-((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(((real_t)(1.0000000000000000e+00)-od[14])*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12]))))*a[3]);
+a[5] = (((real_t)(0.0000000000000000e+00)-((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)))*a[3]);
+a[6] = (exp((((od[15]*((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9]))+(((real_t)(1.0000000000000000e+00)-od[15])*(((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])-(od[16]/(real_t)(2.0000000000000000e+00)))))-xd[1])));
+a[7] = (((od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))+(((real_t)(1.0000000000000000e+00)-od[15])*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12])))*a[6]);
+a[8] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[6]);
+a[9] = ((real_t)(1.0000000000000000e+00)/((real_t)(1.0000000000000000e+00)+(pow(((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[11])*xd[0]))+od[12]),2))));
+a[10] = ((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])+(((real_t)(3.0000000000000000e+00)*od[10])*xd[0]))+((real_t)(2.0000000000000000e+00)*od[11]))*a[9]);
/* Compute outputs: */
-out[0] = ((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))-xd[1]);
+out[0] = (((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])-xd[1]);
out[1] = (od[14]*a[0]);
out[2] = (od[15]*a[1]);
-out[3] = ((((real_t)(2.0000000000000000e+00)*od[1])+(real_t)(1.0000000000000000e+00))*((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*a[2])+(od[15]*a[3])))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*a[4]))-xd[2]));
-out[4] = (((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))))*a[5])+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12])));
+out[3] = ((((real_t)(2.0000000000000000e+00)*od[1])+(real_t)(1.0000000000000000e+00))*(a[2]-xd[2]));
+out[4] = (((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12]);
out[5] = ((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00));
out[6] = (real_t)(0.0000000000000000e+00);
out[7] = (real_t)(0.0000000000000000e+00);
-out[8] = (od[14]*a[8]);
-out[9] = (od[14]*a[9]);
+out[8] = (od[14]*a[4]);
+out[9] = (od[14]*a[5]);
out[10] = (real_t)(0.0000000000000000e+00);
out[11] = (real_t)(0.0000000000000000e+00);
-out[12] = (od[15]*a[12]);
-out[13] = (od[15]*a[13]);
+out[12] = (od[15]*a[7]);
+out[13] = (od[15]*a[8]);
out[14] = (real_t)(0.0000000000000000e+00);
out[15] = (real_t)(0.0000000000000000e+00);
-out[16] = ((((real_t)(2.0000000000000000e+00)*od[1])+(real_t)(1.0000000000000000e+00))*(((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*a[15])+(od[15]*a[17])))*a[18])+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*a[20])));
+out[16] = ((((real_t)(2.0000000000000000e+00)*od[1])+(real_t)(1.0000000000000000e+00))*a[10]);
out[17] = (real_t)(0.0000000000000000e+00);
out[18] = ((((real_t)(2.0000000000000000e+00)*od[1])+(real_t)(1.0000000000000000e+00))*((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)));
out[19] = (real_t)(0.0000000000000000e+00);
@@ -307,24 +286,23 @@ acadoWorkspace.objValueIn[1] = acadoVariables.x[runObj * 4 + 1];
acadoWorkspace.objValueIn[2] = acadoVariables.x[runObj * 4 + 2];
acadoWorkspace.objValueIn[3] = acadoVariables.x[runObj * 4 + 3];
acadoWorkspace.objValueIn[4] = acadoVariables.u[runObj];
-acadoWorkspace.objValueIn[5] = acadoVariables.od[runObj * 18];
-acadoWorkspace.objValueIn[6] = acadoVariables.od[runObj * 18 + 1];
-acadoWorkspace.objValueIn[7] = acadoVariables.od[runObj * 18 + 2];
-acadoWorkspace.objValueIn[8] = acadoVariables.od[runObj * 18 + 3];
-acadoWorkspace.objValueIn[9] = acadoVariables.od[runObj * 18 + 4];
-acadoWorkspace.objValueIn[10] = acadoVariables.od[runObj * 18 + 5];
-acadoWorkspace.objValueIn[11] = acadoVariables.od[runObj * 18 + 6];
-acadoWorkspace.objValueIn[12] = acadoVariables.od[runObj * 18 + 7];
-acadoWorkspace.objValueIn[13] = acadoVariables.od[runObj * 18 + 8];
-acadoWorkspace.objValueIn[14] = acadoVariables.od[runObj * 18 + 9];
-acadoWorkspace.objValueIn[15] = acadoVariables.od[runObj * 18 + 10];
-acadoWorkspace.objValueIn[16] = acadoVariables.od[runObj * 18 + 11];
-acadoWorkspace.objValueIn[17] = acadoVariables.od[runObj * 18 + 12];
-acadoWorkspace.objValueIn[18] = acadoVariables.od[runObj * 18 + 13];
-acadoWorkspace.objValueIn[19] = acadoVariables.od[runObj * 18 + 14];
-acadoWorkspace.objValueIn[20] = acadoVariables.od[runObj * 18 + 15];
-acadoWorkspace.objValueIn[21] = acadoVariables.od[runObj * 18 + 16];
-acadoWorkspace.objValueIn[22] = acadoVariables.od[runObj * 18 + 17];
+acadoWorkspace.objValueIn[5] = acadoVariables.od[runObj * 17];
+acadoWorkspace.objValueIn[6] = acadoVariables.od[runObj * 17 + 1];
+acadoWorkspace.objValueIn[7] = acadoVariables.od[runObj * 17 + 2];
+acadoWorkspace.objValueIn[8] = acadoVariables.od[runObj * 17 + 3];
+acadoWorkspace.objValueIn[9] = acadoVariables.od[runObj * 17 + 4];
+acadoWorkspace.objValueIn[10] = acadoVariables.od[runObj * 17 + 5];
+acadoWorkspace.objValueIn[11] = acadoVariables.od[runObj * 17 + 6];
+acadoWorkspace.objValueIn[12] = acadoVariables.od[runObj * 17 + 7];
+acadoWorkspace.objValueIn[13] = acadoVariables.od[runObj * 17 + 8];
+acadoWorkspace.objValueIn[14] = acadoVariables.od[runObj * 17 + 9];
+acadoWorkspace.objValueIn[15] = acadoVariables.od[runObj * 17 + 10];
+acadoWorkspace.objValueIn[16] = acadoVariables.od[runObj * 17 + 11];
+acadoWorkspace.objValueIn[17] = acadoVariables.od[runObj * 17 + 12];
+acadoWorkspace.objValueIn[18] = acadoVariables.od[runObj * 17 + 13];
+acadoWorkspace.objValueIn[19] = acadoVariables.od[runObj * 17 + 14];
+acadoWorkspace.objValueIn[20] = acadoVariables.od[runObj * 17 + 15];
+acadoWorkspace.objValueIn[21] = acadoVariables.od[runObj * 17 + 16];
acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut );
acadoWorkspace.Dy[runObj * 5] = acadoWorkspace.objValueOut[0];
@@ -342,24 +320,23 @@ acadoWorkspace.objValueIn[0] = acadoVariables.x[80];
acadoWorkspace.objValueIn[1] = acadoVariables.x[81];
acadoWorkspace.objValueIn[2] = acadoVariables.x[82];
acadoWorkspace.objValueIn[3] = acadoVariables.x[83];
-acadoWorkspace.objValueIn[4] = acadoVariables.od[360];
-acadoWorkspace.objValueIn[5] = acadoVariables.od[361];
-acadoWorkspace.objValueIn[6] = acadoVariables.od[362];
-acadoWorkspace.objValueIn[7] = acadoVariables.od[363];
-acadoWorkspace.objValueIn[8] = acadoVariables.od[364];
-acadoWorkspace.objValueIn[9] = acadoVariables.od[365];
-acadoWorkspace.objValueIn[10] = acadoVariables.od[366];
-acadoWorkspace.objValueIn[11] = acadoVariables.od[367];
-acadoWorkspace.objValueIn[12] = acadoVariables.od[368];
-acadoWorkspace.objValueIn[13] = acadoVariables.od[369];
-acadoWorkspace.objValueIn[14] = acadoVariables.od[370];
-acadoWorkspace.objValueIn[15] = acadoVariables.od[371];
-acadoWorkspace.objValueIn[16] = acadoVariables.od[372];
-acadoWorkspace.objValueIn[17] = acadoVariables.od[373];
-acadoWorkspace.objValueIn[18] = acadoVariables.od[374];
-acadoWorkspace.objValueIn[19] = acadoVariables.od[375];
-acadoWorkspace.objValueIn[20] = acadoVariables.od[376];
-acadoWorkspace.objValueIn[21] = acadoVariables.od[377];
+acadoWorkspace.objValueIn[4] = acadoVariables.od[340];
+acadoWorkspace.objValueIn[5] = acadoVariables.od[341];
+acadoWorkspace.objValueIn[6] = acadoVariables.od[342];
+acadoWorkspace.objValueIn[7] = acadoVariables.od[343];
+acadoWorkspace.objValueIn[8] = acadoVariables.od[344];
+acadoWorkspace.objValueIn[9] = acadoVariables.od[345];
+acadoWorkspace.objValueIn[10] = acadoVariables.od[346];
+acadoWorkspace.objValueIn[11] = acadoVariables.od[347];
+acadoWorkspace.objValueIn[12] = acadoVariables.od[348];
+acadoWorkspace.objValueIn[13] = acadoVariables.od[349];
+acadoWorkspace.objValueIn[14] = acadoVariables.od[350];
+acadoWorkspace.objValueIn[15] = acadoVariables.od[351];
+acadoWorkspace.objValueIn[16] = acadoVariables.od[352];
+acadoWorkspace.objValueIn[17] = acadoVariables.od[353];
+acadoWorkspace.objValueIn[18] = acadoVariables.od[354];
+acadoWorkspace.objValueIn[19] = acadoVariables.od[355];
+acadoWorkspace.objValueIn[20] = acadoVariables.od[356];
acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut );
acadoWorkspace.DyN[0] = acadoWorkspace.objValueOut[0];
@@ -4966,24 +4943,23 @@ acadoWorkspace.state[1] = acadoVariables.x[index * 4 + 1];
acadoWorkspace.state[2] = acadoVariables.x[index * 4 + 2];
acadoWorkspace.state[3] = acadoVariables.x[index * 4 + 3];
acadoWorkspace.state[24] = acadoVariables.u[index];
-acadoWorkspace.state[25] = acadoVariables.od[index * 18];
-acadoWorkspace.state[26] = acadoVariables.od[index * 18 + 1];
-acadoWorkspace.state[27] = acadoVariables.od[index * 18 + 2];
-acadoWorkspace.state[28] = acadoVariables.od[index * 18 + 3];
-acadoWorkspace.state[29] = acadoVariables.od[index * 18 + 4];
-acadoWorkspace.state[30] = acadoVariables.od[index * 18 + 5];
-acadoWorkspace.state[31] = acadoVariables.od[index * 18 + 6];
-acadoWorkspace.state[32] = acadoVariables.od[index * 18 + 7];
-acadoWorkspace.state[33] = acadoVariables.od[index * 18 + 8];
-acadoWorkspace.state[34] = acadoVariables.od[index * 18 + 9];
-acadoWorkspace.state[35] = acadoVariables.od[index * 18 + 10];
-acadoWorkspace.state[36] = acadoVariables.od[index * 18 + 11];
-acadoWorkspace.state[37] = acadoVariables.od[index * 18 + 12];
-acadoWorkspace.state[38] = acadoVariables.od[index * 18 + 13];
-acadoWorkspace.state[39] = acadoVariables.od[index * 18 + 14];
-acadoWorkspace.state[40] = acadoVariables.od[index * 18 + 15];
-acadoWorkspace.state[41] = acadoVariables.od[index * 18 + 16];
-acadoWorkspace.state[42] = acadoVariables.od[index * 18 + 17];
+acadoWorkspace.state[25] = acadoVariables.od[index * 17];
+acadoWorkspace.state[26] = acadoVariables.od[index * 17 + 1];
+acadoWorkspace.state[27] = acadoVariables.od[index * 17 + 2];
+acadoWorkspace.state[28] = acadoVariables.od[index * 17 + 3];
+acadoWorkspace.state[29] = acadoVariables.od[index * 17 + 4];
+acadoWorkspace.state[30] = acadoVariables.od[index * 17 + 5];
+acadoWorkspace.state[31] = acadoVariables.od[index * 17 + 6];
+acadoWorkspace.state[32] = acadoVariables.od[index * 17 + 7];
+acadoWorkspace.state[33] = acadoVariables.od[index * 17 + 8];
+acadoWorkspace.state[34] = acadoVariables.od[index * 17 + 9];
+acadoWorkspace.state[35] = acadoVariables.od[index * 17 + 10];
+acadoWorkspace.state[36] = acadoVariables.od[index * 17 + 11];
+acadoWorkspace.state[37] = acadoVariables.od[index * 17 + 12];
+acadoWorkspace.state[38] = acadoVariables.od[index * 17 + 13];
+acadoWorkspace.state[39] = acadoVariables.od[index * 17 + 14];
+acadoWorkspace.state[40] = acadoVariables.od[index * 17 + 15];
+acadoWorkspace.state[41] = acadoVariables.od[index * 17 + 16];
acado_integrate(acadoWorkspace.state, index == 0, index);
@@ -5026,24 +5002,23 @@ else
{
acadoWorkspace.state[24] = acadoVariables.u[19];
}
-acadoWorkspace.state[25] = acadoVariables.od[360];
-acadoWorkspace.state[26] = acadoVariables.od[361];
-acadoWorkspace.state[27] = acadoVariables.od[362];
-acadoWorkspace.state[28] = acadoVariables.od[363];
-acadoWorkspace.state[29] = acadoVariables.od[364];
-acadoWorkspace.state[30] = acadoVariables.od[365];
-acadoWorkspace.state[31] = acadoVariables.od[366];
-acadoWorkspace.state[32] = acadoVariables.od[367];
-acadoWorkspace.state[33] = acadoVariables.od[368];
-acadoWorkspace.state[34] = acadoVariables.od[369];
-acadoWorkspace.state[35] = acadoVariables.od[370];
-acadoWorkspace.state[36] = acadoVariables.od[371];
-acadoWorkspace.state[37] = acadoVariables.od[372];
-acadoWorkspace.state[38] = acadoVariables.od[373];
-acadoWorkspace.state[39] = acadoVariables.od[374];
-acadoWorkspace.state[40] = acadoVariables.od[375];
-acadoWorkspace.state[41] = acadoVariables.od[376];
-acadoWorkspace.state[42] = acadoVariables.od[377];
+acadoWorkspace.state[25] = acadoVariables.od[340];
+acadoWorkspace.state[26] = acadoVariables.od[341];
+acadoWorkspace.state[27] = acadoVariables.od[342];
+acadoWorkspace.state[28] = acadoVariables.od[343];
+acadoWorkspace.state[29] = acadoVariables.od[344];
+acadoWorkspace.state[30] = acadoVariables.od[345];
+acadoWorkspace.state[31] = acadoVariables.od[346];
+acadoWorkspace.state[32] = acadoVariables.od[347];
+acadoWorkspace.state[33] = acadoVariables.od[348];
+acadoWorkspace.state[34] = acadoVariables.od[349];
+acadoWorkspace.state[35] = acadoVariables.od[350];
+acadoWorkspace.state[36] = acadoVariables.od[351];
+acadoWorkspace.state[37] = acadoVariables.od[352];
+acadoWorkspace.state[38] = acadoVariables.od[353];
+acadoWorkspace.state[39] = acadoVariables.od[354];
+acadoWorkspace.state[40] = acadoVariables.od[355];
+acadoWorkspace.state[41] = acadoVariables.od[356];
acado_integrate(acadoWorkspace.state, 1, 19);
@@ -5114,24 +5089,23 @@ acadoWorkspace.objValueIn[1] = acadoVariables.x[lRun1 * 4 + 1];
acadoWorkspace.objValueIn[2] = acadoVariables.x[lRun1 * 4 + 2];
acadoWorkspace.objValueIn[3] = acadoVariables.x[lRun1 * 4 + 3];
acadoWorkspace.objValueIn[4] = acadoVariables.u[lRun1];
-acadoWorkspace.objValueIn[5] = acadoVariables.od[lRun1 * 18];
-acadoWorkspace.objValueIn[6] = acadoVariables.od[lRun1 * 18 + 1];
-acadoWorkspace.objValueIn[7] = acadoVariables.od[lRun1 * 18 + 2];
-acadoWorkspace.objValueIn[8] = acadoVariables.od[lRun1 * 18 + 3];
-acadoWorkspace.objValueIn[9] = acadoVariables.od[lRun1 * 18 + 4];
-acadoWorkspace.objValueIn[10] = acadoVariables.od[lRun1 * 18 + 5];
-acadoWorkspace.objValueIn[11] = acadoVariables.od[lRun1 * 18 + 6];
-acadoWorkspace.objValueIn[12] = acadoVariables.od[lRun1 * 18 + 7];
-acadoWorkspace.objValueIn[13] = acadoVariables.od[lRun1 * 18 + 8];
-acadoWorkspace.objValueIn[14] = acadoVariables.od[lRun1 * 18 + 9];
-acadoWorkspace.objValueIn[15] = acadoVariables.od[lRun1 * 18 + 10];
-acadoWorkspace.objValueIn[16] = acadoVariables.od[lRun1 * 18 + 11];
-acadoWorkspace.objValueIn[17] = acadoVariables.od[lRun1 * 18 + 12];
-acadoWorkspace.objValueIn[18] = acadoVariables.od[lRun1 * 18 + 13];
-acadoWorkspace.objValueIn[19] = acadoVariables.od[lRun1 * 18 + 14];
-acadoWorkspace.objValueIn[20] = acadoVariables.od[lRun1 * 18 + 15];
-acadoWorkspace.objValueIn[21] = acadoVariables.od[lRun1 * 18 + 16];
-acadoWorkspace.objValueIn[22] = acadoVariables.od[lRun1 * 18 + 17];
+acadoWorkspace.objValueIn[5] = acadoVariables.od[lRun1 * 17];
+acadoWorkspace.objValueIn[6] = acadoVariables.od[lRun1 * 17 + 1];
+acadoWorkspace.objValueIn[7] = acadoVariables.od[lRun1 * 17 + 2];
+acadoWorkspace.objValueIn[8] = acadoVariables.od[lRun1 * 17 + 3];
+acadoWorkspace.objValueIn[9] = acadoVariables.od[lRun1 * 17 + 4];
+acadoWorkspace.objValueIn[10] = acadoVariables.od[lRun1 * 17 + 5];
+acadoWorkspace.objValueIn[11] = acadoVariables.od[lRun1 * 17 + 6];
+acadoWorkspace.objValueIn[12] = acadoVariables.od[lRun1 * 17 + 7];
+acadoWorkspace.objValueIn[13] = acadoVariables.od[lRun1 * 17 + 8];
+acadoWorkspace.objValueIn[14] = acadoVariables.od[lRun1 * 17 + 9];
+acadoWorkspace.objValueIn[15] = acadoVariables.od[lRun1 * 17 + 10];
+acadoWorkspace.objValueIn[16] = acadoVariables.od[lRun1 * 17 + 11];
+acadoWorkspace.objValueIn[17] = acadoVariables.od[lRun1 * 17 + 12];
+acadoWorkspace.objValueIn[18] = acadoVariables.od[lRun1 * 17 + 13];
+acadoWorkspace.objValueIn[19] = acadoVariables.od[lRun1 * 17 + 14];
+acadoWorkspace.objValueIn[20] = acadoVariables.od[lRun1 * 17 + 15];
+acadoWorkspace.objValueIn[21] = acadoVariables.od[lRun1 * 17 + 16];
acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut );
acadoWorkspace.Dy[lRun1 * 5] = acadoWorkspace.objValueOut[0] - acadoVariables.y[lRun1 * 5];
@@ -5144,24 +5118,23 @@ acadoWorkspace.objValueIn[0] = acadoVariables.x[80];
acadoWorkspace.objValueIn[1] = acadoVariables.x[81];
acadoWorkspace.objValueIn[2] = acadoVariables.x[82];
acadoWorkspace.objValueIn[3] = acadoVariables.x[83];
-acadoWorkspace.objValueIn[4] = acadoVariables.od[360];
-acadoWorkspace.objValueIn[5] = acadoVariables.od[361];
-acadoWorkspace.objValueIn[6] = acadoVariables.od[362];
-acadoWorkspace.objValueIn[7] = acadoVariables.od[363];
-acadoWorkspace.objValueIn[8] = acadoVariables.od[364];
-acadoWorkspace.objValueIn[9] = acadoVariables.od[365];
-acadoWorkspace.objValueIn[10] = acadoVariables.od[366];
-acadoWorkspace.objValueIn[11] = acadoVariables.od[367];
-acadoWorkspace.objValueIn[12] = acadoVariables.od[368];
-acadoWorkspace.objValueIn[13] = acadoVariables.od[369];
-acadoWorkspace.objValueIn[14] = acadoVariables.od[370];
-acadoWorkspace.objValueIn[15] = acadoVariables.od[371];
-acadoWorkspace.objValueIn[16] = acadoVariables.od[372];
-acadoWorkspace.objValueIn[17] = acadoVariables.od[373];
-acadoWorkspace.objValueIn[18] = acadoVariables.od[374];
-acadoWorkspace.objValueIn[19] = acadoVariables.od[375];
-acadoWorkspace.objValueIn[20] = acadoVariables.od[376];
-acadoWorkspace.objValueIn[21] = acadoVariables.od[377];
+acadoWorkspace.objValueIn[4] = acadoVariables.od[340];
+acadoWorkspace.objValueIn[5] = acadoVariables.od[341];
+acadoWorkspace.objValueIn[6] = acadoVariables.od[342];
+acadoWorkspace.objValueIn[7] = acadoVariables.od[343];
+acadoWorkspace.objValueIn[8] = acadoVariables.od[344];
+acadoWorkspace.objValueIn[9] = acadoVariables.od[345];
+acadoWorkspace.objValueIn[10] = acadoVariables.od[346];
+acadoWorkspace.objValueIn[11] = acadoVariables.od[347];
+acadoWorkspace.objValueIn[12] = acadoVariables.od[348];
+acadoWorkspace.objValueIn[13] = acadoVariables.od[349];
+acadoWorkspace.objValueIn[14] = acadoVariables.od[350];
+acadoWorkspace.objValueIn[15] = acadoVariables.od[351];
+acadoWorkspace.objValueIn[16] = acadoVariables.od[352];
+acadoWorkspace.objValueIn[17] = acadoVariables.od[353];
+acadoWorkspace.objValueIn[18] = acadoVariables.od[354];
+acadoWorkspace.objValueIn[19] = acadoVariables.od[355];
+acadoWorkspace.objValueIn[20] = acadoVariables.od[356];
acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut );
acadoWorkspace.DyN[0] = acadoWorkspace.objValueOut[0] - acadoVariables.yN[0];
acadoWorkspace.DyN[1] = acadoWorkspace.objValueOut[1] - acadoVariables.yN[1];
diff --git a/selfdrive/controls/lib/lateral_mpc/libmpc_py.py b/selfdrive/controls/lib/lateral_mpc/libmpc_py.py
index 6b86d9565..92c0df8da 100644
--- a/selfdrive/controls/lib/lateral_mpc/libmpc_py.py
+++ b/selfdrive/controls/lib/lateral_mpc/libmpc_py.py
@@ -24,8 +24,8 @@ typedef struct {
void init(double pathCost, double laneCost, double headingCost, double steerRateCost);
int run_mpc(state_t * x0, log_t * solution,
- double l_poly[4], double r_poly[4], double p_poly[4],
- double l_prob, double r_prob, double p_prob, double curvature_factor, double v_ref, double lane_width);
+ double l_poly[4], double r_poly[4], double d_poly[4],
+ double l_prob, double r_prob, double curvature_factor, double v_ref, double lane_width);
""")
libmpc = ffi.dlopen(libmpc_fn)
diff --git a/selfdrive/controls/lib/model_parser.py b/selfdrive/controls/lib/model_parser.py
deleted file mode 100644
index 0b4bb241d..000000000
--- a/selfdrive/controls/lib/model_parser.py
+++ /dev/null
@@ -1,66 +0,0 @@
-from common.numpy_fast import interp
-import numpy as np
-from selfdrive.controls.lib.latcontrol_helpers import model_polyfit, calc_desired_path, compute_path_pinv
-
-CAMERA_OFFSET = 0.06 # m from center car to camera
-
-
-class ModelParser(object):
- def __init__(self):
- self.d_poly = [0., 0., 0., 0.]
- self.c_poly = [0., 0., 0., 0.]
- self.c_prob = 0.
- self.last_model = 0.
- self.lead_dist, self.lead_prob, self.lead_var = 0, 0, 1
- self._path_pinv = compute_path_pinv()
-
- self.lane_width_estimate = 3.7
- self.lane_width_certainty = 1.0
- self.lane_width = 3.7
- self.l_prob = 0.
- self.r_prob = 0.
- self.x_points = np.arange(50)
-
- def update(self, v_ego, md):
- if len(md.leftLane.poly):
- l_poly = np.array(md.leftLane.poly)
- r_poly = np.array(md.rightLane.poly)
- p_poly = np.array(md.path.poly)
- else:
- l_poly = model_polyfit(md.leftLane.points, self._path_pinv) # left line
- r_poly = model_polyfit(md.rightLane.points, self._path_pinv) # right line
- p_poly = model_polyfit(md.path.points, self._path_pinv) # predicted path
-
- # only offset left and right lane lines; offsetting p_poly does not make sense
- l_poly[3] += CAMERA_OFFSET
- r_poly[3] += CAMERA_OFFSET
-
- p_prob = 1. # model does not tell this probability yet, so set to 1 for now
- l_prob = md.leftLane.prob # left line prob
- r_prob = md.rightLane.prob # right line prob
-
- # Find current lanewidth
- lr_prob = l_prob * r_prob
- self.lane_width_certainty += 0.05 * (lr_prob - self.lane_width_certainty)
- current_lane_width = abs(l_poly[3] - r_poly[3])
- self.lane_width_estimate += 0.005 * (current_lane_width - self.lane_width_estimate)
- speed_lane_width = interp(v_ego, [0., 31.], [3., 3.8])
- self.lane_width = self.lane_width_certainty * self.lane_width_estimate + \
- (1 - self.lane_width_certainty) * speed_lane_width
-
- self.lead_dist = md.lead.dist
- self.lead_prob = md.lead.prob
- self.lead_var = md.lead.std**2
-
- # compute target path
- self.d_poly, self.c_poly, self.c_prob = calc_desired_path(
- l_poly, r_poly, p_poly, l_prob, r_prob, p_prob, v_ego, self.lane_width)
-
- self.r_poly = r_poly
- self.r_prob = r_prob
-
- self.l_poly = l_poly
- self.l_prob = l_prob
-
- self.p_poly = p_poly
- self.p_prob = p_prob
diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py
index cbd144edc..faa812ac6 100644
--- a/selfdrive/controls/lib/pathplanner.py
+++ b/selfdrive/controls/lib/pathplanner.py
@@ -2,12 +2,13 @@ import os
import math
import numpy as np
+# from common.numpy_fast import clip
from common.realtime import sec_since_boot
from selfdrive.services import service_list
from selfdrive.swaglog import cloudlog
from selfdrive.controls.lib.lateral_mpc import libmpc_py
from selfdrive.controls.lib.drive_helpers import MPC_COST_LAT
-from selfdrive.controls.lib.model_parser import ModelParser
+from selfdrive.controls.lib.lane_planner import LanePlanner
import selfdrive.messaging as messaging
LOG_MPC = os.environ.get('LOG_MPC', False)
@@ -21,10 +22,7 @@ def calc_states_after_delay(states, v_ego, steer_angle, curvature_factor, steer_
class PathPlanner(object):
def __init__(self, CP):
- self.MP = ModelParser()
-
- self.l_poly = [0., 0., 0., 0.]
- self.r_poly = [0., 0., 0., 0.]
+ self.LP = LanePlanner()
self.last_cloudlog_t = 0
@@ -33,6 +31,7 @@ class PathPlanner(object):
self.setup_mpc(CP.steerRateCost)
self.solution_invalid_cnt = 0
+ self.path_offset_i = 0.0
def setup_mpc(self, steer_rate_cost):
self.libmpc = libmpc_py.libmpc
@@ -50,10 +49,6 @@ class PathPlanner(object):
self.angle_steers_des_prev = 0.0
self.angle_steers_des_time = 0.0
- self.l_poly = libmpc_py.ffi.new("double[4]")
- self.r_poly = libmpc_py.ffi.new("double[4]")
- self.p_poly = libmpc_py.ffi.new("double[4]")
-
def update(self, sm, CP, VM):
v_ego = sm['carState'].vEgo
angle_steers = sm['carState'].steeringAngle
@@ -62,23 +57,28 @@ class PathPlanner(object):
angle_offset_average = sm['liveParameters'].angleOffsetAverage
angle_offset_bias = sm['controlsState'].angleModelBias + angle_offset_average
- self.MP.update(v_ego, sm['model'])
+ self.LP.update(v_ego, sm['model'])
# Run MPC
self.angle_steers_des_prev = self.angle_steers_des_mpc
VM.update_params(sm['liveParameters'].stiffnessFactor, sm['liveParameters'].steerRatio)
curvature_factor = VM.curvature_factor(v_ego)
- self.l_poly = list(self.MP.l_poly)
- self.r_poly = list(self.MP.r_poly)
- self.p_poly = list(self.MP.p_poly)
+
+ # TODO: Check for active, override, and saturation
+ # if active:
+ # self.path_offset_i += self.LP.d_poly[3] / (60.0 * 20.0)
+ # self.path_offset_i = clip(self.path_offset_i, -0.5, 0.5)
+ # self.LP.d_poly[3] += self.path_offset_i
+ # else:
+ # self.path_offset_i = 0.0
# account for actuation delay
self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers - angle_offset_average, curvature_factor, VM.sR, CP.steerActuatorDelay)
v_ego_mpc = max(v_ego, 5.0) # avoid mpc roughness due to low speed
self.libmpc.run_mpc(self.cur_state, self.mpc_solution,
- self.l_poly, self.r_poly, self.p_poly,
- self.MP.l_prob, self.MP.r_prob, self.MP.p_prob, curvature_factor, v_ego_mpc, self.MP.lane_width)
+ list(self.LP.l_poly), list(self.LP.r_poly), list(self.LP.d_poly),
+ self.LP.l_prob, self.LP.r_prob, curvature_factor, v_ego_mpc, self.LP.lane_width)
# reset to current steer angle if not active or overriding
if active:
@@ -112,20 +112,20 @@ class PathPlanner(object):
plan_send = messaging.new_message()
plan_send.init('pathPlan')
plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'liveParameters', 'model'])
- plan_send.pathPlan.laneWidth = float(self.MP.lane_width)
- plan_send.pathPlan.dPoly = [float(x) for x in self.MP.d_poly]
- plan_send.pathPlan.cPoly = [float(x) for x in self.MP.c_poly]
- plan_send.pathPlan.cProb = float(self.MP.c_prob)
- plan_send.pathPlan.lPoly = [float(x) for x in self.l_poly]
- plan_send.pathPlan.lProb = float(self.MP.l_prob)
- plan_send.pathPlan.rPoly = [float(x) for x in self.r_poly]
- plan_send.pathPlan.rProb = float(self.MP.r_prob)
+ plan_send.pathPlan.laneWidth = float(self.LP.lane_width)
+ plan_send.pathPlan.dPoly = [float(x) for x in self.LP.d_poly]
+ plan_send.pathPlan.lPoly = [float(x) for x in self.LP.l_poly]
+ plan_send.pathPlan.lProb = float(self.LP.l_prob)
+ plan_send.pathPlan.rPoly = [float(x) for x in self.LP.r_poly]
+ plan_send.pathPlan.rProb = float(self.LP.r_prob)
+
plan_send.pathPlan.angleSteers = float(self.angle_steers_des_mpc)
plan_send.pathPlan.rateSteers = float(rate_desired)
- plan_send.pathPlan.angleOffset = float(angle_offset_average)
+ plan_send.pathPlan.angleOffset = float(self.path_offset_i)
plan_send.pathPlan.mpcSolutionValid = bool(plan_solution_valid)
plan_send.pathPlan.paramsValid = bool(sm['liveParameters'].valid)
plan_send.pathPlan.sensorValid = bool(sm['liveParameters'].sensorValid)
+ plan_send.pathPlan.posenetValid = bool(sm['liveParameters'].posenetValid)
self.plan.send(plan_send.to_bytes())
diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py
index 9aaec513b..52712d15a 100755
--- a/selfdrive/controls/lib/planner.py
+++ b/selfdrive/controls/lib/planner.py
@@ -1,5 +1,4 @@
#!/usr/bin/env python
-import zmq
import math
import numpy as np
from common.params import Params
@@ -16,7 +15,7 @@ from selfdrive.controls.lib.longcontrol import LongCtrlState, MIN_CAN_SPEED
from selfdrive.controls.lib.fcw import FCWChecker
from selfdrive.controls.lib.long_mpc import LongitudinalMpc
-NO_CURVATURE_SPEED = 200. * CV.MPH_TO_MS
+MAX_SPEED = 255.0
LON_MPC_STEP = 0.2 # first step is 0.2s
MAX_SPEED_ERROR = 2.0
@@ -38,6 +37,16 @@ _A_TOTAL_MAX_V = [1.5, 1.9, 3.2]
_A_TOTAL_MAX_BP = [0., 20., 40.]
+# Model speed kalman stuff
+_MODEL_V_A = [[1.0, DT_PLAN], [0.0, 1.0]]
+_MODEL_V_C = [1.0, 0]
+# calculated with observation std of 2m/s and accel proc noise of 2m/s**2
+_MODEL_V_K = [[0.07068858], [0.04826294]]
+
+# 75th percentile
+SPEED_PERCENTILE_IDX = 7
+
+
def calc_cruise_accel_limits(v_ego, following):
a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V)
@@ -58,14 +67,12 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP):
a_y = v_ego**2 * angle_steers * CV.DEG_TO_RAD / (CP.steerRatio * CP.wheelbase)
a_x_allowed = math.sqrt(max(a_total_max**2 - a_y**2, 0.))
- a_target[1] = min(a_target[1], a_x_allowed)
- return a_target
+ return [a_target[0], min(a_target[1], a_x_allowed)]
class Planner(object):
def __init__(self, CP, fcw_enabled):
self.CP = CP
- self.poller = zmq.Poller()
self.plan = messaging.pub_sock(service_list['plan'].port)
self.live_longitudinal_mpc = messaging.pub_sock(service_list['liveLongitudinalMpc'].port)
@@ -81,16 +88,19 @@ class Planner(object):
self.a_acc = 0.0
self.v_cruise = 0.0
self.a_cruise = 0.0
+ self.v_model = 0.0
+ self.a_model = 0.0
self.longitudinalPlanSource = 'cruise'
self.fcw_checker = FCWChecker()
self.fcw_enabled = fcw_enabled
+ self.path_x = np.arange(192)
self.params = Params()
def choose_solution(self, v_cruise_setpoint, enabled):
if enabled:
- solutions = {'cruise': self.v_cruise}
+ solutions = {'cruise': self.v_cruise, 'model': self.v_model}
if self.mpc1.prev_lead_status:
solutions['mpc1'] = self.mpc1.v_mpc
if self.mpc2.prev_lead_status:
@@ -99,7 +109,6 @@ class Planner(object):
slowest = min(solutions, key=solutions.get)
self.longitudinalPlanSource = slowest
-
# Choose lowest of MPC and cruise
if slowest == 'mpc1':
self.v_acc = self.mpc1.v_mpc
@@ -110,10 +119,13 @@ class Planner(object):
elif slowest == 'cruise':
self.v_acc = self.v_cruise
self.a_acc = self.a_cruise
+ elif slowest == 'model':
+ self.v_acc = self.v_model
+ self.a_acc = self.a_model
self.v_acc_future = min([self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint])
- def update(self, sm, CP, VM, PP, live_map_data):
+ def update(self, sm, CP, VM, PP):
"""Gets called when new radarState is available"""
cur_time = sec_since_boot()
v_ego = sm['carState'].vEgo
@@ -129,51 +141,47 @@ class Planner(object):
enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping)
following = lead_1.status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0
- v_speedlimit = NO_CURVATURE_SPEED
- v_curvature = NO_CURVATURE_SPEED
+ if len(sm['model'].path.poly):
+ path = list(sm['model'].path.poly)
- #map_age = cur_time - rcv_times['liveMapData']
- map_valid = False # live_map_data.liveMapData.mapValid and map_age < 10.0
+ # Curvature of polynomial https://en.wikipedia.org/wiki/Curvature#Curvature_of_the_graph_of_a_function
+ # y = a x^3 + b x^2 + c x + d, y' = 3 a x^2 + 2 b x + c, y'' = 6 a x + 2 b
+ # k = y'' / (1 + y'^2)^1.5
+ y_p = 3 * path[0] * self.path_x**2 + 2 * path[1] * self.path_x + path[2]
+ y_pp = 6 * path[0] * self.path_x + 2 * path[1]
+ curv = y_pp / (1. + y_p**2)**1.5
- # Speed limit and curvature
- set_speed_limit_active = self.params.get("LimitSetSpeed") == "1" and self.params.get("SpeedLimitOffset") is not None
- if set_speed_limit_active and map_valid:
- if live_map_data.liveMapData.speedLimitValid:
- speed_limit = live_map_data.liveMapData.speedLimit
- offset = float(self.params.get("SpeedLimitOffset"))
- v_speedlimit = speed_limit + offset
-
- if live_map_data.liveMapData.curvatureValid:
- curvature = abs(live_map_data.liveMapData.curvature)
- a_y_max = 2.975 - v_ego * 0.0375 # ~1.85 @ 75mph, ~2.6 @ 25mph
- v_curvature = math.sqrt(a_y_max / max(1e-4, curvature))
- v_curvature = min(NO_CURVATURE_SPEED, v_curvature)
-
- decel_for_turn = bool(v_curvature < min([v_cruise_setpoint, v_speedlimit, v_ego + 1.]))
- v_cruise_setpoint = min([v_cruise_setpoint, v_curvature, v_speedlimit])
+ a_y_max = 2.975 - v_ego * 0.0375 # ~1.85 @ 75mph, ~2.6 @ 25mph
+ v_curvature = np.sqrt(a_y_max / np.clip(np.abs(curv), 1e-4, None))
+ model_speed = np.min(v_curvature)
+ # print(model_speed * CV.MS_TO_MPH, model_speed)
+ model_speed = max(20.0 * CV.MPH_TO_MS, model_speed) # Don't slow down below 20mph
+ else:
+ model_speed = MAX_SPEED
# Calculate speed for normal cruise control
if enabled:
accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego, following)]
jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])] # TODO: make a separate lookup for jerk tuning
- accel_limits = limit_accel_in_turns(v_ego, sm['carState'].steeringAngle, accel_limits, self.CP)
+ accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngle, accel_limits, self.CP)
if force_slow_decel:
# if required so, force a smooth deceleration
- accel_limits[1] = min(accel_limits[1], AWARENESS_DECEL)
- accel_limits[0] = min(accel_limits[0], accel_limits[1])
-
- # Change accel limits based on time remaining to turn
- if decel_for_turn:
- time_to_turn = max(1.0, live_map_data.liveMapData.distToTurn / max(self.v_cruise, 1.))
- required_decel = min(0, (v_curvature - self.v_cruise) / time_to_turn)
- accel_limits[0] = max(accel_limits[0], required_decel)
+ accel_limits_turns[1] = min(accel_limits_turns[1], AWARENESS_DECEL)
+ accel_limits_turns[0] = min(accel_limits_turns[0], accel_limits_turns[1])
self.v_cruise, self.a_cruise = speed_smoother(self.v_acc_start, self.a_acc_start,
v_cruise_setpoint,
- accel_limits[1], accel_limits[0],
+ accel_limits_turns[1], accel_limits_turns[0],
jerk_limits[1], jerk_limits[0],
LON_MPC_STEP)
+
+ self.v_model, self.a_model = speed_smoother(self.v_acc_start, self.a_acc_start,
+ model_speed,
+ 2*accel_limits[1], accel_limits[0],
+ 2*jerk_limits[1], jerk_limits[0],
+ LON_MPC_STEP)
+
# cruise speed can't be negative even is user is distracted
self.v_cruise = max(self.v_cruise, 0.)
else:
@@ -201,7 +209,9 @@ class Planner(object):
self.fcw_checker.reset_lead(cur_time)
blinkers = sm['carState'].leftBlinker or sm['carState'].rightBlinker
- fcw = self.fcw_checker.update(self.mpc1.mpc_solution, cur_time, v_ego, sm['carState'].aEgo,
+ fcw = self.fcw_checker.update(self.mpc1.mpc_solution, cur_time,
+ sm['controlsState'].active,
+ v_ego, sm['carState'].aEgo,
lead_1.dRel, lead_1.vLead, lead_1.aLeadK,
lead_1.yRel, lead_1.vLat,
lead_1.fcw, blinkers) and not sm['carState'].brakePressed
@@ -224,20 +234,16 @@ class Planner(object):
plan_send.plan.radarStateMonoTime = sm.logMonoTime['radarState']
# longitudal plan
- plan_send.plan.vCruise = self.v_cruise
- plan_send.plan.aCruise = self.a_cruise
- plan_send.plan.vStart = self.v_acc_start
- plan_send.plan.aStart = self.a_acc_start
- plan_send.plan.vTarget = self.v_acc
- plan_send.plan.aTarget = self.a_acc
- plan_send.plan.vTargetFuture = self.v_acc_future
+ plan_send.plan.vCruise = float(self.v_cruise)
+ plan_send.plan.aCruise = float(self.a_cruise)
+ plan_send.plan.vStart = float(self.v_acc_start)
+ plan_send.plan.aStart = float(self.a_acc_start)
+ plan_send.plan.vTarget = float(self.v_acc)
+ plan_send.plan.aTarget = float(self.a_acc)
+ plan_send.plan.vTargetFuture = float(self.v_acc_future)
plan_send.plan.hasLead = self.mpc1.prev_lead_status
plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource
- plan_send.plan.vCurvature = v_curvature
- plan_send.plan.decelForTurn = decel_for_turn
- plan_send.plan.mapValid = map_valid
-
radar_valid = not (radar_dead or radar_fault)
plan_send.plan.radarValid = bool(radar_valid)
plan_send.plan.radarCanError = bool(radar_can_error)
diff --git a/selfdrive/controls/lib/radar_helpers.py b/selfdrive/controls/lib/radar_helpers.py
index 7efb9f334..02a38ca2a 100644
--- a/selfdrive/controls/lib/radar_helpers.py
+++ b/selfdrive/controls/lib/radar_helpers.py
@@ -1,122 +1,66 @@
-import numpy as np
-
-from common.numpy_fast import clip, interp
+from common.realtime import DT_MDL
from common.kalman.simple_kalman import KF1D
+from selfdrive.config import RADAR_TO_CENTER
+
+# the longer lead decels, the more likely it will keep decelerating
+# TODO is this a good default?
_LEAD_ACCEL_TAU = 1.5
-NO_FUSION_SCORE = 100 # bad default fusion score
# 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
# Lead Kalman Filter params
-_VLEAD_A = [[1.0, ts], [0.0, 1.0]]
+_VLEAD_A = [[1.0, DT_MDL], [0.0, 1.0]]
_VLEAD_C = [1.0, 0.0]
#_VLEAD_Q = np.matrix([[10., 0.0], [0.0, 100.]])
#_VLEAD_R = 1e3
#_VLEAD_K = np.matrix([[ 0.05705578], [ 0.03073241]])
-_VLEAD_K = [[ 0.1988689 ], [ 0.28555364]]
-
-RDR_TO_LDR = 2.7
+_VLEAD_K = [[0.1988689], [0.28555364]]
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, measured, steer_override):
- if self.initted:
- # pylint: disable=access-member-before-definition
- self.dPathPrev = self.dPath
- self.vLeadPrev = self.vLead
- self.vRelPrev = self.vRel
+ self.cnt = 0
+ def update(self, d_rel, y_rel, v_rel, v_ego_t_aligned, measured):
# relative values, copy
self.dRel = d_rel # LONG_DIST
self.yRel = y_rel # -LAT_DIST
self.vRel = v_rel # REL_SPEED
self.measured = measured # measured or estimate
- # 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.initted = True
- self.aLeadTau = _LEAD_ACCEL_TAU
- self.cnt = 1
- self.vision_cnt = 0
- self.vision = False
- self.aRel = 0. # nidec gives no information about this
- self.vLat = 0.
+ if self.cnt == 0:
self.kf = KF1D([[self.vLead], [0.0]], _VLEAD_A, _VLEAD_C, _VLEAD_K)
else:
- # estimate acceleration
- # TODO: use Kalman filter
- a_rel_unfilt = (self.vRel - self.vRelPrev) / ts
- a_rel_unfilt = clip(a_rel_unfilt, -10., 10.)
- self.aRel = k_a_lead * a_rel_unfilt + (1 - k_a_lead) * self.aRel
-
- # TODO: use Kalman filter
- # neglect steer override cases as dPath is too noisy
- v_lat_unfilt = 0. if steer_override else (self.dPath - self.dPathPrev) / ts
- self.vLat = k_v_lat * v_lat_unfilt + (1 - k_v_lat) * self.vLat
-
self.kf.update(self.vLead)
- self.cnt += 1
+ self.cnt += 1
self.vLeadK = float(self.kf.x[SPEED][0])
self.aLeadK = float(self.kf.x[ACCEL][0])
- 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
-
- self.vision_score = NO_FUSION_SCORE
-
# Learn if constant acceleration
if abs(self.aLeadK) < 0.5:
self.aLeadTau = _LEAD_ACCEL_TAU
else:
self.aLeadTau *= 0.9
- def update_vision_score(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.:
- self.vision_score = dist_to_vision + rel_speed_diff
- else:
- self.vision_score = NO_FUSION_SCORE
-
- def update_vision_fusion(self):
- # vision point is never stationary
- # don't trust 1 or 2 fusions until model quality is much better
- if self.vision_cnt >= 3:
- self.vision = True
- self.stationary = False
-
def get_key_for_cluster(self):
# Weigh y higher since radar is inaccurate in this dimension
return [self.dRel, self.yRel*2, self.vRel]
+ def reset_a_lead(self, aLeadK, aLeadTau):
+ self.kf = KF1D([[self.vLead], [aLeadK]], _VLEAD_A, _VLEAD_C, _VLEAD_K)
+ self.aLeadK = aLeadK
+ self.aLeadTau = aLeadTau
def mean(l):
return sum(l) / len(l)
@@ -165,95 +109,59 @@ class Cluster(object):
@property
def aLeadK(self):
- return mean([t.aLeadK for t in self.tracks])
+ if all(t.cnt <= 1 for t in self.tracks):
+ return 0.
+ else:
+ return mean([t.aLeadK for t in self.tracks if t.cnt > 1])
@property
def aLeadTau(self):
- return mean([t.aLeadTau for t in self.tracks])
-
- @property
- def vision(self):
- return any([t.vision for t in self.tracks])
+ if all(t.cnt <= 1 for t in self.tracks):
+ return _LEAD_ACCEL_TAU
+ else:
+ return mean([t.aLeadTau for t in self.tracks if t.cnt > 1])
@property
def measured(self):
return any([t.measured 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 toRadarState(self):
+ def get_RadarState(self, model_prob=0.0):
return {
- "dRel": float(self.dRel) - RDR_TO_LDR,
+ "dRel": float(self.dRel),
"yRel": float(self.yRel),
"vRel": float(self.vRel),
- "aRel": float(self.aRel),
"vLead": float(self.vLead),
- "dPath": float(self.dPath),
- "vLat": float(self.vLat),
"vLeadK": float(self.vLeadK),
"aLeadK": float(self.aLeadK),
"status": True,
- "fcw": self.is_potential_fcw(),
+ "fcw": self.is_potential_fcw(model_prob),
+ "modelProb": model_prob,
+ "radar": True,
"aLeadTau": float(self.aLeadTau)
}
+ def get_RadarState_from_vision(self, lead_msg, v_ego):
+ return {
+ "dRel": float(lead_msg.dist - RADAR_TO_CENTER),
+ "yRel": float(lead_msg.relY),
+ "vRel": float(lead_msg.relVel),
+ "vLead": float(v_ego + lead_msg.relVel),
+ "vLeadK": float(v_ego + lead_msg.relVel),
+ "aLeadK": float(0),
+ "aLeadTau": _LEAD_ACCEL_TAU,
+ "fcw": False,
+ "modelProb": float(lead_msg.prob),
+ "radar": False,
+ "status": True
+ }
+
def __str__(self):
- ret = "x: %4.1f y: %4.1f v: %4.1f a: %4.1f d: %4.2f" % (self.dRel, self.yRel, self.vRel, self.aLeadK, self.dPath)
- 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
+ ret = "x: %4.1f y: %4.1f v: %4.1f a: %4.1f" % (self.dRel, self.yRel, self.vRel, self.aLeadK)
return ret
- def is_potential_lead(self, v_ego):
- # 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
+ def potential_low_speed_lead(self, v_ego):
+ # stop for stuff in front of you and low speed, even without model confirmation
+ return abs(self.yRel) < 1.5 and (v_ego < v_ego_stationary) and self.dRel < 25
- # 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
-
- # lat_corr used to be gated on enabled, now always running
- t_lookahead = 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 = clip(t_lookahead * self.vLat, -1., 1.) if self.measured else 0.
-
- # consider only cut-ins
- d_path = clip(d_path + lat_corr, min(0., d_path), max(0.,d_path))
-
- return abs(d_path) < 1.5 and not self.stationary and not self.oncoming
-
- 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
- return (self.dRel - lead_cluster.dRel) > 8. or abs(self.vRel - lead_cluster.vRel) > 1.
- else:
- return False
-
- def is_potential_fcw(self):
- # is this cluster trustrable enough for triggering fcw?
- # fcw can trigger only on clusters that have been fused vision model for at least 20 frames
- return self.vision_cnt >= 20
+ def is_potential_fcw(self, model_prob):
+ return model_prob > .9
diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py
index 306dab05b..9286ea058 100755
--- a/selfdrive/controls/plannerd.py
+++ b/selfdrive/controls/plannerd.py
@@ -37,8 +37,6 @@ def plannerd_thread():
sm['liveParameters'].sensorValid = True
sm['liveParameters'].steerRatio = CP.steerRatio
sm['liveParameters'].stiffnessFactor = 1.0
- live_map_data = messaging.new_message()
- live_map_data.init('liveMapData')
while True:
sm.update()
@@ -46,9 +44,7 @@ def plannerd_thread():
if sm.updated['model']:
PP.update(sm, CP, VM)
if sm.updated['radarState']:
- PL.update(sm, CP, VM, PP, live_map_data.liveMapData)
- # elif socket is live_map_data_sock:
- # live_map_data = msg
+ PL.update(sm, CP, VM, PP)
def main(gctx=None):
diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py
index b37ed8f75..84410fa8e 100755
--- a/selfdrive/controls/radard.py
+++ b/selfdrive/controls/radard.py
@@ -6,18 +6,13 @@ from collections import defaultdict, deque
import selfdrive.messaging as messaging
from selfdrive.services import service_list
-from selfdrive.controls.lib.latcontrol_helpers import calc_lookahead_offset
-from selfdrive.controls.lib.model_parser import ModelParser
-from selfdrive.controls.lib.radar_helpers import Track, Cluster, \
- RDR_TO_LDR, NO_FUSION_SCORE
-
+from selfdrive.controls.lib.radar_helpers import Track, Cluster
+from selfdrive.config import RADAR_TO_CENTER
from selfdrive.controls.lib.cluster.fastcluster_py import cluster_points_centroid
-from selfdrive.controls.lib.vehicle_model import VehicleModel
from selfdrive.swaglog import cloudlog
from cereal import car
from common.params import Params
from common.realtime import set_realtime_priority, Ratekeeper, DT_MDL
-from common.kalman.ekf import EKF, SimpleSensor
DEBUG = False
@@ -26,188 +21,116 @@ DIMSV = 2
XV, SPEEDV = 0, 1
VISION_POINT = -1
-
-class EKFV1D(EKF):
- def __init__(self):
- super(EKFV1D, self).__init__(False)
- self.identity = numpy.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.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
+# Time-alignment
+rate = 1. / DT_MDL # model and radar are both at 20Hz
+v_len = 20 # how many speed data points to remember for t alignment with rdr data
-## fuses camera and radar data for best lead detection
-def radard_thread(gctx=None):
- set_realtime_priority(2)
+def laplacian_cdf(x, mu, b):
+ b = np.max([b, 1e-4])
+ return np.exp(-abs(x-mu)/b)
- # wait for stats about the car to come in from controls
- cloudlog.info("radard is waiting for CarParams")
- CP = car.CarParams.from_bytes(Params().get("CarParams", block=True))
- mocked = CP.carName == "mock"
- VM = VehicleModel(CP)
- cloudlog.info("radard got CarParams")
- # import the radar from the fingerprint
- cloudlog.info("radard is importing %s", CP.carName)
- RadarInterface = importlib.import_module('selfdrive.car.%s.radar_interface' % CP.carName).RadarInterface
+def match_vision_to_cluster(v_ego, lead, clusters):
+ # match vision point to best statistical cluster match
+ probs = []
+ offset_vision_dist = lead.dist - RADAR_TO_CENTER
+ for c in clusters:
+ prob_d = laplacian_cdf(c.dRel, offset_vision_dist, lead.std)
+ prob_y = laplacian_cdf(c.yRel, lead.relY, lead.relYStd)
+ prob_v = laplacian_cdf(c.vRel, lead.relVel, lead.relVelStd)
+ # This is isn't exactly right, but good heuristic
+ combined_prob = prob_d * prob_y * prob_v
+ probs.append(combined_prob)
+ idx = np.argmax(probs)
+ # if no 'sane' match is found return -1
+ # stationary radar points can be false positives
+ dist_sane = abs(clusters[idx].dRel - offset_vision_dist) < max([(offset_vision_dist)*.25, 5.0])
+ vel_sane = (abs(clusters[idx].vRel - lead.relVel) < 10) or (v_ego + clusters[idx].vRel > 2)
+ if dist_sane and vel_sane:
+ return idx
+ else:
+ return None
- sm = messaging.SubMaster(['model', 'controlsState', 'liveParameters'])
- # Default parameters
- live_parameters = messaging.new_message()
- live_parameters.init('liveParameters')
- live_parameters.liveParameters.valid = True
- live_parameters.liveParameters.steerRatio = CP.steerRatio
- live_parameters.liveParameters.stiffnessFactor = 1.0
+def get_lead(v_ego, ready, clusters, lead_msg, low_speed_override=True):
+ # Determine leads, this is where the essential logic happens
+ if len(clusters) > 0 and ready and lead_msg.prob > .5:
+ lead_idx = match_vision_to_cluster(v_ego, lead_msg, clusters)
+ else:
+ lead_idx = None
- MP = ModelParser()
- RI = RadarInterface(CP)
+ lead_dict = {'status': False}
+ if lead_idx is not None:
+ lead_dict = clusters[lead_idx].get_RadarState(lead_msg.prob)
+ elif (lead_idx is None) and ready and (lead_msg.prob > .5):
+ lead_dict = Cluster().get_RadarState_from_vision(lead_msg, v_ego)
- last_md_ts = 0
- last_controls_state_ts = 0
+ if low_speed_override:
+ low_speed_clusters = [c for c in clusters if c.potential_low_speed_lead(v_ego)]
+ if len(low_speed_clusters) > 0:
+ lead_idx = np.argmin([c.dRel for c in low_speed_clusters])
+ if (not lead_dict['status']) or (low_speed_clusters[lead_idx].dRel < lead_dict['dRel']):
+ lead_dict = low_speed_clusters[lead_idx].get_RadarState()
- # *** publish radarState and liveTracks
- radarState = messaging.pub_sock(service_list['radarState'].port)
- liveTracks = messaging.pub_sock(service_list['liveTracks'].port)
+ return lead_dict
- path_x = np.arange(0.0, 140.0, 0.1) # 140 meters is max
- # Time-alignment
- rate = 1. / DT_MDL # model and radar are both at 20Hz
- v_len = 20 # how many speed data points to remember for t alignment with rdr data
+class RadarD(object):
+ def __init__(self, mocked):
+ self.current_time = 0
+ self.mocked = mocked
- active = 0
- steer_angle = 0.
- steer_override = False
+ self.tracks = defaultdict(dict)
- tracks = defaultdict(dict)
+ self.last_md_ts = 0
+ self.last_controls_state_ts = 0
- # Kalman filter stuff:
- ekfv = EKFV1D()
- speedSensorV = SimpleSensor(XV, 1, 2)
+ self.active = 0
- # v_ego
- v_ego = 0.
- v_ego_hist_t = deque([0], maxlen=v_len)
- v_ego_hist_v = deque([0], maxlen=v_len)
- v_ego_t_aligned = 0.
+ # v_ego
+ self.v_ego = 0.
+ self.v_ego_hist_t = deque([0], maxlen=v_len)
+ self.v_ego_hist_v = deque([0], maxlen=v_len)
+ self.v_ego_t_aligned = 0.
+ self.ready = False
- rk = Ratekeeper(rate, print_delay_threshold=None)
- while 1:
- rr = RI.update()
+ def update(self, frame, delay, sm, rr, has_radar):
+ self.current_time = 1e-9*max([sm.logMonoTime[key] for key in sm.logMonoTime.keys()])
+
+ if sm.updated['controlsState']:
+ self.active = sm['controlsState'].active
+ self.v_ego = sm['controlsState'].vEgo
+ self.v_ego_hist_v.append(self.v_ego)
+ self.v_ego_hist_t.append(float(frame)/rate)
+ if sm.updated['model']:
+ self.ready = True
ar_pts = {}
for pt in rr.points:
- ar_pts[pt.trackId] = [pt.dRel + RDR_TO_LDR, pt.yRel, pt.vRel, pt.measured]
-
- sm.update(0)
-
- if sm.updated['liveParameters']:
- VM.update_params(sm['liveParameters'].stiffnessFactor, sm['liveParameters'].steerRatio)
-
- if sm.updated['controlsState']:
- active = sm['controlsState'].active
- v_ego = sm['controlsState'].vEgo
- steer_angle = sm['controlsState'].angleSteers
- steer_override = sm['controlsState'].steerOverride
-
- v_ego_hist_v.append(v_ego)
- v_ego_hist_t.append(float(rk.frame)/rate)
-
- last_controls_state_ts = sm.logMonoTime['controlsState']
-
- if sm.updated['model']:
- last_md_ts = sm.logMonoTime['model']
- MP.update(v_ego, sm['model'])
-
-
- # run kalman filter only if prob is high enough
- if MP.lead_prob > 0.7:
- reading = speedSensorV.read(MP.lead_dist, covar=np.matrix(MP.lead_var))
- ekfv.update_scalar(reading)
- ekfv.predict(DT_MDL)
-
- # When changing lanes the distance to the lead car can suddenly change,
- # which makes the Kalman filter output large relative acceleration
- if mocked and abs(MP.lead_dist - ekfv.state[XV]) > 2.0:
- ekfv.state[XV] = MP.lead_dist
- ekfv.covar = (np.diag([MP.lead_var, ekfv.var_init]))
- ekfv.state[SPEEDV] = 0.
-
- ar_pts[VISION_POINT] = (float(ekfv.state[XV]), np.polyval(MP.d_poly, float(ekfv.state[XV])),
- float(ekfv.state[SPEEDV]), False)
- else:
- ekfv.state[XV] = MP.lead_dist
- ekfv.covar = (np.diag([MP.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 (active and not steer_override) or mocked:
- # use path from model (always when mocking as steering is too noisy)
- path_y = np.polyval(MP.d_poly, path_x)
- else:
- # use path from steer, set angle_offset to 0 it does not only report the physical offset
- path_y = calc_lookahead_offset(v_ego, steer_angle, path_x, VM, angle_offset=live_parameters.liveParameters.angleOffsetAverage)[0]
+ ar_pts[pt.trackId] = [pt.dRel, pt.yRel, pt.vRel, pt.measured]
# *** remove missing points from meta data ***
- for ids in tracks.keys():
+ for ids in self.tracks.keys():
if ids not in ar_pts:
- tracks.pop(ids, None)
+ self.tracks.pop(ids, None)
# *** compute the tracks ***
for ids in ar_pts:
- # ignore standalone vision point, unless we are mocking the radar
- if ids == VISION_POINT and not mocked:
- 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 - RI.delay, v_ego_hist_t, v_ego_hist_v)
-
- d_path = np.sqrt(np.amin((path_x - rpt[0]) ** 2 + (path_y - rpt[1]) ** 2))
- # add sign
- d_path *= np.sign(rpt[1] - np.interp(rpt[0], path_x, path_y))
+ cur_time = float(frame)/rate
+ self.v_ego_t_aligned = np.interp(cur_time - delay, self.v_ego_hist_t, self.v_ego_hist_v)
# create the track if it doesn't exist or it's a new track
- if ids not in tracks:
- tracks[ids] = Track()
- tracks[ids].update(rpt[0], rpt[1], rpt[2], d_path, v_ego_t_aligned, rpt[3], steer_override)
+ if ids not in self.tracks:
+ self.tracks[ids] = Track()
+ self.tracks[ids].update(rpt[0], rpt[1], rpt[2], self.v_ego_t_aligned, rpt[3])
- # allow the vision model to remove the stationary flag if distance and rel speed roughly match
- if VISION_POINT in ar_pts:
- fused_id = None
- best_score = NO_FUSION_SCORE
- for ids in tracks:
- dist_to_vision = np.sqrt((0.5*(ar_pts[VISION_POINT][0] - tracks[ids].dRel)) ** 2 + (2*(ar_pts[VISION_POINT][1] - tracks[ids].yRel)) ** 2)
- rel_speed_diff = abs(ar_pts[VISION_POINT][2] - tracks[ids].vRel)
- tracks[ids].update_vision_score(dist_to_vision, rel_speed_diff)
- if best_score > tracks[ids].vision_score:
- fused_id = ids
- best_score = tracks[ids].vision_score
+ idens = list(self.tracks.keys())
+ track_pts = np.array([self.tracks[iden].get_key_for_cluster() for iden in idens])
- if fused_id is not None:
- tracks[fused_id].vision_cnt += 1
- tracks[fused_id].update_vision_fusion()
-
- if DEBUG:
- print("NEW CYCLE")
- if VISION_POINT in ar_pts:
- print("vision", ar_pts[VISION_POINT])
-
- idens = list(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:
@@ -218,78 +141,99 @@ def radard_thread(gctx=None):
cluster_i = cluster_idxs[idx]
if clusters[cluster_i] is None:
clusters[cluster_i] = Cluster()
- clusters[cluster_i].add(tracks[idens[idx]])
-
+ clusters[cluster_i].add(self.tracks[idens[idx]])
elif len(track_pts) == 1:
- # TODO: why do we need this?
+ # FIXME: cluster_point_centroid hangs forever if len(track_pts) == 1
+ cluster_idxs = [0]
clusters = [Cluster()]
- clusters[0].add(tracks[idens[0]])
+ clusters[0].add(self.tracks[idens[0]])
else:
clusters = []
- if DEBUG:
- for i in clusters:
- print(i)
- # *** extract the lead car ***
- lead_clusters = [c for c in clusters
- if c.is_potential_lead(v_ego)]
- 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)
+ # if a new point, reset accel to the rest of the cluster
+ for idx in xrange(len(track_pts)):
+ if self.tracks[idens[idx]].cnt <= 1:
+ aLeadK = clusters[cluster_idxs[idx]].aLeadK
+ aLeadTau = clusters[cluster_idxs[idx]].aLeadTau
+ self.tracks[idens[idx]].reset_a_lead(aLeadK, aLeadTau)
# *** publish radarState ***
dat = messaging.new_message()
dat.init('radarState')
- dat.valid = sm.all_alive_and_valid(service_list=['controlsState'])
- dat.radarState.mdMonoTime = last_md_ts
+ dat.valid = sm.all_alive_and_valid(service_list=['controlsState', 'model'])
+ dat.radarState.mdMonoTime = self.last_md_ts
dat.radarState.canMonoTimes = list(rr.canMonoTimes)
dat.radarState.radarErrors = list(rr.errors)
- dat.radarState.controlsStateMonoTime = last_controls_state_ts
- if lead_len > 0:
- dat.radarState.leadOne = lead_clusters[0].toRadarState()
- if lead2_len > 0:
- dat.radarState.leadTwo = lead2_clusters[0].toRadarState()
- else:
- dat.radarState.leadTwo.status = False
- else:
- dat.radarState.leadOne.status = False
+ dat.radarState.controlsStateMonoTime = self.last_controls_state_ts
+ if has_radar:
+ dat.radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, sm['model'].lead, low_speed_override=True)
+ dat.radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, sm['model'].leadFuture, low_speed_override=False)
+ return dat
+
+
+# fuses camera and radar data for best lead detection
+def radard_thread(gctx=None):
+ set_realtime_priority(2)
+
+ # wait for stats about the car to come in from controls
+ cloudlog.info("radard is waiting for CarParams")
+ CP = car.CarParams.from_bytes(Params().get("CarParams", block=True))
+ mocked = CP.carName == "mock"
+ cloudlog.info("radard got CarParams")
+
+ # import the radar from the fingerprint
+ cloudlog.info("radard is importing %s", CP.carName)
+ RadarInterface = importlib.import_module('selfdrive.car.%s.radar_interface' % CP.carName).RadarInterface
+
+ can_sock = messaging.sub_sock(service_list['can'].port)
+ sm = messaging.SubMaster(['model', 'controlsState', 'liveParameters'])
+
+ RI = RadarInterface(CP)
+
+ # *** publish radarState and liveTracks
+ radarState = messaging.pub_sock(service_list['radarState'].port)
+ liveTracks = messaging.pub_sock(service_list['liveTracks'].port)
+
+ rk = Ratekeeper(rate, print_delay_threshold=None)
+ RD = RadarD(mocked)
+
+ has_radar = not CP.radarOffCan
+
+ while 1:
+ can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True)
+ rr = RI.update(can_strings)
+
+ if rr is None:
+ continue
+
+ sm.update(0)
+
+ dat = RD.update(rk.frame, RI.delay, sm, rr, has_radar)
dat.radarState.cumLagMs = -rk.remaining*1000.
+
radarState.send(dat.to_bytes())
# *** publish tracks for UI debugging (keep last) ***
+ tracks = RD.tracks
dat = messaging.new_message()
dat.init('liveTracks', len(tracks))
for cnt, ids in enumerate(tracks.keys()):
- if DEBUG:
- print("id: %4.0f x: %4.1f y: %4.1f vr: %4.1f d: %4.1f va: %4.1f vl: %4.1f vlk: %4.1f alk: %4.1f s: %1.0f v: %1.0f" % \
- (ids, tracks[ids].dRel, tracks[ids].yRel, tracks[ids].vRel,
- tracks[ids].dPath, tracks[ids].vLat,
- tracks[ids].vLead, tracks[ids].vLeadK,
- tracks[ids].aLeadK,
- tracks[ids].stationary,
- tracks[ids].measured))
dat.liveTracks[cnt] = {
"trackId": ids,
"dRel": float(tracks[ids].dRel),
"yRel": float(tracks[ids].yRel),
"vRel": float(tracks[ids].vRel),
- "aRel": float(tracks[ids].aRel),
- "stationary": bool(tracks[ids].stationary),
- "oncoming": bool(tracks[ids].oncoming),
}
liveTracks.send(dat.to_bytes())
rk.monitor_time()
+
def main(gctx=None):
radard_thread(gctx)
+
if __name__ == "__main__":
main()
diff --git a/selfdrive/controls/tests/test_lateral_mpc.py b/selfdrive/controls/tests/test_lateral_mpc.py
index 77b37bb74..4bd657134 100644
--- a/selfdrive/controls/tests/test_lateral_mpc.py
+++ b/selfdrive/controls/tests/test_lateral_mpc.py
@@ -1,9 +1,9 @@
import unittest
-import copy
import numpy as np
from selfdrive.car.honda.interface import CarInterface
from selfdrive.controls.lib.lateral_mpc import libmpc_py
from selfdrive.controls.lib.vehicle_model import VehicleModel
+from selfdrive.controls.lib.lane_planner import calc_d_poly
def run_mpc(v_ref=30., x_init=0., y_init=0., psi_init=0., delta_init=0.,
@@ -16,13 +16,17 @@ def run_mpc(v_ref=30., x_init=0., y_init=0., psi_init=0., delta_init=0.,
mpc_solution = libmpc_py.ffi.new("log_t *")
- p_l = copy.copy(poly_l)
+ p_l = poly_l.copy()
p_l[3] += poly_shift
- p_r = copy.copy(poly_r)
+
+ p_r = poly_r.copy()
p_r[3] += poly_shift
- p_p = copy.copy(poly_p)
+
+ p_p = poly_p.copy()
p_p[3] += poly_shift
+ d_poly = calc_d_poly(p_l, p_r, p_p, l_prob, r_prob, lane_width)
+
CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING", {})
VM = VehicleModel(CP)
@@ -31,7 +35,7 @@ def run_mpc(v_ref=30., x_init=0., y_init=0., psi_init=0., delta_init=0.,
l_poly = libmpc_py.ffi.new("double[4]", map(float, p_l))
r_poly = libmpc_py.ffi.new("double[4]", map(float, p_r))
- p_poly = libmpc_py.ffi.new("double[4]", map(float, p_p))
+ d_poly = libmpc_py.ffi.new("double[4]", map(float, d_poly))
cur_state = libmpc_py.ffi.new("state_t *")
cur_state[0].x = x_init
@@ -41,7 +45,7 @@ def run_mpc(v_ref=30., x_init=0., y_init=0., psi_init=0., delta_init=0.,
# converge in no more than 20 iterations
for _ in range(20):
- libmpc.run_mpc(cur_state, mpc_solution, l_poly, r_poly, p_poly, l_prob, r_prob, p_prob,
+ libmpc.run_mpc(cur_state, mpc_solution, l_poly, r_poly, d_poly, l_prob, r_prob,
curvature_factor, v_ref, lane_width)
return mpc_solution
@@ -119,3 +123,7 @@ class TestLateralMpc(unittest.TestCase):
sol = run_mpc(y_init=y_init)
for y in list(sol[0].y):
self.assertGreaterEqual(y_init, abs(y))
+
+
+if __name__ == "__main__":
+ unittest.main()
diff --git a/selfdrive/locationd/.gitignore b/selfdrive/locationd/.gitignore
index 8cdb0d230..6ea757462 100644
--- a/selfdrive/locationd/.gitignore
+++ b/selfdrive/locationd/.gitignore
@@ -1,3 +1,4 @@
ubloxd
ubloxd_test
-params_learner
\ No newline at end of file
+params_learner
+paramsd
\ No newline at end of file
diff --git a/selfdrive/locationd/Makefile b/selfdrive/locationd/Makefile
index f7652648e..ff4684768 100644
--- a/selfdrive/locationd/Makefile
+++ b/selfdrive/locationd/Makefile
@@ -40,11 +40,11 @@ EXTRA_LIBS += -llog -luuid
endif
.PHONY: all
-all: ubloxd params_learner
+all: ubloxd paramsd
include ../common/cereal.mk
-LOC_OBJS = locationd_yawrate.o params_learner.o \
+LOC_OBJS = locationd_yawrate.o params_learner.o paramsd.o \
../common/swaglog.o \
../common/params.o \
../common/util.o \
@@ -71,7 +71,7 @@ liblocationd.so: $(LOC_OBJS)
$(ZMQ_SHARED_LIBS) \
$(EXTRA_LIBS)
-params_learner: $(LOC_OBJS)
+paramsd: $(LOC_OBJS)
@echo "[ LINK ] $@"
$(CXX) -fPIC -o '$@' $^ \
$(CEREAL_LIBS) \
@@ -115,7 +115,7 @@ ubloxd_test: ubloxd_test.o $(OBJS)
.PHONY: clean
clean:
- rm -f ubloxd params_learner liblocationd.so ubloxd.d ubloxd.o ubloxd_test ubloxd_test.o ubloxd_test.d $(OBJS) $(LOC_OBJS) $(DEPS)
+ rm -f ubloxd paramsd liblocationd.so ubloxd.d ubloxd.o ubloxd_test ubloxd_test.o ubloxd_test.d $(OBJS) $(LOC_OBJS) $(DEPS)
-include $(DEPS)
-include $(LOC_DEPS)
diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py
index e3a0b10ad..2890b0f61 100755
--- a/selfdrive/locationd/calibrationd.py
+++ b/selfdrive/locationd/calibrationd.py
@@ -8,7 +8,7 @@ from selfdrive.locationd.calibration_helpers import Calibration
from selfdrive.swaglog import cloudlog
from selfdrive.services import service_list
from common.params import Params
-from common.transformations.model import model_height, get_camera_frame_from_model_frame, get_camera_frame_from_medmodel_frame
+from common.transformations.model import model_height
from common.transformations.camera import view_frame_from_device_frame, get_view_frame_from_road_frame, \
eon_intrinsics, get_calib_from_vp, H, W
@@ -78,21 +78,19 @@ class Calibrator(object):
"valid_points": len(self.vps)}
self.params.put("CalibrationParams", json.dumps(cal_params))
return new_vp
+ else:
+ return None
def send_data(self, livecalibration):
calib = get_calib_from_vp(self.vp)
extrinsic_matrix = get_view_frame_from_road_frame(0, calib[1], calib[2], model_height)
- ke = eon_intrinsics.dot(extrinsic_matrix)
- warp_matrix = get_camera_frame_from_model_frame(ke)
- warp_matrix_big = get_camera_frame_from_medmodel_frame(ke)
cal_send = messaging.new_message()
cal_send.init('liveCalibration')
cal_send.liveCalibration.calStatus = self.cal_status
cal_send.liveCalibration.calPerc = min(len(self.vps) * 100 // INPUTS_NEEDED, 100)
- cal_send.liveCalibration.warpMatrix2 = [float(x) for x in warp_matrix.flatten()]
- cal_send.liveCalibration.warpMatrixBig = [float(x) for x in warp_matrix_big.flatten()]
cal_send.liveCalibration.extrinsicMatrix = [float(x) for x in extrinsic_matrix.flatten()]
+ cal_send.liveCalibration.rpyCalib = [float(x) for x in calib]
livecalibration.send(cal_send.to_bytes())
diff --git a/selfdrive/locationd/locationd_yawrate.cc b/selfdrive/locationd/locationd_yawrate.cc
index f03f808ff..93e706499 100644
--- a/selfdrive/locationd/locationd_yawrate.cc
+++ b/selfdrive/locationd/locationd_yawrate.cc
@@ -1,287 +1,99 @@
#include
-#include
#include
-#include
#include
#include
#include
-#include "json11.hpp"
#include "cereal/gen/cpp/log.capnp.h"
-#include "common/swaglog.h"
-#include "common/messaging.h"
-#include "common/params.h"
-#include "common/timing.h"
-#include "params_learner.h"
-const int num_polls = 3;
+#include "locationd_yawrate.h"
-class Localizer
-{
- Eigen::Matrix2d A;
- Eigen::Matrix2d I;
- Eigen::Matrix2d Q;
- Eigen::Matrix2d P;
- Eigen::Matrix C_posenet;
- Eigen::Matrix C_gyro;
- double R_gyro;
+void Localizer::update_state(const Eigen::Matrix &C, const double R, double current_time, double meas) {
+ double dt = current_time - prev_update_time;
- void update_state(const Eigen::Matrix &C, const double R, double current_time, double meas) {
- double dt = current_time - prev_update_time;
+ if (dt < 0) {
+ dt = 0;
+ } else {
prev_update_time = current_time;
- if (dt < 1.0e-9) {
- return;
- }
-
- // x = A * x;
- // P = A * P * A.transpose() + dt * Q;
- // Simplify because A is unity
- P = P + dt * Q;
-
- double y = meas - C * x;
- double S = R + C * P * C.transpose();
- Eigen::Vector2d K = P * C.transpose() * (1.0 / S);
- x = x + K * y;
- P = (I - K * C) * P;
}
- void handle_sensor_events(capnp::List::Reader sensor_events, double current_time) {
- for (cereal::SensorEventData::Reader sensor_event : sensor_events){
- if (sensor_event.getType() == 4) {
- sensor_data_time = current_time;
+ // x = A * x;
+ // P = A * P * A.transpose() + dt * Q;
+ // Simplify because A is unity
+ P = P + dt * Q;
- double meas = -sensor_event.getGyro().getV()[0];
- update_state(C_gyro, R_gyro, current_time, meas);
- }
- }
+ double y = meas - C * x;
+ double S = R + C * P * C.transpose();
+ Eigen::Vector2d K = P * C.transpose() * (1.0 / S);
+ x = x + K * y;
+ P = (I - K * C) * P;
+}
- }
+void Localizer::handle_sensor_events(capnp::List::Reader sensor_events, double current_time) {
+ for (cereal::SensorEventData::Reader sensor_event : sensor_events){
+ if (sensor_event.getType() == 4) {
+ sensor_data_time = current_time;
- void handle_camera_odometry(cereal::CameraOdometry::Reader camera_odometry, double current_time) {
- double R = 250.0 * pow(camera_odometry.getRotStd()[2], 2);
- double meas = camera_odometry.getRot()[2];
- update_state(C_posenet, R, current_time, meas);
- }
-
- void handle_controls_state(cereal::ControlsState::Reader controls_state, double current_time) {
- steering_angle = controls_state.getAngleSteers() * DEGREES_TO_RADIANS;
- car_speed = controls_state.getVEgo();
- controls_state_time = current_time;
- }
-
-
-public:
- Eigen::Vector2d x;
- double steering_angle = 0;
- double car_speed = 0;
- double prev_update_time = -1;
- double controls_state_time = -1;
- double sensor_data_time = -1;
-
- Localizer() {
- A << 1, 0, 0, 1;
- I << 1, 0, 0, 1;
-
- Q << pow(0.1, 2.0), 0, 0, pow(0.005 / 100.0, 2.0);
- P << pow(1.0, 2.0), 0, 0, pow(0.05, 2.0);
-
- C_posenet << 1, 0;
- C_gyro << 1, 1;
- x << 0, 0;
-
- R_gyro = pow(0.05, 2.0);
- }
-
- cereal::Event::Which handle_log(const unsigned char* msg_dat, size_t msg_size) {
- const kj::ArrayPtr view((const capnp::word*)msg_dat, msg_size);
- capnp::FlatArrayMessageReader msg(view);
- cereal::Event::Reader event = msg.getRoot();
- double current_time = event.getLogMonoTime() / 1.0e9;
-
- if (prev_update_time < 0) {
- prev_update_time = current_time;
- }
-
- auto type = event.which();
- switch(type) {
- case cereal::Event::CONTROLS_STATE:
- handle_controls_state(event.getControlsState(), current_time);
- break;
- case cereal::Event::CAMERA_ODOMETRY:
- handle_camera_odometry(event.getCameraOdometry(), current_time);
- break;
- case cereal::Event::SENSOR_EVENTS:
- handle_sensor_events(event.getSensorEvents(), current_time);
- break;
- default:
- break;
- }
-
- return type;
- }
-};
-
-
-
-int main(int argc, char *argv[]) {
- auto ctx = zmq_ctx_new();
- auto controls_state_sock = sub_sock(ctx, "tcp://127.0.0.1:8007");
- auto sensor_events_sock = sub_sock(ctx, "tcp://127.0.0.1:8003");
- auto camera_odometry_sock = sub_sock(ctx, "tcp://127.0.0.1:8066");
-
- auto live_parameters_sock = zsock_new_pub("@tcp://*:8064");
- assert(live_parameters_sock);
- auto live_parameters_sock_raw = zsock_resolve(live_parameters_sock);
-
- int err;
- Localizer localizer;
-
- zmq_pollitem_t polls[num_polls] = {{0}};
- polls[0].socket = controls_state_sock;
- polls[0].events = ZMQ_POLLIN;
- polls[1].socket = sensor_events_sock;
- polls[1].events = ZMQ_POLLIN;
- polls[2].socket = camera_odometry_sock;
- polls[2].events = ZMQ_POLLIN;
-
- // Read car params
- char *value;
- size_t value_sz = 0;
-
- LOGW("waiting for params to set vehicle model");
- while (true) {
- read_db_value(NULL, "CarParams", &value, &value_sz);
- if (value_sz > 0) break;
- usleep(100*1000);
- }
- LOGW("got %d bytes CarParams", value_sz);
-
- // make copy due to alignment issues
- auto amsg = kj::heapArray((value_sz / sizeof(capnp::word)) + 1);
- memcpy(amsg.begin(), value, value_sz);
- free(value);
-
- capnp::FlatArrayMessageReader cmsg(amsg);
- cereal::CarParams::Reader car_params = cmsg.getRoot();
-
- // Read params from previous run
- const int result = read_db_value(NULL, "LiveParameters", &value, &value_sz);
-
- std::string fingerprint = car_params.getCarFingerprint();
- std::string vin = car_params.getCarVin();
- double sR = car_params.getSteerRatio();
- double x = 1.0;
- double ao = 0.0;
-
- if (result == 0){
- auto str = std::string(value, value_sz);
- free(value);
-
- std::string err;
- auto json = json11::Json::parse(str, err);
- if (json.is_null() || !err.empty()) {
- std::string log = "Error parsing json: " + err;
- LOGW(log.c_str());
- } else {
- std::string new_fingerprint = json["carFingerprint"].string_value();
- std::string new_vin = json["carVin"].string_value();
-
- if (fingerprint == new_fingerprint && vin == new_vin) {
- std::string log = "Parameter starting with: " + str;
- LOGW(log.c_str());
-
- sR = json["steerRatio"].number_value();
- x = json["stiffnessFactor"].number_value();
- ao = json["angleOffsetAverage"].number_value();
- }
+ double meas = -sensor_event.getGyro().getV()[0];
+ update_state(C_gyro, R_gyro, current_time, meas);
}
}
+}
- ParamsLearner learner(car_params, ao, x, sR, 1.0);
+void Localizer::handle_camera_odometry(cereal::CameraOdometry::Reader camera_odometry, double current_time) {
+ double R = 250.0 * pow(camera_odometry.getRotStd()[2], 2);
+ double meas = camera_odometry.getRot()[2];
+ update_state(C_posenet, R, current_time, meas);
- // Main loop
- int save_counter = 0;
- while (true){
- int ret = zmq_poll(polls, num_polls, 100);
+ auto trans = camera_odometry.getTrans();
+ posenet_speed = sqrt(trans[0]*trans[0] + trans[1]*trans[1] + trans[2]*trans[2]);
+}
- if (ret == 0){
- continue;
- } else if (ret < 0){
- break;
- }
-
- for (int i=0; i < num_polls; i++) {
- if (polls[i].revents) {
- zmq_msg_t msg;
- err = zmq_msg_init(&msg);
- assert(err == 0);
- err = zmq_msg_recv(&msg, polls[i].socket, 0);
- assert(err >= 0);
- // make copy due to alignment issues, will be freed on out of scope
- auto amsg = kj::heapArray((zmq_msg_size(&msg) / sizeof(capnp::word)) + 1);
- memcpy(amsg.begin(), zmq_msg_data(&msg), zmq_msg_size(&msg));
-
- auto which = localizer.handle_log((const unsigned char*)amsg.begin(), amsg.size());
- zmq_msg_close(&msg);
-
- if (which == cereal::Event::CONTROLS_STATE){
- save_counter++;
-
- double yaw_rate = -localizer.x[0];
- bool valid = learner.update(yaw_rate, localizer.car_speed, localizer.steering_angle);
-
- // TODO: Fix in replay
- double sensor_data_age = localizer.controls_state_time - localizer.sensor_data_time;
-
- double angle_offset_degrees = RADIANS_TO_DEGREES * learner.ao;
- double angle_offset_average_degrees = RADIANS_TO_DEGREES * learner.slow_ao;
-
- // Send parameters at 10 Hz
- if (save_counter % 10 == 0){
- capnp::MallocMessageBuilder msg;
- cereal::Event::Builder event = msg.initRoot();
- event.setLogMonoTime(nanos_since_boot());
- auto live_params = event.initLiveParameters();
- live_params.setValid(valid);
- live_params.setYawRate(localizer.x[0]);
- live_params.setGyroBias(localizer.x[1]);
- live_params.setSensorValid(sensor_data_age < 5.0);
- live_params.setAngleOffset(angle_offset_degrees);
- live_params.setAngleOffsetAverage(angle_offset_average_degrees);
- live_params.setStiffnessFactor(learner.x);
- live_params.setSteerRatio(learner.sR);
-
- auto words = capnp::messageToFlatArray(msg);
- auto bytes = words.asBytes();
- zmq_send(live_parameters_sock_raw, bytes.begin(), bytes.size(), ZMQ_DONTWAIT);
- }
+void Localizer::handle_controls_state(cereal::ControlsState::Reader controls_state, double current_time) {
+ steering_angle = controls_state.getAngleSteers() * DEGREES_TO_RADIANS;
+ car_speed = controls_state.getVEgo();
+ controls_state_time = current_time;
+}
- // Save parameters every minute
- if (save_counter % 6000 == 0) {
- json11::Json json = json11::Json::object {
- {"carVin", vin},
- {"carFingerprint", fingerprint},
- {"steerRatio", learner.sR},
- {"stiffnessFactor", learner.x},
- {"angleOffsetAverage", angle_offset_average_degrees},
- };
+Localizer::Localizer() {
+ A << 1, 0, 0, 1;
+ I << 1, 0, 0, 1;
- std::string out = json.dump();
- write_db_value(NULL, "LiveParameters", out.c_str(), out.length());
- }
- }
- }
- }
+ Q << pow(0.1, 2.0), 0, 0, pow(0.005 / 100.0, 2.0);
+ P << pow(1.0, 2.0), 0, 0, pow(0.05, 2.0);
+
+ C_posenet << 1, 0;
+ C_gyro << 1, 1;
+ x << 0, 0;
+
+ R_gyro = pow(0.05, 2.0);
+}
+
+void Localizer::handle_log(cereal::Event::Reader event) {
+ double current_time = event.getLogMonoTime() / 1.0e9;
+
+ // Initialize update_time on first update
+ if (prev_update_time < 0) {
+ prev_update_time = current_time;
}
- zmq_close(controls_state_sock);
- zmq_close(sensor_events_sock);
- zmq_close(camera_odometry_sock);
- zmq_close(live_parameters_sock_raw);
- return 0;
+ auto type = event.which();
+ switch(type) {
+ case cereal::Event::CONTROLS_STATE:
+ handle_controls_state(event.getControlsState(), current_time);
+ break;
+ case cereal::Event::CAMERA_ODOMETRY:
+ handle_camera_odometry(event.getCameraOdometry(), current_time);
+ break;
+ case cereal::Event::SENSOR_EVENTS:
+ handle_sensor_events(event.getSensorEvents(), current_time);
+ break;
+ default:
+ break;
+ }
}
@@ -292,8 +104,12 @@ extern "C" {
}
void localizer_handle_log(void * localizer, const unsigned char * data, size_t len) {
+ const kj::ArrayPtr view((const capnp::word*)data, len);
+ capnp::FlatArrayMessageReader msg(view);
+ cereal::Event::Reader event = msg.getRoot();
+
Localizer * loc = (Localizer*) localizer;
- loc->handle_log(data, len);
+ loc->handle_log(event);
}
double localizer_get_yaw(void * localizer) {
@@ -304,6 +120,16 @@ extern "C" {
Localizer * loc = (Localizer*) localizer;
return loc->x[1];
}
+
+ void localizer_set_yaw(void * localizer, double yaw) {
+ Localizer * loc = (Localizer*) localizer;
+ loc->x[0] = yaw;
+ }
+ void localizer_set_bias(void * localizer, double bias) {
+ Localizer * loc = (Localizer*) localizer;
+ loc->x[1] = bias;
+ }
+
double localizer_get_t(void * localizer) {
Localizer * loc = (Localizer*) localizer;
return loc->prev_update_time;
diff --git a/selfdrive/locationd/locationd_yawrate.h b/selfdrive/locationd/locationd_yawrate.h
new file mode 100644
index 000000000..d5db91e79
--- /dev/null
+++ b/selfdrive/locationd/locationd_yawrate.h
@@ -0,0 +1,36 @@
+#pragma once
+
+#include
+#include "cereal/gen/cpp/log.capnp.h"
+
+#define DEGREES_TO_RADIANS 0.017453292519943295
+
+class Localizer
+{
+ Eigen::Matrix2d A;
+ Eigen::Matrix2d I;
+ Eigen::Matrix2d Q;
+ Eigen::Matrix2d P;
+ Eigen::Matrix C_posenet;
+ Eigen::Matrix C_gyro;
+
+ double R_gyro;
+
+ void update_state(const Eigen::Matrix &C, const double R, double current_time, double meas);
+ void handle_sensor_events(capnp::List::Reader sensor_events, double current_time);
+ void handle_camera_odometry(cereal::CameraOdometry::Reader camera_odometry, double current_time);
+ void handle_controls_state(cereal::ControlsState::Reader controls_state, double current_time);
+
+public:
+ Eigen::Vector2d x;
+ double steering_angle = 0;
+ double car_speed = 0;
+ double posenet_speed = 0;
+ double prev_update_time = -1;
+ double controls_state_time = -1;
+ double sensor_data_time = -1;
+
+ Localizer();
+ void handle_log(cereal::Event::Reader event);
+
+};
diff --git a/selfdrive/locationd/params_learner.cc b/selfdrive/locationd/params_learner.cc
index a150805d8..912abde35 100644
--- a/selfdrive/locationd/params_learner.cc
+++ b/selfdrive/locationd/params_learner.cc
@@ -2,6 +2,8 @@
#include
#include
+#include
+#include
#include "cereal/gen/cpp/log.capnp.h"
#include "cereal/gen/cpp/car.capnp.h"
#include "params_learner.h"
@@ -14,14 +16,15 @@ T clip(const T& n, const T& lower, const T& upper) {
}
ParamsLearner::ParamsLearner(cereal::CarParams::Reader car_params,
- double angle_offset,
- double stiffness_factor,
- double steer_ratio,
- double learning_rate) :
- ao(angle_offset * DEGREES_TO_RADIANS),
- slow_ao(angle_offset * DEGREES_TO_RADIANS),
- x(stiffness_factor),
- sR(steer_ratio) {
+ double angle_offset,
+ double stiffness_factor,
+ double steer_ratio,
+ double learning_rate) :
+ ao(angle_offset * DEGREES_TO_RADIANS),
+ slow_ao(angle_offset * DEGREES_TO_RADIANS),
+ x(stiffness_factor),
+ sR(steer_ratio) {
+
cF0 = car_params.getTireStiffnessFront();
cR0 = car_params.getTireStiffnessRear();
@@ -73,3 +76,43 @@ bool ParamsLearner::update(double psi, double u, double sa) {
valid = valid && sR < max_sr_th;
return valid;
}
+
+
+extern "C" {
+ void *params_learner_init(size_t len, char * params, double angle_offset, double stiffness_factor, double steer_ratio, double learning_rate) {
+
+ auto amsg = kj::heapArray((len / sizeof(capnp::word)) + 1);
+ memcpy(amsg.begin(), params, len);
+
+ capnp::FlatArrayMessageReader cmsg(amsg);
+ cereal::CarParams::Reader car_params = cmsg.getRoot();
+
+ ParamsLearner * p = new ParamsLearner(car_params, angle_offset, stiffness_factor, steer_ratio, learning_rate);
+ return (void*)p;
+ }
+
+ bool params_learner_update(void * params_learner, double psi, double u, double sa) {
+ ParamsLearner * p = (ParamsLearner*) params_learner;
+ return p->update(psi, u, sa);
+ }
+
+ double params_learner_get_ao(void * params_learner){
+ ParamsLearner * p = (ParamsLearner*) params_learner;
+ return p->ao;
+ }
+
+ double params_learner_get_x(void * params_learner){
+ ParamsLearner * p = (ParamsLearner*) params_learner;
+ return p->x;
+ }
+
+ double params_learner_get_slow_ao(void * params_learner){
+ ParamsLearner * p = (ParamsLearner*) params_learner;
+ return p->slow_ao;
+ }
+
+ double params_learner_get_sR(void * params_learner){
+ ParamsLearner * p = (ParamsLearner*) params_learner;
+ return p->sR;
+ }
+}
diff --git a/selfdrive/locationd/paramsd.cc b/selfdrive/locationd/paramsd.cc
new file mode 100644
index 000000000..2d9f11b3e
--- /dev/null
+++ b/selfdrive/locationd/paramsd.cc
@@ -0,0 +1,189 @@
+#include
+#include
+#include
+#include
+
+#include "locationd_yawrate.h"
+#include "cereal/gen/cpp/log.capnp.h"
+
+#include "common/swaglog.h"
+#include "common/messaging.h"
+#include "common/params.h"
+#include "common/timing.h"
+#include "params_learner.h"
+#include "json11.hpp"
+
+const int num_polls = 3;
+
+int main(int argc, char *argv[]) {
+ auto ctx = zmq_ctx_new();
+ auto controls_state_sock = sub_sock(ctx, "tcp://127.0.0.1:8007");
+ auto sensor_events_sock = sub_sock(ctx, "tcp://127.0.0.1:8003");
+ auto camera_odometry_sock = sub_sock(ctx, "tcp://127.0.0.1:8066");
+
+ auto live_parameters_sock = zsock_new_pub("@tcp://*:8064");
+ assert(live_parameters_sock);
+ auto live_parameters_sock_raw = zsock_resolve(live_parameters_sock);
+
+ int err;
+ Localizer localizer;
+
+ zmq_pollitem_t polls[num_polls] = {{0}};
+ polls[0].socket = controls_state_sock;
+ polls[0].events = ZMQ_POLLIN;
+ polls[1].socket = camera_odometry_sock;
+ polls[1].events = ZMQ_POLLIN;
+ polls[2].socket = sensor_events_sock;
+ polls[2].events = ZMQ_POLLIN;
+
+ // Read car params
+ char *value;
+ size_t value_sz = 0;
+
+ LOGW("waiting for params to set vehicle model");
+ while (true) {
+ read_db_value(NULL, "CarParams", &value, &value_sz);
+ if (value_sz > 0) break;
+ usleep(100*1000);
+ }
+ LOGW("got %d bytes CarParams", value_sz);
+
+ // make copy due to alignment issues
+ auto amsg = kj::heapArray((value_sz / sizeof(capnp::word)) + 1);
+ memcpy(amsg.begin(), value, value_sz);
+ free(value);
+
+ capnp::FlatArrayMessageReader cmsg(amsg);
+ cereal::CarParams::Reader car_params = cmsg.getRoot();
+
+ // Read params from previous run
+ const int result = read_db_value(NULL, "LiveParameters", &value, &value_sz);
+
+ std::string fingerprint = car_params.getCarFingerprint();
+ std::string vin = car_params.getCarVin();
+ double sR = car_params.getSteerRatio();
+ double x = 1.0;
+ double ao = 0.0;
+ double posenet_invalid_count = 0;
+
+ if (result == 0){
+ auto str = std::string(value, value_sz);
+ free(value);
+
+ std::string err;
+ auto json = json11::Json::parse(str, err);
+ if (json.is_null() || !err.empty()) {
+ std::string log = "Error parsing json: " + err;
+ LOGW(log.c_str());
+ } else {
+ std::string new_fingerprint = json["carFingerprint"].string_value();
+ std::string new_vin = json["carVin"].string_value();
+
+ if (fingerprint == new_fingerprint && vin == new_vin) {
+ std::string log = "Parameter starting with: " + str;
+ LOGW(log.c_str());
+
+ sR = json["steerRatio"].number_value();
+ x = json["stiffnessFactor"].number_value();
+ ao = json["angleOffsetAverage"].number_value();
+ }
+ }
+ }
+
+ ParamsLearner learner(car_params, ao, x, sR, 1.0);
+
+ // Main loop
+ int save_counter = 0;
+ while (true){
+ int ret = zmq_poll(polls, num_polls, 100);
+
+ if (ret == 0){
+ continue;
+ } else if (ret < 0){
+ break;
+ }
+
+ for (int i=0; i < num_polls; i++) {
+ if (polls[i].revents) {
+ zmq_msg_t msg;
+ err = zmq_msg_init(&msg);
+ assert(err == 0);
+ err = zmq_msg_recv(&msg, polls[i].socket, 0);
+ assert(err >= 0);
+
+ // make copy due to alignment issues, will be freed on out of scope
+ auto amsg = kj::heapArray((zmq_msg_size(&msg) / sizeof(capnp::word)) + 1);
+ memcpy(amsg.begin(), zmq_msg_data(&msg), zmq_msg_size(&msg));
+ zmq_msg_close(&msg);
+
+ capnp::FlatArrayMessageReader capnp_msg(amsg);
+ cereal::Event::Reader event = capnp_msg.getRoot();
+
+ localizer.handle_log(event);
+
+ auto which = event.which();
+ // Throw vision failure if posenet and odometric speed too different
+ if (which == cereal::Event::CAMERA_ODOMETRY){
+ if (std::abs(localizer.posenet_speed - localizer.car_speed) > std::max(0.4 * localizer.car_speed, 5.0)) {
+ posenet_invalid_count++;
+ } else {
+ posenet_invalid_count = 0;
+ }
+ } else if (which == cereal::Event::CONTROLS_STATE){
+ save_counter++;
+
+ double yaw_rate = -localizer.x[0];
+ bool valid = learner.update(yaw_rate, localizer.car_speed, localizer.steering_angle);
+
+ // TODO: Fix in replay
+ double sensor_data_age = localizer.controls_state_time - localizer.sensor_data_time;
+
+ double angle_offset_degrees = RADIANS_TO_DEGREES * learner.ao;
+ double angle_offset_average_degrees = RADIANS_TO_DEGREES * learner.slow_ao;
+
+ // Send parameters at 10 Hz
+ if (save_counter % 10 == 0){
+ capnp::MallocMessageBuilder msg;
+ cereal::Event::Builder event = msg.initRoot();
+ event.setLogMonoTime(nanos_since_boot());
+ auto live_params = event.initLiveParameters();
+ live_params.setValid(valid);
+ live_params.setYawRate(localizer.x[0]);
+ live_params.setGyroBias(localizer.x[1]);
+ live_params.setSensorValid(sensor_data_age < 5.0);
+ live_params.setAngleOffset(angle_offset_degrees);
+ live_params.setAngleOffsetAverage(angle_offset_average_degrees);
+ live_params.setStiffnessFactor(learner.x);
+ live_params.setSteerRatio(learner.sR);
+ live_params.setPosenetSpeed(localizer.posenet_speed);
+ live_params.setPosenetValid(posenet_invalid_count < 4);
+
+ auto words = capnp::messageToFlatArray(msg);
+ auto bytes = words.asBytes();
+ zmq_send(live_parameters_sock_raw, bytes.begin(), bytes.size(), ZMQ_DONTWAIT);
+ }
+
+ // Save parameters every minute
+ if (save_counter % 6000 == 0) {
+ json11::Json json = json11::Json::object {
+ {"carVin", vin},
+ {"carFingerprint", fingerprint},
+ {"steerRatio", learner.sR},
+ {"stiffnessFactor", learner.x},
+ {"angleOffsetAverage", angle_offset_average_degrees},
+ };
+
+ std::string out = json.dump();
+ write_db_value(NULL, "LiveParameters", out.c_str(), out.length());
+ }
+ }
+ }
+ }
+ }
+
+ zmq_close(controls_state_sock);
+ zmq_close(sensor_events_sock);
+ zmq_close(camera_odometry_sock);
+ zmq_close(live_parameters_sock_raw);
+ return 0;
+}
diff --git a/selfdrive/locationd/test/test_params_learner.py b/selfdrive/locationd/test/test_params_learner.py
new file mode 100755
index 000000000..b29f591e7
--- /dev/null
+++ b/selfdrive/locationd/test/test_params_learner.py
@@ -0,0 +1,53 @@
+#!/usr/bin/env python
+
+import numpy as np
+import unittest
+
+from selfdrive.car.honda.interface import CarInterface
+from selfdrive.car.honda.values import CAR
+from selfdrive.controls.lib.vehicle_model import VehicleModel
+from selfdrive.locationd.liblocationd_py import liblocationd # pylint: disable=no-name-in-module, import-error
+
+
+class TestParamsLearner(unittest.TestCase):
+ def setUp(self):
+
+ self.CP = CarInterface.get_params(CAR.CIVIC, {})
+ bts = self.CP.to_bytes()
+
+ self.params_learner = liblocationd.params_learner_init(len(bts), bts, 0.0, 1.0, self.CP.steerRatio, 1.0)
+
+ def test_convergence(self):
+ # Setup vehicle model with wrong parameters
+ VM_sim = VehicleModel(self.CP)
+ x_target = 0.75
+ sr_target = self.CP.steerRatio - 0.5
+ ao_target = -1.0
+ VM_sim.update_params(x_target, sr_target)
+
+ # Run simulation
+ times = np.arange(0, 15*3600, 0.01)
+ angle_offset = np.radians(ao_target)
+ steering_angles = np.radians(10 * np.sin(2 * np.pi * times / 100.)) + angle_offset
+ speeds = 10 * np.sin(2 * np.pi * times / 1000.) + 25
+
+ for i, t in enumerate(times):
+ u = speeds[i]
+ sa = steering_angles[i]
+ psi = VM_sim.yaw_rate(sa - angle_offset, u)
+ liblocationd.params_learner_update(self.params_learner, psi, u, sa)
+
+ # Verify learned parameters
+ sr = liblocationd.params_learner_get_sR(self.params_learner)
+ ao_slow = np.degrees(liblocationd.params_learner_get_slow_ao(self.params_learner))
+ x = liblocationd.params_learner_get_x(self.params_learner)
+ self.assertAlmostEqual(x_target, x, places=1)
+ self.assertAlmostEqual(ao_target, ao_slow, places=1)
+ self.assertAlmostEqual(sr_target, sr, places=1)
+
+
+
+
+
+if __name__ == "__main__":
+ unittest.main()
diff --git a/selfdrive/loggerd/uploader.py b/selfdrive/loggerd/uploader.py
index 7cad71bf7..59765d13e 100644
--- a/selfdrive/loggerd/uploader.py
+++ b/selfdrive/loggerd/uploader.py
@@ -17,7 +17,7 @@ from selfdrive.swaglog import cloudlog
from selfdrive.loggerd.config import ROOT
from common.params import Params
-from common.api import api_get
+from common.api import Api
fake_upload = os.getenv("FAKEUPLOAD") is not None
@@ -93,9 +93,9 @@ def is_on_hotspot():
return False
class Uploader(object):
- def __init__(self, dongle_id, access_token, root):
+ def __init__(self, dongle_id, private_key, root):
self.dongle_id = dongle_id
- self.access_token = access_token
+ self.api = Api(dongle_id, private_key)
self.root = root
self.upload_thread = None
@@ -168,11 +168,11 @@ class Uploader(object):
def do_upload(self, key, fn):
try:
- url_resp = api_get("v1.2/"+self.dongle_id+"/upload_url/", timeout=2, path=key, access_token=self.access_token)
+ url_resp = self.api.get("v1.3/"+self.dongle_id+"/upload_url/", timeout=10, path=key, access_token=self.api.get_token())
url_resp_json = json.loads(url_resp.text)
url = url_resp_json['url']
headers = url_resp_json['headers']
- cloudlog.info("upload_url v1.2 %s %s", url, str(headers))
+ cloudlog.info("upload_url v1.3 %s %s", url, str(headers))
if fake_upload:
cloudlog.info("*** WARNING, THIS IS A FAKE UPLOAD TO %s ***" % url)
@@ -223,7 +223,7 @@ class Uploader(object):
try:
os.unlink(fn)
except OSError:
- cloudlog.exception("delete_failed", stat=stat, exc=self.last_exc, key=key, fn=fn, sz=sz)
+ cloudlog.event("delete_failed", stat=stat, exc=self.last_exc, key=key, fn=fn, sz=sz)
success = True
else:
@@ -240,13 +240,14 @@ def uploader_fn(exit_event):
cloudlog.info("uploader_fn")
params = Params()
- dongle_id, access_token = params.get("DongleId"), params.get("AccessToken")
+ dongle_id = params.get("DongleId")
+ private_key = open("/persist/comma/id_rsa").read()
- if dongle_id is None or access_token is None:
- cloudlog.info("uploader MISSING DONGLE_ID or ACCESS_TOKEN")
- raise Exception("uploader can't start without dongle id and access token")
+ if dongle_id is None or private_key is None:
+ cloudlog.info("uploader missing dongle_id or private_key")
+ raise Exception("uploader can't start without dongle id and private key")
- uploader = Uploader(dongle_id, access_token, ROOT)
+ uploader = Uploader(dongle_id, private_key, ROOT)
backoff = 0.1
while True:
diff --git a/selfdrive/manager.py b/selfdrive/manager.py
index c3be2c094..c5990f4fd 100755
--- a/selfdrive/manager.py
+++ b/selfdrive/manager.py
@@ -47,7 +47,7 @@ if __name__ == "__main__":
if is_neos:
version = int(open("/VERSION").read()) if os.path.isfile("/VERSION") else 0
revision = int(open("/REVISION").read()) if version >= 10 else 0 # Revision only present in NEOS 10 and up
- neos_update_required = version < 10 or (version == 10 and revision != 3)
+ neos_update_required = version < 10 or (version == 10 and revision != 4)
if neos_update_required:
# update continue.sh before updating NEOS
@@ -72,12 +72,15 @@ import glob
import shutil
import hashlib
import importlib
+import re
+import stat
import subprocess
import traceback
from multiprocessing import Process
from setproctitle import setproctitle #pylint: disable=no-name-in-module
+from common.file_helpers import atomic_write_in_dir_neos
from common.params import Params
import cereal
ThermalStatus = cereal.log.ThermalData.ThermalStatus
@@ -109,12 +112,14 @@ managed_processes = {
"pandad": "selfdrive.pandad",
"ui": ("selfdrive/ui", ["./start.py"]),
"calibrationd": "selfdrive.locationd.calibrationd",
- "params_learner": ("selfdrive/locationd", ["./params_learner"]),
+ "paramsd": ("selfdrive/locationd", ["./paramsd"]),
"visiond": ("selfdrive/visiond", ["./visiond"]),
"sensord": ("selfdrive/sensord", ["./start_sensord.py"]),
"gpsd": ("selfdrive/sensord", ["./start_gpsd.py"]),
"updated": "selfdrive.updated",
- "athena": "selfdrive.athena.athenad",
+}
+daemon_processes = {
+ "athenad": "selfdrive.athena.athenad",
}
android_packages = ("ai.comma.plus.offroad", "ai.comma.plus.frame")
@@ -128,6 +133,9 @@ unkillable_processes = ['visiond']
# processes to end with SIGINT instead of SIGTERM
interrupt_processes = []
+# processes to end with SIGKILL instead of SIGTERM
+kill_processes = ['sensord']
+
persistent_processes = [
'thermald',
'logmessaged',
@@ -136,7 +144,6 @@ persistent_processes = [
'uploader',
'ui',
'updated',
- 'athena',
]
car_started_processes = [
@@ -146,7 +153,7 @@ car_started_processes = [
'sensord',
'radard',
'calibrationd',
- 'params_learner',
+ 'paramsd',
'visiond',
'proclogd',
'ubloxd',
@@ -209,6 +216,29 @@ def start_managed_process(name):
running[name] = Process(name=name, target=nativelauncher, args=(pargs, cwd))
running[name].start()
+def start_daemon_process(name, params):
+ proc = daemon_processes[name]
+ pid_param = name.capitalize() + 'Pid'
+ pid = params.get(pid_param)
+
+ if pid is not None:
+ try:
+ os.kill(int(pid), 0)
+ # process is running (kill is a poorly-named system call)
+ return
+ except OSError:
+ # process is dead
+ pass
+
+ cloudlog.info("starting daemon %s" % name)
+ proc = subprocess.Popen(['python', '-m', proc],
+ cwd='/',
+ stdout=open('/dev/null', 'w'),
+ stderr=open('/dev/null', 'w'),
+ preexec_fn=os.setpgrp)
+
+ params.put(pid_param, str(proc.pid))
+
def prepare_managed_process(p):
proc = managed_processes[p]
if isinstance(proc, str):
@@ -234,6 +264,8 @@ def kill_managed_process(name):
if running[name].exitcode is None:
if name in interrupt_processes:
os.kill(running[name].pid, signal.SIGINT)
+ elif name in kill_processes:
+ os.kill(running[name].pid, signal.SIGKILL)
else:
running[name].terminate()
@@ -321,6 +353,12 @@ def manager_thread():
# save boot log
subprocess.call(["./loggerd", "--bootlog"], cwd=os.path.join(BASEDIR, "selfdrive/loggerd"))
+ params = Params()
+
+ # start daemon processes
+ for p in daemon_processes:
+ start_daemon_process(p, params)
+
# start persistent processes
for p in persistent_processes:
start_managed_process(p)
@@ -332,11 +370,9 @@ def manager_thread():
if os.getenv("NOBOARD") is None:
start_managed_process("pandad")
- params = Params()
logger_dead = False
while 1:
- # get health of board, log this in "thermal"
msg = messaging.recv_sock(thermal_sock, wait=True)
# uploader is gated based on the phone temperature
@@ -420,10 +456,46 @@ def update_apks():
assert success
+def update_ssh():
+ ssh_home_dirpath = "/system/comma/home/.ssh/"
+ auth_keys_path = os.path.join(ssh_home_dirpath, "authorized_keys")
+ auth_keys_persist_path = os.path.join(ssh_home_dirpath, "authorized_keys.persist")
+ auth_keys_mode = stat.S_IREAD | stat.S_IWRITE
+
+ params = Params()
+ github_keys = params.get("GithubSshKeys") or ''
+
+ old_keys = open(auth_keys_path).read()
+ has_persisted_keys = os.path.exists(auth_keys_persist_path)
+ if has_persisted_keys:
+ persisted_keys = open(auth_keys_persist_path).read()
+ else:
+ # add host filter
+ persisted_keys = re.sub(r'^(?!.+?from.+? )(ssh|ecdsa)', 'from="10.0.0.0/8,172.16.0.0/12,192.168.0.0/16" \\1', old_keys, flags=re.MULTILINE)
+
+ new_keys = persisted_keys + '\n' + github_keys
+
+ if has_persisted_keys and new_keys == old_keys and os.stat(auth_keys_path)[stat.ST_MODE] == auth_keys_mode:
+ # nothing to do - let's avoid remount
+ return
+
+ try:
+ subprocess.check_call(["mount", "-o", "rw,remount", "/system"])
+ if not has_persisted_keys:
+ atomic_write_in_dir_neos(auth_keys_persist_path, persisted_keys, mode=auth_keys_mode)
+
+ atomic_write_in_dir_neos(auth_keys_path, new_keys, mode=auth_keys_mode)
+ finally:
+ try:
+ subprocess.check_call(["mount", "-o", "ro,remount", "/system"])
+ except:
+ cloudlog.exception("Failed to remount as read-only")
+ # this can fail due to "Device busy" - reboot if so
+ os.system("reboot")
+ raise RuntimeError
+
def manager_update():
- if os.path.exists(os.path.join(BASEDIR, "vpn")):
- cloudlog.info("installing vpn")
- os.system(os.path.join(BASEDIR, "vpn", "install.sh"))
+ update_ssh()
update_apks()
def manager_prepare():
@@ -446,6 +518,9 @@ def main():
# the flippening!
os.system('LD_LIBRARY_PATH="" content insert --uri content://settings/system --bind name:s:user_rotation --bind value:i:1')
+ # disable bluetooth
+ os.system('service call bluetooth_manager 8')
+
if os.getenv("NOLOG") is not None:
del managed_processes['loggerd']
del managed_processes['tombstoned']
@@ -498,6 +573,8 @@ def main():
params.put("LongitudinalControl", "0")
if params.get("LimitSetSpeed") is None:
params.put("LimitSetSpeed", "0")
+ if params.get("LimitSetSpeedNeural") is None:
+ params.put("LimitSetSpeedNeural", "0")
# is this chffrplus?
if os.getenv("PASSIVE") is not None:
diff --git a/selfdrive/messaging.py b/selfdrive/messaging.py
index 4e78f66c8..cad7e2e8b 100644
--- a/selfdrive/messaging.py
+++ b/selfdrive/messaging.py
@@ -16,17 +16,34 @@ def pub_sock(port, addr="*"):
sock.bind("tcp://%s:%d" % (addr, port))
return sock
-def sub_sock(port, poller=None, addr="127.0.0.1", conflate=False):
+def sub_sock(port, poller=None, addr="127.0.0.1", conflate=False, timeout=None):
context = zmq.Context.instance()
sock = context.socket(zmq.SUB)
if conflate:
sock.setsockopt(zmq.CONFLATE, 1)
sock.connect("tcp://%s:%d" % (addr, port))
sock.setsockopt(zmq.SUBSCRIBE, b"")
+
+ if timeout is not None:
+ sock.RCVTIMEO = timeout
+
if poller is not None:
poller.register(sock, zmq.POLLIN)
return sock
+def drain_sock_raw(sock, wait_for_one=False):
+ ret = []
+ while 1:
+ try:
+ if wait_for_one and len(ret) == 0:
+ dat = sock.recv()
+ else:
+ dat = sock.recv(zmq.NOBLOCK)
+ ret.append(dat)
+ except zmq.error.Again:
+ break
+ return ret
+
def drain_sock(sock, wait_for_one=False):
ret = []
while 1:
@@ -82,24 +99,29 @@ class SubMaster():
self.valid = {}
for s in services:
# TODO: get address automatically from service_list
- self.sock[s] = sub_sock(service_list[s].port, poller=self.poller, addr=addr, conflate=True)
+ if addr is not None:
+ self.sock[s] = sub_sock(service_list[s].port, poller=self.poller, addr=addr, conflate=True)
self.freq[s] = service_list[s].frequency
data = new_message()
data.init(s)
self.data[s] = getattr(data, s)
- self.logMonoTime[s] = data.logMonoTime
+ self.logMonoTime[s] = 0
self.valid[s] = data.valid
def __getitem__(self, s):
return self.data[s]
def update(self, timeout=-1):
+ msgs = []
+ for sock, _ in self.poller.poll(timeout):
+ msgs.append(recv_one(sock))
+ self.update_msgs(sec_since_boot(), msgs)
+
+ def update_msgs(self, cur_time, msgs):
# TODO: add optional input that specify the service to wait for
self.frame += 1
self.updated = dict.fromkeys(self.updated, False)
- cur_time = sec_since_boot()
- for sock, _ in self.poller.poll(timeout):
- msg = recv_one(sock)
+ for msg in msgs:
s = msg.which()
self.updated[s] = True
self.rcv_time[s] = cur_time
diff --git a/selfdrive/registration.py b/selfdrive/registration.py
index 9f6899849..5d45b1129 100644
--- a/selfdrive/registration.py
+++ b/selfdrive/registration.py
@@ -5,7 +5,7 @@ import struct
from datetime import datetime, timedelta
from selfdrive.swaglog import cloudlog
-from selfdrive.version import version, training_version, get_git_commit, get_git_branch, get_git_remote
+from selfdrive.version import version, terms_version, training_version, get_git_commit, get_git_branch, get_git_remote
from common.api import api_get
from common.params import Params
from common.file_helpers import mkdirs_exists_ok
@@ -53,6 +53,7 @@ def get_subscriber_info():
def register():
params = Params()
params.put("Version", version)
+ params.put("TermsVersion", terms_version)
params.put("TrainingVersion", training_version)
params.put("GitCommit", get_git_commit())
params.put("GitBranch", get_git_branch())
@@ -70,6 +71,10 @@ def register():
os.rename("/persist/comma/id_rsa.tmp", "/persist/comma/id_rsa")
os.rename("/persist/comma/id_rsa.tmp.pub", "/persist/comma/id_rsa.pub")
+ # make key readable by app users (ai.comma.plus.offroad)
+ os.chmod('/persist/comma/', 0o755)
+ os.chmod('/persist/comma/id_rsa', 0o744)
+
dongle_id, access_token = params.get("DongleId"), params.get("AccessToken")
public_key = open("/persist/comma/id_rsa.pub").read()
diff --git a/selfdrive/service_list.yaml b/selfdrive/service_list.yaml
index 9d141c88d..0c82a3187 100644
--- a/selfdrive/service_list.yaml
+++ b/selfdrive/service_list.yaml
@@ -11,14 +11,14 @@ sensorEvents: [8003, true, 100., 100]
# GPS data, also global timestamp
gpsNMEA: [8004, true, 9.] # 9 msgs each sec
# CPU+MEM+GPU+BAT temps
-thermal: [8005, true, 1., 1]
+thermal: [8005, true, 2., 1]
# List(CanData), list of can messages
can: [8006, true, 100.]
controlsState: [8007, true, 100., 100]
#liveEvent: [8008, true, 0.]
model: [8009, true, 20.]
features: [8010, true, 0.]
-health: [8011, true, 1., 1]
+health: [8011, true, 2., 1]
radarState: [8012, true, 20.]
#liveUI: [8014, true, 0.]
encodeIdx: [8015, true, 20.]
diff --git a/selfdrive/test/openpilotci_upload.py b/selfdrive/test/openpilotci_upload.py
new file mode 100755
index 000000000..85509851d
--- /dev/null
+++ b/selfdrive/test/openpilotci_upload.py
@@ -0,0 +1,22 @@
+#!/usr/bin/env python2
+import os
+import sys
+import subprocess
+from azure.storage.blob import BlockBlobService
+
+def upload_file(path, name):
+ sas_token = os.getenv("TOKEN", None)
+ if sas_token is not None:
+ service = BlockBlobService(account_name="commadataci", sas_token=sas_token)
+ else:
+ account_key = subprocess.check_output("az storage account keys list --account-name commadataci --output tsv --query '[0].value'", shell=True)
+ service = BlockBlobService(account_name="commadataci", account_key=account_key)
+ service.create_blob_from_path("openpilotci", name, path)
+ return "https://commadataci.blob.core.windows.net/openpilotci/" + name
+
+if __name__ == "__main__":
+ for f in sys.argv[1:]:
+ name = os.path.basename(f)
+ url = upload_file(f, name)
+ print url
+
diff --git a/selfdrive/test/plant/maneuver.py b/selfdrive/test/plant/maneuver.py
index 551fbd0d9..ea5ebdd9e 100644
--- a/selfdrive/test/plant/maneuver.py
+++ b/selfdrive/test/plant/maneuver.py
@@ -1,3 +1,4 @@
+from collections import defaultdict
from selfdrive.test.plant.maneuverplots import ManeuverPlot
from selfdrive.test.plant.plant import Plant
import numpy as np
@@ -16,6 +17,7 @@ class Maneuver(object):
self.speed_lead_breakpoints = kwargs.get("speed_lead_breakpoints", [0.0, duration])
self.cruise_button_presses = kwargs.get("cruise_button_presses", [])
+ self.checks = kwargs.get("checks", [])
self.duration = duration
self.title = title
@@ -28,6 +30,7 @@ class Maneuver(object):
distance_lead = self.distance_lead
)
+ logs = defaultdict(list)
last_controls_state = None
plot = ManeuverPlot(self.title)
@@ -42,20 +45,24 @@ class Maneuver(object):
grade = np.interp(plant.current_time(), self.grade_breakpoints, self.grade_values)
speed_lead = np.interp(plant.current_time(), self.speed_lead_breakpoints, self.speed_lead_values)
- distance, speed, acceleration, distance_lead, brake, gas, steer_torque, fcw, controls_state= plant.step(speed_lead, current_button, grade)
- if controls_state:
- last_controls_state = controls_state[-1]
+ # distance, speed, acceleration, distance_lead, brake, gas, steer_torque, fcw, controls_state= plant.step(speed_lead, current_button, grade)
+ log = plant.step(speed_lead, current_button, grade)
- d_rel = distance_lead - distance if self.lead_relevancy else 200.
- v_rel = speed_lead - speed if self.lead_relevancy else 0.
+ if log['controls_state_msgs']:
+ last_controls_state = log['controls_state_msgs'][-1]
+
+ d_rel = log['distance_lead'] - log['distance'] if self.lead_relevancy else 200.
+ v_rel = speed_lead - log['speed'] if self.lead_relevancy else 0.
+ log['d_rel'] = d_rel
+ log['v_rel'] = v_rel
if last_controls_state:
# print(last_controls_state)
#develop plots
plot.add_data(
time=plant.current_time(),
- gas=gas, brake=brake, steer_torque=steer_torque,
- distance=distance, speed=speed, acceleration=acceleration,
+ gas=log['gas'], brake=log['brake'], steer_torque=log['steer_torque'],
+ distance=log['distance'], speed=log['speed'], acceleration=log['acceleration'],
up_accel_cmd=last_controls_state.upAccelCmd, ui_accel_cmd=last_controls_state.uiAccelCmd,
uf_accel_cmd=last_controls_state.ufAccelCmd,
d_rel=d_rel, v_rel=v_rel, v_lead=speed_lead,
@@ -63,10 +70,17 @@ class Maneuver(object):
cruise_speed=last_controls_state.vCruise,
jerk_factor=last_controls_state.jerkFactor,
a_target=last_controls_state.aTarget,
- fcw=fcw)
+ fcw=log['fcw'])
- print("maneuver end")
-
- return (None, plot)
+ for k, v in log.items():
+ logs[k].append(v)
+ valid = True
+ for check in self.checks:
+ c = check(logs)
+ if not c:
+ print check.__name__ + " not valid!"
+ valid = valid and c
+ print("maneuver end", valid)
+ return (plot, valid)
diff --git a/selfdrive/test/plant/plant.py b/selfdrive/test/plant/plant.py
index e39bafd79..531ad8ad2 100755
--- a/selfdrive/test/plant/plant.py
+++ b/selfdrive/test/plant/plant.py
@@ -1,6 +1,7 @@
#!/usr/bin/env python
import os
import struct
+import time
from collections import namedtuple
import numpy as np
@@ -133,6 +134,11 @@ class Plant(object):
self.ts = 1./rate
self.cp = get_car_can_parser()
+ self.response_seen = False
+
+ time.sleep(1)
+ messaging.drain_sock(Plant.sendcan)
+ messaging.drain_sock(Plant.controls_state)
def close(self):
Plant.logcan.close()
@@ -158,13 +164,18 @@ class Plant(object):
# ******** get messages sent to the car ********
can_msgs = []
- for a in messaging.drain_sock(Plant.sendcan):
+ for a in messaging.drain_sock(Plant.sendcan, wait_for_one=self.response_seen):
can_msgs.extend(can_capnp_to_can_list(a.sendcan, [0,2]))
+
+ # After the first response the car is done fingerprinting, so we can run in lockstep with controlsd
+ if can_msgs:
+ self.response_seen = True
+
self.cp.update_can(can_msgs)
# ******** get controlsState messages for plotting ***
controls_state_msgs = []
- for a in messaging.drain_sock(Plant.controls_state):
+ for a in messaging.drain_sock(Plant.controls_state, wait_for_one=self.response_seen):
controls_state_msgs.append(a.controlsState)
fcw = None
@@ -217,7 +228,7 @@ class Plant(object):
vls_tuple = namedtuple('vls', [
'XMISSION_SPEED',
'WHEEL_SPEED_FL', 'WHEEL_SPEED_FR', 'WHEEL_SPEED_RL', 'WHEEL_SPEED_RR',
- 'STEER_ANGLE', 'STEER_ANGLE_RATE', 'STEER_TORQUE_SENSOR',
+ 'STEER_ANGLE', 'STEER_ANGLE_RATE', 'STEER_TORQUE_SENSOR', 'STEER_TORQUE_MOTOR',
'LEFT_BLINKER', 'RIGHT_BLINKER',
'GEAR',
'WHEELS_MOVING',
@@ -244,12 +255,13 @@ class Plant(object):
'EPB_STATE',
'BRAKE_HOLD_ACTIVE',
'INTERCEPTOR_GAS',
+ 'INTERCEPTOR_GAS2',
'IMPERIAL_UNIT',
])
vls = vls_tuple(
self.speed_sensor(speed),
self.speed_sensor(speed), self.speed_sensor(speed), self.speed_sensor(speed), self.speed_sensor(speed),
- self.angle_steer, self.angle_steer_rate, 0, #Steer torque sensor
+ self.angle_steer, self.angle_steer_rate, 0, 0,#Steer torque sensor
0, 0, # Blinkers
self.gear_choice,
speed != 0,
@@ -276,6 +288,7 @@ class Plant(object):
0, # EPB State
0, # Brake hold
0, # Interceptor feedback
+ 0, # Interceptor 2 feedback
False
)
@@ -323,20 +336,21 @@ class Plant(object):
msg_data = fix(msg_data, 0xe4)
can_msgs.append([0xe4, 0, msg_data, 2])
- Plant.logcan.send(can_list_to_can_capnp(can_msgs))
# Fake sockets that controlsd subscribes to
live_parameters = messaging.new_message()
live_parameters.init('liveParameters')
live_parameters.liveParameters.valid = True
live_parameters.liveParameters.sensorValid = True
+ live_parameters.liveParameters.posenetValid = True
live_parameters.liveParameters.steerRatio = CP.steerRatio
live_parameters.liveParameters.stiffnessFactor = 1.0
Plant.live_params.send(live_parameters.to_bytes())
driver_monitoring = messaging.new_message()
driver_monitoring.init('driverMonitoring')
- driver_monitoring.driverMonitoring.descriptor = [0.] * 7
+ driver_monitoring.driverMonitoring.faceOrientation = [0.] * 3
+ driver_monitoring.driverMonitoring.facePosition = [0.] * 2
Plant.driverMonitoring.send(driver_monitoring.to_bytes())
health = messaging.new_message()
@@ -362,15 +376,35 @@ class Plant(object):
x.points = [0.0]*50
x.prob = 1.0
x.std = 1.0
+
+ if self.lead_relevancy:
+ d_rel = np.maximum(0., distance_lead - distance)
+ v_rel = v_lead - speed
+ prob = 1.0
+ else:
+ d_rel = 200.
+ v_rel = 0.
+ prob = 0.0
+
md.model.lead.dist = float(d_rel)
- md.model.lead.prob = 1.
- md.model.lead.std = 0.1
+ md.model.lead.prob = prob
+ md.model.lead.relY = 0.0
+ md.model.lead.relYStd = 1.
+ md.model.lead.relVel = float(v_rel)
+ md.model.lead.relVelStd = 1.
+ md.model.lead.relA = 0.0
+ md.model.lead.relAStd = 10.
+ md.model.lead.std = 1.0
+
cal.liveCalibration.calStatus = 1
cal.liveCalibration.calPerc = 100
+ cal.liveCalibration.rpyCalib = [0.] * 3
# fake values?
Plant.model.send(md.to_bytes())
Plant.cal.send(cal.to_bytes())
+ Plant.logcan.send(can_list_to_can_capnp(can_msgs))
+
# ******** update prevs ********
self.speed = speed
self.distance = distance
@@ -380,8 +414,22 @@ class Plant(object):
self.distance_prev = distance
self.distance_lead_prev = distance_lead
- self.rk.keep_time()
- return (distance, speed, acceleration, distance_lead, brake, gas, steer_torque, fcw, controls_state_msgs)
+ if self.response_seen:
+ self.rk.monitor_time()
+ else:
+ self.rk.keep_time()
+
+ return {
+ "distance": distance,
+ "speed": speed,
+ "acceleration": acceleration,
+ "distance_lead": distance_lead,
+ "brake": brake,
+ "gas": gas,
+ "steer_torque": steer_torque,
+ "fcw": fcw,
+ "controls_state_msgs": controls_state_msgs,
+ }
# simple engage in standalone mode
def plant_thread(rate=100):
diff --git a/selfdrive/test/test_car_models_openpilot.py b/selfdrive/test/test_car_models_openpilot.py
new file mode 100755
index 000000000..53184653c
--- /dev/null
+++ b/selfdrive/test/test_car_models_openpilot.py
@@ -0,0 +1,494 @@
+#!/usr/bin/env python2
+import shutil
+import time
+import zmq
+import os
+import sys
+import signal
+import subprocess
+import requests
+from cereal import car
+
+import selfdrive.manager as manager
+from selfdrive.services import service_list
+import selfdrive.messaging as messaging
+from common.params import Params
+from common.basedir import BASEDIR
+from selfdrive.car.honda.values import CAR as HONDA
+from selfdrive.car.toyota.values import CAR as TOYOTA
+from selfdrive.car.gm.values import CAR as GM
+from selfdrive.car.ford.values import CAR as FORD
+from selfdrive.car.hyundai.values import CAR as HYUNDAI
+from selfdrive.car.chrysler.values import CAR as CHRYSLER
+from selfdrive.car.subaru.values import CAR as SUBARU
+from selfdrive.car.mock.values import CAR as MOCK
+
+
+os.environ['NOCRASH'] = '1'
+
+
+def wait_for_socket(name, timeout=10.0):
+ socket = messaging.sub_sock(service_list[name].port)
+ cur_time = time.time()
+
+ r = None
+ while time.time() - cur_time < timeout:
+ print("waiting for %s" % name)
+ try:
+ r = socket.recv(zmq.NOBLOCK)
+ break
+ except zmq.error.Again:
+ pass
+ time.sleep(0.5)
+ return r
+
+def get_route_logs(route_name):
+ for log_f in ["rlog.bz2", "fcamera.hevc"]:
+ log_path = os.path.join("/tmp", "%s--0--%s" % (route_name.replace("|", "_"), log_f))
+
+ if not os.path.isfile(log_path):
+ log_url = "https://commadataci.blob.core.windows.net/openpilotci/%s/0/%s" % (route_name.replace("|", "/"), log_f)
+ r = requests.get(log_url)
+
+ if r.status_code == 200:
+ with open(log_path, "w") as f:
+ f.write(r.content)
+ else:
+ sys.exit(-1)
+
+routes = {
+
+ "975b26878285314d|2018-12-25--14-42-13": {
+ 'carFingerprint': CHRYSLER.PACIFICA_2018_HYBRID,
+ 'enableCamera': True,
+ },
+ "b0c9d2329ad1606b|2019-01-06--10-11-23": {
+ 'carFingerprint': CHRYSLER.PACIFICA_2017_HYBRID,
+ 'enableCamera': True,
+ },
+ "0607d2516fc2148f|2019-02-13--23-03-16": {
+ 'carFingerprint': CHRYSLER.PACIFICA_2019_HYBRID,
+ 'enableCamera': True,
+ },
+ # This pacifica was removed because the fingerprint seemed from a Volt
+ #"9f7a7e50a51fb9db|2019-01-03--14-05-01": {
+ # 'carFingerprint': CHRYSLER.PACIFICA_2018,
+ # 'enableCamera': True,
+ #},
+ "9f7a7e50a51fb9db|2019-01-17--18-34-21": {
+ 'carFingerprint': CHRYSLER.JEEP_CHEROKEE,
+ 'enableCamera': True,
+ },
+ "192a598e34926b1e|2019-04-04--13-27-39": {
+ 'carFingerprint': CHRYSLER.JEEP_CHEROKEE_2019,
+ 'enableCamera': True,
+ },
+ "f1b4c567731f4a1b|2018-04-18--11-29-37": {
+ 'carFingerprint': FORD.FUSION,
+ 'enableCamera': False,
+ },
+ "f1b4c567731f4a1b|2018-04-30--10-15-35": {
+ 'carFingerprint': FORD.FUSION,
+ 'enableCamera': True,
+ },
+ "7ed9cdf8d0c5f43e|2018-05-17--09-31-36": {
+ 'carFingerprint': GM.CADILLAC_CT6,
+ 'enableCamera': True,
+ },
+ "265007255e794bce|2018-11-24--22-08-31": {
+ 'carFingerprint': GM.CADILLAC_ATS,
+ 'enableCamera': True,
+ },
+ "c950e28c26b5b168|2018-05-30--22-03-41": {
+ 'carFingerprint': GM.VOLT,
+ 'enableCamera': True,
+ },
+ # TODO: use another route that has radar data at start
+ "aadda448b49c99ad|2018-10-25--17-16-22": {
+ 'carFingerprint': GM.MALIBU,
+ 'enableCamera': True,
+ },
+ "49c73650e65ff465|2018-11-19--16-58-04": {
+ 'carFingerprint': GM.HOLDEN_ASTRA,
+ 'enableCamera': True,
+ },
+ "7cc2a8365b4dd8a9|2018-12-02--12-10-44": {
+ 'carFingerprint': GM.ACADIA,
+ 'enableCamera': True,
+ },
+ "aa20e335f61ba898|2018-12-17--21-10-37": {
+ 'carFingerprint': GM.BUICK_REGAL,
+ 'enableCamera': False,
+ },
+ "aa20e335f61ba898|2019-02-05--16-59-04": {
+ 'carFingerprint': GM.BUICK_REGAL,
+ 'enableCamera': True,
+ },
+ "7d44af5b7a1b2c8e|2017-09-16--01-50-07": {
+ 'carFingerprint': HONDA.CIVIC,
+ 'enableCamera': True,
+ },
+ "c9d60e5e02c04c5c|2018-01-08--16-01-49": {
+ 'carFingerprint': HONDA.CRV,
+ 'enableCamera': True,
+ },
+ "1851183c395ef471|2018-05-31--18-07-21": {
+ 'carFingerprint': HONDA.CRV_5G,
+ 'enableCamera': True,
+ },
+ "232585b7784c1af4|2019-04-08--14-12-14": {
+ 'carFingerprint': HONDA.CRV_HYBRID,
+ 'enableCamera': True,
+ },
+ "2ac95059f70d76eb|2018-02-05--15-03-29": {
+ 'carFingerprint': HONDA.ACURA_ILX,
+ 'enableCamera': True,
+ },
+ "21aa231dee2a68bd|2018-01-30--04-54-41": {
+ 'carFingerprint': HONDA.ODYSSEY,
+ 'enableCamera': True,
+ },
+ "81722949a62ea724|2019-03-29--15-51-26": {
+ 'carFingerprint': HONDA.ODYSSEY_CHN,
+ 'enableCamera': False,
+ },
+ "81722949a62ea724|2019-04-06--15-19-25": {
+ 'carFingerprint': HONDA.ODYSSEY_CHN,
+ 'enableCamera': True,
+ },
+ "5a2cfe4bb362af9e|2018-02-02--23-41-07": {
+ 'carFingerprint': HONDA.ACURA_RDX,
+ 'enableCamera': True,
+ },
+ "3e9592a1c78a3d63|2018-02-08--20-28-24": {
+ 'carFingerprint': HONDA.PILOT,
+ 'enableCamera': True,
+ },
+ "34a84d2b765df688|2018-08-28--12-41-00": {
+ 'carFingerprint': HONDA.PILOT_2019,
+ 'enableCamera': True,
+ },
+ "900ad17e536c3dc7|2018-04-12--22-02-36": {
+ 'carFingerprint': HONDA.RIDGELINE,
+ 'enableCamera': True,
+ },
+ "f1b4c567731f4a1b|2018-06-06--14-43-46": {
+ 'carFingerprint': HONDA.ACCORD,
+ 'enableCamera': True,
+ },
+ "1582e1dc57175194|2018-08-15--07-46-07": {
+ 'carFingerprint': HONDA.ACCORD_15,
+ 'enableCamera': True,
+ },
+ # TODO: This doesnt fingerprint because the fingerprint overlaps with the Insight
+ # "690c4c9f9f2354c7|2018-09-15--17-36-05": {
+ # 'carFingerprint': HONDA.ACCORDH,
+ # 'enableCamera': True,
+ # },
+ "1632088eda5e6c4d|2018-06-07--08-03-18": {
+ 'carFingerprint': HONDA.CIVIC_BOSCH,
+ 'enableCamera': True,
+ },
+ #"18971a99f3f2b385|2018-11-14--19-09-31": {
+ # 'carFingerprint': HONDA.INSIGHT,
+ # 'enableCamera': True,
+ #},
+ "38bfd238edecbcd7|2018-08-22--09-45-44": {
+ 'carFingerprint': HYUNDAI.SANTA_FE,
+ 'enableCamera': False,
+ },
+ "38bfd238edecbcd7|2018-08-29--22-02-15": {
+ 'carFingerprint': HYUNDAI.SANTA_FE,
+ 'enableCamera': True,
+ },
+ "a893a80e5c5f72c8|2019-01-14--20-02-59": {
+ 'carFingerprint': HYUNDAI.GENESIS,
+ 'enableCamera': True,
+ },
+ "9d5fb4f0baa1b3e1|2019-01-14--17-45-59": {
+ 'carFingerprint': HYUNDAI.KIA_SORENTO,
+ 'enableCamera': True,
+ },
+ "215cd70e9c349266|2018-11-25--22-22-12": {
+ 'carFingerprint': HYUNDAI.KIA_STINGER,
+ 'enableCamera': True,
+ },
+ "31390e3eb6f7c29a|2019-01-23--08-56-00": {
+ 'carFingerprint': HYUNDAI.KIA_OPTIMA,
+ 'enableCamera': True,
+ },
+ "53ac3251e03f95d7|2019-01-10--13-43-32": {
+ 'carFingerprint': HYUNDAI.ELANTRA,
+ 'enableCamera': True,
+ },
+ "f7b6be73e3dfd36c|2019-05-12--18-07-16": {
+ 'carFingerprint': TOYOTA.AVALON,
+ 'enableCamera': False,
+ 'enableDsu': False,
+ },
+ "f7b6be73e3dfd36c|2019-05-11--22-34-20": {
+ 'carFingerprint': TOYOTA.AVALON,
+ 'enableCamera': True,
+ 'enableDsu': False,
+ },
+ "b0f5a01cf604185c|2018-01-26--00-54-32": {
+ 'carFingerprint': TOYOTA.COROLLA,
+ 'enableCamera': True,
+ 'enableDsu': True,
+ },
+ "b0f5a01cf604185c|2018-01-26--10-54-38": {
+ 'carFingerprint': TOYOTA.COROLLA,
+ 'enableCamera': True,
+ 'enableDsu': False,
+ },
+ "b0f5a01cf604185c|2018-01-26--10-59-31": {
+ 'carFingerprint': TOYOTA.COROLLA,
+ 'enableCamera': False,
+ 'enableDsu': False,
+ },
+ "5f5afb36036506e4|2019-05-14--02-09-54": {
+ 'carFingerprint': TOYOTA.COROLLA_TSS2,
+ 'enableCamera': True,
+ 'enableDsu': True,
+ },
+ "56fb1c86a9a86404|2017-11-10--10-18-43": {
+ 'carFingerprint': TOYOTA.PRIUS,
+ 'enableCamera': True,
+ 'enableDsu': True,
+ },
+ "b0f5a01cf604185c|2017-12-18--20-32-32": {
+ 'carFingerprint': TOYOTA.RAV4,
+ 'enableCamera': True,
+ 'enableDsu': True,
+ 'enableGasInterceptor': False,
+ },
+ "b0c9d2329ad1606b|2019-04-02--13-24-43": {
+ 'carFingerprint': TOYOTA.RAV4,
+ 'enableCamera': True,
+ 'enableDsu': True,
+ 'enableGasInterceptor': True,
+ },
+ "cdf2f7de565d40ae|2019-04-25--03-53-41": {
+ 'carFingerprint': TOYOTA.RAV4_TSS2,
+ 'enableCamera': True,
+ 'enableDsu': True,
+ },
+ "f49e8041283f2939|2019-05-29--13-48-33": {
+ 'carFingerprint': TOYOTA.LEXUS_ESH_TSS2,
+ 'enableCamera': False,
+ 'enableDsu': False,
+ },
+ "f49e8041283f2939|2019-05-30--11-51-51": {
+ 'carFingerprint': TOYOTA.LEXUS_ESH_TSS2,
+ 'enableCamera': True,
+ 'enableDsu': True,
+ },
+ "b0f5a01cf604185c|2018-02-01--21-12-28": {
+ 'carFingerprint': TOYOTA.LEXUS_RXH,
+ 'enableCamera': True,
+ 'enableDsu': True,
+ },
+ #FIXME: This works sometimes locally, but never in CI. Timing issue?
+ #"b0f5a01cf604185c|2018-01-31--20-11-39": {
+ # 'carFingerprint': TOYOTA.LEXUS_RXH,
+ # 'enableCamera': False,
+ # 'enableDsu': False,
+ #},
+ "8ae193ceb56a0efe|2018-06-18--20-07-32": {
+ 'carFingerprint': TOYOTA.RAV4H,
+ 'enableCamera': True,
+ 'enableDsu': True,
+ },
+ "fd10b9a107bb2e49|2018-07-24--16-32-42": {
+ 'carFingerprint': TOYOTA.CHR,
+ 'enableCamera': True,
+ 'enableDsu': False,
+ },
+ "fd10b9a107bb2e49|2018-07-24--20-32-08": {
+ 'carFingerprint': TOYOTA.CHR,
+ 'enableCamera': False,
+ 'enableDsu': False,
+ },
+ "b4c18bf13d5955da|2018-07-29--13-39-46": {
+ 'carFingerprint': TOYOTA.CHRH,
+ 'enableCamera': True,
+ 'enableDsu': False,
+ },
+ "d2d8152227f7cb82|2018-07-25--13-40-56": {
+ 'carFingerprint': TOYOTA.CAMRY,
+ 'enableCamera': True,
+ 'enableDsu': False,
+ },
+ "fbd011384db5e669|2018-07-26--20-51-48": {
+ 'carFingerprint': TOYOTA.CAMRYH,
+ 'enableCamera': True,
+ 'enableDsu': False,
+ },
+ # TODO: This replay has no good model/video
+ # "c9fa2dd0f76caf23|2018-02-10--13-40-28": {
+ # 'carFingerprint': TOYOTA.CAMRYH,
+ # 'enableCamera': False,
+ # 'enableDsu': False,
+ # },
+ # TODO: missingsome combos for highlander
+ "aa659debdd1a7b54|2018-08-31--11-12-01": {
+ 'carFingerprint': TOYOTA.HIGHLANDER,
+ 'enableCamera': False,
+ 'enableDsu': False,
+ },
+ "362d23d4d5bea2fa|2018-09-02--17-03-55": {
+ 'carFingerprint': TOYOTA.HIGHLANDERH,
+ 'enableCamera': True,
+ 'enableDsu': True,
+ },
+ "eb6acd681135480d|2019-06-20--20-00-00": {
+ 'carFingerprint': TOYOTA.SIENNA,
+ 'enableCamera': True,
+ 'enableDsu': False,
+ },
+ "362d23d4d5bea2fa|2018-08-10--13-31-40": {
+ 'carFingerprint': TOYOTA.HIGHLANDERH,
+ 'enableCamera': False,
+ 'enableDsu': False,
+ },
+ "791340bc01ed993d|2019-03-10--16-28-08": {
+ 'carFingerprint': SUBARU.IMPREZA,
+ 'enableCamera': True,
+ },
+ # Tesla route, should result in mock car
+ "07cb8a788c31f645|2018-06-17--18-50-29": {
+ 'carFingerprint': MOCK.MOCK,
+ },
+ ## Route with no can data, should result in mock car. This is not supported anymore
+ #"bfa17080b080f3ec|2018-06-28--23-27-47": {
+ # 'carFingerprint': MOCK.MOCK,
+ #},
+}
+
+passive_routes = [
+ "07cb8a788c31f645|2018-06-17--18-50-29",
+ #"bfa17080b080f3ec|2018-06-28--23-27-47",
+]
+
+public_routes = [
+ "f1b4c567731f4a1b|2018-06-06--14-43-46",
+ "f1b4c567731f4a1b|2018-04-18--11-29-37",
+ "f1b4c567731f4a1b|2018-04-18--11-29-37",
+ "7ed9cdf8d0c5f43e|2018-05-17--09-31-36",
+ "38bfd238edecbcd7|2018-08-22--09-45-44",
+ "38bfd238edecbcd7|2018-08-29--22-02-15",
+ "b0f5a01cf604185c|2018-01-26--00-54-32",
+ "b0f5a01cf604185c|2018-01-26--10-54-38",
+ "b0f5a01cf604185c|2018-01-26--10-59-31",
+ "56fb1c86a9a86404|2017-11-10--10-18-43",
+ "b0f5a01cf604185c|2017-12-18--20-32-32",
+ "b0c9d2329ad1606b|2019-04-02--13-24-43",
+ "791340bc01ed993d|2019-03-10--16-28-08",
+]
+
+if __name__ == "__main__":
+
+ results = {}
+ for route, checks in routes.items():
+
+ if route not in public_routes:
+ print "route not public", route
+ continue
+
+ get_route_logs(route)
+
+ for _ in range(3):
+ shutil.rmtree('/data/params')
+ manager.gctx = {}
+ params = Params()
+ params.manager_start()
+ if route in passive_routes:
+ params.put("Passive", "1")
+ else:
+ params.put("Passive", "0")
+
+ print "testing ", route, " ", checks['carFingerprint']
+ print "Preparing processes"
+ manager.prepare_managed_process("radard")
+ manager.prepare_managed_process("controlsd")
+ manager.prepare_managed_process("plannerd")
+ print "Starting processes"
+ manager.start_managed_process("radard")
+ manager.start_managed_process("controlsd")
+ manager.start_managed_process("plannerd")
+ time.sleep(2)
+
+ # Start unlogger
+ print "Start unlogger"
+ unlogger_cmd = [os.path.join(BASEDIR, 'tools/replay/unlogger.py'), '%s' % route, '/tmp', '--disable', 'frame,plan,pathPlan,liveLongitudinalMpc,radarState,controlsState,liveTracks,liveMpc,sendcan,carState,carControl', '--no-interactive']
+ unlogger = subprocess.Popen(unlogger_cmd, preexec_fn=os.setsid)
+
+ print "Check sockets"
+ controls_state_result = wait_for_socket('controlsState', timeout=30)
+ radarstate_result = wait_for_socket('radarState', timeout=30)
+ plan_result = wait_for_socket('plan', timeout=30)
+
+ if route not in passive_routes: # TODO The passive routes have very flaky models
+ path_plan_result = wait_for_socket('pathPlan', timeout=30)
+ else:
+ path_plan_result = True
+
+ carstate_result = wait_for_socket('carState', timeout=30)
+
+ print "Check if everything is running"
+ running = manager.get_running()
+ controlsd_running = running['controlsd'].is_alive()
+ radard_running = running['radard'].is_alive()
+ plannerd_running = running['plannerd'].is_alive()
+
+ manager.kill_managed_process("controlsd")
+ manager.kill_managed_process("radard")
+ manager.kill_managed_process("plannerd")
+ os.killpg(os.getpgid(unlogger.pid), signal.SIGTERM)
+
+ sockets_ok = all([
+ controls_state_result, radarstate_result, plan_result, path_plan_result, carstate_result,
+ controlsd_running, radard_running, plannerd_running
+ ])
+ params_ok = True
+ failures = []
+
+ if not controlsd_running:
+ failures.append('controlsd')
+ if not radard_running:
+ failures.append('radard')
+ if not radarstate_result:
+ failures.append('radarState')
+ if not controls_state_result:
+ failures.append('controlsState')
+ if not plan_result:
+ failures.append('plan')
+ if not path_plan_result:
+ failures.append('pathPlan')
+
+ try:
+ car_params = car.CarParams.from_bytes(params.get("CarParams"))
+ for k, v in checks.items():
+ if not v == getattr(car_params, k):
+ params_ok = False
+ failures.append(k)
+ except:
+ params_ok = False
+
+ if sockets_ok and params_ok:
+ print "Success"
+ results[route] = True, failures
+ break
+ else:
+ print "Failure"
+ results[route] = False, failures
+
+ time.sleep(2)
+
+ print results
+ params.put("Passive", "0") # put back not passive to not leave the params in an unintended state
+ if not all(passed for passed, _ in results.values()):
+ print "TEST FAILED"
+ sys.exit(1)
+ else:
+ print "TEST SUCESSFUL"
diff --git a/selfdrive/test/tests/__init__.py b/selfdrive/test/tests/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/selfdrive/test/tests/plant/test_longitudinal.py b/selfdrive/test/tests/plant/test_longitudinal.py
index 9806f5a19..09e81b2a1 100755
--- a/selfdrive/test/tests/plant/test_longitudinal.py
+++ b/selfdrive/test/tests/plant/test_longitudinal.py
@@ -22,22 +22,34 @@ def create_dir(path):
except OSError:
pass
+
+def check_no_collision(log):
+ return min(log['d_rel']) > 0
+
+def check_fcw(log):
+ return any(log['fcw'])
+
+def check_engaged(log):
+ return log['controls_state_msgs'][-1][-1].active
+
maneuvers = [
Maneuver(
'while cruising at 40 mph, change cruise speed to 50mph',
duration=30.,
initial_speed = 40. * CV.MPH_TO_MS,
cruise_button_presses = [(CB.DECEL_SET, 2.), (0, 2.3),
- (CB.RES_ACCEL, 10.), (0, 10.1),
- (CB.RES_ACCEL, 10.2), (0, 10.3)]
+ (CB.RES_ACCEL, 10.), (0, 10.1),
+ (CB.RES_ACCEL, 10.2), (0, 10.3)],
+ checks=[check_engaged],
),
Maneuver(
'while cruising at 60 mph, change cruise speed to 50mph',
duration=30.,
initial_speed=60. * CV.MPH_TO_MS,
cruise_button_presses = [(CB.DECEL_SET, 2.), (0, 2.3),
- (CB.DECEL_SET, 10.), (0, 10.1),
- (CB.DECEL_SET, 10.2), (0, 10.3)]
+ (CB.DECEL_SET, 10.), (0, 10.1),
+ (CB.DECEL_SET, 10.2), (0, 10.3)],
+ checks=[check_engaged],
),
Maneuver(
'while cruising at 20mph, grade change +10%',
@@ -45,7 +57,8 @@ maneuvers = [
initial_speed=20. * CV.MPH_TO_MS,
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)],
grade_values = [0., 0., 1.0],
- grade_breakpoints = [0., 10., 11.]
+ grade_breakpoints = [0., 10., 11.],
+ checks=[check_engaged],
),
Maneuver(
'while cruising at 20mph, grade change -10%',
@@ -53,7 +66,8 @@ maneuvers = [
initial_speed=20. * CV.MPH_TO_MS,
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)],
grade_values = [0., 0., -1.0],
- grade_breakpoints = [0., 10., 11.]
+ grade_breakpoints = [0., 10., 11.],
+ checks=[check_engaged],
),
Maneuver(
'approaching a 40mph car while cruising at 60mph from 100m away',
@@ -63,7 +77,8 @@ maneuvers = [
initial_distance_lead=100.,
speed_lead_values = [40.*CV.MPH_TO_MS, 40.*CV.MPH_TO_MS],
speed_lead_breakpoints = [0., 100.],
- cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)]
+ cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)],
+ checks=[check_engaged, check_no_collision],
),
Maneuver(
'approaching a 0mph car while cruising at 40mph from 150m away',
@@ -73,7 +88,8 @@ maneuvers = [
initial_distance_lead=150.,
speed_lead_values = [0.*CV.MPH_TO_MS, 0.*CV.MPH_TO_MS],
speed_lead_breakpoints = [0., 100.],
- cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)]
+ cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)],
+ checks=[check_engaged, check_no_collision],
),
Maneuver(
'steady state following a car at 20m/s, then lead decel to 0mph at 1m/s^2',
@@ -83,7 +99,8 @@ maneuvers = [
initial_distance_lead=35.,
speed_lead_values = [20., 20., 0.],
speed_lead_breakpoints = [0., 15., 35.0],
- cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)]
+ cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)],
+ checks=[check_engaged, check_no_collision],
),
Maneuver(
'steady state following a car at 20m/s, then lead decel to 0mph at 2m/s^2',
@@ -93,7 +110,8 @@ maneuvers = [
initial_distance_lead=35.,
speed_lead_values = [20., 20., 0.],
speed_lead_breakpoints = [0., 15., 25.0],
- cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)]
+ cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)],
+ checks=[check_engaged, check_no_collision],
),
Maneuver(
'steady state following a car at 20m/s, then lead decel to 0mph at 3m/s^2',
@@ -103,7 +121,8 @@ maneuvers = [
initial_distance_lead=35.,
speed_lead_values = [20., 20., 0.],
speed_lead_breakpoints = [0., 15., 21.66],
- cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)]
+ cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)],
+ checks=[check_engaged, check_fcw],
),
Maneuver(
'steady state following a car at 20m/s, then lead decel to 0mph at 5m/s^2',
@@ -113,7 +132,8 @@ maneuvers = [
initial_distance_lead=35.,
speed_lead_values = [20., 20., 0.],
speed_lead_breakpoints = [0., 15., 19.],
- cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)]
+ cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)],
+ checks=[check_engaged, check_fcw],
),
Maneuver(
'starting at 0mph, approaching a stopped car 100m away',
@@ -122,9 +142,10 @@ maneuvers = [
lead_relevancy=True,
initial_distance_lead=100.,
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
- (CB.RES_ACCEL, 1.4), (0.0, 1.5),
- (CB.RES_ACCEL, 1.6), (0.0, 1.7),
- (CB.RES_ACCEL, 1.8), (0.0, 1.9)]
+ (CB.RES_ACCEL, 1.4), (0.0, 1.5),
+ (CB.RES_ACCEL, 1.6), (0.0, 1.7),
+ (CB.RES_ACCEL, 1.8), (0.0, 1.9)],
+ checks=[check_engaged, check_no_collision],
),
Maneuver(
"following a car at 60mph, lead accel and decel at 0.5m/s^2 every 2s",
@@ -135,8 +156,9 @@ maneuvers = [
speed_lead_values=[30.,30.,29.,31.,29.,31.,29.],
speed_lead_breakpoints=[0., 6., 8., 12.,16.,20.,24.],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
- (CB.RES_ACCEL, 1.4), (0.0, 1.5),
- (CB.RES_ACCEL, 1.6), (0.0, 1.7)]
+ (CB.RES_ACCEL, 1.4), (0.0, 1.5),
+ (CB.RES_ACCEL, 1.6), (0.0, 1.7)],
+ checks=[check_engaged, check_no_collision],
),
Maneuver(
"following a car at 10mph, stop and go at 1m/s2 lead dece1 and accel",
@@ -147,8 +169,9 @@ maneuvers = [
speed_lead_values=[10., 0., 0., 10., 0.,10.],
speed_lead_breakpoints=[10., 20., 30., 40., 50., 60.],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
- (CB.RES_ACCEL, 1.4), (0.0, 1.5),
- (CB.RES_ACCEL, 1.6), (0.0, 1.7)]
+ (CB.RES_ACCEL, 1.4), (0.0, 1.5),
+ (CB.RES_ACCEL, 1.6), (0.0, 1.7)],
+ checks=[check_engaged, check_no_collision],
),
Maneuver(
"green light: stopped behind lead car, lead car accelerates at 1.5 m/s",
@@ -159,11 +182,12 @@ maneuvers = [
speed_lead_values=[0, 0 , 45],
speed_lead_breakpoints=[0, 10., 40.],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
- (CB.RES_ACCEL, 1.4), (0.0, 1.5),
- (CB.RES_ACCEL, 1.6), (0.0, 1.7),
- (CB.RES_ACCEL, 1.8), (0.0, 1.9),
- (CB.RES_ACCEL, 2.0), (0.0, 2.1),
- (CB.RES_ACCEL, 2.2), (0.0, 2.3)]
+ (CB.RES_ACCEL, 1.4), (0.0, 1.5),
+ (CB.RES_ACCEL, 1.6), (0.0, 1.7),
+ (CB.RES_ACCEL, 1.8), (0.0, 1.9),
+ (CB.RES_ACCEL, 2.0), (0.0, 2.1),
+ (CB.RES_ACCEL, 2.2), (0.0, 2.3)],
+ checks=[check_engaged, check_no_collision],
),
Maneuver(
"stop and go with 1m/s2 lead decel and accel, with full stops",
@@ -175,7 +199,8 @@ maneuvers = [
speed_lead_breakpoints=[10., 20., 30., 40., 50., 60.],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
(CB.RES_ACCEL, 1.4), (0.0, 1.5),
- (CB.RES_ACCEL, 1.6), (0.0, 1.7)]
+ (CB.RES_ACCEL, 1.6), (0.0, 1.7)],
+ checks=[check_engaged, check_no_collision],
),
Maneuver(
"stop and go with 1.5m/s2 lead accel and 3.3m/s^2 lead decel, with full stops",
@@ -186,8 +211,9 @@ maneuvers = [
speed_lead_values=[10., 0., 0., 10., 0., 0.] ,
speed_lead_breakpoints=[10., 13., 26., 33., 36., 45.],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
- (CB.RES_ACCEL, 1.4), (0.0, 1.5),
- (CB.RES_ACCEL, 1.6), (0.0, 1.7)]
+ (CB.RES_ACCEL, 1.4), (0.0, 1.5),
+ (CB.RES_ACCEL, 1.6), (0.0, 1.7)],
+ checks=[check_engaged, check_no_collision],
),
Maneuver(
"accelerate from 20 while lead vehicle decelerates from 40 to 20 at 1m/s2",
@@ -198,11 +224,12 @@ maneuvers = [
speed_lead_values=[20., 10.],
speed_lead_breakpoints=[1., 11.],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
- (CB.RES_ACCEL, 1.4), (0.0, 1.5),
- (CB.RES_ACCEL, 1.6), (0.0, 1.7),
- (CB.RES_ACCEL, 1.8), (0.0, 1.9),
- (CB.RES_ACCEL, 2.0), (0.0, 2.1),
- (CB.RES_ACCEL, 2.2), (0.0, 2.3)]
+ (CB.RES_ACCEL, 1.4), (0.0, 1.5),
+ (CB.RES_ACCEL, 1.6), (0.0, 1.7),
+ (CB.RES_ACCEL, 1.8), (0.0, 1.9),
+ (CB.RES_ACCEL, 2.0), (0.0, 2.1),
+ (CB.RES_ACCEL, 2.2), (0.0, 2.3)],
+ checks=[check_engaged, check_no_collision],
),
Maneuver(
"accelerate from 20 while lead vehicle decelerates from 40 to 0 at 2m/s2",
@@ -213,11 +240,12 @@ maneuvers = [
speed_lead_values=[20., 0.],
speed_lead_breakpoints=[1., 11.],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
- (CB.RES_ACCEL, 1.4), (0.0, 1.5),
- (CB.RES_ACCEL, 1.6), (0.0, 1.7),
- (CB.RES_ACCEL, 1.8), (0.0, 1.9),
- (CB.RES_ACCEL, 2.0), (0.0, 2.1),
- (CB.RES_ACCEL, 2.2), (0.0, 2.3)]
+ (CB.RES_ACCEL, 1.4), (0.0, 1.5),
+ (CB.RES_ACCEL, 1.6), (0.0, 1.7),
+ (CB.RES_ACCEL, 1.8), (0.0, 1.9),
+ (CB.RES_ACCEL, 2.0), (0.0, 2.1),
+ (CB.RES_ACCEL, 2.2), (0.0, 2.3)],
+ checks=[check_engaged, check_no_collision],
),
Maneuver(
"fcw: traveling at 30 m/s and approaching lead traveling at 20m/s",
@@ -227,7 +255,8 @@ maneuvers = [
initial_distance_lead=100.,
speed_lead_values=[20.],
speed_lead_breakpoints=[1.],
- cruise_button_presses = []
+ cruise_button_presses = [],
+ checks=[check_fcw],
),
Maneuver(
"fcw: traveling at 20 m/s following a lead that decels from 20m/s to 0 at 1m/s2",
@@ -237,7 +266,8 @@ maneuvers = [
initial_distance_lead=35.,
speed_lead_values=[20., 0.],
speed_lead_breakpoints=[3., 23.],
- cruise_button_presses = []
+ cruise_button_presses = [],
+ checks=[check_fcw],
),
Maneuver(
"fcw: traveling at 20 m/s following a lead that decels from 20m/s to 0 at 3m/s2",
@@ -247,7 +277,8 @@ maneuvers = [
initial_distance_lead=35.,
speed_lead_values=[20., 0.],
speed_lead_breakpoints=[3., 9.6],
- cruise_button_presses = []
+ cruise_button_presses = [],
+ checks=[check_fcw],
),
Maneuver(
"fcw: traveling at 20 m/s following a lead that decels from 20m/s to 0 at 5m/s2",
@@ -257,7 +288,8 @@ maneuvers = [
initial_distance_lead=35.,
speed_lead_values=[20., 0.],
speed_lead_breakpoints=[3., 7.],
- cruise_button_presses = []
+ cruise_button_presses = [],
+ checks=[check_fcw],
)
]
@@ -293,6 +325,7 @@ def setup_output():
class LongitudinalControl(unittest.TestCase):
@classmethod
def setUpClass(cls):
+ os.environ['NO_CAN_TIMEOUT'] = "1"
setup_output()
@@ -314,26 +347,30 @@ class LongitudinalControl(unittest.TestCase):
def test_longitudinal_setup(self):
pass
-WORKERS = 8
def run_maneuver_worker(k):
+ man = maneuvers[k]
output_dir = os.path.join(os.getcwd(), 'out/longitudinal')
- for i, man in enumerate(maneuvers[k::WORKERS]):
+
+ def run(self):
+ print(man.title)
manager.start_managed_process('radard')
manager.start_managed_process('controlsd')
manager.start_managed_process('plannerd')
- score, plot = man.evaluate()
- plot.write_plot(output_dir, "maneuver" + str(WORKERS * i + k+1).zfill(2))
+ plot, valid = man.evaluate()
+ plot.write_plot(output_dir, "maneuver" + str(k+1).zfill(2))
manager.kill_managed_process('radard')
manager.kill_managed_process('controlsd')
manager.kill_managed_process('plannerd')
time.sleep(5)
-for k in xrange(WORKERS):
- setattr(LongitudinalControl,
- "test_longitudinal_maneuvers_%d" % (k+1),
- lambda self, k=k: run_maneuver_worker(k))
+ self.assertTrue(valid)
+
+ return run
+
+for k in range(len(maneuvers)):
+ setattr(LongitudinalControl, "test_longitudinal_maneuvers_%d" % (k+1), run_maneuver_worker(k))
if __name__ == "__main__":
- unittest.main()
+ unittest.main(failfast=True)
diff --git a/selfdrive/test/tests/process_replay/.gitignore b/selfdrive/test/tests/process_replay/.gitignore
new file mode 100644
index 000000000..6d339d54f
--- /dev/null
+++ b/selfdrive/test/tests/process_replay/.gitignore
@@ -0,0 +1,2 @@
+*.bz2
+diff.txt
diff --git a/selfdrive/test/tests/process_replay/README.md b/selfdrive/test/tests/process_replay/README.md
new file mode 100644
index 000000000..639ca9051
--- /dev/null
+++ b/selfdrive/test/tests/process_replay/README.md
@@ -0,0 +1,15 @@
+# process replay
+
+Process replay is a regression test designed to identify any changes in the output of a process. This test replays a segment through individual processes and compares the output to a known good replay. Each make is represented in the test with a segment.
+
+If the test fails, make sure that you didn't unintentionally change anything. If there are intentional changes, the reference logs will be updated.
+
+Use `test_processes.py` to run the test locally.
+
+Currently the following processes are tested:
+
+* controlsd
+* radard
+* plannerd
+* calibrationd
+
diff --git a/selfdrive/test/tests/process_replay/__init__.py b/selfdrive/test/tests/process_replay/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/selfdrive/test/tests/process_replay/compare_logs.py b/selfdrive/test/tests/process_replay/compare_logs.py
new file mode 100755
index 000000000..260e06c77
--- /dev/null
+++ b/selfdrive/test/tests/process_replay/compare_logs.py
@@ -0,0 +1,45 @@
+#!/usr/bin/env python2
+import bz2
+import os
+import sys
+
+import dictdiffer
+if "CI" in os.environ:
+ tqdm = lambda x: x
+else:
+ from tqdm import tqdm
+
+from tools.lib.logreader import LogReader
+
+
+def save_log(dest, log_msgs):
+ dat = ""
+ for msg in log_msgs:
+ dat += msg.as_builder().to_bytes()
+ dat = bz2.compress(dat)
+
+ with open(dest, "w") as f:
+ f.write(dat)
+
+def compare_logs(log1, log2, ignore=[]):
+ assert len(log1) == len(log2), "logs are not same length"
+
+ diff = []
+ for msg1, msg2 in tqdm(zip(log1, log2)):
+ assert msg1.which() == msg2.which(), "msgs not aligned between logs"
+
+ msg1_bytes = msg1.as_builder().to_bytes()
+ msg2_bytes = msg2.as_builder().to_bytes()
+
+ if msg1_bytes != msg2_bytes:
+ msg1_dict = msg1.to_dict(verbose=True)
+ msg2_dict = msg2.to_dict(verbose=True)
+ dd = dictdiffer.diff(msg1_dict, msg2_dict, ignore=ignore, tolerance=0)
+ diff.extend(dd)
+ return diff
+
+if __name__ == "__main__":
+ log1 = list(LogReader(sys.argv[1]))
+ log2 = list(LogReader(sys.argv[2]))
+
+ compare_logs(log1, log2, sys.argv[3:])
diff --git a/selfdrive/test/tests/process_replay/process_replay.py b/selfdrive/test/tests/process_replay/process_replay.py
new file mode 100755
index 000000000..66067b405
--- /dev/null
+++ b/selfdrive/test/tests/process_replay/process_replay.py
@@ -0,0 +1,185 @@
+#!/usr/bin/env python2
+import gc
+import os
+import time
+
+if "CI" in os.environ:
+ tqdm = lambda x: x
+else:
+ from tqdm import tqdm
+
+from cereal import car
+from selfdrive.car.car_helpers import get_car
+import selfdrive.manager as manager
+import selfdrive.messaging as messaging
+from common.params import Params
+from selfdrive.services import service_list
+from collections import namedtuple
+
+ProcessConfig = namedtuple('ProcessConfig', ['proc_name', 'pub_sub', 'ignore', 'init_callback', 'should_recv_callback'])
+
+def fingerprint(msgs, pub_socks, sub_socks):
+ print "start fingerprinting"
+ manager.prepare_managed_process("logmessaged")
+ manager.start_managed_process("logmessaged")
+
+ can = pub_socks["can"]
+ logMessage = messaging.sub_sock(service_list["logMessage"].port)
+
+ time.sleep(1)
+ messaging.drain_sock(logMessage)
+
+ # controlsd waits for a health packet before fingerprinting
+ msg = messaging.new_message()
+ msg.init("health")
+ pub_socks["health"].send(msg.to_bytes())
+
+ canmsgs = filter(lambda msg: msg.which() == "can", msgs)
+ for msg in canmsgs[:200]:
+ can.send(msg.as_builder().to_bytes())
+
+ time.sleep(0.005)
+ log = messaging.recv_one_or_none(logMessage)
+ if log is not None and "fingerprinted" in log.logMessage:
+ break
+ manager.kill_managed_process("logmessaged")
+ print "finished fingerprinting"
+
+def get_car_params(msgs, pub_socks, sub_socks):
+ sendcan = pub_socks.get("sendcan", None)
+ if sendcan is None:
+ sendcan = messaging.pub_sock(service_list["sendcan"].port)
+ logcan = sub_socks.get("can", None)
+ if logcan is None:
+ logcan = messaging.sub_sock(service_list["can"].port)
+ can = pub_socks.get("can", None)
+ if can is None:
+ can = messaging.pub_sock(service_list["can"].port)
+
+ time.sleep(0.5)
+
+ canmsgs = filter(lambda msg: msg.which() == "can", msgs)
+ for m in canmsgs[:200]:
+ can.send(m.as_builder().to_bytes())
+ _, CP = get_car(logcan, sendcan)
+ Params().put("CarParams", CP.to_bytes())
+ time.sleep(0.5)
+ messaging.drain_sock(logcan)
+
+def radar_rcv_callback(msg, CP):
+ if msg.which() != "can":
+ return []
+
+ # hyundai and subaru don't have radar
+ radar_msgs = {"honda": [0x445], "toyota": [0x19f, 0x22f], "gm": [0x475],
+ "hyundai": [], "chrysler": [0x2d4], "subaru": []}.get(CP.carName, None)
+
+ if radar_msgs is None:
+ raise NotImplementedError
+
+ for m in msg.can:
+ if m.src == 1 and m.address in radar_msgs:
+ return ["radarState", "liveTracks"]
+
+ return []
+
+def plannerd_rcv_callback(msg, CP):
+ if msg.which() in ["model", "radarState"]:
+ time.sleep(0.005)
+ else:
+ time.sleep(0.002)
+ return {"model": ["pathPlan"], "radarState": ["plan"]}.get(msg.which(), [])
+
+CONFIGS = [
+ ProcessConfig(
+ proc_name="controlsd",
+ pub_sub={
+ "can": ["controlsState", "carState", "carControl", "sendcan"],
+ "thermal": [], "health": [], "liveCalibration": [], "driverMonitoring": [], "plan": [], "pathPlan": []
+ },
+ ignore=["logMonoTime", "controlsState.startMonoTime", "controlsState.cumLagMs"],
+ init_callback=fingerprint,
+ should_recv_callback=None,
+ ),
+ ProcessConfig(
+ proc_name="radard",
+ pub_sub={
+ "can": ["radarState", "liveTracks"],
+ "liveParameters": [], "controlsState": [], "model": [],
+ },
+ ignore=["logMonoTime", "radarState.cumLagMs"],
+ init_callback=get_car_params,
+ should_recv_callback=radar_rcv_callback,
+ ),
+ ProcessConfig(
+ proc_name="plannerd",
+ pub_sub={
+ "model": ["pathPlan"], "radarState": ["plan"],
+ "carState": [], "controlsState": [], "liveParameters": [],
+ },
+ ignore=["logMonoTime", "valid", "plan.processingDelay"],
+ init_callback=get_car_params,
+ should_recv_callback=plannerd_rcv_callback,
+ ),
+ ProcessConfig(
+ proc_name="calibrationd",
+ pub_sub={
+ "cameraOdometry": ["liveCalibration"]
+ },
+ ignore=["logMonoTime"],
+ init_callback=get_car_params,
+ should_recv_callback=None,
+ ),
+]
+
+def replay_process(cfg, lr):
+ gc.disable() # gc can occasionally cause canparser to timeout
+
+ pub_socks, sub_socks = {}, {}
+ for pub, sub in cfg.pub_sub.iteritems():
+ pub_socks[pub] = messaging.pub_sock(service_list[pub].port)
+
+ for s in sub:
+ sub_socks[s] = messaging.sub_sock(service_list[s].port)
+
+ all_msgs = sorted(lr, key=lambda msg: msg.logMonoTime)
+ pub_msgs = filter(lambda msg: msg.which() in pub_socks.keys(), all_msgs)
+
+ params = Params()
+ params.manager_start()
+ params.put("Passive", "0")
+
+ manager.gctx = {}
+ manager.prepare_managed_process(cfg.proc_name)
+ manager.start_managed_process(cfg.proc_name)
+ time.sleep(3) # Wait for started process to be ready
+
+ if cfg.init_callback is not None:
+ cfg.init_callback(all_msgs, pub_socks, sub_socks)
+
+ CP = car.CarParams.from_bytes(params.get("CarParams", block=True))
+
+ log_msgs = []
+ for msg in tqdm(pub_msgs):
+ if cfg.should_recv_callback is not None:
+ recv_socks = cfg.should_recv_callback(msg, CP)
+ else:
+ recv_socks = cfg.pub_sub[msg.which()]
+
+ pub_socks[msg.which()].send(msg.as_builder().to_bytes())
+
+ if len(recv_socks):
+ # TODO: add timeout
+ for sock in recv_socks:
+ m = messaging.recv_one(sub_socks[sock])
+
+ # make these values fixed for faster comparison
+ m_builder = m.as_builder()
+ m_builder.logMonoTime = 0
+ m_builder.valid = True
+ log_msgs.append(m_builder.as_reader())
+
+ gc.enable()
+ manager.kill_managed_process(cfg.proc_name)
+ return log_msgs
+
diff --git a/selfdrive/test/tests/process_replay/ref_commit b/selfdrive/test/tests/process_replay/ref_commit
new file mode 100644
index 000000000..30a1a2853
--- /dev/null
+++ b/selfdrive/test/tests/process_replay/ref_commit
@@ -0,0 +1 @@
+e3388c62ffb80f4b3ca8721da56a581a93c44e79
\ No newline at end of file
diff --git a/selfdrive/test/tests/process_replay/test_processes.py b/selfdrive/test/tests/process_replay/test_processes.py
new file mode 100755
index 000000000..8d70c80a2
--- /dev/null
+++ b/selfdrive/test/tests/process_replay/test_processes.py
@@ -0,0 +1,118 @@
+#!/usr/bin/env python2
+import os
+import requests
+import sys
+import tempfile
+
+from selfdrive.test.tests.process_replay.compare_logs import compare_logs
+from selfdrive.test.tests.process_replay.process_replay import replay_process, CONFIGS
+from tools.lib.logreader import LogReader
+
+segments = [
+ "0375fdf7b1ce594d|2019-06-13--08-32-25--3", # HONDA.ACCORD
+ "99c94dc769b5d96e|2019-08-03--14-19-59--2", # HONDA.CIVIC
+ "cce908f7eb8db67d|2019-08-02--15-09-51--3", # TOYOTA.COROLLA_TSS2
+ "7ad88f53d406b787|2019-07-09--10-18-56--8", # GM.VOLT
+ "704b2230eb5190d6|2019-07-06--19-29-10--0", # HYUNDAI.KIA_SORENTO
+ "b6e1317e1bfbefa6|2019-07-06--04-05-26--5", # CHRYSLER.JEEP_CHEROKEE
+ "7873afaf022d36e2|2019-07-03--18-46-44--0", # SUBARU.IMPREZA
+]
+
+def get_segment(segment_name):
+ route_name, segment_num = segment_name.rsplit("--", 1)
+ rlog_url = "https://commadataci.blob.core.windows.net/openpilotci/%s/%s/rlog.bz2" \
+ % (route_name.replace("|", "/"), segment_num)
+ r = requests.get(rlog_url)
+ if r.status_code != 200:
+ return None
+
+ with tempfile.NamedTemporaryFile(delete=False, suffix=".bz2") as f:
+ f.write(r.content)
+ return f.name
+
+if __name__ == "__main__":
+
+ process_replay_dir = os.path.dirname(os.path.abspath(__file__))
+ ref_commit_fn = os.path.join(process_replay_dir, "ref_commit")
+
+ if not os.path.isfile(ref_commit_fn):
+ print "couldn't find reference commit"
+ sys.exit(1)
+
+ ref_commit = open(ref_commit_fn).read().strip()
+ print "***** testing against commit %s *****" % ref_commit
+
+ results = {}
+ for segment in segments:
+ print "***** testing route segment %s *****\n" % segment
+
+ results[segment] = {}
+
+ rlog_fn = get_segment(segment)
+
+ if rlog_fn is None:
+ print "failed to get segment %s" % segment
+ sys.exit(1)
+
+ lr = LogReader(rlog_fn)
+
+ for cfg in CONFIGS:
+ log_msgs = replay_process(cfg, lr)
+
+ log_fn = os.path.join(process_replay_dir, "%s_%s_%s.bz2" % (segment, cfg.proc_name, ref_commit))
+
+ if not os.path.isfile(log_fn):
+ url = "https://commadataci.blob.core.windows.net/openpilotci/"
+ req = requests.get(url + os.path.basename(log_fn))
+ if req.status_code != 200:
+ results[segment][cfg.proc_name] = "failed to download comparison log"
+ continue
+
+ with tempfile.NamedTemporaryFile(suffix=".bz2") as f:
+ f.write(req.content)
+ f.flush()
+ f.seek(0)
+ cmp_log_msgs = list(LogReader(f.name))
+ else:
+ cmp_log_msgs = list(LogReader(log_fn))
+
+ diff = compare_logs(cmp_log_msgs, log_msgs, cfg.ignore)
+ results[segment][cfg.proc_name] = diff
+ os.remove(rlog_fn)
+
+ failed = False
+ with open(os.path.join(process_replay_dir, "diff.txt"), "w") as f:
+ f.write("***** tested against commit %s *****\n" % ref_commit)
+
+ for segment, result in results.items():
+ f.write("***** differences for segment %s *****\n" % segment)
+ print "***** results for segment %s *****" % segment
+
+ for proc, diff in result.items():
+ f.write("*** process: %s ***\n" % proc)
+ print "\t%s" % proc
+
+ if isinstance(diff, str):
+ print "\t\t%s" % diff
+ failed = True
+ elif len(diff):
+ cnt = {}
+ for d in diff:
+ f.write("\t%s\n" % str(d))
+
+ k = str(d[1])
+ cnt[k] = 1 if k not in cnt else cnt[k] + 1
+
+ for k, v in sorted(cnt.items()):
+ print "\t\t%s: %s" % (k, v)
+ failed = True
+
+ if failed:
+ print "TEST FAILED"
+ else:
+ print "TEST SUCCEEDED"
+
+ print "\n\nTo update the reference logs for this test run:"
+ print "./update_refs.py"
+
+ sys.exit(int(failed))
diff --git a/selfdrive/test/tests/process_replay/update_refs.py b/selfdrive/test/tests/process_replay/update_refs.py
new file mode 100755
index 000000000..4bc265939
--- /dev/null
+++ b/selfdrive/test/tests/process_replay/update_refs.py
@@ -0,0 +1,42 @@
+#!/usr/bin/env python2
+import os
+import sys
+
+from selfdrive.test.openpilotci_upload import upload_file
+from selfdrive.test.tests.process_replay.compare_logs import save_log
+from selfdrive.test.tests.process_replay.process_replay import replay_process, CONFIGS
+from selfdrive.test.tests.process_replay.test_processes import segments, get_segment
+from selfdrive.version import get_git_commit
+from tools.lib.logreader import LogReader
+
+if __name__ == "__main__":
+
+ no_upload = "--no-upload" in sys.argv
+
+ process_replay_dir = os.path.dirname(os.path.abspath(__file__))
+ ref_commit_fn = os.path.join(process_replay_dir, "ref_commit")
+
+ ref_commit = get_git_commit()
+ with open(ref_commit_fn, "w") as f:
+ f.write(ref_commit)
+
+ for segment in segments:
+ rlog_fn = get_segment(segment)
+
+ if rlog_fn is None:
+ print "failed to get segment %s" % segment
+ sys.exit(1)
+
+ lr = LogReader(rlog_fn)
+
+ for cfg in CONFIGS:
+ log_msgs = replay_process(cfg, lr)
+ log_fn = os.path.join(process_replay_dir, "%s_%s_%s.bz2" % (segment, cfg.proc_name, ref_commit))
+ save_log(log_fn, log_msgs)
+
+ if not no_upload:
+ upload_file(log_fn, os.path.basename(log_fn))
+ os.remove(log_fn)
+ os.remove(rlog_fn)
+
+ print "done"
diff --git a/selfdrive/thermald.py b/selfdrive/thermald.py
index ef75cfffd..b866521a9 100755
--- a/selfdrive/thermald.py
+++ b/selfdrive/thermald.py
@@ -2,13 +2,13 @@
import os
from smbus2 import SMBus
from cereal import log
-from selfdrive.version import training_version
+from selfdrive.version import terms_version, training_version
from selfdrive.swaglog import cloudlog
import selfdrive.messaging as messaging
from selfdrive.services import service_list
from selfdrive.loggerd.config import get_available_percent
from common.params import Params
-from common.realtime import sec_since_boot
+from common.realtime import sec_since_boot, DT_TRML
from common.numpy_fast import clip
from common.filter_simple import FirstOrderFilter
@@ -138,8 +138,8 @@ def thermald_thread():
ignition_seen = False
started_seen = False
thermal_status = ThermalStatus.green
- health_sock.RCVTIMEO = 1500
- current_filter = FirstOrderFilter(0., CURRENT_TAU, 1.)
+ health_sock.RCVTIMEO = int(1000 * 2 * DT_TRML) # 2x the expected health frequency
+ current_filter = FirstOrderFilter(0., CURRENT_TAU, DT_TRML)
health_prev = None
# Make sure charging is enabled
@@ -189,13 +189,13 @@ def thermald_thread():
if max_cpu_temp > 107. or bat_temp >= 63.:
# onroad not allowed
thermal_status = ThermalStatus.danger
- elif max_comp_temp > 95. or bat_temp > 60.:
+ elif max_comp_temp > 92.5 or bat_temp > 60.: # CPU throttling starts around ~90C
# hysteresis between onroad not allowed and engage not allowed
thermal_status = clip(thermal_status, ThermalStatus.red, ThermalStatus.danger)
- elif max_cpu_temp > 90.0:
+ elif max_cpu_temp > 87.5:
# hysteresis between engage not allowed and uploader not allowed
thermal_status = clip(thermal_status, ThermalStatus.yellow, ThermalStatus.red)
- elif max_cpu_temp > 85.0:
+ elif max_cpu_temp > 80.0:
# uploader not allowed
thermal_status = ThermalStatus.yellow
elif max_cpu_temp > 75.0:
@@ -216,7 +216,7 @@ def thermald_thread():
ignition = True
do_uninstall = params.get("DoUninstall") == "1"
- accepted_terms = params.get("HasAcceptedTerms") == "1"
+ accepted_terms = params.get("HasAcceptedTerms") == terms_version
completed_training = params.get("CompletedTrainingVersion") == training_version
should_start = ignition
@@ -266,7 +266,7 @@ def thermald_thread():
print(msg)
# report to server once per minute
- if (count%60) == 0:
+ if (count % int(60. / DT_TRML)) == 0:
cloudlog.event("STATUS_PACKET",
count=count,
health=(health.to_dict() if health else None),
diff --git a/selfdrive/ui/slplay.c b/selfdrive/ui/slplay.c
index 208505766..ddfbad56c 100644
--- a/selfdrive/ui/slplay.c
+++ b/selfdrive/ui/slplay.c
@@ -111,7 +111,7 @@ void slplay_destroy() {
(*engine)->Destroy(engine);
}
-void slplay_stop (uri_player* player, char **error) {
+void slplay_stop(uri_player* player, char **error) {
SLPlayItf playInterface = player->playInterface;
SLresult result;
diff --git a/selfdrive/ui/spinner/spinner b/selfdrive/ui/spinner/spinner
index acc86c78c..65c198aab 100755
Binary files a/selfdrive/ui/spinner/spinner and b/selfdrive/ui/spinner/spinner differ
diff --git a/selfdrive/ui/spinner/spinner.c b/selfdrive/ui/spinner/spinner.c
index ad7425150..3ec36e740 100644
--- a/selfdrive/ui/spinner/spinner.c
+++ b/selfdrive/ui/spinner/spinner.c
@@ -35,7 +35,7 @@ int main(int argc, char** argv) {
NVGcontext *vg = nvgCreateGLES3(NVG_ANTIALIAS | NVG_STENCIL_STROKES);
assert(vg);
- int font = nvgCreateFont(vg, "Bold", "../../assets/OpenSans-SemiBold.ttf");
+ int font = nvgCreateFont(vg, "Bold", "../../assets/fonts/opensans_semibold.ttf");
assert(font >= 0);
int spinner_img = nvgCreateImage(vg, "../../assets/img_spinner_track.png", 0);
diff --git a/selfdrive/ui/ui.c b/selfdrive/ui/ui.c
index 6e24c9220..3eedb809c 100644
--- a/selfdrive/ui/ui.c
+++ b/selfdrive/ui/ui.c
@@ -107,6 +107,8 @@ const mat3 intrinsic_matrix = (mat3){{
0., 0., 1.
}};
+typedef enum cereal_CarControl_HUDControl_AudibleAlert AudibleAlert;
+
typedef struct UIScene {
int frontview;
int fullview;
@@ -126,8 +128,7 @@ typedef struct UIScene {
float v_cruise;
uint64_t v_cruise_update_ts;
float v_ego;
- float v_curvature;
- bool decel_for_turn;
+ bool decel_for_model;
float speedlimit;
bool speedlimit_valid;
@@ -162,8 +163,6 @@ typedef struct UIScene {
// Used to show gps planner status
bool gps_planner_active;
-
- bool is_playing_alert;
} UIScene;
typedef struct {
@@ -254,6 +253,7 @@ typedef struct UIState {
int awake_timeout;
int volume_timeout;
+ int alert_sound_timeout;
int speed_lim_off_timeout;
int is_metric_timeout;
int longitudinal_control_timeout;
@@ -266,7 +266,7 @@ typedef struct UIState {
float speed_lim_off;
bool is_ego_over_limit;
char alert_type[64];
- char alert_sound[64];
+ AudibleAlert alert_sound;
int alert_size;
float alert_blinking_alpha;
bool alert_blinked;
@@ -432,25 +432,25 @@ static const mat4 full_to_wide_frame_transform = {{
}};
typedef struct {
- const char* name;
+ AudibleAlert alert;
const char* uri;
bool loop;
} sound_file;
sound_file sound_table[] = {
- { "chimeDisengage", "../assets/sounds/disengaged.wav", false },
- { "chimeEngage", "../assets/sounds/engaged.wav", false },
- { "chimeWarning1", "../assets/sounds/warning_1.wav", false },
- { "chimeWarning2", "../assets/sounds/warning_2.wav", false },
- { "chimeWarningRepeat", "../assets/sounds/warning_2.wav", true },
- { "chimeError", "../assets/sounds/error.wav", false },
- { "chimePrompt", "../assets/sounds/error.wav", false },
- { NULL, NULL, false },
+ { cereal_CarControl_HUDControl_AudibleAlert_chimeDisengage, "../assets/sounds/disengaged.wav", false },
+ { cereal_CarControl_HUDControl_AudibleAlert_chimeEngage, "../assets/sounds/engaged.wav", false },
+ { cereal_CarControl_HUDControl_AudibleAlert_chimeWarning1, "../assets/sounds/warning_1.wav", false },
+ { cereal_CarControl_HUDControl_AudibleAlert_chimeWarning2, "../assets/sounds/warning_2.wav", false },
+ { cereal_CarControl_HUDControl_AudibleAlert_chimeWarningRepeat, "../assets/sounds/warning_2.wav", true },
+ { cereal_CarControl_HUDControl_AudibleAlert_chimeError, "../assets/sounds/error.wav", false },
+ { cereal_CarControl_HUDControl_AudibleAlert_chimePrompt, "../assets/sounds/error.wav", false },
+ { cereal_CarControl_HUDControl_AudibleAlert_none, NULL, false },
};
-sound_file* get_sound_file_by_name(const char* name) {
- for (sound_file *s = sound_table; s->name != NULL; s++) {
- if (strcmp(s->name, name) == 0) {
+sound_file* get_sound_file(AudibleAlert alert) {
+ for (sound_file *s = sound_table; s->alert != cereal_CarControl_HUDControl_AudibleAlert_none; s++) {
+ if (s->alert == alert) {
return s;
}
}
@@ -463,7 +463,7 @@ void ui_sound_init(char **error) {
slplay_setup(error);
if (*error) return;
- for (sound_file *s = sound_table; s->name != NULL; s++) {
+ for (sound_file *s = sound_table; s->alert != cereal_CarControl_HUDControl_AudibleAlert_none; s++) {
slplay_create_player_for_uri(s->uri, error);
if (*error) return;
}
@@ -503,13 +503,13 @@ static void ui_init(UIState *s) {
s->vg = nvgCreateGLES3(NVG_ANTIALIAS | NVG_STENCIL_STROKES | NVG_DEBUG);
assert(s->vg);
- s->font_courbd = nvgCreateFont(s->vg, "courbd", "../assets/courbd.ttf");
+ s->font_courbd = nvgCreateFont(s->vg, "courbd", "../assets/fonts/courbd.ttf");
assert(s->font_courbd >= 0);
- s->font_sans_regular = nvgCreateFont(s->vg, "sans-regular", "../assets/OpenSans-Regular.ttf");
+ s->font_sans_regular = nvgCreateFont(s->vg, "sans-regular", "../assets/fonts/opensans_regular.ttf");
assert(s->font_sans_regular >= 0);
- s->font_sans_semibold = nvgCreateFont(s->vg, "sans-semibold", "../assets/OpenSans-SemiBold.ttf");
+ s->font_sans_semibold = nvgCreateFont(s->vg, "sans-semibold", "../assets/fonts/opensans_semibold.ttf");
assert(s->font_sans_semibold >= 0);
- s->font_sans_bold = nvgCreateFont(s->vg, "sans-bold", "../assets/OpenSans-Bold.ttf");
+ s->font_sans_bold = nvgCreateFont(s->vg, "sans-bold", "../assets/fonts/opensans_bold.ttf");
assert(s->font_sans_bold >= 0);
assert(s->img_wheel >= 0);
@@ -1153,17 +1153,6 @@ static void ui_draw_vision_maxspeed(UIState *s) {
nvgText(s->vg, viz_maxspeed_x+(viz_maxspeed_xo/2)+(viz_maxspeed_w/2), 242, "N/A", NULL);
}
-#ifdef DEBUG_TURN
- if (s->scene.decel_for_turn && s->scene.engaged){
- int v_curvature = s->scene.v_curvature * 2.2369363 + 0.5;
- snprintf(maxspeed_str, sizeof(maxspeed_str), "%d", v_curvature);
- nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255));
- nvgFontSize(s->vg, 25*2.5);
- nvgText(s->vg, 200 + viz_maxspeed_x+(viz_maxspeed_xo/2)+(viz_maxspeed_w/2), 148, "TURN", NULL);
- nvgFontSize(s->vg, 50*2.5);
- nvgText(s->vg, 200 + viz_maxspeed_x+(viz_maxspeed_xo/2)+(viz_maxspeed_w/2), 242, maxspeed_str, NULL);
- }
-#endif
}
static void ui_draw_vision_speedlimit(UIState *s) {
@@ -1230,8 +1219,8 @@ static void ui_draw_vision_speedlimit(UIState *s) {
if (is_speedlim_valid && s->is_ego_over_limit) {
nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255));
}
- nvgText(s->vg, viz_speedlim_x+viz_speedlim_w/2 + (is_speedlim_valid ? 6 : 0), viz_speedlim_y + (is_speedlim_valid ? 50 : 45), "SPEED", NULL);
- nvgText(s->vg, viz_speedlim_x+viz_speedlim_w/2 + (is_speedlim_valid ? 6 : 0), viz_speedlim_y + (is_speedlim_valid ? 90 : 85), "LIMIT", NULL);
+ nvgText(s->vg, viz_speedlim_x+viz_speedlim_w/2 + (is_speedlim_valid ? 6 : 0), viz_speedlim_y + (is_speedlim_valid ? 50 : 45), "SMART", NULL);
+ nvgText(s->vg, viz_speedlim_x+viz_speedlim_w/2 + (is_speedlim_valid ? 6 : 0), viz_speedlim_y + (is_speedlim_valid ? 90 : 85), "SPEED", NULL);
// Draw Speed Text
nvgFontFace(s->vg, "sans-bold");
@@ -1294,7 +1283,7 @@ static void ui_draw_vision_event(UIState *s) {
const int viz_event_x = ((ui_viz_rx + ui_viz_rw) - (viz_event_w + (bdr_s*2)));
const int viz_event_y = (box_y + (bdr_s*1.5));
const int viz_event_h = (header_h - (bdr_s*1.5));
- if (s->scene.decel_for_turn && s->scene.engaged && s->limit_set_speed) {
+ if (s->scene.decel_for_model && s->scene.engaged) {
// draw winding road sign
const int img_turn_size = 160*1.5;
const int img_turn_x = viz_event_x-(img_turn_size/4);
@@ -1426,7 +1415,7 @@ static void ui_draw_vision_footer(UIState *s) {
ui_draw_vision_face(s);
#ifdef SHOW_SPEEDLIMIT
- ui_draw_vision_map(s);
+ // ui_draw_vision_map(s);
#endif
}
@@ -1643,29 +1632,30 @@ void handle_message(UIState *s, void *which) {
s->scene.frontview = datad.rearViewCam;
- s->scene.v_curvature = datad.vCurvature;
- s->scene.decel_for_turn = datad.decelForTurn;
+ s->scene.decel_for_model = datad.decelForModel;
- if (datad.alertSound.str && datad.alertSound.str[0] != '\0' && strcmp(s->alert_type, datad.alertType.str) != 0) {
+ s->alert_sound_timeout = 1 * UI_FREQ;
+
+ if (datad.alertSound != cereal_CarControl_HUDControl_AudibleAlert_none && datad.alertSound != s->alert_sound) {
char* error = NULL;
- if (s->alert_sound[0] != '\0') {
- sound_file* active_sound = get_sound_file_by_name(s->alert_sound);
+ if (s->alert_sound != cereal_CarControl_HUDControl_AudibleAlert_none) {
+ sound_file* active_sound = get_sound_file(s->alert_sound);
slplay_stop_uri(active_sound->uri, &error);
if (error) {
LOGW("error stopping active sound %s", error);
}
}
- sound_file* sound = get_sound_file_by_name(datad.alertSound.str);
+ sound_file* sound = get_sound_file(datad.alertSound);
slplay_play(sound->uri, sound->loop, &error);
if(error) {
LOGW("error playing sound: %s", error);
}
- snprintf(s->alert_sound, sizeof(s->alert_sound), "%s", datad.alertSound.str);
+ s->alert_sound = datad.alertSound;
snprintf(s->alert_type, sizeof(s->alert_type), "%s", datad.alertType.str);
- } else if ((!datad.alertSound.str || datad.alertSound.str[0] == '\0') && s->alert_sound[0] != '\0') {
- sound_file* sound = get_sound_file_by_name(s->alert_sound);
+ } else if ((!datad.alertSound || datad.alertSound == cereal_CarControl_HUDControl_AudibleAlert_none) && s->alert_sound != cereal_CarControl_HUDControl_AudibleAlert_none) {
+ sound_file* sound = get_sound_file(s->alert_sound);
char* error = NULL;
@@ -1674,7 +1664,7 @@ void handle_message(UIState *s, void *which) {
LOGW("error stopping sound: %s", error);
}
s->alert_type[0] = '\0';
- s->alert_sound[0] = '\0';
+ s->alert_sound = cereal_CarControl_HUDControl_AudibleAlert_none;
}
if (datad.alertText1.str) {
@@ -1811,8 +1801,6 @@ void handle_message(UIState *s, void *which) {
} else if (eventd.which == cereal_Event_liveMapData) {
struct cereal_LiveMapData datad;
cereal_read_LiveMapData(&datad, eventd.liveMapData);
- s->scene.speedlimit = datad.speedLimit;
- s->scene.speedlimit_valid = datad.speedLimitValid;
s->scene.map_valid = datad.mapValid;
}
capn_free(&ctx);
@@ -2243,7 +2231,10 @@ int main(int argc, char* argv[]) {
float smooth_brightness = BRIGHTNESS_B;
- set_volume(s, 13);
+ const int MIN_VOLUME = LEON ? 12 : 8;
+ const int MAX_VOLUME = LEON ? 15 : 13;
+
+ set_volume(s, MIN_VOLUME);
#ifdef DEBUG_FPS
vipc_t1 = millis_since_boot();
double t1 = millis_since_boot();
@@ -2317,10 +2308,24 @@ int main(int argc, char* argv[]) {
if (s->volume_timeout > 0) {
s->volume_timeout--;
} else {
- int volume = min(13, 11 + s->scene.v_ego / 15); // up one notch every 15 m/s, starting at 11
+ int volume = min(MAX_VOLUME, MIN_VOLUME + s->scene.v_ego / 5); // up one notch every 5 m/s
set_volume(s, volume);
}
+ // stop playing alert sounds if no controlsState msg for 1 second
+ if (s->alert_sound_timeout > 0) {
+ s->alert_sound_timeout--;
+ } else if (s->alert_sound != cereal_CarControl_HUDControl_AudibleAlert_none){
+ sound_file* sound = get_sound_file(s->alert_sound);
+ char* error = NULL;
+
+ slplay_stop_uri(sound->uri, &error);
+ if(error) {
+ LOGW("error stopping sound: %s", error);
+ }
+ s->alert_sound = cereal_CarControl_HUDControl_AudibleAlert_none;
+ }
+
read_param_bool_timeout(&s->is_metric, "IsMetric", &s->is_metric_timeout);
read_param_bool_timeout(&s->longitudinal_control, "LongitudinalControl", &s->longitudinal_control_timeout);
read_param_bool_timeout(&s->limit_set_speed, "LimitSetSpeed", &s->limit_set_speed_timeout);
diff --git a/selfdrive/version.py b/selfdrive/version.py
index 4acc18350..2eb39dc97 100644
--- a/selfdrive/version.py
+++ b/selfdrive/version.py
@@ -59,6 +59,7 @@ except subprocess.CalledProcessError:
dirty = True
training_version = "0.1.0"
+terms_version = "2"
if __name__ == "__main__":
print("Dirty: %s" % dirty)
diff --git a/selfdrive/visiond/build_from_src.mk b/selfdrive/visiond/build_from_src.mk
index d9d320cf3..41e844fe1 100644
--- a/selfdrive/visiond/build_from_src.mk
+++ b/selfdrive/visiond/build_from_src.mk
@@ -46,11 +46,10 @@ else
LIBYUV_FLAGS = -I$(PHONELIBS)/libyuv/include
LIBYUV_LIBS = $(PHONELIBS)/libyuv/x64/lib/libyuv.a
- ZMQ_FLAGS = -I$(PHONELIBS)/zmq/aarch64/include
- ZMQ_LIBS = -l:libczmq.a -l:libzmq.a -lsodium
+ ZMQ_FLAGS = -I$(PHONELIBS)/zmq/x64/include
+ ZMQ_LIBS = -L$(PHONELIBS)/zmq/x64/lib/ -l:libczmq.a -l:libzmq.a
OPENCL_LIBS = -lOpenCL
- UUID_LIBS = -luuid
TF_FLAGS = -I$(EXTERNAL)/tensorflow/include
TF_LIBS = -L$(EXTERNAL)/tensorflow/lib -ltensorflow \
diff --git a/selfdrive/visiond/models/commonmodel.c b/selfdrive/visiond/models/commonmodel.c
index c2bb657bb..33aabd1ce 100644
--- a/selfdrive/visiond/models/commonmodel.c
+++ b/selfdrive/visiond/models/commonmodel.c
@@ -97,39 +97,70 @@ static cereal_ModelData_PathData_ptr path_to_cereal(struct capn_segment *cs, con
for (int i=0; i
#ifdef QCOM
- #include
+#include
#else
- #include
+#include
#endif
#include "common/timing.h"
#include "driving.h"
-#ifdef MEDMODEL
- #define MODEL_WIDTH 512
- #define MODEL_HEIGHT 256
- #define MODEL_NAME "driving_model_dlc"
-#else
- #define MODEL_WIDTH 320
- #define MODEL_HEIGHT 160
- #define MODEL_NAME "driving_model_dlc"
-#endif
-
-#define OUTPUT_SIZE (200 + 2*201 + 26)
-#define LEAD_MDN_N 5
+#define MODEL_WIDTH 512
+#define MODEL_HEIGHT 256
+#define MODEL_NAME "driving_model_dlc"
+#define LEAD_MDN_N 5 // probs for 5 groups
+#define MDN_VALS 4 // output xyva for each lead group
+#define SELECTION 3 //output 3 group (lead now, in 2s and 6s)
+#define MDN_GROUP_SIZE 11
+#define SPEED_BUCKETS 100
+#define OUTPUT_SIZE ((MODEL_PATH_DISTANCE*2) + (2*(MODEL_PATH_DISTANCE*2 + 1)) + MDN_GROUP_SIZE*LEAD_MDN_N + SELECTION)
#ifdef TEMPORAL
#define TEMPORAL_SIZE 512
#else
#define TEMPORAL_SIZE 0
#endif
+// #define DUMP_YUV
+
Eigen::Matrix vander;
void model_init(ModelState* s, cl_device_id device_id, cl_context context, int temporal) {
@@ -60,18 +59,27 @@ ModelData model_eval_frame(ModelState* s, cl_command_queue q,
float *left_lane;
float *right_lane;
float *lead;
+ float *speed;
} net_outputs = {NULL};
//for (int i = 0; i < OUTPUT_SIZE + TEMPORAL_SIZE; i++) { printf("%f ", s->output[i]); } printf("\n");
float *net_input_buf = model_input_prepare(&s->in, q, yuv_cl, width, height, transform);
+ #ifdef DUMP_YUV
+ FILE *dump_yuv_file = fopen("/sdcard/dump.yuv", "wb");
+ fwrite(net_input_buf, MODEL_HEIGHT*MODEL_WIDTH*3/2, sizeof(float), dump_yuv_file);
+ fclose(dump_yuv_file);
+ assert(1==2);
+ #endif
+
//printf("readinggggg \n");
//FILE *f = fopen("goof_frame", "r");
//fread(net_input_buf, sizeof(float), MODEL_HEIGHT*MODEL_WIDTH*3/2, f);
//fclose(f);
//sleep(1);
- //printf("done \n");
+ //printf("%i \n",OUTPUT_SIZE);
+ //printf("%i \n",MDN_GROUP_SIZE);
s->m->execute(net_input_buf);
// net outputs
@@ -79,6 +87,7 @@ ModelData model_eval_frame(ModelState* s, cl_command_queue q,
net_outputs.left_lane = &s->output[MODEL_PATH_DISTANCE*2];
net_outputs.right_lane = &s->output[MODEL_PATH_DISTANCE*2 + MODEL_PATH_DISTANCE*2 + 1];
net_outputs.lead = &s->output[MODEL_PATH_DISTANCE*2 + (MODEL_PATH_DISTANCE*2 + 1)*2];
+ //net_outputs.speed = &s->output[OUTPUT_SIZE - SPEED_BUCKETS];
ModelData model = {0};
@@ -91,9 +100,9 @@ ModelData model_eval_frame(ModelState* s, cl_command_queue q,
model.right_lane.stds[i] = softplus(net_outputs.right_lane[MODEL_PATH_DISTANCE + i]);
}
- model.path.std = softplus(net_outputs.path[MODEL_PATH_DISTANCE + MODEL_PATH_DISTANCE/2]);
- model.left_lane.std = softplus(net_outputs.left_lane[MODEL_PATH_DISTANCE + MODEL_PATH_DISTANCE/2]);
- model.right_lane.std = softplus(net_outputs.right_lane[MODEL_PATH_DISTANCE + MODEL_PATH_DISTANCE/2]);
+ model.path.std = softplus(net_outputs.path[MODEL_PATH_DISTANCE + MODEL_PATH_DISTANCE/4]);
+ model.left_lane.std = softplus(net_outputs.left_lane[MODEL_PATH_DISTANCE + MODEL_PATH_DISTANCE/4]);
+ model.right_lane.std = softplus(net_outputs.right_lane[MODEL_PATH_DISTANCE + MODEL_PATH_DISTANCE/4]);
model.path.prob = 1.;
model.left_lane.prob = sigmoid(net_outputs.left_lane[MODEL_PATH_DISTANCE*2]);
@@ -105,17 +114,63 @@ ModelData model_eval_frame(ModelState* s, cl_command_queue q,
const double max_dist = 140.0;
const double max_rel_vel = 10.0;
+ // Every output distribution from the MDN includes the probabilties
+ // of it representing a current lead car, a lead car in 2s
+ // or a lead car in 4s
+
+ // Find the distribution that corresponds to the current lead
int mdn_max_idx = 0;
for (int i=1; i net_outputs.lead[mdn_max_idx*5 + 4]) {
+ if (net_outputs.lead[i*MDN_GROUP_SIZE + 8] > net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 8]) {
mdn_max_idx = i;
}
}
- model.lead.prob = sigmoid(net_outputs.lead[LEAD_MDN_N*5]);
- model.lead.dist = net_outputs.lead[mdn_max_idx*5] * max_dist;
- model.lead.std = softplus(net_outputs.lead[mdn_max_idx*5 + 2]) * max_dist;
- model.lead.rel_v = net_outputs.lead[mdn_max_idx*5 + 1] * max_rel_vel;
- model.lead.rel_v_std = softplus(net_outputs.lead[mdn_max_idx*5 + 3]) * max_rel_vel;
+ model.lead.prob = sigmoid(net_outputs.lead[LEAD_MDN_N*MDN_GROUP_SIZE]);
+ model.lead.dist = net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE] * max_dist;
+ model.lead.std = softplus(net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS]) * max_dist;
+ model.lead.rel_y = net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 1];
+ model.lead.rel_y_std = softplus(net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 1]);
+ model.lead.rel_v = net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 2] * max_rel_vel;
+ model.lead.rel_v_std = softplus(net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 2]) * max_rel_vel;
+ model.lead.rel_a = net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 3];
+ model.lead.rel_a_std = softplus(net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 3]);
+
+ // Find the distribution that corresponds to the lead in 2s
+ mdn_max_idx = 0;
+ for (int i=1; i net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 9]) {
+ mdn_max_idx = i;
+ }
+ }
+ model.lead_future.prob = sigmoid(net_outputs.lead[LEAD_MDN_N*MDN_GROUP_SIZE + 1]);
+ model.lead_future.dist = net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE] * max_dist;
+ model.lead_future.std = softplus(net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS]) * max_dist;
+ model.lead_future.rel_y = net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 1];
+ model.lead_future.rel_y_std = softplus(net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 1]);
+ model.lead_future.rel_v = net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 2] * max_rel_vel;
+ model.lead_future.rel_v_std = softplus(net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 2]) * max_rel_vel;
+ model.lead_future.rel_a = net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 3];
+ model.lead_future.rel_a_std = softplus(net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 3]);
+
+
+ // get speed percentiles numbers represent 5th, 15th, ... 95th percentile
+ for (int i=0; i < SPEED_PERCENTILES; i++) {
+ model.speed[i] = ((float) SPEED_BUCKETS)/2.0;
+ }
+ //float sum = 0;
+ //for (int idx = 0; idx < SPEED_BUCKETS; idx++) {
+ // sum += net_outputs.speed[idx];
+ // int idx_percentile = (sum + .05) * SPEED_PERCENTILES;
+ // if (idx_percentile < SPEED_PERCENTILES ){
+ // model.speed[idx_percentile] = ((float)idx)/2.0;
+ // }
+ //}
+ // make sure no percentiles are skipped
+ //for (int i=SPEED_PERCENTILES-1; i > 0; i--){
+ // if (model.speed[i-1] > model.speed[i]){
+ // model.speed[i-1] = model.speed[i];
+ // }
+ //}
return model;
}
@@ -135,7 +190,14 @@ void poly_fit(float *in_pts, float *in_stds, float *out) {
Eigen::Matrix lhs = vander.array().colwise() / std.array();
Eigen::Matrix rhs = pts.array() / std.array();
+ // Improve numerical stability
+ Eigen::Matrix scale = 1. / (lhs.array()*lhs.array()).sqrt().colwise().sum();
+ lhs = lhs * scale.asDiagonal();
+
// Solve inplace
Eigen::ColPivHouseholderQR > qr(lhs);
p = qr.solve(rhs);
+
+ // Apply scale to output
+ p = p.transpose() * scale.asDiagonal();
}
diff --git a/selfdrive/visiond/models/driving.h b/selfdrive/visiond/models/driving.h
index 2afb20521..966cf6947 100644
--- a/selfdrive/visiond/models/driving.h
+++ b/selfdrive/visiond/models/driving.h
@@ -2,7 +2,6 @@
#define MODEL_H
// gate this here
-#define MEDMODEL
#define TEMPORAL
#include "common/mat.h"
diff --git a/selfdrive/visiond/models/monitoring.cc b/selfdrive/visiond/models/monitoring.cc
index 7b151d0c4..f6e47880f 100644
--- a/selfdrive/visiond/models/monitoring.cc
+++ b/selfdrive/visiond/models/monitoring.cc
@@ -37,9 +37,13 @@ MonitoringResult monitoring_eval_frame(MonitoringState* s, cl_command_queue q,
s->m->execute(net_input_buf);
MonitoringResult ret = {0};
- memcpy(ret.vs, s->output, sizeof(ret.vs));
- ret.std = sqrtf(2.f) / s->output[OUTPUT_SIZE - 1];
-
+ memcpy(&ret.face_orientation, &s->output[0], sizeof ret.face_orientation);
+ memcpy(&ret.face_position, &s->output[3], sizeof ret.face_position);
+ memcpy(&ret.face_prob, &s->output[12], sizeof ret.face_prob);
+ memcpy(&ret.left_eye_prob, &s->output[21], sizeof ret.left_eye_prob);
+ memcpy(&ret.right_eye_prob, &s->output[30], sizeof ret.right_eye_prob);
+ memcpy(&ret.left_blink_prob, &s->output[31], sizeof ret.right_eye_prob);
+ memcpy(&ret.right_blink_prob, &s->output[32], sizeof ret.right_eye_prob);
return ret;
}
diff --git a/selfdrive/visiond/models/monitoring.h b/selfdrive/visiond/models/monitoring.h
index f9f4516bd..2be96825f 100644
--- a/selfdrive/visiond/models/monitoring.h
+++ b/selfdrive/visiond/models/monitoring.h
@@ -8,11 +8,20 @@
extern "C" {
#endif
-#define OUTPUT_SIZE 8
+#define OUTPUT_SIZE_DEPRECATED 8
+#define OUTPUT_SIZE 33
typedef struct MonitoringResult {
- float vs[OUTPUT_SIZE - 1];
- float std;
+ float descriptor_DEPRECATED[OUTPUT_SIZE_DEPRECATED - 1];
+ float std_DEPRECATED;
+
+ float face_orientation[3];
+ float face_position[2];
+ float face_prob;
+ float left_eye_prob;
+ float right_eye_prob;
+ float left_blink_prob;
+ float right_blink_prob;
} MonitoringResult;
typedef struct MonitoringState {
diff --git a/selfdrive/visiond/runners/run.h b/selfdrive/visiond/runners/run.h
index d92519daf..049f06584 100644
--- a/selfdrive/visiond/runners/run.h
+++ b/selfdrive/visiond/runners/run.h
@@ -7,8 +7,9 @@
#ifdef QCOM
#define DefaultRunModel SNPEModel
#else
- #include "tfmodel.h"
- #define DefaultRunModel TFModel
+#define DefaultRunModel SNPEModel
+ /* #include "tfmodel.h" */
+ /* #define DefaultRunModel TFModel */
#endif
#endif
diff --git a/selfdrive/visiond/visiond.cc b/selfdrive/visiond/visiond.cc
index 530fd68f9..a0cce70ab 100644
--- a/selfdrive/visiond/visiond.cc
+++ b/selfdrive/visiond/visiond.cc
@@ -26,6 +26,12 @@
#include
#include
+#ifdef QCOM
+#include
+#else
+#include
+#endif
+
#include "common/version.h"
#include "common/util.h"
#include "common/timing.h"
@@ -45,6 +51,7 @@
#include "cameras/camera_frame_stream.h"
#endif
+
// 3 models
#include "models/driving.h"
#include "models/monitoring.h"
@@ -58,7 +65,7 @@
#define UI_BUF_COUNT 4
-//#define DUMP_RGB
+// #define DUMP_RGB
//#define DEBUG_DRIVER_MONITOR
@@ -716,6 +723,8 @@ void* monitoring_thread(void *arg) {
MonitoringResult res = monitoring_eval_frame(&s->monitoring, q,
s->yuv_front_cl[buf_idx], s->yuv_front_width, s->yuv_front_height);
+ double t2 = millis_since_boot();
+
// send driver monitoring packet
{
capnp::MallocMessageBuilder msg;
@@ -725,17 +734,30 @@ void* monitoring_thread(void *arg) {
auto framed = event.initDriverMonitoring();
framed.setFrameId(frame_data.frame_id);
- kj::ArrayPtr descriptor_vs(&res.vs[0], ARRAYSIZE(res.vs));
- framed.setDescriptor(descriptor_vs);
+ // junk 0s from legacy model
+ //kj::ArrayPtr descriptor_DEPRECATED(&res.descriptor_DEPRECATED[0], ARRAYSIZE(res.descriptor_DEPRECATED));
+ //framed.setDescriptor(descriptor_DEPRECATED);
+ //framed.setStd(res.std_DEPRECATED);
+ // why not use this junk space for reporting inference time instead
+ // framed.setStdDEPRECATED(static_cast(t2-t1));
+
+ kj::ArrayPtr face_orientation(&res.face_orientation[0], ARRAYSIZE(res.face_orientation));
+ kj::ArrayPtr face_position(&res.face_position[0], ARRAYSIZE(res.face_position));
+ framed.setFaceOrientation(face_orientation);
+ framed.setFacePosition(face_position);
+ framed.setFaceProb(res.face_prob);
+ framed.setLeftEyeProb(res.left_eye_prob);
+ framed.setRightEyeProb(res.right_eye_prob);
+ framed.setLeftBlinkProb(res.left_blink_prob);
+ framed.setRightBlinkProb(res.right_blink_prob);
- framed.setStd(res.std);
auto words = capnp::messageToFlatArray(msg);
auto bytes = words.asBytes();
zmq_send(s->monitoring_sock_raw, bytes.begin(), bytes.size(), ZMQ_DONTWAIT);
}
- double t2 = millis_since_boot();
+ t2 = millis_since_boot();
//LOGD("monitoring process: %.2fms, from last %.2fms", t2-t1, t1-last);
last = t1;
@@ -882,6 +904,8 @@ void* processing_thread(void *arg) {
#endif
#ifdef DUMP_RGB
+ s->rgb_width = s->frame_width;
+ s->rgb_height = s->frame_height;
FILE *dump_rgb_file = fopen("/sdcard/dump.rgb", "wb");
#endif
@@ -946,6 +970,8 @@ void* processing_thread(void *arg) {
#ifdef DUMP_RGB
if (cnt % 20 == 0) {
fwrite(bgr_ptr, s->rgb_buf_size, 1, dump_rgb_file);
+ LOG("%d x %d", s->rgb_width, s->rgb_height);
+ assert(1==2);
}
#endif
@@ -1189,6 +1215,24 @@ void* live_thread(void *arg) {
zpoller_t *poller = zpoller_new(liveCalibration_sock, terminate, NULL);
assert(poller);
+ /*
+ import numpy as np
+ from common.transformations.model import medmodel_frame_from_road_frame
+ medmodel_frame_from_ground = medmodel_frame_from_road_frame[:, (0, 1, 3)]
+ ground_from_medmodel_frame = np.linalg.inv(medmodel_frame_from_ground)
+ */
+ Eigen::Matrix ground_from_medmodel_frame;
+ ground_from_medmodel_frame <<
+ 0.00000000e+00, 0.00000000e+00, 1.00000000e+00,
+ -1.09890110e-03, 0.00000000e+00, 2.81318681e-01,
+ -1.84808520e-20, 9.00738606e-04,-4.28751576e-02;
+
+ Eigen::Matrix eon_intrinsics;
+ eon_intrinsics <<
+ 910.0, 0.0, 582.0,
+ 0.0, 910.0, 437.0,
+ 0.0, 0.0, 1.0;
+
while (!do_exit) {
zsock_t *which = (zsock_t*)zpoller_wait(poller, -1);
if (which == terminate || which == NULL) {
@@ -1213,15 +1257,25 @@ void* live_thread(void *arg) {
if (event.isLiveCalibration()) {
pthread_mutex_lock(&s->transform_lock);
-#ifdef MEDMODEL
- auto wm2 = event.getLiveCalibration().getWarpMatrixBig();
-#else
- auto wm2 = event.getLiveCalibration().getWarpMatrix2();
-#endif
- assert(wm2.size() == 3*3);
- for (int i=0; i<3*3; i++) {
- s->cur_transform.v[i] = wm2[i];
+
+ auto extrinsic_matrix = event.getLiveCalibration().getExtrinsicMatrix();
+ Eigen::Matrix extrinsic_matrix_eigen;
+ for (int i = 0; i < 4*3; i++){
+ extrinsic_matrix_eigen(i / 4, i % 4) = extrinsic_matrix[i];
}
+
+ auto camera_frame_from_road_frame = eon_intrinsics * extrinsic_matrix_eigen;
+ Eigen::Matrix camera_frame_from_ground;
+ camera_frame_from_ground.col(0) = camera_frame_from_road_frame.col(0);
+ camera_frame_from_ground.col(1) = camera_frame_from_road_frame.col(1);
+ camera_frame_from_ground.col(2) = camera_frame_from_road_frame.col(3);
+
+ auto warp_matrix = camera_frame_from_ground * ground_from_medmodel_frame;
+
+ for (int i=0; i<3*3; i++) {
+ s->cur_transform.v[i] = warp_matrix(i / 3, i % 3);
+ }
+
s->run_model = true;
pthread_mutex_unlock(&s->transform_lock);
}