comctrl.c

#include <arpa/inet.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/socket.h>
#include <unistd.h>
#include <pthread.h>

struct gimbal_loop_args {
	int* gimb_udp_sock;
	int* grnd_tx_sock;
	struct sockaddr_in* grnd_tx_address;
};

struct ground_loop_args {
	int* gimb_udp_sock;
	int* grnd_tx_sock;
	int* grnd_rx_sock;
	struct sockaddr_in* gimbal_address;
	struct sockaddr_in* grnd_tx_address;
};

int gimbal_command(const char* buf, int gimb_udp_sock, struct sockaddr_in* gimbal_address) {
	// print the command
	int n;
	printf("Gimbal command: %s\n", buf);

	// send the command to the gimbal
	n = sendto(gimb_udp_sock, buf, strlen(buf), 0, (struct sockaddr *)gimbal_address, sizeof(struct sockaddr_in));
	return 0;
}

int run_command(char* cmd) {
	system(cmd);
}

int control_ir(char* buf) {
	if (strncmp(buf, "K", 1) == 0) {
		system("killall rtp_ir");
	};

	if (strncmp(buf, "S", 1) == 0) {
		system("nohup rtp_ir &");
	};
}

int control_vl(char* buf) {
		if (strncmp(buf, "K", 1) == 0) {
		system("killall rtp_vl");
	};

	if (strncmp(buf, "S", 1) == 0) {
		system("nohup rtp_vl &");
	};
}

void* gimb_rx_loop(void* arg) {
	struct gimbal_loop_args* args = (struct gimbal_loop_args*)arg;
	int* gimb_udp_sock = args->gimb_udp_sock;
	int* grnd_tx_sock = args->grnd_tx_sock;
	struct sockaddr_in* grnd_tx_address = args->grnd_tx_address;
	int n;
	char buf[256];
	for (;;) {
		memset(buf, '\0', sizeof(buf));
		n = recv(*(gimb_udp_sock), buf, sizeof(buf) - 1, 0);
		if (n <= 0) {
			printf("Error: Server did not respond properly");
		}

		n = sendto(*(grnd_tx_sock), buf, strlen(buf), 0, (struct sockaddr*)grnd_tx_address, sizeof(struct sockaddr_in));
		if (n < 0) {
			printf("could not send data\n");
		}
	}
}

void* grnd_rx_loop(void* arg) {
	struct ground_loop_args* args = (struct ground_loop_args*)arg;
	int* gimb_udp_sock = args->gimb_udp_sock;
	int* grnd_tx_sock = args->grnd_tx_sock;
	int* grnd_rx_sock = args->grnd_rx_sock;
	struct sockaddr_in* gimb_udp_address = args->gimbal_address;
	int n;
	char buf[256];
	for (;;) {
		memset(buf, '\0', sizeof(buf));
		n = recv(*(grnd_rx_sock), buf, sizeof(buf) - 1, 0);
		if (n <= 0) {
			printf("Error: Server did not respond properly");
		}

		// if buf starts with "GIM" send gimbal command
		if (strncmp(buf, "GIM", 3) == 0) {
			gimbal_command((char *)&buf+3, *gimb_udp_sock, gimb_udp_address);
		}

		// dont work so good
		if (strncmp(buf, "CMD", 3) == 0) {
			run_command((char *)&buf+3);
		}

		if (strncmp(buf, "CIR", 3) == 0) {
			control_ir((char *)&buf+3);
		}

		if (strncmp(buf, "CVL", 3) == 0) {
			control_ir((char *)&buf+3);
		}
	}
}

int main(int argc, char* const *argv) {
	int r;

	// gimbal socket port 5000 ip 127.0.0.1
	int gimb_udp_sock;
	struct sockaddr_in gimb_udp_address;
	if ((gimb_udp_sock = socket(PF_INET, SOCK_DGRAM, 0)) < 0) {
		printf("could not create gimbal socket\n");
		return 1;
	}
	memset(&gimb_udp_address, 0, sizeof(gimb_udp_address));
	gimb_udp_address.sin_family = AF_INET;
	inet_pton(AF_INET, "192.168.144.108", &gimb_udp_address.sin_addr);
	int por = 5000;
	gimb_udp_address.sin_port = htons(por);

	// ground listen socket port 5100 ip
	int grnd_rx_socket;
	struct sockaddr_in grnd_rx_address;
	if ((grnd_rx_socket = socket(PF_INET, SOCK_DGRAM, 0)) < 0) {
		printf("could not create ground rx socket\n");
		return 1;
	}
	memset(&grnd_rx_address, 0, sizeof(grnd_rx_address));
	grnd_rx_address.sin_family = AF_INET;
	grnd_rx_address.sin_addr.s_addr = htonl(INADDR_ANY);
	grnd_rx_address.sin_port = htons(5100);
	if ((bind(grnd_rx_socket, (struct sockaddr *)&grnd_rx_address,
	          sizeof(grnd_rx_address))) < 0) {
		printf("could not bind socket\n");
		return 1;
	}

	// ground response socket port 5101 ip
	int grnd_tx_socket;
	struct sockaddr_in grnd_tx_address;
	if ((grnd_tx_socket = socket(PF_INET, SOCK_DGRAM, 0)) < 0) {
		printf("could not create ground tx socket\n");
		return 1;
	}
	memset(&grnd_tx_address, 0, sizeof(grnd_tx_address));
	grnd_tx_address.sin_family = AF_INET;
	grnd_tx_address.sin_addr.s_addr = htonl(INADDR_LOOPBACK);
	grnd_tx_address.sin_port = htons(5101);


	// create thread for gimb_rx_loop
	pthread_t gimb_thread;
	struct gimbal_loop_args gimb_args;
	gimb_args.gimb_udp_sock = &gimb_udp_sock;
	gimb_args.grnd_tx_sock = &grnd_tx_socket;
	gimb_args.grnd_tx_address = &grnd_tx_address;
	r = pthread_create(&gimb_thread, NULL, gimb_rx_loop, &gimb_args);
	if (r != 0) {
		printf("Failed to create thread\n");
		return 1;
	}

	// create thread for grnd_rx_loop
	pthread_t grnd_thread;
	struct ground_loop_args grnd_args;
	grnd_args.gimb_udp_sock = &gimb_udp_sock;
	grnd_args.grnd_tx_sock = &grnd_tx_socket;
	grnd_args.grnd_rx_sock = &grnd_rx_socket;
	grnd_args.grnd_tx_address = &grnd_tx_address;
	grnd_args.gimbal_address = &gimb_udp_address;
	r = pthread_create(&grnd_thread, NULL, grnd_rx_loop, &grnd_args);
	if (r != 0) {
		printf("Failed to create thread\n");
		return 1;
	}

	// join threads
	pthread_join(gimb_thread, NULL);
	pthread_join(grnd_thread, NULL);

	close(gimb_udp_sock);
	close(grnd_tx_socket);
	close(grnd_rx_socket);
	return 0;
}