Unable to send messages to Ardupilot SITL

[RishabhBhat2021]

Question:

Hi, first of all, great work on easing the process of building custom mavlink based applications for newbies like me! I’m trying to create an Android application written in Kotlin which allows the user to perform some basic workflows such as uploading missions to a mavlink vehicle. And I plan to use Dronefleet to achieve the same.

Is there any way in MAVProxy to use custom local ports rather than using random local ports? Like we use 14550 as the remote port, just like that use like 14540 or 14556 something for the local port

My setup

I have remote ArduPilot SITL set up with command output add <phone_ip_address>:14550

ArduPilot Version is AP: ArduCopter V4.3.0-dev (ebe2205b)

Just to validate the SITL setup, I tried running QGC Android application and was able to connect to SITL successfully, and QGC was able to send commands as well

The SITL is running on my laptop and the GCS app is an Android app, and both are connected via remote MAVLink over my local network. My laptop is running Ubuntu 20.04 and Android device is a phone, Samsung Galaxy M51: API level 30 (Android 11)

Issue

I can receive messages from the SITL on my android app, but not able to send GCS heartbeat (and any other message) to the SITL

I’m able to send / receive messages with PX4 SITL setup on my app, but can’t figure out with ArduPilot SITL. I can only receive messages from the ArduPilot SITL but the SITL doesn’t recieve my messages

Code snippet to create UDP streams and send message to the vehicle

val remoteAddress: SocketAddress = InetSocketAddress("<laptop_ip_address>", 14550)

val bufferSize = 65535

val datagramSocket = DatagramSocket(14550)
val udpIn = PipedInputStream()

val udpOut: OutputStream = object : OutputStream() {

val buffer = ByteArray(bufferSize)
var position = 0

@Throws(IOException::class)
override fun write(i: Int) {
                write(byteArrayOf(i.toByte()), 0, 1)
            }

@Synchronized
@Throws(IOException::class)
override fun write(b: ByteArray?, off: Int, len: Int) {
    if (position + len > buffer.size) {
            flush()
            }
    System.arraycopy(b, off, buffer, position, len)
    position += len
}

@Synchronized
@Throws(IOException::class)
override fun flush() {
        val packet = DatagramPacket(buffer, 0, position, remoteAddress)
        datagramSocket.send(packet)
        position = 0
        }
}

val appOut = PipedOutputStream(inputStream)

val service: ExecutorService = Executors.newSingleThreadExecutor()
        service.execute {
            try {
                val packet =
                    DatagramPacket(ByteArray(bufferSize), bufferSize)
                while (!datagramSocket.isClosed) {
                    datagramSocket.receive(packet)
                    appOut.write(packet.data, packet.offset, packet.length)
                    appOut.flush()
                }
            } catch (e: IOException) {
                e.printStackTrace()
            } finally {
                try {
                    appOut.close()
                } catch (e: IOException) {
                    e.printStackTrace()
                }
                if (!datagramSocket.isClosed) {
                    datagramSocket.close()
                }
                if (!service.isShutdown) {
                    service.shutdown()
                }
            }
        }

val mavlinkConnection = MavlinkConnection.builder(udpIn, udpOut)
                      .dialect(MavAutopilot.MAV_AUTOPILOT_ARDUPILOTMEGA, ArdupilotmegaDialect())
                              .build()

val heartBeatBuilder = Heartbeat.builder()
            .type(MavType.MAV_TYPE_GCS)
            .autopilot(MavAutopilot.MAV_AUTOPILOT_INVALID)
            .systemStatus(MavState.MAV_STATE_UNINIT)
            .mavlinkVersion(3)
            .build()

mavlinkConnection.send2(
    255, /* systemId */
    0, /* componentId */
    heartBeatBuilder)
udpOut.flush()

while (mavlinkConnection?.next().also { message = it } != null) {
            if (message == null) continue
            Log.i("Message: ", "${message}")
        }

Messages recieved on the Android Device from the Setup

2022-03-21 12:37:08.774 9080-9751/com.example.mavlinkkotlin D/Vehicle: Vehicle System ID: 1, Vehicle Component ID: 1
2022-03-21 12:37:08.775 9080-9751/com.example.mavlinkkotlin D/Vehicle: MavlinkMessage{packet=MavlinkPacket{versionMarker=253, incompatibleFlags=0, compatibleFlags=0, sequence=183, systemId=1, componentId=1, messageId=30, payload=[-80, -59, 0, 0, -48, 57, -113, 58, 65, -28, 103, 58, -104, -5, -59, -67, -55, -65, -82, 57, -6, 47, -98, 57, -86, -117, 15, 59], checksum=57125, signature=[]}, payload=Attitude{timeBootMs=50608, roll=0.0010927264, pitch=8.845963E-4, yaw=-0.09667128, rollspeed=3.3330757E-4, pitchspeed=3.0171854E-4, yawspeed=0.0021903315}}
2022-03-21 12:37:08.791 9080-9751/com.example.mavlinkkotlin D/Vehicle: MavlinkMessage{packet=MavlinkPacket{versionMarker=253, incompatibleFlags=0, compatibleFlags=0, sequence=184, systemId=1, componentId=1, messageId=33, payload=[-80, -59, 0, 0, -109, -2, -21, -22, 23, -49, -24, 88, 124, -23, 8, 0, -20, -1, -1, -1, -1, -1, 1, 0, 0, 0, 119, -118], checksum=32319, signature=[]}, payload=GlobalPositionInt{timeBootMs=50608, lat=-353632621, lon=1491652375, alt=584060, relativeAlt=-20, vx=-1, vy=1, vz=0, hdg=35447}}
2022-03-21 12:37:08.820 9080-9751/com.example.mavlinkkotlin D/Vehicle: MavlinkMessage{packet=MavlinkPacket{versionMarker=253, incompatibleFlags=0, compatibleFlags=0, sequence=185, systemId=1, componentId=1, messageId=1, payload=[47, -4, 113, 83, 47, -100, 97, 82, 47, -100, 113, 83, 0, 0, 43, 49, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100], checksum=6102, signature=[]}, payload=SysStatus{onboardControlSensorsPresent=EnumValue{value=1399979055, entry=null}, onboardControlSensorsEnabled=EnumValue{value=1382128687, entry=null}, onboardControlSensorsHealth=EnumValue{value=1399954479, entry=null}, load=0, voltageBattery=12587, currentBattery=0, batteryRemaining=100, dropRateComm=0, errorsComm=0, errorsCount1=0, errorsCount2=0, errorsCount3=0, errorsCount4=0}}
2022-03-21 12:37:08.823 9080-9751/com.example.mavlinkkotlin D/Vehicle: MavlinkMessage{packet=MavlinkPacket{versionMarker=253, incompatibleFlags=0, compatibleFlags=0, sequence=186, systemId=1, componentId=1, messageId=125, payload=[-120, 19], checksum=22124, signature=[]}, payload=PowerStatus{vcc=5000, vservo=0, flags=EnumValue{value=0, entry=null}}}
2022-03-21 12:37:08.831 9080-9751/com.example.mavlinkkotlin D/Vehicle: MavlinkMessage{packet=MavlinkPacket{versionMarker=253, incompatibleFlags=0, compatibleFlags=0, sequence=188, systemId=1, componentId=1, messageId=62, payload=[-108, 86, 60, -71, -22, 7, -91, 57, -92, 106, -25, 56, 0, 0, 0, 0, 0, 0, 0, 0, -5, -1], checksum=44095, signature=[]}, payload=NavControllerOutput{navRoll=-1.796133E-4, navPitch=3.147715E-4, navBearing=-5, targetBearing=0, wpDist=0, altError=1.1034802E-4, aspdError=0.0, xtrackError=0.0}}
2022-03-21 12:37:08.835 9080-9751/com.example.mavlinkkotlin D/Vehicle: MavlinkMessage{packet=MavlinkPacket{versionMarker=253, incompatibleFlags=0, compatibleFlags=0, sequence=189, systemId=1, componentId=1, messageId=42, payload=[0], checksum=21543, signature=[]}, payload=MissionCurrent{seq=0}}
2022-03-21 12:37:08.839 9080-9751/com.example.mavlinkkotlin D/Vehicle: MavlinkMessage{packet=MavlinkPacket{versionMarker=253, incompatibleFlags=0, compatibleFlags=0, sequence=190, systemId=1, componentId=1, messageId=74, payload=[0, 0, 0, 0, -94, -24, -99, 60, -41, 3, 18, 68, -121, 6, -2, -72, 98, 1], checksum=6936, signature=[]}, payload=VfrHud{airspeed=0.0, groundspeed=0.019275967, heading=354, throttle=0, alt=584.06, climb=-1.21128796E-4}}
2022-03-21 12:37:08.845 9080-9751/com.example.mavlinkkotlin D/Vehicle: MavlinkMessage{packet=MavlinkPacket{versionMarker=253, incompatibleFlags=0, compatibleFlags=0, sequence=191, systemId=1, componentId=1, messageId=36, payload=[19, 59, 4, 3, -24, 3, -24, 3, -24, 3, -24, 3], checksum=50213, signature=[]}, payload=ServoOutputRaw{timeUsec=50608915, port=0, servo1Raw=1000, servo2Raw=1000, servo3Raw=1000, servo4Raw=1000, servo5Raw=0, servo6Raw=0, servo7Raw=0, servo8Raw=0, servo9Raw=0, servo10Raw=0, servo11Raw=0, servo12Raw=0, servo13Raw=0, servo14Raw=0, servo15Raw=0, servo16Raw=0}}
2022-03-21 12:37:08.854 9080-9751/com.example.mavlinkkotlin D/Vehicle: MavlinkMessage{packet=MavlinkPacket{versionMarker=253, incompatibleFlags=0, compatibleFlags=0, sequence=192, systemId=1, componentId=1, messageId=65, payload=[-80, -59, 0, 0, -36, 5, -36, 5, -24, 3, -36, 5, 8, 7, -24, 3, -24, 3, 8, 7, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 16, -1], checksum=12121, signature=[]}, payload=RcChannels{timeBootMs=50608, chancount=16, chan1Raw=1500, chan2Raw=1500, chan3Raw=1000, chan4Raw=1500, chan5Raw=1800, chan6Raw=1000, chan7Raw=1000, chan8Raw=1800, chan9Raw=0, chan10Raw=0, chan11Raw=0, chan12Raw=0, chan13Raw=0, chan14Raw=0, chan15Raw=0, chan16Raw=0, chan17Raw=0, chan18Raw=0, rssi=255}}
2022-03-21 12:37:08.859 9080-9751/com.example.mavlinkkotlin D/Vehicle: MavlinkMessage{packet=MavlinkPacket{versionMarker=253, incompatibleFlags=0, compatibleFlags=0, sequence=193, systemId=1, componentId=1, messageId=27, payload=[19, 59, 4, 3, 0, 0, 0, 0, 0, 0, 0, 0, 23, -4, 0, 0, 0, 0, 0, 0, -32, 0, 80, 0, -16, -3, 0, -16, 10], checksum=20151, signature=[]}, payload=RawImu{timeUsec=50608915, xacc=0, yacc=0, zacc=-1001, xgyro=0, ygyro=0, zgyro=0, xmag=224, ymag=80, zmag=-528, id=0, temperature=2800}}
2022-03-21 12:37:08.864 9080-9751/com.example.mavlinkkotlin D/Vehicle: MavlinkMessage{packet=MavlinkPacket{versionMarker=253, incompatibleFlags=0, compatibleFlags=0, sequence=194, systemId=1, componentId=1, messageId=116, payload=[-80, -59, 0, 0, 0, 0, 0, 0, 23, -4, 0, 0, 0, 0, 0, 0, -32, 0, 80, 0, -16, -3, -16, 10], checksum=5801, signature=[]}, payload=ScaledImu2{timeBootMs=50608, xacc=0, yacc=0, zacc=-1001, xgyro=0, ygyro=0, zgyro=0, xmag=224, ymag=80, zmag=-528, temperature=2800}}
2022-03-21 12:37:08.868 9080-9751/com.example.mavlinkkotlin D/Vehicle: MavlinkMessage{packet=MavlinkPacket{versionMarker=253, incompatibleFlags=0, compatibleFlags=0, sequence=195, systemId=1, componentId=1, messageId=129, payload=[-80, -59, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -32, 0, 80, 0, -16, -3], checksum=10831, signature=[]}, payload=ScaledImu3{timeBootMs=50608, xacc=0, yacc=0, zacc=0, xgyro=0, ygyro=0, zgyro=0, xmag=224, ymag=80, zmag=-528, temperature=0}}
....
....
....

