Merge remote-tracking branch 'origin/quad-yaw-fix2'

quad/src/graph_blocks/node_pow.c

@@ -21,7 +21,3 @@ const struct graph_node_type node_pow_type = {
         .reset = reset,
         .state_size = 0,
 };
-
-int graph_add_node_pow(struct computation_graph *graph, const char* name) {
-    return graph_add_node(graph, name, &node_pow_type, NULL);
-}

quad/src/graph_blocks/node_pow.h

@@ -2,8 +2,6 @@
 #define __NODE_POW_H__
 #include "computation_graph.h"
 
-int graph_add_node_pow(struct computation_graph *graph, const char* name);
-
 extern const struct graph_node_type node_pow_type;
 
 enum graph_node_pow_inputs {

quad/src/graph_blocks/node_yaw_rot.c

+#include "node_yaw_rot.h"
+#include <stdlib.h>
+
+static void rotate_yaw(void *state, const double* params, const double *inputs, double *outputs) {
+	// Psuedo-Nonlinear Extension for determining local x/y position based on yaw angle
+	// provided by Matt Rich
+	//
+	// local x/y/z is the moving frame of reference on the quad that we are transforming so we can assume yaw angle is 0 (well enough)
+	// 		for the autonomous position controllers
+	//
+	// camera given x/y/z is the inertia frame of reference (the global coordinates)
+	//
+	// |local x|	|cos(yaw angle)  -sin(yaw angle)  0| |camera given x|
+	// |local y|  = |sin(yaw angle)   cos(yaw angle)  0| |camera given y|
+	// |local z|	|       0               0         1| |camera given z|
+
+    outputs[ROT_OUT_X] =
+        inputs[ROT_CUR_X] * cos(inputs[ROT_YAW]) + inputs[ROT_CUR_Y] * -sin(inputs[ROT_YAW]);
+
+	outputs[ROT_OUT_Y] =
+			inputs[ROT_CUR_X] * sin(inputs[ROT_YAW]) + inputs[ROT_CUR_Y] * cos(inputs[ROT_YAW]);
+
+}
+static void reset(void *state) {}
+
+static const char* const in_names[3] = {"Current Yaw", "X Position", "Y Position"};
+static const char* const out_names[2] = {"Rotated X", "Rotated Y"};
+static const char* const param_names[1] = {"Error if you see this"};
+const struct graph_node_type node_yaw_rot_type = {
+        .input_names = in_names,
+        .output_names = out_names,
+        .param_names = param_names,
+        .n_inputs = 3,
+        .n_outputs = 2,
+        .n_params = 0,
+        .execute = rotate_yaw,
+        .reset = reset,
+        .state_size = 0,
+        .type_id = BLOCK_YAW_ROT
+};

quad/src/graph_blocks/node_yaw_rot.h

+#ifndef __NODE_ROTATE_H__
+#define __NODE_ROTATE_H__
+#include "computation_graph.h"
+#include "graph_blocks.h"
+
+extern const struct graph_node_type node_yaw_rot_type;
+
+enum graph_node_yaw_rot_inputs {
+    ROT_YAW, // Amount of current yaw
+    ROT_CUR_X, // Input X position
+    ROT_CUR_Y // Input Y position
+};
+
+enum graph_node_yaw_rot_outputs {
+    ROT_OUT_X, // Rotated X position
+    ROT_OUT_Y // Rotated Y position
+};
+#endif // __NODE_ROTATE_H__

quad/src/quad_app/control_algorithm.c

@@ -23,8 +23,8 @@ void connect_autonomous(parameter_t* ps) {
 	struct computation_graph* graph = ps->graph;
 	//graph_set_source(graph, ps->pitch_pid, PID_SETPOINT, ps->x_pos_pid, PID_CORRECTION);
 	//graph_set_source(graph, ps->roll_pid, PID_SETPOINT, ps->y_pos_pid, PID_CORRECTION);
-	graph_set_source(graph, ps->pitch_pid, PID_SETPOINT, ps->x_vel_pid, PID_CORRECTION);
-	graph_set_source(graph, ps->roll_pid, PID_SETPOINT, ps->y_vel_pid, PID_CORRECTION);
+	graph_set_source(graph, ps->pitch_pid, PID_SETPOINT, ps->yaw_correction, ROT_OUT_X);
+	graph_set_source(graph, ps->roll_pid, PID_SETPOINT, ps->yaw_correction, ROT_OUT_Y);
 	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);
 }
