diff options
Diffstat (limited to 'src')
-rw-r--r-- | src/modules/mavlink/mavlink.c | 36 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_main.cpp | 262 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_main.h | 153 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_orb_listener.cpp | 6 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_orb_listener.h | 19 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 94 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.h | 77 |
7 files changed, 402 insertions, 245 deletions
diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index aba8c98da..b143e62f0 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -86,42 +86,6 @@ mavlink_system_t mavlink_system = { }; // System ID, 1-255, Component/Subsystem ID, 1-255 /* - * 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; - case MAVLINK_COMM_4: - uart = Mavlink::get_uart_fd(4); - break; - case MAVLINK_COMM_5: - uart = Mavlink::get_uart_fd(5); - break; - case MAVLINK_COMM_6: - uart = Mavlink::get_uart_fd(6); - break; - } - - write(uart, ch, (size_t)(sizeof(uint8_t) * length)); - -} - -/* * Internal function to give access to the channel status for each channel */ extern mavlink_status_t *mavlink_get_channel_status(uint8_t channel) 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 <errno.h> #include <math.h> #include <poll.h> +#include <termios.h> #include <time.h> #include <sys/ioctl.h> #include <drivers/device/device.h> @@ -70,10 +71,10 @@ #include <mavlink/mavlink_log.h> #include <sys/types.h> #include <sys/stat.h> +#include <commander/px4_custom_mode.h> #include "mavlink_bridge_header.h" -#include "mavlink_parameters.h" #include <uORB/uORB.h> #include "math.h" /* isinf / isnan checks */ #include <assert.h> @@ -87,7 +88,9 @@ #include <systemlib/err.h> #include <sys/stat.h> #include <dataman/dataman.h> -#include <waypoints.h> +#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)); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 8d6f0bd6f..244af04a6 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -73,6 +73,70 @@ #include <drivers/drv_rc_input.h> #include <uORB/topics/navigation_capabilities.h> + +#include <nuttx/fs/fs.h> + + // FIXME XXX - TO BE MOVED TO XML +enum MAVLINK_WPM_STATES { + MAVLINK_WPM_STATE_IDLE = 0, + MAVLINK_WPM_STATE_SENDLIST, + MAVLINK_WPM_STATE_SENDLIST_SENDWPS, + MAVLINK_WPM_STATE_GETLIST, + MAVLINK_WPM_STATE_GETLIST_GETWPS, + MAVLINK_WPM_STATE_GETLIST_GOTALL, + MAVLINK_WPM_STATE_ENUM_END +}; + +enum MAVLINK_WPM_CODES { + MAVLINK_WPM_CODE_OK = 0, + MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED, + MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED, + MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS, + MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED, + MAVLINK_WPM_CODE_ENUM_END +}; + + +/* WAYPOINT MANAGER - MISSION LIB */ + +#define MAVLINK_WPM_MAX_WP_COUNT 15 +#define MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE ///< Enable double buffer and in-flight updates +#ifndef MAVLINK_WPM_TEXT_FEEDBACK +#define MAVLINK_WPM_TEXT_FEEDBACK 0 ///< Report back status information as text +#endif +#define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication timeout in useconds +#define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000 ///< When to send a new setpoint +#define MAVLINK_WPM_PROTOCOL_DELAY_DEFAULT 40000 + + +struct mavlink_wpm_storage { + mavlink_mission_item_t waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Currently active waypoints +#ifdef MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE + mavlink_mission_item_t rcv_waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Receive buffer for next waypoints +#endif + uint16_t size; + uint16_t max_size; + uint16_t rcv_size; + enum MAVLINK_WPM_STATES current_state; + int16_t current_wp_id; ///< Waypoint in current transmission + int16_t current_active_wp_id; ///< Waypoint the system is currently heading towards + uint16_t current_count; + uint8_t current_partner_sysid; + uint8_t current_partner_compid; + uint64_t timestamp_lastaction; + uint64_t timestamp_last_send_setpoint; + uint64_t timestamp_firstinside_orbit; + uint64_t timestamp_lastoutside_orbit; + uint32_t timeout; + uint32_t delay_setpoint; + float accept_range_yaw; + float accept_range_distance; + bool yaw_reached; + bool pos_reached; + bool idle; + int current_dataman_id; +}; + class Mavlink { public: @@ -104,6 +168,8 @@ public: static Mavlink* get_instance(unsigned instance); + static int get_uart_fd(unsigned index); + struct mavlink_subscriptions { int sensor_sub; int att_sub; @@ -149,12 +215,19 @@ public: /** Actuator armed state */ struct actuator_armed_s armed; +protected: + /** + * Pointer to the default cdev file operations table; useful for + * registering clone devices etc. + */ + struct file_operations fops; + int _mavlink_fd; + private: bool _task_should_exit; /**< if true, sensor task should exit */ int _mavlink_task; /**< task handle for sensor task */ - int _mavlink_fd; int _mavlink_incoming_fd; /**< file descriptor on which to receive incoming strings */ perf_counter_t _loop_perf; /**< loop performance counter */ @@ -165,10 +238,45 @@ private: int _params_sub; - orb_advert_t mission_pub = -1; -struct mission_s mission; + orb_advert_t mission_pub; + struct mission_s mission; + uint8_t missionlib_msg_buf[300]; //XXX MAGIC NUMBER + bool thread_running; + bool thread_should_exit; + + uint8_t mavlink_wpm_comp_id; + mavlink_channel_t chan; + +// XXX probably should be in a header... +// extern pthread_t receive_start(int uart); -uint8_t mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER; + pthread_t receive_thread; + pthread_t uorb_receive_thread; + + /* Allocate storage space for waypoints */ + mavlink_wpm_storage wpm_s; + mavlink_wpm_storage *wpm; + + bool verbose; + bool mavlink_hil_enabled; + int uart; + int baudrate; + bool gcs_link; + /** + * 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. + */ + unsigned int mavlink_param_queue_index; + + /* interface mode */ + enum { + MAVLINK_INTERFACE_MODE_OFFBOARD, + MAVLINK_INTERFACE_MODE_ONBOARD + } mavlink_link_mode; + + struct mavlink_logbuffer lb; + bool mavlink_link_termination_allowed; /** * Update our local parameter cache. @@ -240,7 +348,38 @@ uint8_t mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER; * mavlink_pm_queued_send(). * @see mavlink_pm_queued_send() */ - void mavlink_pm_start_queued_send(void); + void mavlink_pm_start_queued_send(); + + void mavlink_update_system(); + + void mavlink_wpm_message_handler(const mavlink_message_t *msg); + void mavlink_waypoint_eventloop(uint64_t now); + void mavlink_wpm_send_waypoint_reached(uint16_t seq); + void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq); + void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq); + void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count); + void mavlink_wpm_send_waypoint_current(uint16_t seq); + void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type); + void mavlink_wpm_init(mavlink_wpm_storage *state); + int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item); + int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item); + void publish_mission(); + + void mavlink_missionlib_send_message(mavlink_message_t *msg); + int mavlink_missionlib_send_gcs_string(const char *string); + + int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); + + int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval); + + void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode); + + /** + * Callback for param interface. + */ + static void mavlink_pm_callback(void *arg, param_t param); + + static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); /** * Shim for calling task_main from task_create. @@ -248,8 +387,8 @@ uint8_t mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER; void task_main_trampoline(int argc, char *argv[]); /** - * Main sensor collection task. + * Main mavlink task. */ - void task_main() __attribute__((noreturn)); + int task_main(int argc, char *argv[]) __attribute__((noreturn)); }; diff --git a/src/modules/mavlink/mavlink_orb_listener.cpp b/src/modules/mavlink/mavlink_orb_listener.cpp index 3edad0f16..e2675dfa7 100644 --- a/src/modules/mavlink/mavlink_orb_listener.cpp +++ b/src/modules/mavlink/mavlink_orb_listener.cpp @@ -661,8 +661,8 @@ MavlinkOrbListener::l_control_mode(const struct listener *l) mavlink_state); } -static void * -uorb_receive_thread(void *arg) +void * +MavlinkOrbListener::uorb_receive_thread(void *arg) { /* Set thread name */ prctl(PR_SET_NAME, "mavlink_orb_rcv", getpid()); @@ -712,7 +712,7 @@ uorb_receive_thread(void *arg) } pthread_t -uorb_receive_start(void) +MavlinkOrbListener::uorb_receive_start(void) { /* --- SENSORS RAW VALUE --- */ mavlink_subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); diff --git a/src/modules/mavlink/mavlink_orb_listener.h b/src/modules/mavlink/mavlink_orb_listener.h index 32a174fcd..29e081b36 100644 --- a/src/modules/mavlink/mavlink_orb_listener.h +++ b/src/modules/mavlink/mavlink_orb_listener.h @@ -55,18 +55,21 @@ public: */ ~MavlinkOrbListener(); - /** - * Start the mavlink task. - * - * @return OK on success. - */ - int start(); + // * + // * Start the mavlink task. + // * + // * @return OK on success. + + // int start(); /** * Display the mavlink status. */ void status(); + pthread_t uorb_receive_start(void); + void * uorb_receive_thread(void *arg); + private: bool _task_should_exit; /**< if true, sensor task should exit */ @@ -122,8 +125,6 @@ private: struct vehicle_attitude_s att; struct airspeed_s airspeed; - struct mavlink_subscriptions mavlink_subs; - int status_sub; int rc_sub; @@ -138,6 +139,6 @@ private: */ uint64_t last_sensor_timestamp; - hrt_abstime last_sent_vfr = 0; + hrt_abstime last_sent_vfr; }; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 6881a2280..ab4074558 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -76,61 +76,42 @@ __BEGIN_DECLS #include "mavlink_bridge_header.h" -#include "waypoints.h" -#include "orb_topics.h" -#include "mavlink_parameters.h" +// #include "waypoints.h" +#include "mavlink_receiver.h" +#include "mavlink_main.h" #include "util.h" __END_DECLS -/* XXX should be in a header somewhere */ -extern "C" pthread_t receive_start(int uart); - -static void handle_message(mavlink_message_t *msg); -static void *receive_thread(void *arg); - -static mavlink_status_t status; -static struct vehicle_vicon_position_s vicon_position; -static struct vehicle_command_s vcmd; -static struct offboard_control_setpoint_s offboard_control_sp; - -struct vehicle_global_position_s hil_global_pos; -struct vehicle_local_position_s hil_local_pos; -struct vehicle_attitude_s hil_attitude; -struct vehicle_gps_position_s hil_gps; -struct sensor_combined_s hil_sensors; -struct battery_status_s hil_battery_status; -static orb_advert_t pub_hil_global_pos = -1; -static orb_advert_t pub_hil_local_pos = -1; -static orb_advert_t pub_hil_attitude = -1; -static orb_advert_t pub_hil_gps = -1; -static orb_advert_t pub_hil_sensors = -1; -static orb_advert_t pub_hil_gyro = -1; -static orb_advert_t pub_hil_accel = -1; -static orb_advert_t pub_hil_mag = -1; -static orb_advert_t pub_hil_baro = -1; -static orb_advert_t pub_hil_airspeed = -1; -static orb_advert_t pub_hil_battery = -1; - -/* packet counter */ -static int hil_counter = 0; -static int hil_frames = 0; -static uint64_t old_timestamp = 0; - -static orb_advert_t cmd_pub = -1; -static orb_advert_t flow_pub = -1; - -static orb_advert_t offboard_control_sp_pub = -1; -static orb_advert_t vicon_position_pub = -1; -static orb_advert_t telemetry_status_pub = -1; - -// variables for HIL reference position -static int32_t lat0 = 0; -static int32_t lon0 = 0; -static double alt0 = 0; - -static void -handle_message(mavlink_message_t *msg) +void MavlinkReceiver::MavlinkReceiver() : + pub_hil_global_pos(-1), + pub_hil_local_pos(-1), + pub_hil_attitude(-1), + pub_hil_gps(-1), + pub_hil_sensors(-1), + pub_hil_gyro(-1), + pub_hil_accel(-1), + pub_hil_mag(-1), + pub_hil_baro(-1), + pub_hil_airspeed(-1), + pub_hil_battery(-1), + hil_counter(0), + hil_frames(0), + old_timestamp(0), + cmd_pub(-1), + flow_pub(-1), + offboard_control_sp_pub(-1), + vicon_position_pub(-1), + telemetry_status_pub(-1), + lat0(0), + lon0(0), + alt0(0) +{ + +} + +void +MavlinkReceiver::handle_message(mavlink_message_t *msg) { if (msg->msgid == MAVLINK_MSG_ID_COMMAND_LONG) { @@ -804,8 +785,8 @@ handle_message(mavlink_message_t *msg) /** * Receive data from UART. */ -static void * -receive_thread(void *arg) +void * +MavlinkReceiver::receive_thread(void *arg) { int uart_fd = *((int *)arg); @@ -851,8 +832,13 @@ receive_thread(void *arg) return NULL; } +void MavlinkReceiver::print_status() +{ + +} + pthread_t -receive_start(int uart) +MavlinkReceiver::receive_start(int uart) { pthread_attr_t receiveloop_attr; pthread_attr_init(&receiveloop_attr); diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 88ae4b110..556b6f8ad 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -40,6 +40,36 @@ #pragma once +#include <systemlib/perf_counter.h> +#include <uORB/uORB.h> +#include <uORB/topics/sensor_combined.h> +#include <uORB/topics/rc_channels.h> +#include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/vehicle_gps_position.h> +#include <uORB/topics/vehicle_global_position.h> +#include <uORB/topics/vehicle_local_position.h> +#include <uORB/topics/home_position.h> +#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/offboard_control_setpoint.h> +#include <uORB/topics/vehicle_command.h> +#include <uORB/topics/vehicle_local_position_setpoint.h> +#include <uORB/topics/mission_item_triplet.h> +#include <uORB/topics/vehicle_vicon_position.h> +#include <uORB/topics/mission_item_triplet.h> +#include <uORB/topics/vehicle_attitude_setpoint.h> +#include <uORB/topics/vehicle_rates_setpoint.h> +#include <uORB/topics/vehicle_control_mode.h> +#include <uORB/topics/optical_flow.h> +#include <uORB/topics/actuator_outputs.h> +#include <uORB/topics/actuator_controls_effective.h> +#include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_armed.h> +#include <uORB/topics/manual_control_setpoint.h> +#include <uORB/topics/telemetry_status.h> +#include <uORB/topics/debug_key_value.h> +#include <uORB/topics/airspeed.h> +#include <uORB/topics/battery_status.h> + class Mavlink; class MavlinkReceiver @@ -65,7 +95,9 @@ public: /** * Display the mavlink status. */ - void status(); + void print_status(); + + pthread_t receive_start(int uart); private: @@ -75,14 +107,41 @@ private: Mavlink* _mavlink; - /** - * Shim for calling task_main from task_create. - */ - void task_main_trampoline(int argc, char *argv[]); - /** - * Main sensor collection task. - */ - void task_main() __attribute__((noreturn)); + void handle_message(mavlink_message_t *msg); + void *receive_thread(void *arg); + + mavlink_status_t status; + struct vehicle_vicon_position_s vicon_position; + struct vehicle_command_s vcmd; + struct offboard_control_setpoint_s offboard_control_sp; + struct vehicle_global_position_s hil_global_pos; + struct vehicle_local_position_s hil_local_pos; + struct vehicle_attitude_s hil_attitude; + struct vehicle_gps_position_s hil_gps; + struct sensor_combined_s hil_sensors; + struct battery_status_s hil_battery_status; + orb_advert_t pub_hil_global_pos; + orb_advert_t pub_hil_local_pos; + orb_advert_t pub_hil_attitude; + orb_advert_t pub_hil_gps; + orb_advert_t pub_hil_sensors; + orb_advert_t pub_hil_gyro; + orb_advert_t pub_hil_accel; + orb_advert_t pub_hil_mag; + orb_advert_t pub_hil_baro; + orb_advert_t pub_hil_airspeed; + orb_advert_t pub_hil_battery; + int hil_counter; + int hil_frames; + uint64_t old_timestamp; + orb_advert_t cmd_pub; + orb_advert_t flow_pub; + orb_advert_t offboard_control_sp_pub; + orb_advert_t vicon_position_pub; + orb_advert_t telemetry_status_pub; + int32_t lat0; + int32_t lon0; + double alt0; }; |