diff --git a/groundStation/src/backend/backend.c b/groundStation/src/backend/backend.c
index 8b523c2749b0b9c929242b74af45c912a1cfa3d0..15e08b2b87f373c6600736d4915d1b3aa318c0fc 100644
--- a/groundStation/src/backend/backend.c
+++ b/groundStation/src/backend/backend.c
@@ -68,6 +68,8 @@ static int clientAddPendResponses(int fd, unsigned char *packet);
static int remove_client(int fd);
/* Receive data from client */
static void client_recv(int fd);
+/* Receive data from quad */
+static void quad_recv(const char * buf, size_t len);
/* Checks to see if socket has disconnected. Returns 1 on disconnect, else returns 0 */
static int wasDisconnected(int fd);
@@ -110,8 +112,7 @@ modular_structs_t structs;
// Callback to be ran whenever the tracker receives data.
// Currently doing much more than it should. It will be slimmed down
// in the future.
-static void cb(struct ucart_vrpn_TrackerData * td)
-{
+static void cb(struct ucart_vrpn_TrackerData * td) {
static int count = 0;
if(!(count % 10)) {
@@ -234,20 +235,28 @@ int main(int argc, char **argv)
}
if (fd == fileno(stdin)) {
-
+ /**
+ * Ignore stdin from the backend
+ */
} else if (fd == zyboSocket) {
- // Read the response from the control loop
+ /**
+ * Read the response from the control loop
+ */
int available;
ioctl(fd, FIONREAD, &available);
if (available < 12) {
continue;
}
- int respLen = readQuad(respBuf, 12);
+ int respLen = readQuad(respBuf, CMD_MAX_LENGTH);
if(respLen <= 0) {
perror("ERROR reading from quad...\n");
}
- int id = getInt((unsigned char *)respBuf, 7);
- findTimeDiff(id);
+
+ //int id = getInt((unsigned char *)respBuf, 7);
+ //findTimeDiff(id);
+
+ quad_recv(respBuf, respLen);
+
// if(respLen == 11) {
// int id = getInt((unsigned char *)respBuf, 7);
// findTimeDiff(id);
@@ -665,7 +674,7 @@ static void client_recv(int fd) {
} else {
if(clientAddPendResponses(fd, packet) == -1) {
- warnx("Ran out of room! Consider increasing CLIENT_MAX_PENDING_RESPONSES!");
+ warnx("Ran out of room! Consider increasing CLIENT_MAX_PENDING_RESPONSES!\n");
} else {
int datalen = (packet[6] << 8) | (packet[5]);
writeQuad((char *) packet, datalen +8);
@@ -679,6 +688,30 @@ static void client_recv(int fd) {
}
}
+static void quad_recv(const char * buf, size_t len) {
+ /* Check to see which command we are receiving. If it is one that needs to be passed on
+ onto the clients, do so.
+ */
+
+ unsigned char packet[len];
+ int validPacket;
+ unsigned char *data;
+ metadata_t metadata;
+
+ memcpy(packet, buf, len);
+
+ // Validate the message is correctly formatted
+ validPacket = parse_packet(packet, &data, &metadata);
+ if(validPacket != 0) {
+ warnx("Could not recognize packet from quad.\n");
+ return;
+ }
+
+ char * cmdText = MessageTypes[(int)metadata.msg_type].subtypes[(int)metadata.msg_subtype].cmdText;
+ float value = getFloat(packet, 7);
+ printf("Quad : %s, %lf\n", cmdText, value);
+}
+
static int wasDisconnected(int fd) {
char buff;
if(recv(fd, &buff, 1, MSG_PEEK | MSG_DONTWAIT) == 0)
diff --git a/groundStation/src/backend/commands.c b/groundStation/src/backend/commands.c
index 708a6ac98a154648d90b1e9ffbf7460d0ed15710..11e68405ce9ad3f6a72d52e5d26f9522f17eec24 100644
--- a/groundStation/src/backend/commands.c
+++ b/groundStation/src/backend/commands.c
@@ -439,6 +439,150 @@ struct MessageType MessageTypes[MAX_TYPE] =
floatType,
// Function pointer
&resprollangle
+ },
+
+ // resp yaw setpoint subtype
+ {
+ // ID
+ 0x04,
+ // Command text
+ "respyaw",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ &setyaw
+ },
+ // resp yaw p constant subtype
+ {
+ // ID
+ 0x05,
+ // Command text
+ "respyawp",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ &respyawp
+ },
+ // resp yaw d constant subtype
+ {
+ // ID
+ 0x06,
+ // Command text
+ "respyawd",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ &respyawd
+ },
+ // resp roll setpoint subtype
+ {
+ // ID
+ 0x07,
+ // Command text
+ "resproll",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ &resproll
+ },
+ // resp roll p constant subtype
+ {
+ // ID
+ 0x08,
+ // Command text
+ "resprollp",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ &resprollp
+ },
+ // resproll d constant subtype
+ {
+ // ID
+ 0x09,
+ // Command text
+ "resprolld",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ &resprolld
+ },
+ // resp pitch setpoint subtype
+ {
+ // ID
+ 0x0A,
+ // Command text
+ "resppitch",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ &resppitch
+ },
+ // resp pitch p constant subtype
+ {
+ // ID
+ 0x0B,
+ // Command text
+ "resppitchp",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ &resppitchp
+ },
+ // resp pitch d constant subtype
+ {
+ // ID
+ 0x0C,
+ // Command text
+ "resppitchd",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ &resppitchd
+ },
+ // resp throttle setpoint subtype
+ {
+ // ID
+ 0x0D,
+ // Command text
+ "respthrottle",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ &respthrottle
+ },
+ // resp throttle p constant subtype
+ {
+ // ID
+ 0x0E,
+ // Command text
+ "respthrottlep",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ &respthrottlep
+ },
+ // resp throttle i constant subtype
+ {
+ // ID
+ 0x0F,
+ // Command text
+ "respthrottlei",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ &respthrottlei
+ },
+ // resp throttle d constant subtype
+ {
+ // ID
+ 0x10,
+ // Command text
+ "respthrottled",
+ // Type of the command data
+ floatType,
+ // Function pointer
+ &respthrottled
}
}
},
@@ -758,12 +902,35 @@ int getthrottled(unsigned char *packet, int dataLen, modular_structs_t *structs)
// 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");
@@ -782,26 +949,49 @@ int resprollangle(unsigned char *packet, int dataLen, modular_structs_t *struct
return 0;
}
-int getgyro(unsigned char *packet, int dataLen, modular_structs_t *structs)
+
+int respaccel(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
- printf("function for getgyro\n");
+ printf("function for respaccel\n");
return 0;
}
-int getpitchangle(unsigned char *packet, int dataLen, modular_structs_t *structs)
-{
- printf("function for getpitchangle\n");
+int respyaw(unsigned char *packet, int dataLen, modular_structs_t *structs){
return 0;
}
-
-int getrollangle(unsigned char *packet, int dataLen, modular_structs_t *structs)
-{
- printf("function for getrollangle\n");
+int respyawp(unsigned char *packet, int dataLen, modular_structs_t *structs){
return 0;
}
-
-int respaccel(unsigned char *packet, int dataLen, modular_structs_t *structs)
-{
- printf("function for respaccel\n");
+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;
}
\ No newline at end of file
diff --git a/groundStation/src/backend/commands.h b/groundStation/src/backend/commands.h
index 3bb62fd8d84800ceb7710ad1a7a0c1b641029a19..7ec5b4629f7c0d05831ab7ee82845846bbdf51e9 100644
--- a/groundStation/src/backend/commands.h
+++ b/groundStation/src/backend/commands.h
@@ -71,13 +71,30 @@ 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 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 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 getrollangle(unsigned char *c, int dataLen, modular_structs_t *structs);
float getFloat(unsigned char* str, int pos);
int getInt(unsigned char* str, int pos);
diff --git a/groundStation/src/cli/cli_setpid.c b/groundStation/src/cli/cli_setpid.c
new file mode 100644
index 0000000000000000000000000000000000000000..7347ab72d574a52d4de4b3778c90967f69ce089a
--- /dev/null
+++ b/groundStation/src/cli/cli_setpid.c
@@ -0,0 +1,70 @@
+#include <stdio.h>
+#include <unistd.h>
+#include <getopt.h>
+
+#include "cli_setpid.h"
+#include "frontend_setpid.h"
+
+int cli_setpid(struct backend_conn * conn, int argc, char **argv) {
+ int c;
+ static int setRoll = 0, setPitch = 0, setYaw = 0, setAll = 0;
+ static int setRollV = 0, setPitchV = 0, setYawV = 0;
+ static int setHeight = 0, setLat = 0, setLong = 0;
+ struct frontend_pid_data pid_data;
+ static int mask;
+ static float pval = 0, ival = 0, dval = 0;
+ static struct option long_options[] = {
+ /* These options don’t set a flag. We distinguish them by their indices. */
+ {"roll", no_argument, &setRoll, 1},
+ {"pitch", no_argument, &setPitch, 1},
+ {"yaw", no_argument, &setYaw, 1},
+ {"rollv", no_argument, &setYawV, 1},
+ {"pitchv", no_argument, &setYawV, 1},
+ {"yawv", no_argument, &setYawV, 1},
+ {"height", no_argument, &setHeight, 1},
+ {"lat", no_argument, &setLat, 1},
+ {"long", no_argument, &setLong, 1},
+ {0, 0, 0, 0}
+ };
+
+ while (1)
+ {
+ /* getopt_long stores the option index here. */
+ int option_index = 0;
+
+ c = getopt_long(argc, argv, "p:i:d:", long_options, &option_index);
+
+ /* Detect the end of the options. */
+ if (c == -1)
+ break;
+
+ switch(c) {
+ case 'p' :
+ pid_data.p = atof(optarg);
+ mask |= SET_P;
+ break;
+ case 'i' :
+ pid_data.i = atof(optarg);
+ mask |= SET_I;
+ break;
+ case 'd' :
+ pid_data.d = atof(optarg);
+ mask |= SET_D;
+ break;
+ default :
+ break;
+ }
+ }
+
+ if(setRoll) {
+ pid_data.controller = PID_ROLL;
+ } else if (setYaw) {
+ pid_data.controller = PID_YAW;
+ } else if (setPitch) {
+ pid_data.controller = PID_PITCH;
+ }
+
+
+ frontend_setpid(conn, &pid_data, mask);
+ return 0;
+}
\ No newline at end of file