/**
* @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();
}
