Merge branch 'master' into backend_fifos

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)

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));

quad/src/gen_diagram/Makefile

+# 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

quad/src/gen_diagram/create_png.sh

 #/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
 

quad/src/gen_diagram/generate.c

 #include <stdio.h>
+#include "computation_graph.h"
 #include "control_algorithm.h"
 
 int main() {

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

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

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) {

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);

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);

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
 

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));