I’m not entirely sure where to post this as it touches on a few subjects.
I am writing an application to communicate with an ArduPilot instance and I have been testing it with the SIL setup and MAVProxy.
My external application is written in C# and is communicating to MAVProxy via UDP.
The ArduPilot simulation is being run like:
sim_vehicle.py -v ArduPlane --out=udp:127.0.0.1:14580
I put together a very simple program to just emit a heartbeat packet over udp just to make sure I can get something sent to MAVProxy:
using System;
using MavLink;
using System.Threading;
using System.Net;
using System.Net.Sockets;
namespace HBTest
{
class MainClass
{
public static void Main (string[] args)
{
Console.WriteLine ("Testing Heartbeat Sending ___________________ ");
Msg_heartbeat hbMsg = new Msg_heartbeat ();
Mavlink mav = new Mavlink ();
// Udp Com ________________________________________________________
IPEndPoint epOut = new IPEndPoint(IPAddress.Parse ("127.0.0.1"), 14580);
IPEndPoint epThis = new IPEndPoint (IPAddress.Parse ("127.0.0.1"), 14660);
UdpClient udpCom = new UdpClient (epThis);
// Heartbeat message
hbMsg.autopilot = (byte)MavLink.MAV_AUTOPILOT.MAV_AUTOPILOT_GENERIC;
hbMsg.base_mode = (byte)MavLink.MAV_MODE.MAV_MODE_PREFLIGHT;
hbMsg.custom_mode = 0;
hbMsg.mavlink_version = 3;
hbMsg.system_status = (byte)MavLink.MAV_STATE.MAV_STATE_STANDBY;
hbMsg.type = (byte)MavLink.MAV_TYPE.MAV_TYPE_GCS;
// Heartbeat packet
MavlinkPacket hbPkt = new MavlinkPacket ();
hbPkt.SystemId = 250;
hbPkt.ComponentId = 0;
hbPkt.SequenceNumber = 0;
hbPkt.TimeStamp = System.DateTime.Now;
int hbSent = 0;
hbPkt.Message = hbMsg;
byte[] pktbytes;
while(true)
{
hbPkt.SequenceNumber = (byte)hbSent;
pktbytes = mav.Send (hbPkt);
Console.WriteLine ("[{0}] AP:{1} BM:{2} T:{3} S:{4}", hbPkt.SequenceNumber, hbMsg.autopilot,
hbMsg.base_mode, hbMsg.type, hbMsg.system_status);
udpCom.Send (pktbytes, pktbytes.Length, epOut);
Thread.Sleep (500);
hbSent++;
}
}
}
}
When I run watch HEARTBEAT from the MAVProxy shell, I only see the simulated vehicle and MAVProxy’s heartbeat:
I looked at what MAVProxy uses for the fields in its heartbeat and it matches the Heartbeat with type 6 (MAV_TYPE_GCS). So mine is not showing up.
I thought maybe I’m missing something with how I am communicating with UDP but when I look at wireshark for the UPD Port that I had set for output but I can see that there is the heartbeat packet being sent from my application’s source UDP address (127.0.0.1:14660) to the output UDP (127.0.0.1:14580) that I had defined:
You can see the packet being sent from 14460 to 14580. So that leads me to believe that the packet is getting there.
I looked at the packets that are in that datagram and can confirm that they are the correct bytes:
- 0xFE Start of Mavlink Packet
- 0x09 Payload Length
- 0x81 Payload Sequence
- 0xFA System ID
- 0x00 Component ID
- 0x00 Message ID (0 = HEARTBEAT)
- 0x00 HeartBeat Custom Mode byte 0
- 0x00 HeartBeat Custom Mode byte 1
- 0x00 HeartBeat Custom Mode byte 2
- 0x00 HeartBeat Custom Mode byte 3
- 0x06 HeartBeat Type
- 0x00 HeartBeat Autopilot
- 0x00 Heartbeat Base Mode
- 0x03 Heartbeat System Status
- 0x03 Heartbeat Mavlink Version
- 0xC8 CRC
- 0x81 CRC
Everything seems to be packed correctly.
When I use APM Planner I can see the heartbeats it emits with the “watch HEARTBEAT” command in MAVProxy. So it seems that I am not initializing communication with MAVProxy properly?
I can receive messages in my application from the output of MAVProxy just fine, but sending messages the other way is giving me problems. I did not see any documentation for initializing a connection or something similar. I thought it was just that you make sure you send enough heartbeats.
Thanks for any suggestions