commands.c

#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
			}
		}
	},
	
	// 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(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
	printf("function for debug\n");
	return 0;
}

/* This is not used. Many flow changes would be need to be made to the
	control algorithm in order to get this to work. */
int update(unsigned char *packet, int dataLen,	modular_structs_t *structs)
{
	printf("function for debug\n");
	return 0;
}

// This is called on the ground station to begin sending VRPN to the quad
int beginupdate(unsigned char *c, int dataLen, modular_structs_t *structs) {
	return 0;
}

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;
}

// ------------------------------------------------------------------
// Quad side implementation

int yawset(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
	float value;
	char buf[255] = {};
	int i;
	
	// Copy the data into the value variable
	memcpy(&value, ((float *)packet), dataLen);
	
	// Update the struct
	structs->setpoint_struct.desiredQuadPosition.yaw = value;

	// Debug print statement
	//printf("function for yawset: %f\n", structs->setpoint_struct.desiredQuadPosition.yaw);
	
	// Send a reply to the ground station
	snprintf(buf, sizeof(buf), "Successfully set desired yaw to %.2f radians\r\n", structs->setpoint_struct.desiredQuadPosition.yaw);
	unsigned char *responsePacket;

	printf("%s\n", buf);

	metadata_t metadata =
	{
		BEGIN_CHAR,
		MessageTypes[5].ID,
		MessageTypes[5].subtypes[1].ID,
		0,
		(strlen(buf) + 1)
	};
	formatPacket(&metadata, buf, &responsePacket);

	// Send each byte of the packet individually
	for(i = 0; i < 8 + metadata.data_len; i++) {
		// Debug print statement for all of the bytes being sent
		printf("%d: 0x%x\n", i, responsePacket[i]);

		uart0_sendByte(responsePacket[i]);
	}

	return 0;
}

int yawp(unsigned char *packet, int dataLen,  modular_structs_t *structs)
{
	float value;
	char buf[255] = {0};
	int i;
	
	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);
	
	// Send a reply to the ground station
	snprintf(buf, sizeof(buf), "Successfully set yaw Kp to %.2f\r\n", structs->parameter_struct.yaw_angle_pid.Kp);
	unsigned char *responsePacket;

	printf("%s\n", buf);

	metadata_t metadata =
	{
		BEGIN_CHAR,
		MessageTypes[5].ID,
		MessageTypes[5].subtypes[1].ID,
		0,
		(strlen(buf) + 1)
	};
	formatPacket(&metadata, buf, &responsePacket);

	// Send each byte of the packet individually
	for(i = 0; i < 8 + metadata.data_len; i++) {
		// Debug print statement for all of the bytes being sent
		//printf("%d: 0x%x\n", i, packet2[i]);

		uart0_sendByte(responsePacket[i]);
	}

	return 0;
}

int yawd(unsigned char *packet, int dataLen,  modular_structs_t *structs)
{
	float value;
	char buf[255] = {};
	int i;
	
	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);
	
	// Send a reply to the ground station
	snprintf(buf, sizeof(buf), "Successfully set yaw Kd to %.2f\r\n", structs->parameter_struct.yaw_angle_pid.Kd);
	unsigned char *responsePacket;

	printf("%s\n", buf);

	metadata_t metadata =
	{
		BEGIN_CHAR,
		MessageTypes[5].ID,
		MessageTypes[5].subtypes[1].ID,
		0,
		(strlen(buf) + 1)
	};
	formatPacket(&metadata, buf, &responsePacket);

	// Send each byte of the packet individually
	for(i = 0; i < 8 + metadata.data_len; i++) {
		// Debug print statement for all of the bytes being sent
		//printf("%d: 0x%x\n", i, responsePacket[i]);

		uart0_sendByte(responsePacket[i]);
	}

	return 0;
}

