From 523637e0f1fb0247111818d0a88ce8c4574728ba Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 14 Feb 2014 13:36:59 +0100 Subject: [PATCH] Mavlink: Start multiple uart listeners, HIL working --- src/modules/mavlink/mavlink_main.cpp | 41 +++++++++++---- src/modules/mavlink/mavlink_main.h | 55 +++++++------------- src/modules/mavlink/mavlink_orb_listener.cpp | 12 ++--- src/modules/mavlink/mavlink_orb_listener.h | 23 +------- src/modules/mavlink/mavlink_receiver.cpp | 16 +++--- src/modules/mavlink/mavlink_receiver.h | 3 +- 6 files changed, 68 insertions(+), 82 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 0d9d061900..79aa6135b3 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -309,6 +309,16 @@ int Mavlink::get_uart_fd(unsigned index) return -1; } +int Mavlink::get_uart_fd() +{ + return _uart; +} + +int Mavlink::get_channel() +{ + return (int)_chan; +} + void Mavlink::parameters_update() { @@ -945,7 +955,7 @@ void Mavlink::mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8 wpa.target_component = compid; wpa.type = type; - mavlink_msg_mission_ack_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpa); + mavlink_msg_mission_ack_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpa); mavlink_missionlib_send_message(&msg); if (verbose) warnx("Sent waypoint ack (%u) to ID %u", wpa.type, wpa.target_system); @@ -968,7 +978,7 @@ void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq) wpc.seq = seq; - mavlink_msg_mission_current_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpc); + mavlink_msg_mission_current_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpc); mavlink_missionlib_send_message(&msg); if (verbose) warnx("Broadcasted new current waypoint %u", wpc.seq); @@ -988,7 +998,7 @@ void Mavlink::mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uin wpc.target_component = compid; wpc.count = mission.count; - mavlink_msg_mission_count_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpc); + mavlink_msg_mission_count_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpc); mavlink_missionlib_send_message(&msg); if (verbose) warnx("Sent waypoint count (%u) to ID %u", wpc.count, wpc.target_system); @@ -1018,7 +1028,7 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t wp.target_system = sysid; wp.target_component = compid; wp.seq = seq; - mavlink_msg_mission_item_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wp); + mavlink_msg_mission_item_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wp); mavlink_missionlib_send_message(&msg); if (verbose) warnx("Sent waypoint %u to ID %u", wp.seq, wp.target_system); @@ -1036,7 +1046,7 @@ void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, u wpr.target_system = sysid; wpr.target_component = compid; wpr.seq = seq; - mavlink_msg_mission_request_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpr); + mavlink_msg_mission_request_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpr); mavlink_missionlib_send_message(&msg); if (verbose) warnx("Sent waypoint request %u to ID %u", wpr.seq, wpr.target_system); @@ -1061,7 +1071,7 @@ void Mavlink::mavlink_wpm_send_waypoint_reached(uint16_t seq) wp_reached.seq = seq; - mavlink_msg_mission_item_reached_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wp_reached); + mavlink_msg_mission_item_reached_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wp_reached); mavlink_missionlib_send_message(&msg); if (verbose) warnx("Sent waypoint %u reached message", wp_reached.seq); @@ -1318,7 +1328,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) mavlink_mission_item_t wp; mavlink_msg_mission_item_decode(msg, &wp); - if (wp.target_system == mavlink_system.sysid && wp.target_component == mavlink_wpm_comp_id) { + if (wp.target_system == mavlink_system.sysid && wp.target_component == _mavlink_wpm_comp_id) { wpm->timestamp_lastaction = now; @@ -1576,6 +1586,21 @@ Mavlink::task_main(int argc, char *argv[]) break; } + switch(_mode) { + case MODE_OFFBOARD: + case MODE_HIL: + _mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER; + break; + case MODE_ONBOARD: + _mavlink_wpm_comp_id = MAV_COMP_ID_CAMERA; + break; + case MODE_TX_HEARTBEAT_ONLY: + default: + _mavlink_wpm_comp_id = MAV_COMP_ID_ALL; + warnx("Error: Unknown mode"); + break; + } + /* Flush stdout in case MAVLink is about to take it over */ fflush(stdout); @@ -1597,11 +1622,9 @@ Mavlink::task_main(int argc, char *argv[]) mavlink_update_system(); /* start the MAVLink receiver */ -// MavlinkReceiver rcv(this); receive_thread = MavlinkReceiver::receive_start(this); /* start the ORB receiver */ -// MavlinkOrbListener listener(this); uorb_receive_thread = MavlinkOrbListener::uorb_receive_start(this); /* initialize waypoint manager */ diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index d5bbb746b9..c667a41daa 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -96,46 +96,27 @@ enum MAVLINK_WPM_CODES { }; -/* 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_MAX_WP_COUNT 255 +#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; + uint16_t size; + uint16_t max_size; + enum MAVLINK_WPM_STATES current_state; + int16_t current_wp_id; ///< Waypoint in current transmission + uint16_t current_count; + uint8_t current_partner_sysid; + uint8_t current_partner_compid; + uint64_t timestamp_lastaction; + uint64_t timestamp_last_send_setpoint; + uint32_t timeout; + int current_dataman_id; }; + class Mavlink { @@ -174,7 +155,9 @@ public: static int get_uart_fd(unsigned index); - int get_uart_fd() { return _uart; } + int get_uart_fd(); + + int get_channel(); const char *device_name; @@ -294,7 +277,7 @@ private: uint8_t missionlib_msg_buf[300]; //XXX MAGIC NUMBER MAVLINK_MODE _mode; - uint8_t mavlink_wpm_comp_id; + uint8_t _mavlink_wpm_comp_id; mavlink_channel_t _chan; // XXX probably should be in a header... diff --git a/src/modules/mavlink/mavlink_orb_listener.cpp b/src/modules/mavlink/mavlink_orb_listener.cpp index a2b71c931c..fdc196371b 100644 --- a/src/modules/mavlink/mavlink_orb_listener.cpp +++ b/src/modules/mavlink/mavlink_orb_listener.cpp @@ -55,14 +55,14 @@ #include #include #include +#include + #include #include "mavlink_orb_listener.h" #include "mavlink_main.h" - void *uorb_receive_thread(void *arg); - uint16_t cm_uint16_from_m_float(float m); @@ -647,9 +647,9 @@ void * MavlinkOrbListener::uorb_receive_thread(void *arg) { /* Set thread name */ - char buf[32]; - sprintf(buf, "mavlink rcv%d", Mavlink::instance_count()); - prctl(PR_SET_NAME, buf, getpid()); + char thread_name[18]; + sprintf(thread_name, "mavlink_uorb_rcv_%d", _mavlink->get_channel()); + prctl(PR_SET_NAME, thread_name, getpid()); /* * set up poll to block for new data, @@ -822,7 +822,7 @@ MavlinkOrbListener::uorb_receive_start(Mavlink* mavlink) pthread_attr_setstacksize(&uorb_attr, 2048); pthread_t thread; - pthread_create(&thread, &uorb_attr, MavlinkOrbListener::uorb_start_helper, mavlink); + pthread_create(&thread, &uorb_attr, MavlinkOrbListener::uorb_start_helper, (void*)mavlink); pthread_attr_destroy(&uorb_attr); return thread; diff --git a/src/modules/mavlink/mavlink_orb_listener.h b/src/modules/mavlink/mavlink_orb_listener.h index 317a489d4b..26a2e832f7 100644 --- a/src/modules/mavlink/mavlink_orb_listener.h +++ b/src/modules/mavlink/mavlink_orb_listener.h @@ -87,20 +87,7 @@ public: */ ~MavlinkOrbListener(); - // * - // * Start the mavlink task. - // * - // * @return OK on success. - - // int start(); - - /** - * Display the mavlink status. - */ - void status(); - static pthread_t uorb_receive_start(Mavlink *mavlink); - void * uorb_receive_thread(void *arg); struct listener { void (*callback)(const struct listener *l); @@ -124,15 +111,7 @@ private: unsigned _n_listeners; static const unsigned _max_listeners = 32; - /** - * 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 *uorb_receive_thread(void *arg); static void l_sensor_combined(const struct listener *l); static void l_vehicle_attitude(const struct listener *l); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 44fff24166..371f945c4c 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -798,7 +798,10 @@ MavlinkReceiver::receive_thread(void *arg) mavlink_message_t msg; - prctl(PR_SET_NAME, "mavlink_uart_rcv", getpid()); + /* Set thread name */ + char thread_name[18]; + sprintf(thread_name, "mavlink_uart_rcv_%d", _mavlink->get_channel()); + prctl(PR_SET_NAME, thread_name, getpid()); struct pollfd fds[1]; fds[0].fd = uart_fd; @@ -842,20 +845,19 @@ void MavlinkReceiver::print_status() void * MavlinkReceiver::start_helper(void *context) { - MavlinkReceiver *rcv = new MavlinkReceiver(((Mavlink *)context)); + MavlinkReceiver *rcv = new MavlinkReceiver((Mavlink*)context); return rcv->receive_thread(NULL); } pthread_t -MavlinkReceiver::receive_start(Mavlink *mavlink) +MavlinkReceiver::receive_start(Mavlink *parent) { - pthread_attr_t receiveloop_attr; pthread_attr_init(&receiveloop_attr); // set to non-blocking read - int flags = fcntl(mavlink->get_uart_fd(), F_GETFL, 0); - fcntl(mavlink->get_uart_fd(), F_SETFL, flags | O_NONBLOCK); + int flags = fcntl(parent->get_uart_fd(), F_GETFL, 0); + fcntl(parent->get_uart_fd(), F_SETFL, flags | O_NONBLOCK); struct sched_param param; (void)pthread_attr_getschedparam(&receiveloop_attr, ¶m); @@ -865,7 +867,7 @@ MavlinkReceiver::receive_start(Mavlink *mavlink) pthread_attr_setstacksize(&receiveloop_attr, 3000); pthread_t thread; - pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, mavlink); + pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, (void*)parent); pthread_attr_destroy(&receiveloop_attr); return thread; diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 6614e13f45..8e139e5e41 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -95,7 +95,7 @@ public: */ void print_status(); - static pthread_t receive_start(Mavlink* mavlink); + static pthread_t receive_start(Mavlink *parent); static void * start_helper(void *context); @@ -105,7 +105,6 @@ private: Mavlink* _mavlink; - void handle_message(mavlink_message_t *msg); void *receive_thread(void *arg);