Use mode_request for RC mode switching

msg/CMakeLists.txt

@@ -109,6 +109,7 @@ set(msg_files
 	mavlink_log.msg
 	mission.msg
 	mission_result.msg
+	mode_request.msg
 	mount_orientation.msg
 	multirotor_motor_limits.msg
 	navigator_mission_item.msg

msg/mode_request.msg

@@ -0,0 +1,7 @@
+uint64 timestamp # time since system start (microseconds)
+
+uint8 mode # what mode is requested according to commander_state.MAIN_STATE_
+
+uint8 source # how the request was triggered
+uint8 SOURCE_RC_MODE_SLOT = 0
+uint8 SOURCE_RC_SWITCH = 1

src/modules/commander/Commander.cpp

@@ -2512,6 +2512,14 @@ Commander::run()
 			}
 		}
 
+		while (_mode_request_sub.updated()) {
+			mode_request_s mode_request;
+
+			if (_mode_request_sub.copy(&mode_request)) {
+				main_state_transition(_status, mode_request.mode, _status_flags, _internal_state);
+			}
+		}
+
 		/* Check for failure detector status */
 		if (_failure_detector.update(_status, _vehicle_control_mode)) {
 			_status.failure_detector_status = _failure_detector.getStatus().value;

src/modules/commander/Commander.hpp

@@ -73,6 +73,7 @@
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/mission.h>
 #include <uORB/topics/mission_result.h>
+#include <uORB/topics/mode_request.h>
 #include <uORB/topics/offboard_control_mode.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/power_button_state.h>
@@ -399,6 +400,7 @@ private:
 	uORB::Subscription					_iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)};
 	uORB::Subscription					_land_detector_sub{ORB_ID(vehicle_land_detected)};
 	uORB::Subscription					_manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
+	uORB::Subscription					_mode_request_sub {ORB_ID(mode_request)};
 	uORB::Subscription					_safety_sub{ORB_ID(safety)};
 	uORB::Subscription					_system_power_sub{ORB_ID(system_power)};
 	uORB::Subscription					_vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};

src/modules/logger/logged_topics.cpp

@@ -73,6 +73,7 @@ void LoggedTopics::add_default_topics()
 	add_topic("manual_control_setpoint", 200);
 	add_topic("manual_control_switches");
 	add_topic("mission_result");
+	add_topic("mode_request");
 	add_topic("navigator_mission_item");
 	add_topic("offboard_control_mode", 100);
 	add_topic("onboard_computer_status", 10);

src/modules/manual_control/ManualControl.cpp

@@ -156,7 +156,7 @@ void ManualControl::Run()
 			if (_selector.setpoint().chosen_input.data_source == manual_control_input_s::SOURCE_RC) {
 				if (_previous_switches_initialized) {
 					if (switches.mode_slot != _previous_switches.mode_slot) {
-						evaluate_mode_slot(switches.mode_slot);
+						evaluateModeSlot(switches.mode_slot);
 					}
 
 					if (_param_com_arm_swisbtn.get()) {
@@ -182,28 +182,28 @@ void ManualControl::Run()
 
 					if (switches.return_switch != _previous_switches.return_switch) {
 						if (switches.return_switch == manual_control_switches_s::SWITCH_POS_ON) {
-							send_rtl_command();
+							sendModeRequest(commander_state_s::MAIN_STATE_AUTO_RTL, mode_request_s::SOURCE_RC_SWITCH);
 
 						} else if (switches.return_switch == manual_control_switches_s::SWITCH_POS_OFF) {
-							send_mode_command(_last_mode_slot_flt);
+							evaluateModeSlot(switches.mode_slot);
 						}
 					}
 
 					if (switches.loiter_switch != _previous_switches.loiter_switch) {
 						if (switches.loiter_switch == manual_control_switches_s::SWITCH_POS_ON) {
-							send_loiter_command();
+							sendModeRequest(commander_state_s::MAIN_STATE_AUTO_LOITER, mode_request_s::SOURCE_RC_SWITCH);
 
 						} else if (switches.loiter_switch == manual_control_switches_s::SWITCH_POS_OFF) {
-							send_mode_command(_last_mode_slot_flt);
+							evaluateModeSlot(switches.mode_slot);
 						}
 					}
 
 					if (switches.offboard_switch != _previous_switches.offboard_switch) {
 						if (switches.offboard_switch == manual_control_switches_s::SWITCH_POS_ON) {
-							send_offboard_command();
+							sendModeRequest(commander_state_s::MAIN_STATE_OFFBOARD, mode_request_s::SOURCE_RC_SWITCH);
 
 						} else if (switches.offboard_switch == manual_control_switches_s::SWITCH_POS_OFF) {
-							send_mode_command(_last_mode_slot_flt);
+							evaluateModeSlot(switches.mode_slot);
 						}
 					}
 
@@ -238,7 +238,7 @@ void ManualControl::Run()
 
 				} else {
 					// Send an initial command to switch to the mode requested by RC
-					evaluate_mode_slot(switches.mode_slot);
+					evaluateModeSlot(switches.mode_slot);
 				}
 
 				_previous_switches_initialized = true;
@@ -246,7 +246,6 @@ void ManualControl::Run()
 
 			} else {
 				_previous_switches_initialized = false;
-				_last_mode_slot_flt = -1;
 			}
 		}
 