@@ -96,6 +96,8 @@ int control_algorithm_init(parameter_t * ps)
     ps->y_vel = graph_add_defined_block(graph, BLOCK_PID, "Y Vel");
     ps->x_vel_clamp = graph_add_defined_block(graph, BLOCK_BOUNDS, "X Vel Clamp");
     ps->y_vel_clamp = graph_add_defined_block(graph, BLOCK_BOUNDS, "Y vel Clamp");
+        // Converts global X/Y to local X/Y
+    ps->yaw_correction = graph_add_defined_block(graph, BLOCK_YAW_ROT, "Yaw Correction");
 
     ps->mixer = graph_add_defined_block(graph, BLOCK_MIXER, "Signal Mixer");
 
@@ -143,6 +145,12 @@ int control_algorithm_init(parameter_t * ps)
     graph_set_source(graph, ps->x_vel_pid, PID_SETPOINT, ps->x_vel_clamp, BOUNDS_OUT);
     graph_set_source(graph, ps->x_vel_clamp, BOUNDS_IN, ps->x_pos_pid, PID_CORRECTION);
 
+    // X/Y global to local conversion
+    graph_set_source(graph, ps->yaw_correction, ROT_YAW, ps->cur_yaw, CONST_VAL);
+    graph_set_source(graph, ps->yaw_correction, ROT_CUR_X, ps->x_vel_pid, PID_CORRECTION);
+    graph_set_source(graph, ps->yaw_correction, ROT_CUR_Y, ps->y_vel_pid, PID_CORRECTION);
+    
+
     // Y velocity PID
         // Use a PID block to compute the derivative
     graph_set_param_val(graph, ps->y_vel, PID_KD, -1);

quad/src/quad_app/sensor.c

@@ -67,6 +67,7 @@ int get_sensors(log_t* log_struct, user_input_t* user_input_struct, raw_sensor_t
 //
 //	/////////// end testing
 
+	int status = 0;
 
 	// the the sensor board and fill in the readings into the GAM struct
 	iic0_mpu9150_read_gam(&(raw_sensor_struct->gam));

quad/src/quad_app/sensor_processing.c

@@ -44,28 +44,6 @@ int sensor_processing(log_t* log_struct, user_input_t *user_input_struct, raw_se
 	// copy currentQuadPosition and trimmedRCValues from raw_sensor_struct to sensor_struct
 	deep_copy_Qpos(&(sensor_struct->currentQuadPosition), &(raw_sensor_struct->currentQuadPosition));
 
-	// Psuedo-Nonlinear Extension for determining local x/y position based on yaw angle
-	// provided by Matt Rich
-	//
-	// local x/y/z is the moving frame of reference on the quad that we are transforming so we can assume yaw angle is 0 (well enough)
-	// 		for the autonomous position controllers
-	//
-	// camera given x/y/z is the inertia frame of reference (the global coordinates)
-	//
-	// |local x|	|cos(yaw angle)  -sin(yaw angle)  0| |camera given x|
-	// |local y|  = |sin(yaw angle)   cos(yaw angle)  0| |camera given y|
-	// |local z|	|       0               0         1| |camera given z|
-
-
-	sensor_struct->currentQuadPosition.x_pos =
-			raw_sensor_struct->currentQuadPosition.x_pos * cos(raw_sensor_struct->currentQuadPosition.yaw) +
-			raw_sensor_struct->currentQuadPosition.y_pos * -sin(raw_sensor_struct->currentQuadPosition.yaw);
-
-	sensor_struct->currentQuadPosition.y_pos =
-			raw_sensor_struct->currentQuadPosition.x_pos * sin(raw_sensor_struct->currentQuadPosition.yaw) +
-			raw_sensor_struct->currentQuadPosition.y_pos * cos(raw_sensor_struct->currentQuadPosition.yaw);
-
-
 	// Calculate Euler angles and velocities using Gimbal Equations below
 	/////////////////////////////////////////////////////////////////////////
 	// | Phi_d   |   |  1  sin(Phi)tan(theta)    cos(Phi)tan(theta) |  | p |

quad/src/quad_app/type_def.h

@@ -369,6 +369,8 @@ typedef struct parameter_t {
 	int y_vel_clamp;
 	int vel_x_gain;
 	int vel_y_gain;
+	// Sensor processing
+	int yaw_correction;
 } parameter_t;
 
 /**