commander: reboot/shutdown usability fixes

- always check with state machine before reboot/shutdown
 - respect BOARD_HAS_POWER_CONTROL (shutdown from command, low battery, power button)
 - px4_shutdown_request add optional delay and always execute from HPWORK
 - px4_shutdown_request split out px4_reboot_request

src/drivers/uavcannode/UavcanNode.cpp

@@ -223,7 +223,7 @@ void UavcanNode::busevent_signal_trampoline()
 
 static void cb_reboot(const uavcan::TimerEvent &)
 {
-	px4_systemreset(false);
+	board_reset(0);
 }
 
 void UavcanNode::cb_beginfirmware_update(const uavcan::ReceivedDataStructure<UavcanNode::BeginFirmwareUpdate::Request>
@@ -276,7 +276,7 @@ class RestartRequestHandler: public uavcan::IRestartRequestHandler
 	{
 		PX4_INFO("UAVCAN: Restarting by request from %i\n", int(request_source.get()));
 		usleep(20 * 1000 * 1000);
-		px4_systemreset(false);
+		board_reset(0);
 		return true; // Will never be executed BTW
 	}
 } restart_request_handler;

src/modules/commander/Commander.cpp

@@ -95,7 +95,6 @@ typedef enum VEHICLE_MODE_FLAG {
 
 /* Mavlink log uORB handle */
 static orb_advert_t mavlink_log_pub = nullptr;
-static orb_advert_t power_button_state_pub = nullptr;
 
 /* flags */
 static volatile bool thread_should_exit = false;	/**< daemon exit flag */
@@ -116,6 +115,8 @@ void *commander_low_prio_loop(void *arg);
 static void answer_command(const vehicle_command_s &cmd, unsigned result,
 			   uORB::PublicationQueued<vehicle_command_ack_s> &command_ack_pub);
 
+#if defined(BOARD_HAS_POWER_CONTROL)
+static orb_advert_t power_button_state_pub = nullptr;
 static int power_button_state_notification_cb(board_power_button_state_notification_e request)
 {
 	// Note: this can be called from IRQ handlers, so we publish a message that will be handled
@@ -155,6 +156,7 @@ static int power_button_state_notification_cb(board_power_button_state_notificat
 
 	return ret;
 }
+#endif // BOARD_HAS_POWER_CONTROL
 
 #ifndef CONSTRAINED_FLASH
 static bool send_vehicle_command(uint16_t cmd, float param1 = NAN, float param2 = NAN, float param3 = NAN,
@@ -231,9 +233,6 @@ extern "C" __EXPORT int commander_main(int argc, char *argv[])
 		thread_should_exit = true;
 
 		Commander::main(argc, argv);
-
-		PX4_INFO("terminated.");
-
 		return 0;
 	}
 
@@ -992,50 +991,47 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
 		cmd_result = handle_command_motor_test(cmd);
 		break;
 
-	case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
+	case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: {
 
-		// do nothing for autopilot
-		if (((int)(cmd.param1)) == 0) {
-			answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
+			const int param1 = cmd.param1;
 
-			break;
-		}
-
-		if (shutdown_if_allowed()) {
-			bool shutdown_ret_val = PX4_ERROR;
-
-			if (((int)(cmd.param1)) == 1) {
+			if (param1 == 0) {
+				// 0: Do nothing for autopilot
 				answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
-				px4_usleep(200000);
-				// reboot
-				shutdown_ret_val = px4_shutdown_request(true, false);
 
-			} else if (((int)(cmd.param1)) == 2) {
-				answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
-				px4_usleep(200000);
-				// shutdown
-				shutdown_ret_val = px4_shutdown_request(false, false);
+#if defined(CONFIG_BOARDCTL_RESET)
 
-			} else if (((int)(cmd.param1)) == 3) {
+			} else if ((param1 == 1) && shutdown_if_allowed() && (px4_reboot_request(false, 400_ms) == 0)) {
+				// 1: Reboot autopilot
 				answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
-				px4_usleep(200000);
-				// reboot to bootloader
-				shutdown_ret_val = px4_shutdown_request(true, true);
+
+				while (1) { px4_usleep(1); }
+
+#endif // CONFIG_BOARDCTL_RESET
+
+#if defined(CONFIG_BOARDCTL_POWEROFF)
+
+			} else if ((param1 == 2) && shutdown_if_allowed() && (px4_shutdown_request(400_ms) == 0)) {
+				// 2: Shutdown autopilot
+				answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
+
+				while (1) { px4_usleep(1); }
+
+#endif // CONFIG_BOARDCTL_POWEROFF
+
+#if defined(CONFIG_BOARDCTL_RESET)
+
+			} else if ((param1 == 3) && shutdown_if_allowed() && (px4_reboot_request(true, 400_ms) == 0)) {
+				// 3: Reboot autopilot and keep it in the bootloader until upgraded.
+				answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
+
+				while (1) { px4_usleep(1); }
+
+#endif // CONFIG_BOARDCTL_RESET
 
 			} else {
 				answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub);
-				break;
 			}
-
-			if (shutdown_ret_val) {
-				mavlink_log_critical(&mavlink_log_pub, "System does not support shutdown");
-
-			} else {
-				while (1) { px4_usleep(1); }
-			}
-
-		} else {
-			answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub);
 		}
 
 		break;
@@ -1219,6 +1215,7 @@ Commander::run()
 	led_init();
 	buzzer_init();
 
+#if defined(BOARD_HAS_POWER_CONTROL)
 	{
 		// we need to do an initial publication to make sure uORB allocates the buffer, which cannot happen
 		// in IRQ context.
@@ -1234,6 +1231,7 @@ Commander::run()
 		PX4_ERR("Failed to register power notification callback");
 	}
 
+#endif // BOARD_HAS_POWER_CONTROL
 
 	get_circuit_breaker_params();
 
@@ -1409,17 +1407,27 @@ Commander::run()
 		/* Update OA parameter */
 		status_flags.avoidance_system_required = _param_com_obs_avoid.get();
 
+#if defined(BOARD_HAS_POWER_CONTROL)
+
 		/* handle power button state */
 		if (_power_button_state_sub.updated()) {
 			power_button_state_s button_state;
 
 			if (_power_button_state_sub.copy(&button_state)) {
 				if (button_state.event == power_button_state_s::PWR_BUTTON_STATE_REQUEST_SHUTDOWN) {
-					px4_shutdown_request(false, false);
+#if defined(CONFIG_BOARDCTL_POWEROFF)
+
+					if (shutdown_if_allowed() && (px4_shutdown_request() == 0)) {
+						while (1) { px4_usleep(1); }
+					}
+
+#endif // CONFIG_BOARDCTL_POWEROFF
 				}
 			}
 		}
 
+#endif // BOARD_HAS_POWER_CONTROL
+
 		_sp_man_sub.update(&_sp_man);
 
 		offboard_control_update();
@@ -1439,8 +1447,10 @@ Commander::run()
 					status_flags.condition_power_input_valid = true;
 				}
 
+#if defined(CONFIG_BOARDCTL_RESET)
+
 				/* if the USB hardware connection went away, reboot */
-				if (status_flags.usb_connected && !system_power.usb_connected && shutdown_if_allowed()) {
+				if (status_flags.usb_connected && !system_power.usb_connected) {
 					/*
 					 * Apparently the USB cable went away but we are still powered,
 					 * so we bring the system back to a nominal state for flight.
@@ -1449,10 +1459,14 @@ Commander::run()
 					 * for flight and continuing to run it would add a software risk
 					 * without a need. The clean approach to unload it is to reboot.
 					 */
-					mavlink_log_critical(&mavlink_log_pub, "USB disconnected, rebooting for flight safety")
-					px4_usleep(400000);
-					px4_shutdown_request(true, false);
+					if (shutdown_if_allowed() && (px4_reboot_request(400_ms) == 0)) {
+						mavlink_log_critical(&mavlink_log_pub, "USB disconnected, rebooting for flight safety");
+
+						while (1) { px4_usleep(1); }
+					}
 				}
+
+#endif // CONFIG_BOARDCTL_RESET
 			}
 		}
 