@@ -285,135 +284,49 @@ void ManualControl::Run()
 	perf_end(_loop_perf);
 }
 
-void ManualControl::evaluate_mode_slot(uint8_t mode_slot)
+void ManualControl::evaluateModeSlot(uint8_t mode_slot)
 {
 	switch (mode_slot) {
 	case manual_control_switches_s::MODE_SLOT_NONE:
-		_last_mode_slot_flt = -1;
 		break;
 
 	case manual_control_switches_s::MODE_SLOT_1:
-		_last_mode_slot_flt = _param_fltmode_1.get();
+		sendModeRequest(_param_fltmode_1.get(), mode_request_s::SOURCE_RC_MODE_SLOT);
 		break;
 
 	case manual_control_switches_s::MODE_SLOT_2:
-		_last_mode_slot_flt = _param_fltmode_2.get();
+		sendModeRequest(_param_fltmode_2.get(), mode_request_s::SOURCE_RC_MODE_SLOT);
 		break;
 
 	case manual_control_switches_s::MODE_SLOT_3:
-		_last_mode_slot_flt = _param_fltmode_3.get();
+		sendModeRequest(_param_fltmode_3.get(), mode_request_s::SOURCE_RC_MODE_SLOT);
 		break;
 
 	case manual_control_switches_s::MODE_SLOT_4:
-		_last_mode_slot_flt = _param_fltmode_4.get();
+		sendModeRequest(_param_fltmode_4.get(), mode_request_s::SOURCE_RC_MODE_SLOT);
 		break;
 
 	case manual_control_switches_s::MODE_SLOT_5:
-		_last_mode_slot_flt = _param_fltmode_5.get();
+		sendModeRequest(_param_fltmode_5.get(), mode_request_s::SOURCE_RC_MODE_SLOT);
 		break;
 
 	case manual_control_switches_s::MODE_SLOT_6:
-		_last_mode_slot_flt = _param_fltmode_6.get();
+		sendModeRequest(_param_fltmode_6.get(), mode_request_s::SOURCE_RC_MODE_SLOT);
 		break;
 
 	default:
-		_last_mode_slot_flt = -1;
 		PX4_WARN("mode slot overflow");
 		break;
-
 	}
-
-	send_mode_command(_last_mode_slot_flt);
 }
 
-void ManualControl::send_mode_command(int32_t commander_main_state)
+void ManualControl::sendModeRequest(uint8_t mode, uint8_t source)
 {
-	if (commander_main_state == -1) {
+	mode_request_s mode_request{};
-		// Not assigned.
+	mode_request.mode = mode;
-		return;
+	mode_request.source = source;
-	}
+	mode_request.timestamp = hrt_absolute_time();
-
+	_mode_request_pub.publish(mode_request);
-	vehicle_command_s command{};
-	command.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE;
-	command.param1 = 1.0f;
-
-	switch (commander_main_state) {
-	case commander_state_s::MAIN_STATE_MANUAL:
-		command.param2 = PX4_CUSTOM_MAIN_MODE_MANUAL;
-		break;
-
-	case commander_state_s::MAIN_STATE_ALTCTL:
-		command.param2 = PX4_CUSTOM_MAIN_MODE_ALTCTL;
-		break;
-
-	case commander_state_s::MAIN_STATE_POSCTL:
-		command.param2 = PX4_CUSTOM_MAIN_MODE_POSCTL;
-		command.param3 = PX4_CUSTOM_SUB_MODE_POSCTL_POSCTL;
-		break;
-
-	case commander_state_s::MAIN_STATE_AUTO_MISSION:
-		command.param2 = PX4_CUSTOM_MAIN_MODE_AUTO;
-		command.param3 = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
-		break;
-
-	case commander_state_s::MAIN_STATE_AUTO_LOITER:
-		command.param2 = PX4_CUSTOM_MAIN_MODE_AUTO;
-		command.param3 = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
-		break;
-
-	case commander_state_s::MAIN_STATE_AUTO_RTL:
-		command.param2 = PX4_CUSTOM_MAIN_MODE_AUTO;
-		command.param3 = PX4_CUSTOM_SUB_MODE_AUTO_RTL;
-		break;
-
-	case commander_state_s::MAIN_STATE_ACRO:
-		command.param2 = PX4_CUSTOM_MAIN_MODE_ACRO;
-		break;
-
-	case commander_state_s::MAIN_STATE_OFFBOARD:
-		command.param2 = PX4_CUSTOM_MAIN_MODE_OFFBOARD;
-		break;
-
-	case commander_state_s::MAIN_STATE_STAB:
-		command.param2 = PX4_CUSTOM_MAIN_MODE_STABILIZED;
-		break;
-
-	case commander_state_s::MAIN_STATE_AUTO_TAKEOFF:
-		command.param2 = PX4_CUSTOM_MAIN_MODE_AUTO;
-		command.param3 = PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF;
-		break;
-
-	case commander_state_s::MAIN_STATE_AUTO_LAND:
-		command.param2 = PX4_CUSTOM_MAIN_MODE_AUTO;
-		command.param3 = PX4_CUSTOM_SUB_MODE_AUTO_LAND;
-		break;
-
-	case commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET:
-		command.param2 = PX4_CUSTOM_MAIN_MODE_AUTO;
-		command.param3 = PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET;
-		break;
-
-	case commander_state_s::MAIN_STATE_AUTO_PRECLAND:
-
-	// FALLTHROUGH
-	case commander_state_s::MAIN_STATE_ORBIT:
-		PX4_WARN("Unhandled main_state");
-		return;
-
-	case commander_state_s::MAIN_STATE_MAX:
-
-	// FALLTHROUGH
-	default:
-		PX4_WARN("Unknown main_state");
-		return;
-	}
-
-	command.target_system = _param_mav_sys_id.get();
-	command.target_component = _param_mav_comp_id.get();
-
-	uORB::Publication<vehicle_command_s> command_pub{ORB_ID(vehicle_command)};
-	command.timestamp = hrt_absolute_time();
-	command_pub.publish(command);
 }
 
 void ManualControl::sendArmRequest(int8_t action, int8_t source)
