mavlink and commander app: Add support for manual input, either directly or via remote control

src/modules/mavlink/mavlink_main.cpp

@@ -123,6 +123,7 @@ Mavlink::Mavlink() :
 	_mavlink_fd(-1),
 	_task_running(false),
 	_hil_enabled(false),
+	_generate_rc(false),
 	_use_hil_gps(false),
 	_forward_externalsp(false),
 	_is_usb_uart(false),
@@ -1497,6 +1498,8 @@ Mavlink::task_main(int argc, char *argv[])
 		if (status_sub->update(&status_time, &status)) {
 			/* switch HIL mode if required */
 			set_hil_enabled(status.hil_state == vehicle_status_s::HIL_STATE_ON);
+
+			set_manual_input_mode_generation(status.rc_input_off == vehicle_status_s::RC_IN_MODE_GENERATED);
 		}
 
 		/* check for requested subscriptions */

src/modules/mavlink/mavlink_main.h

@@ -170,6 +170,23 @@ public:
 	 */
 	int			set_hil_enabled(bool hil_enabled);
 
+	/**
+	 * Set manual input generation mode
+	 *
+	 * Set to true to generate RC_INPUT messages on the system bus from
+	 * MAVLink messages.
+	 *
+	 * @param generation_enabled If set to true, generate RC_INPUT messages
+	 */
+	void			set_manual_input_mode_generation(bool generation_enabled) { _generate_rc = generation_enabled; }
+
+	/**
+	 * Get the manual input generation mode
+	 *
+	 * @return true if manual inputs should generate RC data
+	 */
+	bool			get_manual_input_mode_generation() { return _generate_rc; }
+
 	void			send_message(const uint8_t msgid, const void *msg, uint8_t component_ID = 0);
 
 	/**
@@ -287,6 +304,7 @@ private:
 
 	/* states */
 	bool			_hil_enabled;		/**< Hardware In the Loop mode */
+	bool			_generate_rc;		/**< Generate RC messages from manual input MAVLink messages */
 	bool			_use_hil_gps;		/**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */
 	bool			_forward_externalsp;	/**< Forward external setpoint messages to controllers directly if in offboard mode */
 	bool			_is_usb_uart;		/**< Port is USB */

src/modules/mavlink/mavlink_receiver.cpp

@@ -56,6 +56,7 @@
 #include <drivers/drv_mag.h>
 #include <drivers/drv_baro.h>
 #include <drivers/drv_range_finder.h>
+#include <drivers/drv_rc_input.h>
 #include <time.h>
 #include <float.h>
 #include <unistd.h>
@@ -914,29 +915,60 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
 		return;
 	}
 
-	struct manual_control_setpoint_s manual;
-	memset(&manual, 0, sizeof(manual));
+	if (_mavlink->get_manual_input_mode_generation()) {
 
-	manual.timestamp = hrt_absolute_time();
-	manual.x = man.x / 1000.0f;
-	manual.y = man.y / 1000.0f;
-	manual.r = man.r / 1000.0f;
-	manual.z = man.z / 1000.0f;
+		struct rc_input_values rc = {};
+		rc.timestamp_publication = hrt_absolute_time();
+		rc.timestamp_last_signal = rc.timestamp_publication;
 
-	manual.mode_switch = decode_switch_pos(man.buttons, 0);
-	manual.return_switch = decode_switch_pos(man.buttons, 1);
-	manual.posctl_switch = decode_switch_pos(man.buttons, 2);
-	manual.loiter_switch = decode_switch_pos(man.buttons, 3);
-	manual.acro_switch = decode_switch_pos(man.buttons, 4);
-	manual.offboard_switch = decode_switch_pos(man.buttons, 5);
+		rc.channel_count = 8;
+		rc.rc_failsafe = false;
+		rc.rc_lost = false;
+		rc.rc_lost_frame_count = 0;
+		rc.rc_total_frame_count = 1;
+		rc.rc_ppm_frame_length = (2 * rc.channel_count + 4) * 1000;
+		rc.input_source = RC_INPUT_SOURCE_MAVLINK;
+		rc.values[0] = man.x * 500 + 1500;
+		rc.values[1] = man.y * 500 + 1500;
+		rc.values[2] = man.r / 2.0f + 1500;
+		rc.values[3] = man.z / 2.0f + 1500;
 
-	// warnx("pitch: %.2f, roll: %.2f, yaw: %.2f, throttle: %.2f", (double)manual.x, (double)manual.y, (double)manual.r, (double)manual.z);
+		rc.values[4] = decode_switch_pos(man.buttons, 0) * 1000 + 1000;
+		rc.values[5] = decode_switch_pos(man.buttons, 1) * 1000 + 1000;
+		rc.values[6] = decode_switch_pos(man.buttons, 2) * 1000 + 1000;
+		rc.values[7] = decode_switch_pos(man.buttons, 3) * 1000 + 1000;
+		rc.values[8] = decode_switch_pos(man.buttons, 4) * 1000 + 1000;
+		rc.values[9] = decode_switch_pos(man.buttons, 5) * 1000 + 1000;
 
-	if (_manual_pub < 0) {
-		_manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual);
+		if (_rc_pub <= 0) {
+			_rc_pub = orb_advertise(ORB_ID(input_rc), &rc);
+
+		} else {
+			orb_publish(ORB_ID(input_rc), _rc_pub, &rc);
+		}
 
 	} else {
-		orb_publish(ORB_ID(manual_control_setpoint), _manual_pub, &manual);
+		struct manual_control_setpoint_s manual = {};
+
+		manual.timestamp = hrt_absolute_time();
+		manual.x = man.x / 1000.0f;
+		manual.y = man.y / 1000.0f;
+		manual.r = man.r / 1000.0f;
+		manual.z = man.z / 1000.0f;
+
+		manual.mode_switch = decode_switch_pos(man.buttons, 0);
+		manual.return_switch = decode_switch_pos(man.buttons, 1);
+		manual.posctl_switch = decode_switch_pos(man.buttons, 2);
+		manual.loiter_switch = decode_switch_pos(man.buttons, 3);
+		manual.acro_switch = decode_switch_pos(man.buttons, 4);
+		manual.offboard_switch = decode_switch_pos(man.buttons, 5);
+
+		if (_manual_pub < 0) {
+			_manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual);
+
+		} else {
+			orb_publish(ORB_ID(manual_control_setpoint), _manual_pub, &manual);
+		}
 	}
 }