log_data.c

/*
 * log_data.c
 *
 *  Created on: Feb 20, 2016
 *      Author: ucart
 */
 
#include "log_data.h"
#include "communication.h"
 
// Current index of the log array
int arrayIndex = 0;
// Size of the array
int arraySize = LOG_STARTING_SIZE;
int resized = 0;

// The number of times we resized the array
int resizeCount = 0;

// Pointer to point to the array with all the logging information
// for now its not dynamic
log_t logArray[LOG_STARTING_SIZE * 3];// up to 60 seconds of log

int log_data(log_t* log_struct)
{
	updateLog(*log_struct);
    return 0;
}

/**
 * Fills up an xbox hueg amount of memory with log data
 */
void updateLog(log_t log_struct){
	// If the first iteration, allocate enough memory for "arraySize" elements of logging
//	if(logArray == NULL){
//		// size in memory is 1,720,320 bytes (1.64 megabytes) because logging struct is 420 bytes each
//		// up to 20 seconds of log before resizing
//		logArray = malloc(LOG_STARTING_SIZE * sizeof(log_t));
//		uart0_sendStr("initialized log array.\n");
//		sleep(1);
//	}

	// semi dynamic log
//	if((arrayIndex >= arraySize - 1) && (!resized)){
//		realloc(logArray, LOG_STARTING_SIZE * 3 * sizeof(log_t)); // up to 60 seconds of log
//		resized = 1;
//		arraySize = LOG_STARTING_SIZE * 3;
//		uart0_sendStr("resized log array.\n");
//		sleep(1);
//	}

	if(arrayIndex >= arraySize - 1)
	{
		return;
	}

	// Add log to the array
	logArray[arrayIndex++] = log_struct;

	// If the index is too big, reallocate memory to double the size as before
//	if(arrayIndex == arraySize){
//		arraySize *= 2;
//		logArray = (log_t *) realloc(logArray, arraySize * sizeof(log_t));
//		++resizeCount;
//	}
//	else if(arrayIndex > arraySize){
//		// Something fishy has occured
//		xil_printf("Array index is out of bounds. This shouldn't happen but somehow you did the impossible\n\r");
//	}
}


/**
 * Prints all the log information.
 *
 * TODO: This should probably be transmitting in binary instead of ascii
 */

void printLogging(){
	int i;//, j;
	char buf[2304] = {};
	char comments[256] = {};
	char header[1024] = {};
	char units [1024] = {};

	sprintf(comments, "# MicroCART On-board Quad Log\r\n# Sample size: %d\r\n", arrayIndex);
	sprintf(header, "%%Time\t" "LoopPeriod\t"

			//current points (measurements)
			"X_Current_Position\t" "Y_Current_Position\t" "Z_Current_Position\t"
			"Cam_Meas_Roll\tCam_Meas_Pitch\tCam_Meas_Yaw\t"
			"Quad_Meas_Roll\tQuad_Meas_Pitch\t"
			"roll_velocity\tpitch_velocity\tyaw_velocity\t"

			//setpoints
			"X_setpoint\t" "Y_setpoint\t" "Z_setpoint\t"
			"Roll_setpoint\tPitch_setpoint\tYaw_setpoint\t"
			"Roll_vel_setpoint\tPitch_vel_setpoint\tYaw_vel_setpoint\t"

			//corrections
			"PID_x\t"
			"PID_y\t"
			"PID_z\t"
			"PID_roll\t"
			"PID_pitch\t"
			"PID_yaw\t"
			"PID_roll_vel\t"
			"PID_pitch_vel\t"
			"PID_yaw_vel\t"

			//trims
			"Roll_trim\tPitch_trim\tYaw_trim\tThrottle_trim\t"

			//motor commands
			"Motor_0\tMotor_1\tMotor_2\tMotor_3\n"

					);


	sprintf(units,	"&sec\tsec\t"

			//current points
			"meters\tmeters\tmeters\t"
			"radians\tradians\tradians\t"
			"radians\tradians\t"
			"radians//sec\tradians//sec\tradians//sec\t"

			//setpoints
			"meters\tmeters\tmeters\t"
			"radians\tradians\tradians\t"
			"radians//sec\tradians//sec\tradians//sec\t"

			//corrections
			"radians\tradians\tradians\t"
			"radians//sec\tradians//sec\tradians//sec\t"
			"none\tnone\tnone\t"

			//trims
			"none\tnone\tnone\tnone\t"

			//motors
			"none\tnone\tnone\tnone\n"

					);

	strcat(buf,comments);
	strcat(buf,header);
	strcat(buf,units);

	// Send a reply to the ground station
	unsigned char *responsePacket;

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

//	printf("Checksum: 0x%02x", responsePacket[metadata.data_len + 7]);

	// Send each byte of the packet individually
	for(i = 0; i < 8 + metadata.data_len; i++) {
		uart0_sendByte(responsePacket[i]);
		if(i < 8 || i == metadata.data_len + 8)
			printf("%d: 0x%02x\n", i, responsePacket[i]);
	}

	//uart0_sendBytes(buf, strlen(buf));
	//usleep(100000);

	/*************************/
	/* print & send log data */
	for(i = 0; i < arrayIndex; i++){
		char* logLine = format(logArray[i]);


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

		// Send each byte of the packet individually
//		for(j = 0; j < 8 + metadata.data_len; j++) {
//			uart0_sendByte(responsePacket[j]);
//			printf("%d: 0x%02x\n", j, responsePacket[j]);
//		}
//		uart0_sendBytes(responsePacket, 8 + metadata.data_len);

		uart0_sendMetaData(metadata);
		uart0_sendBytes(logLine, metadata.data_len);
		//uart0_sendByte(0);
		uart0_sendByte(responsePacket[7 + metadata.data_len]);

//
//		if((i % 5) == 0)
		usleep(15000);

		free(logLine);
		//break;
	}
}


