Something went wrong on our end
log_data.c 8.74 KiB
/*
* log_data.c
*
* Created on: Feb 20, 2016
* Author: ucart
*/
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdarg.h>
#include "PID.h"
#include "type_def.h"
#include "log_data.h"
#include "communication.h"
#include "computation_graph.h"
#include "graph_blocks.h"
// Current index of the log array
static int arrayIndex = 0;
// Size of the array
static int arraySize = 0;
struct graph_tuple { // Tuple for
int block_id;
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];
static size_t n_outputs;
static size_t n_params;
static float* logArray = NULL;
static int row_size;
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)};
/*
* 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) {
if (n_outputs < MAX_LOG_NUM) {
log_outputs[n_outputs].block_id = controller_id;
log_outputs[n_outputs].sub_id = output_id;
n_outputs++;
safe_sprintf_cat(&units_output, "\t%s", units);
}
}
void addParamToLog(log_t* log_struct, int controller_id, int param_id, char* units) {
if (n_params < MAX_LOG_NUM) {
log_params[n_params].block_id = controller_id;
log_params[n_params].sub_id = param_id;
n_params++;
safe_sprintf_cat(&units_param, "\t%s", units);
}
}
void initialize_logging(log_t* log_struct, parameter_t* ps) {
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);
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->pitch_trim_add, 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->lidar, CONST_VAL, m);
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->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);
addOutputToLog(log_struct, ps->rc_throttle, PID_CORRECTION, pwm_val);
addOutputToLog(log_struct, ps->rc_pitch, PID_CORRECTION, pwm_val);
addOutputToLog(log_struct, ps->rc_roll, PID_CORRECTION, pwm_val);
addOutputToLog(log_struct, ps->x_vel_pid, PID_CORRECTION, rad);
addOutputToLog(log_struct, ps->y_vel_pid, PID_CORRECTION, rad);
addOutputToLog(log_struct, ps->x_vel, PID_CORRECTION, m_s);
addOutputToLog(log_struct, ps->y_vel, PID_CORRECTION, m_s);
// TODO: Make this not stupid. Adding 6 for IMU and 1 for timestamp
row_size = n_outputs + n_params + 6 + 1;
size_t needed_memory = sizeof(float) * row_size * LOG_STARTING_SIZE;
logArray = malloc(needed_memory);
if (!logArray) { // malloc failed
arraySize = 0;
} else {
arraySize = LOG_STARTING_SIZE;
}}
int log_data(log_t* log_struct, parameter_t* ps)
{
if(arrayIndex >= arraySize)
{
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 < n_params; i++) {
thisRow[offset++] = graph_get_param_val(ps->graph, log_params[i].block_id, log_params[i].sub_id);
}
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;
}
/**
* Prints all the log information.
*
* TODO: This should probably be transmitting in binary instead of ascii
*/
void printLogging(hardware_t *hardware_struct, log_t* log_struct, parameter_t* ps){
if (arrayIndex == 0) {
// Don't send any logs if nothing was logged
return;
}
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) {
safe_sprintf_cat(&buf, "Failed to allocate enough log memory\n");
send_data(&hardware_struct->uart, LOG_ID, 0, buf.str, buf.size);
return;
}
int i;
// Comment header
safe_sprintf_cat(&buf, "# MicroCART On-board Quad Log\n# Sample size: %d\n", arrayIndex);
// List Pid Constants
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;
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);
safe_sprintf_cat(&buf, "# %s :\tKp = %lf Ki = %lf Kd = %lf\n", node->name, kp, ki, kd);
}
}
// Header names for the pre-defined values
safe_sprintf_cat(&buf, "%%Time\taccel_x\taccel_y\taccel_z\tgyro_x\tgyro_y\tgyro_z");
// 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];
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];
safe_sprintf_cat(&buf, "\t%s_%s", block_name, param_name);
}
safe_sprintf_cat(&buf, "\n");
// Send header names
send_data(&hardware_struct->uart, LOG_ID, 0, buf.str, buf.size);
// Send units header
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++){
format_log(i, log_struct, &buf);
send_data(&hardware_struct->uart, LOG_ID, 0, buf.str, buf.size);
}
// 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;
}
/*
* 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;
buf->size = 0;
float* row = &logArray[idx * row_size];\
safe_sprintf_cat(buf, "%f", row[0]);
for (i = 1; i < row_size; i++) {
safe_sprintf_cat(buf, "\t%f", row[i]);
}
safe_sprintf_cat(buf, "\n");
}