might be ok

groundStation/src/backend/backend.c

@@ -694,11 +694,11 @@ static void client_recv(int fd) {
 				}
 			}
 
-			// printf("packet = '");
-			// for(int i = 0; i < (int)psize; ++i) {
-			// 	printf(" %.2x ", packet[i]);
-			// }
-			// printf("'\n");
+			printf("packet = '");
+			for(int i = 0; i < (int)psize; ++i) {
+				printf(" %.2x ", packet[i]);
+			}
+			printf("'\n");
 			
 			writeQuad(packet, psize);
 
@@ -777,6 +777,7 @@ static void quad_recv() {
 
 		switch (m.msg_type) {
 			case DEBUG_ID :
+				printf()
 			case PACKETLOG_ID :
 			case GETPACKETLOGS_ID :
 			case UPDATE_ID :

groundStation/src/backend/cmHandler.c

@@ -6,27 +6,21 @@ const char * respParamStrings[MAX_PARAM_COMMANDS] = {
 	"getrollp",
 	"getrolli",
 	"getrolld",
-	"getroll",
 	"getpitchp",
 	"getpitchi",
 	"getpitchd",
-	"getpitch",
 	"getyawp",
 	"getyawi",
 	"getyawd",
-	"getyaw",
 	"getrollratep",
 	"getrollratei",
 	"getrollrated",
-	"getrollrate",
 	"getpitchratep",
 	"getpitchratei",
 	"getpitchrated",
-	"getpitchrate",
 	"getyawratep",
 	"getyawratei",
 	"getyawrated",
-	"getyawrate",
 	"getlatp",
 	"getlati",
 	"getlatd",
@@ -45,27 +39,21 @@ const char * setParamStrings[MAX_PARAM_COMMANDS] = {
 	"setrollp",
 	"setrolli",
 	"setrolld",
-	"setroll",
 	"setpitchp",
 	"setpitchi",
 	"setpitchd",
-	"setpitch",
 	"setyawp",
 	"setyawi",
 	"setyawd",
-	"setyaw",
 	"setrollratep",
 	"setrollratei",
 	"setrollrated",
-	"setrollrate",
 	"setpitchratep",
 	"setpitchratei",
 	"setpitchrated",
-	"setpitchrate",
 	"setyawratep",
 	"setyawratei",
 	"setyawrated",
-	"setyawrate",
 	"setlatp",
 	"setlati",
 	"setlatd",
@@ -77,7 +65,7 @@ const char * setParamStrings[MAX_PARAM_COMMANDS] = {
 	"setheightp",
 	"setheighti",
 	"setheightd",
-	"setheight"
+	"setheight",
 };
 
 
@@ -141,34 +129,57 @@ struct controller_message * stringToCm(const char * string, struct controller_me
 	if (index == -1) {
 		return NULL;
 	} else {
-		cm->value_id = i % MAX_CONTROL_PARAM_ID;
-		if (i <= PARAM_ROLL) {
+		cm->value_id = i % MAX_CONTROL_PARAM_ID; // Default assuming not a setpoint command
+		if (i <= PARAM_ROLL_D) {
 
 			cm->id = ROLL_ID;
-		} else if (i <= PARAM_PITCH) {
+		} else if (i <= PARAM_PITCH_D) {
 
 			cm->id = PITCH_ID;
-		} else if (i <= PARAM_YAW) {
+		} else if (i <= PARAM_YAW_D) {
 
 			cm->id = YAW_ID;
-		} else if (i <= PARAM_ROLL_RATE) {
+		} else if (i <= PARAM_ROLL_RATE_D) {
 
 			cm->id = ROLL_RATE_ID;
-		} else if (i <= PARAM_PITCH_RATE) {
+		} else if (i <= PARAM_PITCH_RATE_D) {
 
 			cm->id = PITCH_RATE_ID;
-		} else if (i <= PARAM_YAW_RATE) {
+		} else if (i <= PARAM_YAW_RATE_D) {
 
 			cm->id = YAW_RATE_ID;
 		} else if (i <= PARAM_LOCAL_X) {
-
-			cm->id = LOCAL_X_ID;
+			// If setpoint command, change the cm->valueid and cm
+			if (i == PARAM_LOCAL_X) {
+				cm->id = X_SETPOINT_ID;
+				cm->value_id = 0;
+			} else {
+				// these have a slightly different value id offset
+				cm->id = LOCAL_X_ID;
+				cm->value_id = MAX_CONTROL_PARAM_ID - (PARAM_LOCAL_X - i);
+			}
 		} else if (i <= PARAM_LOCAL_Y) {
-
-			cm->id = LOCAL_Y_ID;
+			// If setpoint command, change the cm->valueid and cm
+			if (i == PARAM_LOCAL_Y) {
+				cm->id = Y_SETPOINT_ID;
+				cm->value_id = 0;
+			} else {
+				// these have a slightly different value id offset
+				cm->id = LOCAL_Y_ID;
+				cm->value_id = MAX_CONTROL_PARAM_ID - (PARAM_LOCAL_Y - i);
+
+			}
 		} else if (i <= PARAM_ALT) {
-
-			cm->id = ALT_ID;
+			// If setpoint command, change the cm->valueid and cm
+			if (i == PARAM_ALT) {
+				cm->id = ALT_SETPOINT_ID;
+				cm->value_id = 0;
+			} else {
+				// these have a slightly different value id offset
+				cm->id = ALT_ID;
+				cm->value_id = MAX_CONTROL_PARAM_ID - (PARAM_ALT - i);
+
+			}
 		}
 	}
 	return cm;

groundStation/src/backend/cmHandler.h

@@ -7,31 +7,25 @@ enum paramIndices {
 	PARAM_ROLL_P	,
 	PARAM_ROLL_I	,
 	PARAM_ROLL_D	,
-	PARAM_ROLL	,
 	PARAM_PITCH_P ,
 	PARAM_PITCH_I ,
 	PARAM_PITCH_D ,
-	PARAM_PITCH	,
 	PARAM_YAW_P	,
 	PARAM_YAW_I	,
 	PARAM_YAW_D	,
-	PARAM_YAW		,
 	PARAM_ROLL_RATE_P,
 	PARAM_ROLL_RATE_I,
 	PARAM_ROLL_RATE_D,
-	PARAM_ROLL_RATE,
 	PARAM_PITCH_RATE_P,
 	PARAM_PITCH_RATE_I,
 	PARAM_PITCH_RATE_D,
-	PARAM_PITCH_RATE,
 	PARAM_YAW_RATE_P,
 	PARAM_YAW_RATE_I,
 	PARAM_YAW_RATE_D,
-	PARAM_YAW_RATE,
 	PARAM_LOCAL_X_P	,
 	PARAM_LOCAL_X_I	,
 	PARAM_LOCAL_X_D	,
-	PARAM_LOCAL_X		,
+	PARAM_LOCAL_X	,
 	PARAM_LOCAL_Y_P	,
 	PARAM_LOCAL_Y_I	,
 	PARAM_LOCAL_Y_D	,

groundStation/src/backend/commands.h

@@ -60,7 +60,10 @@ enum ControllerID{
 	LOCAL_X_ID,           // 06 - Local X PID
 	LOCAL_Y_ID,           // 07 - Local Y PID
 	ALT_ID,               // 08 - Altitude PID
-	MAX_CONTROLLER_ID,    // 09 - Just used to keep track of the size
+	X_SETPOINT_ID,		  // 09 - X Setpoint
+	Y_SETPOINT_ID,		  // 10 - Y Setpoint
+	ALT_SETPOINT_ID,	  // 11 - Z Setpoint
+	MAX_CONTROLLER_ID     // 12 - Just used to keep track of the size
 };
 
 /*
@@ -70,8 +73,7 @@ enum ControlParamID{
 	KP_ID,                 // 00 - P constant
 	KI_ID,                 // 01 - I constant
 	KD_ID,                 // 02 - D constant
-	SP_ID,                 // 03 - Setpoint value
-	MAX_CONTROL_PARAM_ID,  // 04 - Just used to keep track of the size
+	MAX_CONTROL_PARAM_ID,  // 03 - Just used to keep track of the size
 };
 
 /*