reboot: Add reboot to ISP option

platforms/common/include/px4_platform_common/shutdown.h

@@ -70,6 +70,12 @@ __EXPORT int px4_register_shutdown_hook(shutdown_hook_t hook);
  */
 __EXPORT int px4_unregister_shutdown_hook(shutdown_hook_t hook);
 
+/** Types of reboot requests for PX4 */
+typedef enum {
+	REBOOT_REQUEST = 0,          ///< Normal reboot
+	REBOOT_TO_BOOTLOADER = 1,    ///< Reboot to PX4 bootloader
+	REBOOT_TO_ISP = 2,           ///< Reboot to ISP bootloader
+} reboot_request_t;
 
 /**
  * Request the system to reboot.
@@ -83,7 +89,7 @@ __EXPORT int px4_unregister_shutdown_hook(shutdown_hook_t hook);
  * @return 0 on success, <0 on error
  */
 #if defined(CONFIG_BOARDCTL_RESET)
-__EXPORT int px4_reboot_request(bool to_bootloader = false, uint32_t delay_us = 0);
+__EXPORT int px4_reboot_request(reboot_request_t request = REBOOT_REQUEST, uint32_t delay_us = 0);
 #endif // CONFIG_BOARDCTL_RESET
 
 

platforms/common/shutdown.cpp

@@ -108,6 +108,7 @@ static uint16_t shutdown_counter = 0; ///< count how many times the shutdown wor
 #define SHUTDOWN_ARG_IN_PROGRESS (1<<0)
 #define SHUTDOWN_ARG_REBOOT (1<<1)
 #define SHUTDOWN_ARG_TO_BOOTLOADER (1<<2)
+#define SHUTDOWN_ARG_TO_ISP (1<<3)
 static uint8_t shutdown_args = 0;
 
 static constexpr int max_shutdown_hooks = 1;
@@ -175,7 +176,17 @@ static void shutdown_worker(void *arg)
 		if (shutdown_args & SHUTDOWN_ARG_REBOOT) {
 #if defined(CONFIG_BOARDCTL_RESET)
 			PX4_INFO_RAW("Reboot NOW.");
-			boardctl(BOARDIOC_RESET, (shutdown_args & SHUTDOWN_ARG_TO_BOOTLOADER) ? 1 : 0);
+
+			if (shutdown_args & SHUTDOWN_ARG_TO_BOOTLOADER) {
+				boardctl(BOARDIOC_RESET, (uintptr_t)REBOOT_TO_BOOTLOADER);
+
+			} else if (shutdown_args & SHUTDOWN_ARG_TO_ISP) {
+				boardctl(BOARDIOC_RESET, (uintptr_t)REBOOT_TO_ISP);
+
+			} else {
+				boardctl(BOARDIOC_RESET, (uintptr_t)REBOOT_REQUEST);
+			}
+
 #else
 			PX4_PANIC("board reset not available");
 #endif
@@ -206,7 +217,7 @@ static void shutdown_worker(void *arg)
 }
 
 #if defined(CONFIG_BOARDCTL_RESET)
-int px4_reboot_request(bool to_bootloader, uint32_t delay_us)
+int px4_reboot_request(reboot_request_t request, uint32_t delay_us)
 {
 	pthread_mutex_lock(&shutdown_mutex);
 
@@ -217,8 +228,11 @@ int px4_reboot_request(bool to_bootloader, uint32_t delay_us)
 
 	shutdown_args |= SHUTDOWN_ARG_REBOOT;
 
-	if (to_bootloader) {
+	if (request == REBOOT_TO_BOOTLOADER) {
 		shutdown_args |= SHUTDOWN_ARG_TO_BOOTLOADER;
+
+	} else if (request == REBOOT_TO_ISP) {
+		shutdown_args |= SHUTDOWN_ARG_TO_ISP;
 	}
 
 	shutdown_time_us = hrt_absolute_time();

platforms/nuttx/src/px4/common/cdc_acm_check.cpp

@@ -187,7 +187,7 @@ static void mavlink_usb_check(void *arg)
 
 										if (param1 == 1) {
 											// 1: Reboot autopilot
-											px4_reboot_request(false, 0);
+											px4_reboot_request(REBOOT_REQUEST, 0);
 
 										} else if (param1 == 2) {
 											// 2: Shutdown autopilot
@@ -197,7 +197,7 @@ static void mavlink_usb_check(void *arg)
 
 										} else if (param1 == 3) {
 											// 3: Reboot autopilot and keep it in the bootloader until upgraded.
-											px4_reboot_request(true, 0);
+											px4_reboot_request(REBOOT_TO_BOOTLOADER, 0);
 										}
 									}
 								}

