Connecting to ArduCopter through UDP

@josealb wrote:

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));
	}
};

Posts: 9

Participants: 2

Read full topic