Skip to content
Snippets Groups Projects
Commit 06150bc1 authored by dawehr's avatar dawehr
Browse files

Updated logging to work with computation graph. Helps pave the way for real-time logging soon.

parent 186ad30e
No related branches found
No related tags found
1 merge request!8Controller network
Hide whitespace changes
Inline Side-by-side
quad/sw/modular_quad_pid/src/initialize_components.c
+ 3
− 0
View file @ 06150bc1
......@@ -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();
......
quad/sw/modular_quad_pid/src/log_data.c
+ 107
− 26
View file @ 06150bc1
......@@ -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;
}
*/
quad/sw/modular_quad_pid/src/log_data.h
+ 4
− 3
View file @ 06150bc1
......@@ -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_ */
quad/sw/modular_quad_pid/src/type_def.h
+ 14
− 1
View file @ 06150bc1
......@@ -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;
/**
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment