From 0008bdfc7ce333d858b667db9d36b7c3047b3c34 Mon Sep 17 00:00:00 2001
From: David Wehr <dawehr@iastate.edu>
Date: Fri, 17 Mar 2017 19:57:36 -0500
Subject: [PATCH] Updated quad logging to use standard format.
---
quad/src/quad_app/log_data.c | 109 ++++++++++++++++++++++++-----------
1 file changed, 75 insertions(+), 34 deletions(-)
diff --git a/quad/src/quad_app/log_data.c b/quad/src/quad_app/log_data.c
index 2c789b1a9..6c0a1698b 100644
--- a/quad/src/quad_app/log_data.c
+++ b/quad/src/quad_app/log_data.c
@@ -27,6 +27,12 @@ struct graph_tuple { // Tuple for
int sub_id;
};
+struct str {
+ char* str;
+ int size;
+ size_t capacity;
+};
+
struct graph_tuple log_outputs[MAX_LOG_NUM];
struct graph_tuple log_params[MAX_LOG_NUM];
size_t n_outputs;
@@ -35,46 +41,70 @@ size_t n_params;
float* logArray;
int row_size;
-void addOutputToLog(log_t* log_struct, int controller_id, int output_id) {
+static char units_output_str[512] = {};
+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, 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 += to_add;
+ }
+}
+
+void addOutputToLog(log_t* log_struct, int controller_id, int output_id, char* units) {
+ static 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);
}
}
-void addParamToLog(log_t* log_struct, int controller_id, int param_id) {
+void addParamToLog(log_t* log_struct, int controller_id, int param_id, char* units) {
+ static 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);
}
}
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->cur_pitch, CONST_VAL);
- addOutputToLog(log_struct, ps->cur_roll, CONST_VAL);
- addOutputToLog(log_struct, ps->cur_yaw, CONST_VAL);
- 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);
- addOutputToLog(log_struct, ps->mixer, MIXER_PWM0);
- addOutputToLog(log_struct, ps->mixer, MIXER_PWM1);
- addOutputToLog(log_struct, ps->mixer, MIXER_PWM2);
- addOutputToLog(log_struct, ps->mixer, MIXER_PWM3);
+ char* rad = "rad";
+ char* rad_s = "rad/s";
+ char* pwm_val = "10us_dutycycle";
+ char* m = "m";
+ 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);
+ addOutputToLog(log_struct, ps->pitch_pid, PID_CORRECTION, rad_s);
+ addOutputToLog(log_struct, ps->roll_pid, PID_CORRECTION, rad_s);
+ addOutputToLog(log_struct, ps->yaw_pid, PID_CORRECTION, rad_s);
+ addOutputToLog(log_struct, ps->pitch_r_pid, PID_CORRECTION, pwm_val);
+ addOutputToLog(log_struct, ps->roll_r_pid, PID_CORRECTION, pwm_val);
+ addOutputToLog(log_struct, ps->yaw_r_pid, PID_CORRECTION, pwm_val);
+ addOutputToLog(log_struct, ps->cur_pitch, CONST_VAL, rad);
+ addOutputToLog(log_struct, ps->cur_roll, CONST_VAL, rad);
+ addOutputToLog(log_struct, ps->cur_yaw, CONST_VAL, rad);
+ addOutputToLog(log_struct, ps->vrpn_x, CONST_VAL, m);
+ addOutputToLog(log_struct, ps->vrpn_y, CONST_VAL, m);
+ addOutputToLog(log_struct, ps->vrpn_alt, CONST_VAL, m);
+ addOutputToLog(log_struct, ps->vrpn_pitch, CONST_VAL, rad);
+ addOutputToLog(log_struct, ps->vrpn_roll, CONST_VAL, rad);
+ addOutputToLog(log_struct, ps->x_set, CONST_VAL, m);
+ addOutputToLog(log_struct, ps->y_set, CONST_VAL, m);
+ addOutputToLog(log_struct, ps->alt_set, CONST_VAL, m);
+ addOutputToLog(log_struct, ps->yaw_set, CONST_VAL, rad);
+ addOutputToLog(log_struct, ps->mixer, MIXER_PWM0, pwm_val);
+ addOutputToLog(log_struct, ps->mixer, MIXER_PWM1, pwm_val);
+ addOutputToLog(log_struct, ps->mixer, MIXER_PWM2, pwm_val);
+ addOutputToLog(log_struct, ps->mixer, MIXER_PWM3, pwm_val);
// TODO: Make this not stupid. Adding 6 for IMU and 1 for timestamp
row_size = n_outputs + n_params + 6 + 1;
@@ -127,8 +157,10 @@ void printLogging(hardware_t *hardware_struct, log_t* log_struct, parameter_t* p
return;
}
int i;//, j;
- char buf[2048] = {};
+ // 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] = {};
@@ -139,27 +171,36 @@ void printLogging(hardware_t *hardware_struct, log_t* log_struct, parameter_t* p
return;
}
- sprintf(header1, "time,accel_x,accel_y,accel_z,gyro_x,gyro_y,gyro_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;
// 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], ",%s-%s", block_name, output_name);
+ end += sprintf(&header2[end], "\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], ",%s-%s", block_name, param_name);
+ end += sprintf(&header2[end], "\t%s_%s", block_name, param_name);
}
end += sprintf(&header2[end], "\n");
+ // Send header names
+ strcat(buf, comments);
+ strcat(buf, header1);
+ strcat(buf, header2);
+ send_data(&hardware_struct->uart, LOG_ID, 0, buf, strlen(buf));
- strcat(buf,header1);
- strcat(buf,header2);
-
+ // 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));
/*************************/
@@ -185,7 +226,7 @@ int format_log(int idx, log_t* log_struct, char* buf) {
end += sprintf(&buf[end], "%f", row[0]);
for (i = 1; i < row_size; i++) {
- end += sprintf(&buf[end], ",%f", row[i]);
+ end += sprintf(&buf[end], "\t%f", row[i]);
}
end += sprintf(&buf[end], "\n");
return end;
--
GitLab