char* format(log_t log){
	char *retString = malloc(4096*2);

			sprintf(retString, 	"%.3f\t%.4f\t" //Time and TimeSlice

								// current points
								"%.3f\t%.3f\t%.3f\t"
								"%.3f\t%.3f\t%.3f\t"
								"%.3f\t%.3f\t"
								"%.3f\t%.3f\t%.3f\t"

								//setpoints
								"%.3f\t%.3f\t%.3f\t"
								"%.3f\t%.3f\t%.3f\t"
								"%.3f\t%.3f\t%.3f\t"

								//corrections
								"%.3f\t%.3f\t%.3f\t"
								"%.3f\t%.3f\t%.3f\t"
								"%.3f\t%.3f\t%.3f\t"

								//trims
								"%.2f\t%.2f\t%.2f\t%.2f\t"

								//motors
								"%d\t%d\t%d\t%d\n"

								,log.time_stamp, log.time_slice,

								// current points
								log.local_x_PID.current_point,log.local_y_PID.current_point, log.altitude_PID.current_point,
								log.currentQuadPosition.roll, log.currentQuadPosition.pitch, log.currentQuadPosition.yaw,
								log.roll_angle_filtered, log.pitch_angle_filtered,
								log.phi_dot, log.theta_dot, log.psi_dot,

								//setpoints
								log.local_x_PID.setpoint, log.local_y_PID.setpoint, log.altitude_PID.setpoint,
								log.angle_roll_PID.setpoint, log.angle_pitch_PID.setpoint, log.angle_yaw_PID.setpoint,
								log.ang_vel_roll_PID.setpoint, log.ang_vel_pitch_PID.setpoint, log.ang_vel_pitch_PID.setpoint,

								//corrections
								log.local_x_PID_values.pid_correction, log.local_y_PID_values.pid_correction, log.altitude_PID_values.pid_correction,
								log.angle_roll_PID_values.pid_correction, log.angle_pitch_PID_values.pid_correction, log.angle_yaw_PID_values.pid_correction,
								log.ang_vel_roll_PID_values.pid_correction, log.ang_vel_pitch_PID_values.pid_correction, log.ang_vel_yaw_PID_values.pid_correction,

								//trims
								log.trims.roll, log.trims.pitch, log.trims.yaw, log.trims.throttle,

								//motors
								log.motors[0], log.motors[1], log.motors[2], log.motors[3]
			);


		return retString;
}