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

msg/vehicle_status.msg

@@ -81,6 +81,10 @@ uint8 VEHICLE_BATTERY_WARNING_NONE = 0	    # no battery low voltage warning acti
 uint8 VEHICLE_BATTERY_WARNING_LOW = 1	    # warning of low voltage
 uint8 VEHICLE_BATTERY_WARNING_CRITICAL = 2  # alerting of critical voltage
 
+uint8 RC_IN_MODE_DEFAULT = 0
+uint8 RC_IN_MODE_OFF = 1
+uint8 RC_IN_MODE_GENERATED = 2
+
 # state machine / state of vehicle.
 # Encodes the complete system state and is set by the commander app.
 uint16 counter   # incremented by the writing thread everytime new data is stored

src/drivers/drv_rc_input.h

@@ -99,7 +99,8 @@ enum RC_INPUT_SOURCE {
 	RC_INPUT_SOURCE_PX4IO_PPM,
 	RC_INPUT_SOURCE_PX4IO_SPEKTRUM,
 	RC_INPUT_SOURCE_PX4IO_SBUS,
-	RC_INPUT_SOURCE_PX4IO_ST24
+	RC_INPUT_SOURCE_PX4IO_ST24,
+	RC_INPUT_SOURCE_MAVLINK
 };
 
 /**

src/modules/commander/commander.cpp

@@ -828,7 +828,7 @@ int commander_thread_main(int argc, char *argv[])
 	param_t _param_ef_time_thres = param_find("COM_EF_TIME");
 	param_t _param_autostart_id = param_find("SYS_AUTOSTART");
 	param_t _param_autosave_params = param_find("COM_AUTOS_PAR");
-	param_t _param_rc_in_off = param_find("COM_RC_IN_OFF");
+	param_t _param_rc_in_off = param_find("COM_RC_IN_MODE");
 
 	const char *main_states_str[vehicle_status_s::MAIN_STATE_MAX];
 	main_states_str[vehicle_status_s::MAIN_STATE_MANUAL]			= "MANUAL";
@@ -1237,7 +1237,7 @@ int commander_thread_main(int argc, char *argv[])
 			param_get(_param_datalink_loss_timeout, &datalink_loss_timeout);
 			param_get(_param_rc_loss_timeout, &rc_loss_timeout);
 			param_get(_param_rc_in_off, &rc_in_off);
-			status.rc_input_off = rc_in_off;
+			status.rc_input_off = (rc_in_off == vehicle_status_s::RC_IN_MODE_OFF);
 			param_get(_param_datalink_regain_timeout, &datalink_regain_timeout);
 			param_get(_param_ef_throttle_thres, &ef_throttle_thres);
 			param_get(_param_ef_current2throttle_thres, &ef_current2throttle_thres);

src/modules/commander/commander_params.c

@@ -199,11 +199,13 @@ PARAM_DEFINE_INT32(COM_AUTOS_PAR, 1);
 /**
  * Disable RC control input
  *
- * Setting this to 1 disables RC input handling and the associated checks. This is
+ * The default value of 0 requires a valid RC transmitter setup.
- * mainly inteded for pure offboard control or tablet control use.
+ * Setting this to 1 disables RC input handling and the associated checks. A value of
+ * 2 will generate RC control data from manual input received via MAVLink instead
+ * of directly forwarding the manual input data.
  *
  * @group Commander
  * @min 0
- * @max 1
+ * @max 2
  */
-PARAM_DEFINE_INT32(COM_RC_IN_OFF, 0);
+PARAM_DEFINE_INT32(COM_RC_IN_MODE, 0);

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;
+	if (_mavlink->get_manual_input_mode_generation()) {
-	memset(&manual, 0, sizeof(manual));
 
-	manual.timestamp = hrt_absolute_time();
+		struct rc_input_values rc = {};
-	manual.x = man.x / 1000.0f;
+		rc.timestamp_publication = hrt_absolute_time();
-	manual.y = man.y / 1000.0f;
+		rc.timestamp_last_signal = rc.timestamp_publication;
-	manual.r = man.r / 1000.0f;
-	manual.z = man.z / 1000.0f;
 
-	manual.mode_switch = decode_switch_pos(man.buttons, 0);
+		rc.channel_count = 8;
-	manual.return_switch = decode_switch_pos(man.buttons, 1);
+		rc.rc_failsafe = false;
-	manual.posctl_switch = decode_switch_pos(man.buttons, 2);
+		rc.rc_lost = false;
-	manual.loiter_switch = decode_switch_pos(man.buttons, 3);
+		rc.rc_lost_frame_count = 0;
-	manual.acro_switch = decode_switch_pos(man.buttons, 4);
+		rc.rc_total_frame_count = 1;
-	manual.offboard_switch = decode_switch_pos(man.buttons, 5);
+		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) {
+		if (_rc_pub <= 0) {
-		_manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual);
+			_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);
+		}
 	}
 }
 

src/modules/navigator/mission_feasibility_checker.cpp

@@ -127,7 +127,7 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss
 			}
 
 			if (!geofence.inside_polygon(missionitem.lat, missionitem.lon, missionitem.altitude)) {
-				mavlink_log_critical(_mavlink_fd, "Geofence violation waypoint %d", i);
+				mavlink_log_critical(_mavlink_fd, "Geofence violation for waypoint %d", i);
 				return false;
 			}
 		}
@@ -177,7 +177,7 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s
 
 		if (dm_read(dm_current, i, &missionitem, len) != len) {
 			// not supposed to happen unless the datamanager can't access the SD card, etc.
-			mavlink_log_critical(_mavlink_fd, "Rejecting Mission: Cannot access mission from SD card");
+			mavlink_log_critical(_mavlink_fd, "Rejecting Mission: Cannot access SD card");
 			return false;
 		}
 
@@ -197,7 +197,7 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s
 			return false;
 		}
 	}
-	mavlink_log_critical(_mavlink_fd, "Mission is valid!");
+	mavlink_log_info(_mavlink_fd, "Mission ready.");
 	return true;
 }