Fixing message id formating and cmd tokenizing

groundStation/src/backend/communication.c

@@ -53,7 +53,7 @@ int checkInt(char *intString, int *value) {
 
 // Formatting commands from ground station CLI
 int formatCommand(char *command, unsigned char **formattedCommand) {
-	fprintf(stderr, "length = %li , received '%s'\n", strlen(command), command);
+	//fprintf(stderr, "length = %li , received '%s'\n", strlen(command), command);
 	char cmd[strlen(command)];
 	strncpy(cmd, command, strlen(command));
 	cmd[strlen(command)] = '\0';
@@ -65,7 +65,7 @@ int formatCommand(char *command, unsigned char **formattedCommand) {
 	metadata_t metadata;
 	
 	// ----------------------------------------------
-	if(tokens.numTokens > 1) {
+	if(tokens.numTokens > 0) {
 		for(int type = 0; type < MAX_TYPE; type++)
 		{
 			for(int subtype = 0; subtype < MAX_SUBTYPE; subtype++)
@@ -79,36 +79,41 @@ int formatCommand(char *command, unsigned char **formattedCommand) {
 					{
 						// 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 = (char) BEGIN_CHAR;
 							metadata.msg_type = MessageTypes[type].ID;
 							metadata.msg_subtype = MessageTypes[type].subtypes[subtype].ID;
-							metadata.msg_id = msgNum++;
-							metadata.data_len = sizeof(floatValue);
+
+							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:
-							valid = checkInt(tokens.tokens[1], &intValue);
-							if(!valid) {
-								return -1;
-							}
-							
 							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 = sizeof(intValue);
 							
+							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;
@@ -160,7 +165,8 @@ int formatPacket(metadata_t *metadata, void *data, unsigned char **formattedComm
 	(*formattedCommand)[2] = metadata->msg_subtype;
 	
 	//Msg id (msgNum is 2 bytes)
-	(*formattedCommand)[3] = metadata->msg_id;
+	(*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;
@@ -176,7 +182,7 @@ int formatPacket(metadata_t *metadata, void *data, unsigned char **formattedComm
 	{
 		data_checksum ^= (*formattedCommand)[i];
 	}
-	
+	printf("checksum = %x\n", (unsigned )data_checksum);
 	(*formattedCommand)[7 + metadata->data_len] = data_checksum;
 	
 	return 0;