#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(char *command, unsigned char **formattedCommand) {
//fprintf(stderr, "length = %li , received '%s'\n", strlen(command), command);
char cmd[strlen(command)];
strncpy(cmd, command, strlen(command));
cmd[strlen(command)] = '\0';
tokenList_t tokens = tokenize(cmd);
float floatValue = 0.0;
int intValue = 0;
int valid;
metadata_t metadata;
// ----------------------------------------------
if(tokens.numTokens > 0) {
for(int type = 0; type < MAX_TYPE; type++)
{
for(int subtype = 0; subtype < MAX_SUBTYPE; subtype++)
{
if(strcmp(tokens.tokens[0], MessageTypes[type].subtypes[subtype].cmdText) == 0)
{
printf("Sending\n\ttype: %d, \n\tsubtype: %d\n\tcommand: %s\n", type, subtype, MessageTypes[type].subtypes[subtype].cmdText);
// Make sure the second token is the right type
switch (MessageTypes[type].subtypes[subtype].cmdDataType)
{
// Validate the float input
case floatType:
metadata.begin_char = (char) BEGIN_CHAR;
metadata.msg_type = MessageTypes[type].ID;
metadata.msg_subtype = MessageTypes[type].subtypes[subtype].ID;
if(MessageTypes[type].ID == 0x01) {
valid = checkFloat(tokens.tokens[1], &floatValue);
if(!valid) {
return -1;
}
metadata.data_len = sizeof(floatValue);
} else {
metadata.data_len = 0;
}
metadata.msg_id = msgNum++;
formatPacket(&metadata, &floatValue, formattedCommand);
break;
// Validate the integer input
case intType:
metadata.begin_char = (char) BEGIN_CHAR;
metadata.msg_type = MessageTypes[type].ID;
metadata.msg_subtype = MessageTypes[type].subtypes[subtype].ID;
if(MessageTypes[type].ID == 0x01) {
valid = checkInt(tokens.tokens[1], &intValue);
if(!valid) {
return -1;
}
metadata.data_len = sizeof(intValue);
} else {
metadata.data_len = 0;
}
metadata.msg_id = msgNum++;
formatPacket(&metadata, &intValue, formattedCommand);
break;
// Validate the string input (doesn't need to happen)
case stringType:
metadata.begin_char = (char) BEGIN_CHAR;
metadata.msg_type = MessageTypes[type].ID;
metadata.msg_subtype = MessageTypes[type].subtypes[subtype].ID;
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 & 0x000000ff);
(*formattedCommand)[4] = ((metadata->msg_id >> 8) & 0x000000ff);
// 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, modular_structs_t *structs) {
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) {
/* Null check*/
if (MessageTypes[(unsigned char)metadata.msg_type].subtypes[
(unsigned char) metadata.msg_subtype].functionPtr) {
// Call the appropriate subtype function
(* (MessageTypes[(unsigned char)metadata.msg_type].subtypes[(unsigned char)metadata.msg_subtype].functionPtr))(data, metadata.data_len, structs);
}
return 0;
}
// Only gets here if there is an error
return -1;
}