packet_processing.c

/*
 * process_packet.c
 *
 *  Created on: Mar 2, 2016
 *      Author: ucart
 */
#include "packet_processing.h"
#include "uart.h"
#include "type_def.h"
#include "sleep.h"
#include "util.h"
#include "communication.h"

#define DEBUG 0

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

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

	return ret;
}

int processUpdate(unsigned char* update, quadPosition_t* currentQuadPosition) {
	//static char buf[16384];
	//sprintf(buf, "update..(%d)..[%s]\r\n", strlen(update), update);
	//uart0_sendStr(buf);

	unsigned char * data;
	metadata_t md;
	parse_packet(update, &data, &md);


	// Packet must come as [NEARPY], 4 bytes each
	int packetId = getInt(data, 0);
//	printf("Packet ID: %d\n", packetId);
	float y_pos = getFloat(data, 4);
//	printf("y_pos: %f\n", y_pos);
	float x_pos = getFloat(data, 8);
//	printf("x_pos: %f\n", x_pos);
	float alt_pos = getFloat(data, 12);
//	printf("alt_pos: %f\n", alt_pos);
	float roll = getFloat(data, 16);
//	printf("roll: %f\n", roll);
	float pitch = getFloat(data, 20);
//	printf("pitch: %f\n", pitch);
	float yaw = getFloat(data, 24);
//	printf("yaw: %f\n", yaw);

	currentQuadPosition->packetId = packetId;
	currentQuadPosition->y_pos = y_pos;
	currentQuadPosition->x_pos = x_pos;
	currentQuadPosition->alt_pos = alt_pos;
	currentQuadPosition->roll = roll;
	currentQuadPosition->pitch = pitch;
	currentQuadPosition->yaw = yaw;

	return 0;
}

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