diff --git a/groundStation/src/backend/backend.c b/groundStation/src/backend/backend.c
index c0043b25606b655874067137e7dad67a6ffb94d2..d801e5ebb1e4c9b39216547b331ea181226f4532 100644
--- a/groundStation/src/backend/backend.c
+++ b/groundStation/src/backend/backend.c
@@ -1011,8 +1011,6 @@ static void quad_recv(int index) {
break;
case SEND_RT_ID:
quadlog_file = fopen("quad_log_data.txt", "w");
- //TODO Add formatting here to populate .txt file in a way QT will be able to read
- char * formatted_data = malloc(sizeof(data) + 1);
fwrite((char *) formatted_data, sizeof(char), m.data_len, quadlog_file);
fclose(quadlog_file);
diff --git a/quad/src/quad_app/log_data.c b/quad/src/quad_app/log_data.c
index 6310f1c1f2f4753ee7e0d6c249e84519c57f75b2..2f0e251d83cd1245645d48d2749f41fa99511ff6 100644
--- a/quad/src/quad_app/log_data.c
+++ b/quad/src/quad_app/log_data.c
@@ -54,12 +54,6 @@ static char units_param_str[512] = {};
static struct str units_output = { .str = units_output_str, .size = 0, .capacity = sizeof(units_output_str)};
static struct str units_param = { .str = units_param_str, .size = 0, .capacity = sizeof(units_output_str)};
-static enum debug_level level;
-
-int set_debug_level(enum debug_level newlevel)
-{
- level = newlevel;
-}
/*
* Does an sprintf and concatenation. Used just like sprintf, but pass in a pointer to a struct str instead.
* Returns the number of bytes that would have been written (just like snprintf)
@@ -97,12 +91,13 @@ void addParamToLog(log_t* log_struct, int controller_id, int param_id, char* uni
}
}
-void initialize_logging(log_t* log_struct, parameter_t* ps) {
+void initialize_logging(log_t* log_struct, parameter_t* ps, SensorRTFlags_t * flags) {
char* rad = "rad";
char* rad_s = "rad/s";
char* pwm_val = "10ns_dutycycle";
char* m = "m";
char* m_s = "m/s";
+
addOutputToLog(log_struct, ps->alt_pid, PID_CORRECTION, pwm_val);
addOutputToLog(log_struct, ps->x_pos_pid, PID_CORRECTION, rad);
addOutputToLog(log_struct, ps->y_pos_pid, PID_CORRECTION, rad);
@@ -142,8 +137,46 @@ void initialize_logging(log_t* log_struct, parameter_t* ps) {
addParamToLog(log_struct, ps->flow_vel_y, CONST_VAL, m_s);
addParamToLog(log_struct, ps->flow_quality, CONST_VAL, "none");
- // TODO: Make this not stupid. Adding 6 for IMU and 1 for timestamp
- row_size = n_outputs + n_params + 9 + 1;
+ int num_outputs = 0;
+ if (flags->imuflags->acc_x)
+ {
+ num_outputs++;
+ }
+ if (flags->imuflags->acc_y)
+ {
+ num_outputs++;
+ }
+ if (flags->imuflags->acc_z)
+ {
+ num_outputs++;
+ }
+ if (flags->imuflags->gyro_x)
+ {
+ num_outputs++;
+ }
+ if (flags->imuflags->gyro_y)
+ {
+ num_outputs++;
+ }
+ if (flags->imuflags->gyro_z)
+ {
+ num_outputs++;
+ }
+ if (flags->imuflags->mag_x)
+ {
+ num_outputs++;
+ }
+ if (flags->imuflags->mag_y)
+ {
+ num_outputs++;
+ }
+ if (flags->imuflags->mag_z)
+ {
+ num_outputs++;
+ }
+ //Track number of outputs desired for log utilizing flags struct. Add 1 for timestamp as
+ //time stamp will always be required for any logging.
+ row_size = n_outputs + n_params + num_outputs + 1;
size_t needed_memory = sizeof(float) * row_size * LOG_STARTING_SIZE;
logArray = malloc(needed_memory);
if (!logArray) { // malloc failed
@@ -153,7 +186,7 @@ void initialize_logging(log_t* log_struct, parameter_t* ps) {
}
}
-int log_data(log_t* log_struct, parameter_t* ps)
+int log_data(log_t* log_struct, parameter_t* ps, SensorRTFlags_t * flags)
{
if(arrayIndex >= arraySize)
{
@@ -161,18 +194,45 @@ int log_data(log_t* log_struct, parameter_t* ps)
}
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;
- thisRow[offset++] = log_struct->gam.mag_x;
- thisRow[offset++] = log_struct->gam.mag_y;
- thisRow[offset++] = log_struct->gam.mag_z;
-
+ //TODO make this not stupid
+ thisRow[offset++] = log_struct->time_stamp;
+ if (flags->imuflags->acc_x)
+ {
+ thisRow[offset++] = log_struct->gam.accel_x;
+ }
+ if (flags->imuflags->acc_y)
+ {
+ thisRow[offset++] = log_struct->gam.accel_y;
+ }
+ if (flags->imuflags->acc_z)
+ {
+ thisRow[offset++] = log_struct->gam.accel_z;
+ }
+ if (flags->imuflags->gyro_x)
+ {
+ thisRow[offset++] = log_struct->gam.gyro_xVel_p;
+ }
+ if (flags->imuflags->gyro_y)
+ {
+ thisRow[offset++] = log_struct->gam.gyro_yVel_q;
+ }
+ if (flags->imuflags->gyro_z)
+ {
+ thisRow[offset++] = log_struct->gam.gyro_zVel_r;
+ }
+ if (flags->imuflags->mag_x)
+ {
+ thisRow[offset++] = log_struct->gam.mag_x;
+ }
+ if (flags->imuflags->mag_y)
+ {
+ thisRow[offset++] = log_struct->gam.mag_y;
+ }
+ if (flags->imuflags->mag_z)
+ {
+ thisRow[offset++] = log_struct->gam.mag_z;
+ }
int i;
for (i = 0; i < n_params; i++) {
thisRow[offset++] = graph_get_param_val(ps->graph, log_params[i].block_id, log_params[i].sub_id);
@@ -180,19 +240,20 @@ int log_data(log_t* log_struct, parameter_t* ps)
for (i = 0; i < n_outputs; i++) {
thisRow[offset++] = graph_get_output(ps->graph, log_outputs[i].block_id, log_outputs[i].sub_id);
}
-
arrayIndex++;
return 0;
}
-/**
- * Print current sensor info to log file
- */
-void RTprintLogging(struct CommDriver *comm, log_t* log_struct, parameter_t* ps, raw_sensor_t* raw_sensors, SensorRTFlags_t * flags){
+void RTprintheader(struct CommDriver *comm, log_t* log_struct, parameter_t* ps, raw_sensor_t* raw_sensors, SensorRTFlags_t * flags) {
if (arrayIndex == 0) {
// Don't send any logs if nothing was logged
return;
}
+ else if (flags->flag_count == 0)
+ {
+ //Don't send anything if flags has not been configured or if no data has been requested to be sent.
+ return;
+ }
char buf_arr[2560] = {};
struct str buf = {.str = buf_arr, .size = 0, .capacity = sizeof(buf_arr)};
@@ -205,40 +266,75 @@ void RTprintLogging(struct CommDriver *comm, log_t* log_struct, parameter_t* ps,
int i;
// Comment header
- safe_sprintf_cat(&buf, "# MicroCART On-board Quad Log\n# Sample size: %d\n", arrayIndex);
+ safe_sprintf_cat(&buf, "# MicroCART On-board Quad Log\n# Sample number: %d\n", arrayIndex);
safe_sprintf_cat(&buf, "# IMU IIC failures: %d\n", raw_sensors->gam.error.consErrorCount);
safe_sprintf_cat(&buf, "# LiDAR IIC failures: %d\n", raw_sensors->lidar.error.consErrorCount);
safe_sprintf_cat(&buf, "# Optical Flow IIC failures: %d\n", raw_sensors->optical_flow.error.consErrorCount);
// List Pid Constants
-
- /*struct graph_node* node = &ps->graph->nodes[n_nodes];
- if (node->type->type_id == BLOCK_PID) {
- double kp, ki, kd, alpha;
- kp = graph_get_param_val(ps->graph, n_nodes, 0);
- ki = graph_get_param_val(ps->graph, n_nodes, 1);
- kd = graph_get_param_val(ps->graph, n_nodes, 2);
- alpha = graph_get_param_val(ps->graph, n_nodes, 3);
- safe_sprintf_cat(&buf, "# %s :\tKp = %lf Ki = %lf Kd = %lf Alpha = %lf\n", node->name, kp, ki, kd, alpha);
-
+ for (i = 0; i < ps->graph->n_nodes; ++i) {
+ struct graph_node* node = &ps->graph->nodes[i];
+ if (node->type->type_id == BLOCK_PID) {
+ double kp, ki, kd, alpha;
+ kp = graph_get_param_val(ps->graph, i, 0);
+ ki = graph_get_param_val(ps->graph, i, 1);
+ kd = graph_get_param_val(ps->graph, i, 2);
+ alpha = graph_get_param_val(ps->graph, i, 3);
+ safe_sprintf_cat(&buf, "# %s :\tKp = %lf Ki = %lf Kd = %lf Alpha = %lf\n", node->name, kp, ki, kd, alpha);
+ }
}
- */
- // Header names for the pre-defined values
- safe_sprintf_cat(&buf, "%%Time\taccel_x\taccel_y\taccel_z\tgyro_x\tgyro_y\tgyro_z\tmag_x\tmag_y\tmag_z");
+ // TODO make this not stupid
+ safe_sprintf_cat(&buf, "%%Time");
+ if (flags->imuflags->acc_x)
+ {
+ safe_sprintf_cat(&buf, "\taccel_x");
+ }
+ if (flags->imuflags->acc_y)
+ {
+ safe_sprintf_cat(&buf, "\taccel_y");
+ }
+ if (flags->imuflags->acc_z)
+ {
+ safe_sprintf_cat(&buf, "\taccel_z");
+ }
+ if (flags->imuflags->gyro_x)
+ {
+ safe_sprintf_cat(&buf, "\tgyro_x");
+ }
+ if (flags->imuflags->gyro_y)
+ {
+ safe_sprintf_cat(&buf, "\tgyro_y");
+ }
+ if (flags->imuflags->gyro_z)
+ {
+ safe_sprintf_cat(&buf, "\tgyro_z");
+ }
+ if (flags->imuflags->mag_x)
+ {
+ safe_sprintf_cat(&buf, "\tmag_x");
+ }
+ if (flags->imuflags->mag_y)
+ {
+ safe_sprintf_cat(&buf, "\tmag_y_x");
+ }
+ if (flags->imuflags->mag_z)
+ {
+ safe_sprintf_cat(&buf, "\tmag_z");
+ }
// Print all the recorded block parameters
-
- const char* block_name = ps->graph->nodes[log_params[n_params].block_id].name;
- const char* output_name = ps->graph->nodes[log_params[n_params].block_id].type->param_names[log_params[n_params].sub_id];
- safe_sprintf_cat(&buf, "\t%s_%s", block_name, output_name);
-
+ for (i = 0; i < n_params; i++) {
+ const char* block_name = ps->graph->nodes[log_params[i].block_id].name;
+ const char* output_name = ps->graph->nodes[log_params[i].block_id].type->param_names[log_params[i].sub_id];
+ safe_sprintf_cat(&buf, "\t%s_%s", block_name, output_name);
+ }
// Print all the recorded block outputs
-
- block_name = ps->graph->nodes[log_outputs[n_outputs].block_id].name;
- const char* param_name = ps->graph->nodes[log_outputs[n_outputs].block_id].type->output_names[log_outputs[n_outputs].sub_id];
- safe_sprintf_cat(&buf, "\t%s_%s", block_name, param_name);
-
+ for (i = 0; i < n_outputs; i++) {
+ const char* block_name = ps->graph->nodes[log_outputs[i].block_id].name;
+ const char* param_name = ps->graph->nodes[log_outputs[i].block_id].type->output_names[log_outputs[i].sub_id];
+ safe_sprintf_cat(&buf, "\t%s_%s", block_name, param_name);
+ }
safe_sprintf_cat(&buf, "\n");
// Send header names
@@ -246,28 +342,88 @@ void RTprintLogging(struct CommDriver *comm, log_t* log_struct, parameter_t* ps,
// Send units header
buf.size = 0;
- safe_sprintf_cat(&buf, "&s\tG\tG\tG\trad/s\trad/s\trad/s\tuT\tuT\tuT"); // The pre-defined ones
+
+ safe_sprintf_cat(&buf, "&s"); // The pre-defined ones
+ if (flags->imuflags->acc_x)
+ {
+ safe_sprintf_cat(&buf, "\tG");
+ }
+ if (flags->imuflags->acc_y)
+ {
+ safe_sprintf_cat(&buf, "\tG");
+ }
+ if (flags->imuflags->acc_z)
+ {
+ safe_sprintf_cat(&buf, "\tG");
+ }
+ if (flags->imuflags->gyro_x)
+ {
+ safe_sprintf_cat(&buf, "\trad/s");
+ }
+ if (flags->imuflags->gyro_y)
+ {
+ safe_sprintf_cat(&buf, "\trad/s");
+ }
+ if (flags->imuflags->gyro_z)
+ {
+ safe_sprintf_cat(&buf, "\trad/s");
+ }
+ if (flags->imuflags->mag_x)
+ {
+ safe_sprintf_cat(&buf, "\tuT");
+ }
+ if (flags->imuflags->mag_y)
+ {
+ safe_sprintf_cat(&buf, "\tuT");
+ }
+ if (flags->imuflags->mag_z)
+ {
+ safe_sprintf_cat(&buf, "\tuT");
+ }
+
safe_sprintf_cat(&buf, units_output.str);
safe_sprintf_cat(&buf, units_param.str);
safe_sprintf_cat(&buf, "\n");
send_data(comm->uart, LOG_ID, 0, (u8*)buf.str, buf.size);
+}
- /*************************/
- /* print & send log data */
-
- format_log(arrayIndex, log_struct, &buf);
+/**
+ * Print current sensor info to log file. This data is held at the current ArrayIndex entry of the log_struct.
+ */
+void RTprintLogging(struct CommDriver *comm, log_t* log_struct, parameter_t* ps, raw_sensor_t* raw_sensors, SensorRTFlags_t * flags){
+ if (arrayIndex == 0)
+ {
+ return;
+ }
+ else if (flags->flag_count == 0)
+ {
+ //Don't send anything if flags has not been configured or if no data has been requested to be sent.
+ return;
+ }
+ char buf_arr[2560] = {};
+ struct str buf = {.str = buf_arr, .size = 0, .capacity = sizeof(buf_arr)};
+ //Send only the most previous log data.
+ format_log(arrayIndex - 1, log_struct, &buf);
send_data(comm->uart, LOG_ID, 0, (u8*)buf.str, buf.size);
// This is a stupid hack because if the axi timer is not reset in awhile, it always returns 0, and the timer_end_loop() call hangs forever after a long log
// Not 100% certain this works
timer_axi_reset();
-
+}
+void RTprintend();
+{
+ if (arrayIndex == 0)
+ {
+ return;
+ }
+ char buf_arr[2560] = {};
+ struct str buf = {.str = buf_arr, .size = 0, .capacity = sizeof(buf_arr)};
// Empty message of type LOG_END to indicate end of log
send_data(comm->uart, LOG_END_ID, 0, (u8*)buf.str, 0);
}
/**
- * Prints all the log information.
+ * Prints all the log information. Currently replaced by the RTprintglogging method.
*
* TODO: This should probably be transmitting in binary instead of ascii
*/
@@ -304,7 +460,6 @@ void printLogging(struct CommDriver *comm, log_t* log_struct, parameter_t* ps, r
kd = graph_get_param_val(ps->graph, i, 2);
alpha = graph_get_param_val(ps->graph, i, 3);
safe_sprintf_cat(&buf, "# %s :\tKp = %lf Ki = %lf Kd = %lf Alpha = %lf\n", node->name, kp, ki, kd, alpha);
-
}
}
diff --git a/quad/src/quad_app/log_data.h b/quad/src/quad_app/log_data.h
index f74e4cc69326edae5a37249c5724d06c2f98aabd..e7f29061cf97cf8fc5f2138dbed3ff542194f364 100644
--- a/quad/src/quad_app/log_data.h
+++ b/quad/src/quad_app/log_data.h
@@ -13,9 +13,7 @@
// Maximum number of block outputs you can record, and maximum number of block parameters to record
#define MAX_LOG_NUM 50
-enum debug_level{LEVEL_ONE, LEVEL_TWO, LEVEL_THREE, LEVEL_FOUR};
-
-void initialize_logging(log_t* log_struct, parameter_t* ps);
+void initialize_logging(log_t* log_struct, parameter_t* ps, SensorRTFlags_t * flags);
/**
* Adds the given block output to the data to be logged
@@ -39,7 +37,7 @@ void addParamToLog(log_t* log_struct, int controller_id, int param_id, char* uni
* error message
*
*/
- int log_data(log_t* log_struct, parameter_t* ps);
+ int log_data(log_t* log_struct, parameter_t* ps, SensorRTFlags_t * flags);
/**
* Fills up an xbox hueg amount of memory with log data
diff --git a/quad/src/quad_app/quad_app.c b/quad/src/quad_app/quad_app.c
index 4f62549cc543fa71de36f74e744fd96a130436bc..636c683a4a4a2f68f4a7b6c03cc86e7f66696b4b 100644
--- a/quad/src/quad_app/quad_app.c
+++ b/quad/src/quad_app/quad_app.c
@@ -20,13 +20,14 @@
int quad_main(int (*setup_hardware)(hardware_t *hardware_struct))
{
+ u8 init_cond = 0x0;
// Structures to be used throughout
modular_structs_t structs = { };
- //flags that indicate which rt data to send
- SensorRTFlags_t flags;
+ //pointer to flags struct that indicate which rt data to send
+ SensorRTFlags_t * flags;
- // initialize internal flag data for RT data transfer
+ // initialize internal flag data for RT data transfer
initializeFlags(&flags);
// Wire up hardware
@@ -47,7 +48,6 @@ int quad_main(int (*setup_hardware)(hardware_t *hardware_struct))
int last_kill_condition = kill_condition(&(structs.user_input_struct));
-
// Main control loop
while (1)
{
@@ -65,10 +65,6 @@ int quad_main(int (*setup_hardware)(hardware_t *hardware_struct))
// Get data from the sensors and put it into raw_sensor_struct
get_sensors(&(structs.hardware_struct), &(structs.log_struct), &(structs.user_input_struct), &(structs.raw_sensor_struct));
-
- //Send the sensor data in RT (TODO WILL NEED TO BE EDITED FROM CURRENT CONDITION DUE TO TINA)
- if (flags.flag_count != 0)
- send_RT_data(&(structs.hardware_struct.comm), &(structs.raw_sensor_struct), &flags);
// Process the sensor data and put it into sensor_struct
sensor_processing(&(structs.log_struct), &(structs.user_input_struct), &(structs.raw_sensor_struct), &(structs.sensor_struct));
@@ -106,8 +102,14 @@ int quad_main(int (*setup_hardware)(hardware_t *hardware_struct))
if (!this_kill_condition) {
// Log the data collected in this loop
+ if (init_cond == 0x0)
+ {
+ init_cond = 0x1;
+ initialize_logging(&(structs.log_struct), &(structs.parameter_struct), &flags);
+ RTprintheader(&structs.hardware_struct.comm, &(structs.log_struct), &(structs.parameter_struct), &structs.raw_sensor_struct, &flags);
+ }
log_data(&(structs.log_struct), &(structs.parameter_struct));
-
+ RTprintLogging(&structs.hardware_struct.comm, &(structs.log_struct), &(structs.parameter_struct), &structs.raw_sensor_struct, &flags);
if(structs.user_defined_struct.flight_mode == AUTO_FLIGHT_MODE)
{
static int loop_counter = 0;
@@ -132,8 +134,7 @@ int quad_main(int (*setup_hardware)(hardware_t *hardware_struct))
}
if (this_kill_condition == 1 && last_kill_condition == 0) {
- // Just disabled
- printLogging(&structs.hardware_struct.comm, &(structs.log_struct), &(structs.parameter_struct), &structs.raw_sensor_struct);
+ RTprintend();
resetLogging();
MIO7_led_off();
}