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
#include <err.h>
#include <stdio.h>
#include "frontend_setpid.h"
#include "pid_common.h"
#include "frontend_common.h"
int frontend_setpid(
struct backend_conn * conn,
struct frontend_pid_data * pid_data,
int mask)
{
if (conn == NULL) {
return -1;
}
char * controller;
switch (pid_data->controller) {
case PID_PITCH:
controller = "pitch";
break;
case PID_ROLL:
controller = "roll";
break;
case PID_YAW:
controller = "yaw";
break;
/* What is the "throttle" pid constant? */
default:
warnx("Unsupported PID variable");
return -1;
}
char buffer[2048];
/* Set the P, I, and D */
if (mask & SET_P) {
if (snprintf(buffer, 2048,
"set%sp %f\n",
controller,
pid_data->p) >= 2048) {
errx(0, "Buffer to small to format!");
}
ucart_backendWrite(conn, buffer);
}
if (mask & SET_I) {
if (snprintf(buffer, 2048,
"set%si %f\n",
controller,
pid_data->i) >= 2048) {
errx(0, "Buffer to small to format!");
}
ucart_backendWrite(conn, buffer);
}
if (mask & SET_D) {
if (snprintf(buffer, 2048,
"set%sd %f\n",
controller,
pid_data->d) >= 2048) {
errx(0, "Buffer to small to format!");
}
ucart_backendWrite(conn, buffer);
}
return 0;
}