Hello everyone,
I am trying to build a bridge between PX4 (autopilot) and QGroundControl using a MOOS-IvP application, but I am stuck on a MAVLink/UDP communication issue.
My goal is:
PX4 ↔ MOOS Bridge ↔ QGroundControl
The MOOS application receives MAVLink messages from PX4, publishes selected information into MOOSDB, and forwards MAVLink traffic to QGroundControl. The opposite direction should also work, so commands sent by QGC reach PX4 through the bridge.
Current setup:
-
PX4 sends MAVLink messages to the bridge through UDP.
-
The bridge successfully receives and decodes MAVLink packets from PX4.
-
The bridge forwards MAVLink packets to QGroundControl.
The problem appears in the reverse direction: QGroundControl replies to messages (e.g. PARAM_REQUEST_LIST) coming from PX4, but the bridge does not forward those messages back to PX4, so PX4 cannot reply correctly. I suspect there may be an issue related to UDP endpoints.
Has anyone implemented a MAVLink UDP proxy/bridge between PX4 and QGroundControl? Does PX4 require packets to come from a specific UDP endpoint or source port?
The following image shows what happens: QGroundControl initially displays vehicle information, but after a few seconds the message shown below appears.
The UDP sockets are opened as follows:
if (m_udp_mode) {
// Setup a UDP socket with the specified endpoint
m_udp = boost::shared_ptr<udp::socket>( //UDP SOCKET PX4 -> MOOS
new udp::socket(m_io, udp::endpoint(ip::address::from_string(m_udp_host), atoi(m_udp_port.c_str())))); //ascolta tutte le interfacce di rete sulla porta specificata, udp::v4()
m_udp->non_blocking(true);
if (m_transmit_to_QGC) {
m_QGC_endpoint = udp::endpoint(ip::address::from_string(m_udp_dest), atoi(m_udp_dest_port.c_str()));
m_udp_QGC = boost::shared_ptr<udp::socket>(new udp::socket(m_io));
m_udp_QGC->open(udp::v4()); //UDP SOCKET MOOS -> QGC
m_udp_QGC->bind(udp::endpoint(udp::v4(), 59016));
m_udp_QGC->non_blocking(true);
}
}
m_udp is the socket used by the MOOS application to communicate with PX4, while m_udp_QGC is the UDP socket used to communicate with QGroundControl.
The following code snippet shows part of the MOOS app responsible for communication between PX4 and QGroundControl.
bool PX4::Iterate()
{
size_t Nbytes = 0;
size_t NbytesQGC = 0;
// Read Mavlink messages here in a loop
// We'll need to iterate fairly quickly...
if (m_udp_mode) {
boost::system::error_code ec;
// Read from udp endpoint
Nbytes = m_udp->receive_from(
buffer(m_buf, BUFFER_LENGTH), m_sender_endpoint, 0, ec); //RECEIVING FROM PX4, 0: nessuna opzione speciale (flag), ec: contenitore errore
if (ec == boost::asio::error::would_block || ec == boost::asio::error::try_again)
{
Nbytes = 0;
cout << "empty PX4 -> MOOS message" << endl;
}
} else {
// Read from serial port
Nbytes = read(*m_serial, buffer(m_buf, BUFFER_LENGTH));
// Read from QGC serial port
NbytesQGC = read(*m_serial_dest, buffer(m_bufQGC, BUFFER_LENGTH_QGC));
}
// Transmit Mavlink messages to QGC, transmit Mavlink messages from QGC to PX4
if (m_udp_mode && m_transmit_to_QGC) {
// Send binary Mavlink message via UDP to QGC
if (Nbytes > 0) {
m_udp_QGC->send_to(buffer(m_buf, Nbytes), m_QGC_endpoint); //FORWARDING PX4 MESSAGES TO QGC
}
auto local_ep = m_udp_QGC->local_endpoint();
cout << local_ep.address().to_string() << endl;
cout << local_ep.port() << endl;
boost::system::error_code ecQGC;
NbytesQGC = m_udp_QGC->receive_from(
buffer(m_bufQGC, BUFFER_LENGTH_QGC), m_QGC_endpoint, 0, ecQGC); //RECEIVING FROM QGC
if (ecQGC == boost::asio::error::would_block || ecQGC == boost::asio::error::try_again)
{
NbytesQGC = 0;
cout << "empty QGC -> MOOS message" << endl;
}
// Send binary Mavlink message via UDP to PX4
if (NbytesQGC > 0) {
m_udp->send_to(buffer(m_bufQGC, NbytesQGC), m_sender_endpoint); //FORWARDING QGC MESSAGES TO PX4
}
}
else if (!m_udp_mode && m_transmit_to_QGC) {
// Send binary Mavlink message via serial to QGC
write(*m_serial_dest, buffer(m_buf, Nbytes));
// Send binary Mavlink message via serial to PX4
write(*m_serial, buffer(m_bufQGC, NbytesQGC));
}
// Publish received Mavlink message
Notify("MAVLINK_RECEIVE", m_buf, Nbytes);
return(true);
}