diff --git a/common/commands.c b/common/commands.c index ef4c67e3fd5c42842d8c431b9ca028640f617935..9111f1050af02e5e26800623266e7e83aaefa9dd 100644 --- a/common/commands.c +++ b/common/commands.c @@ -72,6 +72,9 @@ command_cb cb_respoutput __attribute__((weak, alias("cb_default"))); command_cb cb_getnodes __attribute__((weak, alias("cb_default"))); command_cb cb_respnodes __attribute__((weak, alias("cb_default"))); +command_cb cb_addnode __attribute__((weak, alias("cb_default"))); +command_cb cb_respaddnode __attribute__((weak, alias("cb_default"))); + /* * Command structure. diff --git a/groundStation/Makefile b/groundStation/Makefile index 38c2e2d741e931412f2924fc9cc52f392fe10b23..7a62e3752447f10c21bf81bd92b3389b491bbee2 100644 --- a/groundStation/Makefile +++ b/groundStation/Makefile @@ -28,7 +28,7 @@ CLIBINARY=Cli CLISRCDIR=src/cli CLISOURCES := $(wildcard $(CLISRCDIR)/*.c) CLIOBJECTS = $(CLISOURCES:$(CLISRCDIR)/%.c=$(OBJDIR)/%.o) -SYMLINKS=monitor setpid getpid setsetpoint getsetpoint +SYMLINKS= getparam setparam getsource setsource getoutput # Frontend-common stuff FESRCDIR=src/frontend diff --git a/groundStation/getoutput b/groundStation/getoutput new file mode 120000 index 0000000000000000000000000000000000000000..83c63a5d9a0f1ab92f86859877d2d168eca32fc0 --- /dev/null +++ b/groundStation/getoutput @@ -0,0 +1 @@ +Cli \ No newline at end of file diff --git a/groundStation/getparam b/groundStation/getparam new file mode 120000 index 0000000000000000000000000000000000000000..83c63a5d9a0f1ab92f86859877d2d168eca32fc0 --- /dev/null +++ b/groundStation/getparam @@ -0,0 +1 @@ +Cli \ No newline at end of file diff --git a/groundStation/getsource b/groundStation/getsource new file mode 120000 index 0000000000000000000000000000000000000000..83c63a5d9a0f1ab92f86859877d2d168eca32fc0 --- /dev/null +++ b/groundStation/getsource @@ -0,0 +1 @@ +Cli \ No newline at end of file diff --git a/groundStation/setparam b/groundStation/setparam new file mode 120000 index 0000000000000000000000000000000000000000..83c63a5d9a0f1ab92f86859877d2d168eca32fc0 --- /dev/null +++ b/groundStation/setparam @@ -0,0 +1 @@ +Cli \ No newline at end of file diff --git a/groundStation/setsource b/groundStation/setsource new file mode 120000 index 0000000000000000000000000000000000000000..83c63a5d9a0f1ab92f86859877d2d168eca32fc0 --- /dev/null +++ b/groundStation/setsource @@ -0,0 +1 @@ +Cli \ No newline at end of file diff --git a/groundStation/src/backend/backend.c b/groundStation/src/backend/backend.c index cdd0727a269d0d857fc13be51f6a2e04696c21ae..5b8535dccf26b076e477bbccc640e0ffe3d2949e 100644 --- a/groundStation/src/backend/backend.c +++ b/groundStation/src/backend/backend.c @@ -117,9 +117,6 @@ pthread_mutex_t quadResponseMutex, cliInputMutex ; unsigned char *commandBuf; int newQuadResponse = 0, newCliInput = 0; -// Structures to be used throughout -modular_structs_t structs; - // Callback to be ran whenever the tracker receives data. // Currently doing much more than it should. It will be slimmed down // in the future. @@ -233,7 +230,6 @@ int main(int argc, char **argv) // Tell the quad we are ready to send it vrpn data sendStartPacket(); - if(!getenv(NOVRPN_ENV)){ // create vrpnTracker instance tracker = ucart_vrpn_tracker_createInstance(TRACKER_IP); @@ -629,15 +625,18 @@ static void client_recv(int fd) { char * first_word; char * tmp = strdup(buffer); + // printf("tmpbuff = '%s'\n", tmp); first_word = strtok(tmp, " "); - free(tmp); - + // printf("first word = '%s'\n", first_word); + ssize_t msg_type, i; for (i = 0; i < MAX_TYPE_ID; ++i) { if ((msg_type = findCommand(first_word)) != -1) break; } + free(tmp); + if (msg_type == -1) { /* buffer was not a quad command, handling internally to * backend instead of forwarding to quad @@ -679,6 +678,8 @@ static void client_recv(int fd) { ssize_t result; ssize_t psize; + printf(" found a msg_type of %ld\n", msg_type); + switch (msg_type) { case SETPARAM_ID: result = EncodeSetParam(&m, data, 128, buffer); @@ -723,11 +724,11 @@ static void client_recv(int fd) { } } - // printf("packetToQuad = '"); - // for(int i = 0; i < (int)psize; ++i) { - // printf(" %.2x ", packet[i]); - // } - // printf("'\n"); + printf("packetToQuad = '"); + for(int i = 0; i < (int)psize; ++i) { + printf(" %.2x ", packet[i]); + } + printf("'\n"); writeQuad(packet, psize); diff --git a/groundStation/src/backend/source.c b/groundStation/src/backend/source.c index dac52612d810c500644c8a5286595e090046d631..a0e5ed9c3bdb024dc5ed32749af886937d82f0e1 100644 --- a/groundStation/src/backend/source.c +++ b/groundStation/src/backend/source.c @@ -88,10 +88,14 @@ ssize_t EncodeSetSource( enum ResponseData { - RESP_DST_BLOCK_ID, - RESP_DST_INPUT_ID, - RESP_SRC_BLOCK_ID, - RESP_SRC_OUTPUT_ID, + RESP_DST_BLOCK_ID_L, + RESP_DST_BLOCK_ID_H, + RESP_DST_INPUT_ID_L, + RESP_DST_INPUT_ID_H, + RESP_SRC_BLOCK_ID_L, + RESP_SRC_BLOCK_ID_H, + RESP_SRC_OUTPUT_ID_L, + RESP_SRC_OUTPUT_ID_H, RESP_DATA_SIZE }; @@ -110,5 +114,9 @@ int DecodeResponseSource( return -1; } - return sprintf(msg, "getsource %d %d\n", data[RESP_SRC_BLOCK_ID], data[RESP_SRC_OUTPUT_ID]); + return sprintf(msg, "getsource %hu %hu %hu %hu\n", + BytesTo16(data[RESP_DST_BLOCK_ID_L], data[RESP_DST_BLOCK_ID_H]), + BytesTo16(data[RESP_DST_INPUT_ID_L], data[RESP_DST_INPUT_ID_H]), + BytesTo16(data[RESP_SRC_BLOCK_ID_L], data[RESP_SRC_BLOCK_ID_H]), + BytesTo16(data[RESP_SRC_OUTPUT_ID_L], data[RESP_SRC_OUTPUT_ID_H])); } diff --git a/groundStation/src/cli/cli.c b/groundStation/src/cli/cli.c index acec4dbc6352ca971b84289eb24cfc918da98bbd..5aa3082909e1856ed281091bae4663b1f6c09949 100644 --- a/groundStation/src/cli/cli.c +++ b/groundStation/src/cli/cli.c @@ -78,7 +78,7 @@ int main(int argc, char **argv) } /** - * I the user has asked for help, and we have already found + * If the user has asked for help, and we have already found * the command that they are trying to use. Then we need * to be able to provide the help info wihtout the * requirement of the backend @@ -109,4 +109,33 @@ int main(int argc, char **argv) ucart_backendDisconnect(conn); } return 0; +} + + +/* This function is called by other cli functions to check for a help condition */ +int help_check(int argc, char ** argv) { + int c; + static int needHelp = 0; + static struct option long_options[] = { + /* These options don’t set a flag. We distinguish them by their indices. */ + {"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, "h", long_options, &option_index); + + /* Detect the end of the options. */ + if (c == -1) + break; + + if (c == 'h') { + needHelp = 1; + } + } + return needHelp; } \ No newline at end of file diff --git a/groundStation/src/cli/cli.h b/groundStation/src/cli/cli.h index 707d16836783fe34279d1b0935995fa535a67fad..edd3ebc853bb113c5ac3f6e55f0dee007424ddb3 100644 --- a/groundStation/src/cli/cli.h +++ b/groundStation/src/cli/cli.h @@ -1,13 +1,14 @@ -#ifndef __CLI_H -#define __CLI_H +#ifndef _CLI_H +#define _CLI_H -#include "cli_monitor.h" +#include "frontend_common.h" #include "cli_source.h" #include "cli_param.h" #include "cli_output.h" +struct backend_conn; + enum CommandNameIds{ - CMD_MONITOR, CMD_GETSOURCE, CMD_SETSOURCE, CMD_GETPARAM, @@ -18,21 +19,21 @@ enum CommandNameIds{ typedef int (*cli_function_ptr)(struct backend_conn *, int, char **); static cli_function_ptr cli_functions[] = { - &cli_monitor, &cli_getsource, &cli_setsource, &cli_getparam, &cli_setparam, - &cli_getoutput, + &cli_getoutput }; static char* commandNames[MAX_COMMANDS] = { - "monitor", "getsource", "setsource", "getparam", "setparam", "getoutput" }; +/* This function is called by other cli functions to check for a help condition */ +int help_check(int argc, char ** argv); -#endif /* __CLI_H */ \ No newline at end of file +#endif /* _CLI_H */ \ No newline at end of file diff --git a/groundStation/src/cli/cli_getpid.c b/groundStation/src/cli/cli_getpid.c deleted file mode 100644 index 4bf634e3e2e966ea59ac54d8510c3607815f462d..0000000000000000000000000000000000000000 --- a/groundStation/src/cli/cli_getpid.c +++ /dev/null @@ -1,176 +0,0 @@ -#include <stdio.h> -#include <unistd.h> -#include <getopt.h> - -#include "cli_getpid.h" - -int cli_getpid(struct backend_conn * conn, int argc, char **argv) { - int c; - static int getRoll = 0, getPitch = 0, getYaw = 0, getAll = 0; - static int getRollV = 0, getPitchV = 0, getYawV = 0; - static int getHeight = 0, getLat = 0, getLong = 0; - static int needHelp = 0; - struct frontend_pid_data pid_data; - static struct option long_options[] = { - /* These options don’t set a flag. We distinguish them by their indices. */ - {"roll", no_argument, &getRoll, 1}, - {"pitch", no_argument, &getPitch, 1}, - {"yaw", no_argument, &getYaw, 1}, - {"rollv", no_argument, &getRollV, 1}, - {"pitchv", no_argument, &getPitchV, 1}, - {"yawv", no_argument, &getYawV, 1}, - {"height", no_argument, &getHeight, 1}, - {"lat", no_argument, &getLat, 1}, - {"long", no_argument, &getLong, 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, "a", long_options, &option_index); - - /* Detect the end of the options. */ - if (c == -1) - break; - - if (c == 'a') { - getAll = 1; - } - } - - if (needHelp) { - printf("Getpid gets the p, i , and d constant values of any single controller\n"); - printf("Usage Syntax : \n\t./Cli getpid controller [options...]\n"); - printf("Symlink Usage Syntax : \n\t./getpid 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"); - return 0; - } - - if (argc < 2) { - printf("Incorrect Usage, run './cli getpid --help' for correct usage.\n"); - return 1; - } - - int result; - if(getAll) { - for(int i = 0; i < PID_NUM_PIDS; ++i) { - pid_data.controller = i; - if ((result = getPidValues(conn, &pid_data))) { - return result; - } - } - } else { - if(getPitch) { - pid_data.controller = PID_PITCH; - if ((result = getPidValues(conn, &pid_data))) { - return result; - } - } - if(getRoll) { - pid_data.controller = PID_ROLL; - if ((result = getPidValues(conn, &pid_data))) { - return result; - } - } - if(getYaw) { - pid_data.controller = PID_YAW; - if ((result = getPidValues(conn, &pid_data))) { - return result; - } - } - if(getPitchV) { - pid_data.controller = PID_PITCH_RATE; - if ((result = getPidValues(conn, &pid_data))) { - return result; - } - } - if(getRollV) { - pid_data.controller = PID_ROLL_RATE; - if ((result = getPidValues(conn, &pid_data))) { - return result; - } - } - if(getYawV) { - pid_data.controller = PID_YAW_RATE; - if ((result = getPidValues(conn, &pid_data))) { - return result; - } - } - if(getHeight) { - pid_data.controller = PID_HEIGHT; - if ((result = getPidValues(conn, &pid_data))) { - return result; - } - } - if(getLat) { - pid_data.controller = PID_LAT; - if ((result = getPidValues(conn, &pid_data))) { - return result; - } - } - if(getLong) { - pid_data.controller = PID_LONG; - if ((result = getPidValues(conn, &pid_data))) { - return result; - } - } - - } - - return 0; -} - -int getPidValues(struct backend_conn * conn, struct frontend_pid_data * pid_data) { - if(frontend_getpid(conn, pid_data)) { - return 1; - } - - switch(pid_data->controller) { - case PID_PITCH : - printf("Pitch Constants: P = %f\tI = %f\tD = %f\n", - pid_data->p, pid_data->i, pid_data->d); - break; - case PID_ROLL : - printf("Roll Constants: P = %f\tI = %f\tD = %f\n", - pid_data->p, pid_data->i, pid_data->d); - break; - case PID_YAW : - printf("Yaw Constants: P = %f\tI = %f\tD = %f\n", - pid_data->p, pid_data->i, pid_data->d); - break; - case PID_PITCH_RATE : - printf("Pitch Rate Constants: P = %f\tI = %f\tD = %f\n", - pid_data->p, pid_data->i, pid_data->d); - break; - case PID_ROLL_RATE : - printf("Roll Rate Constants: P = %f\tI = %f\tD = %f\n", - pid_data->p, pid_data->i, pid_data->d); - break; - case PID_YAW_RATE : - printf("Yaw Rate Constants: P = %f\tI = %f\tD = %f\n", - pid_data->p, pid_data->i, pid_data->d); - break; - case PID_HEIGHT : - printf("Height Constants: P = %f\tI = %f\tD = %f\n", - pid_data->p, pid_data->i, pid_data->d); - break; - case PID_LAT : - printf("Latitude Constants: P = %f\tI = %f\tD = %f\n", - pid_data->p, pid_data->i, pid_data->d); - break; - case PID_LONG : - printf("Longitude Constants: P = %f\tI = %f\tD = %f\n", - pid_data->p, pid_data->i, pid_data->d); - break; - default : - break; - } - return 0; -} diff --git a/groundStation/src/cli/cli_getpid.h b/groundStation/src/cli/cli_getpid.h deleted file mode 100644 index 799681f06d114602083e304f2cba321891155fbc..0000000000000000000000000000000000000000 --- a/groundStation/src/cli/cli_getpid.h +++ /dev/null @@ -1,9 +0,0 @@ -#ifndef __CLI_GETPID_H -#define __CLI_GETPID_H - -#include "frontend_getpid.h" - -int getPidValues(struct backend_conn *, struct frontend_pid_data *); -int cli_getpid(struct backend_conn * conn, int argc, char ** argv); - -#endif /* __CLI_GETPID_H */ \ No newline at end of file diff --git a/groundStation/src/cli/cli_getsetpoint.c b/groundStation/src/cli/cli_getsetpoint.c deleted file mode 100644 index 8cb30bb15b14fa643f8bf9ce60c4f1268c36a233..0000000000000000000000000000000000000000 --- a/groundStation/src/cli/cli_getsetpoint.c +++ /dev/null @@ -1,134 +0,0 @@ -#include <stdio.h> -#include <unistd.h> -#include <getopt.h> - -#include "cli_getsetpoint.h" - -int cli_getsetpoint(struct backend_conn * conn, int argc, char **argv) { - int c; - static int getheight = 0, getlat = 0, getlong = 0; - static int getpitch = 0, getroll = 0, getyaw = 0; - struct frontend_setpoint_data setpoint_data; - static int needHelp = 0; - static int mask; - static struct option long_options[] = { - /* These options don’t set a flag. We distinguish them by their indices. */ - {"help", no_argument, &needHelp, 1}, - {"height", no_argument, &getheight, 1}, - {"long", no_argument, &getlong, 1}, - {"lat", no_argument, &getlat, 1}, - {"pitch", no_argument, &getpitch, 1}, - {"roll", no_argument, &getroll, 1}, - {"yaw", no_argument, &getyaw, 1}, - {0, 0, 0, 0} - }; - - while (1) - { - /* getopt_long stores the option index here. */ - int option_index = 0; - - c = getopt_long(argc, argv, "a", long_options, &option_index); - - /* Detect the end of the options. */ - if (c == -1) - break; - - if (c == 'a') { - getheight = 1; - getlat = 1; - getlong = 1; - getpitch = 1; - getroll = 1; - getyaw = 1; - } - } - - if (needHelp) { - printf("Getsetpoint gets the height, lat, long, pitch, roll and yaw set point values for the quad.\n"); - printf("Usage Syntax : \n\t./Cli getsetpoint [options...]\n"); - printf("Symlink Usage Syntax : \n\t./getsetpoint [options...]\n\n"); - printf("Available options include the following\n"); - printf("\t[-a] : Gets all of the following setpoints\n"); - printf("\t[--height] : Gets the height setpoint\n"); - printf("\t[--lat] : Gets the latitude setpoint\n"); - printf("\t[--long] : Gets the longitude setpoint\n"); - printf("\t[--pitch] : Gets the pitch setpoint\n"); - printf("\t[--roll] : Gets the roll setpoint\n"); - printf("\t[--yaw] : Gets the yaw setpoint\n"); - return 0; - } - - if (argc < 2) { - printf("Incorrect Usage, run './cli getsetpoint --help' for correct usage.\n"); - return 1; - } - int result; - if(getheight) { - if ((result = getSetPointValues(conn, &setpoint_data, HEIGHT))) { - return result; - } - } - if(getlat) { - if ((result = getSetPointValues(conn, &setpoint_data, LAT))) { - return result; - } - } - if(getlong) { - if ((result = getSetPointValues(conn, &setpoint_data, LONGG))) { - return result; - } - } - if(getpitch) { - if ((result = getSetPointValues(conn, &setpoint_data, PITCH))) { - return result; - } - } - if(getroll) { - if ((result = getSetPointValues(conn, &setpoint_data, ROLL))) { - return result; - } - } - if(getyaw) { - if ((result = getSetPointValues(conn, &setpoint_data, YAW))) { - return result; - } - } - - return 0; -} - -int getSetPointValues(struct backend_conn * conn, struct frontend_setpoint_data * setpoint_data, int type) { - if(frontend_getsetpoint(conn, setpoint_data, type)) { - return 1; - } - - switch(type) { - case HEIGHT : - printf("Height: %f\n", - setpoint_data->height); - break; - case LAT : - printf("Latitude: %f\n", - setpoint_data->lat); - case LONGG : - printf("Longitude: %f\n", - setpoint_data->longg); - break; - case PITCH : - printf("Pitch: %f\n", - setpoint_data->pitch); - break; - case ROLL : - printf("Roll: %f\n", - setpoint_data->roll); - break; - case YAW : - printf("Yaw: %f\n", - setpoint_data->yaw); - break; - default : - break; - } - return 0; -} diff --git a/groundStation/src/cli/cli_getsetpoint.h b/groundStation/src/cli/cli_getsetpoint.h deleted file mode 100644 index a852b37973c84b21ca985e89dbb90180f35907ba..0000000000000000000000000000000000000000 --- a/groundStation/src/cli/cli_getsetpoint.h +++ /dev/null @@ -1,18 +0,0 @@ -#ifndef __CLI_GETSETPOINT_H -#define __CLI_GETSETPOINT_H - -#include "frontend_getsetpoint.h" - -enum { - HEIGHT, - LAT, - LONGG, - PITCH, - ROLL, - YAW -}; - -int getSetPointValues(struct backend_conn *, struct frontend_setpoint_data *, int type); -int cli_getsetpoint(struct backend_conn * conn, int argc, char ** argv); - -#endif /* __CLI_GETSETPOINT_H */ \ No newline at end of file diff --git a/groundStation/src/cli/cli_monitor.c b/groundStation/src/cli/cli_monitor.c deleted file mode 100644 index 7f1df5bf38236b1a6e88224d25b9ac57e69bc9af..0000000000000000000000000000000000000000 --- a/groundStation/src/cli/cli_monitor.c +++ /dev/null @@ -1,151 +0,0 @@ -#define _GNU_SOURCE - -#include <stdio.h> -#include <unistd.h> -#include <getopt.h> -#include <time.h> -#include <unistd.h> -#include <err.h> - -#include "cli_monitor.h" -#include "frontend_tracker.h" -#include "frontend_getpid.h" -#include "pid_common.h" - -int rate = 10; -static int pidcounter = 0; -static int monitorcounter = 0; - -int cli_monitor(struct backend_conn * conn, int argc, char **argv) { - int c, result; - int countFlag = 0; - int count; - int forever = 0; - static int needHelp = 0; - - static struct option long_options[] = { - /* These options don’t set a flag. We distinguish them by their indices. */ - {"help", no_argument, &needHelp, 1}, - {0, 0, 0, 0} - }; - - while (1) - { - /* getopt_long stores the option index here. */ - int option_index = 0; - - // If you change this VVV please also update the help message - c = getopt_long(argc, argv, "fc:r:", long_options, &option_index); - - if (c == -1) - break; - - switch(c) { - case 'c' : - count = atoi(optarg); - countFlag = 1; - break; - case 'r' : - rate = atoi(optarg) + 1; - break; - case 'f' : - forever = 1; - break; - default : - break; - } - } - - if (needHelp) { - printf("Monitor provides real time information about the quad. Including positional data as well as controller constants\n\n"); - printf("Usage Syntax : \n\t./Cli monitor [options...]\n"); - printf("Symlink Usage Syntax : \n\t./monitor [options...]\n\n"); - printf("Available options include the following\n"); - printf("\t[-f] : Run monitor continuously until you kill the program. (ctrl-C)\n"); - printf("\t[-c] 'count' : Sets the number of times the monitor will refresh\n"); - printf("\t[-r] 'rate' : Sets the 'rate' at which the monitor will refresh per second\n"); - return 0; - } - - if (forever) { - for (;;) { - struct timespec req; - if (rate == 1) { - req.tv_sec = 1; - req.tv_nsec = 0; - } else { - req.tv_sec = 0; - req.tv_nsec = 1000000000 / rate; - } - nanosleep(&req, NULL); - monitor(conn); - } - } else if (countFlag) { - for (int i = 0; i < count; i++) { - result = monitor(conn); - - struct timespec req; - if (rate == 1) { - req.tv_sec = 1; - req.tv_nsec = 0; - } else { - req.tv_sec = 0; - req.tv_nsec = 1000000000 / rate; - } - nanosleep(&req, NULL); - } - } else { - return monitor(conn); - } - - return result; -} - -int monitor(struct backend_conn * conn) { - static struct frontend_pid_data pid_data[PID_NUM_PIDS]; - /* Get tracker data */ - struct frontend_tracker_data td; - - if (frontend_track(conn, &td)) { - errx(1, "Error reading tracker data"); - } - - if (monitorcounter == 0) { - for(int i = 0; i < PID_NUM_PIDS; ++i) { - pid_data[i].controller = i; - if (frontend_getpid(conn, &pid_data[i])) { - errx(1, "Error reading pid values"); - } - } - } else { - if ((monitorcounter % (rate * 5)) == 0) { - pid_data[pidcounter].controller = pidcounter; - if (frontend_getpid(conn, &pid_data[pidcounter])) { - errx(1, "Error reading pid values"); - } - } - } - - /* Print stuff on screen */ - /* Assuming a tab width of 8 columns */ - printf("\033[2J"); - printf("STATUS: NA\n"); - printf("CTRLR : P R Y P_V R_V Y_V H LAT LON\n"); - printf(" P :%10.3lf %10.3lf %10.3lf %10.3lf %10.3lf %10.3lf %10.3lf %10.3lf %10.3lf\n", - pid_data[0].p, pid_data[1].p, pid_data[2].p, pid_data[3].p, pid_data[4].p, - pid_data[5].p, pid_data[6].p, pid_data[7].p, pid_data[8].p); - printf(" I :%10.3lf %10.3lf %10.3lf %10.3lf %10.3lf %10.3lf %10.3lf %10.3lf %10.3lf\n", - pid_data[0].i, pid_data[1].i, pid_data[2].i, pid_data[3].i, pid_data[4].i, - pid_data[5].i, pid_data[6].i, pid_data[7].i, pid_data[8].i); - printf(" D :%10.3lf %10.3lf %10.3lf %10.3lf %10.3lf %10.3lf %10.3lf %10.3lf %10.3lf\n", - pid_data[0].d, pid_data[1].d, pid_data[2].d, pid_data[3].d, pid_data[4].d, - pid_data[5].d, pid_data[6].d, pid_data[7].d, pid_data[8].d); - printf("PosAtt: H LAT LON P R Y\n"); - printf(" :%10.3lf%10.3lf%10.3lf%10.3lf%10.3lf%10.3lf\n", - td.height, td.lateral, td.longitudinal, - td.pitch, td.roll, td.yaw); //TODO: Tracker data long and lat are backwards - - monitorcounter++; - pidcounter = ((pidcounter + 1) % PID_NUM_PIDS); - return 0; -} diff --git a/groundStation/src/cli/cli_monitor.h b/groundStation/src/cli/cli_monitor.h deleted file mode 100644 index 694323739001d0b0e1b820c37b87ff95362f27e5..0000000000000000000000000000000000000000 --- a/groundStation/src/cli/cli_monitor.h +++ /dev/null @@ -1,14 +0,0 @@ -#ifndef __CLI_MONITOR_H -#define __CLI_MONITOR_H - -#include <time.h> - -#include "frontend_getpid.h" - -#define SECOND_IN_NANO 1000000000 - -int cli_monitor(struct backend_conn * conn, int argc, char **argv); - -// Return 0 on success and 1 otherwise -int monitor(struct backend_conn * conn); -#endif /* __CLI_MONITOR_H */ \ No newline at end of file diff --git a/groundStation/src/cli/cli_output.c b/groundStation/src/cli/cli_output.c new file mode 100644 index 0000000000000000000000000000000000000000..f48266f41d0709bf7651afce110a29e878cd3621 --- /dev/null +++ b/groundStation/src/cli/cli_output.c @@ -0,0 +1,37 @@ +#include <stdio.h> +#include <unistd.h> + +#include "cli.h" +#include "cli_output.h" +#include "frontend_output.h" + +int cli_getoutput(struct backend_conn * conn, int argc, char ** argv) { + int needHelp = 0; + struct frontend_output_data output_data; + + if ((needHelp = help_check(argc, argv))) { + printf("getoutput gets the output value of a specified block_id and output_id\n"); + printf("Usage Syntax : \n\t./Cli getoutput block_id output_id\n"); + printf("Symlink Usage Syntax : \n\t./getoutput block_id output_id\n\n"); + return 0; + } + + if (argc < 3) { + printf("Incorrect Usage, run './cli getoutput --help' for correct usage.\n"); + return 1; + } + + output_data.block = atoi(argv[1]); + output_data.output = atoi(argv[2]); + + if (frontend_getoutput(conn, &output_data)) { + return 1; + } + + printf("BLOCK.OUTPUT = VAL\n" \ + " %hu.%hu = %lf\n", + output_data.block, + output_data.output, + output_data.value); + return 0; +} \ No newline at end of file diff --git a/groundStation/src/cli/cli_output.h b/groundStation/src/cli/cli_output.h new file mode 100644 index 0000000000000000000000000000000000000000..153dc10bb5d863d8911b6d9ccb931e41ee471caa --- /dev/null +++ b/groundStation/src/cli/cli_output.h @@ -0,0 +1,8 @@ +#ifndef _CLI_OUTPUT_H +#define _CLI_OUTPUT_H + +#include "frontend_output.h" + +int cli_getoutput(struct backend_conn * conn, int argc, char ** argv); + +#endif /* _CLI_OUTPUT_H */ \ No newline at end of file diff --git a/groundStation/src/cli/cli_param.c b/groundStation/src/cli/cli_param.c new file mode 100644 index 0000000000000000000000000000000000000000..d123a088c9bc1ae7c15c33369b573834a6793b0a --- /dev/null +++ b/groundStation/src/cli/cli_param.c @@ -0,0 +1,64 @@ +#include <stdio.h> +#include <unistd.h> + +#include "cli.h" +#include "cli_param.h" +#include "frontend_param.h" + +int cli_getparam(struct backend_conn * conn, int argc, char ** argv) { + int needHelp = 0; + struct frontend_param_data param_data; + + if ((needHelp = help_check(argc, argv))) { + printf("getparam gets the param_val for a specified block_id and param_id\n"); + printf("Usage Syntax : \n\t./Cli getparam block_id param_id\n"); + printf("Symlink Usage Syntax : \n\t./getparam block_id param_id\n\n"); + return 0; + } + + if (argc < 3) { + printf("Incorrect Usage, run './cli getparam --help' for correct usage.\n"); + return 1; + } + + param_data.block = atoi(argv[1]); + param_data.param = atoi(argv[2]); + + if (frontend_getparam(conn, ¶m_data)) { + return 1; + } + + printf("BLOCK.PARAM = VAL\n" \ + " %hu.%hu = %lf\n", + param_data.block, + param_data.param, + param_data.value); + return 0; +} + +int cli_setparam(struct backend_conn * conn, int argc, char ** argv) { + int needHelp = 0; + struct frontend_param_data param_data; + + if ((needHelp = help_check(argc, argv))) { + printf("setparam sets the param_val for a specified block_id and param_id\n"); + printf("Usage Syntax : \n\t./Cli setparam block_id param_id value\n"); + printf("Symlink Usage Syntax : \n\t./setparam block_id param_id value\n\n"); + return 0; + } + + if (argc < 4) { + printf("Incorrect Usage, run './cli setparam --help' for correct usage.\n"); + return 1; + } + + param_data.block = atoi(argv[1]); + param_data.param = atoi(argv[2]); + param_data.value = atoi(argv[3]); + + if (frontend_setparam(conn, ¶m_data)) { + return 1; + } + + return 0; +} diff --git a/groundStation/src/cli/cli_param.h b/groundStation/src/cli/cli_param.h new file mode 100644 index 0000000000000000000000000000000000000000..2ded347749c77d0f5d6d85e5c28a7371f2da46f5 --- /dev/null +++ b/groundStation/src/cli/cli_param.h @@ -0,0 +1,9 @@ +#ifndef _CLI_PARAM_H +#define _CLI_PARAM_H + +#include "frontend_param.h" + +int cli_getparam(struct backend_conn * conn, int argc, char ** argv); +int cli_setparam(struct backend_conn * conn, int argc, char ** argv); + +#endif /* _CLI_PARAM_H */ \ No newline at end of file diff --git a/groundStation/src/cli/cli_setpid.c b/groundStation/src/cli/cli_setpid.c deleted file mode 100644 index b2ca124f5e16769dae5d4234d3431ce396210b93..0000000000000000000000000000000000000000 --- a/groundStation/src/cli/cli_setpid.c +++ /dev/null @@ -1,103 +0,0 @@ -#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 (argc < 2) { - printf("Incorrect Usage, run './cli setpid --help' for correct usage.\n"); - return 1; - } - - 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_LONG; - } else if (setLat) { - pid_data.controller = PID_LAT; - } - - frontend_setpid(conn, &pid_data, mask); - return 0; -} diff --git a/groundStation/src/cli/cli_setpid.h b/groundStation/src/cli/cli_setpid.h deleted file mode 100644 index d78fcc07042d7d571f35940f798493a5c8cb1c88..0000000000000000000000000000000000000000 --- a/groundStation/src/cli/cli_setpid.h +++ /dev/null @@ -1,8 +0,0 @@ -#ifndef __CLI_SETPID_H -#define __CLI_SETPID_H - -#include "frontend_setpid.h" - -int cli_setpid(struct backend_conn * conn, int argc, char ** argv); - -#endif /* __CLI_SETPID_H */ \ No newline at end of file diff --git a/groundStation/src/cli/cli_setsetpoint.c b/groundStation/src/cli/cli_setsetpoint.c deleted file mode 100644 index e0c43814ff98b2e5326b2d6feb0e14fa468d9032..0000000000000000000000000000000000000000 --- a/groundStation/src/cli/cli_setsetpoint.c +++ /dev/null @@ -1,89 +0,0 @@ -#include <stdio.h> -#include <unistd.h> -#include <getopt.h> - -#include "cli_setsetpoint.h" - -int cli_setsetpoint(struct backend_conn * conn, int argc, char **argv) { - int c; - static int setheight = 0, setlat = 0, setlong = 0; - static int setpitch = 0, setroll = 0, setyaw = 0; - struct frontend_setpoint_data setpoint_data; - static int needHelp = 0; - static int mask; - static struct option long_options[] = { - /* These options don’t set a flag. We distinguish them by their indices. */ - {"help", no_argument, &needHelp, 1}, - {"height", required_argument, 0, 'h'}, - {"long", required_argument, 0, 'l'}, - {"lat", required_argument, 0, 't'}, - {"pitch", required_argument, 0, 'p'}, - {"roll", required_argument, 0, 'r'}, - {"yaw", required_argument, 0, 'y'}, - {0, 0, 0, 0} - }; - - while (1) - { - /* getopt_long stores the option index here. */ - int option_index = 0; - - c = getopt_long(argc, argv, "", long_options, &option_index); - - /* Detect the end of the options. */ - if (c == -1) - break; - - switch(c) { - case 'h' : - setpoint_data.height = atof(optarg); - mask |= SET_HEIGHT; - break; - case 't' : - setpoint_data.lat = atof(optarg); - mask |= SET_LAT; - break; - case 'l' : - setpoint_data.longg = atof(optarg); - mask |= SET_LONG; - break; - case 'p' : - setpoint_data.pitch = atof(optarg); - mask |= SET_PITCH; - break; - case 'r' : - setpoint_data.roll = atof(optarg); - mask |= SET_ROLL; - break; - case 'y' : - setpoint_data.yaw = atof(optarg); - mask |= SET_YAW; - break; - default : - break; - } - - } - - if (needHelp) { - printf("Setsetpoint sets the height, lat, long, pitch, roll, or yaw set point values for the quad.\n"); - printf("Usage Syntax : \n\t./Cli setsetpoint [options...]\n"); - printf("Symlink Usage Syntax : \n\t./setsetpoint [options...]\n\n"); - printf("Available options include the following\n"); - printf("\t[--height] 'val' : Sets the height setpoint value to 'val'\n"); - printf("\t[--lat] 'val' : Sets the latitude setpoint value to 'val'\n"); - printf("\t[--long] 'val' : Sets the longitude setpoint value to 'val'\n"); - printf("\t[--pitch] 'val' : Sets the pitch setpoint value to 'val'\n"); - printf("\t[--roll] 'val' : Sets the roll setpoint value to 'val'\n"); - printf("\t[--yaw] 'val' : Sets the yaw setpoint value to 'val'\n"); - return 0; - } - - if (argc < 2) { - printf("Incorrect Usage, run './cli setpid --help' for correct usage.\n"); - return 1; - } - - frontend_setsetpoint(conn, &setpoint_data, mask); - return 0; -} diff --git a/groundStation/src/cli/cli_setsetpoint.h b/groundStation/src/cli/cli_setsetpoint.h deleted file mode 100644 index 514145a9485733276fb1982418f5a44747103e74..0000000000000000000000000000000000000000 --- a/groundStation/src/cli/cli_setsetpoint.h +++ /dev/null @@ -1,8 +0,0 @@ -#ifndef __CLI_SETSETPOINT_H -#define __CLI_SETSETPOINT_H - -#include "frontend_setsetpoint.h" - -int cli_setsetpoint(struct backend_conn * conn, int argc, char ** argv); - -#endif /* __CLI_SETSETPOINT_H */ \ No newline at end of file diff --git a/groundStation/src/cli/cli_source.c b/groundStation/src/cli/cli_source.c new file mode 100644 index 0000000000000000000000000000000000000000..86f47a922a4b73c9024208f9c274c16138f136af --- /dev/null +++ b/groundStation/src/cli/cli_source.c @@ -0,0 +1,63 @@ +#include <stdio.h> +#include <unistd.h> + +#include "cli.h" +#include "cli_source.h" +#include "frontend_source.h" + +int cli_getsource(struct backend_conn * conn, int argc, char ** argv) { + struct frontend_source_data source_data; + int needHelp = 0; + + if ((needHelp = help_check(argc, argv))) { + printf("getsource gets the src_block_id and src_output_id for a specified dst_block_id and dst_input_id\n"); + printf("Usage Syntax : \n\t./Cli getsource dst_block_id dst_input_id\n"); + printf("Symlink Usage Syntax : \n\t./getsource dst_block_id dst_input_id\n\n"); + return 0; + } + + if (argc < 3) { + printf("Incorrect Usage, run './cli getsource --help' for correct usage.\n"); + return 1; + } + + source_data.dst_block = atoi(argv[1]); + source_data.dst_input = atoi(argv[2]); + + if (frontend_getsource(conn, &source_data)) { + return 1; + } + + printf("SRC BLOCK.OUTPUT ---> DST BLOCK.INPUT\n" \ + " %hu.%hu ---> %hu.%hu\n", + source_data.src_block, source_data.src_output, + source_data.dst_block, source_data.dst_input); + return 0; +} + +int cli_setsource(struct backend_conn * conn, int argc, char ** argv) { + struct frontend_source_data source_data; + int needHelp = 0; + + if ((needHelp = help_check(argc, argv))) { + printf("setsource sets the src_block_id and src_output_id for a specified dst_block_id and dst_input_id\n"); + printf("Usage Syntax : \n\t./Cli setsource dst_block_id dst_input_id src_block_id src_output_id\n"); + printf("Symlink Usage Syntax : \n\t./setsource dst_block_id dst_input_id src_block_id src_output_id\n\n"); + return 0; + } + + if (argc < 5) { + printf("Incorrect Usage, run './cli setsource --help' for correct usage.\n"); + return 1; + } + + source_data.dst_block = atoi(argv[1]); + source_data.dst_input = atoi(argv[2]); + source_data.src_block = atoi(argv[3]); + source_data.src_output = atoi(argv[4]); + + if (frontend_setsource(conn, &source_data)) { + return 1; + } + return 0; +} diff --git a/groundStation/src/cli/cli_source.h b/groundStation/src/cli/cli_source.h new file mode 100644 index 0000000000000000000000000000000000000000..0e1b30cba38db0d7fb2a335aaea89d04cd7c900f --- /dev/null +++ b/groundStation/src/cli/cli_source.h @@ -0,0 +1,9 @@ +#ifndef _CLI_SOURCE_H +#define _CLI_SOURCE_H + +#include "frontend_source.h" + +int cli_getsource(struct backend_conn * conn, int argc, char ** argv); +int cli_setsource(struct backend_conn * conn, int argc, char ** argv); + +#endif /* _CLI_SOURCE_H */ \ No newline at end of file diff --git a/groundStation/src/frontend/frontend_common.h b/groundStation/src/frontend/frontend_common.h index 60ee336269ee95388ed4f334ac1e3dfbe6e143bb..4986d859c2a257f42ad77145c513a54e24671fed 100644 --- a/groundStation/src/frontend/frontend_common.h +++ b/groundStation/src/frontend/frontend_common.h @@ -1,5 +1,5 @@ -#ifndef __FRONTEND_COMMON_H -#define __FRONTEND_COMMON_H +#ifndef _FRONTEND_COMMON_H +#define _FRONTEND_COMMON_H #include <stdlib.h> struct backend_conn; @@ -19,4 +19,23 @@ char * ucart_backendGetline(struct backend_conn * conn); /* Write a line to the backend */ int ucart_backendWrite(struct backend_conn * backend, const char * line); +struct frontend_output_data { + unsigned short block; + unsigned short output; + float value; +}; + +struct frontend_param_data { + unsigned short block; + unsigned short param; + float value; +}; + +struct frontend_source_data { + unsigned short dst_block; + unsigned short dst_input; + unsigned short src_block; + unsigned short src_output; +}; + #endif /* __FRONTEND_COMMON_H */ \ No newline at end of file diff --git a/groundStation/src/frontend/frontend_getimu.h b/groundStation/src/frontend/frontend_getimu.h deleted file mode 100644 index 1747252bbc78a403799fb0744c91e2b04883c0cb..0000000000000000000000000000000000000000 --- a/groundStation/src/frontend/frontend_getimu.h +++ /dev/null @@ -1,6 +0,0 @@ -#ifndef __FRONTEND_GETIMU_H -#define __FRONTEND_GETIMU_H - -#include "frontend_common.h" - -#endif /* __FRONTEND_GETIMU_H */ \ No newline at end of file diff --git a/groundStation/src/frontend/frontend_getpid.c b/groundStation/src/frontend/frontend_getpid.c deleted file mode 100644 index 2fd33c31116bf135a3c1f78cf94b2353bdbb8820..0000000000000000000000000000000000000000 --- a/groundStation/src/frontend/frontend_getpid.c +++ /dev/null @@ -1,91 +0,0 @@ -#include <stdio.h> -#include <string.h> -#include <stdlib.h> -#include <err.h> - -#include "frontend_getpid.h" -#include "pid_common.h" - -/* Get a specified PID. - * - * Example: - * - * struct frontend_pid_data pid_data; - * pid_data.pid = PID_PITCH; - * if (frontend_getpid(conn, &pid_data)) { - * error - * } else { - * pid_data.p, pid_data.i, and pid_data.d are filled - * } - * - * Returns 0 on success, 1 on error - */ -int frontend_getpid( - struct backend_conn * conn, struct frontend_pid_data * pid_data) { - - char line[100] = ""; - switch (pid_data->controller) { - case PID_PITCH : - strncpy(line, "getpitchp\ngetpitchi\ngetpitchd\n", 30); - break; - case PID_ROLL : - strncpy(line, "getrollp\ngetrolli\ngetrolld\n", 27); - break; - case PID_YAW : - strncpy(line, "getyawp\ngetyawi\ngetyawd\n", 24); - break; - case PID_PITCH_RATE : - strncpy(line, "getpitchratep\ngetpitchratei\ngetpitchrated\n", 42); - break; - case PID_ROLL_RATE : - strncpy(line, "getrollratep\ngetrollratei\ngetrollrated\n", 39); - break; - case PID_YAW_RATE : - strncpy(line, "getyawratep\ngetyawratei\ngetyawrated\n", 36); - break; - case PID_HEIGHT : - strncpy(line, "getheightp\ngetheighti\ngetheightd\n", 33); - break; - case PID_LAT : - strncpy(line, "getlatp\ngetlati\ngetlatd\n", 24); - break; - case PID_LONG : - strncpy(line, "getlongp\ngetlongi\ngetlongd\n", 27); - break; - default : - return 1; - } - - int size; - if((size = ucart_backendWrite(conn, line)) < 0 ) { - return 1; - } - - char * response; - char tmpresponse[64]; - char * cmdString; - size_t pendingResponses = 3; - float value; - while (pendingResponses) { - response = ucart_backendGetline(conn); - if (response == NULL) { - warnx("Line not returned from backend"); - return 1; - } - strncpy(tmpresponse, response, 64); - if (strncmp(tmpresponse, "get", 3) == 0) { - cmdString = strtok(tmpresponse, " "); - value = strtof(&response[strlen(cmdString)],NULL); - - if(cmdString[strlen(cmdString)-1] == 'p') { - pid_data->p = value; - } else if(cmdString[strlen(cmdString)-1] == 'i') { - pid_data->i = value; - } else if(cmdString[strlen(cmdString)-1] == 'd') { - pid_data->d = value; - } - pendingResponses--; - } - } - return 0; -} diff --git a/groundStation/src/frontend/frontend_getpid.h b/groundStation/src/frontend/frontend_getpid.h deleted file mode 100644 index e7933f1abf1869ac7f3074d472c8a8a36106507c..0000000000000000000000000000000000000000 --- a/groundStation/src/frontend/frontend_getpid.h +++ /dev/null @@ -1,26 +0,0 @@ -#ifndef __FRONTEND_GETPID_H -#define __FRONTEND_GETPID_H - -#include "frontend_common.h" -#include "pid_common.h" - -/* Get a specified PID. - * - * Example: - * - * struct frontend_pid_data pid_data; - * pid_data.pid = PITCH; - * if (frontend_getpid(conn, &pid_data)) { - * error - * } else { - * pid_data.p, pid_data.i, and pid_data.d are filled - * } - * - * Returns 0 on success, 1 on error - */ -int frontend_getpid( - struct backend_conn * conn, - struct frontend_pid_data * pid_data); - - -#endif /* __FRONTEND_GETPID_H */ \ No newline at end of file diff --git a/groundStation/src/frontend/frontend_getsetpoint.c b/groundStation/src/frontend/frontend_getsetpoint.c deleted file mode 100644 index b23a5dda43847e08eb3d7eb06479bff27c002575..0000000000000000000000000000000000000000 --- a/groundStation/src/frontend/frontend_getsetpoint.c +++ /dev/null @@ -1,94 +0,0 @@ -#include <stdio.h> -#include <string.h> -#include <stdlib.h> -#include <err.h> - -#include "frontend_getsetpoint.h" -#include "setpoint_common.h" -#include "cli_getsetpoint.h" - -/* Get a specified setpoint. - * - * Example: - * - * struct frontend_setpoint_data setpoint_data; - * if (frontend_getpid(conn, &pid_data)) { - * error - * } else { - * setpoint_data.height, setpoint_data.lat, and setpoint.long are filled - * } - * - * Returns 0 on success, 1 on error - */ -int frontend_getsetpoint( - struct backend_conn * conn, struct frontend_setpoint_data * setpoint_data, int type) { - - char line[25] = ""; - switch (type) { - case HEIGHT : - strncpy(line, "getheight\n", 10); - break; - case LAT : - strncpy(line, "getlat\n", 7); - break; - case LONGG : - strncpy(line, "getlong\n", 8); - break; - case PITCH : - strncpy(line, "getpitch\n", 9); - break; - case ROLL : - strncpy(line, "getroll\n", 8); - break; - case YAW : - strncpy(line, "getyaw\n", 7); - break; - } - int size; - if((size = ucart_backendWrite(conn, line)) < 0 ) { - return 1; - } - - char * response; - char tmpresponse[64]; - char * cmdString; - size_t pendingResponses = 1; - float value; - while (pendingResponses) { - response = ucart_backendGetline(conn); - if (response == NULL) { - warnx("Line not returned from backend"); - return 1; - } - strncpy(tmpresponse, response, 64); - if (strncmp(tmpresponse, "get", 3) == 0) { - cmdString = strtok(tmpresponse, " "); - value = strtof(&response[strlen(cmdString)],NULL); - - switch (type) { - case HEIGHT : - setpoint_data->height = value; - break; - case LAT : - setpoint_data->lat = value; - break; - case LONGG : - setpoint_data->longg = value; - break; - case PITCH : - setpoint_data->pitch = value; - break; - case ROLL : - setpoint_data->roll = value; - break; - case YAW : - setpoint_data->yaw = value; - break; - } - - pendingResponses--; - } - } - - return 0; -} diff --git a/groundStation/src/frontend/frontend_getsetpoint.h b/groundStation/src/frontend/frontend_getsetpoint.h deleted file mode 100644 index f8dfdc87b3df057d5fd43714c5b81a52831ce763..0000000000000000000000000000000000000000 --- a/groundStation/src/frontend/frontend_getsetpoint.h +++ /dev/null @@ -1,25 +0,0 @@ -#ifndef __FRONTEND_GETSETPOINT_H -#define __FRONTEND_GETSETPOINT_H - -#include "frontend_common.h" -#include "setpoint_common.h" - -/* Get a specified setpoint. - * - * Example: - * - * struct frontend_setpoint_data setpoint_data; - * if (frontend_getpid(conn, &pid_data)) { - * error - * } else { - * setpoint_data.height, setpoint_data.lat, and setpoint.long are filled - * } - * - * Returns 0 on success, 1 on error - */ -int frontend_getsetpoint( - struct backend_conn * conn, - struct frontend_setpoint_data * setpoint_data, - int type); - -#endif /* __FRONTEND_GETPID_H */ \ No newline at end of file diff --git a/groundStation/src/frontend/frontend_output.c b/groundStation/src/frontend/frontend_output.c new file mode 100644 index 0000000000000000000000000000000000000000..a93bb0403f5096722d3509c5af84f399b839086e --- /dev/null +++ b/groundStation/src/frontend/frontend_output.c @@ -0,0 +1,45 @@ +#include <stdio.h> +#include <string.h> +#include <stdlib.h> +#include <err.h> + +#include "frontend_output.h" + +/* Get the value that corresponds + * to the block.output in question. + * + * Returns 0 on success, 1 on error + */ +int frontend_getoutput( + struct backend_conn * conn, + struct frontend_output_data * output_data) { + + char msg[64] = ""; + int written; + + snprintf(msg, 64, "getoutput %hu %hu\n", output_data->block, output_data->output); + + if((written = ucart_backendWrite(conn, msg)) < 0) { + return 1; + } + + size_t pendingResponses = 1; + char * response; + + while (pendingResponses) { + response = ucart_backendGetline(conn); + if (response == NULL) { + warnx("Line not returned from backend"); + return 1; + } + + if (strncmp(response, "getoutput", 9) == 0) { + sscanf(response, "getoutput %hu %hu %f\n", + &output_data->block, &output_data->output, + &output_data->value); + pendingResponses--; + } + } + + return 0; +} \ No newline at end of file diff --git a/groundStation/src/frontend/frontend_output.h b/groundStation/src/frontend/frontend_output.h new file mode 100644 index 0000000000000000000000000000000000000000..b2a4e4333cc83344c0154c9bc20d764d9439799d --- /dev/null +++ b/groundStation/src/frontend/frontend_output.h @@ -0,0 +1,16 @@ +#ifndef _FRONTEND_OUTPUT_H +#define _FRONTEND_OUTPUT_H + +#include "frontend_common.h" + +/* Get the value that corresponds + * to the block.output in question. + * + * Returns 0 on success, 1 on error + */ +int frontend_getoutput( + struct backend_conn * conn, + struct frontend_output_data * output_data); + + +#endif /* __FRONTEND_OUTPUT_H */ \ No newline at end of file diff --git a/groundStation/src/frontend/frontend_param.c b/groundStation/src/frontend/frontend_param.c new file mode 100644 index 0000000000000000000000000000000000000000..c98972031b9df52f3be0fbf628933798b4e3a80c --- /dev/null +++ b/groundStation/src/frontend/frontend_param.c @@ -0,0 +1,65 @@ +#include <stdio.h> +#include <string.h> +#include <stdlib.h> +#include <err.h> + +#include "frontend_param.h" + + +/* Get the value of block.param + * + * Returns 0 on success, 1 on error + */ +int frontend_getparam( + struct backend_conn * conn, + struct frontend_param_data * param_data) { + + char msg[64] = ""; + int written; + + snprintf(msg, 64, "getparam %hu %hu\n", param_data->block, param_data->param); + + if((written = ucart_backendWrite(conn, msg)) < 0) { + return 1; + } + + size_t pendingResponses = 1; + char * response; + + while (pendingResponses) { + response = ucart_backendGetline(conn); + if (response == NULL) { + warnx("Line not returned from backend"); + return 1; + } + + if (strncmp(response, "getparam", 8) == 0) { + sscanf(response, "getparam %hu %hu %f\n", + ¶m_data->block, ¶m_data->param, + ¶m_data->value); + pendingResponses--; + } + } + + return 0; +} + +/* Set the value of block.param + * + * Returns 0 on success, 1 on error + */ +int frontend_setparam( + struct backend_conn * conn, + struct frontend_param_data * param_data) { + + char msg[64] = ""; + int written; + + snprintf(msg, 64, "setparam %hu %hu %f\n", param_data->block, param_data->param, param_data->value); + + if((written = ucart_backendWrite(conn, msg)) < 0) { + return 1; + } + + return 0; +} \ No newline at end of file diff --git a/groundStation/src/frontend/frontend_param.h b/groundStation/src/frontend/frontend_param.h new file mode 100644 index 0000000000000000000000000000000000000000..26854dd0d7ee104e20edb524a5459a2535794426 --- /dev/null +++ b/groundStation/src/frontend/frontend_param.h @@ -0,0 +1,23 @@ +#ifndef _FRONTEND_PARAM_H +#define _FRONTEND_PARAM_H + +#include "frontend_common.h" + +/* Get the value of block.param + * + * Returns 0 on success, 1 on error + */ +int frontend_getparam( + struct backend_conn * conn, + struct frontend_param_data * param_data); + +/* Set the value of block.param + * + * Returns 0 on success, 1 on error + */ +int frontend_setparam( + struct backend_conn * conn, + struct frontend_param_data * param_data); + + +#endif /* __FRONTEND_PARAM_H */ \ No newline at end of file diff --git a/groundStation/src/frontend/frontend_setpid.c b/groundStation/src/frontend/frontend_setpid.c deleted file mode 100644 index 58bf4109cd1c50d22486e464c101c9f8dd3c367e..0000000000000000000000000000000000000000 --- a/groundStation/src/frontend/frontend_setpid.c +++ /dev/null @@ -1,82 +0,0 @@ -#include <err.h> -#include <stdio.h> - -#include "frontend_setpid.h" -#include "pid_common.h" -#include "frontend_common.h" - -int frontend_setpid( - struct backend_conn * conn, - struct frontend_pid_data * pid_data, - int mask) -{ - if (conn == NULL) { - return -1; - } - - char * controller; - switch (pid_data->controller) { - case PID_PITCH: - controller = "pitch"; - break; - case PID_ROLL: - controller = "roll"; - break; - case PID_YAW: - controller = "yaw"; - break; - case PID_PITCH_RATE: - controller = "pitchrate"; - break; - case PID_ROLL_RATE: - controller = "rollrate"; - break; - case PID_YAW_RATE: - controller = "yawrate"; - break; - case PID_HEIGHT: - controller = "height"; - break; - case PID_LAT: - controller = "lat"; - break; - case PID_LONG: - controller = "long"; - break; - default: - warnx("Unsupported PID constant"); - return -1; - } - - char buffer[2048]; - /* Set the P, I, and D */ - if (mask & SET_P) { - if (snprintf(buffer, 2048, - "set%sp %f\n", - controller, - pid_data->p) >= 2048) { - errx(0, "Buffer to small to format!"); - } - ucart_backendWrite(conn, buffer); - } - if (mask & SET_I) { - if (snprintf(buffer, 2048, - "set%si %f\n", - controller, - pid_data->i) >= 2048) { - errx(0, "Buffer to small to format!"); - } - ucart_backendWrite(conn, buffer); - } - if (mask & SET_D) { - if (snprintf(buffer, 2048, - "set%sd %f\n", - controller, - pid_data->d) >= 2048) { - errx(0, "Buffer to small to format!"); - } - ucart_backendWrite(conn, buffer); - } - - return 0; -} diff --git a/groundStation/src/frontend/frontend_setpid.h b/groundStation/src/frontend/frontend_setpid.h deleted file mode 100644 index b9c6e073d3433a0350ebd9b2d9de8ee55a12f18e..0000000000000000000000000000000000000000 --- a/groundStation/src/frontend/frontend_setpid.h +++ /dev/null @@ -1,20 +0,0 @@ -#ifndef __FRONTEND_SETPID_H -#define __FRONTEND_SETPID_H - -#include "pid_common.h" -#include "frontend_common.h" - -int frontend_setpid( - struct backend_conn * conn, - struct frontend_pid_data * pid_data, - int mask); - -#define SET_P 0x01 -#define SET_I 0x02 -#define SET_D 0x04 - -#ifndef SET_ALL -#define SET_ALL (SET_P | SET_I | SET_D) -#endif /* SET_ALL */ - -#endif /* __FRONTEND_SETPID_H */ \ No newline at end of file diff --git a/groundStation/src/frontend/frontend_setsetpoint.c b/groundStation/src/frontend/frontend_setsetpoint.c deleted file mode 100644 index b9f43caed0ded4d80964dddcc39a12e165f5c54a..0000000000000000000000000000000000000000 --- a/groundStation/src/frontend/frontend_setsetpoint.c +++ /dev/null @@ -1,69 +0,0 @@ -#include <err.h> -#include <stdio.h> - -#include "frontend_setsetpoint.h" -#include "setpoint_common.h" -#include "frontend_common.h" - -int frontend_setsetpoint( - struct backend_conn * conn, - struct frontend_setpoint_data * setpoint_data, - int mask) -{ - if (conn == NULL) { - return -1; - } - - char buffer[2048]; - /* Set the P, I, and D */ - if (mask & SET_HEIGHT) { - if (snprintf(buffer, 2048, - "setheight %f\n", - setpoint_data->height) >= 2048) { - errx(0, "Buffer to small to format!"); - } - ucart_backendWrite(conn, buffer); - } - if (mask & SET_LAT) { - if (snprintf(buffer, 2048, - "setlat %f\n", - setpoint_data->lat) >= 2048) { - errx(0, "Buffer to small to format!"); - } - ucart_backendWrite(conn, buffer); - } - if (mask & SET_LONG) { - if (snprintf(buffer, 2048, - "setlong %f\n", - setpoint_data->longg) >= 2048) { - errx(0, "Buffer to small to format!"); - } - ucart_backendWrite(conn, buffer); - } - if (mask & SET_PITCH) { - if (snprintf(buffer, 2048, - "setpitch %f\n", - setpoint_data->pitch) >= 2048) { - errx(0, "Buffer to small to format!"); - } - ucart_backendWrite(conn, buffer); - } - if (mask & SET_ROLL) { - if (snprintf(buffer, 2048, - "setroll %f\n", - setpoint_data->roll) >= 2048) { - errx(0, "Buffer to small to format!"); - } - ucart_backendWrite(conn, buffer); - } - if (mask & SET_YAW) { - if (snprintf(buffer, 2048, - "setyaw %f\n", - setpoint_data->yaw) >= 2048) { - errx(0, "Buffer to small to format!"); - } - ucart_backendWrite(conn, buffer); - } - - return 0; -} diff --git a/groundStation/src/frontend/frontend_setsetpoint.h b/groundStation/src/frontend/frontend_setsetpoint.h deleted file mode 100644 index 2607c4d564e14b5c4baae8b9500a74d6b878b922..0000000000000000000000000000000000000000 --- a/groundStation/src/frontend/frontend_setsetpoint.h +++ /dev/null @@ -1,23 +0,0 @@ -#ifndef __FRONTEND_SETSETPOINT_H -#define __FRONTEND_SETSETPOINT_H - -#include "setpoint_common.h" -#include "frontend_common.h" - -int frontend_setsetpoint( - struct backend_conn * conn, - struct frontend_setpoint_data * setpoint_data, - int mask); - -#define SET_LAT 0x01 -#define SET_LONG 0x02 -#define SET_HEIGHT 0x04 -#define SET_PITCH 0x08 -#define SET_ROLL 0x10 -#define SET_YAW 0x20 - -#ifndef SET_ALL -#define SET_ALL (SET_LAT | SET_LONG | SET_HEIGHT | SET_PITCH | SET_ROLL | SET_YAW) -#endif /* SET_ALL */ - -#endif /* __FRONTEND_SETSETPOINT_H */ \ No newline at end of file diff --git a/groundStation/src/frontend/frontend_source.c b/groundStation/src/frontend/frontend_source.c new file mode 100644 index 0000000000000000000000000000000000000000..4016de3e1c43766fe42fbd19b85a68c63b828144 --- /dev/null +++ b/groundStation/src/frontend/frontend_source.c @@ -0,0 +1,68 @@ +#include <stdio.h> +#include <string.h> +#include <stdlib.h> +#include <err.h> + +#include "frontend_source.h" + + +/* Get the block.output that corresponds + * to the block.input in question. + * + * Returns 0 on success, 1 on error + */ +int frontend_getsource( + struct backend_conn * conn, + struct frontend_source_data * source_data) { + + char msg[64] = ""; + int written; + + snprintf(msg, 64, "getsource %hu %hu\n", source_data->dst_block, source_data->dst_input); + + if((written = ucart_backendWrite(conn, msg)) < 0) { + return 1; + } + + size_t pendingResponses = 1; + char * response; + + while (pendingResponses) { + response = ucart_backendGetline(conn); + if (response == NULL) { + warnx("Line not returned from backend"); + return 1; + } + + if (strncmp(response, "getsource", 9) == 0) { + sscanf(response, "getsource %hu %hu %hu %hu\n", + &source_data->dst_block, &source_data->dst_input, + &source_data->src_block, &source_data->src_output); + pendingResponses--; + } + } + + return 0; +} + +/* Set the connect the block.output from one block to + * another block.input. + * + * Returns 0 on success, 1 on error + */ +int frontend_setsource( + struct backend_conn * conn, + struct frontend_source_data * source_data) { + char msg[64] = ""; + int written; + + snprintf(msg, 64, "setsource %hu %hu %hu %hu\n", + source_data->dst_block, source_data->dst_input, + source_data->src_block, source_data->src_output); + + if((written = ucart_backendWrite(conn, msg)) < 0) { + return 1; + } + + return 0; +} diff --git a/groundStation/src/frontend/frontend_source.h b/groundStation/src/frontend/frontend_source.h new file mode 100644 index 0000000000000000000000000000000000000000..ac6da68956e933717a51ee9783bd1a821548a5a0 --- /dev/null +++ b/groundStation/src/frontend/frontend_source.h @@ -0,0 +1,25 @@ +#ifndef _FRONTEND_SOURCE_H +#define _FRONTEND_SOURCE_H + +#include "frontend_common.h" + +/* Get the block.output that corresponds + * to the block.input in question. + * + * Returns 0 on success, 1 on error + */ +int frontend_getsource( + struct backend_conn * conn, + struct frontend_source_data * source_data); + +/* Set the connect the block.output from one block to + * another block.input. + * + * Returns 0 on success, 1 on error + */ +int frontend_setsource( + struct backend_conn * conn, + struct frontend_source_data * source_data); + + +#endif /* _FRONTEND_SOURCE_H */ \ No newline at end of file diff --git a/groundStation/src/frontend/pid_common.h b/groundStation/src/frontend/pid_common.h deleted file mode 100644 index b89e3b1cddc85bb8820520e91ea4fa1858becd3a..0000000000000000000000000000000000000000 --- a/groundStation/src/frontend/pid_common.h +++ /dev/null @@ -1,26 +0,0 @@ -#ifndef __PID_COMMON_H -#define __PID_COMMON_H - -enum pid_controller { - PID_PITCH, - PID_ROLL, - PID_YAW, - PID_PITCH_RATE, - PID_ROLL_RATE, - PID_YAW_RATE, - PID_HEIGHT, /* Z */ - PID_LAT, /* X */ - PID_LONG, /* Y */ - PID_NUM_PIDS -}; - -struct frontend_pid_data { - enum pid_controller controller; - - float p; - float i; - float d; -}; - - -#endif /* __PID_COMMON_H */ diff --git a/groundStation/src/frontend/setpoint_common.h b/groundStation/src/frontend/setpoint_common.h deleted file mode 100644 index a661cb1cd6b2a3fe84edbba7fdf2ffac85e9c074..0000000000000000000000000000000000000000 --- a/groundStation/src/frontend/setpoint_common.h +++ /dev/null @@ -1,15 +0,0 @@ -#ifndef __SETPOINT_COMMON_H -#define __SETPOINT_COMMON_H - -struct frontend_setpoint_data { - float height; - float lat; - /* named with two g's due to long type name */ - float longg; - float pitch; - float roll; - float yaw; -}; - - -#endif /* __SETPOINT_COMMON_H */ \ No newline at end of file