From 916ffef04c5cd057a8080831ac695a4f144e56f5 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 6 May 2021 14:16:46 +0200
Subject: [PATCH] manual_control: send initial mode in the beginning

In order for commander to know the desired mode we need to send the
initial position of the mode slot.
---
 src/modules/manual_control/ManualControl.cpp | 87 +++++++++++---------
 src/modules/manual_control/ManualControl.hpp |  1 +
 2 files changed, 49 insertions(+), 39 deletions(-)

diff --git a/src/modules/manual_control/ManualControl.cpp b/src/modules/manual_control/ManualControl.cpp
index 761f25a6a9..deb3ac7b86 100644
--- a/src/modules/manual_control/ManualControl.cpp
+++ b/src/modules/manual_control/ManualControl.cpp
@@ -164,44 +164,7 @@ void ManualControl::Run()
 			if (_selector.setpoint().data_source == manual_control_input_s::SOURCE_RC) {
 				if (_previous_switches_initialized) {
 					if (switches.mode_slot != _previous_switches.mode_slot) {
-
-						switch (switches.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();
-							break;
-
-						case manual_control_switches_s::MODE_SLOT_2:
-							_last_mode_slot_flt = _param_fltmode_2.get();
-							break;
-
-						case manual_control_switches_s::MODE_SLOT_3:
-							_last_mode_slot_flt = _param_fltmode_3.get();
-							break;
-
-						case manual_control_switches_s::MODE_SLOT_4:
-							_last_mode_slot_flt = _param_fltmode_4.get();
-							break;
-
-						case manual_control_switches_s::MODE_SLOT_5:
-							_last_mode_slot_flt = _param_fltmode_5.get();
-							break;
-
-						case manual_control_switches_s::MODE_SLOT_6:
-							_last_mode_slot_flt = _param_fltmode_6.get();
-							break;
-
-						default:
-							_last_mode_slot_flt = -1;
-							PX4_WARN("mode slot overflow");
-							break;
-
-						}
-
-						send_mode_command(_last_mode_slot_flt);
+						evaluate_mode_slot(switches.mode_slot);
 					}
 
 					if (switches.arm_switch != _previous_switches.arm_switch) {
@@ -268,10 +231,14 @@ void ManualControl::Run()
 							send_vtol_transition_command(vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC);
 						}
 					}
+
+				} else {
+					// Send an initial command to switch to the mode requested by R
+					evaluate_mode_slot(switches.mode_slot);
 				}
 
-				_previous_switches = switches;
 				_previous_switches_initialized = true;
+				_previous_switches = switches;
 
 			} else {
 				_previous_switches = {};
@@ -312,6 +279,48 @@ void ManualControl::Run()
 	perf_end(_loop_perf);
 }
 
+void ManualControl::evaluate_mode_slot(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();
+		break;
+
+	case manual_control_switches_s::MODE_SLOT_2:
+		_last_mode_slot_flt = _param_fltmode_2.get();
+		break;
+
+	case manual_control_switches_s::MODE_SLOT_3:
+		_last_mode_slot_flt = _param_fltmode_3.get();
+		break;
+
+	case manual_control_switches_s::MODE_SLOT_4:
+		_last_mode_slot_flt = _param_fltmode_4.get();
+		break;
+
+	case manual_control_switches_s::MODE_SLOT_5:
+		_last_mode_slot_flt = _param_fltmode_5.get();
+		break;
+
+	case manual_control_switches_s::MODE_SLOT_6:
+		_last_mode_slot_flt = _param_fltmode_6.get();
+		break;
+
+	default:
+		_last_mode_slot_flt = -1;
+		PX4_WARN("mode slot overflow");
+		break;
+
+	}
+
+	PX4_WARN("send mode slot: %d", _last_mode_slot_flt);
+	send_mode_command(_last_mode_slot_flt);
+}
+
 void ManualControl::send_mode_command(int32_t commander_main_state)
 {
 	if (commander_main_state == -1) {
diff --git a/src/modules/manual_control/ManualControl.hpp b/src/modules/manual_control/ManualControl.hpp
index 7a7c87ecf6..188103e550 100644
--- a/src/modules/manual_control/ManualControl.hpp
+++ b/src/modules/manual_control/ManualControl.hpp
@@ -84,6 +84,7 @@ private:
 
 	void Run() override;
 
+	void evaluate_mode_slot(uint8_t mode_slot);
 	void send_mode_command(int32_t commander_main_state);
 	void send_arm_command(bool should_arm, ArmingOrigin origin);
 	void send_rtl_command();