Newer
Older
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
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
#include <stdio.h>
#include <unistd.h>
#include <getopt.h>
#include "cli_getpid.h"
int cli_getpid(struct backend_conn * conn, int argc, char **argv) {
int c;
static int getRoll = 0, getPitch = 0, getYaw = 0, getAll = 0;
struct frontend_pid_data pid_data;
static struct option long_options[] = {
/* These options don’t set a flag. We distinguish them by their indices. */
{"roll", no_argument, &getRoll, 1},
{"pitch", no_argument, &getPitch, 1},
{"yaw", no_argument, &getYaw, 1},
{0, 0, 0, 0}
};
while (1)
{
/* getopt_long stores the option index here. */
int option_index = 0;
c = getopt_long(argc, argv, "a", long_options, &option_index);
/* Detect the end of the options. */
if (c == -1)
break;
if (c == 'a') {
getAll = 1;
}
}
int result;
if(getAll) {
pid_data.pid = ROLL;
if ((result = getValues(conn, &pid_data))) {
return result;
}
pid_data.pid = PITCH;
if ((result = getValues(conn, &pid_data))) {
return result;
}
pid_data.pid = YAW;
if ((result = getValues(conn, &pid_data))) {
return result;
}
} else {
if(getPitch) {
pid_data.pid = PITCH;
if ((result = getValues(conn, &pid_data))) {
return result;
}
}
if(getRoll) {
pid_data.pid = ROLL;
if ((result = getValues(conn, &pid_data))) {
return result;
}
}
if(getYaw) {
pid_data.pid = YAW;
if ((result = getValues(conn, &pid_data))) {
return result;
}
}
}
return 0;
}
int getValues(struct backend_conn * conn, struct frontend_pid_data * pid_data) {
if(frontend_getpid(conn, pid_data)) {
return 1;
}
switch(pid_data->pid) {
case PITCH :
printf("Pitch Constants: P = %f\tI = %f\tD = %f\n",
pid_data->p, pid_data->i, pid_data->d);
break;
case ROLL :
printf("Roll Constants: P = %f\tI = %f\tD = %f\n",
pid_data->p, pid_data->i, pid_data->d);
break;
case YAW :
printf("Yaw Constants: P = %f\tI = %f\tD = %f\n",
pid_data->p, pid_data->i, pid_data->d);
break;
default :
break;
}
return 0;
}