diff --git a/quad/Makefile b/quad/Makefile
index 002f34a72fe3e8db7f63456bb0ba22bb9f9f931e..2cabb0468cc8658db03b3bdec963d1f9d0f47f61 100644
--- a/quad/Makefile
+++ b/quad/Makefile
@@ -18,15 +18,15 @@ libs:
$(MAKE) -C src/commands
$(MAKE) -C src/quad_app
-bins:
- $(MAKE) -C src/virt_quad
+bins: libs
+ $(MAKE) -C src/virt_quad clean default
+ $(MAKE) -C src/gen_diagram clean default
zybo:
bash scripts/build_zybo.sh
# For creating an image of the control network.
-gen_diagram:
- $(MAKE) -C src/gen_diagram
+diagram: bins
bash src/gen_diagram/create_png.sh
boot: $(BOOT)
diff --git a/quad/src/computation_graph/computation_graph.c b/quad/src/computation_graph/computation_graph.c
index f20a0d0f50f4824fd41baab7f2adb80a3a8d1960..81ebbf44e5dbdd5e07d039619d26093c4f53ce16 100644
--- a/quad/src/computation_graph/computation_graph.c
+++ b/quad/src/computation_graph/computation_graph.c
@@ -11,9 +11,9 @@ static double exec_input_vals[GRAPH_MAX_INPUTS];
// Macro functions for setting and clearing single bits in int array
// From http://www.mathcs.emory.edu/~cheung/Courses/255/Syllabus/1-C-intro/bit-array.html
-#define setBit(A,k) ( A[(k / (8*sizeof(&A)))] |= (1 << (k % (8*sizeof(&A)))) )
-#define clearBit(A,k) ( A[(k / (8*sizeof(&A)))] &= ~(1 << (k % (8*sizeof(&A)))) )
-#define testBit(A,k) ( A[(k / (8*sizeof(&A)))] & (1 << (k % (8*sizeof(&A)))) )
+#define setBit(A,k) ( A[(k / (8*sizeof(*A)))] |= (1 << (k % (8*sizeof(*A)))) )
+#define clearBit(A,k) ( A[(k / (8*sizeof(*A)))] &= ~(1 << (k % (8*sizeof(*A)))) )
+#define testBit(A,k) ( A[(k / (8*sizeof(*A)))] & (1 << (k % (8*sizeof(*A)))) )
struct computation_graph *create_graph() {
struct computation_graph *the_graph = malloc(sizeof(struct computation_graph));
diff --git a/quad/src/gen_diagram/Makefile b/quad/src/gen_diagram/Makefile
index 302077f0c7aa1fe819be10487b269eb3f478fd8f..c9b2f683d2857f7435fdd615f8ba05334d49c9af 100644
--- a/quad/src/gen_diagram/Makefile
+++ b/quad/src/gen_diagram/Makefile
@@ -1,6 +1,9 @@
+# Running this make file directly will not re-link with quad_app, even if it has changed
+# You should run `make diagram` from the top-level quad folder
TOP=../..
NAME = gen_diagram
-REQLIBS = -lquad_app, -lgraph_blocks, -lcomputation_graph
+REQLIBS = -lquad_app -lgraph_blocks -lcomputation_graph -lm
-include $(TOP)/library.mk
\ No newline at end of file
+
+include $(TOP)/executable.mk
\ No newline at end of file
diff --git a/quad/src/gen_diagram/create_png.sh b/quad/src/gen_diagram/create_png.sh
index dae08313279d6dcc2c9f10d544f2308ace38d931..5c4a2939cf9eda23eb267bc95ca3e6818ce4e846 100755
--- a/quad/src/gen_diagram/create_png.sh
+++ b/quad/src/gen_diagram/create_png.sh
@@ -1,4 +1,6 @@
#/bin/bash
-dot -Tpng "$(dirname $0)/network.dot" -o "$(dirname $0)/network.png"
+cd $(dirname $0)
+./../../bin/gen_diagram
+dot -Tpng network.dot -o network.png
diff --git a/quad/src/gen_diagram/generate.c b/quad/src/gen_diagram/generate.c
index 9b7b323729b2673330350513b2bec89a7142ac6e..3e712a2c04ded92ebc4e3465fcfb5404b466af19 100644
--- a/quad/src/gen_diagram/generate.c
+++ b/quad/src/gen_diagram/generate.c
@@ -1,4 +1,5 @@
#include <stdio.h>
+#include "computation_graph.h"
#include "control_algorithm.h"
int main() {
diff --git a/quad/src/gen_diagram/network.dot b/quad/src/gen_diagram/network.dot
index bd697cf68bc407933c1fb0899382e20387b768ed..862aace068984b2f3bddf264eb8873d092ef1a27 100644
--- a/quad/src/gen_diagram/network.dot
+++ b/quad/src/gen_diagram/network.dot
@@ -7,7 +7,7 @@ label="<f0>Roll PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [
"Ts_IMU" -> "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] |<f7> [alpha=0.000]"]
-"Pitch" -> "Pitch PID":f1 [label="Constant"]
+"Pitch trim add" -> "Pitch PID":f1 [label="Sum"]
"RC Pitch" -> "Pitch PID":f2 [label="Constant"]
"Ts_IMU" -> "Pitch PID":f3 [label="Constant"]
"Yaw PID"[shape=record
@@ -65,6 +65,12 @@ label="<f0>Pitch |<f1> [Constant=0.000]"]
label="<f0>Roll |<f1> [Constant=0.000]"]
"Yaw"[shape=record
label="<f0>Yaw |<f1> [Constant=0.000]"]
+"Pitch trim"[shape=record
+label="<f0>Pitch trim |<f1> [Constant=0.000]"]
+"Pitch trim add"[shape=record
+label="<f0>Pitch trim add |<f1> --\>Summand 1 |<f2> --\>Summand 2"]
+"Pitch trim" -> "Pitch trim add":f1 [label="Constant"]
+"Pitch" -> "Pitch trim add":f2 [label="Constant"]
"dTheta"[shape=record
label="<f0>dTheta |<f1> [Constant=0.000]"]
"dPhi"[shape=record
diff --git a/quad/src/gen_diagram/network.png b/quad/src/gen_diagram/network.png
index 3152aa2cf13de32d5cab578f0a029869757c325c..65999177c759abd9b73da5ee19f7b4fea3b766e4 100644
Binary files a/quad/src/gen_diagram/network.png and b/quad/src/gen_diagram/network.png differ
diff --git a/quad/src/quad_app/PID.h b/quad/src/quad_app/PID.h
index 4c5e4b81f0273a9a62aa175b6c0f350914fb67a2..5a83225b0b9813e4d311bf61c8f1eae89c7f24a0 100644
--- a/quad/src/quad_app/PID.h
+++ b/quad/src/quad_app/PID.h
@@ -21,7 +21,7 @@
//#define YAW_ANGLE_KD 0.0f
// when using units of radians
-#define YAW_ANGULAR_VELOCITY_KP 190.0f * 2292.0f//200.0f * 2292.0f
+#define YAW_ANGULAR_VELOCITY_KP 29700//200.0f * 2292.0f
#define YAW_ANGULAR_VELOCITY_KI 0.0f
#define YAW_ANGULAR_VELOCITY_KD 0.0f
#define YAW_ANGLE_KP 2.6f
diff --git a/quad/src/quad_app/control_algorithm.c b/quad/src/quad_app/control_algorithm.c
index 424f098cbe02be1fb3e7707b6383fa0814c99957..3002cfc2697bc760dd657e4f7d6fa11df0264577 100644
--- a/quad/src/quad_app/control_algorithm.c
+++ b/quad/src/quad_app/control_algorithm.c
@@ -13,8 +13,8 @@
#include "util.h"
#include "timer.h"
-#define ROLL_PITCH_MAX_ANGLE 1.5708 // 90 degrees
-#define PWM_DIFF_BOUNDS 30000
+#define ROLL_PITCH_MAX_ANGLE 0.35 // 20 degrees
+#define PWM_DIFF_BOUNDS 20000
#define VRPN_REFRESH_TIME 0.01f // 10ms
void connect_autonomous(parameter_t* ps) {
diff --git a/quad/src/quad_app/iic_utils.c b/quad/src/quad_app/iic_utils.c
index 90cdfb6872963b2c3ef5c7437a8abb9435e0c581..a0061e6b00432768feded1b15495c850fad5a914 100644
--- a/quad/src/quad_app/iic_utils.c
+++ b/quad/src/quad_app/iic_utils.c
@@ -30,7 +30,7 @@ int iic0_mpu9150_start(){
// Set clock reference to Z Gyro
iic0_mpu9150_write(0x6B, 0x03);
// Configure Digital Low/High Pass filter
- iic0_mpu9150_write(0x1A,0x06); // Level 6 low pass on gyroscope
+ iic0_mpu9150_write(0x1A,0x05); // Level 5 low pass on gyroscope
// Configure Gyro to 2000dps, Accel. to +/-8G
iic0_mpu9150_write(0x1B, 0x18);
diff --git a/quad/src/quad_app/log_data.c b/quad/src/quad_app/log_data.c
index 9448fc10d5d47ec8e32c31e364983f5a94a8c873..3e67c14aa36862cae1ad885955e944a86056007b 100644
--- a/quad/src/quad_app/log_data.c
+++ b/quad/src/quad_app/log_data.c
@@ -112,6 +112,9 @@ void initialize_logging(log_t* log_struct, parameter_t* ps) {
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->mixer, MIXER_PWM0, pwm_val);
addOutputToLog(log_struct, ps->mixer, MIXER_PWM1, pwm_val);
addOutputToLog(log_struct, ps->mixer, MIXER_PWM2, pwm_val);
diff --git a/quad/src/quad_app/log_data.h b/quad/src/quad_app/log_data.h
index 34f1658d02f316918547bc0eedd760bcbf121264..becc2c2e188a819e1e9735cc625e93dd514f52c7 100644
--- a/quad/src/quad_app/log_data.h
+++ b/quad/src/quad_app/log_data.h
@@ -9,7 +9,7 @@
#define LOG_DATA_H_
#include "type_def.h"
-#define LOG_STARTING_SIZE 8192 //262144 // 2^18 32768 2^15
+#define LOG_STARTING_SIZE 60000 //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
diff --git a/quad/src/quad_app/quad_app.c b/quad/src/quad_app/quad_app.c
index 1e301732814d8cf7faba2984ca192c99387afb00..01eabe9017f733685ae8f9737398099e657a0bf0 100644
--- a/quad/src/quad_app/quad_app.c
+++ b/quad/src/quad_app/quad_app.c
@@ -75,15 +75,12 @@ int quad_main(int (*setup_hardware)(hardware_t *hardware_struct))
// update the GUI
update_GUI(&(structs.log_struct));
- // Processing of loop timer at the end of the control loop
- timer_end_loop(&(structs.log_struct));
-
if (!this_kill_condition) {
+ // Log the data collected in this loop
+ log_data(&(structs.log_struct), &(structs.parameter_struct));
+
if(structs.user_defined_struct.flight_mode == AUTO_FLIGHT_MODE)
{
- // Log the data collected in this loop
- log_data(&(structs.log_struct), &(structs.parameter_struct));
-
static int loop_counter = 0;
loop_counter++;
@@ -113,6 +110,9 @@ int quad_main(int (*setup_hardware)(hardware_t *hardware_struct))
}
last_kill_condition = this_kill_condition;
+
+ // Processing of loop timer at the end of the control loop
+ timer_end_loop(&(structs.log_struct));
}
kill_motors(&(structs.hardware_struct.pwm_outputs));