From e1aaaa1f1896fad5b422b0b8fe3c8c1edad7625c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Aug 2015 20:31:45 +0200 Subject: [PATCH] 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 | 17 +---------------- src/modules/mavlink/mavlink_main.cpp | 24 ++++++++++++++++++++++-- src/modules/mavlink/mavlink_main.h | 3 ++- 3 files changed, 25 insertions(+), 19 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index 027d2aca5d..4a157a683f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/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 diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index f2dcd33d79..f172dfbeae 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/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; diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index a484558579..455d9c796e 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/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);