-
Notifications
You must be signed in to change notification settings - Fork 0
/
firmata_client.cpp
89 lines (69 loc) · 2.13 KB
/
firmata_client.cpp
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
#include "firmata_client.h"
#ifdef USE_REMOTE_WITH_FIRMATA
#if (defined(RTE_APP) || defined(PLC))
#include <remote.h>
const remote_abi remote =
{
firmata_client::get_var_bool, firmata_client::get_var_int, firmata_client::get_var_float,
firmata_client::set_var_bool, firmata_client::set_var_int,
firmata_client::set_var_float,
};
#else
#define NO_OBJECT (-1)
#endif
int firmata_client::get_var_bool(int index, u8 len) {
byte buf[6];
*(int *) buf = index;
*(u16 *) &buf[4] = len;
fm_client.sendSysex(stream, FM_READ_BIT, 6, (byte *) buf);
return 0;
}
int firmata_client::get_var_int(int index, short *value, u16 len) {
int res = fm_client.getValue(stream, index, (u8 *) value, len * 2);
return res;
}
int firmata_client::get_var_float(int index, float *value, u16 len) {
int res = fm_client.getValue(stream, index, (u8 *) value, len);
return res;
}
nStream *firmata_client::stream = nullptr;
int firmata_client::set_var_bool(int index, u8 value) {
int res = fm_client.setValue(stream, index, (u8 *) &value, 1);
return res;
}
int firmata_client::set_var_int(int index, int value) {
int res = fm_client.setValue(stream, index, (u8 *) &value, 2);
return res;
}
int firmata_client::set_var_float(int index, float value) {
int res = fm_client.setValue(stream, index, (u8 *) &value, 4);
return res;
}
void firmata_client::thd_loop(void *arg) {
auto *fm = (nStream *) arg;
while (true) {
fm_client.loop(fm);
Rtos::Delay(1);
}
}
firmata_client fm_client;
int firmata_client::get_plc_var(int index) {
fm_client.sendSysex(stream, FM_GET_DBG, 4, (byte *) &index);
return 0;
}
int firmata_client::set_plc_var(int index, byte *varp, int len) {
auto *buf = (byte *) malloc(len + 4);
*(int *) buf = index;
memcpy(&buf[4], varp, len);
fm_client.sendSysex(stream, FM_SET_DBG, len + 4, buf);
free(buf);
return 0;
}
int firmata_client::begin(nStream *s) {
stream = s;
#ifdef USE_FREERTOS
Rtos::create_thread_run("fc", 512, PriorityNormal, (void *) &thd_loop, s);
#endif
return 0;
}
#endif