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
+59 -2
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,
+10 -2
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
+44 -7
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,
+64 -2
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,
+3 -2
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,
+57 -2
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,
+11 -82
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,
+59 -2
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,
+39 -2
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,
-1
View File
@@ -6,7 +6,6 @@
// ********************* Includes *********************
#include "config.h"
#include "drivers/led.h"
#include "drivers/pwm.h"
#include "drivers/usb.h"
+1 -1
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
-6
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;
}
}
}
+1 -1
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
-6
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) {
+1 -2
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();
}
}
-2
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;
+6 -7
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);
}
}
@@ -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
+22 -2
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,
+22 -2
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,
+10 -11
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);
}
}
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
+17 -25
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
+1 -1
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);
+2 -9
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){
-3
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);
}
+1 -1
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;
}
+1
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"
+20
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);
-1
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)
+9 -5
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,
+1 -7
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
+10 -21
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:
+4 -7
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));
}
+2 -3
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 {
+1 -1
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
+17 -38
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},
+2 -2
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
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
-19
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};
-2
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.
-8
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();
}
+1 -222
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
@@ -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
+1 -150
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