From 63b18399c26acb1e3cf771376c3376b4d00a407a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 28 Jan 2014 21:05:00 +0100 Subject: Butchered MAVLink C++ app to compile and link - there is no hope it will work out of the box 8) --- src/modules/mavlink/mavlink_bridge_header.h | 6 +- src/modules/mavlink/mavlink_main.cpp | 122 ++++++++++++++------------- src/modules/mavlink/mavlink_main.h | 42 +++++---- src/modules/mavlink/mavlink_orb_listener.cpp | 24 +++--- src/modules/mavlink/mavlink_orb_listener.h | 1 + src/modules/mavlink/mavlink_receiver.cpp | 5 +- src/modules/mavlink/mavlink_receiver.h | 2 +- src/modules/mavlink/module.mk | 3 +- 8 files changed, 105 insertions(+), 100 deletions(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_bridge_header.h b/src/modules/mavlink/mavlink_bridge_header.h index 1fae3ee9d..374d1511c 100644 --- a/src/modules/mavlink/mavlink_bridge_header.h +++ b/src/modules/mavlink/mavlink_bridge_header.h @@ -42,6 +42,8 @@ #ifndef MAVLINK_BRIDGE_HEADER_H #define MAVLINK_BRIDGE_HEADER_H +__BEGIN_DECLS + #define MAVLINK_USE_CONVENIENCE_FUNCTIONS /* use efficient approach, see mavlink_helpers.h */ @@ -72,11 +74,13 @@ extern mavlink_system_t mavlink_system; * @param chan MAVLink channel to use, usually MAVLINK_COMM_0 = UART0 * @param ch Character to send */ -extern void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int length); +void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int length); extern mavlink_status_t *mavlink_get_channel_status(uint8_t chan); extern mavlink_message_t *mavlink_get_channel_buffer(uint8_t chan); #include +__END_DECLS + #endif /* MAVLINK_BRIDGE_HEADER_H */ diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index cd37c5437..6c04d2aba 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -97,6 +97,8 @@ #endif static const int ERROR = -1; +static Mavlink* _head = nullptr; + /** * mavlink app start / stop handling function * @@ -203,10 +205,10 @@ void Mavlink::set_mode(enum MAVLINK_MODE mode) int Mavlink::instance_count() { /* note: a local buffer count will help if this ever is called often */ - Mavlink* inst = _head; + Mavlink* inst = ::_head; unsigned inst_index = 0; - while (inst->_head != nullptr) { - inst = inst->_head; + while (inst->_next != nullptr) { + inst = inst->_next; inst_index++; } @@ -216,22 +218,22 @@ int Mavlink::instance_count() Mavlink* Mavlink::new_instance(const char *port, unsigned baud_rate) { Mavlink* inst = new Mavlink(port, baud_rate); - Mavlink* parent = _head; - while (parent->_head != nullptr) - parent = parent->_head; + Mavlink* parent = ::_head; + while (parent->_next != nullptr) + parent = parent->_next; /* now parent points to a null pointer, fill it */ - parent->_head = inst; + parent->_next = inst; return inst; } Mavlink* Mavlink::get_instance(unsigned instance) { - Mavlink* inst = _head; + Mavlink* inst = ::_head; unsigned inst_index = 0; - while (inst->_head != nullptr && inst_index < instance) { - inst = inst->_head; + while (inst->_next != nullptr && inst_index < instance) { + inst = inst->_next; inst_index++; } @@ -428,9 +430,9 @@ Mavlink::set_hil_on_off(bool hil_enabled) int ret = OK; /* Enable HIL */ - if (hil_enabled && !mavlink_hil_enabled) { + if (hil_enabled && !_mavlink_hil_enabled) { - mavlink_hil_enabled = true; + _mavlink_hil_enabled = true; /* ramp up some HIL-related subscriptions */ unsigned hil_rate_interval; @@ -456,8 +458,8 @@ Mavlink::set_hil_on_off(bool hil_enabled) set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, hil_rate_interval); } - if (!hil_enabled && mavlink_hil_enabled) { - mavlink_hil_enabled = false; + if (!hil_enabled && _mavlink_hil_enabled) { + _mavlink_hil_enabled = false; orb_set_interval(subs.spa_sub, 200); } else { @@ -1426,12 +1428,6 @@ Mavlink::mavlink_missionlib_send_gcs_string(const char *string) } } -void -Mavlink::task_main_trampoline(int argc, char *argv[]) -{ - mavlink::g_mavlink->task_main(argc, argv); -} - int Mavlink::task_main(int argc, char *argv[]) { @@ -1481,43 +1477,43 @@ Mavlink::task_main(int argc, char *argv[]) /* initialize mavlink text message buffering */ - mavlink_logbuffer_init(&lb, 10); + // mavlink_logbuffer_init(&lb, 10); int ch; char *device_name = "/dev/ttyS1"; baudrate = 57600; - /* work around some stupidity in task_create's argv handling */ - argc -= 2; - argv += 2; + // /* work around some stupidity in task_create's argv handling */ + // argc -= 2; + // argv += 2; - while ((ch = getopt(argc, argv, "b:d:eo")) != EOF) { - switch (ch) { - case 'b': - baudrate = strtoul(optarg, NULL, 10); + // while ((ch = getopt(argc, argv, "b:d:eo")) != EOF) { + // switch (ch) { + // case 'b': + // baudrate = strtoul(optarg, NULL, 10); - if (baudrate < 9600 || baudrate > 921600) - errx(1, "invalid baud rate '%s'", optarg); + // if (baudrate < 9600 || baudrate > 921600) + // errx(1, "invalid baud rate '%s'", optarg); - break; + // break; - case 'd': - device_name = optarg; - break; + // case 'd': + // device_name = optarg; + // break; - case 'e': - mavlink_link_termination_allowed = true; - break; + // case 'e': + // mavlink_link_termination_allowed = true; + // break; - case 'o': - _mode = MODE_ONBOARD; - break; + // case 'o': + // _mode = MODE_ONBOARD; + // break; - default: - usage(); - break; - } - } + // default: + // usage(); + // break; + // } + // } struct termios uart_config_original; @@ -1699,15 +1695,15 @@ Mavlink::task_main(int argc, char *argv[]) /* sleep 10 ms */ usleep(10000); - /* send one string at 10 Hz */ - if (!mavlink_logbuffer_is_empty(&lb)) { - struct mavlink_logmessage msg; - int lb_ret = mavlink_logbuffer_read(&lb, &msg); + // /* send one string at 10 Hz */ + // if (!mavlink_logbuffer_is_empty(&lb)) { + // struct mavlink_logmessage msg; + // int lb_ret = mavlink_logbuffer_read(&lb, &msg); - if (lb_ret == OK) { - mavlink_missionlib_send_gcs_string(msg.text); - } - } + // if (lb_ret == OK) { + // mavlink_missionlib_send_gcs_string(msg.text); + // } + // } /* sleep 15 ms */ usleep(15000); @@ -1742,27 +1738,32 @@ Mavlink::task_main(int argc, char *argv[]) _exit(0); } +int Mavlink::start_helper(int argc, char *argv[]) +{ + // This is beyond evil.. and needs a lock to be safe + return Mavlink::get_instance(Mavlink::instance_count() - 1)->task_main(argc, argv); +} + int -Mavlink::start() +Mavlink::start(Mavlink* mavlink) { - ASSERT(_mavlink_task == -1); /* start the task */ char buf[32]; sprintf(buf, "mavlink if%d", Mavlink::instance_count()); - _mavlink_task = task_spawn_cmd(buf, + mavlink->_mavlink_task = task_spawn_cmd(buf, SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2048, - (main_t)&Mavlink::task_main_trampoline, - nullptr); + (main_t)&Mavlink::start_helper, + NULL); // while (!this->is_running()) { // usleep(200); // } - if (_mavlink_task < 0) { + if (mavlink->_mavlink_task < 0) { warn("task start failed"); return -errno; } @@ -1794,6 +1795,9 @@ int mavlink_main(int argc, char *argv[]) if (mavlink::g_mavlink == nullptr) mavlink::g_mavlink = instance; + // Instantiate thread + Mavlink::start(instance); + // if (mavlink::g_mavlink != nullptr) { // errx(1, "already running"); // } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 3b6714559..e50b0f0c0 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -155,7 +155,7 @@ public: * * @return OK on success. */ - int start(); + static int start(Mavlink* mavlink); /** * Display the mavlink status. @@ -170,7 +170,7 @@ public: static int get_uart_fd(unsigned index); - int get_uart_fd() { return _mavlink_fd; } + int get_uart_fd() { return uart; } enum MAVLINK_MODE { MODE_TX_HEARTBEAT_ONLY=0, @@ -180,7 +180,7 @@ public: }; void set_mode(enum MAVLINK_MODE); - enum MAVLINK_MODE get_mode(); + enum MAVLINK_MODE get_mode() { return _mode; } bool hil_enabled() { return _mavlink_hil_enabled; }; @@ -189,11 +189,24 @@ public: */ void mavlink_wpm_message_handler(const mavlink_message_t *msg); + static int start_helper(int argc, char *argv[]); + /** * Handle parameter related messages. */ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg); + void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode); + + /** + * Enable / disable Hardware in the Loop simulation mode. + * + * @param hil_enabled The new HIL enable/disable state. + * @return OK if the HIL state changed, ERROR if the + * requested change could not be made or was + * redundant. + */ + int set_hil_on_off(bool hil_enabled); struct mavlink_subscriptions { int sensor_sub; @@ -217,12 +230,13 @@ public: int input_rc_sub; int optical_flow; int rates_setpoint_sub; - int home_sub; + int get_sub; int airspeed_sub; int navigation_capabilities_sub; int control_mode_sub; int rc_sub; int status_sub; + int home_sub; }; struct mavlink_subscriptions subs; @@ -252,6 +266,7 @@ protected: */ struct file_operations fops; int _mavlink_fd; + Mavlink* _next; private: @@ -264,7 +279,6 @@ private: /* states */ bool _mavlink_hil_enabled; /**< Hardware in the loop mode */ - static Mavlink* _head; int _params_sub; @@ -289,7 +303,6 @@ private: mavlink_wpm_storage *wpm; bool verbose; - bool mavlink_hil_enabled; int uart; int baudrate; bool gcs_link; @@ -308,16 +321,6 @@ private: */ void parameters_update(); - /** - * Enable / disable Hardware in the Loop simulation mode. - * - * @param hil_enabled The new HIL enable/disable state. - * @return OK if the HIL state changed, ERROR if the - * requested change could not be made or was - * redundant. - */ - int set_hil_on_off(bool hil_enabled); - /** * Send all parameters at once. * @@ -390,8 +393,6 @@ private: 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. */ @@ -399,11 +400,6 @@ private: static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); - /** - * Shim for calling task_main from task_create. - */ - void task_main_trampoline(int argc, char *argv[]); - /** * Main mavlink task. */ diff --git a/src/modules/mavlink/mavlink_orb_listener.cpp b/src/modules/mavlink/mavlink_orb_listener.cpp index fa1a6887e..a6bcd4c0a 100644 --- a/src/modules/mavlink/mavlink_orb_listener.cpp +++ b/src/modules/mavlink/mavlink_orb_listener.cpp @@ -296,11 +296,11 @@ MavlinkOrbListener::l_vehicle_status(const struct listener *l) } void -MavlinkOrbListener::l_rc__mavlink->get_chan()nels(const struct listener *l) +MavlinkOrbListener::l_rc_channels(const struct listener *l) { - /* copy rc _mavlink->get_chan()nels into local buffer */ - orb_copy(ORB_ID(rc__mavlink->get_chan()nels), rc_sub, &rc); - // XXX Add RC _mavlink->get_chan()nels scaled message here + /* copy rc channels into local buffer */ + orb_copy(ORB_ID(rc_channels), l->mavlink->get_subs()->rc_sub, &l->listener->rc); + // XXX Add RC channels scaled message here } void @@ -315,7 +315,7 @@ MavlinkOrbListener::l_input_rc(const struct listener *l) for (unsigned i = 0; (i * port_width) < (l->listener->rc_raw.channel_count + port_width); i++) { /* Channels are sent in MAVLink main loop at a fixed interval */ - mavlink_msg_rc_channels_raw_send(_mavlink->get_chan(), + mavlink_msg_rc_channels_raw_send(l->mavlink->get_chan(), l->listener->rc_raw.timestamp / 1000, i, (l->listener->rc_raw.channel_count > (i * port_width) + 0) ? l->listener->rc_raw.values[(i * port_width) + 0] : UINT16_MAX, @@ -335,7 +335,7 @@ void MavlinkOrbListener::l_global_position(const struct listener *l) { /* copy global position data into local buffer */ - orb_copy(ORB_ID(vehicle_global_position), _mavlink->get_subs()->global_pos_sub, l->listener->global_pos); + orb_copy(ORB_ID(vehicle_global_position), l->mavlink->get_subs()->global_pos_sub, &l->listener->global_pos); mavlink_msg_global_position_int_send(l->mavlink->get_chan(), l->listener->global_pos.timestamp / 1000, @@ -353,7 +353,7 @@ void MavlinkOrbListener::l_local_position(const struct listener *l) { /* copy local position data into local buffer */ - orb_copy(ORB_ID(vehicle_local_position), _mavlink->get_subs()->local_pos_sub, l->listener->local_pos); + orb_copy(ORB_ID(vehicle_local_position), l->mavlink->get_subs()->local_pos_sub, &l->listener->local_pos); if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) mavlink_msg_local_position_ned_send(l->mavlink->get_chan(), @@ -407,7 +407,7 @@ MavlinkOrbListener::l_attitude_setpoint(const struct listener *l) struct vehicle_attitude_setpoint_s att_sp; /* copy local position data into local buffer */ - orb_copy(ORB_ID(vehicle_attitude_setpoint), _mavlink->get_subs()->spa_sub, &att_sp); + orb_copy(ORB_ID(vehicle_attitude_setpoint), l->mavlink->get_subs()->spa_sub, &att_sp); if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(l->mavlink->get_chan(), @@ -464,13 +464,13 @@ MavlinkOrbListener::l_actuator_outputs(const struct listener *l) act_outputs.output[7]); /* only send in HIL mode and only send first group for HIL */ - if (l->listener->mavlink_hil_enabled && l->listener->armed.armed && ids[l->arg] == ORB_ID(actuator_outputs_0)) { + if (l->mavlink->hil_enabled() && l->listener->armed.armed && ids[l->arg] == ORB_ID(actuator_outputs_0)) { /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state = 0; uint8_t mavlink_base_mode = 0; uint32_t mavlink_custom_mode = 0; - get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); + l->mavlink->get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); /* HIL message as per MAVLink spec */ @@ -616,9 +616,9 @@ MavlinkOrbListener::l_optical_flow(const struct listener *l) void MavlinkOrbListener::l_home(const struct listener *l) { - orb_copy(ORB_ID(home_position), l->mavlink->get_subs()->home_sub, &home); + orb_copy(ORB_ID(home_position), l->mavlink->get_subs()->home_sub, &l->listener->home); - mavlink_msg_gps_global_origin_send(l->mavlink->get_chan(), (int32_t)(home.lat*1e7d), (int32_t)(home.lon*1e7d), (int32_t)(home.alt)*1e3f); + mavlink_msg_gps_global_origin_send(l->mavlink->get_chan(), (int32_t)(l->listener->home.lat*1e7d), (int32_t)(l->listener->home.lon*1e7d), (int32_t)(l->listener->home.alt)*1e3f); } void diff --git a/src/modules/mavlink/mavlink_orb_listener.h b/src/modules/mavlink/mavlink_orb_listener.h index 3988103bc..c9f35a1fb 100644 --- a/src/modules/mavlink/mavlink_orb_listener.h +++ b/src/modules/mavlink/mavlink_orb_listener.h @@ -170,6 +170,7 @@ private: struct actuator_controls_s actuators_0; struct vehicle_attitude_s att; struct airspeed_s airspeed; + struct home_position_s home; int status_sub; int rc_sub; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 982d6c1d8..0f1d5293e 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -106,7 +106,8 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : telemetry_status_pub(-1), lat0(0), lon0(0), - alt0(0) + alt0(0), + thread_should_exit(false) { } @@ -818,7 +819,7 @@ MavlinkReceiver::receive_thread(void *arg) /* if read failed, this loop won't execute */ for (ssize_t i = 0; i < nread; i++) { - if (mavlink_parse_char(chan, buf[i], &msg, &status)) { + if (mavlink_parse_char(_mavlink->get_chan(), buf[i], &msg, &status)) { /* handle generic messages and commands */ handle_message(&msg); diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 483d91e72..be32ce0f7 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -102,7 +102,7 @@ public: private: - bool _task_should_exit; /**< if true, sensor task should exit */ + bool thread_should_exit; /**< if true, sensor task should exit */ perf_counter_t _loop_perf; /**< loop performance counter */ diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index 2a005565e..76798eb12 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -39,7 +39,6 @@ MODULE_COMMAND = mavlink SRCS += mavlink_main.cpp \ mavlink.c \ mavlink_receiver.cpp \ - mavlink_orb_listener.cpp \ - waypoints.cpp + mavlink_orb_listener.cpp INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink -- cgit v1.2.3