int rollset(unsigned char *packet, int dataLen,	 modular_structs_t *structs)
{
	float value;
	char buf[255] = {};
	int i;
	
	memcpy(&value, ((float *)packet), dataLen);
	structs->setpoint_struct.desiredQuadPosition.roll = value;
	
	printf("function for rollset: %f\n", structs->setpoint_struct.desiredQuadPosition.roll);
	
	// Send a reply to the ground station
	snprintf(buf, sizeof(buf), "Successfully set desired roll to %.2f radians\r\n", structs->setpoint_struct.desiredQuadPosition.roll);
	unsigned char *responsePacket;

	metadata_t metadata =
	{
		BEGIN_CHAR,
		MessageTypes[5].ID,
		MessageTypes[5].subtypes[1].ID,
		0,
		(strlen(buf) + 1)
	};
	formatPacket(&metadata, buf, &responsePacket);

	// Send each byte of the packet individually
	for(i = 0; i < 8 + metadata.data_len; i++) {
		// Debug print statement for all of the bytes being sent
		//printf("%d: 0x%x\n", i, responsePacket[i]);

		uart0_sendByte(responsePacket[i]);
	}

	return 0;
}

int rollp(unsigned char *packet, int dataLen,  modular_structs_t *structs)
{
	float value;
	char buf[255] = {};
	int i;
	
	memcpy(&value, ((float *)packet), dataLen);
	structs->parameter_struct.roll_angle_pid.Kp = value;
	
	printf("function for rollp: %f\n", structs->parameter_struct.roll_angle_pid.Kp);
	
	// Send a reply to the ground station
	snprintf(buf, sizeof(buf), "Successfully set roll Kp to %.2f\r\n", structs->parameter_struct.roll_angle_pid.Kp);
	unsigned char *responsePacket;

	metadata_t metadata =
	{
		BEGIN_CHAR,
		MessageTypes[5].ID,
		MessageTypes[5].subtypes[1].ID,
		0,
		(strlen(buf) + 1)
	};
	formatPacket(&metadata, buf, &responsePacket);

	// Send each byte of the packet individually
	for(i = 0; i < 8 + metadata.data_len; i++) {
		// Debug print statement for all of the bytes being sent
		//printf("%d: 0x%x\n", i, responsePacket[i]);

		uart0_sendByte(responsePacket[i]);
	}

	return 0;
}

int rolld(unsigned char *packet, int dataLen,  modular_structs_t *structs)
{
	float value;
	char buf[255] = {};
	int i;
	
	memcpy(&value, ((float *)packet), dataLen);
	structs->parameter_struct.roll_angle_pid.Kd = value;
	
	printf("function for rolld: %f\n", structs->parameter_struct.roll_angle_pid.Kd);
	
	// Send a reply to the ground station
	snprintf(buf, sizeof(buf), "Successfully set roll Kd to %.2f\r\n", structs->parameter_struct.roll_angle_pid.Kd);
	unsigned char *responsePacket;

	metadata_t metadata =
	{
		BEGIN_CHAR,
		MessageTypes[5].ID,
		MessageTypes[5].subtypes[1].ID,
		0,
		(strlen(buf) + 1)
	};
	formatPacket(&metadata, buf, &responsePacket);

	// Send each byte of the packet individually
	for(i = 0; i < 8 + metadata.data_len; i++) {
		// Debug print statement for all of the bytes being sent
		//printf("%d: 0x%x\n", i, responsePacket[i]);

		uart0_sendByte(responsePacket[i]);
	}

	return 0;
}

int pitchset(unsigned char *packet, int dataLen,  modular_structs_t *structs)
{
	float value;
	char buf[255] = {};
	int i;
	
	memcpy(&value, ((float *)packet), dataLen);
	structs->setpoint_struct.desiredQuadPosition.pitch = value;
	
	printf("function for pitchset: %f\n", structs->setpoint_struct.desiredQuadPosition.pitch);
	
	// Send a reply to the ground station
	snprintf(buf, sizeof(buf), "Successfully set desired pitch to %.2f radians\r\n", structs->setpoint_struct.desiredQuadPosition.pitch);
	unsigned char *responsePacket;

	metadata_t metadata =
	{
		BEGIN_CHAR,
		MessageTypes[5].ID,
		MessageTypes[5].subtypes[1].ID,
		0,
		(strlen(buf) + 1)
	};
	formatPacket(&metadata, buf, &responsePacket);

	// Send each byte of the packet individually
	for(i = 0; i < 8 + metadata.data_len; i++) {
		// Debug print statement for all of the bytes being sent
		//printf("%d: 0x%x\n", i, responsePacket[i]);

		uart0_sendByte(responsePacket[i]);
	}

	return 0;
}

