1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
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);
// }
// }
|