@@ -3471,12 +3485,6 @@ void *commander_low_prio_loop(void *arg)
 
 					} else if (((int)(cmd.param1)) == 1) {
 
-#ifdef __PX4_QURT
-						// TODO FIXME: on snapdragon the save happens too early when the params
-						// are not set yet. We therefore need to wait some time first.
-						px4_usleep(1000000);
-#endif
-
 						int ret = param_save_default();
 
 						if (ret == OK) {
@@ -3883,18 +3891,19 @@ void Commander::battery_status_check()
 	// Handle shutdown request from emergency battery action
 	if (update_internal_battery_state) {
 
-		if ((_battery_warning == battery_status_s::BATTERY_WARNING_EMERGENCY) && shutdown_if_allowed()) {
-			mavlink_log_critical(&mavlink_log_pub, "Dangerously low battery! Shutting system down");
-			px4_usleep(200000);
+		if (_battery_warning == battery_status_s::BATTERY_WARNING_EMERGENCY) {
+#if defined(CONFIG_BOARDCTL_POWEROFF)
 
-			int ret_val = px4_shutdown_request(false, false);
+			if (shutdown_if_allowed() && (px4_shutdown_request(400_ms) == 0)) {
+				mavlink_log_critical(&mavlink_log_pub, "Dangerously low battery! Shutting system down");
 
-			if (ret_val) {
-				mavlink_log_critical(&mavlink_log_pub, "System does not support shutdown");
+				while (1) { px4_usleep(1); }
 
 			} else {
-				while (1) { px4_usleep(1); }
+				mavlink_log_critical(&mavlink_log_pub, "System does not support shutdown");
 			}
+
+#endif // CONFIG_BOARDCTL_POWEROFF
 		}
 	}
 }

src/modules/commander/Commander.hpp

@@ -398,7 +398,6 @@ private:
 	uORB::Subscription					_iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)};
 	uORB::Subscription					_land_detector_sub{ORB_ID(vehicle_land_detected)};
 	uORB::Subscription					_parameter_update_sub{ORB_ID(parameter_update)};
-	uORB::Subscription					_power_button_state_sub{ORB_ID(power_button_state)};
 	uORB::Subscription					_safety_sub{ORB_ID(safety)};
 	uORB::Subscription					_sp_man_sub{ORB_ID(manual_control_setpoint)};
 	uORB::Subscription					_subsys_sub{ORB_ID(subsystem_info)};
@@ -407,6 +406,10 @@ private:
 	uORB::Subscription					_vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
 	uORB::Subscription					_vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)};
 
