quad/sw/modular_quad_pid/src/new_log_data.c

+/*
+ * log_data.c
+ *
+ *  Created on: Feb 20, 2016
+ *      Author: ucart
+ */
+ 
+/*
+ #include "log_data.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, numBytes;
+	char buf[2304] = {};
+	char comments[256] = {};
+	char header[1024] = {};
+	char units [1024] = {};
+
+	char tempLog[4096*2] = {};
+
+	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);
+
+
+	numBytes = logData(buf, tempLog);
+	uart0_sendBytes(tempLog, strlen(tempLog));
+	usleep(100000);
+
+	***********************
+	 print & send log data
+	for(i = 0; i < arrayIndex; i++){
+			char* logLine = format(logArray[i]);
+			numBytes = logData(logLine, tempLog);
+			uart0_sendBytes(tempLog, numBytes);
+			usleep(10000);
+			//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
+								"%d\t%d\t%d\t%d\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;
+}
+*/

quad/sw/modular_quad_pid/src/new_log_data.h

+/*
+ * log_data.h
+ *
+ *  Created on: Feb 20, 2016
+ *      Author: ucart
+ */
+
+#ifndef LOG_DATA_H_
+#define LOG_DATA_H_
+ 
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include "PID.h"
+#include "type_def.h"
+#include "uart.h"
+#include "sleep.h"
+#include "communication.h"
+
+#define LOG_STARTING_SIZE 4096 //262144 // 2^18      32768  2^15
+
+
+/**
+ * @brief 
+ *      Logs the data obtained throughout the controller loop.
+ *
+ * @param log_struct
+ *      structure of the data to be logged
+ *
+ * @return 
+ *      error message
+ *
+ */
+ int log_data(log_t* log_struct);
+
+ /**
+  * Fills up an xbox hueg amount of memory with log data
+  */
+ void updateLog(log_t log_struct);
+
+ /**
+  * Prints all the log information.
+  */
+ void printLogging();
+
+ char* format(log_t log);
+
+#endif /* LOG_DATA_H_ */

quad/sw/modular_quad_pid/src/old_log_data.h

+/*
+ * log_data.h
+ *
+ *  Created on: Feb 20, 2016
+ *      Author: ucart
+ */
+
+#ifndef LOG_DATA_H_
+#define LOG_DATA_H_
+ 
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include "PID.h"
+#include "type_def.h"
+#include "uart.h"
+#include "sleep.h"
+#include "communication.h"
+
+#define LOG_STARTING_SIZE 4096 //262144 // 2^18      32768  2^15
+
+
+/**
+ * @brief 
+ *      Logs the data obtained throughout the controller loop.
+ *
+ * @param log_struct
+ *      structure of the data to be logged
+ *
+ * @return 
+ *      error message
+ *
+ */
+ int log_data(log_t* log_struct);
+
+ /**
+  * Fills up an xbox hueg amount of memory with log data
+  */
+ void updateLog(log_t log_struct);
+
+ /**
+  * Prints all the log information.
+  */
+ void printLogging();
+
+ char* format(log_t log);
+
+#endif /* LOG_DATA_H_ */

quad/sw/modular_quad_pid/src/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;
+}

quad/sw/modular_quad_pid/src/packet_processing.h

+/*
+ * process_packet.h
+ *
+ *  Created on: Mar 2, 2016
+ *      Author: ucart
+ */
+
+#ifndef PROCESS_PACKET_H_
+#define PROCESS_PACKET_H_
+
+#include <stdlib.h>
+#include <string.h>
+#include <stdio.h>
+#include "type_def.h"
+
+tokenList_t tokenize(char* cmd);
+int processUpdate(unsigned char* update, quadPosition_t* currentQuadPosition);
+//int processCommand(stringBuilder_t * sb, setpoint_t * setpoint_struct, parameter_t * parameter_struct);
+int doProcessing(char* cmd, tokenList_t * tokens, setpoint_t * setpoint_struct, parameter_t * parameter_struct);
+float getFloat(unsigned char* str, int pos);
+int getInt(unsigned char* str, int pos);
+
+#endif /* PROCESS_PACKET_H_ */

quad/sw/modular_quad_pid/src/platform.c

