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
|
#include "controller.h"
#include <stdio.h>
#define CHANNEL_THROTTLE 0
#define CHANNEL_ATTITUDE_OFFSET 1
#define THROTTLE_MIN 1000
#define THROTTLE_MAX 2000
#define THROTTLE_STEP_SMALL 10
#define THROTTLE_STEP_LARGE 100
#define ATTITUDE_MIN 1000
#define ATTITUDE_MAX 2000
/*
* convert the value from an axis to an appropriate ppm value
* between 1000 and 2000
*/
inline channel_t axis_to_attitude(int value) {
long centered = (long) value - AXIS_MIN;
long proportional = centered * ATTITUDE_MIN / (AXIS_MAX - AXIS_MIN);
return (channel_t) ATTITUDE_MAX - ATTITUDE_MIN + proportional;
}
void channel_reset(channel_t* channels) {
for (size_t i = 0; i < CHANNELS; ++i) {
channels[i] = CHANNEL_UNUSED;
}
channels[0] = THROTTLE_MIN;
channels[1] = ATTITUDE_MIN;
channels[2] = ATTITUDE_MIN;
channels[3] = ATTITUDE_MIN;
channels[4] = ATTITUDE_MIN;
}
void event_axis(channel_t* channels, int axis, int value) {
channels[CHANNEL_ATTITUDE_OFFSET+axis] = axis_to_attitude(value);
}
void event_button(channel_t* channels, int button, bool value) {
if (value) {
switch(button) {
case 0:
channel_reset(channels);
break;
case 4:
channel_step(channels, CHANNEL_THROTTLE, -THROTTLE_STEP_SMALL, THROTTLE_MIN, THROTTLE_MAX);
break;
case 5:
channel_step(channels, CHANNEL_THROTTLE, THROTTLE_STEP_SMALL, THROTTLE_MIN, THROTTLE_MAX);
break;
case 6:
channel_step(channels, CHANNEL_THROTTLE, -THROTTLE_STEP_LARGE, THROTTLE_MIN, THROTTLE_MAX);
break;
case 7:
channel_step(channels, CHANNEL_THROTTLE, THROTTLE_STEP_LARGE, THROTTLE_MIN, THROTTLE_MAX);
break;
default:
printf("button unsupported %d\n", button);
break;
}
}
}
|
