From ac77fe9c27d7253b01805ff94d3e0f8e21017709 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Jan 2014 18:40:02 +0100 Subject: WIP state on getting MAVLink as a class, much of the work done, but does not compile yet --- src/modules/mavlink/mavlink_main.cpp | 262 ++++++++++++++++++----------------- 1 file changed, 135 insertions(+), 127 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 339030a86..1c7986cbb 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -47,6 +47,7 @@ #include #include #include +#include #include #include #include @@ -70,10 +71,10 @@ #include #include #include +#include #include "mavlink_bridge_header.h" -#include "mavlink_parameters.h" #include #include "math.h" /* isinf / isnan checks */ #include @@ -87,7 +88,9 @@ #include #include #include -#include +#include "mavlink_main.h" +#include "mavlink_orb_listener.h" +#include "mavlink_receiver.h" /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -102,7 +105,49 @@ static const int ERROR = -1; */ extern "C" __EXPORT int mavlink_main(int argc, char *argv[]); +/* + * Internal function to send the bytes through the right serial port + */ +void +mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length) +{ + int uart = -1; + switch (channel) { + case MAVLINK_COMM_0: + uart = Mavlink::get_uart_fd(0); + break; + case MAVLINK_COMM_1: + uart = Mavlink::get_uart_fd(1); + break; + case MAVLINK_COMM_2: + uart = Mavlink::get_uart_fd(2); + break; + case MAVLINK_COMM_3: + uart = Mavlink::get_uart_fd(3); + break; + #ifdef MAVLINK_COMM_4 + case MAVLINK_COMM_4: + uart = Mavlink::get_uart_fd(4); + break; + #endif + #ifdef MAVLINK_COMM_5 + case MAVLINK_COMM_5: + uart = Mavlink::get_uart_fd(5); + break; + #endif + #ifdef MAVLINK_COMM_6 + case MAVLINK_COMM_6: + uart = Mavlink::get_uart_fd(6); + break; + #endif + } + + write(uart, ch, (size_t)(sizeof(uint8_t) * length)); + +} + +static void usage(void); namespace mavlink { @@ -122,6 +167,8 @@ Mavlink::Mavlink(const char *port, unsigned baud_rate) : _mavlink_hil_enabled(false) // _params_sub(-1) { + wpm = &wpm_s; + fops.ioctl = (int (*)(file*, int, long unsigned int))&mavlink_dev_ioctl; // _parameter_handles.min_altitude = param_find("NAV_MIN_ALT"); } @@ -191,6 +238,15 @@ Mavlink* Mavlink::get_instance(unsigned instance) return inst; } +int Mavlink::get_uart_fd(unsigned index) +{ + Mavlink* inst = get_instance(index); + if (inst) + return inst->_mavlink_fd; + + return -1; +} + void Mavlink::parameters_update() { @@ -202,48 +258,12 @@ Mavlink::parameters_update() } -/* XXX not widely used */ -uint8_t chan = MAVLINK_COMM_0; - -/* XXX probably should be in a header... */ -extern pthread_t receive_start(int uart); - -/* Allocate storage space for waypoints */ -static mavlink_wpm_storage wpm_s; -mavlink_wpm_storage *wpm = &wpm_s; - -bool mavlink_hil_enabled = false; - -/* protocol interface */ -static int uart; -static int baudrate; -bool gcs_link = true; - -/* interface mode */ -static enum { - MAVLINK_INTERFACE_MODE_OFFBOARD, - MAVLINK_INTERFACE_MODE_ONBOARD -} mavlink_link_mode = MAVLINK_INTERFACE_MODE_OFFBOARD; - -static struct mavlink_logbuffer lb; - -static void mavlink_update_system(void); -static int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); -static void usage(void); -int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval); - /**************************************************************************** * MAVLink text message logger ****************************************************************************/ -static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); - -static const struct file_operations mavlink_fops = { - .ioctl = mavlink_dev_ioctl -}; - -static int -mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) +int +Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) { static unsigned int total_counter = 0; @@ -252,10 +272,11 @@ mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) case (int)MAVLINK_IOC_SEND_TEXT_CRITICAL: case (int)MAVLINK_IOC_SEND_TEXT_EMERGENCY: { const char *txt = (const char *)arg; - struct mavlink_logmessage msg; - strncpy(msg.text, txt, sizeof(msg.text)); - mavlink_logbuffer_write(&lb, &msg); - total_counter++; + printf("logmsg: %s\n", txt); + //struct mavlink_logmessage msg; + //strncpy(msg.text, txt, sizeof(msg.text)); + //mavlink_logbuffer_write(&lb, &msg); + //total_counter++; return OK; } @@ -264,7 +285,7 @@ mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) } } -void mavlink_update_system(void) +void Mavlink::mavlink_update_system(void) { static bool initialized = false; static param_t param_system_id; @@ -301,7 +322,7 @@ void mavlink_update_system(void) } } -int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb) +int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb) { /* process baud rate */ int speed; @@ -398,7 +419,7 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf } int -set_hil_on_off(bool hil_enabled) +Mavlink::set_hil_on_off(bool hil_enabled) { int ret = OK; @@ -427,13 +448,13 @@ set_hil_on_off(bool hil_enabled) hil_rate_interval = 5; } - orb_set_interval(mavlink_subs.spa_sub, hil_rate_interval); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, hil_rate_interval); + orb_set_interval(subs.spa_sub, hil_rate_interval); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, hil_rate_interval); } if (!hil_enabled && mavlink_hil_enabled) { mavlink_hil_enabled = false; - orb_set_interval(mavlink_subs.spa_sub, 200); + orb_set_interval(subs.spa_sub, 200); } else { ret = ERROR; @@ -443,7 +464,7 @@ set_hil_on_off(bool hil_enabled) } void -get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode) +Mavlink::get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode) { /* reset MAVLink mode bitfield */ *mavlink_base_mode = 0; @@ -518,7 +539,7 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u } -int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval) +int Mavlink::set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval) { int ret = OK; @@ -552,7 +573,7 @@ int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_m orb_set_interval(subs->actuators_sub, min_interval); orb_set_interval(subs->actuators_effective_sub, min_interval); orb_set_interval(subs->spa_sub, min_interval); - orb_set_interval(mavlink_subs.rates_setpoint_sub, min_interval); + orb_set_interval(subs->rates_setpoint_sub, min_interval); break; case MAVLINK_MSG_ID_MANUAL_CONTROL: @@ -575,34 +596,19 @@ int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_m extern mavlink_system_t mavlink_system; -extern int mavlink_missionlib_send_message(mavlink_message_t *msg); -extern int mavlink_missionlib_send_gcs_string(const char *string); - -/** - * If the queue index is not at 0, the queue sending - * logic will send parameters from the current index - * to len - 1, the end of the param list. - */ -static unsigned int mavlink_param_queue_index = 0; - -/** - * Callback for param interface. - */ -void mavlink_pm_callback(void *arg, param_t param); - -void mavlink_pm_callback(void *arg, param_t param) +void Mavlink::mavlink_pm_callback(void *arg, param_t param) { - mavlink_pm_send_param(param); + //mavlink_pm_send_param(param); usleep(*(unsigned int *)arg); } -void mavlink_pm_send_all_params(unsigned int delay) +void Mavlink::mavlink_pm_send_all_params(unsigned int delay) { unsigned int dbuf = delay; param_foreach(&mavlink_pm_callback, &dbuf, false); } -int mavlink_pm_queued_send() +int Mavlink::mavlink_pm_queued_send() { if (mavlink_param_queue_index < param_count()) { mavlink_pm_send_param(param_for_index(mavlink_param_queue_index)); @@ -614,22 +620,22 @@ int mavlink_pm_queued_send() } } -void mavlink_pm_start_queued_send() +void Mavlink::mavlink_pm_start_queued_send() { mavlink_param_queue_index = 0; } -int mavlink_pm_send_param_for_index(uint16_t index) +int Mavlink::mavlink_pm_send_param_for_index(uint16_t index) { return mavlink_pm_send_param(param_for_index(index)); } -int mavlink_pm_send_param_for_name(const char *name) +int Mavlink::mavlink_pm_send_param_for_name(const char *name) { return mavlink_pm_send_param(param_find(name)); } -int mavlink_pm_send_param(param_t param) +int Mavlink::mavlink_pm_send_param(param_t param) { if (param == PARAM_INVALID) return 1; @@ -679,11 +685,11 @@ int mavlink_pm_send_param(param_t param) mavlink_type, param_count(), param_get_index(param)); - ret = mavlink_missionlib_send_message(&tx_msg); - return ret; + mavlink_missionlib_send_message(&tx_msg); + return OK; } -void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg) +void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg) { switch (msg->msgid) { case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { @@ -748,7 +754,7 @@ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_mess } } -void publish_mission() +void Mavlink::publish_mission() { /* Initialize mission publication if necessary */ if (mission_pub < 0) { @@ -759,7 +765,7 @@ void publish_mission() } } -int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item) +int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item) { /* only support global waypoints for now */ switch (mavlink_mission_item->frame) { @@ -797,7 +803,7 @@ int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavli mission_item->yaw = _wrap_pi(mavlink_mission_item->param4*M_DEG_TO_RAD_F); mission_item->loiter_radius = fabsf(mavlink_mission_item->param3); mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */ - mission_item->nav_cmd = mavlink_mission_item->command; + mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command; mission_item->time_inside = mavlink_mission_item->param1; mission_item->autocontinue = mavlink_mission_item->autocontinue; @@ -807,7 +813,7 @@ int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavli return OK; } -int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item) +int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item) { if (mission_item->altitude_is_relative) { mavlink_mission_item->frame = MAV_FRAME_GLOBAL; @@ -838,7 +844,7 @@ int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *missio return OK; } -void mavlink_wpm_init(mavlink_wpm_storage *state) +void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state) { state->size = 0; state->max_size = MAVLINK_WPM_MAX_WP_COUNT; @@ -854,7 +860,7 @@ void mavlink_wpm_init(mavlink_wpm_storage *state) /* * @brief Sends an waypoint ack message */ -void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type) +void Mavlink::mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type) { mavlink_message_t msg; mavlink_mission_ack_t wpa; @@ -878,7 +884,7 @@ void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type) * * @param seq The waypoint sequence number the MAV should fly to. */ -void mavlink_wpm_send_waypoint_current(uint16_t seq) +void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq) { if (seq < wpm->size) { mavlink_message_t msg; @@ -897,7 +903,7 @@ void mavlink_wpm_send_waypoint_current(uint16_t seq) } } -void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count) +void Mavlink::mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count) { mavlink_message_t msg; mavlink_mission_count_t wpc; @@ -912,7 +918,7 @@ void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t cou if (verbose) warnx("Sent waypoint count (%u) to ID %u", wpc.count, wpc.target_system); } -void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq) +void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq) { struct mission_item_s mission_item; @@ -946,7 +952,7 @@ void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq) } } -void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq) +void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq) { if (seq < wpm->max_size) { mavlink_message_t msg; @@ -972,7 +978,7 @@ void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t s * * @param seq The waypoint sequence number the MAV has reached. */ -void mavlink_wpm_send_waypoint_reached(uint16_t seq) +void Mavlink::mavlink_wpm_send_waypoint_reached(uint16_t seq) { mavlink_message_t msg; mavlink_mission_item_reached_t wp_reached; @@ -986,7 +992,7 @@ void mavlink_wpm_send_waypoint_reached(uint16_t seq) } -void mavlink_waypoint_eventloop(uint64_t now) +void Mavlink::mavlink_waypoint_eventloop(uint64_t now) { /* check for timed-out operations */ if (now - wpm->timestamp_lastaction > wpm->timeout && wpm->current_state != MAVLINK_WPM_STATE_IDLE) { @@ -1002,7 +1008,7 @@ void mavlink_waypoint_eventloop(uint64_t now) } -void mavlink_wpm_message_handler(const mavlink_message_t *msg) +void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) { uint64_t now = hrt_absolute_time(); @@ -1377,7 +1383,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg) } void -mavlink_missionlib_send_message(mavlink_message_t *msg) +Mavlink::mavlink_missionlib_send_message(mavlink_message_t *msg) { uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg); @@ -1387,7 +1393,7 @@ mavlink_missionlib_send_message(mavlink_message_t *msg) int -mavlink_missionlib_send_gcs_string(const char *string) +Mavlink::mavlink_missionlib_send_gcs_string(const char *string) { const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN; mavlink_statustext_t statustext; @@ -1419,11 +1425,11 @@ mavlink_missionlib_send_gcs_string(const char *string) void Mavlink::task_main_trampoline(int argc, char *argv[]) { - mavlink::g_mavlink->task_main(); + mavlink::g_mavlink->task_main(argc, argv); } -void -Mavlink::task_main() +int +Mavlink::task_main(int argc, char *argv[]) { /* inform about start */ warnx("Initializing.."); @@ -1529,16 +1535,18 @@ Mavlink::task_main() err(1, "could not open %s", device_name); /* create the device node that's used for sending text log messages, etc. */ - register_driver(MAVLINK_LOG_DEVICE, &mavlink_fops, 0666, NULL); + register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL); /* Initialize system properties */ mavlink_update_system(); /* start the MAVLink receiver */ - receive_thread = receive_start(uart); + MavlinkReceiver rcv(this); + receive_thread = rcv.receive_start(uart); /* start the ORB receiver */ - uorb_receive_thread = uorb_receive_start(); + MavlinkOrbListener listener(this); + uorb_receive_thread = listener.uorb_receive_start(); /* initialize waypoint manager */ mavlink_wpm_init(wpm); @@ -1546,57 +1554,57 @@ Mavlink::task_main() /* all subscriptions are now active, set up initial guess about rate limits */ if (baudrate >= 230400) { /* 200 Hz / 5 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 20); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 20); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_HIGHRES_IMU, 20); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_RAW_IMU, 20); /* 50 Hz / 20 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 30); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_ATTITUDE, 30); /* 20 Hz / 50 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50); /* 10 Hz */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 100); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_GPS_RAW_INT, 100); /* 10 Hz */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 100); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 100); } else if (baudrate >= 115200) { /* 20 Hz / 50 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 50); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 50); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 50); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_HIGHRES_IMU, 50); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_RAW_IMU, 50); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_ATTITUDE, 50); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50); /* 5 Hz / 200 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200); /* 5 Hz / 200 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 200); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_GPS_RAW_INT, 200); /* 2 Hz */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); } else if (baudrate >= 57600) { /* 10 Hz / 100 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 300); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 300); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_RAW_IMU, 300); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_HIGHRES_IMU, 300); /* 10 Hz / 100 ms ATTITUDE */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 200); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_ATTITUDE, 200); /* 5 Hz / 200 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200); /* 5 Hz / 200 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 500); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 500); /* 2 Hz */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); /* 2 Hz */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 500); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_GPS_RAW_INT, 500); } else { /* very low baud rate, limit to 1 Hz / 1000 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 1000); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 1000); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 1000); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_RAW_IMU, 1000); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_ATTITUDE, 1000); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_HIGHRES_IMU, 1000); /* 1 Hz / 1000 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 1000); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 1000); /* 0.5 Hz */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 2000); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 2000); /* 0.1 Hz */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 10000); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 10000); } int mission_result_sub = orb_subscribe(ORB_ID(mission_result)); -- cgit v1.2.3