+/******************************************************************************
+*
+* (c) Copyright 2010-2012 Xilinx, Inc. All rights reserved.
+*
+* This file contains confidential and proprietary information of Xilinx, Inc.
+* and is protected under U.S. and international copyright and other
+* intellectual property laws.
+*
+* DISCLAIMER
+* This disclaimer is not a license and does not grant any rights to the
+* materials distributed herewith. Except as otherwise provided in a valid
+* license issued to you by Xilinx, and to the maximum extent permitted by
+* applicable law: (1) THESE MATERIALS ARE MADE AVAILABLE "AS IS" AND WITH ALL
+* FAULTS, AND XILINX HEREBY DISCLAIMS ALL WARRANTIES AND CONDITIONS, EXPRESS,
+* IMPLIED, OR STATUTORY, INCLUDING BUT NOT LIMITED TO WARRANTIES OF
+* MERCHANTABILITY, NON-INFRINGEMENT, OR FITNESS FOR ANY PARTICULAR PURPOSE;
+* and (2) Xilinx shall not be liable (whether in contract or tort, including
+* negligence, or under any other theory of liability) for any loss or damage
+* of any kind or nature related to, arising under or in connection with these
+* materials, including for any direct, or any indirect, special, incidental,
+* or consequential loss or damage (including loss of data, profits, goodwill,
+* or any type of loss or damage suffered as a result of any action brought by
+* a third party) even if such damage or loss was reasonably foreseeable or
+* Xilinx had been advised of the possibility of the same.
+*
+* CRITICAL APPLICATIONS
+* Xilinx products are not designed or intended to be fail-safe, or for use in
+* any application requiring fail-safe performance, such as life-support or
+* safety devices or systems, Class III medical devices, nuclear facilities,
+* applications related to the deployment of airbags, or any other applications
+* that could lead to death, personal injury, or severe property or
+* environmental damage (individually and collectively, "Critical
+* Applications"). Customer assumes the sole risk and liability of any use of
+* Xilinx products in Critical Applications, subject only to applicable laws
+* and regulations governing limitations on product liability.
+*
+* THIS COPYRIGHT NOTICE AND DISCLAIMER MUST BE RETAINED AS PART OF THIS FILE
+* AT ALL TIMES.
+*
+******************************************************************************/
+
+#include "xparameters.h"
+#include "xil_cache.h"
+
+#include "platform_config.h"
+
+/*
+ * Uncomment the following line if ps7 init source files are added in the
+ * source directory for compiling example outside of SDK.
+ */
+/*#include "ps7_init.h"*/
+
+#ifdef STDOUT_IS_16550
+ #include "xuartns550_l.h"
+
+ #define UART_BAUD 9600
+#endif
+
+void
+enable_caches()
+{
+#ifdef __PPC__
+    Xil_ICacheEnableRegion(CACHEABLE_REGION_MASK);
+    Xil_DCacheEnableRegion(CACHEABLE_REGION_MASK);
+#elif __MICROBLAZE__
+#ifdef XPAR_MICROBLAZE_USE_ICACHE
+    Xil_ICacheEnable();
+#endif
+#ifdef XPAR_MICROBLAZE_USE_DCACHE
+    Xil_DCacheEnable();
+#endif
+#endif
+}
+
+void
+disable_caches()
+{
+    Xil_DCacheDisable();
+    Xil_ICacheDisable();
+}
+
+void
+init_uart()
+{
+#ifdef STDOUT_IS_16550
+    XUartNs550_SetBaud(STDOUT_BASEADDR, XPAR_XUARTNS550_CLOCK_HZ, UART_BAUD);
+    XUartNs550_SetLineControlReg(STDOUT_BASEADDR, XUN_LCR_8_DATA_BITS);
+#endif
+#ifdef STDOUT_IS_PS7_UART
+    /* Bootrom/BSP configures PS7 UART to 115200 bps */
+#endif
+}
+
+void
+init_platform()
+{
+    /*
+     * If you want to run this example outside of SDK,
+     * uncomment the following line and also #include "ps7_init.h" at the top.
+     * Make sure that the ps7_init.c and ps7_init.h files are included
+     * along with this example source files for compilation.
+     */
+    /* ps7_init();*/
+    enable_caches();
+    init_uart();
+}
+
+void
+cleanup_platform()
+{
+    disable_caches();
+}

quad/sw/modular_quad_pid/src/platform.h

+/*
+ * Copyright (c) 2008 Xilinx, Inc.  All rights reserved.
+ *
+ * Xilinx, Inc.
+ * XILINX IS PROVIDING THIS DESIGN, CODE, OR INFORMATION "AS IS" AS A
+ * COURTESY TO YOU.  BY PROVIDING THIS DESIGN, CODE, OR INFORMATION AS
+ * ONE POSSIBLE   IMPLEMENTATION OF THIS FEATURE, APPLICATION OR
+ * STANDARD, XILINX IS MAKING NO REPRESENTATION THAT THIS IMPLEMENTATION
+ * IS FREE FROM ANY CLAIMS OF INFRINGEMENT, AND YOU ARE RESPONSIBLE
+ * FOR OBTAINING ANY RIGHTS YOU MAY REQUIRE FOR YOUR IMPLEMENTATION.
+ * XILINX EXPRESSLY DISCLAIMS ANY WARRANTY WHATSOEVER WITH RESPECT TO
+ * THE ADEQUACY OF THE IMPLEMENTATION, INCLUDING BUT NOT LIMITED TO
+ * ANY WARRANTIES OR REPRESENTATIONS THAT THIS IMPLEMENTATION IS FREE
+ * FROM CLAIMS OF INFRINGEMENT, IMPLIED WARRANTIES OF MERCHANTABILITY
+ * AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ */
+
+#ifndef __PLATFORM_H_
+#define __PLATFORM_H_
+
+#include "platform_config.h"
+
+void init_platform();
+void cleanup_platform();
+
+#endif

quad/sw/modular_quad_pid/src/platform_config.h

+#ifndef __PLATFORM_CONFIG_H_
+#define __PLATFORM_CONFIG_H_
+
+#define STDOUT_IS_PS7_UART
+#define UART_DEVICE_ID 0
+#ifdef __PPC__
+#define CACHEABLE_REGION_MASK 0xf0000001
+#endif
+
+#endif

quad/sw/modular_quad_pid/src/quadposition.h

+#ifndef _QUADPOSITION_H
+#define _QUADPOSITION_H
+
+
+
+#endif /* _QUADPOSITION_H */

quad/sw/modular_quad_pid/src/send_actuator_commands.c

+/*
+ * send_actuator_commands.c
+ *
+ *  Created on: Feb 20, 2016
+ *      Author: ucart
+ */
+ 
+#include "send_actuator_commands.h"
+ 
+int send_actuator_commands(log_t* log_struct, actuator_command_t* actuator_command_struct)
+{
+	int i;
+	// write the PWMs to the motors
+	for (i = 0; i < 4; i++)
+		pwm_write_channel(actuator_command_struct->pwms[i], i);
+
+    log_struct->motors[0] = actuator_command_struct->pwms[0];
+    log_struct->motors[1] = actuator_command_struct->pwms[1];
+    log_struct->motors[2] = actuator_command_struct->pwms[2];
+    log_struct->motors[3] = actuator_command_struct->pwms[3];
+
+    return 0;
+}
+