diff --git a/quad/src/quad_app/log_data.c b/quad/src/quad_app/log_data.c
index a72129f13aa22942f1884e3c3608f7227c21405e..d7a361dfe69963339efd502e89cf697446bfe5eb 100644
--- a/quad/src/quad_app/log_data.c
+++ b/quad/src/quad_app/log_data.c
@@ -8,6 +8,7 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
+#include <stdarg.h>
#include "PID.h"
#include "type_def.h"
#include "log_data.h"
@@ -16,7 +17,7 @@
#include "node_pid.h"
#include "node_constant.h"
#include "node_mixer.h"
-
+
// Current index of the log array
int arrayIndex = 0;
// Size of the array
@@ -27,12 +28,20 @@ struct graph_tuple { // Tuple for
int sub_id;
};
+// Holds a statically allocated string, as well as info about its size and capacity
+// Used by safe_sprintf_cat
struct str {
char* str;
size_t size;
size_t capacity;
};
+
+ /**
+ * Forward declarations
+ */
+ void format_log(int idx, log_t* log_struct, struct str* buf);
+
struct graph_tuple log_outputs[MAX_LOG_NUM];
struct graph_tuple log_params[MAX_LOG_NUM];
size_t n_outputs;
@@ -46,32 +55,40 @@ 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)};
-void safe_strcat(struct str *str, const char* to_add) {
- size_t add_len = strlen(to_add);
- if (add_len + str->size <= str->capacity) {
- strcpy(&(str->str[str->size]), to_add);
- str->size += add_len;
+/*
+ * 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)
+ * Will not write more than is available in the given string
+*/
+int safe_sprintf_cat(struct str *str, const char *fmt, ...) {
+ size_t available = str->capacity - str->size;
+ va_list args;
+ va_start(args, fmt);
+ // Print to offset not more than remaining capacity characters
+ size_t full_size = vsnprintf(str->str + str->size, available, fmt, args);
+ va_end(args);
+ if (full_size >= available) { // Check for truncation
+ str->size = str->capacity;
+ } else {
+ str->size = str->size + full_size;
}
+ return full_size;
}
void addOutputToLog(log_t* log_struct, int controller_id, int output_id, char* units) {
- static char units_buf[64];
if (n_outputs < MAX_LOG_NUM) {
log_outputs[n_outputs].block_id = controller_id;
log_outputs[n_outputs].sub_id = output_id;
n_outputs++;
- snprintf(units_buf, sizeof(units_buf), "\t%s", units);
- safe_strcat(&units_output, units_buf);
+ safe_sprintf_cat(&units_output, "\t%s", units);
}
}
void addParamToLog(log_t* log_struct, int controller_id, int param_id, char* units) {
- static char units_buf[64];
if (n_params < MAX_LOG_NUM) {
log_params[n_params].block_id = controller_id;
log_params[n_params].sub_id = param_id;
n_params++;
- snprintf(units_buf, sizeof(units_buf), "\t%s", units);
- safe_strcat(&units_param, units_buf);
+ safe_sprintf_cat(&units_param, "\t%s", units);
}
}
@@ -158,78 +175,73 @@ void printLogging(hardware_t *hardware_struct, log_t* log_struct, parameter_t* p
// Don't send any logs if nothing was logged
return;
}
- int i;//, j;
- // TODO: Change these to struct str so we can use safe_strcat
- char buf[2560] = {};
- buf[0] = 0; // Mark buffer as size 0
- char comments[256] = {};
- char header1[256] = {};
- char header2[1024] = {};
+ char buf_arr[2560] = {};
+ struct str buf = {.str = buf_arr, .size = 0, .capacity = sizeof(buf_arr)};
// Let user know that allocation failed
if (arraySize != LOG_STARTING_SIZE) {
- size_t send_len = sprintf(header1, "Failed to allocate enough log memory\n");
- send_data(&hardware_struct->uart, LOG_ID, 0, header1, send_len);
+ safe_sprintf_cat(&buf, "Failed to allocate enough log memory\n");
+ send_data(&hardware_struct->uart, LOG_ID, 0, buf.str, buf.size);
return;
}
+ // Comment header
+ safe_sprintf_cat(&buf, "# MicroCART On-board Quad Log\n# Sample size: %d\n", arrayIndex);
+ // Header names for the pre-defined values
+ safe_sprintf_cat(&buf, "time\taccel_x\taccel_y\taccel_z\tgyro_x\tgyro_y\tgyro_z");
- sprintf(comments, "# MicroCART On-board Quad Log\n# Sample size: %d\n", arrayIndex);
- sprintf(header1, "time\taccel_x\taccel_y\taccel_z\tgyro_x\tgyro_y\tgyro_z");
-
- int end = 0;
+ int i;
// Print all the recorded block parameters
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];
- end += sprintf(&header2[end], "\t%s_%s", block_name, output_name);
+ safe_sprintf_cat(&buf, "\t%s_%s", block_name, output_name);
}
// Print all the recorded block outputs
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];
- end += sprintf(&header2[end], "\t%s_%s", block_name, param_name);
+ safe_sprintf_cat(&buf, "\t%s_%s", block_name, param_name);
}
- end += sprintf(&header2[end], "\n");
+ safe_sprintf_cat(&buf, "\n");
// Send header names
- strcat(buf, comments);
- strcat(buf, header1);
- strcat(buf, header2);
- send_data(&hardware_struct->uart, LOG_ID, 0, buf, strlen(buf));
+ send_data(&hardware_struct->uart, LOG_ID, 0, buf.str, buf.size);
// Send units header
- buf[0] = 0;
- strcat(buf, "s\tG\tG\tG\trad/s\trad/s\trad/s"); // The pre-defined ones
- strcat(buf, units_output.str);
- strcat(buf, units_param.str);
- strcat(buf, "\n");
- send_data(&hardware_struct->uart, LOG_ID, 0, buf, strlen(buf));
+ buf.size = 0;
+ safe_sprintf_cat(&buf, "s\tG\tG\tG\trad/s\trad/s\trad/s"); // The pre-defined ones
+ safe_sprintf_cat(&buf, units_output.str);
+ safe_sprintf_cat(&buf, units_param.str);
+ safe_sprintf_cat(&buf, "\n");
+ send_data(&hardware_struct->uart, LOG_ID, 0, buf.str, buf.size);
/*************************/
/* print & send log data */
for(i = 0; i < arrayIndex; i++){
- int size = format_log(i, log_struct, buf);
- send_data(&hardware_struct->uart, LOG_ID, 0, buf, size);
+ format_log(i, log_struct, &buf);
+ send_data(&hardware_struct->uart, LOG_ID, 0, buf.str, buf.size);
}
- char data[1] = {0}; // 1 byte to make compilers happy
- send_data(&hardware_struct->uart, LOG_END_ID, 0, data, 0);
+ // Empty message of type LOG_END to indicate end of log
+ send_data(&hardware_struct->uart, LOG_END_ID, 0, buf.str, 0);
}
void resetLogging() {
arrayIndex = 0;
}
-int format_log(int idx, log_t* log_struct, char* buf) {
+/*
+ * Fills the given buffer as ASCII for the recorded index, and returns the length of the string created
+*/
+void format_log(int idx, log_t* log_struct, struct str* buf) {
int i;
- int end = 0;
-
+ buf->size = 0;
+
float* row = &logArray[idx * row_size * sizeof(float)];\
- end += sprintf(&buf[end], "%f", row[0]);
+ safe_sprintf_cat(buf, "%f", row[0]);
for (i = 1; i < row_size; i++) {
- end += sprintf(&buf[end], "\t%f", row[i]);
+ safe_sprintf_cat(buf, "\t%f", row[i]);
}
- end += sprintf(&buf[end], "\n");
- return end;
+ safe_sprintf_cat(buf, "\n");
}
diff --git a/quad/src/quad_app/log_data.h b/quad/src/quad_app/log_data.h
index 1dd2d1741de4e08be70910221fe67baee995eced..34f1658d02f316918547bc0eedd760bcbf121264 100644
--- a/quad/src/quad_app/log_data.h
+++ b/quad/src/quad_app/log_data.h
@@ -54,9 +54,5 @@ void addParamToLog(log_t* log_struct, int controller_id, int param_id, char* uni
*/
void resetLogging();
- /**
- * Fills the given buffer as ASCII for the recorded index, and returns the length of the string created
- */
- int format_log(int idx, log_t* log_struct, char* buf);
#endif /* LOG_DATA_H_ */