diff --git a/quad/computation_graph/src/computation_graph.c b/quad/computation_graph/src/computation_graph.c
index 28b10fbb45faac72293d7f50407c9f795d13bbca..dbc07a162214cb4e6282b330322ff19cb0f52d4e 100644
--- a/quad/computation_graph/src/computation_graph.c
+++ b/quad/computation_graph/src/computation_graph.c
@@ -141,7 +141,7 @@ double graph_get_param_val(const struct computation_graph *graph, int node_id, i
double graph_get_output(const struct computation_graph *graph, int node_id, int output_id) {
if (node_id >= graph->n_nodes || output_id >= graph->nodes[node_id].type->n_outputs) {
- return 0;
+ return NAN;
}
return graph->nodes[node_id].output_values[output_id];
}
diff --git a/quad/sw/modular_quad_pid/gen_diagram/gen_diagram b/quad/sw/modular_quad_pid/gen_diagram/gen_diagram
index 2984a72b205bab88196b1dd83121f6f73f25be37..3babc8f1ab7b00d2bdd2ed28c38a08c8976f1fca 100755
Binary files a/quad/sw/modular_quad_pid/gen_diagram/gen_diagram and b/quad/sw/modular_quad_pid/gen_diagram/gen_diagram differ
diff --git a/quad/sw/modular_quad_pid/gen_diagram/network.dot b/quad/sw/modular_quad_pid/gen_diagram/network.dot
index d92f0bc8bd3aa7e9b26e81e90ff27032b5416ae7..516c4a31b125323ad1be1cd32b7e128459789e3f 100644
--- a/quad/sw/modular_quad_pid/gen_diagram/network.dot
+++ b/quad/sw/modular_quad_pid/gen_diagram/network.dot
@@ -2,12 +2,12 @@ digraph G {
rankdir="LR"
"Roll PID"[shape=record
label="<f0>Roll PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=15.000] |<f5> [Ki=0.000] |<f6> [Kd=0.200]"]
-"VRPN Roll" -> "Roll PID":f1 [label="Constant"]
+"Roll" -> "Roll PID":f1 [label="Constant"]
"RC Roll" -> "Roll PID":f2 [label="Constant"]
"Ts_angle" -> "Roll PID":f3 [label="Constant"]
"Pitch PID"[shape=record
label="<f0>Pitch PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=15.000] |<f5> [Ki=0.000] |<f6> [Kd=0.200]"]
-"VRPN Pitch" -> "Pitch PID":f1 [label="Constant"]
+"Pitch" -> "Pitch PID":f1 [label="Constant"]
"RC Pitch" -> "Pitch PID":f2 [label="Constant"]
"Ts_angle" -> "Pitch PID":f3 [label="Constant"]
"Yaw PID"[shape=record
diff --git a/quad/sw/modular_quad_pid/gen_diagram/network.png b/quad/sw/modular_quad_pid/gen_diagram/network.png
index 7e35c5402e1e36381e4cbd07418c1f6567784291..47437dd878b0765f9547545d6b4ba736cad20810 100644
Binary files a/quad/sw/modular_quad_pid/gen_diagram/network.png and b/quad/sw/modular_quad_pid/gen_diagram/network.png differ
diff --git a/quad/sw/modular_quad_pid/src/control_algorithm.c b/quad/sw/modular_quad_pid/src/control_algorithm.c
index b2498e6c29030f62db112dd6c815a508aac3b463..24b098734a9f98d82ca786707827fafda266cc1e 100644
--- a/quad/sw/modular_quad_pid/src/control_algorithm.c
+++ b/quad/sw/modular_quad_pid/src/control_algorithm.c
@@ -24,6 +24,10 @@ void connect_autonomous(parameter_t* ps) {
graph_set_source(graph, ps->roll_pid, PID_SETPOINT, ps->y_pos_pid, PID_CORRECTION);
//graph_set_source(graph, ps->mixer, MIXER_THROTTLE, ps->throttle_trim_add, ADD_SUM);
//graph_set_source(graph, ps->yaw_r_pid, PID_SETPOINT, ps->yaw_pid, PID_CORRECTION);
+ // TODO: Change this to use VRPN time differences
+ // NOTE: This is being set here because if we set it in the initialization, it is
+ // never marked as "updated", and therefore, doesn't get computed. Yeah, I know, that's bad...
+ graph_set_param_val(graph, ps->pos_time, CONST_SET, 0.1);
}
void connect_manual(parameter_t* ps) {
@@ -32,6 +36,8 @@ void connect_manual(parameter_t* ps) {
graph_set_source(graph, ps->roll_pid, PID_SETPOINT, ps->rc_roll, CONST_VAL);
graph_set_source(graph, ps->mixer, MIXER_THROTTLE, ps->rc_throttle, CONST_VAL);
graph_set_source(graph, ps->yaw_r_pid, PID_SETPOINT, ps->rc_yaw, CONST_VAL);
+ // TODO: Change this to use LOOP_TIME
+ graph_set_param_val(graph, ps->angle_time, CONST_SET, 0.005);
}
int control_algorithm_init(parameter_t * ps)
@@ -95,7 +101,7 @@ int control_algorithm_init(parameter_t * ps)
graph_set_source(graph, ps->pitch_r_pid, PID_CUR_POINT, ps->theta_dot, CONST_VAL);
graph_set_source(graph, ps->pitch_r_pid, PID_DT, ps->angle_time, CONST_VAL);
//graph_set_source(graph, ps->pitch_pid, PID_SETPOINT, ps->rc_pitch, CONST_VAL);
- graph_set_source(graph, ps->pitch_pid, PID_CUR_POINT, ps->vrpn_pitch, CONST_VAL);
+ graph_set_source(graph, ps->pitch_pid, PID_CUR_POINT, ps->cur_pitch, CONST_VAL);
graph_set_source(graph, ps->pitch_pid, PID_DT, ps->angle_time, CONST_VAL);
// Connect roll PID chain
@@ -103,7 +109,7 @@ int control_algorithm_init(parameter_t * ps)
graph_set_source(graph, ps->roll_r_pid, PID_CUR_POINT, ps->phi_dot, CONST_VAL);
graph_set_source(graph, ps->roll_r_pid, PID_DT, ps->angle_time, CONST_VAL);
//graph_set_source(graph, ps->roll_pid, PID_SETPOINT, ps->rc_roll, CONST_VAL);
- graph_set_source(graph, ps->roll_pid, PID_CUR_POINT, ps->vrpn_roll, CONST_VAL);
+ graph_set_source(graph, ps->roll_pid, PID_CUR_POINT, ps->cur_roll, CONST_VAL);
graph_set_source(graph, ps->roll_pid, PID_DT, ps->angle_time, CONST_VAL);
// Connect yaw PID chain
@@ -186,10 +192,6 @@ int control_algorithm_init(parameter_t * ps)
graph_set_param_val(graph, ps->clamp_d_pwmY, BOUNDS_MIN, -20000);
graph_set_param_val(graph, ps->clamp_d_pwmY, BOUNDS_MAX, 20000);
- // TODO: Change this to use LOOP_TIME
- graph_set_param_val(graph, ps->angle_time, CONST_SET, 0.005);
- graph_set_param_val(graph, ps->pos_time, CONST_SET, 0.1);
-
// Set initial mode
connect_manual(ps);
@@ -219,11 +221,6 @@ int control_algorithm_init(parameter_t * ps)
user_defined_struct->engaging_auto = 1;
graph_set_param_val(graph, ps->throttle_trim, CONST_SET, user_input_struct->rc_commands[THROTTLE]);
-
- log_struct->trims.roll = sensor_struct->trims.roll;
- log_struct->trims.pitch = sensor_struct->trims.pitch;
- log_struct->trims.yaw = sensor_struct->trims.yaw;
- log_struct->trims.throttle = sensor_struct->trims.throttle;
}
if(user_input_struct->locationFresh && user_defined_struct->engaging_auto == 1)
@@ -277,8 +274,9 @@ int control_algorithm_init(parameter_t * ps)
graph_set_param_val(graph, ps->rc_yaw, CONST_SET, user_input_struct->yaw_manual_setpoint);
graph_set_param_val(graph, ps->rc_throttle, CONST_SET, user_input_struct->rc_commands[THROTTLE]);
- int outputs[1] = {ps->mixer};
- graph_compute_nodes(graph, outputs, 1);
+ // Compute VRPN blocks so they can be logged
+ int outputs[4] = {ps->mixer, ps->vrpn_alt, ps->vrpn_pitch, ps->vrpn_roll};
+ graph_compute_nodes(graph, outputs, 4);
// here for now so in case any flight command is not PID controlled, it will default to rc_command value:
//memcpy(raw_actuator_struct->controller_corrected_motor_commands, user_input_struct->rc_commands, sizeof(int) * 6);
diff --git a/quad/sw/modular_quad_pid/src/initialize_components.c b/quad/sw/modular_quad_pid/src/initialize_components.c
index 84fc120d328e233eedcc2c80a6f92b8a244d6ef0..9c79db823252d961b57acd621fb7a09d662f1d2d 100644
--- a/quad/sw/modular_quad_pid/src/initialize_components.c
+++ b/quad/sw/modular_quad_pid/src/initialize_components.c
@@ -25,13 +25,9 @@ int protection_loops(modular_structs_t *structs)
// wait until throttle is low and the gear switch is engaged (so you don't immediately break out of the main loop below)
// also wait for the flight mode to be set to manual
- while(rc_commands[THROTTLE] > 125000 || read_kill(rc_commands[GEAR]) || !read_flap(rc_commands[FLAP]))
- read_rec_all(rc_commands);
-
- // wait until the ground station has connected to the quad and acknowledged that its ready to start
- while (!structs->user_input_struct.receivedBeginUpdate) {
+ while(rc_commands[THROTTLE] > 125000 || read_kill(rc_commands[GEAR]) || !read_flap(rc_commands[FLAP])) {
process_received(structs);
- usleep(10000);
+ read_rec_all(rc_commands);
}
// let the pilot/observers know that the quad is now active
diff --git a/quad/sw/modular_quad_pid/src/log_data.c b/quad/sw/modular_quad_pid/src/log_data.c
index a1c2db2374006b85bd5fc3b6be4b337409add227..b01698b3c3635d1abf9bee7ef4af4c426de7b6ee 100644
--- a/quad/sw/modular_quad_pid/src/log_data.c
+++ b/quad/sw/modular_quad_pid/src/log_data.c
@@ -17,35 +17,38 @@
#include "computation_graph.h"
#include "graph_blocks/node_pid.h"
#include "graph_blocks/node_constant.h"
+#include "graph_blocks/node_mixer.h"
// Current index of the log array
int arrayIndex = 0;
// Size of the array
int arraySize = LOG_STARTING_SIZE;
-int resized = 0;
-// The number of times we resized the array
-int resizeCount = 0;
+struct graph_tuple { // Tuple for
+ int block_id;
+ int sub_id;
+};
-// Pointer to point to the array with all the logging information
-// for now its not dynamic
-//log_t logArray[LOG_STARTING_SIZE * 3];// up to 60 seconds of log
+struct graph_tuple log_outputs[MAX_LOG_NUM];
+struct graph_tuple log_params[MAX_LOG_NUM];
+size_t n_outputs;
+size_t n_params;
float* logArray;
int row_size;
void addOutputToLog(log_t* log_struct, int controller_id, int output_id) {
- if (log_struct->n_outputs < MAX_LOG_SIZE) {
- log_struct->outputs[log_struct->n_outputs].block_id = controller_id;
- log_struct->outputs[log_struct->n_outputs].sub_id = output_id;
- log_struct->n_outputs++;
+ if (n_outputs < MAX_LOG_NUM) {
+ log_outputs[n_outputs].block_id = controller_id;
+ log_outputs[n_outputs].sub_id = output_id;
+ n_outputs++;
}
}
-void addParamToLog(log_t* log_struct, int controller_id, int output_id) {
- if (log_struct->n_params < MAX_LOG_SIZE) {
- log_struct->params[log_struct->n_params].block_id = controller_id;
- log_struct->params[log_struct->n_params].sub_id = output_id;
- log_struct->n_params++;
+void addParamToLog(log_t* log_struct, int controller_id, int param_id) {
+ if (n_params < MAX_LOG_NUM) {
+ log_params[n_params].block_id = controller_id;
+ log_params[n_params].sub_id = param_id;
+ n_params++;
}
}
@@ -67,37 +70,24 @@ void initialize_logging(log_t* log_struct, parameter_t* ps) {
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);
// TODO: Make this not stupid. Adding 6 for IMU and 1 for timestamp
- row_size = log_struct->n_outputs + log_struct->n_params + 6 + 1;
+ row_size = n_outputs + n_params + 6 + 1;
logArray = malloc(sizeof(float) * row_size * LOG_STARTING_SIZE);
}
int log_data(log_t* log_struct, parameter_t* ps)
{
- // If the first iteration, allocate enough memory for "arraySize" elements of logging
-// if(logArray == NULL){
-// // size in memory is 1,720,320 bytes (1.64 megabytes) because logging struct is 420 bytes each
-// // up to 20 seconds of log before resizing
-// logArray = malloc(LOG_STARTING_SIZE * sizeof(log_t));
-// uart0_sendStr("initialized log array.\n");
-// sleep(1);
-// }
-
- // semi dynamic log
-// if((arrayIndex >= arraySize - 1) && (!resized)){
-// realloc(logArray, LOG_STARTING_SIZE * 3 * sizeof(log_t)); // up to 60 seconds of log
-// resized = 1;
-// arraySize = LOG_STARTING_SIZE * 3;
-// uart0_sendStr("resized log array.\n");
-// sleep(1);
-// }
if(arrayIndex >= arraySize - 1)
{
return 1;
}
- float* thisRow = &logArray[arrayIndex * row_size];
+ float* thisRow = &logArray[arrayIndex * row_size * sizeof(float)];
int offset = 0;
thisRow[offset++] = log_struct->time_stamp;
thisRow[offset++] = log_struct->gam.accel_x;
@@ -108,25 +98,15 @@ int log_data(log_t* log_struct, parameter_t* ps)
thisRow[offset++] = log_struct->gam.gyro_zVel_r;
int i;
- for (i = 0; i < log_struct->n_params; i++) {
- thisRow[offset++] = graph_get_param_val(ps->graph, log_struct->params[i].block_id, log_struct->params[i].sub_id);
+ 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 < log_struct->n_outputs; i++) {
- thisRow[offset++] = graph_get_output(ps->graph, log_struct->outputs[i].block_id, log_struct->outputs[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;
- // If the index is too big, reallocate memory to double the size as before
-// if(arrayIndex == arraySize){
-// arraySize *= 2;
-// logArray = (log_t *) realloc(logArray, arraySize * sizeof(log_t));
-// ++resizeCount;
-// }
-// else if(arrayIndex > arraySize){
-// // Something fishy has occured
-// xil_printf("Array index is out of bounds. This shouldn't happen but somehow you did the impossible\n\r");
-// }
}
@@ -144,87 +124,27 @@ void printLogging(log_t* log_struct, parameter_t* ps){
char header2[1024] = {};
sprintf(header1, "time,accel_x,accel_y,accel_z,gyro_x,gyro_y,gyro_z");
- /*
- sprintf(comments, "# MicroCART On-board Quad Log\r\n# Sample size: %d\r\n", arrayIndex);
- sprintf(header, "%%Time\t" "LoopPeriod\t"
-
- //current points (measurements)
- "X_Current_Position\t" "Y_Current_Position\t" "Z_Current_Position\t"
- "Cam_Meas_Roll\tCam_Meas_Pitch\tCam_Meas_Yaw\t"
- "Quad_Meas_Roll\tQuad_Meas_Pitch\t"
- "roll_velocity\tpitch_velocity\tyaw_velocity\t"
-
- //setpoints
- "X_setpoint\t" "Y_setpoint\t" "Z_setpoint\t"
- "Roll_setpoint\tPitch_setpoint\tYaw_setpoint\t"
- "Roll_vel_setpoint\tPitch_vel_setpoint\tYaw_vel_setpoint\t"
-
- //corrections
- "PID_x\t"
- "PID_y\t"
- "PID_z\t"
- "PID_roll\t"
- "PID_pitch\t"
- "PID_yaw\t"
- "PID_roll_vel\t"
- "PID_pitch_vel\t"
- "PID_yaw_vel\t"
-
- //trims
- "Roll_trim\tPitch_trim\tYaw_trim\tThrottle_trim\t"
-
- //motor commands
- "Motor_0\tMotor_1\tMotor_2\tMotor_3\n"
-
- );
-
-
- sprintf(units, "&sec\tsec\t"
-
- //current points
- "meters\tmeters\tmeters\t"
- "radians\tradians\tradians\t"
- "radians\tradians\t"
- "radians//sec\tradians//sec\tradians//sec\t"
-
- //setpoints
- "meters\tmeters\tmeters\t"
- "radians\tradians\tradians\t"
- "radians//sec\tradians//sec\tradians//sec\t"
-
- //corrections
- "radians\tradians\tradians\t"
- "radians//sec\tradians//sec\tradians//sec\t"
- "none\tnone\tnone\t"
-
- //trims
- "none\tnone\tnone\tnone\t"
-
- //motors
- "none\tnone\tnone\tnone\n"
-
- );
-*/
int end = 0;
- for (i = 0; i < log_struct->n_params; i++) {
- const char* block_name = ps->graph->nodes[log_struct->params[i].block_id].name;
- const char* output_name = ps->graph->nodes[log_struct->params[i].block_id].type->param_names[log_struct->params[i].sub_id];
+ // 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);
}
- for (i = 0; i < log_struct->n_outputs; i++) {
- const char* block_name = ps->graph->nodes[log_struct->outputs[i].block_id].name;
- const char* param_name = ps->graph->nodes[log_struct->outputs[i].block_id].type->output_names[log_struct->outputs[i].sub_id];
+ // 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], "\n");
strcat(buf,header1);
strcat(buf,header2);
send_data(LOG_ID, 0, buf, strlen(buf));
- //uart0_sendBytes(buf, strlen(buf));
- //usleep(100000);
/*************************/
/* print & send log data */
@@ -238,69 +158,12 @@ int format_log(int idx, log_t* log_struct, char* buf) {
int i;
int end = 0;
- float* row = &logArray[idx * row_size];\
+ float* row = &logArray[idx * row_size * sizeof(float)];\
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], "\n");
return end;
}
-
-/*
-char* format(log_t log){
- char *retString = malloc(4096*2);
-
- sprintf(retString, "%.3f\t%.4f\t" //Time and TimeSlice
-
- // current points
- "%.3f\t%.3f\t%.3f\t"
- "%.3f\t%.3f\t%.3f\t"
- "%.3f\t%.3f\t"
- "%.3f\t%.3f\t%.3f\t"
-
- //setpoints
- "%.3f\t%.3f\t%.3f\t"
- "%.3f\t%.3f\t%.3f\t"
- "%.3f\t%.3f\t%.3f\t"
-
- //corrections
- "%.3f\t%.3f\t%.3f\t"
- "%.3f\t%.3f\t%.3f\t"
- "%.3f\t%.3f\t%.3f\t"
-
- //trims
- "%.2f\t%.2f\t%.2f\t%.2f\t"
-
- //motors
- "%d\t%d\t%d\t%d\n"
-
- ,log.time_stamp, log.time_slice,
-
- // current points
- log.local_x_PID.current_point,log.local_y_PID.current_point, log.altitude_PID.current_point,
- log.currentQuadPosition.roll, log.currentQuadPosition.pitch, log.currentQuadPosition.yaw,
- log.roll_angle_filtered, log.pitch_angle_filtered,
- log.phi_dot, log.theta_dot, log.psi_dot,
-
- //setpoints
- log.local_x_PID.setpoint, log.local_y_PID.setpoint, log.altitude_PID.setpoint,
- log.angle_roll_PID.setpoint, log.angle_pitch_PID.setpoint, log.angle_yaw_PID.setpoint,
- log.ang_vel_roll_PID.setpoint, log.ang_vel_pitch_PID.setpoint, log.ang_vel_pitch_PID.setpoint,
-
- //corrections
- log.local_x_PID_values.pid_correction, log.local_y_PID_values.pid_correction, log.altitude_PID_values.pid_correction,
- log.angle_roll_PID_values.pid_correction, log.angle_pitch_PID_values.pid_correction, log.angle_yaw_PID_values.pid_correction,
- log.ang_vel_roll_PID_values.pid_correction, log.ang_vel_pitch_PID_values.pid_correction, log.ang_vel_yaw_PID_values.pid_correction,
-
- //trims
- log.trims.roll, log.trims.pitch, log.trims.yaw, log.trims.throttle,
-
- //motors
- log.motors[0], log.motors[1], log.motors[2], log.motors[3]
- );
-
-
- return retString;
-}
-*/
diff --git a/quad/sw/modular_quad_pid/src/log_data.h b/quad/sw/modular_quad_pid/src/log_data.h
index ecfc50d1783ab27cb8c83e7fb0f8dcc2af4f846a..bb3622c931f7b56420175ea872420a39c7e904d7 100644
--- a/quad/sw/modular_quad_pid/src/log_data.h
+++ b/quad/sw/modular_quad_pid/src/log_data.h
@@ -9,10 +9,23 @@
#define LOG_DATA_H_
#include "type_def.h"
-#define LOG_STARTING_SIZE 4096 //262144 // 2^18 32768 2^15
+#define LOG_STARTING_SIZE 8192 //262144 // 2^18 32768 2^15
+// Maximum number of block outputs you can record, and maximum number of block parameters to record
+#define MAX_LOG_NUM 50
void initialize_logging(log_t* log_struct, parameter_t* ps);
+/**
+ * Adds the given block output to the data to be logged
+ */
+void addOutputToLog(log_t* log_struct, int controller_id, int output_id);
+
+/**
+ * Adds the given block parameter to the data to be logged
+ */
+void addParamToLog(log_t* log_struct, int controller_id, int param_id);
+
+
/**
* @brief
* Logs the data obtained throughout the controller loop.
@@ -36,6 +49,9 @@ void initialize_logging(log_t* log_struct, parameter_t* ps);
*/
void printLogging(log_t* log_struct, parameter_t* ps);
+ /**
+ * 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_ */
diff --git a/quad/sw/modular_quad_pid/src/main.c b/quad/sw/modular_quad_pid/src/main.c
index cfc3076a86995621278deffdc99b40060c92e447..64d82b3a997b1619226249b20c626453398659dd 100644
--- a/quad/sw/modular_quad_pid/src/main.c
+++ b/quad/sw/modular_quad_pid/src/main.c
@@ -95,7 +95,7 @@ int main()
if(structs.user_defined_struct.flight_mode == AUTO_FLIGHT_MODE)
{
// Log the data collected in this loop
- log_data(&(structs.log_struct));
+ log_data(&(structs.log_struct), &(structs.parameter_struct));
static int loop_counter = 0;
loop_counter++;
@@ -128,7 +128,7 @@ int main()
MIO7_led_off();
- printLogging();
+ printLogging(&(structs.log_struct), &(structs.parameter_struct));
flash_MIO_7_led(10, 100);
diff --git a/quad/sw/modular_quad_pid/src/send_actuator_commands.c b/quad/sw/modular_quad_pid/src/send_actuator_commands.c
index 85b1ca76b6df58a472f68cdd5df65a1937932674..3ef6f1a87140fa572d0118c3fc73b3bdf4fb667b 100644
--- a/quad/sw/modular_quad_pid/src/send_actuator_commands.c
+++ b/quad/sw/modular_quad_pid/src/send_actuator_commands.c
@@ -15,11 +15,6 @@ int send_actuator_commands(log_t* log_struct, actuator_command_t* actuator_comma
for (i = 0; i < 4; i++)
pwm_write_channel(actuator_command_struct->pwms[i], i);
- log_struct->motors[0] = actuator_command_struct->pwms[0];
- log_struct->motors[1] = actuator_command_struct->pwms[1];
- log_struct->motors[2] = actuator_command_struct->pwms[2];
- log_struct->motors[3] = actuator_command_struct->pwms[3];
-
return 0;
}
diff --git a/quad/sw/modular_quad_pid/src/sensor_processing.c b/quad/sw/modular_quad_pid/src/sensor_processing.c
index 9465b0228a0f5e36be563d4fcf95ce2d6670701e..410b9219f8c905e16ab07fbfa0e41958bdd2ba53 100644
--- a/quad/sw/modular_quad_pid/src/sensor_processing.c
+++ b/quad/sw/modular_quad_pid/src/sensor_processing.c
@@ -96,14 +96,6 @@ int sensor_processing(log_t* log_struct, user_input_t *user_input_struct, raw_se
// uart0_sendStr(dMsg);
// loop_counter = 0;
// }
-
- //logging
- log_struct->currentQuadPosition = sensor_struct->currentQuadPosition;
- log_struct->roll_angle_filtered = sensor_struct->roll_angle_filtered;
- log_struct->pitch_angle_filtered = sensor_struct->pitch_angle_filtered;
- log_struct->phi_dot = sensor_struct->phi_dot;
- log_struct->theta_dot = sensor_struct->theta_dot;
- log_struct->psi_dot = sensor_struct->psi_dot;
return 0;
}
diff --git a/quad/sw/modular_quad_pid/src/type_def.h b/quad/sw/modular_quad_pid/src/type_def.h
index 82a0cc6c7a4bcc525ee0b927fa4b874829c56833..9850b778b200c1b2e61192af64aadceed9a114ed 100644
--- a/quad/sw/modular_quad_pid/src/type_def.h
+++ b/quad/sw/modular_quad_pid/src/type_def.h
@@ -22,8 +22,6 @@ enum flight_mode{
MANUAL_FLIGHT_MODE
};
-#define MAX_LOG_SIZE 50
-
//----------------------------------------------------------------------------------------------
// index|| 0 | 1 | 2 | 3 & 4 | 5 & 6 | 7+ | end |
//---------------------------------------------------------------------------------------------|
@@ -172,15 +170,6 @@ typedef struct log_t {
gam_t gam; // Raw and calculated gyro, accel, and mag values are all in gam_t
- struct graph_tuple { // Tuple for
- int block_id;
- int sub_id;
- };
-
- struct graph_tuple outputs[MAX_LOG_SIZE];
- struct graph_tuple params[MAX_LOG_SIZE];
- size_t n_outputs;
- size_t n_params;
/*
float phi_dot, theta_dot, psi_dot; // gimbal equation values
diff --git a/quad/sw/modular_quad_pid/src/user_input.c b/quad/sw/modular_quad_pid/src/user_input.c
index 91fb830acf07a8b1d1eaaf44175554625d0cd233..b3b4331e200aad0dfc88c87684e7d28b06def909 100644
--- a/quad/sw/modular_quad_pid/src/user_input.c
+++ b/quad/sw/modular_quad_pid/src/user_input.c
@@ -14,11 +14,6 @@ int get_user_input(log_t* log_struct, user_input_t* user_input_struct)
// Read in values from RC Receiver
read_rec_all(user_input_struct->rc_commands);
- log_struct->commands.pitch = user_input_struct->rc_commands[PITCH];
- log_struct->commands.roll = user_input_struct->rc_commands[ROLL];
- log_struct->commands.throttle = user_input_struct->rc_commands[THROTTLE];
- log_struct->commands.yaw = user_input_struct->rc_commands[YAW];
-
//create setpoints for manual flight
// currently in units of radians
user_input_struct->yaw_manual_setpoint = convert_from_receiver_cmd(user_input_struct->rc_commands[YAW], YAW_MAX, YAW_CENTER, YAW_MIN, YAW_RAD_TARGET, -(YAW_RAD_TARGET));