int pitchp(unsigned char *packet, int dataLen,	modular_structs_t *structs)
{
	float value;
	char buf[255] = {};
	int i;
	
	memcpy(&value, ((float *)packet), dataLen);
	structs->parameter_struct.pitch_angle_pid.Kp = value;
	
	printf("function for pitchp: %f\n", structs->parameter_struct.pitch_angle_pid.Kp);
	
	// Send a reply to the ground station
	snprintf(buf, sizeof(buf), "Successfully set pitch Kp to %.2f\r\n", structs->parameter_struct.pitch_angle_pid.Kp);
	unsigned char *responsePacket;

	metadata_t metadata =
	{
		BEGIN_CHAR,
		MessageTypes[5].ID,
		MessageTypes[5].subtypes[1].ID,
		0,
		(strlen(buf) + 1)
	};
	formatPacket(&metadata, buf, &responsePacket);

	// Send each byte of the packet individually
	for(i = 0; i < 8 + metadata.data_len; i++) {
		// Debug print statement for all of the bytes being sent
		//printf("%d: 0x%x\n", i, responsePacket[i]);

		uart0_sendByte(responsePacket[i]);
	}

	return 0;
}

int pitchd(unsigned char *packet, int dataLen,	modular_structs_t *structs)
{
	float value;
	char buf[255] = {};
	int i;
	
	memcpy(&value, ((float *)packet), dataLen);
	structs->parameter_struct.pitch_angle_pid.Kd = value;
	
	printf("function for pitchd: %f\n", structs->parameter_struct.pitch_angle_pid.Kd);
	
	// Send a reply to the ground station
	snprintf(buf, sizeof(buf), "Successfully set desired yaw to %.2f\r\n", structs->parameter_struct.pitch_angle_pid.Kd);
	unsigned char *responsePacket;

	metadata_t metadata =
	{
		BEGIN_CHAR,
		MessageTypes[5].ID,
		MessageTypes[5].subtypes[1].ID,
		0,
		(strlen(buf) + 1)
	};
	formatPacket(&metadata, buf, &responsePacket);

	// Send each byte of the packet individually
	for(i = 0; i < 8 + metadata.data_len; i++) {
		// Debug print statement for all of the bytes being sent
		//printf("%d: 0x%x\n", i, responsePacket[i]);

		uart0_sendByte(responsePacket[i]);
	}

	return 0;
}

// ------------------------------------------------------------
// These should be renamed to altitude!
int throttleset(unsigned char *packet, int dataLen,	 modular_structs_t *structs)
{
	float value;
	char buf[255] = {};
	int i;
	
	memcpy(&value, ((float *)packet), dataLen);
	structs->setpoint_struct.desiredQuadPosition.alt_pos = value;
	
	printf("function for throttleset: %f\n", structs->setpoint_struct.desiredQuadPosition.alt_pos);
	
	// Send a reply to the ground station
	snprintf(buf, sizeof(buf), "Successfully set desired altitude to %.2f meters\r\n", structs->setpoint_struct.desiredQuadPosition.alt_pos);
	unsigned char *responsePacket;

	metadata_t metadata =
	{
		BEGIN_CHAR,
		MessageTypes[5].ID,
		MessageTypes[5].subtypes[1].ID,
		0,
		(strlen(buf) + 1)
	};
	formatPacket(&metadata, buf, &responsePacket);

	// Send each byte of the packet individually
	for(i = 0; i < 8 + metadata.data_len; i++) {
		// Debug print statement for all of the bytes being sent
		//printf("%d: 0x%x\n", i, responsePacket[i]);

		uart0_sendByte(responsePacket[i]);
	}

	return 0;
}

