Revert "fix"

This reverts commit bdca1881e0.
This commit is contained in:
1okko
2025-04-15 23:32:35 +08:00
parent bdca1881e0
commit cd20b841b9
63 changed files with 508 additions and 2746 deletions

View File

@@ -26,6 +26,33 @@ static void black_enable_can_transceiver(uint8_t transceiver, bool enabled) {
}
}
static void black_enable_can_transceivers(bool enabled) {
for(uint8_t i=1U; i<=4U; i++){
// Leave main CAN always on for CAN-based ignition detection
if((harness.status == HARNESS_STATUS_FLIPPED) ? (i == 3U) : (i == 1U)){
black_enable_can_transceiver(i, true);
} else {
black_enable_can_transceiver(i, enabled);
}
}
}
static void black_set_led(uint8_t color, bool enabled) {
switch (color){
case LED_RED:
set_gpio_output(GPIOC, 9, !enabled);
break;
case LED_GREEN:
set_gpio_output(GPIOC, 7, !enabled);
break;
case LED_BLUE:
set_gpio_output(GPIOC, 6, !enabled);
break;
default:
break;
}
}
static void black_set_usb_load_switch(bool enabled) {
set_gpio_output(GPIOB, 1, !enabled);
}
@@ -75,12 +102,41 @@ static void black_init(void) {
set_gpio_alternate(GPIOA, 8, GPIO_AF11_CAN3);
set_gpio_alternate(GPIOA, 15, GPIO_AF11_CAN3);
// C0: OBD_SBU1 (orientation detection)
// C3: OBD_SBU2 (orientation detection)
set_gpio_mode(GPIOC, 0, MODE_ANALOG);
set_gpio_mode(GPIOC, 3, MODE_ANALOG);
// GPS OFF
set_gpio_output(GPIOC, 5, 0);
set_gpio_output(GPIOC, 12, 0);
// C10: OBD_SBU1_RELAY (harness relay driving output)
// C11: OBD_SBU2_RELAY (harness relay driving output)
set_gpio_mode(GPIOC, 10, MODE_OUTPUT);
set_gpio_mode(GPIOC, 11, MODE_OUTPUT);
set_gpio_output_type(GPIOC, 10, OUTPUT_TYPE_OPEN_DRAIN);
set_gpio_output_type(GPIOC, 11, OUTPUT_TYPE_OPEN_DRAIN);
set_gpio_output(GPIOC, 10, 1);
set_gpio_output(GPIOC, 11, 1);
// Turn on USB load switch.
black_set_usb_load_switch(true);
// Initialize harness
harness_init();
// Enable CAN transceivers
black_enable_can_transceivers(true);
// Disable LEDs
black_set_led(LED_RED, false);
black_set_led(LED_GREEN, false);
black_set_led(LED_BLUE, false);
// Set normal CAN mode
black_set_can_mode(CAN_MODE_NORMAL);
}
static void black_init_bootloader(void) {
@@ -106,6 +162,7 @@ static harness_configuration black_harness_config = {
board board_black = {
.set_bootkick = unused_set_bootkick,
.harness_config = &black_harness_config,
.has_obd = true,
.has_spi = false,
.has_canfd = false,
.fan_max_rpm = 0U,
@@ -116,8 +173,8 @@ board board_black = {
.init = black_init,
.init_bootloader = black_init_bootloader,
.enable_can_transceiver = black_enable_can_transceiver,
.led_GPIO = {GPIOC, GPIOC, GPIOC},
.led_pin = {9, 7, 6},
.enable_can_transceivers = black_enable_can_transceivers,
.set_led = black_set_led,
.set_can_mode = black_set_can_mode,
.check_ignition = black_check_ignition,
.read_voltage_mV = white_read_voltage_mV,

View File

@@ -13,6 +13,8 @@ typedef enum {
typedef void (*board_init)(void);
typedef void (*board_init_bootloader)(void);
typedef void (*board_enable_can_transceiver)(uint8_t transceiver, bool enabled);
typedef void (*board_enable_can_transceivers)(bool enabled);
typedef void (*board_set_led)(uint8_t color, bool enabled);
typedef void (*board_set_can_mode)(uint8_t mode);
typedef bool (*board_check_ignition)(void);
typedef uint32_t (*board_read_voltage_mV)(void);
@@ -26,8 +28,7 @@ typedef void (*board_set_amp_enabled)(bool enabled);
struct board {
harness_configuration *harness_config;
GPIO_TypeDef * const led_GPIO[3];
const uint8_t led_pin[3];
const bool has_obd;
const bool has_spi;
const bool has_canfd;
const uint16_t fan_max_rpm;
@@ -38,6 +39,8 @@ struct board {
board_init init;
board_init_bootloader init_bootloader;
board_enable_can_transceiver enable_can_transceiver;
board_enable_can_transceivers enable_can_transceivers;
board_set_led set_led;
board_set_can_mode set_can_mode;
board_check_ignition check_ignition;
board_read_voltage_mV read_voltage_mV;
@@ -64,6 +67,11 @@ struct board {
#define HW_TYPE_TRES 9U
#define HW_TYPE_CUATRO 10U
// LED colors
#define LED_RED 0U
#define LED_GREEN 1U
#define LED_BLUE 2U
// USB power modes (from cereal.log.health)
#define USB_POWER_NONE 0U
#define USB_POWER_CLIENT 1U

View File

@@ -6,6 +6,22 @@
// Cuatro (STM32H7) + Harness //
// ////////////////////////// //
static void cuatro_set_led(uint8_t color, bool enabled) {
switch (color) {
case LED_RED:
set_gpio_output(GPIOC, 6, !enabled);
break;
case LED_GREEN:
set_gpio_output(GPIOC, 7, !enabled);
break;
case LED_BLUE:
set_gpio_output(GPIOC, 9, !enabled);
break;
default:
break;
}
}
static void cuatro_enable_can_transceiver(uint8_t transceiver, bool enabled) {
switch (transceiver) {
case 1U:
@@ -25,6 +41,18 @@ static void cuatro_enable_can_transceiver(uint8_t transceiver, bool enabled) {
}
}
static void cuatro_enable_can_transceivers(bool enabled) {
uint8_t main_bus = (harness.status == HARNESS_STATUS_FLIPPED) ? 3U : 1U;
for (uint8_t i=1U; i<=4U; i++) {
// Leave main CAN always on for CAN-based ignition detection
if (i == main_bus) {
cuatro_enable_can_transceiver(i, true);
} else {
cuatro_enable_can_transceiver(i, enabled);
}
}
}
static uint32_t cuatro_read_voltage_mV(void) {
return adc_get_mV(8) * 11U;
}
@@ -40,7 +68,7 @@ static void cuatro_set_fan_enabled(bool enabled) {
static void cuatro_set_bootkick(BootState state) {
set_gpio_output(GPIOA, 0, state != BOOT_BOOTKICK);
// TODO: confirm we need this
//set_gpio_output(GPIOC, 12, state != BOOT_RESET);
set_gpio_output(GPIOC, 12, state != BOOT_RESET);
}
static void cuatro_set_amp_enabled(bool enabled){
@@ -48,9 +76,14 @@ static void cuatro_set_amp_enabled(bool enabled){
}
static void cuatro_init(void) {
common_init_gpio();
red_chiplet_init();
// open drain
// init LEDs as open drain
set_gpio_output_type(GPIOC, 6, OUTPUT_TYPE_OPEN_DRAIN);
set_gpio_output_type(GPIOC, 7, OUTPUT_TYPE_OPEN_DRAIN);
set_gpio_output_type(GPIOC, 9, OUTPUT_TYPE_OPEN_DRAIN);
// more open drain
set_gpio_output_type(GPIOD, 3, OUTPUT_TYPE_OPEN_DRAIN); // FAN_EN
set_gpio_output_type(GPIOC, 12, OUTPUT_TYPE_OPEN_DRAIN); // VBAT_EN
@@ -81,6 +114,9 @@ static void cuatro_init(void) {
gpio_uart7_init();
uart_init(&uart_ring_som_debug, 115200);
// SPI init
gpio_spi_init();
// fan setup
set_gpio_alternate(GPIOC, 8, GPIO_AF2_TIM3);
register_set_bits(&(GPIOC->OTYPER), GPIO_OTYPER_OT8); // open drain
@@ -106,7 +142,8 @@ static void cuatro_init(void) {
}
board board_cuatro = {
.harness_config = &tres_harness_config,
.harness_config = &red_chiplet_harness_config,
.has_obd = true,
.has_spi = true,
.has_canfd = true,
.fan_max_rpm = 12500U,
@@ -117,9 +154,9 @@ board board_cuatro = {
.init = cuatro_init,
.init_bootloader = unused_init_bootloader,
.enable_can_transceiver = cuatro_enable_can_transceiver,
.led_GPIO = {GPIOC, GPIOC, GPIOC},
.led_pin = {6, 7, 9},
.set_can_mode = tres_set_can_mode,
.enable_can_transceivers = cuatro_enable_can_transceivers,
.set_led = cuatro_set_led,
.set_can_mode = red_chiplet_set_can_mode,
.check_ignition = red_check_ignition,
.read_voltage_mV = cuatro_read_voltage_mV,
.read_current_mA = cuatro_read_current_mA,

View File

@@ -26,6 +26,33 @@ static void dos_enable_can_transceiver(uint8_t transceiver, bool enabled) {
}
}
static void dos_enable_can_transceivers(bool enabled) {
for(uint8_t i=1U; i<=4U; i++){
// Leave main CAN always on for CAN-based ignition detection
if((harness.status == HARNESS_STATUS_FLIPPED) ? (i == 3U) : (i == 1U)){
dos_enable_can_transceiver(i, true);
} else {
dos_enable_can_transceiver(i, enabled);
}
}
}
static void dos_set_led(uint8_t color, bool enabled) {
switch (color){
case LED_RED:
set_gpio_output(GPIOC, 9, !enabled);
break;
case LED_GREEN:
set_gpio_output(GPIOC, 7, !enabled);
break;
case LED_BLUE:
set_gpio_output(GPIOC, 6, !enabled);
break;
default:
break;
}
}
static void dos_set_bootkick(BootState state) {
set_gpio_output(GPIOC, 4, state != BOOT_BOOTKICK);
}
@@ -90,6 +117,25 @@ static void dos_init(void) {
set_gpio_alternate(GPIOA, 8, GPIO_AF11_CAN3);
set_gpio_alternate(GPIOA, 15, GPIO_AF11_CAN3);
// C0: OBD_SBU1 (orientation detection)
// C3: OBD_SBU2 (orientation detection)
set_gpio_mode(GPIOC, 0, MODE_ANALOG);
set_gpio_mode(GPIOC, 3, MODE_ANALOG);
// C10: OBD_SBU1_RELAY (harness relay driving output)
// C11: OBD_SBU2_RELAY (harness relay driving output)
set_gpio_mode(GPIOC, 10, MODE_OUTPUT);
set_gpio_mode(GPIOC, 11, MODE_OUTPUT);
set_gpio_output_type(GPIOC, 10, OUTPUT_TYPE_OPEN_DRAIN);
set_gpio_output_type(GPIOC, 11, OUTPUT_TYPE_OPEN_DRAIN);
set_gpio_output(GPIOC, 10, 1);
set_gpio_output(GPIOC, 11, 1);
#ifdef ENABLE_SPI
// SPI init
gpio_spi_init();
#endif
// C8: FAN PWM aka TIM3_CH3
set_gpio_alternate(GPIOC, 8, GPIO_AF2_TIM3);
@@ -102,9 +148,24 @@ static void dos_init(void) {
pwm_init(TIM4, 2);
dos_set_ir_power(0U);
// Initialize harness
harness_init();
// Enable CAN transceivers
dos_enable_can_transceivers(true);
// Disable LEDs
dos_set_led(LED_RED, false);
dos_set_led(LED_GREEN, false);
dos_set_led(LED_BLUE, false);
// Bootkick
dos_set_bootkick(true);
// Set normal CAN mode
dos_set_can_mode(CAN_MODE_NORMAL);
// Init clock source (camera strobe) using PWM
clock_source_init();
}
@@ -125,6 +186,7 @@ static harness_configuration dos_harness_config = {
board board_dos = {
.harness_config = &dos_harness_config,
.has_obd = true,
#ifdef ENABLE_SPI
.has_spi = true,
#else
@@ -139,8 +201,8 @@ board board_dos = {
.init = dos_init,
.init_bootloader = unused_init_bootloader,
.enable_can_transceiver = dos_enable_can_transceiver,
.led_GPIO = {GPIOC, GPIOC, GPIOC},
.led_pin = {9, 7, 6},
.enable_can_transceivers = dos_enable_can_transceivers,
.set_led = dos_set_led,
.set_can_mode = dos_set_can_mode,
.check_ignition = dos_check_ignition,
.read_voltage_mV = white_read_voltage_mV,

View File

@@ -11,6 +11,7 @@
board board_grey = {
.set_bootkick = unused_set_bootkick,
.harness_config = &white_harness_config,
.has_obd = false,
.has_spi = false,
.has_canfd = false,
.fan_max_rpm = 0U,
@@ -21,8 +22,8 @@ board board_grey = {
.init = white_grey_init,
.init_bootloader = white_grey_init_bootloader,
.enable_can_transceiver = white_enable_can_transceiver,
.led_GPIO = {GPIOC, GPIOC, GPIOC},
.led_pin = {9, 7, 6},
.enable_can_transceivers = white_enable_can_transceivers,
.set_led = white_set_led,
.set_can_mode = white_set_can_mode,
.check_ignition = white_check_ignition,
.read_voltage_mV = white_read_voltage_mV,

View File

@@ -25,6 +25,34 @@ static void red_enable_can_transceiver(uint8_t transceiver, bool enabled) {
}
}
static void red_enable_can_transceivers(bool enabled) {
uint8_t main_bus = (harness.status == HARNESS_STATUS_FLIPPED) ? 3U : 1U;
for (uint8_t i=1U; i<=4U; i++) {
// Leave main CAN always on for CAN-based ignition detection
if (i == main_bus) {
red_enable_can_transceiver(i, true);
} else {
red_enable_can_transceiver(i, enabled);
}
}
}
static void red_set_led(uint8_t color, bool enabled) {
switch (color) {
case LED_RED:
set_gpio_output(GPIOE, 4, !enabled);
break;
case LED_GREEN:
set_gpio_output(GPIOE, 3, !enabled);
break;
case LED_BLUE:
set_gpio_output(GPIOE, 2, !enabled);
break;
default:
break;
}
}
static void red_set_can_mode(uint8_t mode) {
red_enable_can_transceiver(2U, false);
red_enable_can_transceiver(4U, false);
@@ -79,6 +107,17 @@ static uint32_t red_read_voltage_mV(void){
static void red_init(void) {
common_init_gpio();
//C10,C11 : OBD_SBU1_RELAY, OBD_SBU2_RELAY
set_gpio_output_type(GPIOC, 10, OUTPUT_TYPE_OPEN_DRAIN);
set_gpio_pullup(GPIOC, 10, PULL_NONE);
set_gpio_mode(GPIOC, 10, MODE_OUTPUT);
set_gpio_output(GPIOC, 10, 1);
set_gpio_output_type(GPIOC, 11, OUTPUT_TYPE_OPEN_DRAIN);
set_gpio_pullup(GPIOC, 11, PULL_NONE);
set_gpio_mode(GPIOC, 11, MODE_OUTPUT);
set_gpio_output(GPIOC, 11, 1);
// G11,B3,D7,B4: transceiver enable
set_gpio_pullup(GPIOG, 11, PULL_NONE);
set_gpio_mode(GPIOG, 11, MODE_OUTPUT);
@@ -101,6 +140,21 @@ static void red_init(void) {
set_gpio_pullup(GPIOB, 14, PULL_UP);
set_gpio_mode(GPIOB, 14, MODE_OUTPUT);
set_gpio_output(GPIOB, 14, 1);
// Initialize harness
harness_init();
// Enable CAN transceivers
red_enable_can_transceivers(true);
// Disable LEDs
red_set_led(LED_RED, false);
red_set_led(LED_GREEN, false);
red_set_led(LED_BLUE, false);
// Set normal CAN mode
red_set_can_mode(CAN_MODE_NORMAL);
}
static harness_configuration red_harness_config = {
@@ -120,6 +174,7 @@ static harness_configuration red_harness_config = {
board board_red = {
.set_bootkick = unused_set_bootkick,
.harness_config = &red_harness_config,
.has_obd = true,
.has_spi = false,
.has_canfd = true,
.fan_max_rpm = 0U,
@@ -130,8 +185,8 @@ board board_red = {
.init = red_init,
.init_bootloader = unused_init_bootloader,
.enable_can_transceiver = red_enable_can_transceiver,
.led_GPIO = {GPIOE, GPIOE, GPIOE},
.led_pin = {4, 3, 2},
.enable_can_transceivers = red_enable_can_transceivers,
.set_led = red_set_led,
.set_can_mode = red_set_can_mode,
.check_ignition = red_check_ignition,
.read_voltage_mV = red_read_voltage_mV,

View File

@@ -9,7 +9,7 @@
static bool tres_ir_enabled;
static bool tres_fan_enabled;
static void tres_update_fan_ir_power(void) {
set_gpio_output(GPIOD, 3, tres_ir_enabled || tres_fan_enabled);
red_chiplet_set_fan_or_usb_load_switch(tres_ir_enabled || tres_fan_enabled);
}
static void tres_set_ir_power(uint8_t percentage){
@@ -29,67 +29,6 @@ static void tres_set_fan_enabled(bool enabled) {
tres_update_fan_ir_power();
}
static void tres_enable_can_transceiver(uint8_t transceiver, bool enabled) {
switch (transceiver) {
case 1U:
set_gpio_output(GPIOG, 11, !enabled);
break;
case 2U:
set_gpio_output(GPIOB, 10, !enabled);
break;
case 3U:
set_gpio_output(GPIOD, 7, !enabled);
break;
case 4U:
set_gpio_output(GPIOB, 11, !enabled);
break;
default:
break;
}
}
static void tres_set_can_mode(uint8_t mode) {
current_board->enable_can_transceiver(2U, false);
current_board->enable_can_transceiver(4U, false);
switch (mode) {
case CAN_MODE_NORMAL:
case CAN_MODE_OBD_CAN2:
if ((bool)(mode == CAN_MODE_NORMAL) != (bool)(harness.status == HARNESS_STATUS_FLIPPED)) {
// B12,B13: disable normal mode
set_gpio_pullup(GPIOB, 12, PULL_NONE);
set_gpio_mode(GPIOB, 12, MODE_ANALOG);
set_gpio_pullup(GPIOB, 13, PULL_NONE);
set_gpio_mode(GPIOB, 13, MODE_ANALOG);
// B5,B6: FDCAN2 mode
set_gpio_pullup(GPIOB, 5, PULL_NONE);
set_gpio_alternate(GPIOB, 5, GPIO_AF9_FDCAN2);
set_gpio_pullup(GPIOB, 6, PULL_NONE);
set_gpio_alternate(GPIOB, 6, GPIO_AF9_FDCAN2);
current_board->enable_can_transceiver(2U, true);
} else {
// B5,B6: disable normal mode
set_gpio_pullup(GPIOB, 5, PULL_NONE);
set_gpio_mode(GPIOB, 5, MODE_ANALOG);
set_gpio_pullup(GPIOB, 6, PULL_NONE);
set_gpio_mode(GPIOB, 6, MODE_ANALOG);
// B12,B13: FDCAN2 mode
set_gpio_pullup(GPIOB, 12, PULL_NONE);
set_gpio_alternate(GPIOB, 12, GPIO_AF9_FDCAN2);
set_gpio_pullup(GPIOB, 13, PULL_NONE);
set_gpio_alternate(GPIOB, 13, GPIO_AF9_FDCAN2);
current_board->enable_can_transceiver(4U, true);
}
break;
default:
break;
}
}
static bool tres_read_som_gpio (void) {
return (get_gpio_input(GPIOC, 2) != 0);
}
@@ -100,7 +39,7 @@ static void tres_init(void) {
register_set_bits(&(PWR->CR3), PWR_CR3_USB33DEN);
while ((PWR->CR3 & PWR_CR3_USB33RDY) == 0U);
common_init_gpio();
red_chiplet_init();
// C2: SOM GPIO used as input (fan control at boot)
set_gpio_mode(GPIOC, 2, MODE_INPUT);
@@ -115,6 +54,9 @@ static void tres_init(void) {
gpio_uart7_init();
uart_init(&uart_ring_som_debug, 115200);
// SPI init
gpio_spi_init();
// fan setup
set_gpio_alternate(GPIOC, 8, GPIO_AF2_TIM3);
@@ -132,22 +74,9 @@ static void tres_init(void) {
clock_source_init();
}
static harness_configuration tres_harness_config = {
.has_harness = true,
.GPIO_SBU1 = GPIOC,
.GPIO_SBU2 = GPIOA,
.GPIO_relay_SBU1 = GPIOA,
.GPIO_relay_SBU2 = GPIOA,
.pin_SBU1 = 4,
.pin_SBU2 = 1,
.pin_relay_SBU1 = 8,
.pin_relay_SBU2 = 3,
.adc_channel_SBU1 = 4, // ADC12_INP4
.adc_channel_SBU2 = 17 // ADC1_INP17
};
board board_tres = {
.harness_config = &tres_harness_config,
.harness_config = &red_chiplet_harness_config,
.has_obd = true,
.has_spi = true,
.has_canfd = true,
.fan_max_rpm = 6600U,
@@ -157,10 +86,10 @@ board board_tres = {
.fan_enable_cooldown_time = 3U,
.init = tres_init,
.init_bootloader = unused_init_bootloader,
.enable_can_transceiver = tres_enable_can_transceiver,
.led_GPIO = {GPIOE, GPIOE, GPIOE},
.led_pin = {4, 3, 2},
.set_can_mode = tres_set_can_mode,
.enable_can_transceiver = red_chiplet_enable_can_transceiver,
.enable_can_transceivers = red_chiplet_enable_can_transceivers,
.set_led = red_set_led,
.set_can_mode = red_chiplet_set_can_mode,
.check_ignition = red_check_ignition,
.read_voltage_mV = red_read_voltage_mV,
.read_current_mA = unused_read_current,

View File

@@ -26,6 +26,33 @@ static void uno_enable_can_transceiver(uint8_t transceiver, bool enabled) {
}
}
static void uno_enable_can_transceivers(bool enabled) {
for(uint8_t i=1U; i<=4U; i++){
// Leave main CAN always on for CAN-based ignition detection
if((harness.status == HARNESS_STATUS_FLIPPED) ? (i == 3U) : (i == 1U)){
uno_enable_can_transceiver(i, true);
} else {
uno_enable_can_transceiver(i, enabled);
}
}
}
static void uno_set_led(uint8_t color, bool enabled) {
switch (color){
case LED_RED:
set_gpio_output(GPIOC, 9, !enabled);
break;
case LED_GREEN:
set_gpio_output(GPIOC, 7, !enabled);
break;
case LED_BLUE:
set_gpio_output(GPIOC, 6, !enabled);
break;
default:
break;
}
}
static void uno_set_bootkick(BootState state) {
if (state == BOOT_BOOTKICK) {
set_gpio_output(GPIOB, 14, false);
@@ -91,11 +118,25 @@ static void uno_init(void) {
set_gpio_alternate(GPIOA, 8, GPIO_AF11_CAN3);
set_gpio_alternate(GPIOA, 15, GPIO_AF11_CAN3);
// C0: OBD_SBU1 (orientation detection)
// C3: OBD_SBU2 (orientation detection)
set_gpio_mode(GPIOC, 0, MODE_ANALOG);
set_gpio_mode(GPIOC, 3, MODE_ANALOG);
// GPS off
set_gpio_output(GPIOB, 1, 0);
set_gpio_output(GPIOC, 5, 0);
set_gpio_output(GPIOC, 12, 0);
// C10: OBD_SBU1_RELAY (harness relay driving output)
// C11: OBD_SBU2_RELAY (harness relay driving output)
set_gpio_mode(GPIOC, 10, MODE_OUTPUT);
set_gpio_mode(GPIOC, 11, MODE_OUTPUT);
set_gpio_output_type(GPIOC, 10, OUTPUT_TYPE_OPEN_DRAIN);
set_gpio_output_type(GPIOC, 11, OUTPUT_TYPE_OPEN_DRAIN);
set_gpio_output(GPIOC, 10, 1);
set_gpio_output(GPIOC, 11, 1);
// C8: FAN PWM aka TIM3_CH3
set_gpio_alternate(GPIOC, 8, GPIO_AF2_TIM3);
@@ -107,6 +148,21 @@ static void uno_init(void) {
pwm_init(TIM4, 2);
uno_set_ir_power(0U);
// Initialize harness
harness_init();
// Enable CAN transceivers
uno_enable_can_transceivers(true);
// Disable LEDs
uno_set_led(LED_RED, false);
uno_set_led(LED_GREEN, false);
uno_set_led(LED_BLUE, false);
// Set normal CAN mode
uno_set_can_mode(CAN_MODE_NORMAL);
// Switch to phone usb mode if harness connection is powered by less than 7V
if(white_read_voltage_mV() < 7000U){
uno_set_usb_switch(true);
@@ -141,6 +197,7 @@ static harness_configuration uno_harness_config = {
board board_uno = {
.harness_config = &uno_harness_config,
.has_obd = true,
.has_spi = false,
.has_canfd = false,
.fan_max_rpm = 5100U,
@@ -151,8 +208,8 @@ board board_uno = {
.init = uno_init,
.init_bootloader = uno_init_bootloader,
.enable_can_transceiver = uno_enable_can_transceiver,
.led_GPIO = {GPIOC, GPIOC, GPIOC},
.led_pin = {9, 7, 6},
.enable_can_transceivers = uno_enable_can_transceivers,
.set_led = uno_set_led,
.set_can_mode = uno_set_can_mode,
.check_ignition = uno_check_ignition,
.read_voltage_mV = white_read_voltage_mV,

View File

@@ -17,6 +17,30 @@ static void white_enable_can_transceiver(uint8_t transceiver, bool enabled) {
case 3U:
set_gpio_output(GPIOA, 0, !enabled);
break;
default:
print("Invalid CAN transceiver ("); puth(transceiver); print("): enabling failed\n");
break;
}
}
static void white_enable_can_transceivers(bool enabled) {
uint8_t t1 = enabled ? 1U : 2U; // leave transceiver 1 enabled to detect CAN ignition
for(uint8_t i=t1; i<=3U; i++) {
white_enable_can_transceiver(i, enabled);
}
}
static void white_set_led(uint8_t color, bool enabled) {
switch (color){
case LED_RED:
set_gpio_output(GPIOC, 9, !enabled);
break;
case LED_GREEN:
set_gpio_output(GPIOC, 7, !enabled);
break;
case LED_BLUE:
set_gpio_output(GPIOC, 6, !enabled);
break;
default:
break;
}
@@ -128,6 +152,18 @@ static void white_grey_init(void) {
set_gpio_alternate(GPIOC, 11, GPIO_AF7_USART3);
set_gpio_pullup(GPIOC, 11, PULL_UP);
// Enable CAN transceivers
white_enable_can_transceivers(true);
// Disable LEDs
white_set_led(LED_RED, false);
white_set_led(LED_GREEN, false);
white_set_led(LED_BLUE, false);
// Set normal CAN mode
white_set_can_mode(CAN_MODE_NORMAL);
// Init usb power mode
// Init in CDP mode only if panda is powered by 12V.
// Otherwise a PC would not be able to flash a standalone panda
@@ -155,6 +191,7 @@ static harness_configuration white_harness_config = {
board board_white = {
.set_bootkick = unused_set_bootkick,
.harness_config = &white_harness_config,
.has_obd = false,
.has_spi = false,
.has_canfd = false,
.fan_max_rpm = 0U,
@@ -165,8 +202,8 @@ board board_white = {
.init = white_grey_init,
.init_bootloader = white_grey_init_bootloader,
.enable_can_transceiver = white_enable_can_transceiver,
.led_GPIO = {GPIOC, GPIOC, GPIOC},
.led_pin = {9, 7, 6},
.enable_can_transceivers = white_enable_can_transceivers,
.set_led = white_set_led,
.set_can_mode = white_set_can_mode,
.check_ignition = white_check_ignition,
.read_voltage_mV = white_read_voltage_mV,

View File

@@ -6,7 +6,6 @@
// ********************* Includes *********************
#include "config.h"
#include "drivers/led.h"
#include "drivers/pwm.h"
#include "drivers/usb.h"

View File

@@ -178,7 +178,7 @@ void can_rx(uint8_t can_number) {
safety_rx_invalid += safety_rx_hook(&to_push) ? 0U : 1U;
ignition_can_hook(&to_push);
led_set(LED_BLUE, true);
current_board->set_led(LED_BLUE, true);
rx_buffer_overflow += can_push(&can_rx_q, &to_push) ? 0U : 1U;
// next

View File

@@ -209,12 +209,6 @@ void ignition_can_hook(CANPacket_t *to_push) {
ignition_can_cnt = 0U;
}
// Volkswagen MEB exception
if ((addr == 0x3C0) && (len == 4)) {
ignition_can = GET_BIT(to_push, 17U);
ignition_can_cnt = 0U;
}
}
}

View File

@@ -223,7 +223,7 @@ void can_rx(uint8_t can_number) {
safety_rx_invalid += safety_rx_hook(&to_push) ? 0U : 1U;
ignition_can_hook(&to_push);
led_set(LED_BLUE, true);
current_board->set_led(LED_BLUE, true);
rx_buffer_overflow += can_push(&can_rx_q, &to_push) ? 0U : 1U;
// Enable CAN FD and BRS if CAN FD message was received

View File

@@ -94,12 +94,6 @@ void harness_tick(void) {
}
void harness_init(void) {
// init OBD_SBUx_RELAY
set_gpio_output_type(current_board->harness_config->GPIO_relay_SBU1, current_board->harness_config->pin_relay_SBU1, OUTPUT_TYPE_OPEN_DRAIN);
set_gpio_output_type(current_board->harness_config->GPIO_relay_SBU2, current_board->harness_config->pin_relay_SBU2, OUTPUT_TYPE_OPEN_DRAIN);
set_gpio_output(current_board->harness_config->GPIO_relay_SBU1, current_board->harness_config->pin_relay_SBU1, 1);
set_gpio_output(current_board->harness_config->GPIO_relay_SBU2, current_board->harness_config->pin_relay_SBU2, 1);
// try to detect orientation
harness.status = harness_detect_orientation();
if (harness.status != HARNESS_STATUS_NC) {

View File

@@ -56,11 +56,10 @@ void early_initialization(void) {
detect_board_type();
if (enter_bootloader_mode == ENTER_BOOTLOADER_MAGIC) {
led_init();
#ifdef PANDA
current_board->init_bootloader();
#endif
led_set(LED_GREEN, 1);
current_board->set_led(LED_GREEN, 1);
jump_to_bootloader();
}
}

View File

@@ -31,5 +31,3 @@ uint32_t microsecond_timer_get(void);
uint32_t microsecond_timer_get(void) {
return MICROSECOND_TIMER->CNT;
}
typedef uint32_t GPIO_TypeDef;

View File

@@ -35,7 +35,7 @@ int comms_control_handler(ControlPacket_t *req, uint8_t *resp) {
flash_unlock();
resp[1] = 0xff;
}
led_set(LED_GREEN, 1);
current_board->set_led(LED_GREEN, 1);
unlocked = true;
prog_ptr = (uint32_t *)APP_START_ADDRESS;
break;
@@ -112,14 +112,14 @@ int comms_can_read(uint8_t *data, uint32_t max_len) {
void refresh_can_tx_slots_available(void) {}
void comms_endpoint2_write(const uint8_t *data, uint32_t len) {
led_set(LED_RED, 0);
current_board->set_led(LED_RED, 0);
for (uint32_t i = 0; i < len/4; i++) {
flash_write_word(prog_ptr, *(uint32_t*)(data+(i*4)));
//*(uint64_t*)(&spi_tx_buf[0x30+(i*4)]) = *prog_ptr;
prog_ptr++;
}
led_set(LED_RED, 1);
current_board->set_led(LED_RED, 1);
}
@@ -132,7 +132,6 @@ void soft_flasher_start(void) {
gpio_usart2_init();
gpio_usb_init();
led_init();
// enable USB
usb_init();
@@ -144,15 +143,15 @@ void soft_flasher_start(void) {
}
// green LED on for flashing
led_set(LED_GREEN, 1);
current_board->set_led(LED_GREEN, 1);
enable_interrupts();
for (;;) {
// blink the green LED fast
led_set(LED_GREEN, 0);
current_board->set_led(LED_GREEN, 0);
delay(500000);
led_set(LED_GREEN, 1);
current_board->set_led(LED_GREEN, 1);
delay(500000);
}
}

View File

@@ -1,5 +1,6 @@
// ******************** Prototypes ********************
typedef void (*board_init)(void);
typedef void (*board_set_led)(uint8_t color, bool enabled);
typedef void (*board_board_tick)(void);
typedef bool (*board_get_button)(void);
typedef void (*board_set_panda_power)(bool enabled);
@@ -14,12 +15,11 @@ typedef float (*board_get_channel_power)(uint8_t channel);
typedef uint16_t (*board_get_sbu_mV)(uint8_t channel, uint8_t sbu);
struct board {
GPIO_TypeDef * const led_GPIO[3];
const uint8_t led_pin[3];
const bool has_canfd;
const bool has_sbu_sense;
const uint16_t avdd_mV;
board_init init;
board_set_led set_led;
board_board_tick board_tick;
board_get_button get_button;
board_set_panda_power set_panda_power;
@@ -42,6 +42,11 @@ struct board {
#define HW_TYPE_V1 1U
#define HW_TYPE_V2 2U
// LED colors
#define LED_RED 0U
#define LED_GREEN 1U
#define LED_BLUE 2U
// CAN modes
#define CAN_MODE_NORMAL 0U
#define CAN_MODE_OBD_CAN2 3U

View File

@@ -2,6 +2,22 @@
// Jungle board v1 (STM32F4) //
// ///////////////////////// //
void board_v1_set_led(uint8_t color, bool enabled) {
switch (color) {
case LED_RED:
set_gpio_output(GPIOC, 9, !enabled);
break;
case LED_GREEN:
set_gpio_output(GPIOC, 7, !enabled);
break;
case LED_BLUE:
set_gpio_output(GPIOC, 6, !enabled);
break;
default:
break;
}
}
void board_v1_enable_can_transceiver(uint8_t transceiver, bool enabled) {
switch (transceiver) {
case 1U:
@@ -124,6 +140,11 @@ void board_v1_init(void) {
board_v1_enable_can_transceiver(i, true);
}
// Disable LEDs
board_v1_set_led(LED_RED, false);
board_v1_set_led(LED_GREEN, false);
board_v1_set_led(LED_BLUE, false);
// Set normal CAN mode
board_v1_set_can_mode(CAN_MODE_NORMAL);
@@ -141,8 +162,7 @@ board board_v1 = {
.has_sbu_sense = false,
.avdd_mV = 3300U,
.init = &board_v1_init,
.led_GPIO = {GPIOC, GPIOC, GPIOC},
.led_pin = {9, 7, 6},
.set_led = &board_v1_set_led,
.board_tick = &board_v1_tick,
.get_button = &board_v1_get_button,
.set_panda_power = &board_v1_set_panda_power,

View File

@@ -65,6 +65,22 @@ adc_channel_t sbu2_channels[] = {
{.adc = ADC3, .channel = 11},
};
void board_v2_set_led(uint8_t color, bool enabled) {
switch (color) {
case LED_RED:
set_gpio_output(GPIOE, 4, !enabled);
break;
case LED_GREEN:
set_gpio_output(GPIOE, 3, !enabled);
break;
case LED_BLUE:
set_gpio_output(GPIOE, 2, !enabled);
break;
default:
break;
}
}
void board_v2_set_harness_orientation(uint8_t orientation) {
switch (orientation) {
case HARNESS_ORIENTATION_NONE:
@@ -236,6 +252,11 @@ uint16_t board_v2_get_sbu_mV(uint8_t channel, uint8_t sbu) {
void board_v2_init(void) {
common_init_gpio();
// Disable LEDs
board_v2_set_led(LED_RED, false);
board_v2_set_led(LED_GREEN, false);
board_v2_set_led(LED_BLUE, false);
// Normal CAN mode
board_v2_set_can_mode(CAN_MODE_NORMAL);
@@ -291,8 +312,7 @@ board board_v2 = {
.has_sbu_sense = true,
.avdd_mV = 3300U,
.init = &board_v2_init,
.led_GPIO = {GPIOE, GPIOE, GPIOE},
.led_pin = {4, 3, 2},
.set_led = &board_v2_set_led,
.board_tick = &board_v2_tick,
.get_button = &board_v2_get_button,
.set_panda_power = &board_v2_set_panda_power,

View File

@@ -3,7 +3,6 @@
#include "safety.h"
#include "board/drivers/led.h"
#include "board/drivers/pwm.h"
#include "board/drivers/usb.h"
@@ -77,7 +76,7 @@ void tick_handler(void) {
check_registers();
// turn off the blue LED, turned on by CAN
led_set(LED_BLUE, false);
current_board->set_led(LED_BLUE, false);
// Blink and OBD CAN
#ifdef FINAL_PROVISIONING
@@ -88,7 +87,7 @@ void tick_handler(void) {
uptime_cnt += 1U;
}
led_set(LED_GREEN, green_led_enabled);
current_board->set_led(LED_GREEN, green_led_enabled);
// Check on button
bool current_button_status = current_board->get_button();
@@ -147,8 +146,8 @@ int main(void) {
peripherals_init();
detect_board_type();
// red+green leds enabled until succesful USB init, as a debug indicator
led_set(LED_RED, true);
led_set(LED_GREEN, true);
current_board->set_led(LED_RED, true);
current_board->set_led(LED_GREEN, true);
// print hello
print("\n\n\n************************ MAIN START ************************\n");
@@ -177,8 +176,8 @@ int main(void) {
// enable USB (right before interrupts or enum can fail!)
usb_init();
led_set(LED_RED, false);
led_set(LED_GREEN, false);
current_board->set_led(LED_RED, false);
current_board->set_led(LED_GREEN, false);
print("**** INTERRUPTS ON ****\n");
enable_interrupts();
@@ -224,16 +223,16 @@ int main(void) {
// useful for debugging, fade breaks = panda is overloaded
for (uint32_t fade = 0U; fade < MAX_LED_FADE; fade += 1U) {
led_set(LED_RED, true);
current_board->set_led(LED_RED, true);
delay(fade >> 4);
led_set(LED_RED, false);
current_board->set_led(LED_RED, false);
delay((MAX_LED_FADE - fade) >> 4);
}
for (uint32_t fade = MAX_LED_FADE; fade > 0U; fade -= 1U) {
led_set(LED_RED, true);
current_board->set_led(LED_RED, true);
delay(fade >> 4);
led_set(LED_RED, false);
current_board->set_led(LED_RED, false);
delay((MAX_LED_FADE - fade) >> 4);
}
}

View File

@@ -1,7 +1,6 @@
// ********************* Includes *********************
#include "config.h"
#include "drivers/led.h"
#include "drivers/pwm.h"
#include "drivers/usb.h"
#include "drivers/simple_watchdog.h"
@@ -77,14 +76,14 @@ void set_safety_mode(uint16_t mode, uint16_t param) {
switch (mode_copy) {
case SAFETY_SILENT:
set_intercept_relay(false, false);
if (current_board->harness_config->has_harness) {
if (current_board->has_obd) {
current_board->set_can_mode(CAN_MODE_NORMAL);
}
can_silent = ALL_CAN_SILENT;
break;
case SAFETY_NOOUTPUT:
set_intercept_relay(false, false);
if (current_board->harness_config->has_harness) {
if (current_board->has_obd) {
current_board->set_can_mode(CAN_MODE_NORMAL);
}
can_silent = ALL_CAN_LIVE;
@@ -93,7 +92,7 @@ void set_safety_mode(uint16_t mode, uint16_t param) {
set_intercept_relay(false, false);
heartbeat_counter = 0U;
heartbeat_lost = false;
if (current_board->harness_config->has_harness) {
if (current_board->has_obd) {
// Clear any pending messages in the can core (i.e. sending while comma power is unplugged)
// TODO: rewrite using hardware queues rather than fifo to cancel specific messages
can_clear_send(CANIF_FROM_CAN_NUM(1), 1);
@@ -109,7 +108,7 @@ void set_safety_mode(uint16_t mode, uint16_t param) {
set_intercept_relay(true, false);
heartbeat_counter = 0U;
heartbeat_lost = false;
if (current_board->harness_config->has_harness) {
if (current_board->has_obd) {
current_board->set_can_mode(CAN_MODE_NORMAL);
}
can_silent = ALL_CAN_LIVE;
@@ -190,11 +189,11 @@ static void tick_handler(void) {
#endif
// set green LED to be controls allowed
led_set(LED_GREEN, controls_allowed | green_led_enabled);
current_board->set_led(LED_GREEN, controls_allowed | green_led_enabled);
// turn off the blue LED, turned on by CAN
// unless we are in power saving mode
led_set(LED_BLUE, (uptime_cnt & 1U) && (power_save_status == POWER_SAVE_STATUS_ENABLED));
current_board->set_led(LED_BLUE, (uptime_cnt & 1U) && (power_save_status == POWER_SAVE_STATUS_ENABLED));
const bool recent_heartbeat = heartbeat_counter == 0U;
@@ -304,10 +303,9 @@ int main(void) {
clock_init();
peripherals_init();
detect_board_type();
led_init();
// red+green leds enabled until succesful USB/SPI init, as a debug indicator
led_set(LED_RED, true);
led_set(LED_GREEN, true);
current_board->set_led(LED_RED, true);
current_board->set_led(LED_GREEN, true);
adc_init();
// print hello
@@ -321,10 +319,6 @@ int main(void) {
// init board
current_board->init();
current_board->set_can_mode(CAN_MODE_NORMAL);
if (current_board->harness_config->has_harness) {
harness_init();
}
// panda has an FPU, let's use it!
enable_fpu();
@@ -340,7 +334,7 @@ int main(void) {
set_safety_mode(SAFETY_SILENT, 0U);
// enable CAN TXs
enable_can_transceivers(true);
current_board->enable_can_transceivers(true);
// init watchdog for heartbeat loop, fed at 8Hz
simple_watchdog_init(FAULT_HEARTBEAT_LOOP_WATCHDOG, (3U * 1000000U / 8U));
@@ -357,14 +351,12 @@ int main(void) {
#ifdef ENABLE_SPI
if (current_board->has_spi) {
gpio_spi_init();
spi_init();
}
#endif
led_set(LED_RED, false);
led_set(LED_GREEN, false);
led_set(LED_BLUE, false);
current_board->set_led(LED_RED, false);
current_board->set_led(LED_GREEN, false);
print("**** INTERRUPTS ON ****\n");
enable_interrupts();
@@ -377,24 +369,24 @@ int main(void) {
#endif
// useful for debugging, fade breaks = panda is overloaded
for (uint32_t fade = 0U; fade < MAX_LED_FADE; fade += 1U) {
led_set(LED_RED, true);
current_board->set_led(LED_RED, true);
delay(fade >> 4);
led_set(LED_RED, false);
current_board->set_led(LED_RED, false);
delay((MAX_LED_FADE - fade) >> 4);
}
for (uint32_t fade = MAX_LED_FADE; fade > 0U; fade -= 1U) {
led_set(LED_RED, true);
current_board->set_led(LED_RED, true);
delay(fade >> 4);
led_set(LED_RED, false);
current_board->set_led(LED_RED, false);
delay((MAX_LED_FADE - fade) >> 4);
}
#ifdef DEBUG_FAULTS
} else {
led_set(LED_RED, 1);
current_board->set_led(LED_RED, 1);
delay(512000U);
led_set(LED_RED, 0);
current_board->set_led(LED_RED, 0);
delay(512000U);
}
#endif

View File

@@ -212,7 +212,7 @@ int comms_control_handler(ControlPacket_t *req, uint8_t *resp) {
break;
// **** 0xdb: set OBD CAN multiplexing mode
case 0xdb:
if (current_board->harness_config->has_harness) {
if (current_board->has_obd) {
if (req->param1 == 1U) {
// Enable OBD CAN
current_board->set_can_mode(CAN_MODE_OBD_CAN2);

View File

@@ -5,15 +5,8 @@
int power_save_status = POWER_SAVE_STATUS_DISABLED;
void enable_can_transceivers(bool enabled) {
// Leave main CAN always on for CAN-based ignition detection
uint8_t main_bus = (current_board->harness_config->has_harness && (harness.status == HARNESS_STATUS_FLIPPED)) ? 3U : 1U;
for(uint8_t i=1U; i<=4U; i++){
current_board->enable_can_transceiver(i, (i == main_bus) || enabled);
}
}
void set_power_save_state(int state) {
bool is_valid_state = (state == POWER_SAVE_STATUS_ENABLED) || (state == POWER_SAVE_STATUS_DISABLED);
if (is_valid_state && (state != power_save_status)) {
bool enable = false;
@@ -40,7 +33,7 @@ void set_power_save_state(int state) {
enable = true;
}
enable_can_transceivers(enable);
current_board->enable_can_transceivers(enable);
// Switch off IR when in power saving
if(!enable){

View File

@@ -39,7 +39,4 @@ void detect_board_type(void) {
hw_type = HW_TYPE_BLACK_PANDA;
current_board = &board_black;
}
// Return A13 to the alt mode to fix SWD
set_gpio_alternate(GPIOA, 13, GPIO_AF0_SWJ);
}

View File

@@ -73,7 +73,7 @@
void early_gpio_float(void) {
RCC->AHB1ENR = RCC_AHB1ENR_GPIOAEN | RCC_AHB1ENR_GPIOBEN | RCC_AHB1ENR_GPIOCEN;
GPIOB->MODER = 0; GPIOC->MODER = 0;
GPIOA->MODER = 0; GPIOB->MODER = 0; GPIOC->MODER = 0;
GPIOA->ODR = 0; GPIOB->ODR = 0; GPIOC->ODR = 0;
GPIOA->PUPDR = 0; GPIOB->PUPDR = 0; GPIOC->PUPDR = 0;
}

View File

@@ -14,6 +14,7 @@
#include "stm32h7/sound.h"
#include "drivers/clock_source.h"
#include "boards/red.h"
#include "boards/red_chiplet.h"
#include "boards/tres.h"
#include "boards/cuatro.h"

View File

@@ -31,6 +31,26 @@ void gpio_uart7_init(void) {
// Common GPIO initialization
void common_init_gpio(void) {
/// E2,E3,E4: RGB LED
set_gpio_pullup(GPIOE, 2, PULL_NONE);
set_gpio_mode(GPIOE, 2, MODE_OUTPUT);
set_gpio_output_type(GPIOE, 2, OUTPUT_TYPE_OPEN_DRAIN);
set_gpio_pullup(GPIOE, 3, PULL_NONE);
set_gpio_mode(GPIOE, 3, MODE_OUTPUT);
set_gpio_output_type(GPIOE, 3, OUTPUT_TYPE_OPEN_DRAIN);
set_gpio_pullup(GPIOE, 4, PULL_NONE);
set_gpio_mode(GPIOE, 4, MODE_OUTPUT);
set_gpio_output_type(GPIOE, 4, OUTPUT_TYPE_OPEN_DRAIN);
//C4,A1: OBD_SBU1, OBD_SBU2
set_gpio_pullup(GPIOC, 4, PULL_NONE);
set_gpio_mode(GPIOC, 4, MODE_ANALOG);
set_gpio_pullup(GPIOA, 1, PULL_NONE);
set_gpio_mode(GPIOA, 1, MODE_ANALOG);
//F11: VOLT_S
set_gpio_pullup(GPIOF, 11, PULL_NONE);
set_gpio_mode(GPIOF, 11, MODE_ANALOG);

View File

@@ -201,7 +201,6 @@ class Controls:
# carControl
cc_send = messaging.new_message('carControl')
cc_send.valid = CS.canValid
CC.currentCurvature = cs.curvature
cc_send.carControl = CC
self.pm.send('carControl', cc_send)

View File

@@ -6,7 +6,6 @@ from typing import Any
import capnp
from cereal import messaging, log, car
from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.common.params import Params
from openpilot.common.realtime import DT_MDL, Priority, config_realtime_process
from openpilot.common.swaglog import cloudlog
@@ -52,7 +51,7 @@ class Track:
def __init__(self, identifier: int, v_lead: float, kalman_params: KalmanParams):
self.identifier = identifier
self.cnt = 0
self.aLeadTau = FirstOrderFilter(_LEAD_ACCEL_TAU, 0.45, DT_MDL)
self.aLeadTau = _LEAD_ACCEL_TAU
self.K_A = kalman_params.A
self.K_C = kalman_params.C
self.K_K = kalman_params.K
@@ -75,12 +74,17 @@ class Track:
# Learn if constant acceleration
if abs(self.aLeadK) < 0.5:
self.aLeadTau.x = _LEAD_ACCEL_TAU
self.aLeadTau = _LEAD_ACCEL_TAU
else:
self.aLeadTau.update(0.0)
self.aLeadTau *= 0.9
self.cnt += 1
def reset_a_lead(self, aLeadK: float, aLeadTau: float):
self.kf = KF1D([[self.vLead], [aLeadK]], self.K_A, self.K_C, self.K_K)
self.aLeadK = aLeadK
self.aLeadTau = aLeadTau
def get_RadarState(self, model_prob: float = 0.0):
return {
"dRel": float(self.dRel),
@@ -89,7 +93,7 @@ class Track:
"vLead": float(self.vLead),
"vLeadK": float(self.vLeadK),
"aLeadK": float(self.aLeadK),
"aLeadTau": float(self.aLeadTau.x),
"aLeadTau": float(self.aLeadTau),
"status": True,
"fcw": self.is_potential_fcw(model_prob),
"modelProb": model_prob,

View File

@@ -14,14 +14,8 @@ class ModelConstants:
# model inputs constants
MODEL_FREQ = 20
HISTORY_FREQ = 5
HISTORY_LEN_SECONDS = 5
TEMPORAL_SKIP = MODEL_FREQ // HISTORY_FREQ
FULL_HISTORY_BUFFER_LEN = MODEL_FREQ * HISTORY_LEN_SECONDS
INPUT_HISTORY_BUFFER_LEN = HISTORY_FREQ * HISTORY_LEN_SECONDS
FEATURE_LEN = 512
FULL_HISTORY_BUFFER_LEN = 100
DESIRE_LEN = 8
TRAFFIC_CONVENTION_LEN = 2
LAT_PLANNER_STATE_LEN = 4

View File

@@ -56,24 +56,16 @@ class ModelState:
prev_desire: np.ndarray # for tracking the rising edge of the pulse
def __init__(self, context: CLContext):
self.frames = {
'input_imgs': DrivingModelFrame(context, ModelConstants.TEMPORAL_SKIP),
'big_input_imgs': DrivingModelFrame(context, ModelConstants.TEMPORAL_SKIP)
}
self.frames = {'input_imgs': DrivingModelFrame(context), 'big_input_imgs': DrivingModelFrame(context)}
self.prev_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32)
self.full_features_buffer = np.zeros((1, ModelConstants.FULL_HISTORY_BUFFER_LEN, ModelConstants.FEATURE_LEN), dtype=np.float32)
self.full_desire = np.zeros((1, ModelConstants.FULL_HISTORY_BUFFER_LEN, ModelConstants.DESIRE_LEN), dtype=np.float32)
self.full_prev_desired_curv = np.zeros((1, ModelConstants.FULL_HISTORY_BUFFER_LEN, ModelConstants.PREV_DESIRED_CURV_LEN), dtype=np.float32)
self.temporal_idxs = slice(-1-(ModelConstants.TEMPORAL_SKIP*(ModelConstants.INPUT_HISTORY_BUFFER_LEN-1)), None, ModelConstants.TEMPORAL_SKIP)
# policy inputs
self.numpy_inputs = {
'desire': np.zeros((1, ModelConstants.INPUT_HISTORY_BUFFER_LEN, ModelConstants.DESIRE_LEN), dtype=np.float32),
'desire': np.zeros((1, ModelConstants.FULL_HISTORY_BUFFER_LEN, ModelConstants.DESIRE_LEN), dtype=np.float32),
'traffic_convention': np.zeros((1, ModelConstants.TRAFFIC_CONVENTION_LEN), dtype=np.float32),
'lateral_control_params': np.zeros((1, ModelConstants.LATERAL_CONTROL_PARAMS_LEN), dtype=np.float32),
'prev_desired_curv': np.zeros((1, ModelConstants.INPUT_HISTORY_BUFFER_LEN, ModelConstants.PREV_DESIRED_CURV_LEN), dtype=np.float32),
'features_buffer': np.zeros((1, ModelConstants.INPUT_HISTORY_BUFFER_LEN, ModelConstants.FEATURE_LEN), dtype=np.float32),
'prev_desired_curv': np.zeros((1, ModelConstants.FULL_HISTORY_BUFFER_LEN, ModelConstants.PREV_DESIRED_CURV_LEN), dtype=np.float32),
'features_buffer': np.zeros((1, ModelConstants.FULL_HISTORY_BUFFER_LEN, ModelConstants.FEATURE_LEN), dtype=np.float32),
}
with open(VISION_METADATA_PATH, 'rb') as f:
@@ -112,9 +104,8 @@ class ModelState:
new_desire = np.where(inputs['desire'] - self.prev_desire > .99, inputs['desire'], 0)
self.prev_desire[:] = inputs['desire']
self.full_desire[0,:-1] = self.full_desire[0,1:]
self.full_desire[0,-1] = new_desire
self.numpy_inputs['desire'][:] = self.full_desire.reshape((1,ModelConstants.INPUT_HISTORY_BUFFER_LEN,ModelConstants.TEMPORAL_SKIP,-1)).max(axis=2)
self.numpy_inputs['desire'][0,:-1] = self.numpy_inputs['desire'][0,1:]
self.numpy_inputs['desire'][0,-1] = new_desire
self.numpy_inputs['traffic_convention'][:] = inputs['traffic_convention']
self.numpy_inputs['lateral_control_params'][:] = inputs['lateral_control_params']
@@ -137,17 +128,15 @@ class ModelState:
self.vision_output = self.vision_run(**self.vision_inputs).numpy().flatten()
vision_outputs_dict = self.parser.parse_vision_outputs(self.slice_outputs(self.vision_output, self.vision_output_slices))
self.full_features_buffer[0,:-1] = self.full_features_buffer[0,1:]
self.full_features_buffer[0,-1] = vision_outputs_dict['hidden_state'][0, :]
self.numpy_inputs['features_buffer'][:] = self.full_features_buffer[0, self.temporal_idxs]
self.numpy_inputs['features_buffer'][0,:-1] = self.numpy_inputs['features_buffer'][0,1:]
self.numpy_inputs['features_buffer'][0,-1] = vision_outputs_dict['hidden_state'][0, :]
self.policy_output = self.policy_run(**self.policy_inputs).numpy().flatten()
policy_outputs_dict = self.parser.parse_policy_outputs(self.slice_outputs(self.policy_output, self.policy_output_slices))
# TODO model only uses last value now
self.full_prev_desired_curv[0,:-1] = self.full_prev_desired_curv[0,1:]
self.full_prev_desired_curv[0,-1,:] = policy_outputs_dict['desired_curvature'][0, :]
self.numpy_inputs['prev_desired_curv'][:] = self.full_prev_desired_curv[0, self.temporal_idxs]
self.numpy_inputs['prev_desired_curv'][0,:-1] = self.numpy_inputs['prev_desired_curv'][0,1:]
self.numpy_inputs['prev_desired_curv'][0,-1,:] = policy_outputs_dict['desired_curvature'][0, :]
combined_outputs_dict = {**vision_outputs_dict, **policy_outputs_dict}
if SEND_RAW_PRED:

View File

@@ -5,12 +5,11 @@
#include "common/clutil.h"
DrivingModelFrame::DrivingModelFrame(cl_device_id device_id, cl_context context, int _temporal_skip) : ModelFrame(device_id, context) {
DrivingModelFrame::DrivingModelFrame(cl_device_id device_id, cl_context context) : ModelFrame(device_id, context) {
input_frames = std::make_unique<uint8_t[]>(buf_size);
temporal_skip = _temporal_skip;
input_frames_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, buf_size, NULL, &err));
img_buffer_20hz_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, (temporal_skip+1)*frame_size_bytes, NULL, &err));
region.origin = temporal_skip * frame_size_bytes;
img_buffer_20hz_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, 2*frame_size_bytes, NULL, &err));
region.origin = 1 * frame_size_bytes;
region.size = frame_size_bytes;
last_img_cl = CL_CHECK_ERR(clCreateSubBuffer(img_buffer_20hz_cl, CL_MEM_READ_WRITE, CL_BUFFER_CREATE_TYPE_REGION, &region, &err));
@@ -21,7 +20,7 @@ DrivingModelFrame::DrivingModelFrame(cl_device_id device_id, cl_context context,
cl_mem* DrivingModelFrame::prepare(cl_mem yuv_cl, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3& projection) {
run_transform(yuv_cl, MODEL_WIDTH, MODEL_HEIGHT, frame_width, frame_height, frame_stride, frame_uv_offset, projection);
for (int i = 0; i < temporal_skip; i++) {
for (int i = 0; i < 1; i++) {
CL_CHECK(clEnqueueCopyBuffer(q, img_buffer_20hz_cl, img_buffer_20hz_cl, (i+1)*frame_size_bytes, i*frame_size_bytes, frame_size_bytes, 0, nullptr, nullptr));
}
loadyuv_queue(&loadyuv, q, y_cl, u_cl, v_cl, last_img_cl);
@@ -37,7 +36,6 @@ cl_mem* DrivingModelFrame::prepare(cl_mem yuv_cl, int frame_width, int frame_hei
DrivingModelFrame::~DrivingModelFrame() {
deinit_transform();
loadyuv_destroy(&loadyuv);
CL_CHECK(clReleaseMemObject(input_frames_cl));
CL_CHECK(clReleaseMemObject(img_buffer_20hz_cl));
CL_CHECK(clReleaseMemObject(last_img_cl));
CL_CHECK(clReleaseCommandQueue(q));
@@ -59,6 +57,5 @@ cl_mem* MonitoringModelFrame::prepare(cl_mem yuv_cl, int frame_width, int frame_
MonitoringModelFrame::~MonitoringModelFrame() {
deinit_transform();
CL_CHECK(clReleaseMemObject(input_frame_cl));
CL_CHECK(clReleaseCommandQueue(q));
}

View File

@@ -64,21 +64,20 @@ protected:
class DrivingModelFrame : public ModelFrame {
public:
DrivingModelFrame(cl_device_id device_id, cl_context context, int _temporal_skip);
DrivingModelFrame(cl_device_id device_id, cl_context context);
~DrivingModelFrame();
cl_mem* prepare(cl_mem yuv_cl, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3& projection);
const int MODEL_WIDTH = 512;
const int MODEL_HEIGHT = 256;
const int MODEL_FRAME_SIZE = MODEL_WIDTH * MODEL_HEIGHT * 3 / 2;
const int buf_size = MODEL_FRAME_SIZE * 2; // 2 frames are temporal_skip frames apart
const int buf_size = MODEL_FRAME_SIZE * 2;
const size_t frame_size_bytes = MODEL_FRAME_SIZE * sizeof(uint8_t);
private:
LoadYUVState loadyuv;
cl_mem img_buffer_20hz_cl, last_img_cl, input_frames_cl;
cl_buffer_region region;
int temporal_skip;
};
class MonitoringModelFrame : public ModelFrame {

View File

@@ -20,7 +20,7 @@ cdef extern from "selfdrive/modeld/models/commonmodel.h":
cppclass DrivingModelFrame:
int buf_size
DrivingModelFrame(cl_device_id, cl_context, int)
DrivingModelFrame(cl_device_id, cl_context)
cppclass MonitoringModelFrame:
int buf_size

View File

@@ -3548,7 +3548,6 @@ static const char __pyx_k_stringsource[] = "<stringsource>";
static const char __pyx_k_version_info[] = "version_info";
static const char __pyx_k_class_getitem[] = "__class_getitem__";
static const char __pyx_k_reduce_cython[] = "__reduce_cython__";
static const char __pyx_k_temporal_skip[] = "temporal_skip";
static const char __pyx_k_AssertionError[] = "AssertionError";
static const char __pyx_k_buffer_from_cl[] = "buffer_from_cl";
static const char __pyx_k_View_MemoryView[] = "View.MemoryView";
@@ -3665,7 +3664,7 @@ static PyObject *__pyx_pf_9selfdrive_6modeld_6models_15commonmodel_pyx_10ModelFr
static PyObject *__pyx_pf_9selfdrive_6modeld_6models_15commonmodel_pyx_10ModelFrame_4buffer_from_cl(struct __pyx_obj_9selfdrive_6modeld_6models_15commonmodel_pyx_ModelFrame *__pyx_v_self, struct __pyx_obj_9selfdrive_6modeld_6models_15commonmodel_pyx_CLMem *__pyx_v_in_frames); /* proto */
static PyObject *__pyx_pf_9selfdrive_6modeld_6models_15commonmodel_pyx_10ModelFrame_6__reduce_cython__(CYTHON_UNUSED struct __pyx_obj_9selfdrive_6modeld_6models_15commonmodel_pyx_ModelFrame *__pyx_v_self); /* proto */
static PyObject *__pyx_pf_9selfdrive_6modeld_6models_15commonmodel_pyx_10ModelFrame_8__setstate_cython__(CYTHON_UNUSED struct __pyx_obj_9selfdrive_6modeld_6models_15commonmodel_pyx_ModelFrame *__pyx_v_self, CYTHON_UNUSED PyObject *__pyx_v___pyx_state); /* proto */
static int __pyx_pf_9selfdrive_6modeld_6models_15commonmodel_pyx_17DrivingModelFrame___cinit__(struct __pyx_obj_9selfdrive_6modeld_6models_15commonmodel_pyx_DrivingModelFrame *__pyx_v_self, struct __pyx_obj_9selfdrive_6modeld_6models_15commonmodel_pyx_CLContext *__pyx_v_context, int __pyx_v_temporal_skip); /* proto */
static int __pyx_pf_9selfdrive_6modeld_6models_15commonmodel_pyx_17DrivingModelFrame___cinit__(struct __pyx_obj_9selfdrive_6modeld_6models_15commonmodel_pyx_DrivingModelFrame *__pyx_v_self, struct __pyx_obj_9selfdrive_6modeld_6models_15commonmodel_pyx_CLContext *__pyx_v_context); /* proto */
static PyObject *__pyx_pf_9selfdrive_6modeld_6models_15commonmodel_pyx_17DrivingModelFrame_2__reduce_cython__(CYTHON_UNUSED struct __pyx_obj_9selfdrive_6modeld_6models_15commonmodel_pyx_DrivingModelFrame *__pyx_v_self); /* proto */
static PyObject *__pyx_pf_9selfdrive_6modeld_6models_15commonmodel_pyx_17DrivingModelFrame_4__setstate_cython__(CYTHON_UNUSED struct __pyx_obj_9selfdrive_6modeld_6models_15commonmodel_pyx_DrivingModelFrame *__pyx_v_self, CYTHON_UNUSED PyObject *__pyx_v___pyx_state); /* proto */
static int __pyx_pf_9selfdrive_6modeld_6models_15commonmodel_pyx_20MonitoringModelFrame___cinit__(struct __pyx_obj_9selfdrive_6modeld_6models_15commonmodel_pyx_MonitoringModelFrame *__pyx_v_self, struct __pyx_obj_9selfdrive_6modeld_6models_15commonmodel_pyx_CLContext *__pyx_v_context); /* proto */
@@ -3940,7 +3939,6 @@ typedef struct {
PyObject *__pyx_kp_s_stringsource;
PyObject *__pyx_n_s_struct;
PyObject *__pyx_n_s_sys;
PyObject *__pyx_n_s_temporal_skip;
PyObject *__pyx_n_s_test;
PyObject *__pyx_kp_s_unable_to_allocate_array_data;
PyObject *__pyx_kp_s_unable_to_allocate_shape_and_str;
@@ -4227,7 +4225,6 @@ static int __pyx_m_clear(PyObject *m) {
Py_CLEAR(clear_module_state->__pyx_kp_s_stringsource);
Py_CLEAR(clear_module_state->__pyx_n_s_struct);
Py_CLEAR(clear_module_state->__pyx_n_s_sys);
Py_CLEAR(clear_module_state->__pyx_n_s_temporal_skip);
Py_CLEAR(clear_module_state->__pyx_n_s_test);
Py_CLEAR(clear_module_state->__pyx_kp_s_unable_to_allocate_array_data);
Py_CLEAR(clear_module_state->__pyx_kp_s_unable_to_allocate_shape_and_str);
@@ -4492,7 +4489,6 @@ static int __pyx_m_traverse(PyObject *m, visitproc visit, void *arg) {
Py_VISIT(traverse_module_state->__pyx_kp_s_stringsource);
Py_VISIT(traverse_module_state->__pyx_n_s_struct);
Py_VISIT(traverse_module_state->__pyx_n_s_sys);
Py_VISIT(traverse_module_state->__pyx_n_s_temporal_skip);
Py_VISIT(traverse_module_state->__pyx_n_s_test);
Py_VISIT(traverse_module_state->__pyx_kp_s_unable_to_allocate_array_data);
Py_VISIT(traverse_module_state->__pyx_kp_s_unable_to_allocate_shape_and_str);
@@ -4803,7 +4799,6 @@ static int __pyx_m_traverse(PyObject *m, visitproc visit, void *arg) {
#define __pyx_kp_s_stringsource __pyx_mstate_global->__pyx_kp_s_stringsource
#define __pyx_n_s_struct __pyx_mstate_global->__pyx_n_s_struct
#define __pyx_n_s_sys __pyx_mstate_global->__pyx_n_s_sys
#define __pyx_n_s_temporal_skip __pyx_mstate_global->__pyx_n_s_temporal_skip
#define __pyx_n_s_test __pyx_mstate_global->__pyx_n_s_test
#define __pyx_kp_s_unable_to_allocate_array_data __pyx_mstate_global->__pyx_kp_s_unable_to_allocate_array_data
#define __pyx_kp_s_unable_to_allocate_shape_and_str __pyx_mstate_global->__pyx_kp_s_unable_to_allocate_shape_and_str
@@ -21647,8 +21642,8 @@ static PyObject *__pyx_pf_9selfdrive_6modeld_6models_15commonmodel_pyx_10ModelFr
/* "selfdrive/modeld/models/commonmodel_pyx.pyx":62
* cdef cppDrivingModelFrame * _frame
*
* def __cinit__(self, CLContext context, int temporal_skip): # <<<<<<<<<<<<<<
* self._frame = new cppDrivingModelFrame(context.device_id, context.context, temporal_skip)
* def __cinit__(self, CLContext context): # <<<<<<<<<<<<<<
* self._frame = new cppDrivingModelFrame(context.device_id, context.context)
* self.frame = <cppModelFrame*>(self._frame)
*/
@@ -21656,10 +21651,9 @@ static PyObject *__pyx_pf_9selfdrive_6modeld_6models_15commonmodel_pyx_10ModelFr
static int __pyx_pw_9selfdrive_6modeld_6models_15commonmodel_pyx_17DrivingModelFrame_1__cinit__(PyObject *__pyx_v_self, PyObject *__pyx_args, PyObject *__pyx_kwds); /*proto*/
static int __pyx_pw_9selfdrive_6modeld_6models_15commonmodel_pyx_17DrivingModelFrame_1__cinit__(PyObject *__pyx_v_self, PyObject *__pyx_args, PyObject *__pyx_kwds) {
struct __pyx_obj_9selfdrive_6modeld_6models_15commonmodel_pyx_CLContext *__pyx_v_context = 0;
int __pyx_v_temporal_skip;
CYTHON_UNUSED Py_ssize_t __pyx_nargs;
CYTHON_UNUSED PyObject *const *__pyx_kwvalues;
PyObject* values[2] = {0,0};
PyObject* values[1] = {0};
int __pyx_lineno = 0;
const char *__pyx_filename = NULL;
int __pyx_clineno = 0;
@@ -21673,12 +21667,10 @@ static int __pyx_pw_9selfdrive_6modeld_6models_15commonmodel_pyx_17DrivingModelF
#endif
__pyx_kwvalues = __Pyx_KwValues_VARARGS(__pyx_args, __pyx_nargs);
{
PyObject **__pyx_pyargnames[] = {&__pyx_n_s_context,&__pyx_n_s_temporal_skip,0};
PyObject **__pyx_pyargnames[] = {&__pyx_n_s_context,0};
if (__pyx_kwds) {
Py_ssize_t kw_args;
switch (__pyx_nargs) {
case 2: values[1] = __Pyx_Arg_VARARGS(__pyx_args, 1);
CYTHON_FALLTHROUGH;
case 1: values[0] = __Pyx_Arg_VARARGS(__pyx_args, 0);
CYTHON_FALLTHROUGH;
case 0: break;
@@ -21693,33 +21685,21 @@ static int __pyx_pw_9selfdrive_6modeld_6models_15commonmodel_pyx_17DrivingModelF
}
else if (unlikely(PyErr_Occurred())) __PYX_ERR(1, 62, __pyx_L3_error)
else goto __pyx_L5_argtuple_error;
CYTHON_FALLTHROUGH;
case 1:
if (likely((values[1] = __Pyx_GetKwValue_VARARGS(__pyx_kwds, __pyx_kwvalues, __pyx_n_s_temporal_skip)) != 0)) {
(void)__Pyx_Arg_NewRef_VARARGS(values[1]);
kw_args--;
}
else if (unlikely(PyErr_Occurred())) __PYX_ERR(1, 62, __pyx_L3_error)
else {
__Pyx_RaiseArgtupleInvalid("__cinit__", 1, 2, 2, 1); __PYX_ERR(1, 62, __pyx_L3_error)
}
}
if (unlikely(kw_args > 0)) {
const Py_ssize_t kwd_pos_args = __pyx_nargs;
if (unlikely(__Pyx_ParseOptionalKeywords(__pyx_kwds, __pyx_kwvalues, __pyx_pyargnames, 0, values + 0, kwd_pos_args, "__cinit__") < 0)) __PYX_ERR(1, 62, __pyx_L3_error)
}
} else if (unlikely(__pyx_nargs != 2)) {
} else if (unlikely(__pyx_nargs != 1)) {
goto __pyx_L5_argtuple_error;
} else {
values[0] = __Pyx_Arg_VARARGS(__pyx_args, 0);
values[1] = __Pyx_Arg_VARARGS(__pyx_args, 1);
}
__pyx_v_context = ((struct __pyx_obj_9selfdrive_6modeld_6models_15commonmodel_pyx_CLContext *)values[0]);
__pyx_v_temporal_skip = __Pyx_PyInt_As_int(values[1]); if (unlikely((__pyx_v_temporal_skip == (int)-1) && PyErr_Occurred())) __PYX_ERR(1, 62, __pyx_L3_error)
}
goto __pyx_L6_skip;
__pyx_L5_argtuple_error:;
__Pyx_RaiseArgtupleInvalid("__cinit__", 1, 2, 2, __pyx_nargs); __PYX_ERR(1, 62, __pyx_L3_error)
__Pyx_RaiseArgtupleInvalid("__cinit__", 1, 1, 1, __pyx_nargs); __PYX_ERR(1, 62, __pyx_L3_error)
__pyx_L6_skip:;
goto __pyx_L4_argument_unpacking_done;
__pyx_L3_error:;
@@ -21734,7 +21714,7 @@ static int __pyx_pw_9selfdrive_6modeld_6models_15commonmodel_pyx_17DrivingModelF
return -1;
__pyx_L4_argument_unpacking_done:;
if (unlikely(!__Pyx_ArgTypeTest(((PyObject *)__pyx_v_context), __pyx_ptype_9selfdrive_6modeld_6models_15commonmodel_pyx_CLContext, 1, "context", 0))) __PYX_ERR(1, 62, __pyx_L1_error)
__pyx_r = __pyx_pf_9selfdrive_6modeld_6models_15commonmodel_pyx_17DrivingModelFrame___cinit__(((struct __pyx_obj_9selfdrive_6modeld_6models_15commonmodel_pyx_DrivingModelFrame *)__pyx_v_self), __pyx_v_context, __pyx_v_temporal_skip);
__pyx_r = __pyx_pf_9selfdrive_6modeld_6models_15commonmodel_pyx_17DrivingModelFrame___cinit__(((struct __pyx_obj_9selfdrive_6modeld_6models_15commonmodel_pyx_DrivingModelFrame *)__pyx_v_self), __pyx_v_context);
/* function exit code */
goto __pyx_L0;
@@ -21751,22 +21731,22 @@ static int __pyx_pw_9selfdrive_6modeld_6models_15commonmodel_pyx_17DrivingModelF
return __pyx_r;
}
static int __pyx_pf_9selfdrive_6modeld_6models_15commonmodel_pyx_17DrivingModelFrame___cinit__(struct __pyx_obj_9selfdrive_6modeld_6models_15commonmodel_pyx_DrivingModelFrame *__pyx_v_self, struct __pyx_obj_9selfdrive_6modeld_6models_15commonmodel_pyx_CLContext *__pyx_v_context, int __pyx_v_temporal_skip) {
static int __pyx_pf_9selfdrive_6modeld_6models_15commonmodel_pyx_17DrivingModelFrame___cinit__(struct __pyx_obj_9selfdrive_6modeld_6models_15commonmodel_pyx_DrivingModelFrame *__pyx_v_self, struct __pyx_obj_9selfdrive_6modeld_6models_15commonmodel_pyx_CLContext *__pyx_v_context) {
int __pyx_r;
int __pyx_t_1;
/* "selfdrive/modeld/models/commonmodel_pyx.pyx":63
*
* def __cinit__(self, CLContext context, int temporal_skip):
* self._frame = new cppDrivingModelFrame(context.device_id, context.context, temporal_skip) # <<<<<<<<<<<<<<
* def __cinit__(self, CLContext context):
* self._frame = new cppDrivingModelFrame(context.device_id, context.context) # <<<<<<<<<<<<<<
* self.frame = <cppModelFrame*>(self._frame)
* self.buf_size = self._frame.buf_size
*/
__pyx_v_self->_frame = new DrivingModelFrame(__pyx_v_context->__pyx_base.device_id, __pyx_v_context->__pyx_base.context, __pyx_v_temporal_skip);
__pyx_v_self->_frame = new DrivingModelFrame(__pyx_v_context->__pyx_base.device_id, __pyx_v_context->__pyx_base.context);
/* "selfdrive/modeld/models/commonmodel_pyx.pyx":64
* def __cinit__(self, CLContext context, int temporal_skip):
* self._frame = new cppDrivingModelFrame(context.device_id, context.context, temporal_skip)
* def __cinit__(self, CLContext context):
* self._frame = new cppDrivingModelFrame(context.device_id, context.context)
* self.frame = <cppModelFrame*>(self._frame) # <<<<<<<<<<<<<<
* self.buf_size = self._frame.buf_size
*
@@ -21774,7 +21754,7 @@ static int __pyx_pf_9selfdrive_6modeld_6models_15commonmodel_pyx_17DrivingModelF
__pyx_v_self->__pyx_base.frame = ((ModelFrame *)__pyx_v_self->_frame);
/* "selfdrive/modeld/models/commonmodel_pyx.pyx":65
* self._frame = new cppDrivingModelFrame(context.device_id, context.context, temporal_skip)
* self._frame = new cppDrivingModelFrame(context.device_id, context.context)
* self.frame = <cppModelFrame*>(self._frame)
* self.buf_size = self._frame.buf_size # <<<<<<<<<<<<<<
*
@@ -21786,8 +21766,8 @@ static int __pyx_pf_9selfdrive_6modeld_6models_15commonmodel_pyx_17DrivingModelF
/* "selfdrive/modeld/models/commonmodel_pyx.pyx":62
* cdef cppDrivingModelFrame * _frame
*
* def __cinit__(self, CLContext context, int temporal_skip): # <<<<<<<<<<<<<<
* self._frame = new cppDrivingModelFrame(context.device_id, context.context, temporal_skip)
* def __cinit__(self, CLContext context): # <<<<<<<<<<<<<<
* self._frame = new cppDrivingModelFrame(context.device_id, context.context)
* self.frame = <cppModelFrame*>(self._frame)
*/
@@ -24164,7 +24144,6 @@ static int __Pyx_CreateStringTabAndInitStrings(void) {
{&__pyx_kp_s_stringsource, __pyx_k_stringsource, sizeof(__pyx_k_stringsource), 0, 0, 1, 0},
{&__pyx_n_s_struct, __pyx_k_struct, sizeof(__pyx_k_struct), 0, 0, 1, 1},
{&__pyx_n_s_sys, __pyx_k_sys, sizeof(__pyx_k_sys), 0, 0, 1, 1},
{&__pyx_n_s_temporal_skip, __pyx_k_temporal_skip, sizeof(__pyx_k_temporal_skip), 0, 0, 1, 1},
{&__pyx_n_s_test, __pyx_k_test, sizeof(__pyx_k_test), 0, 0, 1, 1},
{&__pyx_kp_s_unable_to_allocate_array_data, __pyx_k_unable_to_allocate_array_data, sizeof(__pyx_k_unable_to_allocate_array_data), 0, 0, 1, 0},
{&__pyx_kp_s_unable_to_allocate_shape_and_str, __pyx_k_unable_to_allocate_shape_and_str, sizeof(__pyx_k_unable_to_allocate_shape_and_str), 0, 0, 1, 0},

View File

@@ -59,8 +59,8 @@ cdef class ModelFrame:
cdef class DrivingModelFrame(ModelFrame):
cdef cppDrivingModelFrame * _frame
def __cinit__(self, CLContext context, int temporal_skip):
self._frame = new cppDrivingModelFrame(context.device_id, context.context, temporal_skip)
def __cinit__(self, CLContext context):
self._frame = new cppDrivingModelFrame(context.device_id, context.context)
self.frame = <cppModelFrame*>(self._frame)
self.buf_size = self._frame.buf_size

View File

@@ -66,25 +66,6 @@ void Panda::set_alternative_experience(uint16_t alternative_experience) {
handle->control_write(0xdf, alternative_experience, 0);
}
std::string Panda::serial_read(int port_number) {
std::string ret;
char buffer[USBPACKET_MAX_SIZE] = {};
while (true) {
int bytes_read = handle->control_read(0xe0, port_number, 0, (unsigned char *)buffer, USBPACKET_MAX_SIZE);
if (bytes_read <= 0) {
break;
}
ret.append(buffer, bytes_read);
}
return ret;
}
void Panda::set_uart_baud(int uart, int rate) {
handle->control_write(0xe4, uart, int(rate / 300));
}
cereal::PandaState::PandaType Panda::get_hw_type() {
unsigned char hw_query[1] = {0};

View File

@@ -64,8 +64,6 @@ public:
cereal::PandaState::PandaType get_hw_type();
void set_safety_model(cereal::CarParams::SafetyModel safety_model, uint16_t safety_param=0U);
void set_alternative_experience(uint16_t alternative_experience);
std::string serial_read(int port_number = 0);
void set_uart_baud(int uart, int rate);
void set_fan_speed(uint16_t fan_speed);
uint16_t get_fan_speed();
void set_ir_pwr(uint16_t ir_pwr);

Binary file not shown.

View File

@@ -452,14 +452,6 @@ void pandad_run(std::vector<Panda *> &pandas) {
send_peripheral_state(peripheral_panda, &pm);
}
// Forward logs from pandas to cloudlog if available
for (auto *panda : pandas) {
std::string log = panda->serial_read();
if (!log.empty()) {
LOGD("%s", log.c_str());
}
}
rk.keepTime();
}

View File

@@ -1,222 +1 @@
#!/usr/bin/env python3
from pathlib import Path
from ctypes import *
import json
import collections
import numpy as np
import faulthandler
import struct
faulthandler.enable()
basedir = Path(__file__).resolve().parent
libane = None
aneregs = None
def init_libane():
global libane, aneregs
libane = cdll.LoadLibrary((basedir / "libane.dylib").as_posix())
libane.ANE_Compile.argtypes = [c_char_p, c_int]
libane.ANE_Compile.restype = c_void_p
libane.ANE_TensorCreate.restype = c_void_p
libane.ANE_TensorData.argtypes = [c_void_p]
libane.ANE_TensorData.restype = POINTER(c_uint16)
libane.ANE_Run.argtypes = [c_void_p]*4
libane.ANE_Run.restype = c_int
#libane.ANE_RegDebug.restype = c_char_p
with open(basedir / "aneregs.json") as f:
aneregs = json.load(f)
ANE_Struct = [
# aneTD.Header
("u32", 0x1C, "NextCommandOffset"),
# KernelDMASrc @ section @ 0x2C len 0xF4
# reloc 0x2c-0x34?? = weights
# u32[16] 0x34-0x74 = 0x80 | 1 if used
# u32[16] 0x74-0xB4 = <channel data offset>
# u32[16] 0xB4-0xF4 = <channel data length>
# Common @ section @ 0x128 len 0x3C (conv)
("u16", 0x128, "InputWidth"),
("u16", 0x12A, "InputHeight"),
("u16", 0x12C, "InputDepth"),
("u32", 0x130, "InputOutputType"), # (OutputType * 0x10) | InputType
# UInt8 = 0, Int8 = 1, Float16 = 2
("u32", 0x134, "InputChannels"),
("u32", 0x138, "OutputChannels"),
("u16", 0x13C, "OutputWidth"),
("u16", 0x13E, "OutputHeight"),
("u16", 0x140, "OutputDepth"),
("u16", 0x144, "KernelSize"), # 0xa000 | (KernelHeight * 0x20) | KernelWidth
("u16", 0x146, "Padding"), # 0x5000 | (PadTop * 0x40) | (PadLeft * 2)
("u16", 0x14C, "BatchSize"),
# TileDMASrc @ section @ 0x16C len 0x6C (input)
# reloc 0x16c-0x174 = image
("u32", 0x178, "InputRowStride"),
("u32", 0x17C, "InputPlaneStride"),
("u32", 0x180, "InputDepthStride"),
("u32", 0x184, "InputBatchStride"),
("u8", 0x1A7, "InputInterleave"),
# L2 @ section @ 0x1E0 len 0x44
# [0x1ec, 0x1f0, 0x1f4, 0x1f8, 0x214] = number of engines
# [0x1f0, 0x1f4, 0x1f8, 0x214] = engines for inconv?
# [0x21c, 0x220, 0x224] = engines for outconv?
# NE @ section @ 0x22c len 0xC (scaling)
("u16", 0x230, "BiasScalar"),
("u16", 0x232, "ScaleScalar"),
# section @ 0x240 len 0x10
("u16", 0x246, "NeuronType"), # 0x10 = copy, 0x11 = ReLU, 0x12 = custom
("u32", 0x250, "PostScale"),
# TileDMADst @ section @ 0x258 len 0x18
# HandleTileDmaDstConfig
# 0x258 -- *(uint *)(this + 0x334) = *(uint *)(this + 0x334) & 0xfffffc3f | 0xc0;
# (GetCacheHintRegisterValue & 0xf) << 6;
("u32", 0x25C, "OutputOffset"), # offset into output buffer to write at?
# 0x260 -- *(uint *)(this + 0x33c) = *(uint *)(this + 0x33c) & 0x3f | (int)uVar10 << 6;
("u32", 0x260, "OutputRowStride"),
("u32", 0x264, "OutputPlaneStride"),
("u32", 0x268, "OutputDepthStride"),
("u32", 0x26C, "OutputBatchStride"),
# 0x270 -- *(uint *)(this + 0x34c) = *(uint *)(this + 0x34c) & 0xf0ffffff | 0x1000000;
# uVar6 = *(uint *)(this + 0x34c) & 0xffffcfcc | 0x2031;
# (ZinTensorDescriptorDmaInterleave & 0xf) << 0x18;
("u8", 0x273, "OutputInterleave"), # i also have this at 0x211?
]
ANE_Struct_Dict = {}
for typ, num, nam in ANE_Struct:
styp = {"u32": "I", "u16": "H", "u8": "B"}[typ]
ANE_Struct_Dict[nam] = (styp, num)
class ANETensor:
def __init__(self, *shape):
self.shape = shape
self.dtype = np.float16
self.sz = int(np.prod(shape))
assert(self.sz <= 0x4000)
self.tt = libane.ANE_TensorCreate(self.sz, 1)
assert(self.tt is not None)
def data(self):
data = libane.ANE_TensorData(self.tt)
assert(data is not None)
#print(hex(addressof(data.contents)))
buf = np.ctypeslib.as_array(data, shape=(self.sz,))
ret = np.frombuffer(buf, dtype=self.dtype)
#print(ret.data)
return ret
class ANE:
def __init__(self):
init_libane()
libane.ANE_Open()
def compile(self, dat):
ret = libane.ANE_Compile(create_string_buffer(dat), len(dat))
assert(ret is not None)
return ret
def run(self, prog, tin, tout, tweights=None):
libane.ANE_Run(prog, tin.tt, tout.tt, tweights.tt if tweights is not None else 0)
def tensor(self, shape):
return ANETensor(shape)
def unpack(self, dat):
dat = struct.unpack("Q"*(len(dat)//8), dat)
ret = {}
for k,v in aneregs:
by,bi,sz = v
bi += (by%8)*8
by //= 8
rv = (dat[by] >> bi) & ((1 << sz)-1)
ret[k] = rv
return ret
def pack(self, pk, dat):
dat = list(struct.unpack("Q"*(len(dat)//8), dat))
for k,v in aneregs:
by,bi,sz = v
bi += (by%8)*8
by //= 8
dat[by] &= ~(((1 << sz)-1) << bi)
dat[by] |= pk[k] << bi
dat = struct.pack("Q"*len(dat), *dat)
return dat
def debug(self, dat, mems=0):
add = [0x30, 0x1d4, 0x220, 0x29c, 0x2f0, 0x30c, 0x32c]
lens = [244, 60, 108, 68, 12, 16, 24]
ptr = 0x2b
ddat = dat[0:0x28]
for a, pm in zip(add, lens):
#assert pm == dat[ptr]
ddat += b"\x00" * (a-len(ddat))
ddat += dat[ptr+1:ptr+1+pm+4]
ptr += pm+8
ddat += b"\x00" * 0x100
ret = collections.OrderedDict()
for ln in libane.ANE_RegDebug(0, create_string_buffer(ddat), mems).decode('utf-8').strip().split("\n"):
lnn = ln.split(" = ")
if len(lnn) == 2:
ret[lnn[0]] = int(lnn[1])
return ret
def filln(self, dat, nvdict, base=0x4000):
for n,v in nvdict.items():
styp, num = ANE_Struct_Dict[n]
dat = self.fill(dat, [num], styp, v)
return dat
def fill(self, dat, addrs, type, val, base=0x4000):
x = struct.pack(type, val)
for a in addrs:
dat[base+a:base+a+len(x)] = x
return dat
if __name__ == "__main__":
ane = ANE()
tin = ANETensor(16)
tout = ANETensor(16)
tind = tin.data()
toutd = tout.data()
tind[0:4] = [-1,1,-2,2]
print("** before **")
print(tind)
print(toutd)
dat = open("../ops/relu.hwx", "rb").read()
md = dat[0x4000:0x4300]
dd = ane.unpack(md)
mdf = ane.pack(dd, md)
assert(md == mdf)
comp = ane.compile(dat)
ret = ane.run(comp, tin, tout)
print("** after **")
print(tind)
print(toutd)
../lib/ane.py

File diff suppressed because it is too large Load Diff

View File

@@ -1,9 +1 @@
<?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE plist PUBLIC "-//Apple//DTD PLIST 1.0//EN" "http://www.apple.com/DTDs/PropertyList-1.0.dtd">
<plist version="1.0">
<dict>
<key>com.apple.ane.iokit-user-access</key><true/>
</dict>
</plist>
../3_run/entitlements.xml

View File

@@ -1,150 +1 @@
enum ANEDeviceUsageType {
UsageNoProgram,
UsageWithProgram, // used in running process
UsageCompile // used in aned
};
struct H11ANEDeviceInfoStruct {
uint64_t program_handle;
uint64_t program_auth_code;
uint64_t sleep_timer;
uint64_t junk[0x100];
};
struct H11ANEStatusStruct {
uint64_t junk[0x100];
};
struct H11ANEProgramCreateArgsStruct {
void *program;
uint64_t program_length;
uint64_t empty[4];
char has_signature;
};
struct H11ANEProgramCreateArgsStructOutput {
uint64_t program_handle;
int unknown[0x2000];
};
struct H11ANEProgramPrepareArgsStruct {
uint64_t program_handle;
uint64_t flags;
uint64_t empty[0x100];
};
struct H11ANEProgramRequestArgsStruct {
uint64_t args[0x1000];
};
namespace H11ANE {
class H11ANEDevice;
class H11ANEDeviceController {
public:
H11ANEDeviceController(
int (*callback)(H11ANE::H11ANEDeviceController*, void*, H11ANE::H11ANEDevice*),
void *arg);
int SetupDeviceController();
private: // size is 0x50
CFArrayRef array_ref;
mach_port_t *master_port;
IONotificationPortRef port_ref;
CFRunLoopSourceRef source_ref;
int (*callback)(H11ANE::H11ANEDeviceController*, void*, H11ANE::H11ANEDevice*);
void *callback_arg;
CFRunLoopRef run_loop_ref;
io_iterator_t io_iterator;
pthread_t thread_self;
uint64_t unused;
};
// we should switch to the IOKit kernel interface, it's likely a lot more stable
// actually this probably isn't true. ANEServices is normal dynamic links
// https://googleprojectzero.blogspot.com/2020/11/oops-i-missed-it-again.html
// H11ANEInDirectPathClient
// _ANE_DeviceOpen
// _ANE_DeviceClose
// _ANE_ProgramSendRequest
// * if they need kernel debugger attached
// H11ANEInUserClient
// _ANE_DeviceOpen
// _ANE_DeviceClose
// _ANE_ProgramSendRequest
// _ANE_ProgramCreate
// _ANE_ProgramPrepare
// _ANE_ProgramUnprepare
// _ANE_ProgramDestroy
// _ANE_GetStatus
// _ANE_PowerOn
// _ANE_PowerOff
// _ANE_IsPowered
// * _ANE_LoadFirmware
// * _ANE_ForgetFirmware
// * _ANE_SendCommand
// _ANE_SetPowerManagement
// _ANE_GetTime
// * _ANE_SetDriverLoggingFlags
// * _ANE_ShowSharedMemoryAllocations
// * _ANE_SetDARTCacheTTL
// * _ANE_SetFirmwareBootArg
// * _ANE_SetThrottlingPercentage
// * _ANE_AddPersistentClient
// * _ANE_RemovePersistentClient
// * _ANE_CreateClientLoggingSession
// * _ANE_TerminateClientLoggingSession
// _ANE_GetDriverLoggingFlags
// * _ANE_FlushInactiveDARTMappings
// _ANE_GetVersion
// _ANE_RegisterFirmwareWorkProcessor
// _ANE_UnregisterFirmwareWorkProcessor
// * _ANE_GetFirmwareWorkProcessorItem
// _ANE_CompleteFirmwareWorkProcessorItem
// _ANE_ReleaseFirmwareWorkProcessorBuffers
// * _ANE_ReadANERegister
// * _ANE_WriteANERegister
// _ANE_ProgramCreateInstance
// note, this is not the raw IOKit class, it's in ANEServices.framework
class H11ANEDevice {
public:
H11ANEDevice(H11ANE::H11ANEDeviceController *param_1, unsigned int param_2);
unsigned long H11ANEDeviceOpen(
int (*callback)(H11ANE::H11ANEDevice*, unsigned int, void*, void*),
void *param_2, ANEDeviceUsageType param_3, H11ANEDeviceInfoStruct *param_4);
void EnableDeviceMessages();
int ANE_AddPersistentClient();
int ANE_GetStatus(H11ANEStatusStruct *param_1);
// power management
int ANE_IsPowered();
int ANE_PowerOn();
int ANE_PowerOff();
// logging (e00002c7 error, needs PE_i_can_has_debugger)
int ANE_CreateClientLoggingSession(unsigned int log_iosurface);
int ANE_TerminateClientLoggingSession(unsigned int log_iosurface);
int ANE_GetDriverLoggingFlags(unsigned int *flags);
int ANE_SetDriverLoggingFlags(unsigned int flags);
// program creation
int ANE_ProgramCreate(H11ANEProgramCreateArgsStruct*,
H11ANEProgramCreateArgsStructOutput*);
int ANE_ProgramPrepare(H11ANEProgramPrepareArgsStruct*);
int ANE_ProgramSendRequest(H11ANEProgramRequestArgsStruct*, mach_port_t);
// need PE_i_can_has_debugger
int ANE_ReadANERegister(unsigned int param_1, unsigned int *param_2);
int ANE_ForgetFirmware();
private: // size is 0x88
unsigned char unknown[0x88];
};
};
../3_run/h11ane.h