[benbarkay]

This type of issues usually end up happening due to the local setup of the SITL, mavproxy, or the UDP forwarding code, and don't have much to do with the library itself.

I've left it open just in case someone else could help you or if you've progressed to find an actual issue with the library.

Have you managed to resolve it?

[RishabhBhat2021]

I came up with one solution, whose idea I got by looking at the QGC ‘s source code In QGC, they were listening to the 14550 port for any connection. And when any vehicle (SITL) starts sending messages, they dynamically get the local port of the UDP Packets they are receiving and assign the remote port for the packet to use that port

Similarly, I was able to resolve it by assigning the remotePort while we recieve the UDP Packets from the SITL

private var remotePort = 14556 // remotePort is 14556 by default
....
....
val service: ExecutorService = Executors.newSingleThreadExecutor()
service.execute {
            try {
                val packet =
                    DatagramPacket(ByteArray(bufferSize), bufferSize)
                while (!datagramSocket.isClosed) {
                    datagramSocket.receive(packet)
                    appOut.write(packet.data, packet.offset, packet.length)

            // Check for the port of the incoming packet
                    if (packet.port != remotePort) {
                // Change the remoteAddress to use the local port of the sitl
                        remoteAddress = InetSocketAddress(ipAddress, packet.port)
                        remotePort = packet.port
                        Timber.d("Connected to: ${packet.address}, remote port: ${packet.port}")
                    }
                    appOut.flush()
                }
            } catch (e: IOException) {
                e.printStackTrace()
            } finally { ... } 
}
....
....

I’m not sure whether this is the best approach, but the code works and I was able to successfully send the messages to the SITL

[benbarkay]

@RishabhBhat2021 Happy to hear you've got it working.

Thank you for posting this, others who experience the same problem may find this information useful :)