From 8c8cbcfdac3bbe248bd4389815f2d12b49f37126 Mon Sep 17 00:00:00 2001
From: Kris Burney <burneykb@iastate.edu>
Date: Fri, 28 Oct 2016 22:36:11 -0500
Subject: [PATCH] Fixed select and formatCommand bugs
---
groundStation/src/commands.c | 625 +++++++++++++++++++++++++++---
groundStation/src/commands.h | 94 ++---
groundStation/src/communication.c | 143 ++++---
groundStation/src/communication.h | 8 +-
groundStation/src/microcart_cli.c | 121 +++---
5 files changed, 750 insertions(+), 241 deletions(-)
diff --git a/groundStation/src/commands.c b/groundStation/src/commands.c
index dc4749ee..cdbbbe8a 100644
--- a/groundStation/src/commands.c
+++ b/groundStation/src/commands.c
@@ -1,109 +1,624 @@
#include "commands.h"
-static command_t registeredCommands[NUM_COMMANDS] = {
- { 0x00, 0x00, stringType, "debug", &debug }, //DEBUG
- { 0x01, 0x00, stringType, "setyaw", &setyaw }, //CALIBRATION
- { 0x01, 0x01, stringType, "setyawp", &setyawp },
- { 0x01, 0x02, stringType, "setyawd", &setyawd },
- { 0x01, 0x03, stringType, "setroll", &setroll },
- { 0x01, 0x04, stringType, "setrollp", &setrollp },
- { 0x01, 0x05, stringType, "setrolld", &setrolld },
- { 0x01, 0x06, stringType, "setpitch", &setpitch },
- { 0x01, 0x07, stringType, "setpitchp", &setpitchp },
- { 0x01, 0x08, stringType, "setpitchd", &setpitchd },
- { 0x01, 0x09, stringType, "setthrottle", &setthrottle },
- { 0x01, 0x0A, stringType, "setthrottlep", &setthrottlep },
- { 0x01, 0x0B, stringType, "setthrottlei", &setthrottlei },
- { 0x01, 0x0C, stringType, "setthrottled", &setthrottled },
- { 0x02, 0x00, stringType, "getaccel", &getaccel }, //REQUEST
- { 0x02, 0x01, stringType, "getgyro", &getgyro },
- { 0x02, 0x02, stringType, "getpitchangle", &getpitchangle },
- { 0x02, 0x03, stringType, "getrollangle", &getrollangle },
- { 0x03, 0x00, stringType, "accelresp", accelresp }, //RESPONSE
- { 0x03, 0x01, stringType, "gyroresp", &gyroresp },
- { 0x03, 0x02, stringType, "pitchangleresp", &pitchangleresp },
- { 0x03, 0x03, stringType, "rollangleresp", &rollangleresp },
- { 0x04, 0x00, stringType, "update", &update }, //UPDATE
- { 0x04, 0x01, stringType, "beginupdate", &beginupdate },
- { 0x05, 0x00, stringType, "log", &logdata }, //LOG
- { 0x05, 0x01, stringType, "response", &response },
-};
+// 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 debug(unsigned char *c, int dataLen){
- return 0;
+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 setyaw(unsigned char *c, int dataLen){
+//------------------------------------------------
+
+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
+ }
+ }
+ },
+
+ // 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
+ }
+ }
+ },
+
+ // 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(unsigned char *packet, int dataLen, modular_structs_t *structs)
+{
+ printf("function for debug\n");
return 0;
}
-int setyawp(unsigned char *c, int dataLen){
+
+int 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;
}
-int setyawd(unsigned char *c, int dataLen){
+
+// 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 setroll(unsigned char *c, int dataLen){
+
+int response(unsigned char *packet, int dataLen, modular_structs_t *structs)
+{
+ printf("This is the response: %s\n", packet);
+
return 0;
}
-int setrollp(unsigned char *c, int dataLen){
+
+// ------------------------------------------------------------------
+
+int yawset(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 yawset: %f\n", structs->setpoint_struct.desiredQuadPosition.yaw);
+
return 0;
}
-int setrolld(unsigned char *c, int dataLen){
+
+int yawp(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 yawp: %f\n", structs->parameter_struct.yaw_angle_pid.Kp);
+
return 0;
}
-int setpitch(unsigned char *c, int dataLen){
+
+int yawd(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 yawd: %f\n", structs->parameter_struct.yaw_angle_pid.Kd);
+
return 0;
}
-int setpitchp(unsigned char *c, int dataLen){
+
+int rollset(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 rollset: %f\n", structs->setpoint_struct.desiredQuadPosition.roll);
+
return 0;
}
-int setpitchd(unsigned char *c, int dataLen){
+
+int rollp(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 rollp: %f\n", structs->parameter_struct.local_y_pid.Kp);
+
return 0;
}
-int setthrottle(unsigned char *c, int dataLen){
+
+int rolld(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 rolld: %f\n", structs->parameter_struct.local_y_pid.Kd);
+
return 0;
}
-int setthrottlep(unsigned char *c, int dataLen){
+
+int pitchset(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 pitchset: %f\n", structs->setpoint_struct.desiredQuadPosition.pitch);
+
return 0;
}
-int setthrottlei(unsigned char *c, int dataLen){
+
+int pitchp(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 pitchp: %f\n", structs->parameter_struct.local_x_pid.Kp);
+
return 0;
}
-int setthrottled(unsigned char *c, int dataLen){
+
+int pitchd(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 pitchd: %f\n", structs->parameter_struct.local_x_pid.Kd);
+
return 0;
}
-int getaccel(unsigned char *c, int dataLen){
+
+// ------------------------------------------------------------
+// These should be renamed to altitude!
+int throttleset(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 throttleset: %f\n", structs->setpoint_struct.desiredQuadPosition.alt_pos);
+
return 0;
}
-int getgyro(unsigned char *c, int dataLen){
+
+int throttlep(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 throttlep: %f\n", structs->parameter_struct.alt_pid.Kp);
+
return 0;
}
-int getpitchangle(unsigned char *c, int dataLen){
+
+int throttlei(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 throttlei: %f\n", structs->parameter_struct.alt_pid.Ki);
+
return 0;
}
-int getrollangle(unsigned char *c, int dataLen){
+
+int throttled(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 throttled: %f\n", structs->parameter_struct.alt_pid.Kd);
+
return 0;
}
-int accelresp(unsigned char *c, int dataLen){
+// These should be renamed to altitude!
+// ------------------------------------------------------------
+
+int accelreq(unsigned char *packet, int dataLen, modular_structs_t *structs)
+{
+ printf("function for accelreq\n");
return 0;
}
-int gyroresp(unsigned char *c, int dataLen){
+
+int gyroresp(unsigned char *packet, int dataLen, modular_structs_t *structs)
+{
+ printf("function for accelreq\n");
return 0;
}
-int pitchangleresp(unsigned char *c, int dataLen){
+
+int pitchangleresp(unsigned char *packet, int dataLen, modular_structs_t *structs)
+{
+ printf("function for accelreq\n");
return 0;
}
-int rollangleresp(unsigned char *c, int dataLen){
+
+int rollangleresp(unsigned char *packet, int dataLen, modular_structs_t *structs)
+{
+ printf("function for accelreq\n");
return 0;
}
-int update(unsigned char *c, int dataLen){
+
+int gyroreq(unsigned char *packet, int dataLen, modular_structs_t *structs)
+{
+ printf("function for accelreq\n");
return 0;
}
-int beginupdate(unsigned char *c, int dataLen){
+
+int pitchanglereq(unsigned char *packet, int dataLen, modular_structs_t *structs)
+{
+ printf("function for accelreq\n");
return 0;
}
-int logdata(unsigned char *c, int dataLen){
+
+int rollanglereq(unsigned char *packet, int dataLen, modular_structs_t *structs)
+{
+ printf("function for accelreq\n");
return 0;
}
-int response(unsigned char *packet, int dataLen){
+
+int accelresp(unsigned char *packet, int dataLen, modular_structs_t *structs)
+{
+ printf("function for accelreq\n");
return 0;
}
\ No newline at end of file
diff --git a/groundStation/src/commands.h b/groundStation/src/commands.h
index 050990de..51c82bd7 100644
--- a/groundStation/src/commands.h
+++ b/groundStation/src/commands.h
@@ -1,44 +1,24 @@
#ifndef _COMMANDS_H
#define _COMMANDS_H
-#define NUM_COMMANDS 26
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include "type_def.h"
+
+
+// ----------------------
+// Helper stuff
-//TODO handle with enums
#define MAX_TYPE 6
#define MAX_SUBTYPE 100
-int debug(unsigned char *c, int dataLen);
-int setyaw(unsigned char *c, int dataLen);
-int setyawp(unsigned char *c, int dataLen);
-int setyawd(unsigned char *c, int dataLen);
-int setroll(unsigned char *c, int dataLen);
-int setrollp(unsigned char *c, int dataLen);
-int setrolld(unsigned char *c, int dataLen);
-int setpitch(unsigned char *c, int dataLen);
-int setpitchp(unsigned char *c, int dataLen);
-int setpitchd(unsigned char *c, int dataLen);
-int setthrottle(unsigned char *c, int dataLen);
-int setthrottlep(unsigned char *c, int dataLen);
-int setthrottlei(unsigned char *c, int dataLen);
-int setthrottled(unsigned char *c, int dataLen);
-int getaccel(unsigned char *c, int dataLen);
-int getgyro(unsigned char *c, int dataLen);
-int getpitchangle(unsigned char *c, int dataLen);
-int getrollangle(unsigned char *c, int dataLen);
-int accelresp(unsigned char *c, int dataLen);
-int gyroresp(unsigned char *c, int dataLen);
-int pitchangleresp(unsigned char *c, int dataLen);
-int rollangleresp(unsigned char *c, int dataLen);
-int update(unsigned char *c, int dataLen);
-int beginupdate(unsigned char *c, int dataLen);
-int logdata(unsigned char *c, int dataLen);
-int response(unsigned char *packet, int dataLen);
-
enum Message{
BEGIN_CHAR = 0xBE,
END_CHAR = 0xED
};
+// This should also have double to avoid confusion with float values.
enum DataType
{
floatType,
@@ -46,24 +26,50 @@ enum DataType
stringType
};
-typedef struct command {
+// MESSAGE SUBTYPES
+struct MessageSubtype{
char ID;
- char subID;
- char dataType;
- char commandText[100];
- int (*functionPtr)(unsigned char *command, int dataLen);
-}command_t;
+ char cmdText[100];
+ char cmdDataType;
+ int (*functionPtr)(unsigned char *command, int dataLen, modular_structs_t *structs);
+};
-enum CommandIDs{
- DEBUG,
- CALIBRATION,
- REQUEST,
- RESPONSE,
- UPDATE,
- LOG,
- MAX_COMMAND_IDS
+// MESSAGE TYPES
+struct MessageType{
+ char ID;
+ struct MessageSubtype subtypes[MAX_SUBTYPE];
};
-static command_t registeredCommands[NUM_COMMANDS];
+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 yawset(unsigned char *c, int dataLen, modular_structs_t *structs);
+int yawp(unsigned char *c, int dataLen, modular_structs_t *structs);
+int yawd(unsigned char *c, int dataLen, modular_structs_t *structs);
+int rollset(unsigned char *c, int dataLen, modular_structs_t *structs);
+int rollp(unsigned char *c, int dataLen, modular_structs_t *structs);
+int rolld(unsigned char *c, int dataLen, modular_structs_t *structs);
+int pitchset(unsigned char *c, int dataLen, modular_structs_t *structs);
+int pitchp(unsigned char *c, int dataLen, modular_structs_t *structs);
+int pitchd(unsigned char *c, int dataLen, modular_structs_t *structs);
+int throttleset(unsigned char *c, int dataLen, modular_structs_t *structs);
+int throttlep(unsigned char *c, int dataLen, modular_structs_t *structs);
+int throttlei(unsigned char *c, int dataLen, modular_structs_t *structs);
+int throttled(unsigned char *c, int dataLen, modular_structs_t *structs);
+int accelreq(unsigned char *c, int dataLen, modular_structs_t *structs);
+int gyroresp(unsigned char *c, int dataLen, modular_structs_t *structs);
+int pitchangleresp(unsigned char *c, int dataLen, modular_structs_t *structs);
+int rollangleresp(unsigned char *c, int dataLen, modular_structs_t *structs);
+int gyroreq(unsigned char *c, int dataLen, modular_structs_t *structs);
+int pitchanglereq(unsigned char *c, int dataLen, modular_structs_t *structs);
+int rollanglereq(unsigned char *c, int dataLen, modular_structs_t *structs);
+int accelresp(unsigned char *c, int dataLen, modular_structs_t *structs);
+
+float getFloat(unsigned char* str, int pos);
+int getInt(unsigned char* str, int pos);
+
+// 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
diff --git a/groundStation/src/communication.c b/groundStation/src/communication.c
index f9d35792..77202ed4 100644
--- a/groundStation/src/communication.c
+++ b/groundStation/src/communication.c
@@ -52,10 +52,13 @@ int checkInt(char *intString, int *value) {
//--------------------------------
// Formatting commands from ground station CLI
-int formatCommand(unsigned char *command, unsigned char **formattedCommand) {
- //command[strlen((char *)command) - 1] = 0;
-
- tokenList_t tokens = tokenize((char *)command);
+int formatCommand(char *command, unsigned char **formattedCommand) {
+ fprintf(stderr, "length = %li , received '%s'\n", strlen(command), command);
+ char cmd[strlen(command)];
+ strncpy(cmd, command, strlen(command));
+ cmd[strlen(command)] = '\0';
+
+ tokenList_t tokens = tokenize(cmd);
float floatValue = 0.0;
int intValue = 0;
int valid;
@@ -63,72 +66,76 @@ int formatCommand(unsigned char *command, unsigned char **formattedCommand) {
// ----------------------------------------------
if(tokens.numTokens > 1) {
- for(int cmdIndex = 0; cmdIndex < NUM_COMMANDS; ++cmdIndex)
+ for(int type = 0; type < MAX_TYPE; type++)
{
- if(strcmp(tokens.tokens[0], registeredCommands[cmdIndex].commandText) == 0)
+ for(int subtype = 0; subtype < MAX_SUBTYPE; subtype++)
{
- switch (registeredCommands[cmdIndex].dataType)
+ if(strcmp(tokens.tokens[0], MessageTypes[type].subtypes[subtype].cmdText) == 0)
{
- // Validate the float input
- case floatType:
- valid = checkFloat(tokens.tokens[1], &floatValue);
- if(!valid) {
- return -1;
- }
-
- printf("%f, %s\n", floatValue, tokens.tokens[1]);
-
- metadata.begin_char = (char) BEGIN_CHAR;
- metadata.msg_type = registeredCommands[cmdIndex].ID;
- metadata.msg_subtype = registeredCommands[cmdIndex].subID;
- metadata.msg_id = msgNum++;
- metadata.data_len = sizeof(floatValue);
-
- formatPacket(&metadata, &floatValue, formattedCommand);
-
- break;
-
- // Validate the integer input
- case intType:
- valid = checkInt(tokens.tokens[1], &intValue);
- if(!valid) {
- return -1;
- }
-
- metadata.begin_char = (char) BEGIN_CHAR;
- metadata.msg_type = registeredCommands[cmdIndex].ID;
- metadata.msg_subtype = registeredCommands[cmdIndex].subID;
- metadata.msg_id = msgNum++;
- metadata.data_len = sizeof(intValue);
-
- formatPacket(&metadata, &intValue, formattedCommand);
-
- break;
+ printf("Sending\n\ttype: %d, \n\tsubtype: %d\n\tcommand: %s\n", type, subtype, MessageTypes[type].subtypes[subtype].cmdText);
- // Validate the string input (doesn't need to happen)
- case stringType:
- metadata.begin_char = (char) BEGIN_CHAR;
- metadata.msg_type = registeredCommands[cmdIndex].ID;
- metadata.msg_subtype = registeredCommands[cmdIndex].subID;
- metadata.msg_id = msgNum++;
- metadata.data_len = strlen(tokens.tokens[1]);
+ // Make sure the second token is the right type
+ switch (MessageTypes[type].subtypes[subtype].cmdDataType)
+ {
+ // Validate the float input
+ case floatType:
+ valid = checkFloat(tokens.tokens[1], &floatValue);
+ if(!valid) {
+ return -1;
+ }
+
+ printf("%f, %s\n", floatValue, tokens.tokens[1]);
+
+ metadata.begin_char = (char) BEGIN_CHAR;
+ metadata.msg_type = MessageTypes[type].ID;
+ metadata.msg_subtype = MessageTypes[type].subtypes[subtype].ID;
+ metadata.msg_id = msgNum++;
+ metadata.data_len = sizeof(floatValue);
+
+ formatPacket(&metadata, &floatValue, formattedCommand);
+
+ break;
- formatPacket(&metadata, &tokens.tokens[1], formattedCommand);
+ // Validate the integer input
+ case intType:
+ valid = checkInt(tokens.tokens[1], &intValue);
+ if(!valid) {
+ return -1;
+ }
+
+ metadata.begin_char = (char) BEGIN_CHAR;
+ metadata.msg_type = MessageTypes[type].ID;
+ metadata.msg_subtype = MessageTypes[type].subtypes[subtype].ID;
+ metadata.msg_id = msgNum++;
+ metadata.data_len = sizeof(intValue);
+
+ formatPacket(&metadata, &intValue, formattedCommand);
+
+ break;
- break;
- default:
- return -1;
+ // Validate the string input (doesn't need to happen)
+ case stringType:
+ metadata.begin_char = (char) BEGIN_CHAR;
+ metadata.msg_type = MessageTypes[type].ID;
+ metadata.msg_subtype = MessageTypes[type].subtypes[subtype].ID;
+ metadata.msg_id = msgNum++;
+ metadata.data_len = strlen(tokens.tokens[1]);
+
+ formatPacket(&metadata, &tokens.tokens[1], formattedCommand);
+
+ break;
+ default:
+ return -1;
+ }
+ return 0;
}
-
- return 0;
}
}
}
-
+
// Only gets here if the command does not exist
- return -1;
+ return -1;
}
-
// QUAD & Ground Station
// Format the log data from log_message
//int formatData(unsigned char *log_msg, unsigned char *formattedCommand)
@@ -231,7 +238,7 @@ int parse_packet(unsigned char * packet, unsigned char ** data, metadata_t * met
// QUAD & Ground Station
// Process the command received
-int processCommand(unsigned char *packet, unsigned int cmdIndex) {
+int processCommand(unsigned char *packet, modular_structs_t *structs) {
int validPacket;
unsigned char *data;
metadata_t metadata;
@@ -244,25 +251,11 @@ int processCommand(unsigned char *packet, unsigned int cmdIndex) {
if(metadata.data_len >= 0) {
// Call the appropriate subtype function
- (* (registeredCommands[cmdIndex].functionPtr))(data, metadata.data_len);
+ (* (MessageTypes[(unsigned char)metadata.msg_type].subtypes[(unsigned char)metadata.msg_subtype].functionPtr))(data, metadata.data_len, structs);
return 0;
}
// Only gets here if there is an error
return -1;
-}
-
-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;
-}
+}
\ No newline at end of file
diff --git a/groundStation/src/communication.h b/groundStation/src/communication.h
index 94a3c33c..746b9f5d 100644
--- a/groundStation/src/communication.h
+++ b/groundStation/src/communication.h
@@ -10,12 +10,10 @@
tokenList_t tokenize(char* cmd);
int checkFloat(char *floatString, float *value);
int checkInt(char *intString, int *value);
-int formatCommand(unsigned char *command, unsigned char **formattedCommand);
+int formatCommand(char *command, unsigned char **formattedCommand);
int formatPacket(metadata_t *metadata, void *data, unsigned char **formattedCommand);
int parse_packet(unsigned char * packet, unsigned char ** data, metadata_t * meta_data);
-int processCommand(unsigned char *command, unsigned int cmdIndex);
+int processCommand(unsigned char *command, modular_structs_t *structs);
int logData(unsigned char *log_msg, unsigned char *formattedCommand);
-float getFloat(unsigned char* str, int pos);
-int getInt(unsigned char* str, int pos);
-#endif
+#endif
\ No newline at end of file
diff --git a/groundStation/src/microcart_cli.c b/groundStation/src/microcart_cli.c
index 7589cdb2..a439a897 100644
--- a/groundStation/src/microcart_cli.c
+++ b/groundStation/src/microcart_cli.c
@@ -1,4 +1,4 @@
-/* Author: Kris Burney
+/* Author: Kris Burney & Jake Drahos
*
* BlueTooth socket program for passing vrpn data to quad.
*/
@@ -39,10 +39,6 @@ void sendStartPacket(void);
void getVRPNPacket(struct ucart_vrpn_TrackerData *);
void printVrpnData(struct ucart_vrpn_TrackerData *);
int connectToZybo();
-// void *handleQuadResponse();
-// void *handleCliInput();
-// int atomic_check(int*, pthread_mutex_t*);
-void performCommand(char *, char *);
int startsWith(const char *, const char *);
int safe_fd_set(int , fd_set* , int* );
int safe_fd_clr(int , fd_set* , int* );
@@ -58,6 +54,8 @@ static char * get_client_buffer(int fd);
static int remove_client(int fd);
/* Receive data from client */
static void client_recv(int fd);
+/* Checks to see if socket has disconnected. Returns 1 on disconnect, else returns 0 */
+static int wasDisconnected(int fd);
/* Thread-safe wrappers */
pthread_mutex_t quadSocketMutex;
@@ -75,6 +73,8 @@ const char *logHeader = "";//"#\n#\tDefault log header\n#\tEverything after '#'`
#define CLIENT_BUFFER_SIZE 1024
static char client_buffers[MAX_CLIENTS][CLIENT_BUFFER_SIZE];
static int client_fds[MAX_CLIENTS];
+fd_set rfds_master;
+int max_fd = 0;
pthread_mutex_t quadResponseMutex, cliInputMutex ;
unsigned char *respBuf, *commandBuf;
@@ -109,10 +109,7 @@ static void cb(struct ucart_vrpn_TrackerData * td)
int main(int argc, char **argv)
{
- // pthread_t quadResponse, cliInput;
- fd_set rfds_master;
int activity;
- int max_fd = 0;
FD_ZERO(&rfds_master);
/*
@@ -156,7 +153,7 @@ int main(int argc, char **argv)
client_buffers[i][0] = '\n';
}
- signal(SIGINT, killHandler);
+ //signal(SIGINT, killHandler);
if (pthread_mutex_lock(&quadSocketMutex)) {
err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
@@ -201,7 +198,7 @@ int main(int argc, char **argv)
// ucart_vrpn_tracker_addCallback(tracker, cb);
// start the prompt
- fprintf(stdout, "$microcart> ");
+ // fprintf(stdout, "$microcart> ");
struct timeval timeout = {
.tv_sec = 1,
@@ -218,35 +215,35 @@ int main(int argc, char **argv)
fprintf(stderr, "Select activity = %d\n", activity);
for(int fd = 0; fd <= max_fd; ++fd) {
if (FD_ISSET(fd, &rfds)) {
- fprintf(stderr, "Select woke for fd %d\n", fd);
- if (fd == fileno(stdin)) {
- unsigned char userCommand[CMD_MAX_LENGTH];
- read(fileno(stdin), (char *)userCommand, sizeof(userCommand));
- unsigned int cmdLen = strlen((char*) userCommand);
- // if the user simply hit enter then let them try again
- if((userCommand[0] != '\n') && (userCommand[0] != '\r'))
- {
- // remove newline and return line chars
- if((userCommand[cmdLen - 1] == '\n') || (userCommand[cmdLen - 1] == '\r'))
- userCommand[cmdLen - 1] = '\0';
-
- unsigned char *packet;
- formatCommand(userCommand, &packet);
-
- printf("received input from cli: '%s'\n", userCommand);
-
- fprintf(stdout, "CLI sees as: %f\n", getFloat(packet, 7));
-
- // Write the command to the control_loop socket
- // int n = writeQuad(packet, ((packet[6] << 8) | packet[5]) + 8);
- // if(n < 0) {
- // fprintf(stdout, "CLI: ERROR writing to socket\n");
- // }
- }
-
+ if(wasDisconnected(fd))
+ break;
- fprintf(stdout, "$microcart> ");
- memset(userCommand, 0, cmdLen);
+ if (fd == fileno(stdin)) {
+ // break;
+ // unsigned char userCommand[CMD_MAX_LENGTH];
+ // read(fileno(stdin), (char *)userCommand, sizeof(userCommand));
+ // unsigned int cmdLen = strlen((char*) userCommand);
+ // // if the user simply hit enter then let them try again
+ // if((userCommand[0] != '\n') && (userCommand[0] != '\r'))
+ // {
+ // // remove newline and return line chars
+ // if((userCommand[cmdLen - 1] == '\n') || (userCommand[cmdLen - 1] == '\r'))
+ // userCommand[cmdLen - 1] = '\0';
+
+ // unsigned char *packet;
+ // formatCommand(userCommand, &packet);
+
+ // printf("received input from cli: '%s'\n", userCommand);
+
+ // // Write the command to the control_loop socket
+ // // int n = writeQuad(packet, ((packet[6] << 8) | packet[5]) + 8);
+ // // if(n < 0) {
+ // // fprintf(stdout, "CLI: ERROR writing to socket\n");
+ // // }
+ // }
+
+ // fprintf(stdout, "$microcart> ");
+ // memset(userCommand, 0, cmdLen);
} else if (fd == zyboSocket) {
} else if (fd == backendSocket) {
@@ -267,13 +264,6 @@ int main(int argc, char **argv)
}
}
} else {
- for(int i = 0; i < MAX_CLIENTS; ++i) {
- if(client_fds[i] != -1) {
- char buff;
- if(recv(client_fds[i],&buff, 1, MSG_PEEK | MSG_DONTWAIT) < 0)
- remove_client(client_fds[i]);
- }
- }
timeout.tv_sec = 1;
timeout.tv_usec = 0;
}
@@ -404,15 +394,6 @@ int connectToZybo() {
}
}
-void performCommand(char *cmdName, char * command) {
- for(int i = 0; i < NUM_COMMANDS; ++i)
- {
- if(startsWith(registeredCommands[i].commandText, command)) {
- fprintf(stdout, "\r\n You used cmd '%s'\n",registeredCommands[i].commandText);
- }
- }
-}
-
int startsWith(const char *pre, const char *str) {
size_t lenpre = strlen(pre),
lenstr = strlen(str);
@@ -443,7 +424,7 @@ int safe_fd_clr(int fd, fd_set* fds, int* max_fd) {
static ssize_t writeQuad(const char * buf, size_t count) {
ssize_t retval;
- if (getenv(NOQUAD_ENV)) {
+ if (1) {//getenv(NOQUAD_ENV)) {
return count;
}
if (pthread_mutex_lock(&quadSocketMutex)) {
@@ -537,7 +518,6 @@ static void client_recv(int fd)
}
buffer[len_pre + r] = '\0';
-
/* Parse buffer and handle commands */
while (1) {
/* not using strtok because reasons */
@@ -554,16 +534,33 @@ static void client_recv(int fd)
if (newline == -1) {
break;
}
-
buffer[newline] = '\0';
+
unsigned char * packet;
- fprintf(stderr, "Client sent: %s\n", buffer);
- formatCommand((unsigned char *) buffer, &packet);
- writeQuad((char *) packet, 4);
- free(packet);
+ // fprintf(stderr, "newline =%li, Client sent: '%s'\n", newline, buffer);
+ if(formatCommand(buffer, &packet) != -1) {
+ fprintf(stdout, "Backend sees as: %f\n", getFloat(packet, 7));
+ } else {
+ fprintf(stderr, "Could not recognize command '%s'\n", buffer);
+ }
+ writeQuad((char *) packet, len);
+ //free(packet);
char * rest = &buffer[newline] + 1;
+ size_t restLen = (strlen(rest) == 0) ? 1 : strlen(rest);
/* Delete parsed data and move the rest to the left */
- memmove(buffer, rest, strlen(rest));
+ memmove(buffer, rest, restLen);
}
}
+
+static int wasDisconnected(int fd) {
+ char buff;
+ if(recv(fd, &buff, 1, MSG_PEEK | MSG_DONTWAIT) == 0)
+ {
+ remove_client(fd);
+ safe_fd_clr(fd, &rfds_master, &max_fd);
+ fprintf(stderr, "fd %d has disconnect and was removed\n", fd);
+ return 1;
+ }
+ return 0;
+}
--
GitLab