+#if defined(BOARD_HAS_POWER_CONTROL)
+	uORB::Subscription					_power_button_state_sub {ORB_ID(power_button_state)};
+#endif // BOARD_HAS_POWER_CONTROL
+
 	uORB::SubscriptionData<airspeed_s>			_airspeed_sub{ORB_ID(airspeed)};
 	uORB::SubscriptionData<estimator_status_s>		_estimator_status_sub{ORB_ID(estimator_status)};
 	uORB::SubscriptionData<mission_result_s>		_mission_result_sub{ORB_ID(mission_result)};

src/modules/replay/Replay.cpp

@@ -852,7 +852,7 @@ Replay::run()
 
 		//TODO: add parameter -q?
 		replay_file.close();
-		px4_shutdown_request(false, false);
+		px4_shutdown_request();
 	}
 
 	onExitMainLoop();

src/systemcmds/reboot/CMakeLists.txt

@@ -35,7 +35,8 @@ px4_add_module(
 	MAIN reboot
 	COMPILE_FLAGS
 	SRCS
-		reboot.c
+		reboot.cpp
 	DEPENDS
+		px4_platform
 	)
 

src/systemcmds/reboot/reboot.cpp

@@ -45,9 +45,7 @@
 #include <px4_platform_common/shutdown.h>
 #include <string.h>
 
-__EXPORT int reboot_main(int argc, char *argv[]);
-
-static void print_usage(void)
+static void print_usage()
 {
 	PRINT_MODULE_DESCRIPTION("Reboot the system");
 
@@ -57,13 +55,13 @@ static void print_usage(void)
 	PRINT_MODULE_USAGE_ARG("lock|unlock", "Take/release the shutdown lock (for testing)", true);
 }
 
