Connecting to ArduCopter through UDP

Hardware Companion Computers
1

Hello,

I am trying to make a companion computer, not really a ground station but I didn’t find any more appropriate sub.
The first step for my project is to set the mode to guided and all controls go through UDP.

Now, I am trying to create my first example but it can’t get it to work.

I am using the ArduCopter SITL on a virtual Ubuntu and my code runs on a Raspberry Pi. I add the output through mavproxy and run my program. I receive packets in my program, but when I send mine to change flight mode, nothing happens.
I have mission planner connected in parallel, and when I change modes there, it does work. I copied my code here in case you can take a look.

What am I doing wrong? Any help is appreciated

class DroneComms
{
public: 
	void sendTestPackage()
	{
		char target_ip[100];
		strcpy(target_ip, "192.168.1.113");
		int sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
		int newsock;
		struct sockaddr_in gcAddr; 
		struct sockaddr_in locAddr;
		uint8_t buf[BUFFER_LENGTH];
		socklen_t fromlen;
		ssize_t recsize;
			
		memset(&locAddr, 0, sizeof(locAddr));
		locAddr.sin_family = AF_INET;
		locAddr.sin_addr.s_addr = INADDR_ANY;
		locAddr.sin_port = htons(14550);
 
		/* Bind the socket to port 14550 */ 
		if (-1 == bind(sock, (struct sockaddr *)&locAddr, sizeof(struct sockaddr)))
		{
			perror("error bind failed");
			close(sock);
			exit(EXIT_FAILURE);
		} 

		/*Listen for message from MAV*/
		memset(buf, 0, BUFFER_LENGTH);
		recsize = recvfrom(sock, (void *)buf, BUFFER_LENGTH, 0, (struct sockaddr *)&gcAddr, &fromlen);
		
		/*Use port as sending port*/
		int remotePort = gcAddr.sin_port;
		char *droneIp = inet_ntoa(gcAddr.sin_addr);
		
		memset(&gcAddr, 0, sizeof(gcAddr));
		gcAddr.sin_family = AF_INET;
		gcAddr.sin_addr.s_addr = inet_addr(droneIp);
		gcAddr.sin_port = remotePort;
		
		/*Creating message*/
 
 		int system_id = 0xff; // Copying the Mission Planner IDs here. Is this correct?
		int companion_id = 0xbe;
		
		mavlink_command_long_t sc;
		mavlink_message_t message;
		memset(&message, 0, sizeof(message));

		mavlink_set_mode_t sm;
		sm.target_system = 0;
		sm.custom_mode = 0;
		sm.base_mode = MAV_MODE_STABILIZE_ARMED;
		
		/*Encoding message*/
		
		mavlink_msg_set_mode_encode(system_id, companion_id, &message, &sm);
		
		uint16_t len;
		int bytes_sent;
		len = mavlink_msg_to_send_buffer(buf, &message);
		
		/*Sending message*/
		
		bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
	}
};
1 Like
2

@billb I am guessing you can be of help here :slight_smile:

3

It’s hard to tell, but you should get a response from the vehicle accepted or rejected your command. look out for that as it maybe rejected the command i.e. it revcieved it OK.

The other thing is that modes are set using custom_mode flags. The base mode is ignored for set AFAIR. look at APM Planner 2.0 code for example.s see uas.cc.h and ArduPilotMegaMav.cc/h

Set component_id to 1 for simplicity.

1 Like
4

It seems to be working now! I have spent days with this and it works minutes after posting :stuck_out_tongue_closed_eyes:

I had to change these parameters:
sm.target_system = 1;
sm.custom_mode = 3;
sm.base_mode = 1;

I got these values by sniffing mission planner packets

The weird thing is I thought I would use sm.base_mode = MAV_MODE_STABILIZE_ARMED;

Why these params? (They change mode to auto)

5

you will need Guided Mode if you are controlling from a companion

ARM/DISARM

see https://github.com/ArduPilot/apm_planner/blob/master/src/uas/ArduPilotMegaMAV.cc#L316

1 Like
6

Thank you Bill. This is not the common Mavlink implementation though, do you know if there is any complete documentation for the ardupilot use of mavlink protocol?
I have read through tons of source code already hehe
Thanks for these source files though, I will check them out.

7

I know I know, I was just playing around copying the mission planner, I thought I had a fundamental problem with my code

8

Final example code for completion. This changes the mode to auto

class DroneComms
{
public: 
	void sendTestPackage()
	{
		int sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
		int newsock;
		struct sockaddr_in gcAddr; 
		struct sockaddr_in locAddr;
		uint8_t buf[BUFFER_LENGTH];
		socklen_t fromlen;
		ssize_t recsize;
			
		memset(&locAddr, 0, sizeof(locAddr));
		locAddr.sin_family = AF_INET;
		locAddr.sin_addr.s_addr = INADDR_ANY;
		locAddr.sin_port = htons(14550);
 
		/* Bind the socket to port 14550 */ 
		if (-1 == bind(sock, (struct sockaddr *)&locAddr, sizeof(struct sockaddr)))
		{
			perror("error bind failed");
			close(sock);
			exit(EXIT_FAILURE);
		} 

		/*Listen for message from MAV*/
		memset(buf, 0, BUFFER_LENGTH);
		recsize = recvfrom(sock, (void *)buf, BUFFER_LENGTH, 0, (struct sockaddr *)&gcAddr, &fromlen);
		
		/*Use port as sending port*/
		int remotePort = gcAddr.sin_port;
		char *droneIp = inet_ntoa(gcAddr.sin_addr);
		
		memset(&gcAddr, 0, sizeof(gcAddr));
		gcAddr.sin_family = AF_INET;
		gcAddr.sin_addr.s_addr = inet_addr(droneIp);
		gcAddr.sin_port = remotePort;
		
		/*Creating message*/
 
 		int system_id = 0xff; // Copying the Mission Planner IDs here. Is this correct?
		int companion_id = 0xbe;
		
		mavlink_command_long_t sc;
		mavlink_message_t message;
		memset(&message, 0, sizeof(message));

		mavlink_set_mode_t sm;
		sm.target_system = 1;
		sm.custom_mode = 3;
		sm.base_mode = 1;
		
		/*Encoding message*/
		
		mavlink_msg_set_mode_encode(system_id, companion_id, &message, &sm);
		
		uint16_t len;
		int bytes_sent;
		len = mavlink_msg_to_send_buffer(buf, &message);
		
		/*Sending message*/
		
		bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
	}
};
1 Like
9

Without a conformance test suite and detailed protocol specification it was always open to some interpretation. In this case for the mode. the ‘base’ modes just don’t fit ArduPilot modes so you can only set them via the custom_m0de flag. It does however report the closest base mode, so MAVLink generic ‘compatible’ software will display its best guess at the mode.

There are two things that ArduPilot differ (mainly of which messages are implemented ) from PX4 and that is modes. And the other is parameters. In the spec they are a union type, but ArduPilot adopted a float only params for size issues. This can be be easily managed, someday i’m sure ArduPilot will be changed.

The best places to look are http://mavlink.org or ardupilot firmware code base, one on the OpenSource GCSs. APM Planner/QGroundControl/MissionPlanner etc… Also DroneKit-Python has some useful docs on set_postiion etc… MAVLink messages that ArduCopter understand (it’s python code, but the API is the same and behaves the same)

1 Like