diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-02-18 10:36:46 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-02-21 08:36:45 +0100 |
commit | f1f1ecd096f2ad7b791127c9ee943a7dfc0ab3f4 (patch) | |
tree | cc997acab250c45b9d4a732e4bade1e70d6087b0 /src/drivers/px4io/px4io.cpp | |
parent | b58fa2dffc22b3ba27ac87f1f514c298024cea4a (diff) | |
download | px4-firmware-f1f1ecd096f2ad7b791127c9ee943a7dfc0ab3f4.tar.gz px4-firmware-f1f1ecd096f2ad7b791127c9ee943a7dfc0ab3f4.tar.bz2 px4-firmware-f1f1ecd096f2ad7b791127c9ee943a7dfc0ab3f4.zip |
Fix mavlink feedback FD handling
Diffstat (limited to 'src/drivers/px4io/px4io.cpp')
-rw-r--r-- | src/drivers/px4io/px4io.cpp | 27 |
1 files changed, 12 insertions, 15 deletions
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index efcf4d12b..13326e962 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -244,8 +244,7 @@ private: volatile int _task; ///< worker task id volatile bool _task_should_exit; ///< worker terminate flag - int _mavlink_fd; ///< mavlink file descriptor. This is opened by class instantiation and Doesn't appear to be usable in main thread. - int _thread_mavlink_fd; ///< mavlink file descriptor for thread. + int _mavlink_fd; ///< mavlink file descriptor. perf_counter_t _perf_update; ///<local performance counter for status updates perf_counter_t _perf_write; ///<local performance counter for PWM control writes @@ -474,7 +473,6 @@ PX4IO::PX4IO(device::Device *interface) : _task(-1), _task_should_exit(false), _mavlink_fd(-1), - _thread_mavlink_fd(-1), _perf_update(perf_alloc(PC_ELAPSED, "io update")), _perf_write(perf_alloc(PC_ELAPSED, "io write")), _perf_chan_count(perf_alloc(PC_COUNT, "io rc #")), @@ -507,9 +505,6 @@ PX4IO::PX4IO(device::Device *interface) : /* we need this potentially before it could be set in task_main */ g_dev = this; - /* open MAVLink text channel */ - _mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0); - _debug_enabled = false; _servorail_status.rssi_v = 0; } @@ -785,7 +780,7 @@ PX4IO::task_main() hrt_abstime poll_last = 0; hrt_abstime orb_check_last = 0; - _thread_mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0); + _mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0); /* * Subscribe to the appropriate PWM output topic based on whether we are the @@ -880,6 +875,10 @@ PX4IO::task_main() /* run at 5Hz */ orb_check_last = now; + /* try to claim the MAVLink log FD */ + if (_mavlink_fd < 0) + _mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0); + /* check updates on uORB topics and handle it */ bool updated = false; @@ -1275,16 +1274,14 @@ void PX4IO::dsm_bind_ioctl(int dsmMode) { if (!(_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) { - /* 0: dsm2, 1:dsmx */ - if ((dsmMode == 0) || (dsmMode == 1)) { - mavlink_log_info(_thread_mavlink_fd, "[IO] binding dsm%s rx", (dsmMode == 0) ? "2" : ((dsmMode == 1) ? "x" : "x8")); - ioctl(nullptr, DSM_BIND_START, (dsmMode == 0) ? DSM2_BIND_PULSES : ((dsmMode == 1) ? DSMX_BIND_PULSES : DSMX8_BIND_PULSES)); - } else { - mavlink_log_info(_thread_mavlink_fd, "[IO] invalid dsm bind mode, bind request rejected"); - } + mavlink_log_info(_mavlink_fd, "[IO] binding DSM%s RX", (dsmMode == 0) ? "2" : ((dsmMode == 1) ? "-X" : "-X8")); + int ret = ioctl(nullptr, DSM_BIND_START, (dsmMode == 0) ? DSM2_BIND_PULSES : ((dsmMode == 1) ? DSMX_BIND_PULSES : DSMX8_BIND_PULSES)); + + if (ret) + mavlink_log_critical(_mavlink_fd, "binding failed."); } else { - mavlink_log_info(_thread_mavlink_fd, "[IO] system armed, bind request rejected"); + mavlink_log_info(_mavlink_fd, "[IO] system armed, bind request rejected"); } } |