diff --git a/quad/sw/modular_quad_pid/src/callbacks.c b/quad/sw/modular_quad_pid/src/callbacks.c
new file mode 100644
index 0000000000000000000000000000000000000000..19cbc568ce37c4a663dfd667d8aaec7db3f55e02
--- /dev/null
+++ b/quad/sw/modular_quad_pid/src/callbacks.c
@@ -0,0 +1,341 @@
+#include "communication.h"
+#include "commands.h"
+#include "type_def.h"
+#include "uart.h"
+
+int debug(modular_structs_t *structs)
+{
+ printf("function for debug\n");
+ return 0;
+}
+
+static int n_msg_received = 0;
+static size_t total_payload_received = 0;
+
+int cb_packetlog(modular_structs_t* structs) {
+ n_msg_received += 1;
+ total_payload_received += uart_buff_data_length();
+ return 0;
+}
+
+int cb_getpacketlogs(modular_structs_t* structs) {
+ char buf[255];
+
+ // Message logging number of messages received and size of payload received
+ int length = snprintf(buf, sizeof buf, "%d,%d", n_msg_received, total_payload_received);
+
+ send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
+ return 0;
+}
+
+/* Handles receiving new location updates */
+int cb_update(modular_structs_t *structs)
+{
+ //processUpdate(packet, &(structs->raw_sensor_struct.currentQuadPosition));
+
+ quadPosition_t* currentQuadPosition = &(structs->raw_sensor_struct.currentQuadPosition);
+ // Packet must come as [NEARPY], 4 bytes each
+ int packetId = uart_buff_data_get_u32(0);
+// printf("Packet ID: %d\n", packetId);
+ float y_pos = uart_buff_data_get_float(4);
+// printf("y_pos: %f\n", y_pos);
+ float x_pos = uart_buff_data_get_float(8);
+// printf("x_pos: %f\n", x_pos);
+ float alt_pos = uart_buff_data_get_float(12);
+// printf("alt_pos: %f\n", alt_pos);
+ float roll = uart_buff_data_get_float(16);
+// printf("roll: %f\n", roll);
+ float pitch = uart_buff_data_get_float(20);
+// printf("pitch: %f\n", pitch);
+ float yaw = uart_buff_data_get_float(24);
+// printf("yaw: %f\n", yaw);
+
+ currentQuadPosition->packetId = packetId;
+ currentQuadPosition->y_pos = y_pos;
+ currentQuadPosition->x_pos = x_pos;
+ currentQuadPosition->alt_pos = alt_pos;
+ currentQuadPosition->roll = roll;
+ currentQuadPosition->pitch = pitch;
+ currentQuadPosition->yaw = yaw;
+
+ // Make location as fresh
+ structs->user_input_struct.locationFresh = 1;
+
+ return 0;
+}
+
+// This is called on the ground station to begin sending VRPN to the quad
+int beginupdate(modular_structs_t *structs) {
+ structs->user_input_struct.receivedBeginUpdate = 1;
+ return 0;
+}
+
+int logdata(modular_structs_t *structs)
+{
+ size_t length;
+ unsigned char *packet = uart_buff_get_raw(&length);
+ printf("Logging: %s\n", packet);
+ return 0;
+}
+
+int response(modular_structs_t *structs)
+{
+ size_t length;
+ char *packet = uart_buff_get_raw(&length);
+ printf("This is the response: %s\n", packet);
+ return 0;
+}
+
+// ------------------------------------------------------------------
+// Quad side implementation
+
+// TODO: Erase memory leaks
+
+int yawset(modular_structs_t *structs)
+{
+ char buf[255] = {};
+
+ structs->setpoint_struct.desiredQuadPosition.yaw = uart_buff_data_get_float(0);
+
+ // Debug print statement
+ //printf("function for yawset: %f\n", structs->setpoint_struct.desiredQuadPosition.yaw);
+
+ // Send a reply to the ground station
+ int length = snprintf(buf, sizeof(buf), "Successfully set desired yaw to %.2f radians\r\n", structs->setpoint_struct.desiredQuadPosition.yaw);
+
+ send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
+ return 0;
+}
+
+int yawp(modular_structs_t *structs)
+{
+ char buf[255] = {0};
+
+ structs->parameter_struct.yaw_angle_pid.Kp = uart_buff_data_get_float(0);
+
+ printf("function for yawp: %f\n", structs->parameter_struct.yaw_angle_pid.Kp);
+
+ // Send a reply to the ground station
+ int length = snprintf(buf, sizeof(buf), "Successfully set yaw Kp to %.2f\r\n", structs->parameter_struct.yaw_angle_pid.Kp);
+
+ send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
+ return 0;
+}
+
+int yawd(modular_structs_t *structs)
+{
+ char buf[255] = {};
+
+ structs->parameter_struct.yaw_angle_pid.Kd = uart_buff_data_get_float(0);
+
+ printf("function for yawd: %f\n", structs->parameter_struct.yaw_angle_pid.Kd);
+
+ // Send a reply to the ground station
+ int length = snprintf(buf, sizeof(buf), "Successfully set yaw Kd to %.2f\r\n", structs->parameter_struct.yaw_angle_pid.Kd);
+
+ send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
+ return 0;
+}
+
+int rollset(modular_structs_t *structs)
+{
+ char buf[255] = {};
+
+ structs->setpoint_struct.desiredQuadPosition.roll = uart_buff_data_get_float(0);
+
+ printf("function for rollset: %f\n", structs->setpoint_struct.desiredQuadPosition.roll);
+
+ // Send a reply to the ground station
+ int length = snprintf(buf, sizeof(buf), "Successfully set desired roll to %.2f radians\r\n", structs->setpoint_struct.desiredQuadPosition.roll);
+
+ send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
+ return 0;
+}
+
+int rollp(modular_structs_t *structs)
+{
+ char buf[255] = {};
+
+ structs->parameter_struct.roll_angle_pid.Kp = uart_buff_data_get_float(0);
+
+ printf("function for rollp: %f\n", structs->parameter_struct.roll_angle_pid.Kp);
+
+ // Send a reply to the ground station
+ size_t length = snprintf(buf, sizeof(buf), "Successfully set roll Kp to %.2f\r\n", structs->parameter_struct.roll_angle_pid.Kp);
+
+ send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
+
+ return 0;
+}
+
+int rolld(modular_structs_t *structs)
+{
+ char buf[255] = {};
+
+ structs->parameter_struct.roll_angle_pid.Kd = uart_buff_data_get_float(0);
+
+ printf("function for rolld: %f\n", structs->parameter_struct.roll_angle_pid.Kd);
+
+ // Send a reply to the ground station
+ size_t length = snprintf(buf, sizeof(buf), "Successfully set roll Kd to %.2f\r\n", structs->parameter_struct.roll_angle_pid.Kd);
+
+ send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
+ return 0;
+}
+
+int pitchset(modular_structs_t *structs)
+{
+ char buf[255] = {};
+
+ structs->setpoint_struct.desiredQuadPosition.pitch = uart_buff_data_get_float(0);
+
+ printf("function for pitchset: %f\n", structs->setpoint_struct.desiredQuadPosition.pitch);
+
+ // Send a reply to the ground station
+ int length = snprintf(buf, sizeof(buf), "Successfully set desired pitch to %.2f radians\r\n", structs->setpoint_struct.desiredQuadPosition.pitch);
+
+ send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
+ return 0;
+}
+
+int pitchp(modular_structs_t *structs)
+{
+ char buf[255] = {};
+
+ structs->parameter_struct.pitch_angle_pid.Kp = uart_buff_data_get_float(0);
+
+ printf("function for pitchp: %f\n", structs->parameter_struct.pitch_angle_pid.Kp);
+
+ // Send a reply to the ground station
+ int length = snprintf(buf, sizeof(buf), "Successfully set pitch Kp to %.2f\r\n", structs->parameter_struct.pitch_angle_pid.Kp);
+
+ send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
+ return 0;
+}
+
+int pitchd(modular_structs_t *structs)
+{
+ char buf[255] = {};
+
+ structs->parameter_struct.pitch_angle_pid.Kd = uart_buff_data_get_float(0);
+
+ printf("function for pitchd: %f\n", structs->parameter_struct.pitch_angle_pid.Kd);
+
+ // Send a reply to the ground station
+ int length = snprintf(buf, sizeof(buf), "Successfully set desired yaw to %.2f\r\n", structs->parameter_struct.pitch_angle_pid.Kd);
+
+ send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
+ return 0;
+}
+
+// ------------------------------------------------------------
+// These should be renamed to altitude!
+int throttleset(modular_structs_t *structs)
+{
+ char buf[255] = {};
+
+ structs->setpoint_struct.desiredQuadPosition.alt_pos = uart_buff_data_get_float(0);
+
+ printf("function for throttleset: %f\n", structs->setpoint_struct.desiredQuadPosition.alt_pos);
+
+ // Send a reply to the ground station
+ int length = snprintf(buf, sizeof(buf), "Successfully set desired altitude to %.2f meters\r\n", structs->setpoint_struct.desiredQuadPosition.alt_pos);
+
+ send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
+ return 0;
+}
+
+int throttlep(modular_structs_t *structs)
+{
+ char buf[255] = {};
+
+ structs->parameter_struct.alt_pid.Kp = uart_buff_data_get_float(0);
+
+ printf("function for throttlep: %f\n", structs->parameter_struct.alt_pid.Kp);
+
+ // Send a reply to the ground station
+ int length = snprintf(buf, sizeof(buf), "Successfully set alt Kp to %.2f\r\n", structs->parameter_struct.alt_pid.Kp);
+
+ send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
+ return 0;
+}
+
+int throttlei(modular_structs_t *structs)
+{
+ char buf[255] = {};
+
+ structs->parameter_struct.alt_pid.Ki = uart_buff_data_get_float(0);
+
+ printf("function for throttlei: %f\n", structs->parameter_struct.alt_pid.Ki);
+
+ // Send a reply to the ground station
+ int length = snprintf(buf, sizeof(buf), "Successfully set alt Ki to %.2f\r\n", structs->parameter_struct.alt_pid.Ki);
+
+ send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
+ return 0;
+}
+
+int throttled(modular_structs_t *structs)
+{
+ char buf[255] = {};
+
+ structs->parameter_struct.alt_pid.Kd = uart_buff_data_get_float(0);
+
+ printf("function for throttled: %f\n", structs->parameter_struct.alt_pid.Kd);
+
+ // Send a reply to the ground station
+ int length = snprintf(buf, sizeof(buf), "Successfully set alt Kd to %.2f\r\n", structs->parameter_struct.alt_pid.Kd);
+
+ send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
+ return 0;
+}
+// These should be renamed to altitude!
+// ------------------------------------------------------------
+
+int accelreq(modular_structs_t *structs)
+{
+ printf("function for accelreq\n");
+ return 0;
+}
+
+int gyroresp(modular_structs_t *structs)
+{
+ printf("function for accelreq\n");
+ return 0;
+}
+
+int pitchangleresp(modular_structs_t *structs)
+{
+ printf("function for accelreq\n");
+ return 0;
+}
+
+int rollangleresp(modular_structs_t *structs)
+{
+ printf("function for accelreq\n");
+ return 0;
+}
+
+int gyroreq(modular_structs_t *structs)
+{
+ printf("function for accelreq\n");
+ return 0;
+}
+
+int pitchanglereq(modular_structs_t *structs)
+{
+ printf("function for accelreq\n");
+ return 0;
+}
+
+int rollanglereq(modular_structs_t *structs)
+{
+ printf("function for accelreq\n");
+ return 0;
+}
+
+int accelresp(modular_structs_t *structs)
+{
+ printf("function for accelreq\n");
+ return 0;
+}
diff --git a/quad/sw/modular_quad_pid/src/callbacks.h b/quad/sw/modular_quad_pid/src/callbacks.h
new file mode 100644
index 0000000000000000000000000000000000000000..1afa1b659aded1a54827554c02f22fa8d52cddc6
--- /dev/null
+++ b/quad/sw/modular_quad_pid/src/callbacks.h
@@ -0,0 +1,10 @@
+#ifndef __callbacks_
+#define __callbacks_h
+
+/* Grab some stupid stuff from legacy code */
+#include "type_def.h"
+
+/* Make commands.c happy */
+typedef void (command_cb)(modular_structs_t *structs);
+
+#endif
diff --git a/quad/sw/modular_quad_pid/src/cb_default.h b/quad/sw/modular_quad_pid/src/cb_default.h
new file mode 100644
index 0000000000000000000000000000000000000000..99982e63f73c37f9fdd1175bdf94c1f4b268a6c9
--- /dev/null
+++ b/quad/sw/modular_quad_pid/src/cb_default.h
@@ -0,0 +1,10 @@
+#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(modular_structs_t *structs)
+{
+ return 0;
+}
diff --git a/quad/sw/modular_quad_pid/src/commands.c b/quad/sw/modular_quad_pid/src/commands.c
deleted file mode 100644
index a57f6fefae851f371378355fcfec04e289fad3b2..0000000000000000000000000000000000000000
--- a/quad/sw/modular_quad_pid/src/commands.c
+++ /dev/null
@@ -1,725 +0,0 @@
-#include "communication.h"
-#include "commands.h"
-#include "type_def.h"
-#include "uart.h"
-
-struct MessageType MessageTypes[MAX_TYPE] =
-{
- // DEBUG
- {
- // Message Type ID
- 0x00,
-
- // Debug Subtypes
- {
- // NONE subtype
- {
- // ID
- 0x00,
- // Command text
- "debug",
- // Type of the command data
- stringType,
- // Function pointer
- &debug
- },
-
- // Echo subtype
- {
- // ID
- 0x01,
- // Command text
- "ack",
- // Type of the command data
- intType,
- // Function pointer
- &debug
- },
- // Log packet receiving
- {
- // ID
- 0x02,
- // Command text
- "packetlog",
- // Type of the command data
- stringType,
- // Function pointer
- &packetlog
- },
- // Get packet receiving logs
- {
- // ID
- 0x03,
- // Command text
- "getpacketlogs",
- // Type of the command data
- stringType,
- // Function pointer
- &getpacketlogs
- }
- }
- },
-
- // CALIBRATION
- {
- // Message Type ID
- 0x01,
-
- // Calibration Subtypes
- {
- // yaw setpoint subtype
- {
- // ID
- 0x00,
- // Command text
- "setyaw",
- // Type of the command data
- floatType,
- // Function pointer
- &yawset
- },
- // yaw p constant subtype
- {
- // ID
- 0x01,
- // Command text
- "setyawp",
- // Type of the command data
- floatType,
- // Function pointer
- &yawp
- },
- // yaw d constant subtype
- {
- // ID
- 0x02,
- // Command text
- "setyawd",
- // Type of the command data
- floatType,
- // Function pointer
- &yawd
- },
- // roll setpoint subtype
- {
- // ID
- 0x03,
- // Command text
- "setroll",
- // Type of the command data
- floatType,
- // Function pointer
- &rollset
- },
- // roll p constant subtype
- {
- // ID
- 0x04,
- // Command text
- "setrollp",
- // Type of the command data
- floatType,
- // Function pointer
- &rollp
- },
- // roll d constant subtype
- {
- // ID
- 0x05,
- // Command text
- "setrolld",
- // Type of the command data
- floatType,
- // Function pointer
- &rolld
- },
- // pitch setpoint subtype
- {
- // ID
- 0x06,
- // Command text
- "setpitch",
- // Type of the command data
- floatType,
- // Function pointer
- &pitchset
- },
- // pitch p constant subtype
- {
- // ID
- 0x07,
- // Command text
- "setpitchp",
- // Type of the command data
- floatType,
- // Function pointer
- &pitchp
- },
- // pitch d constant subtype
- {
- // ID
- 0x08,
- // Command text
- "setpitchd",
- // Type of the command data
- floatType,
- // Function pointer
- &pitchd
- },
- // throttle setpoint subtype
- {
- // ID
- 0x09,
- // Command text
- "setthrottle",
- // Type of the command data
- floatType,
- // Function pointer
- &throttleset
- },
- // throttle p constant subtype
- {
- // ID
- 0x0A,
- // Command text
- "setthrottlep",
- // Type of the command data
- floatType,
- // Function pointer
- &throttlep
- },
- // throttle i constant subtype
- {
- // ID
- 0x0B,
- // Command text
- "setthrottlei",
- // Type of the command data
- floatType,
- // Function pointer
- &throttlei
- },
- // throttle d constant subtype
- {
- // ID
- 0x0C,
- // Command text
- "setthrottled",
- // Type of the command data
- floatType,
- // Function pointer
- &throttled
- }
- }
- },
-
- // REQUEST
- {
- // Message Type ID
- 0x02,
-
- // Request Subtypes
- {
- // accelerometer subtype
- {
- // ID
- 0x00,
- // Command text
- "accelreq",
- // Type of the command data
- floatType,
- // Function pointer
- &accelreq
- },
- // gyroscope subtype
- {
- // ID
- 0x01,
- // Command text
- "gyroreq",
- // Type of the command data
- floatType,
- // Function pointer
- &gyroreq
- },
- // pitch angle subtype
- {
- // ID
- 0x02,
- // Command text
- "reqpitchangle",
- // Type of the command data
- floatType,
- // Function pointer
- &pitchanglereq
- },
- // roll angle subtype
- {
- // ID
- 0x03,
- // Command text
- "reqrollangle",
- // Type of the command data
- floatType,
- // Function pointer
- &rollanglereq
- }
- }
- },
-
- // RESPONSE
- {
- // Message Type ID
- 0x03,
-
- // Response Subtypes
- {
- // accelerometer subtype
- {
- // ID
- 0x00,
- // Command text
- "respaccel",
- // Type of the command data
- floatType,
- // Function pointer
- &accelresp
- },
- // gyroscope subtype
- {
- // ID
- 0x01,
- // Command text
- "respgyro",
- // Type of the command data
- floatType,
- // Function pointer
- &gyroresp
- },
- // pitch angle subtype
- {
- // ID
- 0x02,
- // Command text
- "resppitchangle",
- // Type of the command data
- floatType,
- // Function pointer
- &pitchangleresp
- },
- // roll angle subtype
- {
- // ID
- 0x03,
- // Command text
- "resprollangle",
- // Type of the command data
- floatType,
- // Function pointer
- &rollangleresp
- }
- }
- },
-
- // UPDATE
- {
- // Message Type ID
- 0x04,
-
- // Update Subtypes
- {
- // NONE subtype
- {
- // ID
- 0x00,
- // Command text
- "update",
- // Type of the command data
- stringType,
- // Function pointer
- &update
- },
- // BEGIN UPDATE subtype
- {
- // ID
- 0x01,
- // Command text
- "beginupdate",
- // Type of the command data
- stringType,
- // Function pointer
- &beginupdate
- }
- }
- },
-
- // LOG
- {
- // Message Type ID
- 0x05,
-
- // Log Subtypes
- {
- // NONE subtype
- {
- // ID
- 0x00,
- // Command text
- "log",
- // Type of the command data
- stringType,
- // Function pointer
- &logdata
- },
- // Response subtype
- {
- // ID
- 0x01,
- // Command text
- "response",
- // Type of the command data
- stringType,
- // Function pointer
- &response
- }
- }
- },
-
-};
-
-int debug(modular_structs_t *structs)
-{
- printf("function for debug\n");
- return 0;
-}
-
-static int n_msg_received = 0;
-static size_t total_payload_received = 0;
-
-int packetlog(modular_structs_t* structs) {
- n_msg_received += 1;
- total_payload_received += uart_buff_data_length();
- return 0;
-}
-
-int getpacketlogs(modular_structs_t* structs) {
- char buf[255];
-
- // Message logging number of messages received and size of payload received
- int length = snprintf(buf, sizeof buf, "%d,%d", n_msg_received, total_payload_received);
-
- send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
- return 0;
-}
-
-/* Handles receiving new location updates */
-int update(modular_structs_t *structs)
-{
- //processUpdate(packet, &(structs->raw_sensor_struct.currentQuadPosition));
-
- quadPosition_t* currentQuadPosition = &(structs->raw_sensor_struct.currentQuadPosition);
- // Packet must come as [NEARPY], 4 bytes each
- int packetId = uart_buff_data_get_u32(0);
-// printf("Packet ID: %d\n", packetId);
- float y_pos = uart_buff_data_get_float(4);
-// printf("y_pos: %f\n", y_pos);
- float x_pos = uart_buff_data_get_float(8);
-// printf("x_pos: %f\n", x_pos);
- float alt_pos = uart_buff_data_get_float(12);
-// printf("alt_pos: %f\n", alt_pos);
- float roll = uart_buff_data_get_float(16);
-// printf("roll: %f\n", roll);
- float pitch = uart_buff_data_get_float(20);
-// printf("pitch: %f\n", pitch);
- float yaw = uart_buff_data_get_float(24);
-// printf("yaw: %f\n", yaw);
-
- currentQuadPosition->packetId = packetId;
- currentQuadPosition->y_pos = y_pos;
- currentQuadPosition->x_pos = x_pos;
- currentQuadPosition->alt_pos = alt_pos;
- currentQuadPosition->roll = roll;
- currentQuadPosition->pitch = pitch;
- currentQuadPosition->yaw = yaw;
-
- // Make location as fresh
- structs->user_input_struct.locationFresh = 1;
-
- return 0;
-}
-
-// This is called on the ground station to begin sending VRPN to the quad
-int beginupdate(modular_structs_t *structs) {
- structs->user_input_struct.receivedBeginUpdate = 1;
- return 0;
-}
-
-int logdata(modular_structs_t *structs)
-{
- size_t length;
- unsigned char *packet = uart_buff_get_raw(&length);
- printf("Logging: %s\n", packet);
- return 0;
-}
-
-int response(modular_structs_t *structs)
-{
- size_t length;
- char *packet = uart_buff_get_raw(&length);
- printf("This is the response: %s\n", packet);
- return 0;
-}
-
-// ------------------------------------------------------------------
-// Quad side implementation
-
-// TODO: Erase memory leaks
-
-int yawset(modular_structs_t *structs)
-{
- char buf[255] = {};
-
- structs->setpoint_struct.desiredQuadPosition.yaw = uart_buff_data_get_float(0);
-
- // Debug print statement
- //printf("function for yawset: %f\n", structs->setpoint_struct.desiredQuadPosition.yaw);
-
- // Send a reply to the ground station
- int length = snprintf(buf, sizeof(buf), "Successfully set desired yaw to %.2f radians\r\n", structs->setpoint_struct.desiredQuadPosition.yaw);
-
- send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
- return 0;
-}
-
-int yawp(modular_structs_t *structs)
-{
- char buf[255] = {0};
-
- structs->parameter_struct.yaw_angle_pid.Kp = uart_buff_data_get_float(0);
-
- printf("function for yawp: %f\n", structs->parameter_struct.yaw_angle_pid.Kp);
-
- // Send a reply to the ground station
- int length = snprintf(buf, sizeof(buf), "Successfully set yaw Kp to %.2f\r\n", structs->parameter_struct.yaw_angle_pid.Kp);
-
- send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
- return 0;
-}
-
-int yawd(modular_structs_t *structs)
-{
- char buf[255] = {};
-
- structs->parameter_struct.yaw_angle_pid.Kd = uart_buff_data_get_float(0);
-
- printf("function for yawd: %f\n", structs->parameter_struct.yaw_angle_pid.Kd);
-
- // Send a reply to the ground station
- int length = snprintf(buf, sizeof(buf), "Successfully set yaw Kd to %.2f\r\n", structs->parameter_struct.yaw_angle_pid.Kd);
-
- send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
- return 0;
-}
-
-int rollset(modular_structs_t *structs)
-{
- char buf[255] = {};
-
- structs->setpoint_struct.desiredQuadPosition.roll = uart_buff_data_get_float(0);
-
- printf("function for rollset: %f\n", structs->setpoint_struct.desiredQuadPosition.roll);
-
- // Send a reply to the ground station
- int length = snprintf(buf, sizeof(buf), "Successfully set desired roll to %.2f radians\r\n", structs->setpoint_struct.desiredQuadPosition.roll);
-
- send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
- return 0;
-}
-
-int rollp(modular_structs_t *structs)
-{
- char buf[255] = {};
-
- structs->parameter_struct.roll_angle_pid.Kp = uart_buff_data_get_float(0);
-
- printf("function for rollp: %f\n", structs->parameter_struct.roll_angle_pid.Kp);
-
- // Send a reply to the ground station
- size_t length = snprintf(buf, sizeof(buf), "Successfully set roll Kp to %.2f\r\n", structs->parameter_struct.roll_angle_pid.Kp);
-
- send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
-
- return 0;
-}
-
-int rolld(modular_structs_t *structs)
-{
- char buf[255] = {};
-
- structs->parameter_struct.roll_angle_pid.Kd = uart_buff_data_get_float(0);
-
- printf("function for rolld: %f\n", structs->parameter_struct.roll_angle_pid.Kd);
-
- // Send a reply to the ground station
- size_t length = snprintf(buf, sizeof(buf), "Successfully set roll Kd to %.2f\r\n", structs->parameter_struct.roll_angle_pid.Kd);
-
- send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
- return 0;
-}
-
-int pitchset(modular_structs_t *structs)
-{
- char buf[255] = {};
-
- structs->setpoint_struct.desiredQuadPosition.pitch = uart_buff_data_get_float(0);
-
- printf("function for pitchset: %f\n", structs->setpoint_struct.desiredQuadPosition.pitch);
-
- // Send a reply to the ground station
- int length = snprintf(buf, sizeof(buf), "Successfully set desired pitch to %.2f radians\r\n", structs->setpoint_struct.desiredQuadPosition.pitch);
-
- send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
- return 0;
-}
-
-int pitchp(modular_structs_t *structs)
-{
- char buf[255] = {};
-
- structs->parameter_struct.pitch_angle_pid.Kp = uart_buff_data_get_float(0);
-
- printf("function for pitchp: %f\n", structs->parameter_struct.pitch_angle_pid.Kp);
-
- // Send a reply to the ground station
- int length = snprintf(buf, sizeof(buf), "Successfully set pitch Kp to %.2f\r\n", structs->parameter_struct.pitch_angle_pid.Kp);
-
- send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
- return 0;
-}
-
-int pitchd(modular_structs_t *structs)
-{
- char buf[255] = {};
-
- structs->parameter_struct.pitch_angle_pid.Kd = uart_buff_data_get_float(0);
-
- printf("function for pitchd: %f\n", structs->parameter_struct.pitch_angle_pid.Kd);
-
- // Send a reply to the ground station
- int length = snprintf(buf, sizeof(buf), "Successfully set desired yaw to %.2f\r\n", structs->parameter_struct.pitch_angle_pid.Kd);
-
- send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
- return 0;
-}
-
-// ------------------------------------------------------------
-// These should be renamed to altitude!
-int throttleset(modular_structs_t *structs)
-{
- char buf[255] = {};
-
- structs->setpoint_struct.desiredQuadPosition.alt_pos = uart_buff_data_get_float(0);
-
- printf("function for throttleset: %f\n", structs->setpoint_struct.desiredQuadPosition.alt_pos);
-
- // Send a reply to the ground station
- int length = snprintf(buf, sizeof(buf), "Successfully set desired altitude to %.2f meters\r\n", structs->setpoint_struct.desiredQuadPosition.alt_pos);
-
- send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
- return 0;
-}
-
-int throttlep(modular_structs_t *structs)
-{
- char buf[255] = {};
-
- structs->parameter_struct.alt_pid.Kp = uart_buff_data_get_float(0);
-
- printf("function for throttlep: %f\n", structs->parameter_struct.alt_pid.Kp);
-
- // Send a reply to the ground station
- int length = snprintf(buf, sizeof(buf), "Successfully set alt Kp to %.2f\r\n", structs->parameter_struct.alt_pid.Kp);
-
- send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
- return 0;
-}
-
-int throttlei(modular_structs_t *structs)
-{
- char buf[255] = {};
-
- structs->parameter_struct.alt_pid.Ki = uart_buff_data_get_float(0);
-
- printf("function for throttlei: %f\n", structs->parameter_struct.alt_pid.Ki);
-
- // Send a reply to the ground station
- int length = snprintf(buf, sizeof(buf), "Successfully set alt Ki to %.2f\r\n", structs->parameter_struct.alt_pid.Ki);
-
- send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
- return 0;
-}
-
-int throttled(modular_structs_t *structs)
-{
- char buf[255] = {};
-
- structs->parameter_struct.alt_pid.Kd = uart_buff_data_get_float(0);
-
- printf("function for throttled: %f\n", structs->parameter_struct.alt_pid.Kd);
-
- // Send a reply to the ground station
- int length = snprintf(buf, sizeof(buf), "Successfully set alt Kd to %.2f\r\n", structs->parameter_struct.alt_pid.Kd);
-
- send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
- return 0;
-}
-// These should be renamed to altitude!
-// ------------------------------------------------------------
-
-int accelreq(modular_structs_t *structs)
-{
- printf("function for accelreq\n");
- return 0;
-}
-
-int gyroresp(modular_structs_t *structs)
-{
- printf("function for accelreq\n");
- return 0;
-}
-
-int pitchangleresp(modular_structs_t *structs)
-{
- printf("function for accelreq\n");
- return 0;
-}
-
-int rollangleresp(modular_structs_t *structs)
-{
- printf("function for accelreq\n");
- return 0;
-}
-
-int gyroreq(modular_structs_t *structs)
-{
- printf("function for accelreq\n");
- return 0;
-}
-
-int pitchanglereq(modular_structs_t *structs)
-{
- printf("function for accelreq\n");
- return 0;
-}
-
-int rollanglereq(modular_structs_t *structs)
-{
- printf("function for accelreq\n");
- return 0;
-}
-
-int accelresp(modular_structs_t *structs)
-{
- printf("function for accelreq\n");
- return 0;
-}
diff --git a/quad/sw/modular_quad_pid/src/commands.c b/quad/sw/modular_quad_pid/src/commands.c
new file mode 120000
index 0000000000000000000000000000000000000000..e03a0ea45faafddf983aed9e6bb2eae3b2902ac3
--- /dev/null
+++ b/quad/sw/modular_quad_pid/src/commands.c
@@ -0,0 +1 @@
+../../../../groundStation/src/backend/commands.c
\ No newline at end of file
diff --git a/quad/sw/modular_quad_pid/src/commands.h b/quad/sw/modular_quad_pid/src/commands.h
deleted file mode 100644
index d93478dbff738aa497881a24e773e27ad5f01432..0000000000000000000000000000000000000000
--- a/quad/sw/modular_quad_pid/src/commands.h
+++ /dev/null
@@ -1,75 +0,0 @@
-#ifndef _COMMANDS_H
-#define _COMMANDS_H
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include "type_def.h"
-#include "packet_processing.h"
-
-// ----------------------
-// Helper stuff
-
-#define MAX_TYPE 6
-#define MAX_SUBTYPE 100
-
-enum Message{
- BEGIN_CHAR = 0xBE,
- END_CHAR = 0xED
-};
-
-// This should also have double to avoid confusion with float values.
-enum DataType
-{
- floatType,
- intType,
- stringType
-};
-
-// MESSAGE SUBTYPES
-struct MessageSubtype{
- char ID;
- char cmdText[100];
- char cmdDataType;
- int (*functionPtr)(modular_structs_t *structs);
-};
-
-// MESSAGE TYPES
-struct MessageType{
- char ID;
- struct MessageSubtype subtypes[MAX_SUBTYPE];
-};
-
-int debug(modular_structs_t *structs);
-int packetlog(modular_structs_t* structs);
-int getpacketlogs(modular_structs_t* structs);
-int update(modular_structs_t *structs);
-int beginupdate(modular_structs_t *structs);
-int logdata(modular_structs_t *structs);
-int response(modular_structs_t *structs);
-int yawset(modular_structs_t *structs);
-int yawp(modular_structs_t *structs);
-int yawd(modular_structs_t *structs);
-int rollset(modular_structs_t *structs);
-int rollp(modular_structs_t *structs);
-int rolld(modular_structs_t *structs);
-int pitchset(modular_structs_t *structs);
-int pitchp(modular_structs_t *structs);
-int pitchd(modular_structs_t *structs);
-int throttleset(modular_structs_t *structs);
-int throttlep(modular_structs_t *structs);
-int throttlei(modular_structs_t *structs);
-int throttled(modular_structs_t *structs);
-int accelreq(modular_structs_t *structs);
-int gyroresp(modular_structs_t *structs);
-int pitchangleresp(modular_structs_t *structs);
-int rollangleresp(modular_structs_t *structs);
-int gyroreq(modular_structs_t *structs);
-int pitchanglereq(modular_structs_t *structs);
-int rollanglereq(modular_structs_t *structs);
-int accelresp(modular_structs_t *structs);
-
-// TODO add in string to be read from the command line when sending a subtype of message
-extern struct MessageType MessageTypes[MAX_TYPE];
-
-#endif
diff --git a/quad/sw/modular_quad_pid/src/commands.h b/quad/sw/modular_quad_pid/src/commands.h
new file mode 120000
index 0000000000000000000000000000000000000000..b0ff517e5fdb2363f2afda70b29e57aee9fdc663
--- /dev/null
+++ b/quad/sw/modular_quad_pid/src/commands.h
@@ -0,0 +1 @@
+../../../../groundStation/src/backend/commands.h
\ No newline at end of file