1 files changed, 60 insertions, 0 deletions
diff --git a/apps/ardrone_control/ardrone_control_helper.c b/apps/ardrone_control/ardrone_control_helper.c
new file mode 100644
index 000000000..c073119e0
--- /dev/null
+++ b/apps/ardrone_control/ardrone_control_helper.c
@@ -0,0 +1,60 @@
+#include "ardrone_control_helper.h"
+#include <unistd.h>
+#include <nuttx/config.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+// int read_sensors_raw(sensors_raw_t *sensors_raw)
+// {
+// 	static int ret;
+// 	ret = global_data_wait(&global_data_sensors_raw->access_conf_rate_full);
+
+// 	if (ret == 0) {
+// 		memcpy(sensors_raw->gyro_raw, global_data_sensors_raw->gyro_raw, sizeof(sensors_raw->gyro_raw));
+// //		printf("Timestamp %d\n", &global_data_sensors_raw->timestamp);
+
+// 	} else {
+// 		printf("Controller timeout, no new sensor values available\n");
+// 	}
+
+// 	global_data_unlock(&global_data_sensors_raw->access_conf_rate_full);
+// 	return ret;
+// }
+
+// int read_attitude(global_data_attitude_t *attitude)
+// {
+
+// 	static int ret;
+// 	ret = global_data_wait(&global_data_attitude->access_conf);
+
+// 	if (ret == 0) {
+// 		memcpy(&attitude->roll, &global_data_attitude->roll, sizeof(global_data_attitude->roll));
+// 		memcpy(&attitude->pitch, &global_data_attitude->pitch, sizeof(global_data_attitude->pitch));
+// 		memcpy(&attitude->yaw, &global_data_attitude->yaw, sizeof(global_data_attitude->yaw));
+// 		memcpy(&attitude->rollspeed, &global_data_attitude->rollspeed, sizeof(global_data_attitude->rollspeed));
+// 		memcpy(&attitude->pitchspeed, &global_data_attitude->pitchspeed, sizeof(global_data_attitude->pitchspeed));
+// 		memcpy(&attitude->yawspeed, &global_data_attitude->yawspeed, sizeof(global_data_attitude->yawspeed));
+
+// 	} else {
+// 		printf("Controller timeout, no new attitude values available\n");
+// 	}
+
+// 	global_data_unlock(&global_data_attitude->access_conf);
+
+
+
+// 	return ret;
+// }
+
+// void read_quad_motors_setpoint(quad_motors_setpoint_t *rate_setpoint)
+// {
+
+// 	if (0 == global_data_trylock(&global_data_quad_motors_setpoint->access_conf)) { //TODO: check if trylock is the right choice, maybe only lock?
+// 		rate_setpoint->motor_front_nw = global_data_quad_motors_setpoint->motor_front_nw;
+// 		rate_setpoint->motor_right_ne = global_data_quad_motors_setpoint->motor_right_ne;
+// 		rate_setpoint->motor_back_se = global_data_quad_motors_setpoint->motor_back_se;
+// 		rate_setpoint->motor_left_sw = global_data_quad_motors_setpoint->motor_left_sw;
+
+// 		global_data_unlock(&global_data_quad_motors_setpoint->access_conf);
+// 	}
+// }