Working with all commands + controller network

groundStation/scripts/andy_params.txt

+lat p -0.0081
+lat i -0.000104
+lat d -0.157
+long p 0.0081
+long i 0.000104
+long d 0.157
+height p 1095.83
+height i 70.08
+height d 4284.1
+pitch p 3.355
+pitch i 0.0
+pitch d 0.0
+roll p 3.355
+roll i 0.0
+roll d 0.0
+yaw p 7.423
+yaw i 0.0
+yaw d 0.0
+pitchv p 3375.4
+pitchv i 0.0
+pitchv d 0.0
+rollv p 3375.4
+rollv i 0.0
+rollv d 0.0
+yawv p 47344.3
+yawv i 0.0
+yawv d 0.0

groundStation/scripts/setpid_batch.sh

@@ -14,4 +14,4 @@ regex='(lat|long|height|pitch|roll|yaw|pitchv|rollv|yawv) (p|i|d) ([-+]?[0-9]*\.
 awk_prog='{print "./Cli setpid" " " "--" $1 " " "-" $2 " " $3}'
 
 # Search, parse, and execute ./Cli commands
-grep -E "$regex" $filename | awk "$awk_prog" | bash
+grep -E "$regex" $filename | awk "$awk_prog" | bash
\ No newline at end of file

groundStation/scripts/tuning.txt

+lat p -0.01
+lat i 0
+lat d -0.05
+long p 0.01
+long i 0
+long d 0.05

groundStation/src/backend/backend.c

@@ -127,10 +127,7 @@ modular_structs_t structs;
 // 		in the future.
 static void cb(struct ucart_vrpn_TrackerData * td) {
 	static int count = 0;
-
-	if(!(count % 10)) {
-		sendVrpnPacket(td);
-	}
+	sendVrpnPacket(td);
 	count++;
 }
 
@@ -694,7 +691,7 @@ static void client_recv(int fd) {
 				}
 			}
 
-			// printf("packet = '");
+			// printf("packetToQuad = '");
 			// for(int i = 0; i < (int)psize; ++i) {
 			// 	printf(" %.2x ", packet[i]);
 			// }
@@ -704,7 +701,6 @@ static void client_recv(int fd) {
 
 		}
 
-		
 		char * rest = &buffer[newline] + 1;
 		size_t restLen = (strlen(rest) == 0) ? 1 : strlen(rest);
 		/* Delete parsed data and move the rest to the left */
@@ -731,7 +727,7 @@ static void quad_recv() {
 	}
 	respBufLen += respLen;
 
-	// printf("packet = '");
+	// printf("packetFromQuad = '");
 	// for(int i = 0; i < (int)respBufLen; ++i) {
 	// 	printf(" %.2x ", respBuf[i]);
 	// }
@@ -801,8 +797,7 @@ static void quad_recv() {
 			default:
 				printf("(Backend): message type %d unrecognized\n", m.msg_type);
 		}
-	}
-	
+	}	
 }
 
 static void handleResponseParam(struct metadata *m, uint8_t * data)
@@ -817,7 +812,6 @@ static void handleResponseParam(struct metadata *m, uint8_t * data)
 	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

@@ -24,14 +24,14 @@ const char * respParamStrings[MAX_PARAM_COMMANDS] = {
 	"getlatp",
 	"getlati",
 	"getlatd",
-	"getlat",
 	"getlongp",
 	"getlongi",
 	"getlongd",
-	"getlong",
 	"getheightp",
 	"getheighti",
 	"getheightd",
+	"getlat",
+	"getlong",
 	"getheight"
 };
 
@@ -76,10 +76,9 @@ const char * cmToString(int msgType, const struct controller_message *cm)
 	} 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
+		index =  PARAM_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 :
@@ -133,6 +132,7 @@ struct controller_message * stringToCm(const char * string, struct controller_me
 			break;	
 		}
 	}
+	// printf("index = %d\n", index);
 
 	/* Error, nothing found */
 	if (index == -1) {
@@ -167,7 +167,7 @@ struct controller_message * stringToCm(const char * string, struct controller_me
 
 			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) {

groundStation/src/backend/commands.h

@@ -57,11 +57,11 @@ enum ControllerID{
 	ROLL_RATE_ID,         // 03 - Roll rate PID
 	PITCH_RATE_ID,        // 04 - Pitch rate PID
 	YAW_RATE_ID,          // 05 - Yaw rate PID
-	LOCAL_X_ID,           // 06 - Local X PID
-	LOCAL_Y_ID,           // 07 - Local Y PID
-	ALT_ID,               // 08 - Altitude PID
-	X_SETPOINT_ID,		  // 09 - X Setpoint
-	Y_SETPOINT_ID,		  // 10 - Y Setpoint
+	LOCAL_X_ID,           // 06 - Local X PID // lat
+	LOCAL_Y_ID,           // 07 - Local Y PID  //long
+	ALT_ID,               // 08 - Altitude PID 
+	X_SETPOINT_ID,		  // 09 - X Setpoint //lat
+	Y_SETPOINT_ID,		  // 10 - Y Setpoint //long
 	ALT_SETPOINT_ID,	  // 11 - Z Setpoint
 	MAX_CONTROLLER_ID     // 12 - Just used to keep track of the size
 };

groundStation/src/cli/cli_monitor.c

@@ -143,7 +143,7 @@ int monitor(struct backend_conn * conn) {
 	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);
+			td.pitch, td.roll, td.yaw); //TODO: Tracker data long and lat are backwards
 	
 	monitorcounter++;
 	pidcounter = ((pidcounter + 1)  % PID_NUM_PIDS);

groundStation/src/cli/cli_setpid.c

@@ -93,9 +93,9 @@ int cli_setpid(struct backend_conn * conn,	int argc, char **argv) {
 	} else if (setHeight) {
 		pid_data.controller = PID_HEIGHT;
 	} else if (setLong) {
-		pid_data.controller = PID_LAT;
-	} else if (setLat) {
 		pid_data.controller = PID_LONG;
+	} else if (setLat) {
+		pid_data.controller = PID_LAT;
 	}
 
 	frontend_setpid(conn, &pid_data, mask);

groundStation/src/frontend/pid_common.h

@@ -9,8 +9,8 @@ enum pid_controller {
 	PID_ROLL_RATE,
 	PID_YAW_RATE,
 	PID_HEIGHT, /* Z */
-	PID_LONG, /* Y */
 	PID_LAT, /* X */
+	PID_LONG, /* Y */
 	PID_NUM_PIDS
 };