platforms/nuttx/src/px4/nxp/imxrt/board_reset/board_reset.cpp

@@ -38,6 +38,7 @@
  */
 
 #include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/shutdown.h>
 #include <errno.h>
 #include <nuttx/board.h>
 #include <arm_internal.h>
@@ -61,7 +62,7 @@ static int board_reset_enter_bootloader()
 
 int board_reset(int status)
 {
-	if (status == 1) {
+	if (status == REBOOT_TO_BOOTLOADER) {
 		board_reset_enter_bootloader();
 	}
 

platforms/nuttx/src/px4/nxp/kinetis/board_reset/board_reset.cpp

@@ -38,6 +38,7 @@
  */
 
 #include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/shutdown.h>
 #include <errno.h>
 #include <nuttx/board.h>
 
@@ -72,7 +73,7 @@ static int board_reset_enter_bootloader()
 
 int board_reset(int status)
 {
-	if (status == 1) {
+	if (status == REBOOT_TO_BOOTLOADER) {
 		board_reset_enter_bootloader();
 	}
 

platforms/nuttx/src/px4/nxp/s32k1xx/board_reset/board_reset.cpp

@@ -39,6 +39,7 @@
  */
 
 #include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/shutdown.h>
 #include <systemlib/px4_macros.h>
 #include <errno.h>
 #include <nuttx/board.h>
@@ -96,7 +97,7 @@ int board_configure_reset(reset_mode_e mode, uint32_t arg)
 
 int board_reset(int status)
 {
-	if (status == 1) {
+	if (status == REBOOT_TO_BOOTLOADER) {
 		board_reset_enter_bootloader();
 	}
 

platforms/nuttx/src/px4/nxp/s32k3xx/board_reset/board_reset.cpp

@@ -39,6 +39,7 @@
  */
 
 #include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/shutdown.h>
 #include <errno.h>
 #include <nuttx/board.h>
 #include <hardware/s32k3xx_mc_me.h>

platforms/nuttx/src/px4/rpi/rpi_common/board_reset/board_reset.cpp

@@ -38,6 +38,7 @@
  */
 
 #include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/shutdown.h>
 #include <systemlib/px4_macros.h>
 #include <errno.h>
 
@@ -119,7 +120,7 @@
 
 int board_reset(int status)
 {
-	if (status == 1) {
+	if (status == REBOOT_TO_BOOTLOADER) {
 		// board_configure_reset(BOARD_RESET_MODE_BOOT_TO_BL, 0);
 	}
 

platforms/nuttx/src/px4/stm/stm32_common/board_reset/board_reset.cpp

@@ -38,6 +38,7 @@
  */
 
 #include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/shutdown.h>
 #include <systemlib/px4_macros.h>
 #include <errno.h>
 #include <stm32_pwr.h>
@@ -117,7 +118,7 @@ int board_configure_reset(reset_mode_e mode, uint32_t arg)
 
 int board_reset(int status)
 {
-	if (status == 1) {
+	if (status == REBOOT_TO_BOOTLOADER) {
 		board_configure_reset(BOARD_RESET_MODE_BOOT_TO_BL, 0);
 	}
 

src/modules/commander/Commander.cpp

@@ -1201,7 +1201,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 
 #if defined(CONFIG_BOARDCTL_RESET)
 
-			} else if ((param1 == 1) && !isArmed() && (px4_reboot_request(false, 400_ms) == 0)) {
+			} else if ((param1 == 1) && !isArmed() && (px4_reboot_request(REBOOT_REQUEST, 400_ms) == 0)) {
 				// 1: Reboot autopilot
 				answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 
@@ -1221,7 +1221,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 
 #if defined(CONFIG_BOARDCTL_RESET)
 
-			} else if ((param1 == 3) && !isArmed() && (px4_reboot_request(true, 400_ms) == 0)) {
+			} else if ((param1 == 3) && !isArmed() && (px4_reboot_request(REBOOT_TO_BOOTLOADER, 400_ms) == 0)) {
 				// 3: Reboot autopilot and keep it in the bootloader until upgraded.
 				answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 

src/modules/commander/worker_thread.cpp

@@ -177,7 +177,7 @@ void WorkerThread::threadEntry()
 			param_reset_specific(reset_cal, sizeof(reset_cal) / sizeof(reset_cal[0]));
 			_ret_value = param_save_default(true);
 #if defined(CONFIG_BOARDCTL_RESET)
-			px4_reboot_request(false, 400_ms);
+			px4_reboot_request(REBOOT_REQUEST, 400_ms);
 #endif // CONFIG_BOARDCTL_RESET
 			break;
 		}

src/systemcmds/netman/netman.cpp

@@ -375,7 +375,7 @@ write_reboot:
 
 	sleep(1);
 
-	px4_reboot_request(false);
+	px4_reboot_request(REBOOT_REQUEST);
 
 	while (1) { px4_usleep(1); } // this command should not return on success
 

src/systemcmds/reboot/reboot.cpp

@@ -44,6 +44,7 @@
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/shutdown.h>
 #include <string.h>
+#include <board_config.h>
 
 static void print_usage()
 {
@@ -51,6 +52,9 @@ static void print_usage()
 
 	PRINT_MODULE_USAGE_NAME_SIMPLE("reboot", "command");
 	PRINT_MODULE_USAGE_PARAM_FLAG('b', "Reboot into bootloader", true);
+#ifdef BOARD_HAS_ISP_BOOTLOADER
+	PRINT_MODULE_USAGE_PARAM_FLAG('i', "Reboot into ISP (1st stage bootloader)", true);
+#endif
 
 	PRINT_MODULE_USAGE_ARG("lock|unlock", "Take/release the shutdown lock (for testing)", true);
 }
@@ -58,17 +62,24 @@ static void print_usage()
 extern "C" __EXPORT int reboot_main(int argc, char *argv[])
 {
 	int ch;
-	bool to_bootloader = false;
+	reboot_request_t request = REBOOT_REQUEST;
 
 	int myoptind = 1;
 	const char *myoptarg = nullptr;
 
-	while ((ch = px4_getopt(argc, argv, "b", &myoptind, &myoptarg)) != -1) {
+	while ((ch = px4_getopt(argc, argv, "bi", &myoptind, &myoptarg)) != -1) {
 		switch (ch) {
 		case 'b':
-			to_bootloader = true;
+			request = REBOOT_TO_BOOTLOADER;
 			break;
 
+#ifdef BOARD_HAS_ISP_BOOTLOADER
+
+		case 'i':
+			request = REBOOT_TO_ISP;
+			break;
+#endif
+
 		default:
 			print_usage();
 			return 1;
@@ -98,7 +109,7 @@ extern "C" __EXPORT int reboot_main(int argc, char *argv[])
 		return ret;
 	}
 
-	int ret = px4_reboot_request(to_bootloader);
+	int ret = px4_reboot_request(request);
 
 	if (ret < 0) {
 		PX4_ERR("reboot failed (%i)", ret);