int throttlep(unsigned char *packet, int dataLen,  modular_structs_t *structs)
{
	float value;
	char buf[255] = {};
	int i;
	
	memcpy(&value, ((float *)packet), dataLen);
	structs->parameter_struct.alt_pid.Kp = value;
	
	printf("function for throttlep: %f\n", structs->parameter_struct.alt_pid.Kp);
	
	// Send a reply to the ground station
	snprintf(buf, sizeof(buf), "Successfully set alt Kp to %.2f\r\n", structs->parameter_struct.alt_pid.Kp);
	unsigned char *responsePacket;

	metadata_t metadata =
	{
		BEGIN_CHAR,
		MessageTypes[5].ID,
		MessageTypes[5].subtypes[1].ID,
		0,
		(strlen(buf) + 1)
	};
	formatPacket(&metadata, buf, &responsePacket);

	// Send each byte of the packet individually
	for(i = 0; i < 8 + metadata.data_len; i++) {
		// Debug print statement for all of the bytes being sent
		//printf("%d: 0x%x\n", i, responsePacket[i]);

		uart0_sendByte(responsePacket[i]);
	}

	return 0;
}

int throttlei(unsigned char *packet, int dataLen,  modular_structs_t *structs)
{
	float value;
	char buf[255] = {};
	int i;
	
	memcpy(&value, ((float *)packet), dataLen);
	structs->parameter_struct.alt_pid.Ki = value;
	
	printf("function for throttlei: %f\n", structs->parameter_struct.alt_pid.Ki);
	
	// Send a reply to the ground station
	snprintf(buf, sizeof(buf), "Successfully set alt Ki to %.2f\r\n", structs->parameter_struct.alt_pid.Ki);
	unsigned char *responsePacket;

	metadata_t metadata =
	{
		BEGIN_CHAR,
		MessageTypes[5].ID,
		MessageTypes[5].subtypes[1].ID,
		0,
		(strlen(buf) + 1)
	};
	formatPacket(&metadata, buf, &responsePacket);

	// Send each byte of the packet individually
	for(i = 0; i < 8 + metadata.data_len; i++) {
		// Debug print statement for all of the bytes being sent
		//printf("%d: 0x%x\n", i, responsePacket[i]);

		uart0_sendByte(responsePacket[i]);
	}

	return 0;
}

int throttled(unsigned char *packet, int dataLen,  modular_structs_t *structs)
{
	float value;
	char buf[255] = {};
	int i;
	
	memcpy(&value, ((float *)packet), dataLen);
	structs->parameter_struct.alt_pid.Kd = value;
	
	printf("function for throttled: %f\n", structs->parameter_struct.alt_pid.Kd);
	
	// Send a reply to the ground station
	snprintf(buf, sizeof(buf), "Successfully set alt Kd to %.2f\r\n", structs->parameter_struct.alt_pid.Kd);
	unsigned char *responsePacket;

	metadata_t metadata =
	{
		BEGIN_CHAR,
		MessageTypes[5].ID,
		MessageTypes[5].subtypes[1].ID,
		0,
		(strlen(buf) + 1)
	};
	formatPacket(&metadata, buf, &responsePacket);

	// Send each byte of the packet individually
	for(i = 0; i < 8 + metadata.data_len; i++) {
		// Debug print statement for all of the bytes being sent
		//printf("%d: 0x%x\n", i, responsePacket[i]);

		uart0_sendByte(responsePacket[i]);
	}

	return 0;
}
// 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 *packet, int dataLen,  modular_structs_t *structs)
{
	printf("function for accelreq\n");
	return 0;
}

int pitchangleresp(unsigned char *packet, int dataLen,	modular_structs_t *structs)
{
	printf("function for accelreq\n");
	return 0;
}

int rollangleresp(unsigned char *packet, int dataLen,  modular_structs_t *structs)
{
	printf("function for accelreq\n");
	return 0;
}

int gyroreq(unsigned char *packet, int dataLen,	 modular_structs_t *structs)
{
	printf("function for accelreq\n");
	return 0;
}

int pitchanglereq(unsigned char *packet, int dataLen,  modular_structs_t *structs)
{
	printf("function for accelreq\n");
	return 0;
}

int rollanglereq(unsigned char *packet, int dataLen,  modular_structs_t *structs)
{
	printf("function for accelreq\n");
	return 0;
}

int accelresp(unsigned char *packet, int dataLen,  modular_structs_t *structs)
{
	printf("function for accelreq\n");
	return 0;
}