Merge remote branch 'origin/master' into new-uart-circ

Conflicts:
	quad/.gitignore
	quad/sw/modular_quad_pid/.cproject

.gitignore

+test.log
\ No newline at end of file

controls/model/modelParameters.m

     % Model Parameters
-    m = 1.19;            % Quadrotor + battery mass
-    g = 9.81;            % Acceleration of gravity
-    Jxx = 0.0218;        % Quadrotor and battery motor of inertia around bx (pitch)
-    Jyy = 0.0277;        % Quadrotor and battery motor of inertia around by (roll)
-    Jzz = 0.0332;        % Quadrotor and battery motor of inertia around bz (yaw)
-    Jreq = 4.2012e-05;   % Rotor and motor moment of inertia around axis of rotation
-    Kt = 8.1558*10^-6;   % Rotor thrust constant
-    Kh = 0;              % Rotor in-plane drag constant
-    Kd = 1.8087e-07;     % Rotor drag constant
-    rhx = 0.016;         % X-axis distance from center of mass to a rotor hub
-    rhy = 0.016;         % Y-axis distance from center of mass to a rotor hub
-    rhz = 0.003;         % Z-axis distance from center of mass to a rotor hub
-    Rm = 0.2308;         % Motor resistance
-    Kq = 96.3422;        % Motor torque constant
-    Kv = 96.3422;        % Motor back emf constant
-    If = 0.511;          % Motor internal friction current
-    Pmin = 0.40;         % Minimum zybo output duty cycle command
-    Pmax = 0.80;         % Maximum zybo output duty cycle command
-    Tc = 0.01;           % Camera system sampling period
-    tau_c = 0;           % Camera system total latency
-    Vb = 11.1;           % Nominal battery voltage (V)
\ No newline at end of file
+    m = 1.19;                       % Quadrotor + battery mass
+    g = 9.81;                       % Acceleration of gravity
+    Jxx = 0.0218;                   % Quadrotor and battery motor of inertia around bx (pitch)
+    Jyy = 0.0277;                   % Quadrotor and battery motor of inertia around by (roll)
+    Jzz = 0.0332;                   % Quadrotor and battery motor of inertia around bz (yaw)
+    Jreq = 4.2012e-05;              % Rotor and motor moment of inertia around axis of rotation
+    Kt = 8.1558*10^-6;              % Rotor thrust constant
+    Kh = 0;                         % Rotor in-plane drag constant
+    Kd = 1.8087e-07;                % Rotor drag constant
+    rhx = 0.016;                    % X-axis distance from center of mass to a rotor hub
+    rhy = 0.016;                    % Y-axis distance from center of mass to a rotor hub
+    rhz = 0.003;                    % Z-axis distance from center of mass to a rotor hub
+    Rm = 0.2308;                    % Motor resistance
+    Kq = 96.3422;                   % Motor torque constant
+    Kv = 96.3422;                   % Motor back emf constant
+    If = 0.511;                     % Motor internal friction current
+    Pmin = 0.40;                    % Minimum zybo output duty cycle command
+    Pmax = 0.80;                    % Maximum zybo output duty cycle command
+    Tc = 0.01;                      % Camera system sampling period
+    tau_c = 0;                      % Camera system total latency
+    Vb = 11.1;                      % Nominal battery voltage (V)
+    omega_o = 598.088;              % Equilibrium Rotor Speed
+    x_controlled_o = 0;             % Equilibrium lateral controller output
+    y_controlled_o = 0;             % Equilibrium longitudinal controller output
+    yaw_controlled_o = 0;           % Equilibrium yaw controller output
+    
+    % Equilibrium height controller output
+    height_controlled_o = (((Rm*If + ...
+    + (((omega_o * 2 * Rm * Kv * Kq  ...
+    * Kd + 1)^2) - 1)/(4* Rm*Kv^2*Kq ...
+    *Kd))/Vb)*(Pmax- Pmin)+Pmin)*100;     
\ No newline at end of file

