cli_setpid.c

#include <stdio.h>
#include <unistd.h>
#include <getopt.h>

#include "cli_setpid.h"
#include "frontend_setpid.h"

int cli_setpid(struct backend_conn * conn,	int argc, char **argv) {
	int c;
	static int setRoll = 0, setPitch = 0, setYaw = 0, setAll = 0;
	static int setRollV = 0, setPitchV = 0, setYawV = 0;
	static int setHeight = 0, setLat = 0, setLong = 0;
	struct frontend_pid_data pid_data;
	static int mask;
	static float pval = 0, ival = 0, dval = 0;
	static int needHelp = 0;
	static struct option long_options[] = {
 		/* These options don’t set a flag. We distinguish them by their indices. */
 		{"roll",	no_argument,	&setRoll,	1},
 		{"pitch",   no_argument,   	&setPitch, 	1},
 		{"yaw",   no_argument,   	&setYaw, 	1},
 		{"rollv",   no_argument,   	&setRollV, 	1},
 		{"pitchv",   no_argument,   &setPitchV, 	1},
 		{"yawv",   no_argument,   	&setYawV, 	1},
 		{"height",   no_argument,   &setHeight,	1},
 		{"lat",   no_argument,   	&setLat, 	1},
 		{"long",   no_argument,   	&setLong, 	1},
 		{"help",	no_argument,	&needHelp,	1},
 		{0, 0, 0, 0}
 	};

 	while (1)
	{
		/* getopt_long stores the option index here. */
		int option_index = 0;

		c = getopt_long(argc, argv, "p:i:d:", long_options, &option_index);

		/* Detect the end of the options. */
		if (c == -1)
			break;

		switch(c) {
			case 'p' :
				pid_data.p  = atof(optarg);
				mask |= SET_P;
				break;
			case 'i' :
				pid_data.i  = atof(optarg);
				mask |= SET_I;
				break;
			case 'd' :
				pid_data.d  = atof(optarg);
				mask |= SET_D;
				break;
			default :
				break;
		}
	}

	if (needHelp) {
		printf("Setpid sets the p, i , or d constant values of any single controller\n");
		printf("Usage Syntax : \n\t./Cli setpid controller [options...]\n");
		printf("Symlink Usage Syntax : \n\t./setpid controller [options...]\n\n");
		printf("Available 'controllers' include the following\n");
		printf("\t[--pitch] : Pitch\n\t[--roll] : Roll\n\t[--yaw] : Yaw\n");
		printf("\t[--pitchv] : Pitch Velocity\n\t[--rollv] : Roll Velocity\n\t[--yawv] : Yaw Velocity\n");
		printf("\t[--height] : Z\n\t[--lat] : X\n\t[--long] : Y\n\n");
		printf("Available 'controller' options include the following\n");
		printf("\t[-p] 'val' : Sets the p constant of the 'controller' to 'val'\n");
		printf("\t[-i] 'val' : Sets the i constant of the 'controller' to 'val'\n");
		printf("\t[-d] 'val' : Sets the d constant of the 'controller' to 'val'\n");
		return 0;
	}

	if (setRoll) {
		pid_data.controller = PID_ROLL;
	} else if (setYaw) {
		pid_data.controller = PID_YAW;
	} else if (setPitch) {
		pid_data.controller = PID_PITCH;
	} else if (setRollV) {
		pid_data.controller = PID_ROLL_RATE;
	} else if (setPitchV) {
		pid_data.controller = PID_PITCH_RATE;
	} else if (setYawV) {
		pid_data.controller = PID_YAW_RATE;
	} else if (setHeight) {
		pid_data.controller = PID_HEIGHT;
	} else if (setLong) {
		pid_data.controller = PID_LAT;
	} else if (setLat) {
		pid_data.controller = PID_LONG;
	}

	frontend_setpid(conn, &pid_data, mask);
	return 0;
}