MAVLink: Start app using pre-configured streams to save some script execution overhead. Frees 1K during app startup, which should help FMUv1 considerably.

ROMFS/px4fmu_common/init.d/rc.usb

@@ -3,22 +3,7 @@
 # USB MAVLink start
 #
 
-mavlink start -r 800000 -d /dev/ttyACM0 -x
-# Enable a number of interesting streams we want via USB
-mavlink stream -d /dev/ttyACM0 -s PARAM_VALUE -r 300
-mavlink stream -d /dev/ttyACM0 -s MISSION_ITEM -r 50
-mavlink stream -d /dev/ttyACM0 -s NAMED_VALUE_FLOAT -r 10
-mavlink stream -d /dev/ttyACM0 -s OPTICAL_FLOW_RAD -r 10
-mavlink stream -d /dev/ttyACM0 -s VFR_HUD -r 20
-mavlink stream -d /dev/ttyACM0 -s ATTITUDE -r 100
-mavlink stream -d /dev/ttyACM0 -s ACTUATOR_CONTROL_TARGET0 -r 30
-mavlink stream -d /dev/ttyACM0 -s RC_CHANNELS_RAW -r 5
-mavlink stream -d /dev/ttyACM0 -s SERVO_OUTPUT_RAW_0 -r 20
-mavlink stream -d /dev/ttyACM0 -s POSITION_TARGET_GLOBAL_INT -r 10
-mavlink stream -d /dev/ttyACM0 -s LOCAL_POSITION_NED -r 30
-mavlink stream -d /dev/ttyACM0 -s MANUAL_CONTROL -r 5
-mavlink stream -d /dev/ttyACM0 -s HIGHRES_IMU -r 100
-mavlink stream -d /dev/ttyACM0 -s GPS_RAW_INT -r 20
+mavlink start -r 800000 -d /dev/ttyACM0 -m config -x
 
 # Exit shell to make it available to MAVLink
 exit

src/modules/mavlink/mavlink_main.cpp

@@ -1353,6 +1353,8 @@ Mavlink::task_main(int argc, char *argv[])
 				_mode = MAVLINK_MODE_ONBOARD;
 			} else if (strcmp(optarg, "osd") == 0) {
 				_mode = MAVLINK_MODE_OSD;
+			} else if (strcmp(optarg, "config") == 0) {
+				_mode = MAVLINK_MODE_CONFIG;
 			}
 
 			break;
@@ -1449,8 +1451,6 @@ Mavlink::task_main(int argc, char *argv[])
 	/* start the MAVLink receiver */
 	_receive_thread = MavlinkReceiver::receive_start(this);
 
-	_task_running = true;
-
 	MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update));
 	uint64_t param_time = 0;
 	MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status));
@@ -1537,6 +1537,23 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("RC_CHANNELS_RAW", 5.0f);
 		break;
 
+	case MAVLINK_MODE_CONFIG:
+		// Enable a number of interesting streams we want via USB
+		configure_stream("PARAM_VALUE", 300.0f);
+		configure_stream("MISSION_ITEM", 50.0f);
+		configure_stream("NAMED_VALUE_FLOAT", 10.0f);
+		configure_stream("OPTICAL_FLOW_RAD", 10.0f);
+		configure_stream("VFR_HUD", 20.0f);
+		configure_stream("ATTITUDE", 100.0f);
+		configure_stream("ACTUATOR_CONTROL_TARGET0", 30.0f);
+		configure_stream("RC_CHANNELS_RAW", 5.0f);
+		configure_stream("SERVO_OUTPUT_RAW_0", 20.0f);
+		configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f);
+		configure_stream("LOCAL_POSITION_NED", 30.0f);
+		configure_stream("MANUAL_CONTROL", 5.0f);
+		configure_stream("HIGHRES_IMU", 100.0f);
+		configure_stream("GPS_RAW_INT", 20.0f);
+
 	default:
 		break;
 	}
@@ -1703,6 +1720,9 @@ Mavlink::task_main(int argc, char *argv[])
 		}
 
 		perf_end(_loop_perf);
+
+		/* confirm task running only once fully initialized */
+		_task_running = true;
 	}
 
 	delete _subscribe_to_stream;

src/modules/mavlink/mavlink_main.h

@@ -129,7 +129,8 @@ public:
 		MAVLINK_MODE_NORMAL = 0,
 		MAVLINK_MODE_CUSTOM,
 		MAVLINK_MODE_ONBOARD,
-		MAVLINK_MODE_OSD
+		MAVLINK_MODE_OSD,
+		MAVLINK_MODE_CONFIG
 	};
 
 	void			set_mode(enum MAVLINK_MODE);