fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 19:03:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #3433 from PX4/posix_uart

Browse Source 
MAVLink app: Enable network and serial on POSIX
This commit is contained in:
Lorenz Meier
2016-01-06 09:27:18 +01:00
parent 99f4487cc3 5227dbe26a
commit 268990f216
2 changed files with 61 additions and 50 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_main.cpp
+45 -50
View File
@@ -51,9 +51,7 @@
#include <assert.h>
#include <math.h>
#include <poll.h>
#ifndef __PX4_POSIX
#include <termios.h>
#endif
#include <time.h>
#include <math.h> /* isinf / isnan checks */
@@ -157,9 +155,7 @@ Mavlink::Mavlink() :
_verbose(false),
_forwarding_on(false),
_ftp_on(false),
#ifndef __PX4_POSIX
_uart_fd(-1),
#endif
_baudrate(57600),
_datarate(1000),
_datarate_events(500),
@@ -607,9 +603,16 @@ int Mavlink::get_component_id()
return mavlink_system.compid;
}
#ifndef __PX4_POSIX
int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original)
{
#ifndef B460800
#define B460800 460800
#endif
#ifndef B921600
#define B921600 921600
#endif
/* process baud rate */
int speed;
@@ -701,7 +704,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
_uart_fd = ::open(uart_name, O_RDWR | O_NOCTTY);
};
close(armed_fd);
::close(armed_fd);
}
if (_uart_fd < 0) {
@@ -808,8 +811,6 @@ Mavlink::enable_flow_control(bool enabled)
return ret;
}
#endif
int
Mavlink::set_hil_enabled(bool hil_enabled)
{
@@ -842,8 +843,6 @@ Mavlink::get_free_tx_buf()
*/
int buf_free = 0;
#ifndef __PX4_POSIX
// No FIONWRITE on Linux
#if !defined(__PX4_LINUX) && !defined(__PX4_DARWIN)
(void) ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free);
@@ -862,8 +861,6 @@ Mavlink::get_free_tx_buf()
}
}
#endif
// if we are using network sockets, return max lenght of one packet
if (get_protocol() == UDP || get_protocol() == TCP ) {
return 1500;
@@ -924,12 +921,13 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID
buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8);
size_t ret = -1;
#ifndef __PX4_POSIX
/* send message to UART */
if (get_protocol() == SERIAL) {
ret = ::write(_uart_fd, buf, packet_len);
}
#else
#ifdef __PX4_POSIX
if (get_protocol() == UDP) {
if (get_client_source_initialized()) {
ret = sendto(_socket_fd, buf, packet_len, 0, (struct sockaddr *)&_src_addr, sizeof(_src_addr));
@@ -1003,21 +1001,20 @@ Mavlink::resend_message(mavlink_message_t *msg)
buf[MAVLINK_NUM_HEADER_BYTES + msg->len] = (uint8_t)(msg->checksum & 0xFF);
buf[MAVLINK_NUM_HEADER_BYTES + msg->len + 1] = (uint8_t)(msg->checksum >> 8);
#ifndef __PX4_POSIX
/* send message to UART */
ssize_t ret = ::write(_uart_fd, buf, packet_len);
if (_uart_fd >= 0) {
/* send message to UART */
ssize_t ret = ::write(_uart_fd, buf, packet_len);
if (ret != (int) packet_len) {
count_txerr();
count_txerrbytes(packet_len);
if (ret != (int) packet_len) {
count_txerr();
count_txerrbytes(packet_len);
} else {
_last_write_success_time = _last_write_try_time;
count_txbytes(packet_len);
} else {
_last_write_success_time = _last_write_try_time;
count_txbytes(packet_len);
}
}
#endif
pthread_mutex_unlock(&_send_mutex);
}
@@ -1618,6 +1615,8 @@ Mavlink::task_main(int argc, char *argv[])
_datarate = MAX_DATA_RATE;
}
struct termios uart_config_original = {};
if (get_protocol() == SERIAL) {
if (Mavlink::instance_exists(_device_name, this)) {
warnx("%s already running", _device_name);
@@ -1626,6 +1625,20 @@ Mavlink::task_main(int argc, char *argv[])
warnx("mode: %u, data rate: %d B/s on %s @ %dB", _mode, _datarate, _device_name, _baudrate);
/* flush stdout in case MAVLink is about to take it over */
fflush(stdout);
/* default values for arguments */
_uart_fd = mavlink_open_uart(_baudrate, _device_name, &uart_config_original);
if (_uart_fd < 0 && _mode != MAVLINK_MODE_CONFIG) {
warn("could not open %s", _device_name);
return ERROR;
} else if (_uart_fd < 0 && _mode == MAVLINK_MODE_CONFIG) {
/* the config link is optional */
return OK;
}
} else if (get_protocol() == UDP) {
if (Mavlink::get_instance_for_network_port(_network_port) != nullptr) {
warnx("port %d already occupied", _network_port);
@@ -1634,24 +1647,6 @@ Mavlink::task_main(int argc, char *argv[])
warnx("mode: %u, data rate: %d B/s on udp port %hu", _mode, _datarate, _network_port);
}
/* flush stdout in case MAVLink is about to take it over */
fflush(stdout);
#ifndef __PX4_POSIX
struct termios uart_config_original;
/* default values for arguments */
_uart_fd = mavlink_open_uart(_baudrate, _device_name, &uart_config_original);
if (_uart_fd < 0 && _mode != MAVLINK_MODE_CONFIG) {
warn("could not open %s", _device_name);
return ERROR;
} else if (_uart_fd < 0 && _mode == MAVLINK_MODE_CONFIG) {
/* the config link is optional */
return OK;
}
#endif
/* initialize send mutex */
pthread_mutex_init(&_send_mutex, NULL);
@@ -1874,7 +1869,7 @@ Mavlink::task_main(int argc, char *argv[])
}
/* radio config check */
if (_radio_id != 0 && _rstatus.type == telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO) {
if (_uart_fd >= 0 && _radio_id != 0 && _rstatus.type == telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO) {
/* request to configure radio and radio is present */
FILE *fs = fdopen(_uart_fd, "w");
@@ -2069,13 +2064,13 @@ Mavlink::task_main(int argc, char *argv[])
/* wait for threads to complete */
pthread_join(_receive_thread, NULL);
#ifndef __PX4_POSIX
/* reset the UART flags to original state */
tcsetattr(_uart_fd, TCSANOW, &uart_config_original);
if (_uart_fd >= 0) {
/* reset the UART flags to original state */
tcsetattr(_uart_fd, TCSANOW, &uart_config_original);
/* close UART */
::close(_uart_fd);
#endif
/* close UART */
::close(_uart_fd);
}
/* close mavlink logging device */
px4_close(_mavlink_fd);
src/modules/mavlink/mavlink_messages.cpp
+16
View File
@@ -2170,6 +2170,22 @@ protected:
msg.rssi = rc.rssi;
_mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS, &msg);
/* send override message - harmless if connected to GCS, allows to connect a board to a Linux system */
/* http://mavlink.org/messages/common#RC_CHANNELS_OVERRIDE */
mavlink_rc_channels_override_t over;
over.target_system = mavlink_system.sysid;
over.target_component = 0;
over.chan1_raw = msg.chan1_raw;
over.chan2_raw = msg.chan2_raw;
over.chan3_raw = msg.chan3_raw;
over.chan4_raw = msg.chan4_raw;
over.chan5_raw = msg.chan5_raw;
over.chan6_raw = msg.chan6_raw;
over.chan7_raw = msg.chan7_raw;
over.chan8_raw = msg.chan8_raw;
_mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, &over);
}
}
};