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 /* __CALLBACKS_H */

groundStation/src/backend/cb_default.h

+#ifndef __CB_DEFAULT_H
+#define __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;
+}
+
+#endif /* __CB_DEFAULT_H */
\ No newline at end of file

groundStation/src/backend/commands.h

+#ifndef __COMMANDS_H
+#define __COMMANDS_H
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "callbacks.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;
+	command_cb * functionPtr;
+};
+
+// MESSAGE TYPES
+struct MessageType{
+	char ID;
+	struct MessageSubtype subtypes[MAX_SUBTYPE];
+};
+
+/* 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 */
+
+#endif /* __COMMANDS_H */
\ No newline at end of file

groundStation/src/backend/communication.h

+#ifndef __COMMUNICATION_H
+#define __COMMUNICATION_H
+
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <limits.h>
+#include "type_def.h"
+
+tokenList_t tokenize(char* cmd);
+int checkFloat(char *floatString, float *value);
+int checkInt(char *intString, int *value);
+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, modular_structs_t *structs);
+int logData(unsigned char *log_msg, unsigned char *formattedCommand);
+
+#endif /* __COMMUNICATION_H */
\ No newline at end of file

groundStation/src/backend/config.h

+#ifndef __CONFIG_H
+#define __CONFIG_H
+
+
+#define DEFAULT_SOCKET "/var/run/ucart.socket"
+#define SOCKET_ENV "UCART_SOCKET"
+#define NOQUAD_ENV "UCART_NO_QUAD"
+#define NOVRPN_ENV "UCART_NO_VRPN"
+
+
+// If you are planning on using any of these env vars and you have
+// 	exported them with normal user rights. You will need to run the 
+// 	backend with sudo elevation and with the --preserve-env flag or -E
+#define QUAD_WIFI_ENV "UCART_USE_WIFI"
+#define QUAD_IP_ENV "UCART_QUAD_IP"
+#define QUAD_IP_DEFAULT "192.168.4.1"
+#define QUAD_PORT_ENV "UCART_QUAD_PORT"
+#define QUAD_PORT_DEFAULT 8080
+
+#endif /* __CONFIG_H */
\ No newline at end of file

src/logger.c → groundStation/src/backend/logger.c

@@ -4,21 +4,30 @@
  */
 #include "logger.h"
 #include <stdio.h>
+#include <err.h>
+#include <pthread.h>
 
-int quadlog_file;
+static FILE * quadlog_file = NULL;
+static pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
 
 int createLogFile(int argc, char* argv)
 {
-	char log_file[300] = {'l', 'o', 'g','s', '/'};
+	if (quadlog_file != NULL) {
+		return -1;
+	}
+
+	if (pthread_mutex_lock(&mutex)) {
+		err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
+	}
+
+	char log_file[300];
+	strcpy(log_file, "logs/");
 	if(argc >= 2)
 	{
-		strcat(log_file, argv);
+		strncat(log_file, argv, 294);
 		printf("Creating log file '%s'...\n",log_file);
-		quadlog_file = open(log_file, O_WRONLY | O_CREAT, 0666);
-		return quadlog_file;
-	}
-	else
-	{
+		quadlog_file = fopen(log_file, "a");
+	} else {
 		time_t rawtime;
 		char timestr [30];
 		time(&rawtime);
@@ -26,7 +35,7 @@ int createLogFile(int argc, char* argv)
 
 		// Lets convert space to _ in
 		char *p = timestr;
-		int i = 0;
+		size_t i = 0;
 		while(i < strlen(timestr))
 		{ 
 		    if (*p == ' ')
@@ -40,18 +49,60 @@ int createLogFile(int argc, char* argv)
 		strncat(log_file, timestr, strlen(timestr) -1 );
 		strcat(log_file, ".txt");
 		printf("Creating log file '%s'...\n",log_file);
-		quadlog_file = open(log_file, O_WRONLY | O_CREAT, 0666);
-		return quadlog_file;
+		quadlog_file = fopen(log_file, "a");
+	}
+	if (pthread_mutex_unlock(&mutex)) {
+		err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
 	}
+	return 0;
 }
 
 int updateLogFile(const struct ucart_vrpn_TrackerData * td)
 {
-	return dprintf(quadlog_file, "FPS: %lf Pos (xyz): (%lf %lf %lf) Att (pry): (%lf %lf %lf)\n",
+	int retval;
+
+	if (pthread_mutex_lock(&mutex)) {
+		err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
+	}
+     
+	retval = fprintf(quadlog_file, 
+			"FPS: %lf Pos (xyz): (%lf %lf %lf) Att (pry): (%lf %lf %lf)\n",
 		td->fps, td->x, td->y, td->z, td->pitch, td->roll, td->yaw);
+
+	if (pthread_mutex_unlock(&mutex)) {
+		err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
+	}
+	
+	return retval;
 }
 
 int writeStringToLog(const char * string)
 {
-	return dprintf(quadlog_file, "%s", string);
+	int retval;
+
+	if (pthread_mutex_lock(&mutex)) {
+		err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
+	}
+
+	retval = fprintf(quadlog_file, "%s", string);
+
+	if (pthread_mutex_unlock(&mutex)) {
+		err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
+	}
+
+	return retval;
+}
+
+void closeLogFile(void)
+{
+	if (pthread_mutex_lock(&mutex)) {
+		err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
+	}
+
+	fclose(quadlog_file);
+
+	if (pthread_mutex_unlock(&mutex)) {
+		err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
+	}
+
 }