fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 23:53:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

mavlink: get mavlink reconnection working

Browse Source
This commit is contained in:
Julian Oes
2016-04-12 10:13:52 -07:00
committed by Lorenz Meier
parent 02fb3d1856
commit 361c057412
2 changed files with 40 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_main.cpp
+38 -32
View File
@@ -888,8 +888,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID
&& (msgid == MAVLINK_MSG_ID_HEARTBEAT)) {
if (!_broadcast_address_found) {
// Try to initialize UDP and broadcast address again.
init_udp();
find_broadcast_address();
}
if (_broadcast_address_found) {
@@ -973,38 +972,9 @@ Mavlink::resend_message(mavlink_message_t *msg)
}
void
Mavlink::init_udp()
Mavlink::find_broadcast_address()
{
#if defined (__PX4_LINUX) || defined (__PX4_DARWIN)
PX4_INFO("Setting up UDP w/port %d",_network_port);
_myaddr.sin_family = AF_INET;
_myaddr.sin_addr.s_addr = htonl(INADDR_ANY);
_myaddr.sin_port = htons(_network_port);
if ((_socket_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
PX4_WARN("create socket failed");
return;
}
int broadcast_opt = 1;
if (setsockopt(_socket_fd, SOL_SOCKET, SO_BROADCAST, &broadcast_opt, sizeof(broadcast_opt)) < 0) {
PX4_WARN("setting broadcast permission failed");
return;
}
if (bind(_socket_fd, (struct sockaddr *)&_myaddr, sizeof(_myaddr)) < 0) {
PX4_WARN("bind failed");
return;
}
/* set default target address, but not for onboard mode (will be set on first received packet) */
if (!_src_addr_initialized) {
_src_addr.sin_family = AF_INET;
inet_aton("127.0.0.1", &_src_addr.sin_addr);
}
_src_addr.sin_port = htons(_remote_port);
struct ifconf ifconf;
int ret;
@@ -1119,11 +1089,46 @@ Mavlink::init_udp()
if (_broadcast_address_found) {
_bcast_addr.sin_port = htons(_remote_port);
int broadcast_opt = 1;
if (setsockopt(_socket_fd, SOL_SOCKET, SO_BROADCAST, &broadcast_opt, sizeof(broadcast_opt)) < 0) {
PX4_WARN("setting broadcast permission failed");
}
} else {
PX4_WARN("no broadcasting address found");
}
delete[] ifconf.ifc_req;
}
void
Mavlink::init_udp()
{
PX4_INFO("Setting up UDP w/port %d", _network_port);
_myaddr.sin_family = AF_INET;
_myaddr.sin_addr.s_addr = htonl(INADDR_ANY);
_myaddr.sin_port = htons(_network_port);
if ((_socket_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
PX4_WARN("create socket failed: %s", strerror(errno));
return;
}
if (bind(_socket_fd, (struct sockaddr *)&_myaddr, sizeof(_myaddr)) < 0) {
PX4_WARN("bind failed: %s", strerror(errno));
return;
}
/* set default target address, but not for onboard mode (will be set on first received packet) */
if (!_src_addr_initialized) {
_src_addr.sin_family = AF_INET;
inet_aton("127.0.0.1", &_src_addr.sin_addr);
}
_src_addr.sin_port = htons(_remote_port);
#endif
}
@@ -1926,6 +1931,7 @@ Mavlink::task_main(int argc, char *argv[])
/* init socket if necessary */
if (get_protocol() == UDP) {
find_broadcast_address();
init_udp();
}
src/modules/mavlink/mavlink_main.h
+2
View File
@@ -510,6 +510,8 @@ private:
*/
void update_rate_mult();
void find_broadcast_address();
void init_udp();
/**