ready to test

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);
 
@@ -733,7 +733,7 @@ static void quad_recv() {
 
 	// printf("packet = '");
 	// for(int i = 0; i < (int)respBufLen; ++i) {
-	// 	printf(" 0x%.2x ", respBuf[i]);
+	// 	printf(" %.2x ", respBuf[i]);
 	// }
 	// printf("'\n");
 
@@ -760,7 +760,6 @@ static void quad_recv() {
 					respBufLen -=i;
 					return;
 				}
-
 			}
 			respBufLen = 0;
 			return;
@@ -777,7 +776,6 @@ static void quad_recv() {
 
 		switch (m.msg_type) {
 			case DEBUG_ID :
-				printf();
 			case PACKETLOG_ID :
 			case GETPACKETLOGS_ID :
 			case UPDATE_ID :
@@ -814,12 +812,12 @@ static void handleResponseParam(struct metadata *m, uint8_t * data)
 		warnx("DecodeResponse error");
 		return;
 	}
-
 	char buffer[128];
 
 	const char * message = cmToString(RESPPARAM_ID, &cm);
 
 	size_t len = snprintf(buffer, 128, "%s %f\n", message, cm.value);
+	printf("response = %s\n", buffer);
 
 	for(int fd = 0; fd <= max_fd; ++fd) {
 		if (get_client_index(fd) > -1) {

groundStation/src/backend/cmHandler.c

@@ -57,21 +57,30 @@ const char * setParamStrings[MAX_PARAM_COMMANDS] = {
 	"setlatp",
 	"setlati",
 	"setlatd",
-	"setlat",
 	"setlongp",
 	"setlongi",
 	"setlongd",
-	"setlong",
 	"setheightp",
 	"setheighti",
 	"setheightd",
+	"setlat",
+	"setlong",
 	"setheight",
 };
 
-
 const char * cmToString(int msgType, const struct controller_message *cm) 
 {
-	size_t index = (cm->id * MAX_CONTROL_PARAM_ID)  + cm->value_id;
+	size_t index;
+	if (cm->id == X_SETPOINT_ID) {
+		index =  PARAM_LOCAL_X; // This will change when we make node structure changes
+	} else if (cm->id == X_SETPOINT_ID) {
+		index =  PARAM_LOCAL_Y; // This will change when we make node structure changes
+	} else if (cm->id == X_SETPOINT_ID) {
+		index =  PARMA_ALT; // This will change when we make node structure changes
+	} else {
+		index = (cm->id * MAX_CONTROL_PARAM_ID)  + cm->value_id;
+		printf("index = %lu\n", index);
+	}	
 	switch (msgType) {
 		case DEBUG_ID :
 		case PACKETLOG_ID :
@@ -148,39 +157,29 @@ struct controller_message * stringToCm(const char * string, struct controller_me
 		} else if (i <= PARAM_YAW_RATE_D) {
 
 			cm->id = YAW_RATE_ID;
-		} else if (i <= PARAM_LOCAL_X) {
-			// 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) {
-			// 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) {
-			// 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);
-
-			}
+		} else if (i <= PARAM_LOCAL_X_D) {
+
+			cm->id = LOCAL_X_ID;
+		} else if (i <= PARAM_LOCAL_Y_D) {
+
+			cm->id = LOCAL_Y_ID;
+		} else if (i <= PARAM_ALT_D) {
+
+			cm->id = ALT_ID;
+		} else if ( i == PARAM_LOCAL_X) {
+
+			cm->id = X_SETPOINT_ID;
+			cm->value_id = 0;
+		} else if ( i == PARAM_LOCAL_Y) {
+			
+			cm->id = Y_SETPOINT_ID;
+			cm->value_id = 0;
+		} else if ( i == PARAM_ALT) {
+			
+			cm->id = ALT_SETPOINT_ID;
+			cm->value_id = 0;
 		}
 	}
+	// printf("cm->id = %d\ncm->value_id = %d\n", cm->id, cm->value_id);
 	return cm;
 }
\ No newline at end of file

groundStation/src/backend/cmHandler.h

@@ -25,14 +25,14 @@ enum paramIndices {
 	PARAM_LOCAL_X_P	,
 	PARAM_LOCAL_X_I	,
 	PARAM_LOCAL_X_D	,
-	PARAM_LOCAL_X	,
 	PARAM_LOCAL_Y_P	,
 	PARAM_LOCAL_Y_I	,
 	PARAM_LOCAL_Y_D	,
-	PARAM_LOCAL_Y	,
 	PARAM_ALT_P,
 	PARAM_ALT_I,
 	PARAM_ALT_D,
+	PARAM_LOCAL_X	,
+	PARAM_LOCAL_Y	,
 	PARAM_ALT	,
 	MAX_PARAM_COMMANDS
 };

groundStation/src/cli/cli_getsetpoint.c

@@ -114,6 +114,7 @@ int getSetPointValues(struct backend_conn * conn, struct frontend_setpoint_data
 		case LONGG :
 			printf("Longitude: %f\n",
 				setpoint_data->longg);
+			break;
 		case PITCH :
 			printf("Pitch: %f\n",
 				setpoint_data->pitch);
@@ -121,9 +122,11 @@ int getSetPointValues(struct backend_conn * conn, struct frontend_setpoint_data
 		case ROLL :
 			printf("Roll: %f\n",
 				setpoint_data->roll);
+			break;
 		case YAW :
 			printf("Yaw: %f\n",
 				setpoint_data->yaw);
+			break;
 		default :
 			break;
 	}

groundStation/src/frontend/frontend_getsetpoint.c

 #include <stdio.h>
 #include <string.h>
 #include <stdlib.h>
+#include <err.h>
 
 #include "frontend_getsetpoint.h"
 #include "setpoint_common.h"
@@ -48,5 +49,46 @@ int frontend_getsetpoint(
 		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;
 }