Skip to content
Snippets Groups Projects
Commit 0008bdfc authored by dawehr's avatar dawehr
Browse files

Updated quad logging to use standard format.

parent 461199e9
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
quad/src/quad_app/log_data.c
+ 75
− 34
View file @ 0008bdfc
......@@ -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;
......
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