From 141982a3ac21e7a0437f1d7692e4024daf873c21 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 27 Feb 2014 13:54:55 +0400 Subject: mavlink: minor refactoring and cleanup, rate limiter class implemented, new messages added --- src/modules/mavlink/mavlink_main.cpp | 68 +++++++++++++++++++++--------------- 1 file changed, 40 insertions(+), 28 deletions(-) (limited to 'src/modules/mavlink/mavlink_main.cpp') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 3102c937d..8a026742c 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -79,6 +79,7 @@ #include "mavlink_main.h" #include "mavlink_messages.h" #include "mavlink_receiver.h" +#include "mavlink_rate_limiter.h" /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -321,7 +322,7 @@ Mavlink::get_uart_fd() mavlink_channel_t Mavlink::get_channel() { - return _chan; + return _channel; } /**************************************************************************** @@ -1321,7 +1322,7 @@ Mavlink::mavlink_missionlib_send_message(mavlink_message_t *msg) { uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg); - mavlink_send_uart_bytes(_chan, missionlib_msg_buf, len); + mavlink_send_uart_bytes(_channel, missionlib_msg_buf, len); } @@ -1356,7 +1357,7 @@ Mavlink::mavlink_missionlib_send_gcs_string(const char *string) } } -MavlinkOrbSubscription *Mavlink::add_orb_subscription(const struct orb_metadata *topic, const size_t size) +MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic, const size_t size) { /* check if already subscribed to this topic */ MavlinkOrbSubscription *sub; @@ -1406,7 +1407,7 @@ Mavlink::task_main(int argc, char *argv[]) int ch; _baudrate = 57600; - _chan = MAVLINK_COMM_0; + _channel = MAVLINK_COMM_0; _mode = MODE_OFFBOARD; @@ -1520,8 +1521,6 @@ Mavlink::task_main(int argc, char *argv[]) thread_running = true; - unsigned lowspeed_counter = 0; - MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update), sizeof(parameter_update_s)); MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status), sizeof(vehicle_status_s)); @@ -1530,20 +1529,42 @@ Mavlink::task_main(int argc, char *argv[]) warnx("started"); mavlink_log_info(_mavlink_fd, "[mavlink] started"); - /* add default streams, intervals depend on baud rate */ + /* add default streams depending on mode, intervals depend on baud rate */ float rate_mult = _baudrate / 57600.0f; if (rate_mult > 4.0f) { rate_mult = 4.0f; } add_stream("HEARTBEAT", 1.0f); - add_stream("SYS_STATUS", 1.0f * rate_mult); - add_stream("GPS_GLOBAL_ORIGIN", 0.5f * rate_mult); - add_stream("HIGHRES_IMU", 1.0f * rate_mult); - add_stream("ATTITUDE", 10.0f * rate_mult); - add_stream("GPS_RAW_INT", 1.0f * rate_mult); - add_stream("GLOBAL_POSITION_INT", 5.0f * rate_mult); - add_stream("LOCAL_POSITION_NED", 5.0f * rate_mult); + + switch(_mode) { + case MODE_OFFBOARD: + add_stream("SYS_STATUS", 1.0f * rate_mult); + add_stream("GPS_GLOBAL_ORIGIN", 0.5f * rate_mult); + add_stream("HIGHRES_IMU", 1.0f * rate_mult); + add_stream("ATTITUDE", 10.0f * rate_mult); + add_stream("GPS_RAW_INT", 1.0f * rate_mult); + add_stream("GLOBAL_POSITION_INT", 5.0f * rate_mult); + add_stream("LOCAL_POSITION_NED", 5.0f * rate_mult); + add_stream("SERVO_OUTPUT_RAW_0", 1.0f * rate_mult); + break; + + case MODE_HIL: + add_stream("SYS_STATUS", 1.0f * rate_mult); + add_stream("GPS_GLOBAL_ORIGIN", 0.5f * rate_mult); + add_stream("HIGHRES_IMU", 1.0f * rate_mult); + add_stream("ATTITUDE", 10.0f * rate_mult); + add_stream("GPS_RAW_INT", 1.0f * rate_mult); + add_stream("GLOBAL_POSITION_INT", 5.0f * rate_mult); + add_stream("LOCAL_POSITION_NED", 5.0f * rate_mult); + break; + + default: + break; + } + + MavlinkRateLimiter slow_rate_limiter(2000.0f / rate_mult); + MavlinkRateLimiter fast_rate_limiter(100.0f / rate_mult); while (!_task_should_exit) { /* main loop */ @@ -1571,13 +1592,6 @@ Mavlink::task_main(int argc, char *argv[]) stream->update(t); } - /* 0.5 Hz */ - if (lowspeed_counter % (2000000 / MAIN_LOOP_DELAY) == 0) { - mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission); - } - - lowspeed_counter++; - bool updated; orb_check(mission_result_sub, &updated); @@ -1591,18 +1605,16 @@ Mavlink::task_main(int argc, char *argv[]) } mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission); + } else { + if (slow_rate_limiter.check(t)) { + mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission); + } } - if (lowspeed_counter % (50000 / MAIN_LOOP_DELAY) == 0) { - /* send parameters at 20 Hz (if queued for sending) */ + if (fast_rate_limiter.check(t)) { mavlink_pm_queued_send(); mavlink_waypoint_eventloop(hrt_absolute_time()); - if (_baudrate > 57600) { - mavlink_pm_queued_send(); - } - - /* send one string at 20 Hz */ if (!mavlink_logbuffer_is_empty(&lb)) { struct mavlink_logmessage msg; int lb_ret = mavlink_logbuffer_read(&lb, &msg); -- cgit v1.2.3