-int reboot_main(int argc, char *argv[])
+extern "C" __EXPORT int reboot_main(int argc, char *argv[])
 {
 	int ch;
 	bool to_bootloader = false;
 
 	int myoptind = 1;
-	const char *myoptarg = NULL;
+	const char *myoptarg = nullptr;
 
 	while ((ch = px4_getopt(argc, argv, "b", &myoptind, &myoptarg)) != -1) {
 		switch (ch) {
@@ -100,7 +98,7 @@ int reboot_main(int argc, char *argv[])
 		return ret;
 	}
 
-	int ret = px4_shutdown_request(true, to_bootloader);
+	int ret = px4_reboot_request(to_bootloader);
 
 	if (ret < 0) {
 		PX4_ERR("reboot failed (%i)", ret);

src/systemcmds/shutdown/CMakeLists.txt

@@ -33,7 +33,6 @@
 px4_add_module(
 	MODULE systemcmds__shutdown
 	MAIN shutdown
-	COMPILE_FLAGS
 	SRCS
-		shutdown.c
+		shutdown.cpp
 	)

src/systemcmds/shutdown/shutdown.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2016 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2016-2020 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -35,16 +35,10 @@
  * @file shutdown.c
  * Tool similar to UNIX shutdown command.
  */
-
+#include <px4_platform_common/shutdown.h>
 #include <px4_platform_common/tasks.h>
 
-__EXPORT int shutdown_main(int argc, char *argv[]);
-
-int shutdown_main(int argc, char *argv[])
+extern "C" __EXPORT int shutdown_main(int argc, char *argv[])
 {
-	(void)argc;
-	(void)argv;
-
-	const bool to_bootloader = false;
-	px4_systemreset(to_bootloader);
+	return px4_shutdown_request();
 }

src/systemcmds/tests/CMakeLists.txt

@@ -58,7 +58,6 @@ set(srcs
 	test_microbench_matrix.cpp
 	test_microbench_uorb.cpp
 	test_mixer.cpp
-	test_mount.c
 	test_param.c
 	test_parameters.cpp
 	test_perf.c
@@ -80,6 +79,7 @@ set(srcs
 
 if(${PX4_PLATFORM} STREQUAL "nuttx")
 	list(APPEND srcs
+		test_mount.cpp
 		test_time.c
 		test_uart_break.c
 		)

src/systemcmds/tests/test_mount.cpp

@@ -40,6 +40,7 @@
 #include <px4_platform_common/px4_config.h>
 #include <px4_platform_common/posix.h>
 #include <px4_platform_common/tasks.h>
+#include <px4_platform_common/shutdown.h>
 
 #include <sys/stat.h>
 #include <dirent.h>
@@ -56,8 +57,7 @@
 const int fsync_tries = 1;
 const int abort_tries = 10;
 
-int
-test_mount(int argc, char *argv[])
+int test_mount(int argc, char *argv[])
 {
 	const unsigned iterations = 2000;
 	const unsigned alignments = 10;
@@ -289,7 +289,7 @@ test_mount(int argc, char *argv[])
 	fsync(fileno(stdout));
 	fsync(fileno(stderr));
 	px4_usleep(50000);
-	px4_systemreset(false);
+	px4_reboot_request();
 
 	/* never going to get here */
 	return 0;

src/systemcmds/tests/tests_main.c

@@ -69,6 +69,7 @@ const struct {
 #ifdef __PX4_NUTTX
 	{"file",		test_file,		OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"led",			test_led,		0},
+	{"mount",		test_mount,		OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"time",		test_time,		OPT_NOJIGTEST},
 	{"uart_baudchange",	test_uart_baudchange,	OPT_NOJIGTEST},
 	{"uart_break",		test_uart_break,	OPT_NOJIGTEST | OPT_NOALLTEST},
@@ -102,7 +103,6 @@ const struct {
 	{"microbench_uorb",	test_microbench_uorb,	0},
 	{"mixer",		test_mixer,		OPT_NOJIGTEST},
 	{"mixer",		test_mixer,		OPT_NOJIGTEST},
-	{"mount",		test_mount,		OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"param",		test_param,		0},
 	{"parameters",		test_parameters,	0},
 	{"perf",		test_perf,		OPT_NOJIGTEST},