Ready to test setsetpoint

groundStation/src/cli/cli_setsetpoint.c

@@ -3,12 +3,14 @@
 #include <getopt.h>
 
 #include "cli_setsetpoint.h"
-// #include "frontend_setpid.h"
+#include "frontend_setsetpoint.h"
 
 int cli_set(struct backend_conn * conn,	int argc, char **argv) {
 	int c;
-	struct frontend_pid_data pid_data;
+	static int setX = 0, setY = 0, setZ = 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},
@@ -20,11 +22,28 @@ int cli_set(struct backend_conn * conn,	int argc, char **argv) {
 		/* getopt_long stores the option index here. */
 		int option_index = 0;
 
-		c = getopt_long(argc, argv, "", long_options, &option_index);
+		c = getopt_long(argc, argv, "x:y:z:", long_options, &option_index);
 
 		/* Detect the end of the options. */
 		if (c == -1)
 			break;
+
+		switch(c) {
+			case 'x' :
+				setpoint_data.x  = atof(optarg);
+				mask |= SET_X;
+				break;
+			case 'y' :
+				setpoint_data.y  = atof(optarg);
+				mask |= SET_Y;
+				break;
+			case 'z' :
+				setpoint_data.z  = atof(optarg);
+				mask |= SET_Z;
+				break;
+			default :
+				break;
+		}
 	}
 
 	if (needHelp) {
@@ -42,27 +61,7 @@ int cli_set(struct backend_conn * conn,	int argc, char **argv) {
 		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_LAT;
-	} else if (setLat) {
-		pid_data.controller = PID_LONG;
-	}
-
-	frontend_setpid(conn, &pid_data, mask);
+	
+	frontend_setsetpoint(conn, &setpoint_data, mask);
 	return 0;
 }

groundStation/src/cli/cli_setsetpoint.h

 #ifndef CLI_SETSETPOINT_H
 #define CLI_SETSETPOINT_H
 
-// #include "frontend_setpid.h"
+#include "frontend_setsetpoint.h"
 
 int cli_setsetpoint(struct backend_conn * conn, int argc, char ** argv);
 

groundStation/src/frontend/frontend_setpid.h

-#ifndef _FRONTEND_SETPID_H
-#define _FRONTEND_SETPID_H
+#ifndef FRONTEND_SETPID_H
+#define FRONTEND_SETPID_H
 
 #include "pid_common.h"
 #include "frontend_common.h"
@@ -14,4 +14,4 @@ int frontend_setpid(
 #define SET_D 0x04
 #define SET_ALL (SET_P | SET_I | SET_D)
 
-#endif
+#endif /* FRONTEND_SETPID_H */
\ No newline at end of file

groundStation/src/frontend/frontend_setsetpoint.c

+#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_X) {
+		if (snprintf(buffer, 2048,
+					"setsetpointx %f\n",  
+					setpoint_data->x) >= 2048) {
+			errx(0, "Buffer to small to format!");
+		}
+		ucart_backendWrite(conn, buffer);
+	}
+	if (mask & SET_Y) {
+		if (snprintf(buffer, 2048,
+					"setsetpointy %f\n",
+					setpoint_data->y) >= 2048) {
+			errx(0, "Buffer to small to format!");
+		}
+		ucart_backendWrite(conn, buffer);
+	}
+	if (mask & SET_Z) {
+		if (snprintf(buffer, 2048,
+					"setsetpointz %f\n",
+					setpoint_data->z) >= 2048) {
+			errx(0, "Buffer to small to format!");
+		}
+		ucart_backendWrite(conn, buffer);
+	}
+
+	return 0;
+}

groundStation/src/frontend/frontend_setsetpoint.h

+#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_X 0x01
+#define SET_Y 0x02
+#define SET_Z 0x04
+#define SET_ALL (SET_X | SET_Y | SET_Z)
+
+#endif /* FRONTEND_SETSETPOINT_H */
\ No newline at end of file

groundStation/src/frontend/setpoint_common.h

+#ifndef SETPOINT_COMMON_H
+#define SETPOINT_COMMON_H
+
+struct frontend_setpoint_data {
+	float x;
+	float y;
+	float z;
+};
+
+
+#endif /* SETPOINT_COMMON_H */
\ No newline at end of file