From 1e3d2acbf66b1101a9b17f97d2b786ffaa0e423a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 28 Jan 2014 19:30:23 +0100 Subject: Not building yet, things are coming together slowly on mavlink app --- src/modules/mavlink/mavlink_main.cpp | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 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 1c7986cbb..cd37c5437 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -56,7 +56,6 @@ #include #include #include -#include #include #include #include @@ -196,6 +195,11 @@ Mavlink::~Mavlink() } } +void Mavlink::set_mode(enum MAVLINK_MODE mode) +{ + _mode = mode; +} + int Mavlink::instance_count() { /* note: a local buffer count will help if this ever is called often */ @@ -1506,7 +1510,7 @@ Mavlink::task_main(int argc, char *argv[]) break; case 'o': - mavlink_link_mode = MAVLINK_INTERFACE_MODE_ONBOARD; + _mode = MODE_ONBOARD; break; default: @@ -1523,7 +1527,7 @@ Mavlink::task_main(int argc, char *argv[]) warnx("MAVLink v1.0 serial interface starting..."); /* inform about mode */ - warnx((mavlink_link_mode == MAVLINK_INTERFACE_MODE_ONBOARD) ? "ONBOARD MODE" : "DOWNLINK MODE"); + warnx((_mode == MODE_ONBOARD) ? "ONBOARD MODE" : "DOWNLINK MODE"); /* Flush stdout in case MAVLink is about to take it over */ fflush(stdout); @@ -1541,12 +1545,12 @@ Mavlink::task_main(int argc, char *argv[]) mavlink_update_system(); /* start the MAVLink receiver */ - MavlinkReceiver rcv(this); - receive_thread = rcv.receive_start(uart); + // MavlinkReceiver rcv(this); + receive_thread = MavlinkReceiver::receive_start(this); /* start the ORB receiver */ - MavlinkOrbListener listener(this); - uorb_receive_thread = listener.uorb_receive_start(); + //MavlinkOrbListener listener(this); + uorb_receive_thread = MavlinkOrbListener::uorb_receive_start(this); /* initialize waypoint manager */ mavlink_wpm_init(wpm); -- cgit v1.2.3