/** * @brief performs a simple reset of the system */ static uint8_t gb_control_reboot_reset(struct gb_operation *operation) { #ifdef CONFIG_ARCH_HAVE_SYSRESET up_systemreset(); /* will not return */ #endif return GB_OP_INVALID; }
static void sysreset(void) { /* workaround to flush eMMC cache */ usleep(100000); up_systemreset(); }
/** * @brief sets the flag to tell the bootloader to stay in flash mode */ static uint8_t gb_control_reboot_flash(struct gb_operation *operation) { if (gb_bootmode_set(BOOTMODE_REQUEST_FLASH)) gb_error("error setting boot state to flash\n"); #ifdef CONFIG_ARCH_HAVE_SYSRESET up_systemreset(); /* will not return */ #endif return GB_OP_SUCCESS; }
int board_shutdown() { stm32_pwr_enablebkp(true); /* XXX wow, this is evil - write a magic number into backup register zero */ *(uint32_t *)0x40002850 = 0xdeaddead; stm32_pwr_enablebkp(false); up_mdelay(50); up_systemreset(); while (1); return 0; }
static int button_pressed_down_handler(int irq, FAR void *context) { standby_do_trace('B'); clear_rst_stdby_flags(); /* Do system reset. */ up_systemreset(); for(;;); return 0; }
void systemreset(bool to_bootloader) { if (to_bootloader) { stm32_pwr_enablebkp(); /* XXX wow, this is evil - write a magic number into backup register zero */ *(uint32_t *)0x40002850 = 0xb007b007; } up_systemreset(); /* lock up here */ while(true); }
__END_DECLS /**************************************************************************** * Private Functions ****************************************************************************/ static int _bootloader_force_pin_callback(int irq, void *context) { if (stm32_gpioread(GPIO_FORCE_BOOTLOADER)) { up_systemreset(); } return 0; }
void px4_systemreset(bool to_bootloader) { if (to_bootloader) { #ifndef CONFIG_ARCH_BOARD_SIM stm32_pwr_enablebkp(); #endif /* XXX wow, this is evil - write a magic number into backup register zero */ *(uint32_t *)0x40002850 = 0xb007b007; } up_systemreset(); /* lock up here */ while (true); }
static int board_button_irq(int irq, FAR void *context) { static struct timespec time_down; if (board_pwr_button_down()) { led_on(BOARD_LED_RED); clock_gettime(CLOCK_REALTIME, &time_down); } else { led_off(BOARD_LED_RED); struct timespec now; clock_gettime(CLOCK_REALTIME, &now); uint64_t tdown_ms = time_down.tv_sec * 1000 + time_down.tv_nsec / 1000000; uint64_t tnow_ms = now.tv_sec * 1000 + now.tv_nsec / 1000000; if (tdown_ms != 0 && (tnow_ms - tdown_ms) >= MS_PWR_BUTTON_DOWN) { led_on(BOARD_LED_BLUE); up_mdelay(200); stm32_pwr_enablebkp(true); /* XXX wow, this is evil - write a magic number into backup register zero */ *(uint32_t *)0x40002850 = 0xdeaddead; stm32_pwr_enablebkp(false); up_mdelay(50); up_systemreset(); while (1); } } return OK; }
/** check for a scheduled reboot */ static void check_reboot(void) { if (reboot_time != 0 && hrt_absolute_time() > reboot_time) { up_systemreset(); } }
/** * Transition from one state to another */ int do_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, commander_state_machine_t new_state) { int invalid_state = false; int ret = ERROR; commander_state_machine_t old_state = current_status->state_machine; switch (new_state) { case SYSTEM_STATE_MISSION_ABORT: { /* Indoor or outdoor */ // if (flight_environment_parameter == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_LANDING); // } else { // ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_CUTOFF); // } } break; case SYSTEM_STATE_EMCY_LANDING: /* Tell the controller to land */ /* set system flags according to state */ current_status->flag_system_armed = true; warnx("EMERGENCY LANDING!\n"); mavlink_log_critical(mavlink_fd, "EMERGENCY LANDING!"); break; case SYSTEM_STATE_EMCY_CUTOFF: /* Tell the controller to cutoff the motors (thrust = 0) */ /* set system flags according to state */ current_status->flag_system_armed = false; warnx("EMERGENCY MOTOR CUTOFF!\n"); mavlink_log_critical(mavlink_fd, "EMERGENCY MOTOR CUTOFF!"); break; case SYSTEM_STATE_GROUND_ERROR: /* set system flags according to state */ /* prevent actuators from arming */ current_status->flag_system_armed = false; warnx("GROUND ERROR, locking down propulsion system\n"); mavlink_log_critical(mavlink_fd, "GROUND ERROR, locking down system"); break; case SYSTEM_STATE_PREFLIGHT: if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { /* set system flags according to state */ current_status->flag_system_armed = false; mavlink_log_critical(mavlink_fd, "Switched to PREFLIGHT state"); } else { invalid_state = true; mavlink_log_critical(mavlink_fd, "REFUSED to switch to PREFLIGHT state"); } break; case SYSTEM_STATE_REBOOT: if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT || current_status->flag_hil_enabled) { invalid_state = false; /* set system flags according to state */ current_status->flag_system_armed = false; mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); usleep(500000); up_systemreset(); /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ } else { invalid_state = true; mavlink_log_critical(mavlink_fd, "REFUSED to REBOOT"); } break; case SYSTEM_STATE_STANDBY: /* set system flags according to state */ /* standby enforces disarmed */ current_status->flag_system_armed = false; mavlink_log_critical(mavlink_fd, "Switched to STANDBY state"); break; case SYSTEM_STATE_GROUND_READY: /* set system flags according to state */ /* ground ready has motors / actuators armed */ current_status->flag_system_armed = true; mavlink_log_critical(mavlink_fd, "Switched to GROUND READY state"); break; case SYSTEM_STATE_AUTO: /* set system flags according to state */ /* auto is airborne and in auto mode, motors armed */ current_status->flag_system_armed = true; mavlink_log_critical(mavlink_fd, "Switched to FLYING / AUTO mode"); break; case SYSTEM_STATE_STABILIZED: /* set system flags according to state */ current_status->flag_system_armed = true; mavlink_log_critical(mavlink_fd, "Switched to FLYING / STABILIZED mode"); break; case SYSTEM_STATE_MANUAL: /* set system flags according to state */ current_status->flag_system_armed = true; mavlink_log_critical(mavlink_fd, "Switched to FLYING / MANUAL mode"); break; default: invalid_state = true; break; } if (invalid_state == false || old_state != new_state) { current_status->state_machine = new_state; state_machine_publish(status_pub, current_status, mavlink_fd); publish_armed_status(current_status); ret = OK; } if (invalid_state) { mavlink_log_critical(mavlink_fd, "REJECTING invalid state transition"); ret = ERROR; } return ret; }
static void up_check_and_restore_valid_optionbytes(void) { static const uint32_t reset_optb[][2] = { /* Default option bytes for STM32L162VE. */ { 0x1FF80004, 0xFF0700F8 }, { 0x1FF80008, 0xFFFF0000 }, { 0x1FF8000C, 0xFFFF0000 }, { 0x1FF80010, 0xFFFF0000 }, { 0x1FF80014, 0xFFFF0000 }, { 0x1FF80018, 0xFFFF0000 }, { 0x1FF8001C, 0xFFFF0000 }, { 0x1FF80080, 0xFFFF0000 }, { 0x1FF80084, 0xFFFF0000 }, { 0x1FF80000, 0xFF5500AA }, { 0x0, 0x0 }, }; int i; char buf[64]; const char *str; int modcount = 0; if (!(getreg32(STM32_FLASH_SR) & (FLASH_SR_OPTVERR | FLASH_SR_OPTVERRUSR))) return; snprintf(buf, sizeof(buf), "\nSTM32_FLASH_SR: %08X\n", getreg32(STM32_FLASH_SR)); str = buf; while (*str) up_lowputc(*str++); for (i = 0; reset_optb[i][0] != 0x0; i++) { snprintf(buf, sizeof(buf), "%08X: %08X\n", reset_optb[i][0], *(const uint32_t *)reset_optb[i][0]); str = buf; while (*str) up_lowputc(*str++); } stm32_ob_unlock(); /* Clear pending status flags. */ putreg32(FLASH_SR_WRPERR | FLASH_SR_PGAERR | FLASH_SR_SIZERR | FLASH_SR_OPTVERR | FLASH_SR_OPTVERRUSR | FLASH_SR_RDERR, STM32_FLASH_SR); for (i = 0; reset_optb[i][0] != 0x0; i++) { if (*(const uint32_t *)reset_optb[i][0] == reset_optb[i][1]) continue; snprintf(buf, sizeof(buf), "Programming %08X: %08X => %08X\n", reset_optb[i][0], *(const uint32_t *)reset_optb[i][0], reset_optb[i][1]); str = buf; while (*str) up_lowputc(*str++); putreg32(reset_optb[i][1], reset_optb[i][0]); /* ... and wait to complete. */ while (getreg32(STM32_FLASH_SR) & FLASH_SR_BSY); /* Verify */ if (getreg32(STM32_FLASH_SR) & (FLASH_SR_WRPERR | FLASH_SR_PGAERR | FLASH_SR_SIZERR | FLASH_SR_RDERR)) { snprintf(buf, sizeof(buf), "Failed to program %08X!\n", reset_optb[i][0]); str = buf; while (*str) up_lowputc(*str++); goto err; } if (getreg32(reset_optb[i][0]) != reset_optb[i][1]) { snprintf(buf, sizeof(buf), "Failed to verify %08X, reads %08X.\n", reset_optb[i][0], getreg32(reset_optb[i][0])); str = buf; while (*str) up_lowputc(*str++); goto err; } modcount++; } putreg32(FLASH_SR_OPTVERR | FLASH_SR_OPTVERRUSR, STM32_FLASH_SR); stm32_ob_lock(); if (modcount == 0) return; err: while (1) up_systemreset(); }
int board_reset(int status) { up_systemreset(); return 0; }
int reboot_main(int argc, char *argv[]) { up_systemreset(); }