diff options
| -rw-r--r-- | ROMFS/px4fmu_common/init.d/rc.usb | 2 | ||||
| -rw-r--r-- | ROMFS/px4fmu_common/init.d/rcS | 2 | ||||
| -rw-r--r-- | src/modules/mavlink/mavlink_main.cpp | 21 | ||||
| -rw-r--r-- | src/modules/mavlink/mavlink_main.h | 4 |
4 files changed, 18 insertions, 11 deletions
diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index 558be4275..0de2d4295 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -5,7 +5,7 @@ echo "Starting MAVLink on this USB console" -mavlink start -r 10000 -d /dev/ttyACM0 +mavlink start -r 5000 -d /dev/ttyACM0 # Exit shell to make it available to MAVLink exit diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index c96f44e02..48532e7af 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -387,7 +387,7 @@ then if [ $HIL == yes ] then sleep 1 - set MAVLINK_FLAGS "-d 10000 -d /dev/ttyACM0" + set MAVLINK_FLAGS "-r 10000 -d /dev/ttyACM0 -m hil" usleep 5000 else # Normal mode, use baudrate 57600 (default) and data rate 1000 bytes/s diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 99bef1f73..fb29c9c71 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -90,7 +90,7 @@ static const int ERROR = -1; #define DEFAULT_DEVICE_NAME "/dev/ttyS1" #define MAX_DATA_RATE 10000 // max data rate in bytes/s -#define MAIN_LOOP_DELAY 10000 // 100 Hz +#define MAIN_LOOP_DELAY 10000 // 100 Hz @ 1000 bytes/s data rate static Mavlink *_mavlink_instances = nullptr; @@ -166,6 +166,7 @@ Mavlink::Mavlink() : _mavlink_fd(-1), _task_running(false), _mavlink_hil_enabled(false), + _main_loop_delay(1000), _subscriptions(nullptr), _streams(nullptr), _mission_pub(-1), @@ -1723,7 +1724,7 @@ Mavlink::task_main(int argc, char *argv[]) switch (_mode) { case MODE_OFFBOARD: configure_stream("SYS_STATUS", 1.0f); - configure_stream("GPS_GLOBAL_ORIGIN", 0.5f * rate_mult); + configure_stream("GPS_GLOBAL_ORIGIN", 0.5f); configure_stream("HIGHRES_IMU", 1.0f * rate_mult); configure_stream("ATTITUDE", 10.0f * rate_mult); configure_stream("GPS_RAW_INT", 1.0f * rate_mult); @@ -1733,14 +1734,15 @@ Mavlink::task_main(int argc, char *argv[]) break; case MODE_HIL: + /* HIL mode normally runs at high data rate, rate_mult ~ 10 */ configure_stream("SYS_STATUS", 1.0f); - configure_stream("GPS_GLOBAL_ORIGIN", 0.5f * rate_mult); + configure_stream("GPS_GLOBAL_ORIGIN", 0.5f); configure_stream("HIGHRES_IMU", 1.0f * rate_mult); - configure_stream("ATTITUDE", 10.0f * rate_mult); + configure_stream("ATTITUDE", 2.0f * rate_mult); configure_stream("GPS_RAW_INT", 1.0f * rate_mult); - configure_stream("GLOBAL_POSITION_INT", 5.0f * rate_mult); - configure_stream("LOCAL_POSITION_NED", 5.0f * rate_mult); - configure_stream("HIL_CONTROLS", 20.0f * rate_mult); + configure_stream("GLOBAL_POSITION_INT", 1.0f * rate_mult); + configure_stream("LOCAL_POSITION_NED", 1.0f * rate_mult); + configure_stream("HIL_CONTROLS", 15.0f * rate_mult); break; default: @@ -1753,9 +1755,12 @@ Mavlink::task_main(int argc, char *argv[]) MavlinkRateLimiter slow_rate_limiter(2000.0f / rate_mult); MavlinkRateLimiter fast_rate_limiter(100.0f / rate_mult); + /* set main loop delay depending on data rate to minimize CPU overhead */ + _main_loop_delay = MAIN_LOOP_DELAY / rate_mult; + while (!_task_should_exit) { /* main loop */ - usleep(MAIN_LOOP_DELAY); + usleep(_main_loop_delay); perf_begin(_loop_perf); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 94bef7e72..b52c12796 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -196,7 +196,9 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ /* states */ - bool _mavlink_hil_enabled; /**< Hardware in the loop mode */ + bool _mavlink_hil_enabled; /**< Hardware In the Loop mode */ + + unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */ MavlinkOrbSubscription *_subscriptions; MavlinkStream *_streams; |