librobotcontrol-sys 0.4.0

/**
 * @file rc_test_mavlink
 * @example rc_test_mavlink
 *
 * @brief      Basic Mavlink UDP heartbeat tester.
 *
 *             Sends a heartbeat packets every second and prints to the screen
 *             when one has been received along with the system identifier of
 *             the device that sent the received heartbeat. Optionally specify
 *             the destination IP address to send to with the -a option,
 *             otherwise messages will be sent to 127.0.0.1 (localhost).
 *             Optionally specify the UDP port which will be used for listening
 *             and sending with the -p option, otherwise RC_MAV_DEFAULT_UDP_PORT
 *             (14551) will be used. Optionally specify the system ID which will
 *             be specified in each sent heartbeat packet to be read by the
 *             listener, otherwise a system ID of 1 will be used.
 *
 * @author     James Strawson & Henry Gaudet
 *
 * @date       1/24/2018
 */

#include <ctype.h> // for isprint()
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <signal.h> // to SIGINT signal handler
#include <rc/mavlink_udp.h>

#define LOCALHOST_IP	"127.0.0.1"
#define DEFAULT_SYS_ID	1

static const char* dest_ip;
static uint8_t my_sys_id;
static uint16_t port;
static int running = 0;


static void __print_usage(void)
{
	fprintf(stderr,"usage: rc_test_mavlink [-h] [-a dest_ip_addr] [-p port] [-s sys_id]\n");
	return;
}

static int __parse_args(int argc, char * argv[])
{
	int c,tmp;
	opterr = 0;

	while ((c = getopt(argc, argv, "ha:p:s:")) != -1)
		switch (c)
		{
		case 'h':
			__print_usage();
			exit(0);
			break;
		case 'a':
			dest_ip = optarg;
			break;
		case 'p':
			tmp = atoi(optarg);
			if(tmp<0){
				fprintf(stderr, "UDP port must be greater than 0\n");
				exit(-1);
			}
			if(tmp>UINT16_MAX){
				fprintf(stderr, "UDP port must be less than %d\n", UINT16_MAX);
				exit(-1);
			}
			if(tmp<1024){
				fprintf(stderr, "WARNING: ports less than 1024 are privaledged ports\n");
			}
			port=tmp;
			break;
		case 's':
			tmp=atoi(optarg);
			if(tmp>UINT8_MAX||tmp<0){
				fprintf(stderr, "sys_id must be between 0 and %d\n", UINT8_MAX);
				exit(-1);
			}
			my_sys_id = tmp;
			break;
		case '?':
			if (optopt == 'a' || optopt=='p' || optopt=='s')
				fprintf (stderr, "Option -%c requires an argument.\n", optopt);
			else if (isprint (optopt))
				fprintf (stderr, "Unknown option `-%c'.\n", optopt);
			else
				fprintf (stderr,"Unknown option `\\x%x'.\n",optopt);
			__print_usage();
			exit(-1);
		default:
			__print_usage();
			exit(-1);
		}
	return 0;
}

// called by the rc_mav lib whenever a packet is received
static void __callback_func_any(void)
{
	int sysid = rc_mav_get_sys_id_of_last_msg_any();
	int msg_id = rc_mav_msg_id_of_last_msg();
	printf("received msg_id: %d ", msg_id);
	// TODO uncomment print msg name when this works
	//rc_mav_print_msg_name(msg_id);
	printf(" from sysid: %d \n", sysid);
	return;
}

static void __callback_func_connection_lost(void)
{
	fprintf(stderr,"CONNECTION LOST\n");
	return;
}


// interrupt handler to catch ctrl-c
static void __signal_handler(__attribute__ ((unused)) int dummy)
{
	running=0;
	return;
}

int main(int argc, char * argv[])
{
	// set default options before checking options
	dest_ip=LOCALHOST_IP;
	my_sys_id=DEFAULT_SYS_ID;
	port=RC_MAV_DEFAULT_UDP_PORT;

	// parse arguments
	if(__parse_args(argc,argv)){
		fprintf(stderr,"failed to parse arguments\n");
		return -1;
	}

	printf("run with -h option to see usage and other options\n");
	// inform the user what settings are being used
	printf("\n");
	printf("Initializing with the following settings:\n");
	printf("dest ip addr: %s\n", dest_ip);
	printf("my system id: %d\n", my_sys_id);
	printf("UDP port: %d\n", port);
	printf("\n");

	// set signal handler so the loop can exit cleanly
	signal(SIGINT, __signal_handler);

	// initialize the UDP port and listening thread with the rc_mav lib
	if(rc_mav_init(my_sys_id, dest_ip, port,RC_MAV_DEFAULT_CONNECTION_TIMEOUT_US)<0){
		return -1;
	}

	// set the heartbeat callback to print something when receiving
	rc_mav_set_callback_all(__callback_func_any);
	rc_mav_set_callback_connection_lost(__callback_func_connection_lost);
	running=1;
	while(running){
		sleep(1);
		if(rc_mav_send_heartbeat_abbreviated()){
			fprintf(stderr,"failed to send heartbeat\n");
		}
		else{
			printf("sent heartbeat\n");
		}
	}

	// stop listening thread and close UDP port
	printf("closing UDP port\n");
	rc_mav_cleanup();
}