From 06150bc1dbfb99222fd79d6fb6857b5fed77e0c9 Mon Sep 17 00:00:00 2001
From: "ucart@co3050-12" <dawehr@iastate.edu>
Date: Thu, 2 Mar 2017 21:41:36 -0600
Subject: [PATCH] Updated logging to work with computation graph. Helps pave
the way for real-time logging soon.
---
.../src/initialize_components.c | 3 +
quad/sw/modular_quad_pid/src/log_data.c | 133 ++++++++++++++----
quad/sw/modular_quad_pid/src/log_data.h | 7 +-
quad/sw/modular_quad_pid/src/type_def.h | 15 +-
4 files changed, 128 insertions(+), 30 deletions(-)
diff --git a/quad/sw/modular_quad_pid/src/initialize_components.c b/quad/sw/modular_quad_pid/src/initialize_components.c
index 0009bac9b..84fc120d3 100644
--- a/quad/sw/modular_quad_pid/src/initialize_components.c
+++ b/quad/sw/modular_quad_pid/src/initialize_components.c
@@ -50,6 +50,9 @@ int initializeAllComponents(user_input_t * user_input_struct, log_t * log_struct
// Initialize the controller
control_algorithm_init(parameter_struct);
+ // Initialize the logging
+ initialize_logging(log_struct, parameter_struct);
+
// Xilinx given initialization
init_platform();
diff --git a/quad/sw/modular_quad_pid/src/log_data.c b/quad/sw/modular_quad_pid/src/log_data.c
index 8a91a40be..a1c2db237 100644
--- a/quad/sw/modular_quad_pid/src/log_data.c
+++ b/quad/sw/modular_quad_pid/src/log_data.c
@@ -14,6 +14,9 @@
#include "sleep.h"
#include "log_data.h"
#include "communication.h"
+#include "computation_graph.h"
+#include "graph_blocks/node_pid.h"
+#include "graph_blocks/node_constant.h"
// Current index of the log array
int arrayIndex = 0;
@@ -26,18 +29,53 @@ 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
+//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;
+float* logArray;
+int row_size;
+
+void addOutputToLog(log_t* log_struct, int controller_id, int output_id) {
+ if (log_struct->n_outputs < MAX_LOG_SIZE) {
+ log_struct->outputs[log_struct->n_outputs].block_id = controller_id;
+ log_struct->outputs[log_struct->n_outputs].sub_id = output_id;
+ log_struct->n_outputs++;
+ }
+}
+void addParamToLog(log_t* log_struct, int controller_id, int output_id) {
+ if (log_struct->n_params < MAX_LOG_SIZE) {
+ log_struct->params[log_struct->n_params].block_id = controller_id;
+ log_struct->params[log_struct->n_params].sub_id = output_id;
+ log_struct->n_params++;
+ }
}
-/**
- * Fills up an xbox hueg amount of memory with log data
- */
-void updateLog(log_t log_struct){
+void initialize_logging(log_t* log_struct, parameter_t* ps) {
+ addOutputToLog(log_struct, ps->alt_pid, PID_CORRECTION);
+ addOutputToLog(log_struct, ps->x_pos_pid, PID_CORRECTION);
+ addOutputToLog(log_struct, ps->y_pos_pid, PID_CORRECTION);
+ addOutputToLog(log_struct, ps->pitch_pid, PID_CORRECTION);
+ addOutputToLog(log_struct, ps->roll_pid, PID_CORRECTION);
+ addOutputToLog(log_struct, ps->yaw_pid, PID_CORRECTION);
+ addOutputToLog(log_struct, ps->pitch_r_pid, PID_CORRECTION);
+ addOutputToLog(log_struct, ps->roll_r_pid, PID_CORRECTION);
+ addOutputToLog(log_struct, ps->yaw_r_pid, PID_CORRECTION);
+ addOutputToLog(log_struct, ps->vrpn_x, CONST_VAL);
+ addOutputToLog(log_struct, ps->vrpn_y, CONST_VAL);
+ addOutputToLog(log_struct, ps->vrpn_alt, CONST_VAL);
+ addOutputToLog(log_struct, ps->vrpn_pitch, CONST_VAL);
+ addOutputToLog(log_struct, ps->vrpn_roll, CONST_VAL);
+ addOutputToLog(log_struct, ps->x_set, CONST_VAL);
+ addOutputToLog(log_struct, ps->y_set, CONST_VAL);
+ addOutputToLog(log_struct, ps->alt_set, CONST_VAL);
+
+ // TODO: Make this not stupid. Adding 6 for IMU and 1 for timestamp
+ row_size = log_struct->n_outputs + log_struct->n_params + 6 + 1;
+ logArray = malloc(sizeof(float) * row_size * LOG_STARTING_SIZE);
+}
+
+
+int log_data(log_t* log_struct, parameter_t* ps)
+{
// 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
@@ -55,15 +93,30 @@ void updateLog(log_t log_struct){
// uart0_sendStr("resized log array.\n");
// sleep(1);
// }
-
if(arrayIndex >= arraySize - 1)
{
- return;
+ return 1;
+ }
+ float* thisRow = &logArray[arrayIndex * row_size];
+ int offset = 0;
+ thisRow[offset++] = log_struct->time_stamp;
+ thisRow[offset++] = log_struct->gam.accel_x;
+ thisRow[offset++] = log_struct->gam.accel_y;
+ thisRow[offset++] = log_struct->gam.accel_z;
+ thisRow[offset++] = log_struct->gam.gyro_xVel_p;
+ thisRow[offset++] = log_struct->gam.gyro_yVel_q;
+ thisRow[offset++] = log_struct->gam.gyro_zVel_r;
+
+ int i;
+ for (i = 0; i < log_struct->n_params; i++) {
+ thisRow[offset++] = graph_get_param_val(ps->graph, log_struct->params[i].block_id, log_struct->params[i].sub_id);
+ }
+ for (i = 0; i < log_struct->n_outputs; i++) {
+ thisRow[offset++] = graph_get_output(ps->graph, log_struct->outputs[i].block_id, log_struct->outputs[i].sub_id);
}
- // Add log to the array
- logArray[arrayIndex++] = log_struct;
-
+ arrayIndex++;
+ return 0;
// If the index is too big, reallocate memory to double the size as before
// if(arrayIndex == arraySize){
// arraySize *= 2;
@@ -83,13 +136,15 @@ void updateLog(log_t log_struct){
* TODO: This should probably be transmitting in binary instead of ascii
*/
-void printLogging(){
+void printLogging(log_t* log_struct, parameter_t* ps){
int i;//, j;
- char buf[2304] = {};
- char comments[256] = {};
- char header[1024] = {};
- char units [1024] = {};
+ char buf[2048] = {};
+ buf[0] = 0; // Mark buffer as size 0
+ char header1[256] = {};
+ char header2[1024] = {};
+ sprintf(header1, "time,accel_x,accel_y,accel_z,gyro_x,gyro_y,gyro_z");
+ /*
sprintf(comments, "# MicroCART On-board Quad Log\r\n# Sample size: %d\r\n", arrayIndex);
sprintf(header, "%%Time\t" "LoopPeriod\t"
@@ -149,25 +204,50 @@ void printLogging(){
"none\tnone\tnone\tnone\n"
);
+*/
+
+ int end = 0;
+ for (i = 0; i < log_struct->n_params; i++) {
+ const char* block_name = ps->graph->nodes[log_struct->params[i].block_id].name;
+ const char* output_name = ps->graph->nodes[log_struct->params[i].block_id].type->param_names[log_struct->params[i].sub_id];
+ end += sprintf(&header2[end], ",%s-%s", block_name, output_name);
+ }
+ for (i = 0; i < log_struct->n_outputs; i++) {
+ const char* block_name = ps->graph->nodes[log_struct->outputs[i].block_id].name;
+ const char* param_name = ps->graph->nodes[log_struct->outputs[i].block_id].type->output_names[log_struct->outputs[i].sub_id];
+ end += sprintf(&header2[end], ",%s-%s", block_name, param_name);
+ }
- strcat(buf,comments);
- strcat(buf,header);
- strcat(buf,units);
- send_data(LOG_ID, 0, buf, strlen(buf) + 1);
+ strcat(buf,header1);
+ strcat(buf,header2);
+
+ send_data(LOG_ID, 0, buf, strlen(buf));
//uart0_sendBytes(buf, strlen(buf));
//usleep(100000);
/*************************/
/* print & send log data */
for(i = 0; i < arrayIndex; i++){
- char* logLine = format(logArray[i]);
- send_data(LOG_ID, 0, logLine, strlen(logLine) + 1);
- free(logLine);
+ int size = format_log(i, log_struct, buf);
+ send_data(LOG_ID, 0, buf, size);
}
}
+int format_log(int idx, log_t* log_struct, char* buf) {
+ int i;
+ int end = 0;
+
+ float* row = &logArray[idx * row_size];\
+
+ end += sprintf(&buf[end], "%f", row[0]);
+ for (i = 1; i < row_size; i++) {
+ end += sprintf(&buf[end], ",%f", row[i]);
+ }
+ return end;
+}
+/*
char* format(log_t log){
char *retString = malloc(4096*2);
@@ -223,3 +303,4 @@ char* format(log_t log){
return retString;
}
+*/
diff --git a/quad/sw/modular_quad_pid/src/log_data.h b/quad/sw/modular_quad_pid/src/log_data.h
index 14d06b3bb..ecfc50d17 100644
--- a/quad/sw/modular_quad_pid/src/log_data.h
+++ b/quad/sw/modular_quad_pid/src/log_data.h
@@ -11,6 +11,7 @@
#define LOG_STARTING_SIZE 4096 //262144 // 2^18 32768 2^15
+void initialize_logging(log_t* log_struct, parameter_t* ps);
/**
* @brief
@@ -23,7 +24,7 @@
* error message
*
*/
- int log_data(log_t* log_struct);
+ int log_data(log_t* log_struct, parameter_t* ps);
/**
* Fills up an xbox hueg amount of memory with log data
@@ -33,8 +34,8 @@
/**
* Prints all the log information.
*/
- void printLogging();
+ void printLogging(log_t* log_struct, parameter_t* ps);
- char* format(log_t log);
+ int format_log(int idx, log_t* log_struct, char* buf);
#endif /* LOG_DATA_H_ */
diff --git a/quad/sw/modular_quad_pid/src/type_def.h b/quad/sw/modular_quad_pid/src/type_def.h
index 10edff1f2..82a0cc6c7 100644
--- a/quad/sw/modular_quad_pid/src/type_def.h
+++ b/quad/sw/modular_quad_pid/src/type_def.h
@@ -22,6 +22,8 @@ enum flight_mode{
MANUAL_FLIGHT_MODE
};
+#define MAX_LOG_SIZE 50
+
//----------------------------------------------------------------------------------------------
// index|| 0 | 1 | 2 | 3 & 4 | 5 & 6 | 7+ | end |
//---------------------------------------------------------------------------------------------|
@@ -169,6 +171,17 @@ typedef struct log_t {
int packetId;
gam_t gam; // Raw and calculated gyro, accel, and mag values are all in gam_t
+
+ struct graph_tuple { // Tuple for
+ int block_id;
+ int sub_id;
+ };
+
+ struct graph_tuple outputs[MAX_LOG_SIZE];
+ struct graph_tuple params[MAX_LOG_SIZE];
+ size_t n_outputs;
+ size_t n_params;
+ /*
float phi_dot, theta_dot, psi_dot; // gimbal equation values
quadPosition_t currentQuadPosition;
@@ -194,7 +207,7 @@ typedef struct log_t {
quadTrims_t trims;
int motors[4];
-
+ */
} log_t;
/**
--
GitLab