Implemented new commands.c

It's portable between quad and groundstation.

groundStation/src/backend/callbacks.c

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

groundStation/src/backend/callbacks.h

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

groundStation/src/backend/cb_default.h

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

groundStation/src/backend/commands.h

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