diff --git a/README.md b/README.md index 4be1dabcab..522b010ca5 100644 --- a/README.md +++ b/README.md @@ -22,7 +22,7 @@ pip install -r requirements.txt # macOS brew tap ArmMbed/homebrew-formulae -brew install python dfu-util arm-none-eabi-gcc +brew install python dfu-util arm-none-eabi-gcc gcc@12 pip install -r requirements.txt ``` diff --git a/board/boards/black.h b/board/boards/black.h index 7c7894874e..e4d5bb8e58 100644 --- a/board/boards/black.h +++ b/board/boards/black.h @@ -191,6 +191,9 @@ const board board_black = { .has_canfd = false, .has_rtc_battery = false, .fan_max_rpm = 0U, + .adc_scale = 8862U, + .fan_stall_recovery = false, + .fan_enable_cooldown_time = 0U, .init = black_init, .enable_can_transceiver = black_enable_can_transceiver, .enable_can_transceivers = black_enable_can_transceivers, @@ -204,4 +207,4 @@ const board board_black = { .set_phone_power = unused_set_phone_power, .set_siren = unused_set_siren, .read_som_gpio = unused_read_som_gpio -}; \ No newline at end of file +}; diff --git a/board/boards/board_declarations.h b/board/boards/board_declarations.h index 4ca1fb4fe6..db5c742295 100644 --- a/board/boards/board_declarations.h +++ b/board/boards/board_declarations.h @@ -25,6 +25,9 @@ struct board { const bool has_canfd; const bool has_rtc_battery; const uint16_t fan_max_rpm; + const uint16_t adc_scale; + const bool fan_stall_recovery; + const uint8_t fan_enable_cooldown_time; board_init init; board_enable_can_transceiver enable_can_transceiver; board_enable_can_transceivers enable_can_transceivers; diff --git a/board/boards/dos.h b/board/boards/dos.h index d2e8f3df22..b6451bd6a6 100644 --- a/board/boards/dos.h +++ b/board/boards/dos.h @@ -140,12 +140,8 @@ void dos_init(void) { set_gpio_output(GPIOC, 11, 1); #ifdef ENABLE_SPI - // A4-A7: SPI - set_gpio_alternate(GPIOA, 4, GPIO_AF5_SPI1); - set_gpio_alternate(GPIOA, 5, GPIO_AF5_SPI1); - set_gpio_alternate(GPIOA, 6, GPIO_AF5_SPI1); - set_gpio_alternate(GPIOA, 7, GPIO_AF5_SPI1); - register_set_bits(&(GPIOA->OSPEEDR), GPIO_OSPEEDER_OSPEEDR4 | GPIO_OSPEEDER_OSPEEDR5 | GPIO_OSPEEDER_OSPEEDR6 | GPIO_OSPEEDER_OSPEEDR7); + // SPI init + gpio_spi_init(); #endif // C8: FAN PWM aka TIM3_CH3 @@ -219,6 +215,9 @@ const board board_dos = { .has_canfd = false, .has_rtc_battery = true, .fan_max_rpm = 6500U, + .adc_scale = 8862U, + .fan_stall_recovery = true, + .fan_enable_cooldown_time = 0U, .init = dos_init, .enable_can_transceiver = dos_enable_can_transceiver, .enable_can_transceivers = dos_enable_can_transceivers, diff --git a/board/boards/grey.h b/board/boards/grey.h index 1ee5efb0a1..ccc7787203 100644 --- a/board/boards/grey.h +++ b/board/boards/grey.h @@ -45,6 +45,9 @@ const board board_grey = { .has_canfd = false, .has_rtc_battery = false, .fan_max_rpm = 0U, + .adc_scale = 8862U, + .fan_stall_recovery = false, + .fan_enable_cooldown_time = 0U, .init = grey_init, .enable_can_transceiver = white_enable_can_transceiver, .enable_can_transceivers = white_enable_can_transceivers, diff --git a/board/boards/pedal.h b/board/boards/pedal.h index a2c51f3d59..509a4bb4b0 100644 --- a/board/boards/pedal.h +++ b/board/boards/pedal.h @@ -83,6 +83,9 @@ const board board_pedal = { .has_canfd = false, .has_rtc_battery = false, .fan_max_rpm = 0U, + .adc_scale = 8862U, + .fan_stall_recovery = false, + .fan_enable_cooldown_time = 0U, .init = pedal_init, .enable_can_transceiver = pedal_enable_can_transceiver, .enable_can_transceivers = pedal_enable_can_transceivers, diff --git a/board/boards/red.h b/board/boards/red.h index 0a333cdf4c..2be859d9f6 100644 --- a/board/boards/red.h +++ b/board/boards/red.h @@ -178,6 +178,9 @@ const board board_red = { .has_canfd = true, .has_rtc_battery = false, .fan_max_rpm = 0U, + .adc_scale = 5539U, + .fan_stall_recovery = false, + .fan_enable_cooldown_time = 0U, .init = red_init, .enable_can_transceiver = red_enable_can_transceiver, .enable_can_transceivers = red_enable_can_transceivers, diff --git a/board/boards/red_chiplet.h b/board/boards/red_chiplet.h index 463f6a2b33..c4816a9e90 100644 --- a/board/boards/red_chiplet.h +++ b/board/boards/red_chiplet.h @@ -70,13 +70,10 @@ void red_chiplet_init(void) { set_gpio_pullup(GPIOD, 3, PULL_NONE); set_gpio_mode(GPIOD, 3, MODE_OUTPUT); - //B0: 5VOUT_S + // B0: 5VOUT_S set_gpio_pullup(GPIOB, 0, PULL_NONE); set_gpio_mode(GPIOB, 0, MODE_ANALOG); - // Turn on USB load switch. - red_chiplet_set_fan_or_usb_load_switch(true); - // Initialize harness harness_init(); diff --git a/board/boards/red_v2.h b/board/boards/red_v2.h index 5b613358e2..3e8f2f2e0e 100644 --- a/board/boards/red_v2.h +++ b/board/boards/red_v2.h @@ -2,6 +2,14 @@ // Red Panda V2 with chiplet + Harness // // ///////////////////// // +void red_panda_v2_init(void) { + // common chiplet init + red_chiplet_init(); + + // Turn on USB load switch + red_chiplet_set_fan_or_usb_load_switch(true); +} + const board board_red_v2 = { .board_type = "Red_v2", .board_tick = unused_board_tick, @@ -14,7 +22,10 @@ const board board_red_v2 = { .has_canfd = true, .has_rtc_battery = true, .fan_max_rpm = 0U, - .init = red_chiplet_init, + .adc_scale = 5539U, + .fan_stall_recovery = false, + .fan_enable_cooldown_time = 0U, + .init = red_panda_v2_init, .enable_can_transceiver = red_chiplet_enable_can_transceiver, .enable_can_transceivers = red_chiplet_enable_can_transceivers, .set_led = red_set_led, diff --git a/board/boards/tres.h b/board/boards/tres.h index c1b792c414..c7c954ab5a 100644 --- a/board/boards/tres.h +++ b/board/boards/tres.h @@ -62,11 +62,7 @@ void tres_init(void) { uart_init(&uart_ring_som_debug, 115200); // SPI init - set_gpio_alternate(GPIOE, 11, GPIO_AF5_SPI4); - set_gpio_alternate(GPIOE, 12, GPIO_AF5_SPI4); - set_gpio_alternate(GPIOE, 13, GPIO_AF5_SPI4); - set_gpio_alternate(GPIOE, 14, GPIO_AF5_SPI4); - register_set_bits(&(GPIOE->OSPEEDR), GPIO_OSPEEDR_OSPEED11 | GPIO_OSPEEDR_OSPEED12 | GPIO_OSPEEDR_OSPEED13 | GPIO_OSPEEDR_OSPEED14); + gpio_spi_init(); // fan setup set_gpio_alternate(GPIOC, 8, GPIO_AF2_TIM3); @@ -97,7 +93,10 @@ const board board_tres = { .has_spi = true, .has_canfd = true, .has_rtc_battery = true, - .fan_max_rpm = 6500U, // TODO: verify this, copied from dos + .fan_max_rpm = 6600U, + .adc_scale = 3021U, + .fan_stall_recovery = false, + .fan_enable_cooldown_time = 3U, .init = tres_init, .enable_can_transceiver = red_chiplet_enable_can_transceiver, .enable_can_transceivers = red_chiplet_enable_can_transceivers, diff --git a/board/boards/uno.h b/board/boards/uno.h index feb784dd30..abf8331c6f 100644 --- a/board/boards/uno.h +++ b/board/boards/uno.h @@ -215,7 +215,7 @@ void uno_init(void) { } // Switch to phone usb mode if harness connection is powered by less than 7V - if(adc_get_voltage() < 7000U){ + if(adc_get_voltage(current_board->adc_scale) < 7000U){ uno_set_usb_switch(true); } else { uno_set_usb_switch(false); @@ -251,6 +251,9 @@ const board board_uno = { .has_canfd = false, .has_rtc_battery = true, .fan_max_rpm = 5100U, + .adc_scale = 8862U, + .fan_stall_recovery = false, + .fan_enable_cooldown_time = 0U, .init = uno_init, .enable_can_transceiver = uno_enable_can_transceiver, .enable_can_transceivers = uno_enable_can_transceivers, diff --git a/board/boards/white.h b/board/boards/white.h index 0fe817db1b..ebca3b2233 100644 --- a/board/boards/white.h +++ b/board/boards/white.h @@ -214,7 +214,7 @@ void white_grey_common_init(void) { white_set_can_mode(CAN_MODE_NORMAL); // Init usb power mode - uint32_t voltage = adc_get_voltage(); + uint32_t voltage = adc_get_voltage(current_board->adc_scale); // Init in CDP mode only if panda is powered by 12V. // Otherwise a PC would not be able to flash a standalone panda if (voltage > 8000U) { // 8V threshold @@ -247,6 +247,9 @@ const board board_white = { .has_canfd = false, .has_rtc_battery = false, .fan_max_rpm = 0U, + .adc_scale = 8862U, + .fan_stall_recovery = false, + .fan_enable_cooldown_time = 0U, .init = white_init, .enable_can_transceiver = white_enable_can_transceiver, .enable_can_transceivers = white_enable_can_transceivers, diff --git a/board/drivers/fake_siren.h b/board/drivers/fake_siren.h index 312db6cf0b..c139cb2abc 100644 --- a/board/drivers/fake_siren.h +++ b/board/drivers/fake_siren.h @@ -11,8 +11,8 @@ void fake_siren_codec_enable(bool enabled) { bool success = false; success &= i2c_set_reg_bits(I2C5, CODEC_I2C_ADDR, 0x2B, (1U << 1)); // Left speaker mix from INA1 success &= i2c_set_reg_bits(I2C5, CODEC_I2C_ADDR, 0x2C, (1U << 1)); // Right speaker mix from INA1 - success &= i2c_set_reg_mask(I2C5, CODEC_I2C_ADDR, 0x3D, 0x1F, 0b11111); // Left speaker volume - success &= i2c_set_reg_mask(I2C5, CODEC_I2C_ADDR, 0x3E, 0x1F, 0b11111); // Right speaker volume + success &= i2c_set_reg_mask(I2C5, CODEC_I2C_ADDR, 0x3D, 0x17, 0b11111); // Left speaker volume + success &= i2c_set_reg_mask(I2C5, CODEC_I2C_ADDR, 0x3E, 0x17, 0b11111); // Right speaker volume success &= i2c_set_reg_mask(I2C5, CODEC_I2C_ADDR, 0x37, 0b101, 0b111); // INA gain success &= i2c_set_reg_bits(I2C5, CODEC_I2C_ADDR, 0x4C, (1U << 7)); // Enable INA success &= i2c_set_reg_bits(I2C5, CODEC_I2C_ADDR, 0x51, (1U << 7)); // Disable global shutdown diff --git a/board/drivers/fan.h b/board/drivers/fan.h index d5e32ae5f9..cf4faa09be 100644 --- a/board/drivers/fan.h +++ b/board/drivers/fan.h @@ -5,6 +5,7 @@ struct fan_state_t { uint8_t power; float error_integral; uint8_t stall_counter; + uint8_t cooldown_counter; } fan_state_t; struct fan_state_t fan_state; @@ -15,6 +16,10 @@ void fan_set_power(uint8_t percentage){ fan_state.target_rpm = ((current_board->fan_max_rpm * MIN(100U, MAX(0U, percentage))) / 100U); } +void fan_reset_cooldown(void){ + fan_state.cooldown_counter = current_board->fan_enable_cooldown_time * 8U; +} + // Call this at 8Hz void fan_tick(void){ if (current_board->fan_max_rpm > 0U) { @@ -24,26 +29,31 @@ void fan_tick(void){ fan_state.rpm = (fan_rpm_fast + (3U * fan_state.rpm)) / 4U; // Enable fan if we want it to spin - current_board->set_fan_enabled(fan_state.target_rpm > 0U); + current_board->set_fan_enabled((fan_state.target_rpm > 0U) || (fan_state.cooldown_counter > 0U)); + if (fan_state.target_rpm > 0U) { + fan_reset_cooldown(); + } // Stall detection - if (fan_state.power > 0U) { - if (fan_rpm_fast == 0U) { - fan_state.stall_counter = MIN(fan_state.stall_counter + 1U, 255U); - } else { - fan_state.stall_counter = 0U; - } + if (current_board->fan_stall_recovery) { + if (fan_state.power > 0U) { + if (fan_rpm_fast == 0U) { + fan_state.stall_counter = MIN(fan_state.stall_counter + 1U, 255U); + } else { + fan_state.stall_counter = 0U; + } - if (fan_state.stall_counter > FAN_STALL_THRESHOLD) { - // Stall detected, power cycling fan controller - current_board->set_fan_enabled(false); + if (fan_state.stall_counter > FAN_STALL_THRESHOLD) { + // Stall detected, power cycling fan controller + current_board->set_fan_enabled(false); - // clip integral, can't fully reset otherwise we may always be stuck in stall detection - fan_state.error_integral = MIN(50.0f, MAX(0.0f, fan_state.error_integral)); + // clip integral, can't fully reset otherwise we may always be stuck in stall detection + fan_state.error_integral = MIN(50.0f, MAX(0.0f, fan_state.error_integral)); + } + } else { + fan_state.stall_counter = 0U; + fan_state.error_integral = 0.0f; } - } else { - fan_state.stall_counter = 0U; - fan_state.error_integral = 0.0f; } // Update controller @@ -55,5 +65,10 @@ void fan_tick(void){ fan_state.power = MIN(100U, MAX(0U, feedforward + fan_state.error_integral)); pwm_set(TIM3, 3, fan_state.power); + + // Cooldown counter + if (fan_state.cooldown_counter > 0U) { + fan_state.cooldown_counter--; + } } } diff --git a/board/flasher.h b/board/flasher.h index f1a4f645e9..97d81096e5 100644 --- a/board/flasher.h +++ b/board/flasher.h @@ -2,6 +2,8 @@ uint32_t *prog_ptr = NULL; bool unlocked = false; +void spi_init(void); + #ifdef uart_ring void debug_ring_callback(uart_ring *ring) {} #endif @@ -41,6 +43,11 @@ int comms_control_handler(ControlPacket_t *req, uint8_t *resp) { resp[1] = 0xff; } break; + // **** 0xc1: get hardware type + case 0xc1: + resp[0] = hw_type; + resp_len = 1; + break; // **** 0xc3: fetch MCU UID case 0xc3: #ifdef UID_BASE @@ -289,6 +296,12 @@ void soft_flasher_start(void) { // enable USB usb_init(); + // enable SPI + if (current_board->has_spi) { + gpio_spi_init(); + spi_init(); + } + // green LED on for flashing current_board->set_led(LED_GREEN, 1); diff --git a/board/main_comms.h b/board/main_comms.h index 0d3a97aa33..4b7b3c5c94 100644 --- a/board/main_comms.h +++ b/board/main_comms.h @@ -9,7 +9,7 @@ int get_health_pkt(void *dat) { struct health_t * health = (struct health_t*)dat; health->uptime_pkt = uptime_cnt; - health->voltage_pkt = adc_get_voltage(); + health->voltage_pkt = adc_get_voltage(current_board->adc_scale); health->current_pkt = current_board->read_current(); //Use the GPIO pin to determine ignition or use a CAN based logic diff --git a/board/safety.h b/board/safety.h index a33a0e6f91..45f76a3332 100644 --- a/board/safety.h +++ b/board/safety.h @@ -196,7 +196,7 @@ void update_counter(AddrCheckStruct addr_list[], int index, uint8_t counter) { bool is_msg_valid(AddrCheckStruct addr_list[], int index) { bool valid = true; if (index != -1) { - if ((!addr_list[index].valid_checksum) || (addr_list[index].wrong_counters >= MAX_WRONG_COUNTERS)) { + if (!addr_list[index].valid_checksum || !addr_list[index].valid_quality_flag || (addr_list[index].wrong_counters >= MAX_WRONG_COUNTERS)) { valid = false; controls_allowed = 0; } @@ -215,7 +215,8 @@ bool addr_safety_check(CANPacket_t *to_push, const addr_checks *rx_checks, uint32_t (*get_checksum)(CANPacket_t *to_push), uint32_t (*compute_checksum)(CANPacket_t *to_push), - uint8_t (*get_counter)(CANPacket_t *to_push)) { + uint8_t (*get_counter)(CANPacket_t *to_push), + bool (*get_quality_flag_valid)(CANPacket_t *to_push)) { int index = get_addr_check_index(to_push, rx_checks->check, rx_checks->len); update_addr_timestamp(rx_checks->check, index); @@ -237,6 +238,13 @@ bool addr_safety_check(CANPacket_t *to_push, } else { rx_checks->check[index].wrong_counters = 0U; } + + // quality flag check + if ((get_quality_flag_valid != NULL) && rx_checks->check[index].msg[rx_checks->check[index].index].quality_flag) { + rx_checks->check[index].valid_quality_flag = get_quality_flag_valid(to_push); + } else { + rx_checks->check[index].valid_quality_flag = true; + } } return is_msg_valid(rx_checks->check, index); } diff --git a/board/safety/safety_body.h b/board/safety/safety_body.h index d9a9fcd335..a4d21ef011 100644 --- a/board/safety/safety_body.h +++ b/board/safety/safety_body.h @@ -9,7 +9,7 @@ addr_checks body_rx_checks = {body_addr_checks, BODY_ADDR_CHECK_LEN}; static int body_rx_hook(CANPacket_t *to_push) { - bool valid = addr_safety_check(to_push, &body_rx_checks, NULL, NULL, NULL); + bool valid = addr_safety_check(to_push, &body_rx_checks, NULL, NULL, NULL, NULL); controls_allowed = valid; diff --git a/board/safety/safety_chrysler.h b/board/safety/safety_chrysler.h index 7c444bc0e9..694d034501 100644 --- a/board/safety/safety_chrysler.h +++ b/board/safety/safety_chrysler.h @@ -9,7 +9,7 @@ const SteeringLimits CHRYSLER_STEERING_LIMITS = { }; const SteeringLimits CHRYSLER_RAM_DT_STEERING_LIMITS = { - .max_steer = 261, + .max_steer = 350, .max_rt_delta = 112, .max_rt_interval = 250000, .max_rate_up = 6, @@ -180,7 +180,7 @@ static int chrysler_rx_hook(CANPacket_t *to_push) { bool valid = addr_safety_check(to_push, &chrysler_rx_checks, chrysler_get_checksum, chrysler_compute_checksum, - chrysler_get_counter); + chrysler_get_counter, NULL); const int bus = GET_BUS(to_push); const int addr = GET_ADDR(to_push); diff --git a/board/safety/safety_ford.h b/board/safety/safety_ford.h index 746c685d1c..69bada7c90 100644 --- a/board/safety/safety_ford.h +++ b/board/safety/safety_ford.h @@ -2,6 +2,8 @@ #define MSG_EngBrakeData 0x165 // RX from PCM, for driver brake pedal and cruise state #define MSG_EngVehicleSpThrottle 0x204 // RX from PCM, for driver throttle input #define MSG_DesiredTorqBrk 0x213 // RX from ABS, for standstill state +#define MSG_BrakeSysFeatures 0x415 // RX from ABS, for vehicle speed +#define MSG_Yaw_Data_FD1 0x91 // RX from RCM, for yaw rate #define MSG_Steering_Data_FD1 0x083 // TX by OP, various driver switches and LKAS/CC buttons #define MSG_ACCDATA_3 0x18A // TX by OP, ACC/TJA user interface #define MSG_Lane_Assist_Data1 0x3CA // TX by OP, Lane Keep Assist @@ -22,7 +24,12 @@ const CanMsg FORD_TX_MSGS[] = { }; #define FORD_TX_LEN (sizeof(FORD_TX_MSGS) / sizeof(FORD_TX_MSGS[0])) +// warning: quality flags are not yet checked in openpilot's CAN parser, +// this may be the cause of blocked messages AddrCheckStruct ford_addr_checks[] = { + {.msg = {{MSG_BrakeSysFeatures, 0, 8, .check_checksum = true, .max_counter = 15U, .quality_flag=true, .expected_timestep = 20000U}, { 0 }, { 0 }}}, + {.msg = {{MSG_Yaw_Data_FD1, 0, 8, .check_checksum = true, .max_counter = 255U, .quality_flag=true, .expected_timestep = 10000U}, { 0 }, { 0 }}}, + // These messages have no counter or checksum {.msg = {{MSG_EngBrakeData, 0, 8, .expected_timestep = 100000U}, { 0 }, { 0 }}}, {.msg = {{MSG_EngVehicleSpThrottle, 0, 8, .expected_timestep = 10000U}, { 0 }, { 0 }}}, {.msg = {{MSG_DesiredTorqBrk, 0, 8, .expected_timestep = 20000U}, { 0 }, { 0 }}}, @@ -30,6 +37,73 @@ AddrCheckStruct ford_addr_checks[] = { #define FORD_ADDR_CHECK_LEN (sizeof(ford_addr_checks) / sizeof(ford_addr_checks[0])) addr_checks ford_rx_checks = {ford_addr_checks, FORD_ADDR_CHECK_LEN}; +static uint8_t ford_get_counter(CANPacket_t *to_push) { + int addr = GET_ADDR(to_push); + + uint8_t cnt; + if (addr == MSG_BrakeSysFeatures) { + // Signal: VehVActlBrk_No_Cnt + cnt = (GET_BYTE(to_push, 2) >> 2) & 0xFU; + } else if (addr == MSG_Yaw_Data_FD1) { + // Signal: VehRollYaw_No_Cnt + cnt = GET_BYTE(to_push, 5); + } else { + cnt = 0; + } + return cnt; +} + +static uint32_t ford_get_checksum(CANPacket_t *to_push) { + int addr = GET_ADDR(to_push); + + uint8_t chksum; + if (addr == MSG_BrakeSysFeatures) { + // Signal: VehVActlBrk_No_Cs + chksum = GET_BYTE(to_push, 3); + } else if (addr == MSG_Yaw_Data_FD1) { + // Signal: VehRollYawW_No_Cs + chksum = GET_BYTE(to_push, 4); + } else { + chksum = 0; + } + return chksum; +} + +static uint32_t ford_compute_checksum(CANPacket_t *to_push) { + int addr = GET_ADDR(to_push); + + uint8_t chksum = 0; + if (addr == MSG_BrakeSysFeatures) { + chksum += GET_BYTE(to_push, 0) + GET_BYTE(to_push, 1); // Veh_V_ActlBrk + chksum += GET_BYTE(to_push, 2) >> 6; // VehVActlBrk_D_Qf + chksum += (GET_BYTE(to_push, 2) >> 2) & 0xFU; // VehVActlBrk_No_Cnt + chksum = 0xFFU - chksum; + } else if (addr == MSG_Yaw_Data_FD1) { + chksum += GET_BYTE(to_push, 0) + GET_BYTE(to_push, 1); // VehRol_W_Actl + chksum += GET_BYTE(to_push, 2) + GET_BYTE(to_push, 3); // VehYaw_W_Actl + chksum += GET_BYTE(to_push, 5); // VehRollYaw_No_Cnt + chksum += GET_BYTE(to_push, 6) >> 6; // VehRolWActl_D_Qf + chksum += (GET_BYTE(to_push, 6) >> 4) & 0x3U; // VehYawWActl_D_Qf + chksum = 0xFFU - chksum; + } else { + } + + return chksum; +} + +static bool ford_get_quality_flag_valid(CANPacket_t *to_push) { + int addr = GET_ADDR(to_push); + + bool valid = false; + if (addr == MSG_BrakeSysFeatures) { + valid = (GET_BYTE(to_push, 2) >> 6) == 0x3U; // VehVActlBrk_D_Qf + } else if (addr == MSG_Yaw_Data_FD1) { + valid = (GET_BYTE(to_push, 6) >> 4) == 0xFU; // VehRolWActl_D_Qf & VehYawWActl_D_Qf + } else { + } + return valid; +} + #define INACTIVE_CURVATURE 1000U #define INACTIVE_CURVATURE_RATE 4096U #define INACTIVE_PATH_OFFSET 512U @@ -43,7 +117,8 @@ static bool ford_lkas_msg_check(int addr) { } static int ford_rx_hook(CANPacket_t *to_push) { - bool valid = addr_safety_check(to_push, &ford_rx_checks, NULL, NULL, NULL); + bool valid = addr_safety_check(to_push, &ford_rx_checks, + ford_get_checksum, ford_compute_checksum, ford_get_counter, ford_get_quality_flag_valid); if (valid && (GET_BUS(to_push) == FORD_MAIN_BUS)) { int addr = GET_ADDR(to_push); @@ -54,6 +129,12 @@ static int ford_rx_hook(CANPacket_t *to_push) { vehicle_moving = ((GET_BYTE(to_push, 3) >> 3) & 0x3U) == 0U; } + // Update vehicle speed + if (addr == MSG_BrakeSysFeatures) { + // Signal: Veh_V_ActlBrk + vehicle_speed = ((GET_BYTE(to_push, 0) << 8) | GET_BYTE(to_push, 1)) * 0.01 / 3.6; + } + // Update gas pedal if (addr == MSG_EngVehicleSpThrottle) { // Pedal position: (0.1 * val) in percent diff --git a/board/safety/safety_gm.h b/board/safety/safety_gm.h index 3e7c2d3b90..176a4e1c63 100644 --- a/board/safety/safety_gm.h +++ b/board/safety/safety_gm.h @@ -68,7 +68,7 @@ bool gm_pcm_cruise = false; static int gm_rx_hook(CANPacket_t *to_push) { - bool valid = addr_safety_check(to_push, &gm_rx_checks, NULL, NULL, NULL); + bool valid = addr_safety_check(to_push, &gm_rx_checks, NULL, NULL, NULL, NULL); if (valid && (GET_BUS(to_push) == 0U)) { int addr = GET_ADDR(to_push); diff --git a/board/safety/safety_honda.h b/board/safety/safety_honda.h index b8af7cac20..51a908b853 100644 --- a/board/safety/safety_honda.h +++ b/board/safety/safety_honda.h @@ -119,7 +119,7 @@ static uint8_t honda_get_counter(CANPacket_t *to_push) { static int honda_rx_hook(CANPacket_t *to_push) { bool valid = addr_safety_check(to_push, &honda_rx_checks, - honda_get_checksum, honda_compute_checksum, honda_get_counter); + honda_get_checksum, honda_compute_checksum, honda_get_counter, NULL); if (valid) { const bool pcm_cruise = ((honda_hw == HONDA_BOSCH) && !honda_bosch_long) || \ diff --git a/board/safety/safety_hyundai.h b/board/safety/safety_hyundai.h index 515f683966..80e09edddc 100644 --- a/board/safety/safety_hyundai.h +++ b/board/safety/safety_hyundai.h @@ -211,7 +211,7 @@ static int hyundai_rx_hook(CANPacket_t *to_push) { bool valid = addr_safety_check(to_push, &hyundai_rx_checks, hyundai_get_checksum, hyundai_compute_checksum, - hyundai_get_counter); + hyundai_get_counter, NULL); int bus = GET_BUS(to_push); int addr = GET_ADDR(to_push); diff --git a/board/safety/safety_hyundai_canfd.h b/board/safety/safety_hyundai_canfd.h index 3e002941b4..0481273444 100644 --- a/board/safety/safety_hyundai_canfd.h +++ b/board/safety/safety_hyundai_canfd.h @@ -167,7 +167,7 @@ static uint32_t hyundai_canfd_compute_checksum(CANPacket_t *to_push) { static int hyundai_canfd_rx_hook(CANPacket_t *to_push) { bool valid = addr_safety_check(to_push, &hyundai_canfd_rx_checks, - hyundai_canfd_get_checksum, hyundai_canfd_compute_checksum, hyundai_canfd_get_counter); + hyundai_canfd_get_checksum, hyundai_canfd_compute_checksum, hyundai_canfd_get_counter, NULL); int bus = GET_BUS(to_push); int addr = GET_ADDR(to_push); diff --git a/board/safety/safety_mazda.h b/board/safety/safety_mazda.h index dae484ce34..70866fe608 100644 --- a/board/safety/safety_mazda.h +++ b/board/safety/safety_mazda.h @@ -37,7 +37,7 @@ addr_checks mazda_rx_checks = {mazda_addr_checks, MAZDA_ADDR_CHECKS_LEN}; // track msgs coming from OP so that we know what CAM msgs to drop and what to forward static int mazda_rx_hook(CANPacket_t *to_push) { - bool valid = addr_safety_check(to_push, &mazda_rx_checks, NULL, NULL, NULL); + bool valid = addr_safety_check(to_push, &mazda_rx_checks, NULL, NULL, NULL, NULL); if (valid && ((int)GET_BUS(to_push) == MAZDA_MAIN)) { int addr = GET_ADDR(to_push); diff --git a/board/safety/safety_nissan.h b/board/safety/safety_nissan.h index df7402623c..cc8048aa40 100644 --- a/board/safety/safety_nissan.h +++ b/board/safety/safety_nissan.h @@ -42,7 +42,7 @@ bool nissan_alt_eps = false; static int nissan_rx_hook(CANPacket_t *to_push) { - bool valid = addr_safety_check(to_push, &nissan_rx_checks, NULL, NULL, NULL); + bool valid = addr_safety_check(to_push, &nissan_rx_checks, NULL, NULL, NULL, NULL); if (valid) { int bus = GET_BUS(to_push); diff --git a/board/safety/safety_subaru.h b/board/safety/safety_subaru.h index 3c782e5f58..538c5a7ce1 100644 --- a/board/safety/safety_subaru.h +++ b/board/safety/safety_subaru.h @@ -82,7 +82,7 @@ static uint32_t subaru_compute_checksum(CANPacket_t *to_push) { static int subaru_rx_hook(CANPacket_t *to_push) { bool valid = addr_safety_check(to_push, &subaru_rx_checks, - subaru_get_checksum, subaru_compute_checksum, subaru_get_counter); + subaru_get_checksum, subaru_compute_checksum, subaru_get_counter, NULL); if (valid) { const int bus = GET_BUS(to_push); diff --git a/board/safety/safety_subaru_legacy.h b/board/safety/safety_subaru_legacy.h index a0b6b120d0..d84d4a6491 100644 --- a/board/safety/safety_subaru_legacy.h +++ b/board/safety/safety_subaru_legacy.h @@ -26,7 +26,7 @@ addr_checks subaru_l_rx_checks = {subaru_l_addr_checks, SUBARU_L_ADDR_CHECK_LEN} static int subaru_legacy_rx_hook(CANPacket_t *to_push) { - bool valid = addr_safety_check(to_push, &subaru_l_rx_checks, NULL, NULL, NULL); + bool valid = addr_safety_check(to_push, &subaru_l_rx_checks, NULL, NULL, NULL, NULL); if (valid && (GET_BUS(to_push) == 0U)) { int addr = GET_ADDR(to_push); diff --git a/board/safety/safety_tesla.h b/board/safety/safety_tesla.h index 6835694005..d862d8d314 100644 --- a/board/safety/safety_tesla.h +++ b/board/safety/safety_tesla.h @@ -60,7 +60,7 @@ bool tesla_stock_aeb = false; static int tesla_rx_hook(CANPacket_t *to_push) { bool valid = addr_safety_check(to_push, tesla_powertrain ? (&tesla_pt_rx_checks) : (&tesla_rx_checks), - NULL, NULL, NULL); + NULL, NULL, NULL, NULL); if(valid) { int bus = GET_BUS(to_push); diff --git a/board/safety/safety_toyota.h b/board/safety/safety_toyota.h index 5aef744a86..6206130844 100644 --- a/board/safety/safety_toyota.h +++ b/board/safety/safety_toyota.h @@ -48,9 +48,11 @@ const uint32_t TOYOTA_PARAM_OFFSET = 8U; const uint32_t TOYOTA_EPS_FACTOR = (1U << TOYOTA_PARAM_OFFSET) - 1U; const uint32_t TOYOTA_PARAM_ALT_BRAKE = 1U << TOYOTA_PARAM_OFFSET; const uint32_t TOYOTA_PARAM_STOCK_LONGITUDINAL = 2U << TOYOTA_PARAM_OFFSET; +const uint32_t TOYOTA_PARAM_LTA = 4U << TOYOTA_PARAM_OFFSET; bool toyota_alt_brake = false; bool toyota_stock_longitudinal = false; +bool toyota_lta = false; int toyota_dbc_eps_torque_factor = 100; // conversion factor for STEER_TORQUE_EPS in %: see dbc file static uint32_t toyota_compute_checksum(CANPacket_t *to_push) { @@ -71,7 +73,7 @@ static uint32_t toyota_get_checksum(CANPacket_t *to_push) { static int toyota_rx_hook(CANPacket_t *to_push) { bool valid = addr_safety_check(to_push, &toyota_rx_checks, - toyota_get_checksum, toyota_compute_checksum, NULL); + toyota_get_checksum, toyota_compute_checksum, NULL, NULL); if (valid && (GET_BUS(to_push) == 0U)) { int addr = GET_ADDR(to_push); @@ -188,14 +190,15 @@ static int toyota_tx_hook(CANPacket_t *to_send) { // LTA steering check // only sent to prevent dash errors, no actuation is accepted if (addr == 0x191) { - // check the STEER_REQUEST, STEER_REQUEST_2, and STEER_ANGLE_CMD signals + // check the STEER_REQUEST, STEER_REQUEST_2, SETME_X64 STEER_ANGLE_CMD signals bool lta_request = (GET_BYTE(to_send, 0) & 1U) != 0U; bool lta_request2 = ((GET_BYTE(to_send, 3) >> 1) & 1U) != 0U; + int setme_x64 = GET_BYTE(to_send, 5); int lta_angle = (GET_BYTE(to_send, 1) << 8) | GET_BYTE(to_send, 2); lta_angle = to_signed(lta_angle, 16); // block LTA msgs with actuation requests - if (lta_request || lta_request2 || (lta_angle != 0)) { + if (lta_request || lta_request2 || (lta_angle != 0) || (setme_x64 != 0)) { tx = 0; } } @@ -208,6 +211,10 @@ static int toyota_tx_hook(CANPacket_t *to_send) { if (steer_torque_cmd_checks(desired_torque, steer_req, TOYOTA_STEERING_LIMITS)) { tx = 0; } + // When using LTA (angle control), assert no actuation on LKA message + if (toyota_lta && ((desired_torque != 0) || steer_req)) { + tx = 0; + } } } @@ -219,6 +226,12 @@ static const addr_checks* toyota_init(uint16_t param) { toyota_alt_brake = GET_FLAG(param, TOYOTA_PARAM_ALT_BRAKE); toyota_stock_longitudinal = GET_FLAG(param, TOYOTA_PARAM_STOCK_LONGITUDINAL); toyota_dbc_eps_torque_factor = param & TOYOTA_EPS_FACTOR; + +#ifdef ALLOW_DEBUG + toyota_lta = GET_FLAG(param, TOYOTA_PARAM_LTA); +#else + toyota_lta = false; +#endif return &toyota_rx_checks; } diff --git a/board/safety/safety_volkswagen_mqb.h b/board/safety/safety_volkswagen_mqb.h index 7c22e9f0fd..afedc20a18 100644 --- a/board/safety/safety_volkswagen_mqb.h +++ b/board/safety/safety_volkswagen_mqb.h @@ -115,7 +115,7 @@ static const addr_checks* volkswagen_mqb_init(uint16_t param) { static int volkswagen_mqb_rx_hook(CANPacket_t *to_push) { bool valid = addr_safety_check(to_push, &volkswagen_mqb_rx_checks, - volkswagen_mqb_get_checksum, volkswagen_mqb_compute_crc, volkswagen_mqb_get_counter); + volkswagen_mqb_get_checksum, volkswagen_mqb_compute_crc, volkswagen_mqb_get_counter, NULL); if (valid && (GET_BUS(to_push) == 0U)) { int addr = GET_ADDR(to_push); diff --git a/board/safety/safety_volkswagen_pq.h b/board/safety/safety_volkswagen_pq.h index 20b9da63de..2cacb1227f 100644 --- a/board/safety/safety_volkswagen_pq.h +++ b/board/safety/safety_volkswagen_pq.h @@ -28,14 +28,14 @@ const LongitudinalLimits VOLKSWAGEN_PQ_LONG_LIMITS = { #define MSG_MOTOR_3 0x380 // RX from ECU, for driver throttle input #define MSG_GRA_NEU 0x38A // TX by OP, ACC control buttons for cancel/resume #define MSG_MOTOR_5 0x480 // RX from ECU, for ACC main switch state -#define MSG_ACC_GRA_ANZIEGE 0x56A // TX by OP, ACC HUD +#define MSG_ACC_GRA_ANZEIGE 0x56A // TX by OP, ACC HUD #define MSG_LDW_1 0x5BE // TX by OP, Lane line recognition and text alerts // Transmit of GRA_Neu is allowed on bus 0 and 2 to keep compatibility with gateway and camera integration const CanMsg VOLKSWAGEN_PQ_STOCK_TX_MSGS[] = {{MSG_HCA_1, 0, 5}, {MSG_LDW_1, 0, 8}, {MSG_GRA_NEU, 0, 4}, {MSG_GRA_NEU, 2, 4}}; const CanMsg VOLKSWAGEN_PQ_LONG_TX_MSGS[] = {{MSG_HCA_1, 0, 5}, {MSG_LDW_1, 0, 8}, - {MSG_ACC_SYSTEM, 0, 8}, {MSG_ACC_GRA_ANZIEGE, 0, 8}}; + {MSG_ACC_SYSTEM, 0, 8}, {MSG_ACC_GRA_ANZEIGE, 0, 8}}; AddrCheckStruct volkswagen_pq_addr_checks[] = { {.msg = {{MSG_LENKHILFE_3, 0, 6, .check_checksum = true, .max_counter = 15U, .expected_timestep = 10000U}, { 0 }, { 0 }}}, @@ -100,7 +100,7 @@ static const addr_checks* volkswagen_pq_init(uint16_t param) { static int volkswagen_pq_rx_hook(CANPacket_t *to_push) { bool valid = addr_safety_check(to_push, &volkswagen_pq_rx_checks, - volkswagen_pq_get_checksum, volkswagen_pq_compute_checksum, volkswagen_pq_get_counter); + volkswagen_pq_get_checksum, volkswagen_pq_compute_checksum, volkswagen_pq_get_counter, NULL); if (valid && (GET_BUS(to_push) == 0U)) { int addr = GET_ADDR(to_push); @@ -240,7 +240,7 @@ static int volkswagen_pq_fwd_hook(int bus_num, CANPacket_t *to_fwd) { if ((addr == MSG_HCA_1) || (addr == MSG_LDW_1)) { // openpilot takes over LKAS steering control and related HUD messages from the camera bus_fwd = -1; - } else if (volkswagen_longitudinal && ((addr == MSG_ACC_SYSTEM) || (addr == MSG_ACC_GRA_ANZIEGE))) { + } else if (volkswagen_longitudinal && ((addr == MSG_ACC_SYSTEM) || (addr == MSG_ACC_GRA_ANZEIGE))) { // openpilot takes over acceleration/braking control and related HUD messages from the stock ACC radar } else { // Forward all remaining traffic from Extended CAN devices to J533 gateway diff --git a/board/safety_declarations.h b/board/safety_declarations.h index 7bc1175150..49c8db03d4 100644 --- a/board/safety_declarations.h +++ b/board/safety_declarations.h @@ -85,6 +85,7 @@ typedef struct { const int len; const bool check_checksum; // true is checksum check is performed const uint8_t max_counter; // maximum value of the counter. 0 means that the counter check is skipped + const bool quality_flag; // true is quality flag check is performed const uint32_t expected_timestep; // expected time between message updates [us] } CanMsgCheck; @@ -97,6 +98,7 @@ typedef struct { int index; // if multiple messages are allowed to be checked, this stores the index of the first one seen. only msg[msg_index] will be used bool valid_checksum; // true if and only if checksum check is passed int wrong_counters; // counter of wrong counters, saturated between 0 and MAX_WRONG_COUNTERS + bool valid_quality_flag; // true if the message's quality/health/status signals are valid uint8_t last_counter; // last counter value uint32_t last_timestamp; // micro-s bool lagging; // true if and only if the time between updates is excessive @@ -133,7 +135,8 @@ bool addr_safety_check(CANPacket_t *to_push, const addr_checks *rx_checks, uint32_t (*get_checksum)(CANPacket_t *to_push), uint32_t (*compute_checksum)(CANPacket_t *to_push), - uint8_t (*get_counter)(CANPacket_t *to_push)); + uint8_t (*get_counter)(CANPacket_t *to_push), + bool (*get_quality_flag_valid)(CANPacket_t *to_push)); void generic_rx_checks(bool stock_ecu_detected); void relay_malfunction_set(void); void relay_malfunction_reset(void); diff --git a/board/stm32fx/lladc.h b/board/stm32fx/lladc.h index 2ae14a2dcb..0d3e8e028a 100644 --- a/board/stm32fx/lladc.h +++ b/board/stm32fx/lladc.h @@ -28,7 +28,7 @@ uint32_t adc_get(unsigned int channel) { return ADC1->JDR1; } -uint32_t adc_get_voltage(void) { +uint32_t adc_get_voltage(uint16_t scale) { // REVC has a 10, 1 (1/11) voltage divider // Here is the calculation for the scale (s) // ADCV = VIN_S * (1/11) * (4095/3.3) @@ -36,5 +36,5 @@ uint32_t adc_get_voltage(void) { // s = 1000/((4095/3.3)*(1/11)) = 8.8623046875 // Avoid needing floating point math, so output in mV - return (adc_get(ADCCHAN_VOLTAGE) * 8862U) / 1000U; + return (adc_get(ADCCHAN_VOLTAGE) * scale) / 1000U; } diff --git a/board/stm32fx/llfan.h b/board/stm32fx/llfan.h index 3e4b0fa3fd..f66fc187ff 100644 --- a/board/stm32fx/llfan.h +++ b/board/stm32fx/llfan.h @@ -2,12 +2,14 @@ void EXTI2_IRQ_Handler(void) { volatile unsigned int pr = EXTI->PR & (1U << 2); if ((pr & (1U << 2)) != 0U) { - fan_state.tach_counter++; + fan_state.tach_counter++; } EXTI->PR = (1U << 2); } void llfan_init(void) { + fan_reset_cooldown(); + // 5000RPM * 4 tach edges / 60 seconds REGISTER_INTERRUPT(EXTI2_IRQn, EXTI2_IRQ_Handler, 700U, FAULT_INTERRUPT_RATE_TACH) diff --git a/board/stm32fx/peripherals.h b/board/stm32fx/peripherals.h index 40ef6cb573..3eeebf0eb6 100644 --- a/board/stm32fx/peripherals.h +++ b/board/stm32fx/peripherals.h @@ -5,6 +5,15 @@ void gpio_usb_init(void) { GPIOA->OSPEEDR = GPIO_OSPEEDER_OSPEEDR11 | GPIO_OSPEEDER_OSPEEDR12; } +void gpio_spi_init(void) { + // A4-A7: SPI + set_gpio_alternate(GPIOA, 4, GPIO_AF5_SPI1); + set_gpio_alternate(GPIOA, 5, GPIO_AF5_SPI1); + set_gpio_alternate(GPIOA, 6, GPIO_AF5_SPI1); + set_gpio_alternate(GPIOA, 7, GPIO_AF5_SPI1); + register_set_bits(&(GPIOA->OSPEEDR), GPIO_OSPEEDER_OSPEEDR4 | GPIO_OSPEEDER_OSPEEDR5 | GPIO_OSPEEDER_OSPEEDR6 | GPIO_OSPEEDER_OSPEEDR7); +} + void gpio_usart2_init(void) { // A2,A3: USART 2 for debugging set_gpio_alternate(GPIOA, 2, GPIO_AF7_USART2); diff --git a/board/stm32fx/stm32fx_config.h b/board/stm32fx/stm32fx_config.h index 0661606932..073cb07774 100644 --- a/board/stm32fx/stm32fx_config.h +++ b/board/stm32fx/stm32fx_config.h @@ -67,7 +67,7 @@ #include "stm32fx/board.h" #include "stm32fx/clock.h" -#ifdef ENABLE_SPI +#if defined(PANDA) || defined(BOOTSTUB) #include "drivers/spi.h" #include "stm32fx/llspi.h" #endif diff --git a/board/stm32h7/lladc.h b/board/stm32h7/lladc.h index 795ca3533f..3abfef96b7 100644 --- a/board/stm32h7/lladc.h +++ b/board/stm32h7/lladc.h @@ -21,7 +21,7 @@ uint32_t adc_get(unsigned int channel) { ADC1->SQR1 &= ~(ADC_SQR1_L); ADC1->SQR1 = (channel << 6U); - + ADC1->SMPR1 = (0x7U << (channel * 3U) ); ADC1->PCSEL_RES0 = (0x1U << channel); @@ -36,13 +36,14 @@ uint32_t adc_get(unsigned int channel) { return res; } -uint32_t adc_get_voltage(void) { +uint32_t adc_get_voltage(uint16_t scale) { // REVC has a 10, 1 (1/11) voltage divider // Here is the calculation for the scale (s) // ADCV = VIN_S * (1/11) * (65535/3.3) // RETVAL = ADCV * s = VIN_S*1000 // s = 1000/((65535/3.3)*(1/11)) = 0.553902494 + // s = 1000/((65535/1.8)*(1/11)) = 0.3021 // Avoid needing floating point math, so output in mV - return (adc_get(ADCCHAN_VOLTAGE) * 5539U) / 10000U; + return (adc_get(ADCCHAN_VOLTAGE) * scale) / 10000U; } diff --git a/board/stm32h7/llfan.h b/board/stm32h7/llfan.h index dce622503a..bbcda63e09 100644 --- a/board/stm32h7/llfan.h +++ b/board/stm32h7/llfan.h @@ -8,6 +8,8 @@ void EXTI2_IRQ_Handler(void) { } void llfan_init(void) { + fan_reset_cooldown(); + // 5000RPM * 4 tach edges / 60 seconds REGISTER_INTERRUPT(EXTI2_IRQn, EXTI2_IRQ_Handler, 700U, FAULT_INTERRUPT_RATE_TACH) diff --git a/board/stm32h7/peripherals.h b/board/stm32h7/peripherals.h index 754f157ae3..7e3b2e5b16 100644 --- a/board/stm32h7/peripherals.h +++ b/board/stm32h7/peripherals.h @@ -1,10 +1,18 @@ void gpio_usb_init(void) { - // A11,A12: USB: + // A11,A12: USB set_gpio_alternate(GPIOA, 11, GPIO_AF10_OTG1_FS); set_gpio_alternate(GPIOA, 12, GPIO_AF10_OTG1_FS); GPIOA->OSPEEDR = GPIO_OSPEEDR_OSPEED11 | GPIO_OSPEEDR_OSPEED12; } +void gpio_spi_init(void) { + set_gpio_alternate(GPIOE, 11, GPIO_AF5_SPI4); + set_gpio_alternate(GPIOE, 12, GPIO_AF5_SPI4); + set_gpio_alternate(GPIOE, 13, GPIO_AF5_SPI4); + set_gpio_alternate(GPIOE, 14, GPIO_AF5_SPI4); + register_set_bits(&(GPIOE->OSPEEDR), GPIO_OSPEEDR_OSPEED11 | GPIO_OSPEEDR_OSPEED12 | GPIO_OSPEEDR_OSPEED13 | GPIO_OSPEEDR_OSPEED14); +} + void gpio_usart2_init(void) { // A2,A3: USART 2 for debugging set_gpio_alternate(GPIOA, 2, GPIO_AF7_USART2); @@ -85,7 +93,10 @@ void common_init_gpio(void) { void flasher_peripherals_init(void) { RCC->AHB1ENR |= RCC_AHB1ENR_USB1OTGHSEN; + + // SPI + DMA RCC->APB2ENR |= RCC_APB2ENR_SPI4EN; + RCC->AHB1ENR |= RCC_AHB1ENR_DMA2EN; } // Peripheral initialization diff --git a/python/__init__.py b/python/__init__.py index 180b305190..8920fe8638 100644 --- a/python/__init__.py +++ b/python/__init__.py @@ -13,6 +13,7 @@ from typing import Optional from itertools import accumulate +from .base import BaseHandle from .constants import McuType from .dfu import PandaDFU from .isotp import isotp_send, isotp_recv @@ -25,12 +26,12 @@ LOGLEVEL = os.environ.get('LOGLEVEL', 'INFO').upper() logging.basicConfig(level=LOGLEVEL, format='%(message)s') - USBPACKET_MAX_SIZE = 0x40 CANPACKET_HEAD_SIZE = 0x6 DLC_TO_LEN = [0, 1, 2, 3, 4, 5, 6, 7, 8, 12, 16, 20, 24, 32, 48, 64] LEN_TO_DLC = {length: dlc for (dlc, length) in enumerate(DLC_TO_LEN)} + def calculate_checksum(data): res = 0 for b in data: @@ -196,6 +197,7 @@ class Panda: # first byte is for EPS scaling factor FLAG_TOYOTA_ALT_BRAKE = (1 << 8) FLAG_TOYOTA_STOCK_LONGITUDINAL = (2 << 8) + FLAG_TOYOTA_LTA = (4 << 8) FLAG_HONDA_ALT_BRAKE = 1 FLAG_HONDA_BOSCH_LONG = 2 @@ -225,10 +227,11 @@ class Panda: FLAG_GM_HW_CAM_LONG = 2 def __init__(self, serial: Optional[str] = None, claim: bool = True, disable_checks: bool = True): - self._serial = serial + self._connect_serial = serial self._disable_checks = disable_checks - self._handle = None + self._handle: BaseHandle + self._handle_open = False self.can_rx_overflow_buffer = b'' # connect and set mcu type @@ -244,18 +247,17 @@ def __exit__(self, *args): self.close() def close(self): - self._handle.close() - self._handle = None + if self._handle_open: + self._handle.close() + self._handle_open = False def connect(self, claim=True, wait=False): - if self._handle is not None: - self.close() - self._handle = None + self.close() # try USB first, then SPI - self._handle, serial, self.bootstub, bcd = self.usb_connect(self._serial, claim=claim, wait=wait) + self._handle, serial, self.bootstub, bcd = self.usb_connect(self._connect_serial, claim=claim, wait=wait) if self._handle is None: - self._handle, serial, self.bootstub, bcd = self.spi_connect(self._serial) + self._handle, serial, self.bootstub, bcd = self.spi_connect(self._connect_serial) if self._handle is None: raise Exception("failed to connect to panda") @@ -278,6 +280,8 @@ def connect(self, claim=True, wait=False): self._assume_f4_mcu = (self._bcd_hw_type is None) and missing_hw_type_endpoint self._serial = serial + self._connect_serial = serial + self._handle_open = True self._mcu_type = self.get_mcu_type() self.health_version, self.can_version, self.can_health_version = self.get_packets_versions() logging.debug("connected") @@ -292,20 +296,22 @@ def spi_connect(serial): # get UID to confirm slave is present and up handle = None spi_serial = None + bootstub = None try: handle = PandaSpiHandle() - dat = handle.controlRead(Panda.REQUEST_IN, 0xc3, 0, 0, 12) + dat = handle.controlRead(Panda.REQUEST_IN, 0xc3, 0, 0, 12, timeout=100) spi_serial = binascii.hexlify(dat).decode() + bootstub = Panda.flasher_present(handle) except PandaSpiException: pass # no connection or wrong panda - if spi_serial is None or (serial is not None and (spi_serial != serial)): + if None in (spi_serial, bootstub) or (serial is not None and (spi_serial != serial)): handle = None spi_serial = None + bootstub = False - # TODO: detect bootstub - return handle, spi_serial, False, None + return handle, spi_serial, bootstub, None @staticmethod def usb_connect(serial, claim=True, wait=False): @@ -397,7 +403,7 @@ def reset(self, enter_bootstub=False, enter_bootloader=False, reconnect=True): self.reconnect() def reconnect(self): - if self._handle is not None: + if self._handle_open: self.close() time.sleep(1.0) @@ -419,13 +425,17 @@ def reconnect(self): if not success: raise Exception("reconnect failed") + @staticmethod + def flasher_present(handle: BaseHandle) -> bool: + fr = handle.controlRead(Panda.REQUEST_IN, 0xb0, 0, 0, 0xc) + return fr[4:8] == b"\xde\xad\xd0\x0d" + @staticmethod def flash_static(handle, code, mcu_type): assert mcu_type is not None, "must set valid mcu_type to flash" # confirm flasher is present - fr = handle.controlRead(Panda.REQUEST_IN, 0xb0, 0, 0, 0xc) - assert fr[4:8] == b"\xde\xad\xd0\x0d" + assert Panda.flasher_present(handle) # determine sectors to erase apps_sectors_cumsum = accumulate(mcu_type.config.sector_sizes[1:]) @@ -506,6 +516,11 @@ def wait_for_dfu(dfu_serial: str, timeout: Optional[int] = None) -> bool: return False return True + def up_to_date(self) -> bool: + current = self.get_signature() + expected = Panda.get_signature_from_firmware(self.get_mcu_type().config.app_path) + return (current == expected) + def call_control_api(self, msg): self._handle.controlWrite(Panda.REQUEST_OUT, msg, 0, 0, b'') @@ -597,7 +612,7 @@ def get_signature_from_firmware(fn) -> bytes: f.seek(-128, 2) # Seek from end of file return f.read(128) - def get_signature(self): + def get_signature(self) -> bytes: part_1 = self._handle.controlRead(Panda.REQUEST_IN, 0xd3, 0, 0, 0x40) part_2 = self._handle.controlRead(Panda.REQUEST_IN, 0xd4, 0, 0, 0x40) return bytes(part_1 + part_2) diff --git a/python/base.py b/python/base.py index d051e8770e..5bfa564892 100644 --- a/python/base.py +++ b/python/base.py @@ -1,25 +1,67 @@ from abc import ABC, abstractmethod from typing import List +from .constants import McuType + +TIMEOUT = int(15 * 1e3) # default timeout, in milliseconds -# This mimics the handle given by libusb1 for easy interoperability class BaseHandle(ABC): + """ + A handle to talk to a panda. + Borrows heavily from the libusb1 handle API. + """ + @abstractmethod def close(self) -> None: ... @abstractmethod - def controlWrite(self, request_type: int, request: int, value: int, index: int, data, timeout: int = 0) -> int: + def controlWrite(self, request_type: int, request: int, value: int, index: int, data, timeout: int = TIMEOUT) -> int: + ... + + @abstractmethod + def controlRead(self, request_type: int, request: int, value: int, index: int, length: int, timeout: int = TIMEOUT) -> bytes: + ... + + @abstractmethod + def bulkWrite(self, endpoint: int, data: List[int], timeout: int = TIMEOUT) -> int: ... @abstractmethod - def controlRead(self, request_type: int, request: int, value: int, index: int, length: int, timeout: int = 0) -> bytes: + def bulkRead(self, endpoint: int, length: int, timeout: int = TIMEOUT) -> bytes: ... + +class BaseSTBootloaderHandle(ABC): + """ + A handle to talk to a panda while it's in the STM32 bootloader. + """ + @abstractmethod - def bulkWrite(self, endpoint: int, data: List[int], timeout: int = 0) -> int: + def get_mcu_type(self) -> McuType: + ... + + @abstractmethod + def close(self) -> None: ... @abstractmethod - def bulkRead(self, endpoint: int, length: int, timeout: int = 0) -> bytes: + def clear_status(self) -> None: ... + + @abstractmethod + def program(self, address: int, dat: bytes) -> None: + ... + + @abstractmethod + def erase_app(self) -> None: + ... + + @abstractmethod + def erase_bootstub(self) -> None: + ... + + @abstractmethod + def jump(self, address: int) -> None: + ... + diff --git a/python/constants.py b/python/constants.py index d964192c8d..c55fd2c9b8 100644 --- a/python/constants.py +++ b/python/constants.py @@ -8,6 +8,7 @@ class McuConfig(NamedTuple): mcu: str mcu_idcode: int + uid_address: int block_size: int sector_sizes: List[int] serial_number_address: int @@ -17,6 +18,7 @@ class McuConfig(NamedTuple): bootstub_path: str Fx = ( + 0x1FFF7A10, 0x800, [0x4000 for _ in range(4)] + [0x10000] + [0x20000 for _ in range(11)], 0x1FFF79C0, @@ -31,6 +33,7 @@ class McuConfig(NamedTuple): H7Config = McuConfig( "STM32H7", 0x483, + 0x1FF1E800, 0x400, # there is an 8th sector, but we use that for the provisioning chunk, so don't program over that! [0x20000 for _ in range(7)], @@ -50,3 +53,5 @@ class McuType(enum.Enum): @property def config(self): return self.value + +MCU_TYPE_BY_IDCODE = {m.config.mcu_idcode: m for m in McuType} diff --git a/python/dfu.py b/python/dfu.py index dfdec1fd30..15e018c5ba 100644 --- a/python/dfu.py +++ b/python/dfu.py @@ -1,20 +1,31 @@ import usb1 import struct import binascii +from typing import List, Optional +from .base import BaseSTBootloaderHandle +from .spi import STBootloaderSPIHandle, PandaSpiException +from .usb import STBootloaderUSBHandle from .constants import McuType -# *** DFU mode *** -DFU_DNLOAD = 1 -DFU_UPLOAD = 2 -DFU_GETSTATUS = 3 -DFU_CLRSTATUS = 4 -DFU_ABORT = 6 - class PandaDFU: - def __init__(self, dfu_serial): - self._handle = None + def __init__(self, dfu_serial: Optional[str]): + # try USB, then SPI + handle: Optional[BaseSTBootloaderHandle] + handle = PandaDFU.usb_connect(dfu_serial) + if handle is None: + handle = PandaDFU.spi_connect(dfu_serial) + + if handle is None: + raise Exception(f"failed to open DFU device {dfu_serial}") + + self._handle: BaseSTBootloaderHandle = handle + self._mcu_type: McuType = self._handle.get_mcu_type() + + @staticmethod + def usb_connect(dfu_serial: Optional[str]) -> Optional[STBootloaderUSBHandle]: + handle = None context = usb1.USBContext() for device in context.getDeviceList(skip_on_error=True): if device.getVendorID() == 0x0483 and device.getProductID() == 0xdf11: @@ -22,16 +33,37 @@ def __init__(self, dfu_serial): this_dfu_serial = device.open().getASCIIStringDescriptor(3) except Exception: continue + if this_dfu_serial == dfu_serial or dfu_serial is None: - self._handle = device.open() - self._mcu_type = self.get_mcu_type(device) + handle = STBootloaderUSBHandle(device, device.open()) break - if self._handle is None: - raise Exception(f"failed to open DFU device {dfu_serial}") + return handle + + @staticmethod + def spi_connect(dfu_serial: Optional[str]) -> Optional[STBootloaderSPIHandle]: + handle = None + this_dfu_serial = None + + try: + handle = STBootloaderSPIHandle() + this_dfu_serial = PandaDFU.st_serial_to_dfu_serial(handle.get_uid(), handle.get_mcu_type()) + except PandaSpiException: + handle = None + + if dfu_serial is not None and dfu_serial != this_dfu_serial: + handle = None + + return handle @staticmethod - def list(): + def list() -> List[str]: + ret = PandaDFU.usb_list() + ret += PandaDFU.spi_list() + return list(set(ret)) + + @staticmethod + def usb_list() -> List[str]: context = usb1.USBContext() dfu_serials = [] try: @@ -46,7 +78,18 @@ def list(): return dfu_serials @staticmethod - def st_serial_to_dfu_serial(st, mcu_type=McuType.F4): + def spi_list() -> List[str]: + try: + h = PandaDFU.spi_connect(None) + if h is not None: + dfu_serial = PandaDFU.st_serial_to_dfu_serial(h.get_uid(), h.get_mcu_type()) + return [dfu_serial, ] + except PandaSpiException: + pass + return [] + + @staticmethod + def st_serial_to_dfu_serial(st: str, mcu_type: McuType = McuType.F4): if st is None or st == "none": return None uid_base = struct.unpack("H" * 6, bytes.fromhex(st)) @@ -55,52 +98,17 @@ def st_serial_to_dfu_serial(st, mcu_type=McuType.F4): else: return binascii.hexlify(struct.pack("!HHH", uid_base[1] + uid_base[5], uid_base[0] + uid_base[4] + 0xA, uid_base[3])).upper().decode("utf-8") - def get_mcu_type(self, dev) -> McuType: - # TODO: Find a way to detect F4 vs F2 - # TODO: also check F4 BCD, don't assume in else - return McuType.H7 if dev.getbcdDevice() == 512 else McuType.F4 - - def status(self): - while 1: - dat = self._handle.controlRead(0x21, DFU_GETSTATUS, 0, 0, 6) - if dat[1] == 0: - break - - def clear_status(self): - # Clear status - stat = self._handle.controlRead(0x21, DFU_GETSTATUS, 0, 0, 6) - if stat[4] == 0xa: - self._handle.controlRead(0x21, DFU_CLRSTATUS, 0, 0, 0) - elif stat[4] == 0x9: - self._handle.controlWrite(0x21, DFU_ABORT, 0, 0, b"") - self.status() - stat = str(self._handle.controlRead(0x21, DFU_GETSTATUS, 0, 0, 6)) - - def erase(self, address): - self._handle.controlWrite(0x21, DFU_DNLOAD, 0, 0, b"\x41" + struct.pack("I", address)) - self.status() - - def program(self, address, dat, block_size=None): - if block_size is None: - block_size = len(dat) - - # Set Address Pointer - self._handle.controlWrite(0x21, DFU_DNLOAD, 0, 0, b"\x21" + struct.pack("I", address)) - self.status() - - # Program - dat += b"\xFF" * ((block_size - len(dat)) % block_size) - for i in range(0, len(dat) // block_size): - ldat = dat[i * block_size:(i + 1) * block_size] - print("programming %d with length %d" % (i, len(ldat))) - self._handle.controlWrite(0x21, DFU_DNLOAD, 2 + i, 0, ldat) - self.status() + def get_mcu_type(self) -> McuType: + return self._mcu_type + + def reset(self): + self._handle.jump(self._mcu_type.config.bootstub_address) def program_bootstub(self, code_bootstub): - self.clear_status() - self.erase(self._mcu_type.config.bootstub_address) - self.erase(self._mcu_type.config.app_address) - self.program(self._mcu_type.config.bootstub_address, code_bootstub, self._mcu_type.config.block_size) + self._handle.clear_status() + self._handle.erase_bootstub() + self._handle.erase_app() + self._handle.program(self._mcu_type.config.bootstub_address, code_bootstub) self.reset() def recover(self): @@ -108,11 +116,3 @@ def recover(self): code = f.read() self.program_bootstub(code) - def reset(self): - self._handle.controlWrite(0x21, DFU_DNLOAD, 0, 0, b"\x21" + struct.pack("I", self._mcu_type.config.bootstub_address)) - self.status() - try: - self._handle.controlWrite(0x21, DFU_DNLOAD, 2, 0, b"") - _ = str(self._handle.controlRead(0x21, DFU_GETSTATUS, 0, 0, 6)) - except Exception: - pass diff --git a/python/spi.py b/python/spi.py index 8b3cab1056..0ff15c3bc3 100644 --- a/python/spi.py +++ b/python/spi.py @@ -1,3 +1,4 @@ +import binascii import os import fcntl import math @@ -7,9 +8,10 @@ import threading from contextlib import contextmanager from functools import reduce -from typing import List +from typing import List, Optional -from .base import BaseHandle +from .base import BaseHandle, BaseSTBootloaderHandle, TIMEOUT +from .constants import McuType, MCU_TYPE_BY_IDCODE try: import spidev @@ -23,7 +25,7 @@ NACK = 0x1F CHECKSUM_START = 0xAB -ACK_TIMEOUT_SECONDS = 0.1 +MIN_ACK_TIMEOUT_MS = 100 MAX_XFER_RETRY_COUNT = 5 USB_MAX_SIZE = 0x40 @@ -94,9 +96,11 @@ def _calc_checksum(self, data: List[int]) -> int: cksum ^= b return cksum - def _wait_for_ack(self, spi, ack_val: int) -> None: + def _wait_for_ack(self, spi, ack_val: int, timeout: int) -> None: + timeout_s = max(MIN_ACK_TIMEOUT_MS, timeout) * 1e-3 + start = time.monotonic() - while (time.monotonic() - start) < ACK_TIMEOUT_SECONDS: + while (timeout == 0) or ((time.monotonic() - start) < timeout_s): dat = spi.xfer2(b"\x12")[0] if dat == NACK: raise PandaSpiNackResponse @@ -105,7 +109,7 @@ def _wait_for_ack(self, spi, ack_val: int) -> None: raise PandaSpiMissingAck - def _transfer(self, spi, endpoint: int, data, max_rx_len: int = 1000) -> bytes: + def _transfer(self, spi, endpoint: int, data, timeout: int, max_rx_len: int = 1000) -> bytes: logging.debug("starting transfer: endpoint=%d, max_rx_len=%d", endpoint, max_rx_len) logging.debug("==============================================") @@ -119,7 +123,7 @@ def _transfer(self, spi, endpoint: int, data, max_rx_len: int = 1000) -> bytes: spi.xfer2(packet) logging.debug("- waiting for header ACK") - self._wait_for_ack(spi, HACK) + self._wait_for_ack(spi, HACK, timeout) # send data logging.debug("- sending data") @@ -127,11 +131,13 @@ def _transfer(self, spi, endpoint: int, data, max_rx_len: int = 1000) -> bytes: spi.xfer2(packet) logging.debug("- waiting for data ACK") - self._wait_for_ack(spi, DACK) + self._wait_for_ack(spi, DACK, timeout) # get response length, then response response_len_bytes = bytes(spi.xfer2(b"\x00" * 2)) response_len = struct.unpack(" max_rx_len: + raise PandaSpiException("response length greater than max") logging.debug("- receiving response") dat = bytes(spi.xfer2(b"\x00" * (response_len + 1))) @@ -148,27 +154,160 @@ def _transfer(self, spi, endpoint: int, data, max_rx_len: int = 1000) -> bytes: def close(self): self.dev.close() - def controlWrite(self, request_type: int, request: int, value: int, index: int, data, timeout: int = 0): + def controlWrite(self, request_type: int, request: int, value: int, index: int, data, timeout: int = TIMEOUT): with self.dev.acquire() as spi: - return self._transfer(spi, 0, struct.pack(" int: + def bulkWrite(self, endpoint: int, data: List[int], timeout: int = TIMEOUT) -> int: with self.dev.acquire() as spi: for x in range(math.ceil(len(data) / USB_MAX_SIZE)): - self._transfer(spi, endpoint, data[USB_MAX_SIZE*x:USB_MAX_SIZE*(x+1)]) + self._transfer(spi, endpoint, data[USB_MAX_SIZE*x:USB_MAX_SIZE*(x+1)], timeout) return len(data) - def bulkRead(self, endpoint: int, length: int, timeout: int = 0) -> bytes: + def bulkRead(self, endpoint: int, length: int, timeout: int = TIMEOUT) -> bytes: ret: List[int] = [] with self.dev.acquire() as spi: for _ in range(math.ceil(length / USB_MAX_SIZE)): - d = self._transfer(spi, endpoint, [], max_rx_len=USB_MAX_SIZE) + d = self._transfer(spi, endpoint, [], timeout, max_rx_len=USB_MAX_SIZE) ret += d if len(d) < USB_MAX_SIZE: break return bytes(ret) + + +class STBootloaderSPIHandle(BaseSTBootloaderHandle): + """ + Implementation of the STM32 SPI bootloader protocol described in: + https://www.st.com/resource/en/application_note/an4286-spi-protocol-used-in-the-stm32-bootloader-stmicroelectronics.pdf + """ + + SYNC = 0x5A + ACK = 0x79 + NACK = 0x1F + + def __init__(self): + self.dev = SpiDevice(speed=1000000) + + # say hello + try: + with self.dev.acquire() as spi: + spi.xfer([self.SYNC, ]) + try: + self._get_ack(spi) + except PandaSpiNackResponse: + # NACK ok here, will only ACK the first time + pass + + self._mcu_type = MCU_TYPE_BY_IDCODE[self.get_chip_id()] + except PandaSpiException: + raise PandaSpiException("failed to connect to panda") # pylint: disable=W0707 + + def _get_ack(self, spi, timeout=1.0): + data = 0x00 + start_time = time.monotonic() + while data not in (self.ACK, self.NACK) and (time.monotonic() - start_time < timeout): + data = spi.xfer([0x00, ])[0] + time.sleep(0.001) + spi.xfer([self.ACK, ]) + + if data == self.NACK: + raise PandaSpiNackResponse + elif data != self.ACK: + raise PandaSpiMissingAck + + def _cmd(self, cmd: int, data: Optional[List[bytes]] = None, read_bytes: int = 0, predata=None) -> bytes: + ret = b"" + with self.dev.acquire() as spi: + # sync + command + spi.xfer([self.SYNC, ]) + spi.xfer([cmd, cmd ^ 0xFF]) + self._get_ack(spi) + + # "predata" - for commands that send the first data without a checksum + if predata is not None: + spi.xfer(predata) + self._get_ack(spi) + + # send data + if data is not None: + for d in data: + if predata is not None: + spi.xfer(d + self._checksum(predata + d)) + else: + spi.xfer(d + self._checksum(d)) + self._get_ack(spi, timeout=20) + + # receive + if read_bytes > 0: + ret = spi.xfer([0x00, ]*(read_bytes + 1))[1:] + if data is None or len(data) == 0: + self._get_ack(spi) + + return bytes(ret) + + def _checksum(self, data: bytes) -> bytes: + if len(data) == 1: + ret = data[0] ^ 0xFF + else: + ret = reduce(lambda a, b: a ^ b, data) + return bytes([ret, ]) + + # *** Bootloader commands *** + + def read(self, address: int, length: int): + data = [struct.pack('>I', address), struct.pack('B', length - 1)] + return self._cmd(0x11, data=data, read_bytes=length) + + def get_chip_id(self) -> int: + r = self._cmd(0x02, read_bytes=3) + assert r[0] == 1 # response length - 1 + return ((r[1] << 8) + r[2]) + + def go_cmd(self, address: int) -> None: + self._cmd(0x21, data=[struct.pack('>I', address), ]) + + # *** helpers *** + + def get_uid(self): + dat = self.read(McuType.H7.config.uid_address, 12) + return binascii.hexlify(dat).decode() + + def erase_sector(self, sector: int): + p = struct.pack('>H', 0) # number of sectors to erase + d = struct.pack('>H', sector) + self._cmd(0x44, data=[d, ], predata=p) + + # *** PandaDFU API *** + + def erase_app(self): + self.erase_sector(1) + + def erase_bootstub(self): + self.erase_sector(0) + + def get_mcu_type(self): + return self._mcu_type + + def clear_status(self): + pass + + def close(self): + self.dev.close() + + def program(self, address, dat): + bs = 256 # max block size for writing to flash over SPI + dat += b"\xFF" * ((bs - len(dat)) % bs) + for i in range(0, len(dat) // bs): + block = dat[i * bs:(i + 1) * bs] + self._cmd(0x31, data=[ + struct.pack('>I', address + i*bs), + bytes([len(block) - 1]) + block, + ]) + + def jump(self, address): + self.go_cmd(self._mcu_type.config.bootstub_address) diff --git a/python/spi_dfu.py b/python/spi_dfu.py deleted file mode 100644 index 73d9305280..0000000000 --- a/python/spi_dfu.py +++ /dev/null @@ -1,118 +0,0 @@ -import time -import struct -from functools import reduce - -from .constants import McuType -from .spi import SpiDevice - -SYNC = 0x5A -ACK = 0x79 -NACK = 0x1F - -# https://www.st.com/resource/en/application_note/an4286-spi-protocol-used-in-the-stm32-bootloader-stmicroelectronics.pdf -class PandaSpiDFU: - def __init__(self, dfu_serial): - self.dev = SpiDevice(speed=1000000) - - # say hello - with self.dev.acquire() as spi: - try: - spi.xfer([SYNC, ]) - self._get_ack(spi) - except Exception: - raise Exception("failed to connect to panda") # pylint: disable=W0707 - - self._mcu_type = self.get_mcu_type() - - def _get_ack(self, spi, timeout=1.0): - data = 0x00 - start_time = time.monotonic() - while data not in (ACK, NACK) and (time.monotonic() - start_time < timeout): - data = spi.xfer([0x00, ])[0] - time.sleep(0.001) - spi.xfer([ACK, ]) - - if data == NACK: - raise Exception("Got NACK response") - elif data != ACK: - raise Exception("Missing ACK") - - def _cmd(self, cmd, data=None, read_bytes=0) -> bytes: - ret = b"" - with self.dev.acquire() as spi: - # sync - spi.xfer([SYNC, ]) - - # send command - spi.xfer([cmd, cmd ^ 0xFF]) - self._get_ack(spi) - - # send data - if data is not None: - for d in data: - spi.xfer(self.add_checksum(d)) - self._get_ack(spi, timeout=20) - - # receive - if read_bytes > 0: - # send busy byte - ret = spi.xfer([0x00, ]*(read_bytes + 1))[1:] - self._get_ack(spi) - - return ret - - def add_checksum(self, data): - return data + bytes([reduce(lambda a, b: a ^ b, data)]) - - # ***** ST Bootloader functions ***** - - def get_bootloader_version(self) -> int: - return self._cmd(0x01, read_bytes=1)[0] - - def get_id(self) -> int: - ret = self._cmd(0x02, read_bytes=3) - assert ret[0] == 1 - return ((ret[1] << 8) + ret[2]) - - def go_cmd(self, address: int) -> None: - self._cmd(0x21, data=[struct.pack('>I', address), ]) - - def erase(self, address: int) -> None: - d = struct.pack('>H', address) - self._cmd(0x44, data=[d, ]) - - # ***** panda api ***** - - def get_mcu_type(self) -> McuType: - mcu_by_id = {mcu.config.mcu_idcode: mcu for mcu in McuType} - return mcu_by_id[self.get_id()] - - def global_erase(self): - self.erase(0xFFFF) - - def program_file(self, address, fn): - with open(fn, 'rb') as f: - code = f.read() - - i = 0 - while i < len(code): - #print(i, len(code)) - block = code[i:i+256] - if len(block) < 256: - block += b'\xFF' * (256 - len(block)) - - self._cmd(0x31, data=[ - struct.pack('>I', address + i), - bytes([len(block) - 1]) + block, - ]) - #print(f"Written {len(block)} bytes to {hex(address + i)}") - i += 256 - - def program_bootstub(self): - self.program_file(self._mcu_type.config.bootstub_address, self._mcu_type.config.bootstub_path) - - def program_app(self): - self.program_file(self._mcu_type.config.app_address, self._mcu_type.config.app_path) - - def reset(self): - self.go_cmd(self._mcu_type.config.bootstub_address) diff --git a/python/usb.py b/python/usb.py index 179ebe53c9..2e236a9999 100644 --- a/python/usb.py +++ b/python/usb.py @@ -1,6 +1,8 @@ +import struct from typing import List -from .base import BaseHandle +from .base import BaseHandle, BaseSTBootloaderHandle, TIMEOUT +from .constants import McuType class PandaUsbHandle(BaseHandle): def __init__(self, libusb_handle): @@ -9,15 +11,85 @@ def __init__(self, libusb_handle): def close(self): self._libusb_handle.close() - def controlWrite(self, request_type: int, request: int, value: int, index: int, data, timeout: int = 0): + def controlWrite(self, request_type: int, request: int, value: int, index: int, data, timeout: int = TIMEOUT): return self._libusb_handle.controlWrite(request_type, request, value, index, data, timeout) - def controlRead(self, request_type: int, request: int, value: int, index: int, length: int, timeout: int = 0): + def controlRead(self, request_type: int, request: int, value: int, index: int, length: int, timeout: int = TIMEOUT): return self._libusb_handle.controlRead(request_type, request, value, index, length, timeout) - def bulkWrite(self, endpoint: int, data: List[int], timeout: int = 0) -> int: + def bulkWrite(self, endpoint: int, data: List[int], timeout: int = TIMEOUT) -> int: return self._libusb_handle.bulkWrite(endpoint, data, timeout) # type: ignore - def bulkRead(self, endpoint: int, length: int, timeout: int = 0) -> bytes: + def bulkRead(self, endpoint: int, length: int, timeout: int = TIMEOUT) -> bytes: return self._libusb_handle.bulkRead(endpoint, length, timeout) # type: ignore + + +class STBootloaderUSBHandle(BaseSTBootloaderHandle): + DFU_DNLOAD = 1 + DFU_UPLOAD = 2 + DFU_GETSTATUS = 3 + DFU_CLRSTATUS = 4 + DFU_ABORT = 6 + + def __init__(self, libusb_device, libusb_handle): + self._libusb_handle = libusb_handle + + # TODO: Find a way to detect F4 vs F2 + # TODO: also check F4 BCD, don't assume in else + self._mcu_type = McuType.H7 if libusb_device.getbcdDevice() == 512 else McuType.F4 + + def _status(self) -> None: + while 1: + dat = self._libusb_handle.controlRead(0x21, self.DFU_GETSTATUS, 0, 0, 6) + if dat[1] == 0: + break + + def _erase_page_address(self, address: int) -> None: + self._libusb_handle.controlWrite(0x21, self.DFU_DNLOAD, 0, 0, b"\x41" + struct.pack("I", address)) + self._status() + + def get_mcu_type(self): + return self._mcu_type + + def erase_app(self): + self._erase_page_address(self._mcu_type.config.app_address) + + def erase_bootstub(self): + self._erase_page_address(self._mcu_type.config.bootstub_address) + + def clear_status(self): + # Clear status + stat = self._libusb_handle.controlRead(0x21, self.DFU_GETSTATUS, 0, 0, 6) + if stat[4] == 0xa: + self._libusb_handle.controlRead(0x21, self.DFU_CLRSTATUS, 0, 0, 0) + elif stat[4] == 0x9: + self._libusb_handle.controlWrite(0x21, self.DFU_ABORT, 0, 0, b"") + self._status() + stat = str(self._libusb_handle.controlRead(0x21, self.DFU_GETSTATUS, 0, 0, 6)) + + def close(self): + self._libusb_handle.close() + + def program(self, address, dat): + # Set Address Pointer + self._libusb_handle.controlWrite(0x21, self.DFU_DNLOAD, 0, 0, b"\x21" + struct.pack("I", address)) + self._status() + + # Program + bs = self._mcu_type.config.block_size + dat += b"\xFF" * ((bs - len(dat)) % bs) + for i in range(0, len(dat) // bs): + ldat = dat[i * bs:(i + 1) * bs] + print("programming %d with length %d" % (i, len(ldat))) + self._libusb_handle.controlWrite(0x21, self.DFU_DNLOAD, 2 + i, 0, ldat) + self._status() + + def jump(self, address): + self._libusb_handle.controlWrite(0x21, self.DFU_DNLOAD, 0, 0, b"\x21" + struct.pack("I", address)) + self._status() + try: + self._libusb_handle.controlWrite(0x21, self.DFU_DNLOAD, 2, 0, b"") + _ = str(self._libusb_handle.controlRead(0x21, self.DFU_GETSTATUS, 0, 0, 6)) + except Exception: + pass diff --git a/requirements.txt b/requirements.txt index ceb0ef1234..600be4b2bf 100644 --- a/requirements.txt +++ b/requirements.txt @@ -12,6 +12,6 @@ cffi==1.14.3 crcmod pre-commit==2.13.0 pylint==2.15.4 -scons +scons==4.4.0 flaky spidev diff --git a/tests/SConscript b/tests/SConscript index 69f67ecc0f..86e9f933d4 100644 --- a/tests/SConscript +++ b/tests/SConscript @@ -8,5 +8,6 @@ env = Environment( CPPPATH=[".", "../board"], ) +env.PrependENVPath('PATH', '/opt/homebrew/bin') env.SharedLibrary("safety/libpandasafety.so", ["safety/test.c"]) env.SharedLibrary("usbprotocol/libpandaprotocol.so", ["usbprotocol/test.c"]) diff --git a/tests/hitl/0_dfu.py b/tests/hitl/0_dfu.py new file mode 100644 index 0000000000..ff697ba545 --- /dev/null +++ b/tests/hitl/0_dfu.py @@ -0,0 +1,21 @@ +from panda import Panda, PandaDFU +from .helpers import test_all_pandas, panda_connect_and_init + +@test_all_pandas +@panda_connect_and_init +def test_dfu(p): + app_mcu_type = p.get_mcu_type() + dfu_serial = PandaDFU.st_serial_to_dfu_serial(p.get_usb_serial(), p.get_mcu_type()) + + p.reset(enter_bootstub=True) + p.reset(enter_bootloader=True) + assert Panda.wait_for_dfu(dfu_serial, timeout=20), "failed to enter DFU" + + dfu = PandaDFU(dfu_serial) + assert dfu.get_mcu_type() == app_mcu_type + + assert dfu_serial in PandaDFU.list() + + dfu._handle.clear_status() + dfu.reset() + p.reconnect() diff --git a/tests/hitl/2_health.py b/tests/hitl/2_health.py index 47dc51c23e..3a7f1e74a2 100644 --- a/tests/hitl/2_health.py +++ b/tests/hitl/2_health.py @@ -39,7 +39,7 @@ def test_orientation_detection(p): def test_voltage(p): for _ in range(10): voltage = p.health()['voltage'] - assert ((voltage > 10000) and (voltage < 14000)) + assert ((voltage > 11000) and (voltage < 13000)) time.sleep(0.1) @test_all_pandas diff --git a/tests/libpanda/SConscript b/tests/libpanda/SConscript index 0353b44538..71ed743897 100644 --- a/tests/libpanda/SConscript +++ b/tests/libpanda/SConscript @@ -8,5 +8,6 @@ env = Environment( ], CPPPATH=[".", "../../board/"], ) +env.PrependENVPath('PATH', '/opt/homebrew/bin') env.SharedLibrary("libpanda.so", ["panda.c",]) diff --git a/tests/libpanda/safety_helpers.h b/tests/libpanda/safety_helpers.h index 46b05fc7a0..7933431d2b 100644 --- a/tests/libpanda/safety_helpers.h +++ b/tests/libpanda/safety_helpers.h @@ -10,9 +10,9 @@ bool addr_checks_valid() { for (int i = 0; i < current_rx_checks->len; i++) { const AddrCheckStruct addr = current_rx_checks->check[i]; - bool valid = addr.msg_seen && !addr.lagging && addr.valid_checksum && (addr.wrong_counters < MAX_WRONG_COUNTERS); + bool valid = addr.msg_seen && !addr.lagging && addr.valid_checksum && (addr.wrong_counters < MAX_WRONG_COUNTERS) && addr.valid_quality_flag; if (!valid) { - printf("i %d seen %d lagging %d valid checksum %d wrong counters %d\n", i, addr.msg_seen, addr.lagging, addr.valid_checksum, addr.wrong_counters); + printf("i %d seen %d lagging %d valid checksum %d wrong counters %d valid quality flag %d\n", i, addr.msg_seen, addr.lagging, addr.valid_checksum, addr.wrong_counters, addr.valid_quality_flag); return false; } } diff --git a/tests/safety/common.py b/tests/safety/common.py index 99bb37cadb..71bdff0e89 100644 --- a/tests/safety/common.py +++ b/tests/safety/common.py @@ -778,7 +778,7 @@ def test_tx_hook_on_wrong_safety_mode(self): # No point in comparing different Tesla safety modes if 'Tesla' in attr and 'Tesla' in current_test: continue - if {attr, current_test}.issubset({'TestToyotaSafety', 'TestToyotaAltBrakeSafety', 'TestToyotaStockLongitudinal'}): + if attr.startswith('TestToyota') and current_test.startswith('TestToyota'): continue if {attr, current_test}.issubset({'TestSubaruSafety', 'TestSubaruGen2Safety'}): continue diff --git a/tests/safety/test.sh b/tests/safety/test.sh index 5a35ef33e7..70164ec54e 100755 --- a/tests/safety/test.sh +++ b/tests/safety/test.sh @@ -4,7 +4,8 @@ # Make sure test fails if one HW_TYPE fails set -e -for hw_type in {0..7}; do +HW_TYPES=( 6 7 9 ) +for hw_type in "${HW_TYPES[@]}"; do echo "Testing HW_TYPE: $hw_type" HW_TYPE=$hw_type python -m unittest discover . done diff --git a/tests/safety/test_chrysler.py b/tests/safety/test_chrysler.py index afeca29446..b9390f5536 100755 --- a/tests/safety/test_chrysler.py +++ b/tests/safety/test_chrysler.py @@ -79,6 +79,7 @@ class TestChryslerRamDTSafety(TestChryslerSafety): MAX_RATE_UP = 6 MAX_RATE_DOWN = 6 + MAX_TORQUE = 350 DAS_BUS = 2 diff --git a/tests/safety/test_ford.py b/tests/safety/test_ford.py index c8082c5218..8d34598621 100755 --- a/tests/safety/test_ford.py +++ b/tests/safety/test_ford.py @@ -10,6 +10,8 @@ MSG_EngBrakeData = 0x165 # RX from PCM, for driver brake pedal and cruise state MSG_EngVehicleSpThrottle = 0x204 # RX from PCM, for driver throttle input +MSG_BrakeSysFeatures = 0x415 # RX from ABS, for vehicle speed +MSG_Yaw_Data_FD1 = 0x91 # RX from RCM, for yaw rate MSG_Steering_Data_FD1 = 0x083 # TX by OP, various driver switches and LKAS/CC buttons MSG_ACCDATA_3 = 0x18A # TX by OP, ACC/TJA user interface MSG_Lane_Assist_Data1 = 0x3CA # TX by OP, Lane Keep Assist @@ -17,6 +19,29 @@ MSG_IPMA_Data = 0x3D8 # TX by OP, IPMA and LKAS user interface +def checksum(msg): + addr, t, dat, bus = msg + ret = bytearray(dat) + + if addr == MSG_Yaw_Data_FD1: + chksum = dat[0] + dat[1] # VehRol_W_Actl + chksum += dat[2] + dat[3] # VehYaw_W_Actl + chksum += dat[5] # VehRollYaw_No_Cnt + chksum += dat[6] >> 6 # VehRolWActl_D_Qf + chksum += (dat[6] >> 4) & 0x3 # VehYawWActl_D_Qf + chksum = 0xff - (chksum & 0xff) + ret[4] = chksum + + elif addr == MSG_BrakeSysFeatures: + chksum = dat[0] + dat[1] # Veh_V_ActlBrk + chksum += (dat[2] >> 2) & 0xf # VehVActlBrk_No_Cnt + chksum += dat[2] >> 6 # VehVActlBrk_D_Qf + chksum = 0xff - (chksum & 0xff) + ret[3] = chksum + + return addr, t, ret, bus + + class Buttons: CANCEL = 0 RESUME = 1 @@ -35,6 +60,9 @@ class TestFordSafety(common.PandaSafetyTest): FWD_BLACKLISTED_ADDRS = {2: [MSG_ACCDATA_3, MSG_Lane_Assist_Data1, MSG_LateralMotionControl, MSG_IPMA_Data]} FWD_BUS_LOOKUP = {0: 2, 2: 0} + cnt_speed = 0 + cnt_yaw_rate = 0 + def setUp(self): self.packer = CANPackerPanda("ford_lincoln_base_pt") self.safety = libpanda_py.libpanda @@ -52,14 +80,24 @@ def _user_brake_msg(self, brake: bool): } return self.packer.make_can_msg_panda("EngBrakeData", 0, values) - def _speed_msg(self, speed: float): - pass + # Vehicle speed + def _speed_msg(self, speed: float, quality_flag=True): + values = {"Veh_V_ActlBrk": speed * 3.6, "VehVActlBrk_D_Qf": 3 if quality_flag else 0, "VehVActlBrk_No_Cnt": self.cnt_speed % 16} + self.__class__.cnt_speed += 1 + return self.packer.make_can_msg_panda("BrakeSysFeatures", 0, values, fix_checksum=checksum) # Standstill state def _vehicle_moving_msg(self, speed: float): values = {"VehStop_D_Stat": 1 if speed <= self.STANDSTILL_THRESHOLD else 0} return self.packer.make_can_msg_panda("DesiredTorqBrk", 0, values) + # Current curvature + def _yaw_rate_msg(self, curvature: float, speed: float, quality_flag=True): + values = {"VehYaw_W_Actl": curvature * speed, "VehYawWActl_D_Qf": 3 if quality_flag else 0, + "VehRolWActl_D_Qf": 3 if quality_flag else 0, "VehRollYaw_No_Cnt": self.cnt_yaw_rate % 256} + self.__class__.cnt_yaw_rate += 1 + return self.packer.make_can_msg_panda("Yaw_Data_FD1", 0, values, fix_checksum=checksum) + # Drive throttle input def _user_gas_msg(self, gas: float): values = {"ApedPos_Pc_ActlArb": gas} @@ -103,6 +141,26 @@ def _acc_button_msg(self, button: int, bus: int): } return self.packer.make_can_msg_panda("Steering_Data_FD1", bus, values) + def test_rx_hook(self): + # checksum, counter, and quality flag checks + for quality_flag in [True, False]: + for msg in ["speed", "yaw"]: + self.safety.set_controls_allowed(True) + # send multiple times to verify counter checks + for _ in range(10): + if msg == "speed": + to_push = self._speed_msg(0, quality_flag=quality_flag) + elif msg == "yaw": + to_push = self._yaw_rate_msg(0, 0, quality_flag=quality_flag) + + self.assertEqual(quality_flag, self._rx(to_push)) + self.assertEqual(quality_flag, self.safety.get_controls_allowed()) + + # Mess with checksum to make it fail + to_push[0].data[3] = 0 # Speed checksum & half of yaw signal + self.assertFalse(self._rx(to_push)) + self.assertFalse(self.safety.get_controls_allowed()) + def test_steer_allowed(self): path_offsets = np.arange(-5.12, 5.11, 1).round() path_angles = np.arange(-0.5, 0.5235, 0.1).round(1) diff --git a/tests/safety/test_toyota.py b/tests/safety/test_toyota.py index 7053ce931e..0a6cb8738b 100755 --- a/tests/safety/test_toyota.py +++ b/tests/safety/test_toyota.py @@ -2,6 +2,7 @@ import numpy as np import random import unittest +import itertools from panda import Panda from panda.tests.libpanda import libpanda_py @@ -21,8 +22,7 @@ def interceptor_msg(gas, addr): return to_send -class TestToyotaSafety(common.PandaSafetyTest, common.InterceptorSafetyTest, - common.MotorTorqueSteeringSafetyTest): +class TestToyotaSafetyBase(common.PandaSafetyTest, common.InterceptorSafetyTest): TX_MSGS = [[0x283, 0], [0x2E6, 0], [0x2E7, 0], [0x33E, 0], [0x344, 0], [0x365, 0], [0x366, 0], [0x4CB, 0], # DSU bus 0 [0x128, 1], [0x141, 1], [0x160, 1], [0x161, 1], [0x470, 1], # DSU bus 1 @@ -34,37 +34,26 @@ class TestToyotaSafety(common.PandaSafetyTest, common.InterceptorSafetyTest, FWD_BLACKLISTED_ADDRS = {2: [0x2E4, 0x412, 0x191, 0x343]} FWD_BUS_LOOKUP = {0: 2, 2: 0} INTERCEPTOR_THRESHOLD = 805 - - MAX_RATE_UP = 15 - MAX_RATE_DOWN = 25 - MAX_TORQUE = 1500 - MAX_RT_DELTA = 450 - RT_INTERVAL = 250000 - MAX_TORQUE_ERROR = 350 - TORQUE_MEAS_TOLERANCE = 1 # toyota safety adds one to be conservative for rounding EPS_SCALE = 73 - # Safety around steering req bit - MIN_VALID_STEERING_FRAMES = 18 - MAX_INVALID_STEERING_FRAMES = 1 - MIN_VALID_STEERING_RT_INTERVAL = 170000 # a ~10% buffer, can send steer up to 110Hz - - def setUp(self): - self.packer = CANPackerPanda("toyota_nodsu_pt_generated") - self.safety = libpanda_py.libpanda - self.safety.set_safety_hooks(Panda.SAFETY_TOYOTA, self.EPS_SCALE) - self.safety.init_tests() + @classmethod + def setUpClass(cls): + if cls.__name__.endswith("Base"): + cls.packer = None + cls.safety = None + raise unittest.SkipTest def _torque_meas_msg(self, torque): values = {"STEER_TORQUE_EPS": (torque / self.EPS_SCALE) * 100.} return self.packer.make_can_msg_panda("STEER_TORQUE_SENSOR", 0, values) + # Both torque and angle safety modes test with each other's steering commands def _torque_cmd_msg(self, torque, steer_req=1): values = {"STEER_TORQUE_CMD": torque, "STEER_REQUEST": steer_req} return self.packer.make_can_msg_panda("STEERING_LKA", 0, values) - def _lta_msg(self, req, req2, angle_cmd): - values = {"STEER_REQUEST": req, "STEER_REQUEST_2": req2, "STEER_ANGLE_CMD": angle_cmd} + def _lta_msg(self, req, req2, angle_cmd, setme_x64=100): + values = {"STEER_REQUEST": req, "STEER_REQUEST_2": req2, "STEER_ANGLE_CMD": angle_cmd, "SETME_X64": setme_x64} return self.packer.make_can_msg_panda("STEERING_LTA", 0, values) def _accel_msg(self, accel, cancel_req=0): @@ -124,23 +113,14 @@ def test_accel_actuation_limits(self, stock_longitudinal=False): # Only allow LTA msgs with no actuation def test_lta_steer_cmd(self): - for engaged in [True, False]: + for engaged, req, req2, setme_x64, angle in itertools.product([True, False], + [0, 1], [0, 1], + [0, 50, 100], + np.linspace(-20, 20, 5)): self.safety.set_controls_allowed(engaged) - # good msg - self.assertTrue(self._tx(self._lta_msg(0, 0, 0))) - - # bad msgs - self.assertFalse(self._tx(self._lta_msg(1, 0, 0))) - self.assertFalse(self._tx(self._lta_msg(0, 1, 0))) - self.assertFalse(self._tx(self._lta_msg(0, 0, 1))) - - for _ in range(20): - req = random.choice([0, 1]) - req2 = random.choice([0, 1]) - angle = random.randint(-50, 50) - should_tx = not req and not req2 and angle == 0 - self.assertEqual(should_tx, self._tx(self._lta_msg(req, req2, angle))) + should_tx = not req and not req2 and angle == 0 and setme_x64 == 0 + self.assertEqual(should_tx, self._tx(self._lta_msg(req, req2, angle, setme_x64))) def test_rx_hook(self): # checksum checks @@ -159,7 +139,49 @@ def test_rx_hook(self): self.assertFalse(self.safety.get_controls_allowed()) -class TestToyotaAltBrakeSafety(TestToyotaSafety): +class TestToyotaSafetyTorque(TestToyotaSafetyBase, common.MotorTorqueSteeringSafetyTest): + + MAX_RATE_UP = 15 + MAX_RATE_DOWN = 25 + MAX_TORQUE = 1500 + MAX_RT_DELTA = 450 + RT_INTERVAL = 250000 + MAX_TORQUE_ERROR = 350 + TORQUE_MEAS_TOLERANCE = 1 # toyota safety adds one to be conservative for rounding + + # Safety around steering req bit + MIN_VALID_STEERING_FRAMES = 18 + MAX_INVALID_STEERING_FRAMES = 1 + MIN_VALID_STEERING_RT_INTERVAL = 170000 # a ~10% buffer, can send steer up to 110Hz + + def setUp(self): + self.packer = CANPackerPanda("toyota_nodsu_pt_generated") + self.safety = libpanda_py.libpanda + self.safety.set_safety_hooks(Panda.SAFETY_TOYOTA, self.EPS_SCALE) + self.safety.init_tests() + + +class TestToyotaSafetyAngle(TestToyotaSafetyBase): + + def setUp(self): + self.packer = CANPackerPanda("toyota_nodsu_pt_generated") + self.safety = libpanda_py.libpanda + self.safety.set_safety_hooks(Panda.SAFETY_TOYOTA, self.EPS_SCALE | Panda.FLAG_TOYOTA_LTA) + self.safety.init_tests() + + # Only allow LKA msgs with no actuation + def test_lka_steer_cmd(self): + for engaged, steer_req, torque in itertools.product([True, False], + [0, 1], + np.linspace(-1500, 1500, 7)): + self.safety.set_controls_allowed(engaged) + + should_tx = not steer_req and torque == 0 + self.assertEqual(should_tx, self._tx(self._torque_cmd_msg(torque, steer_req))) + + +class TestToyotaAltBrakeSafety(TestToyotaSafetyTorque): + def setUp(self): self.packer = CANPackerPanda("toyota_new_mc_pt_generated") self.safety = libpanda_py.libpanda @@ -170,17 +192,15 @@ def _user_brake_msg(self, brake): values = {"BRAKE_PRESSED": brake} return self.packer.make_can_msg_panda("BRAKE_MODULE", 0, values) - # No LTA on these cars + # No LTA message in the DBC def test_lta_steer_cmd(self): pass -class TestToyotaStockLongitudinal(TestToyotaSafety): - def setUp(self): - self.packer = CANPackerPanda("toyota_nodsu_pt_generated") - self.safety = libpanda_py.libpanda - self.safety.set_safety_hooks(Panda.SAFETY_TOYOTA, self.EPS_SCALE | Panda.FLAG_TOYOTA_STOCK_LONGITUDINAL) - self.safety.init_tests() +class TestToyotaStockLongitudinalBase(TestToyotaSafetyBase): + + # Base fwd addresses minus ACC_CONTROL (0x343) + FWD_BLACKLISTED_ADDRS = {2: [0x2E4, 0x412, 0x191]} def test_accel_actuation_limits(self, stock_longitudinal=True): super().test_accel_actuation_limits(stock_longitudinal=stock_longitudinal) @@ -196,10 +216,23 @@ def test_acc_cancel(self): should_tx = np.isclose(accel, 0, atol=0.0001) self.assertEqual(should_tx, self._tx(self._accel_msg(accel, cancel_req=1))) - def test_fwd_hook(self): - # forward ACC_CONTROL - self.FWD_BLACKLISTED_ADDRS[2].remove(0x343) - super().test_fwd_hook() + +class TestToyotaStockLongitudinalTorque(TestToyotaStockLongitudinalBase, TestToyotaSafetyTorque): + + def setUp(self): + self.packer = CANPackerPanda("toyota_nodsu_pt_generated") + self.safety = libpanda_py.libpanda + self.safety.set_safety_hooks(Panda.SAFETY_TOYOTA, self.EPS_SCALE | Panda.FLAG_TOYOTA_STOCK_LONGITUDINAL) + self.safety.init_tests() + + +class TestToyotaStockLongitudinalAngle(TestToyotaStockLongitudinalBase, TestToyotaSafetyAngle): + + def setUp(self): + self.packer = CANPackerPanda("toyota_nodsu_pt_generated") + self.safety = libpanda_py.libpanda + self.safety.set_safety_hooks(Panda.SAFETY_TOYOTA, self.EPS_SCALE | Panda.FLAG_TOYOTA_STOCK_LONGITUDINAL | Panda.FLAG_TOYOTA_LTA) + self.safety.init_tests() if __name__ == "__main__": diff --git a/tests/safety/test_volkswagen_pq.py b/tests/safety/test_volkswagen_pq.py index b3e802e6fe..563d5de011 100755 --- a/tests/safety/test_volkswagen_pq.py +++ b/tests/safety/test_volkswagen_pq.py @@ -14,7 +14,7 @@ MSG_MOTOR_3 = 0x380 # RX from ECU, for driver throttle input MSG_GRA_NEU = 0x38A # TX by OP, ACC control buttons for cancel/resume MSG_MOTOR_5 = 0x480 # RX from ECU, for ACC main switch state -MSG_ACC_GRA_ANZIEGE = 0x56A # TX by OP, ACC HUD +MSG_ACC_GRA_ANZEIGE = 0x56A # TX by OP, ACC HUD MSG_LDW_1 = 0x5BE # TX by OP, Lane line recognition and text alerts MAX_ACCEL = 2.0 @@ -143,8 +143,8 @@ def test_spam_cancel_safety_check(self): class TestVolkswagenPqLongSafety(TestVolkswagenPqSafety): - TX_MSGS = [[MSG_HCA_1, 0], [MSG_LDW_1, 0], [MSG_ACC_SYSTEM, 0], [MSG_ACC_GRA_ANZIEGE, 0]] - FWD_BLACKLISTED_ADDRS = {2: [MSG_HCA_1, MSG_LDW_1, MSG_ACC_SYSTEM, MSG_ACC_GRA_ANZIEGE]} + TX_MSGS = [[MSG_HCA_1, 0], [MSG_LDW_1, 0], [MSG_ACC_SYSTEM, 0], [MSG_ACC_GRA_ANZEIGE, 0]] + FWD_BLACKLISTED_ADDRS = {2: [MSG_HCA_1, MSG_LDW_1, MSG_ACC_SYSTEM, MSG_ACC_GRA_ANZEIGE]} FWD_BUS_LOOKUP = {0: 2, 2: 0} INACTIVE_ACCEL = 3.01 diff --git a/tests/safety_replay/helpers.py b/tests/safety_replay/helpers.py index a989bd3fa2..9eb26ee82e 100644 --- a/tests/safety_replay/helpers.py +++ b/tests/safety_replay/helpers.py @@ -22,26 +22,30 @@ def is_steering_msg(mode, addr): ret = addr == 0x292 elif mode == Panda.SAFETY_SUBARU: ret = addr == 0x122 + elif mode == Panda.SAFETY_FORD: + ret = addr == 0x3d3 return ret -def get_steer_torque(mode, to_send): - ret = 0 +def get_steer_value(mode, to_send): + torque, angle = 0, 0 if mode in (Panda.SAFETY_HONDA_NIDEC, Panda.SAFETY_HONDA_BOSCH): - ret = to_send.RDLR & 0xFFFF0000 + torque = to_send.RDLR & 0xFFFF0000 elif mode == Panda.SAFETY_TOYOTA: - ret = (to_send.RDLR & 0xFF00) | ((to_send.RDLR >> 16) & 0xFF) - ret = to_signed(ret, 16) + torque = (to_send.data[1] << 8) | (to_send.data[2]) + torque = to_signed(torque, 16) elif mode == Panda.SAFETY_GM: - ret = ((to_send.data[0] & 0x7) << 8) | to_send.data[1] - ret = to_signed(ret, 11) + torque = ((to_send.data[0] & 0x7) << 8) | to_send.data[1] + torque = to_signed(torque, 11) elif mode == Panda.SAFETY_HYUNDAI: - ret = (((to_send.data[3] & 0x7) << 8) | to_send.data[2]) - 1024 + torque = (((to_send.data[3] & 0x7) << 8) | to_send.data[2]) - 1024 elif mode == Panda.SAFETY_CHRYSLER: - ret = ((to_send.RDLR & 0x7) << 8) + ((to_send.RDLR & 0xFF00) >> 8) - 1024 + torque = (((to_send.data[0] & 0x7) << 8) | to_send.data[1]) - 1024 elif mode == Panda.SAFETY_SUBARU: - ret = ((to_send.RDLR >> 16) & 0x1FFF) - ret = to_signed(ret, 13) - return ret + torque = ((to_send.data[3] & 0x1F) << 8) | to_send.data[2] + torque = -to_signed(torque, 13) + elif mode == Panda.SAFETY_FORD: + angle = ((to_send.data[0] << 3) | (to_send.data[1] >> 5)) - 1000 + return torque, angle def package_can_msg(msg): return libpanda_py.make_CANPacket(msg.address, msg.src % 4, msg.dat) @@ -56,8 +60,11 @@ def init_segment(safety, lr, mode): return to_send = package_can_msg(msg) - torque = get_steer_torque(mode, to_send) + torque, angle = get_steer_value(mode, to_send) if torque != 0: safety.set_controls_allowed(1) safety.set_desired_torque_last(torque) - assert safety.safety_tx_hook(to_send), "failed to initialize panda safety for segment" + elif angle != 0: + safety.set_controls_allowed(1) + safety.set_desired_angle_last(angle) + assert safety.safety_tx_hook(to_send), "failed to initialize panda safety for segment" diff --git a/tests/spi_flash.py b/tests/spi_flash.py deleted file mode 100755 index b42bcc92ac..0000000000 --- a/tests/spi_flash.py +++ /dev/null @@ -1,23 +0,0 @@ -#!/usr/bin/env python3 - -from panda.python.spi_dfu import PandaSpiDFU - -if __name__ == "__main__": - p = PandaSpiDFU('') - - print("Bootloader version", p.get_bootloader_version()) - print("MCU ID", p.get_id()) - - print("erasing...") - p.global_erase() - print("done") - - print("flashing bootstub") - p.program_bootstub() - - print("flashing app") - p.program_app() - - print("reset") - p.reset() - print("done")