@@ -425,50 +338,6 @@ void ManualControl::sendArmRequest(int8_t action, int8_t source)
 	_arm_request_pub.publish(arm_request);
 }
 
-void ManualControl::send_rtl_command()
-{
-	vehicle_command_s command{};
-	command.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE;
-	command.param1 = 1.0f;
-	command.param2 = PX4_CUSTOM_MAIN_MODE_AUTO;
-	command.param3 = PX4_CUSTOM_SUB_MODE_AUTO_RTL;
-	command.target_system = _param_mav_sys_id.get();
-	command.target_component = _param_mav_comp_id.get();
-
-	uORB::Publication<vehicle_command_s> command_pub{ORB_ID(vehicle_command)};
-	command.timestamp = hrt_absolute_time();
-	command_pub.publish(command);
-}
-
-void ManualControl::send_loiter_command()
-{
-	vehicle_command_s command{};
-	command.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE;
-	command.param1 = 1.0f;
-	command.param2 = PX4_CUSTOM_MAIN_MODE_AUTO;
-	command.param3 = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
-	command.target_system = _param_mav_sys_id.get();
-	command.target_component = _param_mav_comp_id.get();
-
-	uORB::Publication<vehicle_command_s> command_pub{ORB_ID(vehicle_command)};
-	command.timestamp = hrt_absolute_time();
-	command_pub.publish(command);
-}
-
-void ManualControl::send_offboard_command()
-{
-	vehicle_command_s command{};
-	command.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE;
-	command.param1 = 1.0f;
-	command.param2 = PX4_CUSTOM_MAIN_MODE_OFFBOARD;
-	command.target_system = _param_mav_sys_id.get();
-	command.target_component = _param_mav_comp_id.get();
-
-	uORB::Publication<vehicle_command_s> command_pub{ORB_ID(vehicle_command)};
-	command.timestamp = hrt_absolute_time();
-	command_pub.publish(command);
-}
-
 void ManualControl::publish_landing_gear(int8_t action)
 {
 	landing_gear_s landing_gear{};

src/modules/manual_control/ManualControl.hpp

@@ -44,6 +44,7 @@
 #include <uORB/topics/manual_control_input.h>
 #include <uORB/topics/manual_control_switches.h>
 #include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/mode_request.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
@@ -119,17 +120,15 @@ private:
 
 	void Run() override;
 
-	void evaluate_mode_slot(uint8_t mode_slot);
+	void evaluateModeSlot(uint8_t mode_slot);
-	void send_mode_command(int32_t commander_main_state);
+	void sendModeRequest(uint8_t mode, uint8_t source);
 	void sendArmRequest(int8_t action, int8_t source);
-	void send_rtl_command();
-	void send_loiter_command();
-	void send_offboard_command();
 	void publish_landing_gear(int8_t action);
 	void send_vtol_transition_command(uint8_t action);
 
 	uORB::Publication<arm_request_s> _arm_request_pub{ORB_ID(arm_request)};
 	uORB::Publication<manual_control_setpoint_s> _manual_control_setpoint_pub{ORB_ID(manual_control_setpoint)};
+	uORB::Publication<mode_request_s> _mode_request_pub{ORB_ID(mode_request)};
 
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 	uORB::SubscriptionCallbackWorkItem _manual_control_input_subs[MAX_MANUAL_INPUT_COUNT] {
@@ -153,7 +152,6 @@ private:
 
 	manual_control_switches_s _previous_switches{};
 	bool _previous_switches_initialized{false};
-	int32_t _last_mode_slot_flt{-1};
 
 	hrt_abstime _last_time{0};