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