communication.c

#include "communication.h"
#include "commands.h"
#include <string.h>
#include <ctype.h>

static int msgNum = 0;

tokenList_t tokenize(char* cmd) {
	int maxTokens = 16;
	tokenList_t ret;
	ret.numTokens = 0;
	ret.tokens = malloc(sizeof(char*) * maxTokens);
	ret.tokens[0] = NULL;

	int i = 0;
	char* token = strtok(cmd, " ");
	while (token != NULL && i < maxTokens - 1) {
		ret.tokens[i++] = token;
		ret.tokens[i] = NULL;
		ret.numTokens++;
		token = strtok(NULL, " ");
	}

	return ret;
}

int checkFloat(char *floatString, float *value) {
	char *tmp;
	*value = strtod(floatString, &tmp);
	if(!(isspace(*tmp) || *tmp == 0)) {
		fprintf(stderr, "%s is not a valid floating-point number\n", floatString);
		return 0;
	}
	return 1;
}

int checkInt(char *intString, int *value) {
	char *tmp;
	long temp_long;
	temp_long = strtol(intString, &tmp, 0); // base 10 number inputted
	if(temp_long < INT_MIN || temp_long > INT_MAX ||  !(isspace(*tmp) || *tmp == 0)) {
		fprintf(stderr, "%s is not a valid integer number\n", intString);
		return 0;
	}
	printf("temp: %ld\n\n", temp_long);
	*value = (int) temp_long;
	return 1;
}

//--------------------------------
// Ground Station
//--------------------------------

// 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);
	float floatValue = 0.0;
	int intValue = 0;
	int valid;
	metadata_t metadata = {};
	
	// ----------------------------------------------
	if(tokens.numTokens > 1) {
		for(int cmdIndex = 0; cmdIndex < NUM_COMMANDS; ++cmdIndex)
		{
			if(strcmp(tokens.tokens[0], registeredCommands[cmdIndex].commandText) == 0)
			{
				switch (registeredCommands[cmdIndex].dataType)
				{
					// 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  = 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  = 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;
					
					// Validate the string input (doesn't need to happen)
					case stringType:
						metadata.begin_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]);
						
						formatPacket(&metadata, &tokens.tokens[1], formattedCommand);
						
						break;
					default:
						return -1;
				}

				return 0;
			}
		}
	}
	
	// Only gets here if the command does not exist
	return -1;
}

// QUAD & Ground Station
// Format the log data from log_message
//int formatData(unsigned char *log_msg, unsigned char *formattedCommand)
int formatPacket(metadata_t *metadata, void *data, unsigned char **formattedCommand)
{
	*formattedCommand = malloc(sizeof(unsigned char) * metadata->data_len + 8);
	//----------------------------------------------------------------------------------------------
	//	   index||	   0	|	  1	   |	  2		 |	3 & 4 |		 5 & 6		 |	7+	|	end	   |
	//---------------------------------------------------------------------------------------------|
	// msg param|| beg char | msg type | msg subtype | msg id | data len (bytes) | data | checksum |
	//-------------------------------------------------------------------------------------------- |
	//	   bytes||	   1	|	  1	   |	  1		 |	  2	  |		   2		 | var	|	 1	   |
	//----------------------------------------------------------------------------------------------
	
	// Begin Char:
	(*formattedCommand)[0] = metadata->begin_char;

	// Msg type:
	(*formattedCommand)[1] = metadata->msg_type;
	
	// Msg subtype
	(*formattedCommand)[2] = metadata->msg_subtype;
	
	//Msg id (msgNum is 2 bytes)
	(*formattedCommand)[3] = metadata->msg_id;
	
	// Data length and data - bytes 5&6 for len, 7+ for data
	(*formattedCommand)[5] = metadata->data_len & 0x000000ff;
	(*formattedCommand)[6] = (metadata->data_len >> 8) & 0x000000ff;
	
	memcpy(&((*formattedCommand)[7]), data, metadata->data_len);
	
	// Checksum
	// receive data and calculate checksum
	int i;
	char data_checksum;
	for(i = 0; i < 7 + metadata->data_len; i++)
	{
		data_checksum ^= (*formattedCommand)[i];
	}
	
	(*formattedCommand)[7 + metadata->data_len] = data_checksum;
	
	return 0;
}

// returns the length of the data in bytes (datalen from packet) and fills data
// and metadata with the packet information
// use as follows:
//
//		packet is the entire packet message (formatted) 
//		data is an unallocated (char *) (pass it to this function as &data) 
//		meta_data is a pointer to an instance of metadata_t
//
int parse_packet(unsigned char * packet, unsigned char ** data, metadata_t * meta_data)
{
	//----------------------------------------------------------------------------------------------
	//     index||     0    |     1    |      2      |  3 & 4 |      5 & 6       |  7+  |   end    |
	//---------------------------------------------------------------------------------------------|
	// msg param|| beg char | msg type | msg subtype | msg id | data len (bytes) | data | checksum |
	//-------------------------------------------------------------------------------------------- |
	//     bytes||     1    |     1    |      1      |    2   |        2         | var  |    1     |
	//----------------------------------------------------------------------------------------------	
	
	// first byte must be the begin char
	if(packet[0] != 0xBE)
		return -1;

	// receive metadata
	meta_data->begin_char = packet[0];
	meta_data->msg_type = packet[1];
	meta_data->msg_subtype = packet[2];
	meta_data->msg_id = (packet[4] << 8) | (packet[3]);
	meta_data->data_len = (packet[6] << 8) | (packet[5]);
	unsigned char packet_checksum = packet[7+meta_data->data_len];
	//fprintf(stderr, "datalen: %d\n", meta_data->data_len);
	
	int i;
	
	// receive data
	*data = malloc(meta_data->data_len);
	for(i = 0; i < meta_data->data_len; i++)
	{
		(*data)[i] = packet[7+i];
	}

	// calculate checksum
	unsigned char calculated_checksum = 0;
	for(i = 0; i < meta_data->data_len + 7; i++)
	{
		calculated_checksum ^= packet[i];
	}

	// compare checksum
	if(packet_checksum != calculated_checksum)
		fprintf(stderr, "Checksums did not match (Parse Packet): 0x%02x\t0x%02x\n", packet_checksum, calculated_checksum);

	return 0;
}

// QUAD & Ground Station
// Process the command received
int processCommand(unsigned char *packet, unsigned int cmdIndex) {
	int validPacket;
	unsigned char *data;
	metadata_t metadata;
	
	// Validate the message is correctly formatted
	validPacket = parse_packet(packet, &data, &metadata);
	if(validPacket != 0) {
		return -1;
	}
	
	if(metadata.data_len >= 0) {
		// Call the appropriate subtype function
		(* (registeredCommands[cmdIndex].functionPtr))(data, metadata.data_len);
		
		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;
}