groundStation/Makefile

@@ -23,7 +23,7 @@ CLIBINARY=Cli
 CLISRCDIR=src/cli
 CLISOURCES := $(wildcard $(CLISRCDIR)/*.c)
 CLIOBJECTS = $(CLISOURCES:$(CLISRCDIR)/%.c=$(OBJDIR)/%.o)
-SYMLINKS=monitor setpid getpid
+SYMLINKS=monitor setpid getpid setsetpoint
 
 # Frontend-common stuff
 FESRCDIR=src/frontend

groundStation/src/backend/callbacks.c

+#include "commands.h"
+
+/* New stuff - this is nice and clean */
+
+/* Override any callbacks here */
+
+
+/****** LEGACY CODE BE VERY AFRAID ********/
+
+// TAKE THESE OUT WHEN IMPLEMENTING ON THE QUAD SIDE
+float getFloat(unsigned char* str, int pos) {
+	union {
+		float f;
+		int i;
+	} x;
+	x.i = ((str[pos+3] << 24) | (str[pos+2] << 16) | (str[pos+1] << 8) | (str[pos]));
+	return x.f;
+}
+
+int getInt(unsigned char* str, int pos) {
+	int i = ((str[pos+3] << 24) | (str[pos+2] << 16) | (str[pos+1] << 8) | (str[pos]));
+	return i;
+}
+//------------------------------------------------
+
+int cb_debug(unsigned char *packet, int dataLen, modular_structs_t *structs)
+{
+	printf("function for debug\n");
+	return 0;
+}
+
+int cb_update(unsigned char *packet, int dataLen,	modular_structs_t *structs)
+{
+	unsigned char update[28];
+	memcpy(update, ((float *)packet), 28);
+
+	int packetId = getInt(update, 0);
+	float y_pos = getFloat(update, 4);
+	float x_pos = getFloat(update, 8);
+	float alt_pos = getFloat(update, 12);
+	float roll = getFloat(update, 16);
+	float pitch = getFloat(update, 20);
+	float yaw = getFloat(update, 24);
+
+	structs->log_struct.currentQuadPosition.packetId = packetId;
+	structs->log_struct.currentQuadPosition.y_pos = y_pos;
+	structs->log_struct.currentQuadPosition.x_pos = x_pos;
+	structs->log_struct.currentQuadPosition.alt_pos = alt_pos;
+	structs->log_struct.currentQuadPosition.roll = roll;
+	structs->log_struct.currentQuadPosition.pitch = pitch;
+	structs->log_struct.currentQuadPosition.yaw = yaw;
+
+	printf("QUAD: VRPN Packet:");
+	printf("Packet ID: %d\n", packetId);
+	printf("Y Position: %f\n", y_pos);
+	printf("X Position: %f\n", x_pos);
+	printf("Altitude Position: %f\n", alt_pos);
+	printf("Roll: %f\n", roll);
+	printf("Pitch: %f\n", pitch);
+	printf("Yaw: %f\n", yaw);
+
+	printf("function for update\n");
+	return 0;
+}
+
+// Why is this here?
+// This should be on the ground station side
+int logdata(unsigned char *packet, int dataLen, modular_structs_t *structs)
+{
+	printf("Logging: %s\n", packet);
+	return 0;
+}
+
+int response(unsigned char *packet, int dataLen, modular_structs_t *structs)
+{
+	printf("This is the response: %s\n", packet);
+
+	return 0;
+}
+
+// ------------------------------------------------------------------
+
+int setyaw(unsigned char *packet, int dataLen, modular_structs_t *structs)
+{
+	float value;
+
+	memcpy(&value, ((float *)packet), dataLen);
+
+	printf("%f\n", value);
+
+	structs->setpoint_struct.desiredQuadPosition.yaw = value;
+
+	printf("function for setyaw: %f\n", structs->setpoint_struct.desiredQuadPosition.yaw);
+
+	return 0;
+}
+
+int setyawp(unsigned char *packet, int dataLen,  modular_structs_t *structs)
+{
+	float value;
+
+	memcpy(&value, ((float *)packet), dataLen);
+	structs->parameter_struct.yaw_angle_pid.Kp = value;
+
+	printf("function for setyawp: %f\n", structs->parameter_struct.yaw_angle_pid.Kp);
+
+	return 0;
+}
+
+int setyawd(unsigned char *packet, int dataLen,  modular_structs_t *structs)
+{
+	float value;
+
+	memcpy(&value, ((float *)packet), dataLen);
+	structs->parameter_struct.yaw_angle_pid.Kd = value;
+
+	printf("function for setyawd: %f\n", structs->parameter_struct.yaw_angle_pid.Kd);
+
+	return 0;
+}
+
+int setroll(unsigned char *packet, int dataLen,	 modular_structs_t *structs)
+{
+	float value;
+
+	memcpy(&value, ((float *)packet), dataLen);
+	structs->setpoint_struct.desiredQuadPosition.roll = value;
+
+	printf("function for setroll: %f\n", structs->setpoint_struct.desiredQuadPosition.roll);
+
+	return 0;
+}
+
+int setrollp(unsigned char *packet, int dataLen,  modular_structs_t *structs)
+{
+	float value;
+
+	memcpy(&value, ((float *)packet), dataLen);
+	structs->parameter_struct.local_y_pid.Kp = value;
+
+	printf("function for setrollp: %f\n", structs->parameter_struct.local_y_pid.Kp);
+
+	return 0;
+}
+
+int setrolld(unsigned char *packet, int dataLen,  modular_structs_t *structs)
+{
+	float value;
+
+	memcpy(&value, ((float *)packet), dataLen);
+	structs->parameter_struct.local_y_pid.Kd = value;
+
+	printf("function for setrolld: %f\n", structs->parameter_struct.local_y_pid.Kd);
+
+	return 0;
+}
+
+int setpitch(unsigned char *packet, int dataLen,  modular_structs_t *structs)
+{
+	float value;
+
+	memcpy(&value, ((float *)packet), dataLen);
+	structs->setpoint_struct.desiredQuadPosition.pitch = value;
+
+	printf("function for setpitch: %f\n", structs->setpoint_struct.desiredQuadPosition.pitch);
+
+	return 0;
+}
+
+int setpitchp(unsigned char *packet, int dataLen,	modular_structs_t *structs)
+{
+	float value;
+
+	memcpy(&value, ((float *)packet), dataLen);
+	structs->parameter_struct.local_x_pid.Kp = value;
+
+	printf("function for setpitchp: %f\n", structs->parameter_struct.local_x_pid.Kp);
+
+	return 0;
+}
+
+int setpitchd(unsigned char *packet, int dataLen,	modular_structs_t *structs)
+{
+	float value;
+
+	memcpy(&value, ((float *)packet), dataLen);
+	structs->parameter_struct.local_x_pid.Kd = value;
+
+	printf("function for setpitchd: %f\n", structs->parameter_struct.local_x_pid.Kd);
+
+	return 0;
+}
+
+// ------------------------------------------------------------
+// These should be renamed to altitude!
+int setthrottle(unsigned char *packet, int dataLen,	 modular_structs_t *structs)
+{
+	float value;
+
+	memcpy(&value, ((float *)packet), dataLen);
+	structs->setpoint_struct.desiredQuadPosition.alt_pos = value;
+
+	printf("function for setthrottle: %f\n", structs->setpoint_struct.desiredQuadPosition.alt_pos);
+
+	return 0;
+}
+
+int setthrottlep(unsigned char *packet, int dataLen,  modular_structs_t *structs)
+{
+	float value;
+
+	memcpy(&value, ((float *)packet), dataLen);
+	structs->parameter_struct.alt_pid.Kp = value;
+
+	printf("function for setthrottlep: %f\n", structs->parameter_struct.alt_pid.Kp);
+
+	return 0;
+}
+
+int setthrottlei(unsigned char *packet, int dataLen,  modular_structs_t *structs)
+{
+	float value;
+
+	memcpy(&value, ((float *)packet), dataLen);
+	structs->parameter_struct.alt_pid.Ki = value;
+
+	printf("function for setthrottlei: %f\n", structs->parameter_struct.alt_pid.Ki);
+
+	return 0;
+}
+
+int setthrottled(unsigned char *packet, int dataLen,  modular_structs_t *structs)
+{
+	float value;
+
+	memcpy(&value, ((float *)packet), dataLen);
+	structs->parameter_struct.alt_pid.Kd = value;
+
+	printf("function for setthrottled: %f\n", structs->parameter_struct.alt_pid.Kd);
+
+	return 0;
+}
+int getyaw(unsigned char *packet, int dataLen, modular_structs_t *structs){
+	return 0;
+}
+int getyawp(unsigned char *packet, int dataLen, modular_structs_t *structs){
+	return 0;
+}
+int getyawd(unsigned char *packet, int dataLen, modular_structs_t *structs) {
+	return 0;
+}
+int getroll(unsigned char *packet, int dataLen, modular_structs_t *structs){
+	return 0;
+}
+int getrollp(unsigned char *packet, int dataLen, modular_structs_t *structs){
+	return 0;
+}
+int getrolld(unsigned char *packet, int dataLen, modular_structs_t *structs){
+	return 0;
+}
+int getpitch(unsigned char *packet, int dataLen, modular_structs_t *structs){
+	return 0;
+}
+int getpitchp(unsigned char *packet, int dataLen, modular_structs_t *structs){
+	return 0;
+}
+int getpitchd(unsigned char *packet, int dataLen, modular_structs_t *structs){
+	return 0;
+}
+int getthrottle(unsigned char *packet, int dataLen, modular_structs_t *structs){
+	return 0;
+}
+int getthrottlep(unsigned char *packet, int dataLen, modular_structs_t *structs){
+	return 0;
+}
+int getthrottlei(unsigned char *packet, int dataLen, modular_structs_t *structs){
+	return 0;
+}
+int getthrottled(unsigned char *packet, int dataLen, modular_structs_t *structs){
+	return 0;
+}
+
+// These should be renamed to altitude!
+// ------------------------------------------------------------
+
+
+
+
+int getgyro(unsigned char *packet, int dataLen,	 modular_structs_t *structs)
+{
+	printf("function for getgyro\n");
+	return 0;
+}
+
+int getpitchangle(unsigned char *packet, int dataLen,  modular_structs_t *structs)
+{
+	printf("function for getpitchangle\n");
+	return 0;
+}
+
+int getrollangle(unsigned char *packet, int dataLen,  modular_structs_t *structs)
+{
+	printf("function for getrollangle\n");
+	return 0;
+}
+
+
+int getaccel(unsigned char *packet, int dataLen,  modular_structs_t *structs)
+{
+	printf("function for getaccel\n");
+	return 0;
+}
+
+
+int respgyro(unsigned char *packet, int dataLen,  modular_structs_t *structs)
+{
+	printf("function for respgyro\n");
+	return 0;
+}
+
+int resppitchangle(unsigned char *packet, int dataLen,	modular_structs_t *structs)
+{
+	printf("function for resppitchangle\n");
+	return 0;
+}
+
+int resprollangle(unsigned char *packet, int dataLen,  modular_structs_t *structs)
+{
+	printf("function for resprollangle\n");
+	return 0;
+}
+
+
+int respaccel(unsigned char *packet, int dataLen,  modular_structs_t *structs)
+{
+	printf("function for respaccel\n");
+	return 0;
+}
+
+int respyaw(unsigned char *packet, int dataLen, modular_structs_t *structs){
+	return 0;
+}
+int respyawp(unsigned char *packet, int dataLen, modular_structs_t *structs){
+	return 0;
+}
+int respyawd(unsigned char *packet, int dataLen, modular_structs_t *structs) {
+	return 0;
+}
+int resproll(unsigned char *packet, int dataLen, modular_structs_t *structs){
+	return 0;
+}
+int resprollp(unsigned char *packet, int dataLen, modular_structs_t *structs){
+	return 0;
+}
+int resprolld(unsigned char *packet, int dataLen, modular_structs_t *structs){
+	return 0;
+}
+int resppitch(unsigned char *packet, int dataLen, modular_structs_t *structs){
+	return 0;
+}
+int resppitchp(unsigned char *packet, int dataLen, modular_structs_t *structs){
+	return 0;
+}
+int resppitchd(unsigned char *packet, int dataLen, modular_structs_t *structs){
+	return 0;
+}
+int respthrottle(unsigned char *packet, int dataLen, modular_structs_t *structs){
+	return 0;
+}
+int respthrottlep(unsigned char *packet, int dataLen, modular_structs_t *structs){
+	return 0;
+}
+int respthrottlei(unsigned char *packet, int dataLen, modular_structs_t *structs){
+	return 0;
+}
+int respthrottled(unsigned char *packet, int dataLen, modular_structs_t *structs){
+	return 0;
+}

groundStation/src/backend/callbacks.h

+#ifndef __callbacks_h
+#define __callbacks_h
+
+/* Grab some stupid stuff from legacy code */
+#include "type_def.h"
+
+/* Make commands.c happy */
+typedef void (command_cb)(unsigned char *command, 
+		int dataLen, modular_structs_t *structs);
+
+#endif

groundStation/src/backend/cb_default.h

+#include "commands.h"
+
+/* The cb_default used on the groundStation. This file MUST NOT BE INCLUDED
+ * by anything except for commands.c */
+
+/* cb_default used by portable commands.c */
+int cb_default(unsigned char * command, int dataLen, modular_structs_t *structs)
+{
+	    return 0;
+}

groundStation/src/backend/commands.h

@@ -4,8 +4,8 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
-#include "type_def.h"
 
+#include "callbacks.h"
 
 // ----------------------
 // Helper stuff
@@ -31,7 +31,7 @@ struct MessageSubtype{
 	char ID;
 	char cmdText[100];
 	char cmdDataType;
-	int (*functionPtr)(unsigned char *command, int dataLen, modular_structs_t *structs);
+	command_cb * functionPtr;
 };
 
 // MESSAGE TYPES
@@ -40,66 +40,12 @@ struct MessageType{
 	struct MessageSubtype subtypes[MAX_SUBTYPE];
 };
 
-int debug(unsigned char *c, int dataLen, modular_structs_t *structs);
-int update(unsigned char *c, int dataLen, modular_structs_t *structs);
-int logdata(unsigned char *c, int dataLen, modular_structs_t *structs);
-int response(unsigned char *packet, int dataLen, modular_structs_t *structs);
-int setyaw(unsigned char *c, int dataLen, modular_structs_t *structs);
-int setyawp(unsigned char *c, int dataLen, modular_structs_t *structs);
-int setyawd(unsigned char *c, int dataLen, modular_structs_t *structs);
-int setroll(unsigned char *c, int dataLen, modular_structs_t *structs);
-int setrollp(unsigned char *c, int dataLen, modular_structs_t *structs);
-int setrolld(unsigned char *c, int dataLen, modular_structs_t *structs);
-int setpitch(unsigned char *c, int dataLen, modular_structs_t *structs);
-int setpitchp(unsigned char *c, int dataLen, modular_structs_t *structs);
-int setpitchd(unsigned char *c, int dataLen, modular_structs_t *structs);
-int setthrottle(unsigned char *c, int dataLen, modular_structs_t *structs);
-int setthrottlep(unsigned char *c, int dataLen, modular_structs_t *structs);
-int setthrottlei(unsigned char *c, int dataLen, modular_structs_t *structs);
-int setthrottled(unsigned char *c, int dataLen, modular_structs_t *structs);
-int getyaw(unsigned char *c, int dataLen, modular_structs_t *structs);
-int getyawp(unsigned char *c, int dataLen, modular_structs_t *structs);
-int getyawd(unsigned char *c, int dataLen, modular_structs_t *structs);
-int getroll(unsigned char *c, int dataLen, modular_structs_t *structs);
-int getrollp(unsigned char *c, int dataLen, modular_structs_t *structs);
-int getrolld(unsigned char *c, int dataLen, modular_structs_t *structs);
-int getpitch(unsigned char *c, int dataLen, modular_structs_t *structs);
-int getpitchp(unsigned char *c, int dataLen, modular_structs_t *structs);
-int getpitchd(unsigned char *c, int dataLen, modular_structs_t *structs);
-int getthrottle(unsigned char *c, int dataLen, modular_structs_t *structs);
-int getthrottlep(unsigned char *c, int dataLen, modular_structs_t *structs);
-int getthrottlei(unsigned char *c, int dataLen, modular_structs_t *structs);
-int getthrottled(unsigned char *c, int dataLen, modular_structs_t *structs);
-int getaccel(unsigned char *c, int dataLen, modular_structs_t *structs);
-int getgyro(unsigned char *c, int dataLen, modular_structs_t *structs);
-int getpitchangle(unsigned char *c, int dataLen, modular_structs_t *structs);
-int getrollangle(unsigned char *c, int dataLen, modular_structs_t *structs);
-int respaccel(unsigned char *c, int dataLen, modular_structs_t *structs);
-int respgyro(unsigned char *c, int dataLen, modular_structs_t *structs);
-int resppitchangle(unsigned char *c, int dataLen, modular_structs_t *structs);
-int resprollangle(unsigned char *c, int dataLen, modular_structs_t *structs);
-int respyaw(unsigned char *c, int dataLen, modular_structs_t *structs);
-int respyawp(unsigned char *c, int dataLen, modular_structs_t *structs);
-int respyawd(unsigned char *c, int dataLen, modular_structs_t *structs);
-int resproll(unsigned char *c, int dataLen, modular_structs_t *structs);
-int resprollp(unsigned char *c, int dataLen, modular_structs_t *structs);
-int resprolld(unsigned char *c, int dataLen, modular_structs_t *structs);
-int resppitch(unsigned char *c, int dataLen, modular_structs_t *structs);
-int resppitchp(unsigned char *c, int dataLen, modular_structs_t *structs);
-int resppitchd(unsigned char *c, int dataLen, modular_structs_t *structs);
-int respthrottle(unsigned char *c, int dataLen, modular_structs_t *structs);
-int respthrottlep(unsigned char *c, int dataLen, modular_structs_t *structs);
-int respthrottlei(unsigned char *c, int dataLen, modular_structs_t *structs);
-int respthrottled(unsigned char *c, int dataLen, modular_structs_t *structs);
-int respaccel(unsigned char *c, int dataLen, modular_structs_t *structs);
-int respgyro(unsigned char *c, int dataLen, modular_structs_t *structs);
-int resppitchangle(unsigned char *c, int dataLen, modular_structs_t *structs);
-int resprollangle(unsigned char *c, int dataLen, modular_structs_t *structs);
+/* Defined in commands.c */
+extern struct MessageType MessageTypes[MAX_TYPE];
 
+/* Legacy functions - no idea what uses these. Please do not delete. */
 float getFloat(unsigned char* str, int pos);
 int getInt(unsigned char* str, int pos);
+/* end legacy crap */
 
-// TODO add in string to be read from the command line when sending a subtype of message
-extern struct MessageType MessageTypes[MAX_TYPE];
-
-#endif
\ No newline at end of file
+#endif

groundStation/src/cli/cli.h

 #ifndef __CLI_H
 #define __CLI_H
 
+#include "cli_setsetpoint.h"
 #include "cli_monitor.h"
 #include "cli_setpid.h"
 #include "cli_getpid.h"
@@ -11,6 +12,7 @@ enum CommandNameIds{
 	CMD_GETPID,
 	CMD_SETPID,
 	CMD_GETIMU,
+	CMD_SETSETPOINT,
 	MAX_COMMANDS
 };
 
@@ -19,13 +21,16 @@ static cli_function_ptr cli_functions[] = {
 	&cli_monitor,
 	&cli_getpid,
 	&cli_setpid,
-	&cli_getimu
+	&cli_getimu,
+	&cli_setsetpoint
 };
 
 static char* commandNames[MAX_COMMANDS] = {
 	"monitor",
 	"getpid",
 	"setpid",
-	"getimu"
+	"getimu",
+	"setsetpoint"
 };
+
 #endif

groundStation/src/cli/cli_getpid.c

@@ -16,10 +16,10 @@ int cli_getpid(struct backend_conn * conn,	int argc, char **argv) {
  		{"roll",	no_argument,	&getRoll,	1},
  		{"pitch",   no_argument,   	&getPitch, 	1},
  		{"yaw",   no_argument,   	&getYaw, 	1},
- 		{"rollv",   no_argument,   	&getYawV, 	1},
- 		{"pitchv",   no_argument,   	&getYawV, 	1},
+ 		{"rollv",   no_argument,   	&getRollV, 	1},
+ 		{"pitchv",   no_argument,   &getPitchV, 1},
  		{"yawv",   no_argument,   	&getYawV, 	1},
- 		{"height",   no_argument,   	&getHeight,	1},
+ 		{"height",   no_argument,   &getHeight,	1},
  		{"lat",   no_argument,   	&getLat, 	1},
  		{"long",   no_argument,   	&getLong, 	1},
  		{"help",	no_argument,	&needHelp,	1},
@@ -43,17 +43,13 @@ int cli_getpid(struct backend_conn * conn,	int argc, char **argv) {
 	}
 
 	if (needHelp) {
-		printf("Getpid gets the p, i , or d constant values of any single controller\n");
+		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");
-		printf("Available 'controller' options include the following\n");
-		printf("\t[-p] 'val' : Gets the p constant of the 'controller\n");
-		printf("\t[-i] 'val' : Gets the i constant of the 'controller'\n");
-		printf("\t[-d] 'val' : Gets the d constant of the 'controller'\n");
 		return 0;
 	}
 
@@ -64,17 +60,11 @@ int cli_getpid(struct backend_conn * conn,	int argc, char **argv) {
 	
 	int result;
 	if(getAll) {
-		pid_data.controller = PID_ROLL;
-		if ((result = getValues(conn, &pid_data))) {
-			return result;
-		}
-		pid_data.controller = PID_PITCH;
-		if ((result = getValues(conn, &pid_data))) {
-			return result;
-		}
-		pid_data.controller = PID_YAW;
-		if ((result = getValues(conn, &pid_data))) {
-			return result;
+		for(int i = 0; i < PID_NUM_PIDS; ++i) {
+			pid_data.controller = i;
+			if ((result = getValues(conn, &pid_data))) {
+				return result;
+			}
 		}
 	} else {
 		if(getPitch) {
@@ -95,6 +85,43 @@ int cli_getpid(struct backend_conn * conn,	int argc, char **argv) {
 				return result;
 			}
 		}
+		if(getPitchV) {
+			pid_data.controller = PID_PITCH_RATE;
+			if ((result = getValues(conn, &pid_data))) {
+				return result;
+			}
+		}
+		if(getRollV) {
+			pid_data.controller = PID_ROLL_RATE;
+			if ((result = getValues(conn, &pid_data))) {
+				return result;
+			}
+		}
+		if(getYawV) {
+			pid_data.controller = PID_YAW_RATE;
+			if ((result = getValues(conn, &pid_data))) {
+				return result;
+			}
+		}
+		if(getHeight) {
+			pid_data.controller = PID_HEIGHT;
+			if ((result = getValues(conn, &pid_data))) {
+				return result;
+			}
+		}
+		if(getLat) {
+			pid_data.controller = PID_LAT;
+			if ((result = getValues(conn, &pid_data))) {
+				return result;
+			}
+		}
+		if(getLong) {
+			pid_data.controller = PID_LONG;
+			if ((result = getValues(conn, &pid_data))) {
+				return result;
+			}
+		}
+
 	}
 
 	return 0;
@@ -105,8 +132,6 @@ int getValues(struct backend_conn * conn, struct frontend_pid_data * pid_data) {
 		return 1;
 	}
 
-	return 0;
-
 	switch(pid_data->controller) {
 		case PID_PITCH :
 			printf("Pitch Constants: P = %f\tI = %f\tD = %f\n",
@@ -120,6 +145,30 @@ int getValues(struct backend_conn * conn, struct frontend_pid_data * pid_data) {
 			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;
 	}

groundStation/src/cli/cli_setpid.h

-#ifndef _CLI_SETPID_H
-#define _CLI_SETPID_H
+#ifndef CLI_SETPID_H
+#define CLI_SETPID_H
 
 #include "frontend_setpid.h"
 

groundStation/src/cli/cli_setsetpoint.c

+#include <stdio.h>
+#include <unistd.h>
+#include <getopt.h>
+
+#include "cli_setsetpoint.h"
+
+int cli_set(struct backend_conn * conn,	int argc, char **argv) {
+	int c;
+	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},
+ 		{0, 0, 0, 0}
+ 	};
+
+ 	while (1)
+	{
+		/* getopt_long stores the option index here. */
+		int option_index = 0;
+
+		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) {
+		printf("Setsetpoint sets the x, y , or z 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[-x] 'val' : Sets the x setpoint value to 'val'\n");
+		printf("\t[-z] 'val' : Sets the y setpoint value to 'val'\n");
+		printf("\t[-z] 'val' : Sets the z 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;
+}

groundStation/src/cli/cli_setsetpoint.h

+#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 */

groundStation/src/frontend/frontend_getpid.c

@@ -25,13 +25,31 @@ int frontend_getpid(
 	char line[100] = "";
 	switch (pid_data->controller) {
 		case PID_PITCH :
-			strncpy(line, "getpitchp\ngetpitchd\n", 20);
+			strncpy(line, "getpitchp\ngetpitchi\ngetpitchd\n", 30);
 			break;
 		case PID_ROLL :
-			strncpy(line, "getrollp\ngetrolld\n", 18);
+			strncpy(line, "getrollp\ngetrolli\ngetrolld\n", 27);
 			break;
 		case PID_YAW :
-			strncpy(line, "getyawp\ngetyawd\n", 17);
+			strncpy(line, "getyawp\ngetyawi\ngetyawd\n", 24);
+			break;
+		case PID_PITCH_RATE :
+			strncpy(line, "getpitchratep\ngetpitchratei\ngetpitchrated\n", 33);
+			break;
+		case PID_ROLL_RATE :
+			strncpy(line, "getrollratep\ngetrollratei\ngetrollrated\n", 30);
+			break;
+		case PID_YAW_RATE :
+			strncpy(line, "getyawratep\ngetyawratei\ngetyawrated\n", 27);
+			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;

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"
@@ -12,6 +12,9 @@ int frontend_setpid(
 #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
+#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
+
+#ifndef SET_ALL
+#define SET_ALL (SET_X | SET_Y | SET_Z)
+#endif /* SET_ALL */
+
+#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

quad/.gitignore

-sw/modular_quad_pid/Debug
-sw/.metadata/
-sw/SDK.log
+zybo_fsbl_bsp/
 test.log