Browse Source

don't use default source address for onboard udp link, wait on remote

sbg
Andreas Antener 9 years ago
parent
commit
11ed5169cc
  1. 15
      src/modules/mavlink/mavlink_main.cpp
  2. 5
      src/modules/mavlink/mavlink_main.h
  3. 4
      src/modules/mavlink/mavlink_receiver.cpp

15
src/modules/mavlink/mavlink_main.cpp

@ -930,7 +930,9 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID @@ -930,7 +930,9 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID
}
#else
if (get_protocol() == UDP) {
ret = sendto(_socket_fd, buf, packet_len, 0, (struct sockaddr *)&_src_addr, sizeof(_src_addr));
if (get_client_source_initialized()) {
ret = sendto(_socket_fd, buf, packet_len, 0, (struct sockaddr *)&_src_addr, sizeof(_src_addr));
}
struct telemetry_status_s &tstatus = get_rx_status();
@ -1045,11 +1047,14 @@ Mavlink::init_udp() @@ -1045,11 +1047,14 @@ Mavlink::init_udp()
return;
}
/* set default target address */
/* set default target address, but not for onboard mode (will be set on first recieved packet) */
memset((char *)&_src_addr, 0, sizeof(_src_addr));
_src_addr.sin_family = AF_INET;
inet_aton("127.0.0.1", &_src_addr.sin_addr);
_src_addr.sin_port = htons(DEFAULT_REMOTE_PORT_UDP);
if (_mode != MAVLINK_MODE_ONBOARD) {
set_client_source_initialized();
_src_addr.sin_family = AF_INET;
inet_aton("127.0.0.1", &_src_addr.sin_addr);
_src_addr.sin_port = htons(DEFAULT_REMOTE_PORT_UDP);
}
/* default broadcast address */
memset((char *)&_bcast_addr, 0, sizeof(_bcast_addr));

5
src/modules/mavlink/mavlink_main.h

@ -339,6 +339,10 @@ public: @@ -339,6 +339,10 @@ public:
int get_socket_fd () { return _socket_fd; };
#ifdef __PX4_POSIX
struct sockaddr_in * get_client_source_address() {return &_src_addr;};
void set_client_source_initialized() { _src_addr_initialized = true; };
bool get_client_source_initialized() { return _src_addr_initialized; };
#endif
static bool boot_complete() { return _boot_complete; }
@ -423,6 +427,7 @@ private: @@ -423,6 +427,7 @@ private:
struct sockaddr_in _myaddr;
struct sockaddr_in _src_addr;
struct sockaddr_in _bcast_addr;
bool _src_addr_initialized;
#endif
int _socket_fd;

4
src/modules/mavlink/mavlink_receiver.cpp

@ -1814,10 +1814,12 @@ MavlinkReceiver::receive_thread(void *arg) @@ -1814,10 +1814,12 @@ MavlinkReceiver::receive_thread(void *arg)
struct sockaddr_in * srcaddr_last = _mavlink->get_client_source_address();
int localhost = (127 << 24) + 1;
if (srcaddr_last->sin_addr.s_addr == htonl(localhost) && srcaddr.sin_addr.s_addr != htonl(localhost)) {
if ((srcaddr_last->sin_addr.s_addr == htonl(localhost) && srcaddr.sin_addr.s_addr != htonl(localhost))
|| (_mavlink->get_mode() == Mavlink::MAVLINK_MODE_ONBOARD && !_mavlink->get_client_source_initialized())) {
// if we were sending to localhost before but have a new host then accept him
memcpy(srcaddr_last, &srcaddr, sizeof(srcaddr));
PX4_WARN("UDP source addr changed: %s", inet_ntoa(srcaddr.sin_addr));
_mavlink->set_client_source_initialized();
}
#endif
/* if read failed, this loop won't execute */

Loading…
Cancel
Save